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, ivzint
c
c  ltrim: t for trimble output, f if solution has gpstime in first col
c  ivzint: 0 = use GPS coor (no .dif file written)
c          1 = use EGI with height from integrated velocity
c          2 = use EGI with direct heights (note: resolution problem)
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), igap
c  gpsegi.dif:  thr, dlat, dlon, dh (INS-GPS, meter)
c
c  igap:  0 for no gps gap, 1 for interpolation at gaps 
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  updated cfs alert may 2004 (improved warnings for gps gaps etc)
c  implemented midnight (avo05) from gpsegikk
c  warnings for gap and time errors in egi data rf feb 08
c  Mode for GPS interpolation instead of EGI, rf oct 08
c  Updated jan 2011, rf
c
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      character gpsfile*72,egifile*72
      logical ltrim,lvzint,lgap,lgps
      dimension xgps(28801),ygps(28801),hgps(28801),
     .xdif(28801),ydif(28801),hdif(28801),cosfi(28801),xx(28801),
     .lgap(28801)
      maxn = 28801
c
      degm = 1852*60
      radeg = 180/3.14159265d0
      posmax = 0
      tposmax = 0
      hsum2 = 0
      nsum2 = 0
c
      write(*,*) '- GPSEGI -'
      read(*,'(a)') egifile
      read(*,'(a)') gpsfile
      read(*,*) tstart,tstop,tshift,tc,ltrim,ivzint
c
      tshift = tshift/3600
      lvzint = (ivzint.eq.1)
      if (ivzint.gt.0) then
        lgps = .false.  
        write(*,1) tc
1       format(' draping time constant INS to GPS (sec): ',f6.1)
      else
        lgps = .true.
        write(*,*) 'using GPS instead of EGI for coordinates'
        if (tc.gt.0) 
     .  write(*,*) '*** warning, tc not zero - coor filtered'
      endif
c
      open(10,file=gpsfile,status='old')
      open(11,file=egifile,status='old')
      open(20,file='gpsegi.pos',status='unknown')
      if (.not.lgps) 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(*,*)
      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
        lgap(i) = .true.
      enddo
      if (ltrim) read(10,*)
40    if (ltrim) then
        read(10,*,end=49) it,rfi,rla,rh,rsec
      else
        read(10,*,end=49) rsec,rfi,rla,rh
      endif
c 
c  3 lines added to handle midnight (avo)
      if (ngps.eq.-1) nday0 = int(rsec/86400)
      rsec = rsec - 86400.0d0*nday0
      tdec = rsec/3600.0d0
c
      if (ngps.eq.-1) then 
        write(*,41) tdec, rsec
41      format(' GPS data start (dechr and sec): ',f9.4,f11.2)
        ngps = 0
      endif
      ngps = ngps+1
      if (abs(nint(tdec*3600)-rsec).gt.1) 
     .write(*,*) 'warning: GPS time inconsistency sec = ',it
c
      tdec = tdec - tshift
c
      if (tdec.lt.tstart-9.9d-9) goto 40
      i = nint((tdec-tstart)*3600)+1      
      if (i.gt.nn) goto 49
      ygps(i) = rfi
      xgps(i) = rla
      hgps(i) = rh
      lgap(i) = .false.
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(*,*)
      write(*,*) '- reading ins data'
      nins = 1
      if (lvzint) then 
        read(11,*) thr,rlatp,rlonp,hp,pitch,roll,hdg,hvz
        hp = hvz
      else      
        read(11,*) thr,rlatp,rlonp,hp,pitch,roll,hdg
      endif
      rp = (thr-tstart)*3600+1      
      ip = rp 
c
50    if (lvzint) then
        read(11,*,end=55) thr,rlat,rlon,r,pitch,roll,hdg,h
      else
        read(11,*,end=55) thr,rlat,rlon,h,pitch,roll,hdg
      endif
      r = (thr-tstart)*3600+1      
      i = r
      dt = r - rp
      if (nins.gt.1.and.thr.gt.tstart) then
        if (thrp.ge.thr) then
          write(*,51) thr
51        format(' *** warning, EGI time reversal at:',f8.4)
          goto 54
        endif
        if (abs(thrp-thr)*3600.gt.1.1) then
          write(*,52) thr,abs(thrp-thr)*3600
52        format(' *** warning, EGI time gap at:',f8.4,', sec: ',f8.2)
          goto 54
        endif
      endif
c
c  make differences INS-GPS into ydif,xdif,hdif
c  set egi values to zero in gps forcing case

c
      if (ip.lt.i.and.ip.ge.1.and.i.le.nn.and.(.not.lgps)) 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
        if (ygps(i).lt.9e9) ydif(i) = yint - ygps(i)
        if (ygps(i).lt.9e9) cosfi(i) = cos(ygps(i)/radeg)
        if (xgps(i).lt.9e9) xdif(i) = xint - xgps(i)
        if (hgps(i).lt.9e9) then
          hdif(i) = hint - hgps(i)
          hsum2 = hsum2+hdif(i)**2
          nsum2 = nsum2+1
        endif
      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
c
54    thrp = thr
      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  or interpolate for pure gps output of h
c  -------------------------------------------------
c
      if (lgps) then
        call rfill(ygps,nn,nfill,igapmax)
        call rfill(xgps,nn,nfill,igapmax)
        call rfill(hgps,nn,nfill,igapmax)
        if (nfill.gt.0) write(*,56) nfill,igapmax
      else
        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(*,56) nfill,igapmax
56      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,57) thr,dy,dx,hdif(i)
57        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
      endif
c
c  now read ins data again and apply interpolated GPS correction
c  again set egi values to zero if gps forcing
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
c
      if (lgps) then
        rlat = (1-r)*ygps(i)+r*ygps(i+1)
        rlon = (1-r)*xgps(i)+r*xgps(i+1)
        h = (1-r)*hgps(i)+r*hgps(i+1)
        write(20,65) thr,rlat,rlon,h,pitch,roll,hdg
      else
        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)
        igap = 0
        if (lgap(i).or.lgap(i+1)) igap = 1
        write(20,65) thr,rlat-dlat,rlon-dlon,h-dh,pitch,roll,hdg,igap
65      format(' ',f10.7,f12.7,f13.7,f9.2,2f8.3,f9.3,i3)
      endif
      goto 60
c
69    if (lgps) then
        write(*,*) 
     .  'gpsegi.pos written, coor from GPS and pitch,roll,hdg from EGI'
      else 
        if (tposmax.gt.0) write(*,70) posmax,posmax/tposmax/1852 
70      format(' Maximum position error:',f8.1,' m,',f8.2,' nm/hr')
        if (nsum2.gt.0) write(*,71) sqrt(hsum2/nsum2)
71      format(' R.m.s. height difference INS-GPS:',f9.2)
      endif
      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