program gpsegi
      implicit real*8(a-h,o-z)
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c                        G P S E G I
c
c  Simple program for merging GPS and INS from 'readegi' (.coo files)
c  The program fill in GPS blanks with interpolated INS values,
c  and output composite file (.pos) with pitch, roll and heading 
c 
c  input: 
c
c  egifile (.COO)
c  gpsfile - must be one sec
c  tstart, tstop (dechr), tshift (sec, UTC offset), tc, ltrim, lvzint
c
c  ltrim: t for trimble output, f if solution has gpstime in first col
c  lvzint: use integrated velocity rather than height from 'readegi'
c  tshift: GPS-UTC time offset (+13 sec in 2001)
c  tc: time constant (sec) for smoothing INS-GPS differences 
c      (tc=0 mean no smoothing; note: if tc>0 the GPS solution is modified)
c
c  Outputfiles:
c  gpsegi.pos:  thr, lat, lon, h (GPS), pitch, roll, az (INS)
c  gpsegi.dif:  thr, dlat, dlon, dh (INS-GPS, meter)
c
c  Note: INS-GPS difference has some noise at the m-level (unknown source)
c  This noise is very sensitive to the correct utc offset
c
c  (c) rf/kms june 2001
c      adjusted to use the GPStime as a real, not integer KK, June 02
c  this version runs past midnight, avo Aug 05
c
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      character gpsfile*72,egifile*72
      logical ltrim,lvzint
      dimension xgps(43200),ygps(43200),hgps(43200),
     .xdif(43200),ydif(43200),hdif(43200),cosfi(43200),xx(43200)
      maxn = 43200
c
      degm = 1852*60
      radeg = 180d0/3.14159265d0
      posmax = 0
      tposmax = 0
c
      write(*,*) '- GPSEGI -'
      read(*,'(a)') egifile
      read(*,'(a)') gpsfile
      read(*,*) tstart,tstop,tshift,tc,ltrim,lvzint
      tshift = tshift/3600
      write(*,1) tc
1     format(' draping time constant INS to GPS (sec): ',f6.1)
c
      open(10,file=gpsfile,status='old')
      open(11,file=egifile,status='old')
      open(20,file='gpsegi.pos',status='unknown')
      open(21,file='gpsegi.dif',status='unknown')
      open(30,status='scratch',form='unformatted')
c  
      nn = nint((tstop-tstart)*3600)+1
      if (nn.gt.maxn) stop '*** too many points, increase maxn'
c
c  read gps data 
c  -------------
c
      write(*,*) '- reading GPS data'
      ngps = -1
      do i = 1,nn
        xgps(i) = 9.99d9
        ygps(i) = 9.99d9
        hgps(i) = 9.99d9
        xdif(i) = 9.99d9
        ydif(i) = 9.99d9
        hdif(i) = 9.99d9
        cosfi(i) = 9.99d9
      enddo
      if (ltrim) read(10,*)
40    if (ltrim) then
        read(10,*,end=49) it,rfi,rla,rh,asec
      else
        read(10,*,end=49) asec,rfi,rla,rh
      endif
c       
c  3 lines added to handle midnight (avo)
      if (ngps.eq.-1) nday0 = int(asec/86400)
      asec = asec - 86400.0d0*nday0
      tdec = asec/3600.0d0
c      asec = mod(asec,3600*24)
c      tdec = asec/3600.d0
c
      if (ngps.eq.-1) then 
        write(*,41) tdec, asec
41      format(' GPS data start (dechr and sec): ',f7.2, f13.6)
        ngps = 0
      endif
      ngps = ngps+1
      if (abs(nint(tdec*3600)-asec).gt.1) 
     .write(*,*) 'warning: GPS time inconsistency sec = ',asec
c
      tdec = tdec - tshift
c
      if (tdec.lt.tstart) goto 40
      if (tdec.gt.tstop) goto 49
      i = nint((tdec-tstart)*3600)+1      
      ygps(i) = rfi
      xgps(i) = rla
      hgps(i) = rh
!      print*, "gps: ",i,ygps(i),xgps(i),hgps(i)
c
      goto 40
c
49    write(*,*) 'no of gps data read: ',ngps
c
c  read ins data - linear interpolation to GPS time epochs     
c  -------------------------------------------------------
c
      write(*,*) '- reading ins data'
      nins = 1
      read(11,*) thr,rlatp,rlonp,hp,pitch,roll,hdg,hvz
      if (lvzint) hp = hvz
      rp = (thr-tstart)*3600+1      
      ip = nint(rp) !!!
c
50    read(11,*,end=55) thr,rlat,rlon,h,pitch,roll,hdg,hvz
      if (lvzint) h = hvz
      r = (thr-tstart)*3600+1      
      i = nint(r) !!!
      dt = r - rp
c
c  make differences INS-GPS into ydif,xdif,hdif
c
      if (ip.lt.i.and.ip.ge.1.and.i.le.nn) then
         if (dt.le.0) stop '*** dt in INS file zero'
         yint = ((r-i)*rlatp + (i-rp)*rlat)/dt
         xint = ((r-i)*rlonp + (i-rp)*rlon)/dt
         hint = ((r-i)*hp + (i-rp)*h)/dt
!         print*, ip, i, yint, xint,hint
         if (ygps(i).lt.9e9) then 
            ydif(i) = yint - ygps(i)
         endif
         if (ygps(i).lt.9e9) then
            cosfi(i) = cos(ygps(i)/radeg)
         else
!            print*, "y hole", i,r
         end if
         if (xgps(i).lt.9e9) then
            xdif(i) = xint - xgps(i)
         else
!            print*, "x hole", i,r
         end if
         if (hgps(i).lt.9e9) then
            hdif(i) = hint - hgps(i)
         else
!            print*, "h hole", i,r
         end if
      else
         print*, "there is a hole:",ip,rp," - ",i,r,nn
      endif
c     
c  write insdata on temp file to speed up reread
c
      if (ip.ge.1.and.i.le.nn) 
     .write(30) thr,rlat,rlon,h,pitch,roll,hdg
      nins = nins+1
      rlatp = rlat
      rlonp = rlon
      hp = h
      rp = r
      ip = i
      goto 50
c
55    write(*,*) 
     .'number of ins data read in wanted time interval: ',nins
      rewind(30)
c
c  now fill-up differences with linear interpolation
c  -------------------------------------------------
c
      call rfill(ydif,nn, nfill,igapmax)
      call rfill(xdif,nn, nfill,igapmax)
      call rfill(hdif,nn, nfill,igapmax)
      call rfill(cosfi,nn,nfill,igapmax)
      if (nfill.gt.0) write(*,21) nfill,igapmax
21    format(' no of interpolated values in gps file:',i6,
     .', max gap:',i5, ' sec')
c
      if (tc.gt.0) then 
        call rc(nn,ydif,xx,tc,1.d0,1,1)
        call rc(nn,xx,ydif,tc,1.d0,1,-1)
        call rc(nn,xdif,xx,tc,1.d0,1,1)
        call rc(nn,xx,xdif,tc,1.d0,1,-1)
        call rc(nn,hdif,xx,tc,1.d0,1,1)
        call rc(nn,xx,hdif,tc,1.d0,1,-1)
      endif
c
c  output differences
c
      do i = 1, nn
        thr = tstart + (i-1)/3600.d0
        dy = ydif(i)*degm
        dx = xdif(i)*cosfi(i)*degm
        write(21,56) thr,dy,dx,hdif(i)
56      format(f11.7,3f12.3)
        r = sqrt(dx**2+dy**2)
        if (r.gt.posmax) then
          posmax = r
          tposmax = (i-1)/3600.d0 
        endif
      enddo
c
c  now read ins data again and apply interpolated GPS correction
c  -------------------------------------------------------------
c
60    read(30,end=69) thr,rlat,rlon,h,pitch,roll,hdg
      r = (thr-tstart)*3600+1
      if (r.lt.1) then 
        i = 1
        r = 0
      elseif (r.ge.nn) then 
        i = nn-1
        r = 1
      else
        i = r
        r = r-i
      endif
      dlat = (1-r)*ydif(i)+r*ydif(i+1)
      dlon = (1-r)*xdif(i)+r*xdif(i+1)
      dh = (1-r)*hdif(i)+r*hdif(i+1)
c
      write(20,65) thr,rlat-dlat,rlon-dlon,h-dh,pitch,roll,hdg
65    format(' ',f10.7,f12.7,f13.7,f9.2,2f8.3,f9.3)
      goto 60
c
69    if (tposmax.gt.0) write(*,70) posmax,posmax/tposmax/1852 
70    format(' Maximum position error:',f8.1,' m,',f8.2,' nm/hr')
      end
c
      subroutine rc(n,x,y,tc,dt,m,idir)
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c                          R C
c
c  rc filter iir operator
c  tc = filter time const.
c  dt = data sampling interval
c  m  = number of stages
c  idir = 1: forward
c        -1: backward
c
c  dma/ds, modified jan 1992, rf
c
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      implicit double precision(a-h,o-z)
      dimension x(*),y(*)
      a = dt/(2*tc)
      b = (1.d0-a)/(1+a)
      c = a/(1+a)
      do 1 i=1,n
1     y(i)=x(i)
      if (idir.eq.1) then
        i1 = 1
        i2 = n
        i3 = 1
      else
        i1 = n
        i2 = 1
        i3 = -1
      endif
c
      do 3 k = 1,m
        a = y(i1)
        d = a
        do 2 i = i1+i3,i2,i3
          e = b*a + c*(y(i)+d)
          d = y(i)
          y(i) = e
2       a = e
3     continue
      end
c
      subroutine rfill(x, n, nfill, igapmax)
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c                          r f i l l
c
c  fills array with holes with interpolated values
c  a hole is defined as a value .ge. 9.98d9
c
c  (c) rf/kms sept 96
c
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      implicit real*8(a-h,o-z)
      dimension x(*)
      dimension xx(28801)
      nfill = 0
      igapmax = 0
      do 10 i = 1, n
         if (x(i).le.9.98d9) then
            xx(i) = x(i)
         else
          nfill = nfill+1
          j1 = i
5         j1 = j1-1
          if (j1.lt.1) goto 6
          if (x(j1).ge.9.98d9) goto 5
6         j2 = i
7         j2 = j2+1
          if (j2.gt.n) goto 8
          if (x(j2).ge.9.98d9) goto 7
8         if (j1.lt.1) then
            xx(i) = x(j2)
            ii = j2
          elseif (j2.gt.n) then
            xx(i) = x(j1)
            ii = n-j1
          else
            xx(i) = ((j2-i)*x(j1)+(i-j1)*x(j2))/(j2-j1)
            ii = j2-j1-1
          endif
          if (ii.gt.igapmax) igapmax = ii
       endif

10    continue
      do 20 i = 1,n
20    x(i) = xx(i)
      return
      end
c