program merge_ins
      implicit real*8(a-h,o-z)
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c                       M E R G E _ I N S
c
c  Program for merging EGI data from 'readegi' (.coo files)
c  and OXTS data from cryovex11 processing  (or the reverse option)
c
c  The program fill in EGI gaps with draped OXTS values,
c  and output EGI format joint file (.coo) with pitch, roll and heading 
c 
c  input: 
c
c  egifile (.coo)
c  oxts-file
c  tstart, tstop (dechr), lh4, tgap, tc
c
c  lh4:    true if col 4 is used (original egi), false col 8 (vz_int)
c
c  tgap:   minimum time gap (sec) for use of draped OXTS
c          (0.2 would correspond to 5 Hz, 0.1 to 10 Hz EGI data) 
c  tc:     time constant (sec) for smoothing EGI-OXTS differences 
c          (tc=0 mean no smoothing; note: if tc>0 the GPS solution is modified)
c
c  Outputfiles:
c  merge_ins.coo:  thr, lat, lon, h (GPS), pitch, roll, hdg (INS), igap
c  merge_ins.dif:  thr, dlat, dlon, dh (meter), dpitch, droll, dhdg
c
c  (c) rf/dtu-space jan 2012 (based on gpsegi)
c
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      character oxtsfile*72,egifile*72
      logical lgap, lh4
      real*8 othr(250000),olat(250000),olon(250000),oh(250000),
     .oroll(250000),opitch(250000),ohdg(250000),
     .tegi(250000),dlat(250000),dlon(250000),dh(250000),
     .droll(250000),dpitch(250000),dhdg(250000),tt(250000)
      maxn = 250000
c
      degm = 1852*60
      radeg = 180/3.14159265d0
      posmax = 0
      tposmax = 0
      hsum2 = 0
      nsum2 = 0
c
      write(*,*) '- EGI_OXTS -'
      read(*,'(a)') egifile
      read(*,'(a)') oxtsfile
      read(*,*) tstart,tstop,lh4,tgap,tc
c
      tshift = tshift/3600
      write(*,1) tc
1     format(' draping time constant OXTS to EGI (sec): ',f6.1)
c
      open(10,file=oxtsfile,status='old')
      open(11,file=egifile,status='old')
      open(20,file='merge_ins.coo',status='unknown')
      open(21,file='merge_ins.dif',status='unknown')
      open(30,status='scratch',form='unformatted')
c
c  read oxts data 
c  -------------
c
      write(*,*)
      write(*,*) '- Reading OXTS data'
      n = 0
      dtmax = 0
      dtave = 0
c
10    n = n+1
      read(10,*,end=15) 
     .othr(n),olat(n),olon(n),oh(n),opitch(n),oroll(n),ohdg(n)
      if (n.gt.maxn) 
     .stop '*** too many pts in oxts file, increase maxn'
      if (ohdg(n).gt.180) ohdg(n) = ohdg(n)-360
      goto 10
15    nn = n-1
c
      do i = 2,nn
        dt = (othr(i)-othr(i-1))*3600
        if (dt.lt.0) then
          write(*,*) othr(i)          
          stop ' *** time inversion'
        endif 
        dtave = dtave + dt
        if (dt.gt.dtmax) then
          thrmax = othr(i)
          dtmax = dt        
        endif
      enddo
      write(*,16) nn,othr(1),othr(nn),dtave/nn,dtmax,thrmax
16    format(' read',i7,' oxts data, time interval:',2f9.4,/
     .' average time spacing:',f6.3,' sec, max gap:',f6.3,' at',f9.4)
c  
c  read egi data - interpolate oxts data to egi epochs     
c  ---------------------------------------------------
c
      write(*,*)
      write(*,*) '- Reading EGI data'
      read(11,*) thrp,rlatp,rlonp,h1,pitchp,rollp,hdgp,h2
      if (lh4) then
        hp = h1
      else
        hp = h2
      endif
      i = 0
      dtmax = 0
      dtave = 0
      thrmax = 0
c
20    i = i+1
      if (negi.gt.maxn) stop '*** too many egi pts, increase maxn'
21    read(11,*,end=25) thr,rlat,rlon,h1,pitch,roll,hdg,h2
      if (lh4) then
        h = h1
      else
        h = h2
      endif
c
      if (thr.lt.othr(1)) goto 21
      if (thr.gt.othr(nn)) goto 25
      dt = (thr-thrp)*3600
      dtave = dtave + dt
      if (dt.gt.dtmax.and.i.gt.1) then
        dtmax = dt
        thrmax = thr
      endif
c
c  make differences OXTS-EGI into dlat,dlon,dh 

c
      tegi(i) = thr
      dlat(i) = rlin(thr,othr,olat,nn) - rlat
      dlon(i) = rlin(thr,othr,olon,nn) - rlon
      dh(i) = rlin(thr,othr,oh,nn) - h
      dpitch(i) = rlin(thr,othr,opitch,nn) - pitch
      droll(i) = rlin(thr,othr,oroll,nn) - roll
      dhdg(i) = rlin(thr,othr,ohdg,-nn) - hdg
      if (dhdg(i).lt.-180) dhdg(i) = dhdg(i)+360
      if (dhdg(i).gt.180) dhdg(i) = dhdg(i)-360
      write(21,30) thr,dlat(i),dlon(i),dh(i),dpitch(i),droll(i),dhdg(i)
c
c  write egidata on temp file to speed up reread
c  save times in array for ease of gap detection
c
      write(30) thr,rlat,rlon,h,pitch,roll,hdg
      thrp = thr
      goto 20
c
25    negi = i-1
      write(*,26) negi,dtmax/negi,dtmax,thrmax
26    format(' number of egi data read in oxts time interval: ',i7,
     ./' average time spacing: ',f6.3,' sec, max gap:',f6.3,
     .' sec at ',f8.4)
      if (negi.lt.2) stop '*** too few egi pts'
c
c  filter differences
c  ------------------
c
      if (tc.gt.0) then 
        call rc(nn,dlat,tt,tc,1.d0,1,1)
        call rc(nn,tt,dlat,tc,1.d0,1,-1)
        call rc(nn,dlon,tt,tc,1.d0,1,1)
        call rc(nn,tt,dlon,tc,1.d0,1,-1)
        call rc(nn,dh,tt,tc,1.d0,1,1)
        call rc(nn,tt,dh,tc,1.d0,1,-1)
        call rc(nn,dpitch,tt,tc,1.d0,1,1)
        call rc(nn,tt,dpitch,tc,1.d0,1,-1)
        call rc(nn,droll,tt,tc,1.d0,1,1)
        call rc(nn,tt,droll,tc,1.d0,1,-1)
        call rc(nn,dhdg,tt,tc,1.d0,1,1)
        call rc(nn,tt,dhdg,tc,1.d0,1,-1)
      endif
c
c  now read through egi file, and use oxts file in gaps
c  -----------------------------------------------------
c
      rewind(30)
      i = 1
      j = 1
      igap = 0
      ngap = 0
      dtsum = 0
      read(30) thrp,rlat,rlon,h,pitch,roll,hdg
      write(20,30) thrp,rlat,rlon,h,pitch,roll,hdg,igap
30    format(' ',f10.7,f12.7,f13.7,f9.2,2f8.3,f9.3,i3)
c      
31    read(30) thr,rlat,rlon,h,pitch,roll,hdg
      dt = (thr-thrp)*3600
c
c  skip oxts recs until in gap, and output gap recs
c
      if (dt.gt.tgap) then
        igap = 1
        dtsum = dtsum+dt
c
32      if (othr(j).le.thrp) then
          j = j+1
          if (j.gt.nn) goto 39
          goto 32
        endif
c        
33      if (othr(j).lt.thr) then
          t = othr(j)
          head = ohdg(j)-rlin(t,tegi,dhdg,-negi)
          if (head.lt.-180) head = head+360
          if (head.gt.180) head = head-360
c
          write(20,30) t,
     .    olat(j)-rlin(t,tegi,dlat,negi),
     .    olon(j)-rlin(t,tegi,dlon,negi),
     .    oh(j)-rlin(t,tegi,dh,negi),
     .    opitch(j)-rlin(t,tegi,dpitch,negi),
     .    oroll(j)-rlin(t,tegi,droll,negi),
     .    head,igap
          ngap = ngap+1
          j = j+1
          if (j.gt.nn) goto 39
          goto 33
        endif
      endif
c
c  output egi rec
c
      igap = 0        
      write(20,30) thr,rlat,rlon,h,pitch,roll,hdg,igap
      i = i+1
      thrp = thr
      if (i.ge.negi) goto 39
      goto 31
c
39    write(*,40) i-1,ngap,dtsum/3600/(tegi(negi)-tegi(1))*100
40    format(/' Output egi recs:',i7,', oxts draped recs:',i7,
     ./' percentage of gaps vs. total time:',f7.2)      
      end
c
      real*8 function rlin(x, xx, yy, n)
      implicit real*8(a-h,o-z)
      dimension xx(250000), yy(250000)
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c                          r l i n
c
c  linear interpolation procedure. function values yy(1)..yy(n)
c  given at arguments xx(1)..xx(n). outside argument interval
c  the value at the end point is used.
c  the arguments xx(1) ..  must be in increasing order.
c
c  rf june 87
c  updated for heading interpolation jan 2012 (negative n)
c
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      integer*4 dk
      logical l360
      l360 = (n.lt.0)
      n = abs(n)
      rx = x
      if (x.lt.xx(1)) x = xx(1)
      if (x.gt.xx(n)) x = xx(n)
      k = n/2
      dk = k
11    dk = dk/2
      if (dk.lt.1) dk = 1
      if (k.le.1.or.k.ge.n-1) goto 20
      if (x.ge.xx(k).and.x.lt.xx(k+1)) goto 20
      if (x.lt.xx(k)) k = k-dk
      if (x.ge.xx(k+1)) k = k+dk
      goto 11
c
20    if (k.lt.1) k = 1
      if (k.ge.n) k = n-1
      dx = x-xx(k)
      y1 = yy(k+1)
      y0 = yy(k)
      if (l360) then
        if (y1-y0.gt.180) y1 = y1-360
        if (y1-y0.le.-180) y1 = y1+360
      endif
c
      rr = (x-xx(k))/(xx(k+1)-xx(k))*(y1-y0) + y0
c
      if (l360) then
        if (rr.lt.-180) rr = rr+360
        if (rr.gt.180) rr = rr-360
      endif
      rlin = rr
      x = rx
c
      return
      end

      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