program readegi8
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c                          R E A D E G I
c
c  program to read H764G raw data as logged by "egicon"
c  
c  input:
c
c  egifile
c  subw, dt, t1, t2, lvel
c
c  where
c
c  subw - wanted record type (17 blended GPS/INS, 29 free INS, -29: free INS before 2005)
c  dt - time spacing of output data (sec), 0 = all data
c  t1,t2 - wanted output epochs process (dechr)
c  lvel - true if outputfile with velocity/acc and wander azim wanted
c
c  outputfiles (2nd only if lvel true):
c
c  readegi.coo - thr, lat, lon, h, pitch, roll, true heading, vzint
c  readegi.vel - thr, vx, vy, vz, accx, accy, accz, alpha 
c  (test.vel - thr, h, vzint, vz, acc, dv/dz)
c 
c  NB: time, velocity and acc data are averaged for dt > 0 to limit noise
c  units: m, m/s, m/s**2 and degrees. thr is the GPS time in dechr
c
c  (c) Rene Forsberg/National Survey and Cadastre (KMS)
c  8 jan 2001
c  updated 26 jun 2001
c  updated with integrated velocities aug 3, 2001, rf
c  VERSION TO GET EGI TIME TAG FROM GPS word 2
c  UPDATED BY K.KELLER MAY 2003
c  -----  to run past midnight, avo Aug 05
c   info about EGI data: 'EGIcon.doc' June 20, 2000 by Kim Woelders
c  EGI time tag error fixes and cleanup, RF jan 08
c  Correction for EGI upgrade 2005 (change of acc and v) and data gaps, 
c  Damping for vertical acceleration errors - RF jan 2012
c
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      implicit real*8(a-h,o-z)
      logical lalign,lstart,lout,louth,lvel,lold
      character*72 ifile
      character*1 tchar
      character*3 mode2char
      integer*1 iyr,imon,iday,ihr,imin,isec,type,sub,len,subw
      integer*2 iswap,hdr(12),w(32),dhdr(4),datseq,ms,fom,mode,mode2
      integer*2 w8(29)
      integer*4 hdrseqn,iutc8,iutc,imw2
      integer*1 butc(4)
      equivalence (iutc,butc)
      equivalence (hdr(5),hdrseqn),(hdr(8),datseq),(hdr(12),ms)
c
c  program input
c  -------------
c
      read(*,'(a)') ifile
      read(*,*) subw, dt, t1, t2, lvel
c
      lold = (subw.eq.-29.or.subw.eq.17)
      subw = abs(subw)
      if (.not.(subw.eq.17.or.subw.eq.29)) stop '*** subw wrong'
c
      write(*,5) dt,ifile
5     format(' --- READEGI ---'/
     .' data output at:',f5.2,' sec, EGI file: ',a36)
      if (lold) write(*,*) '--- Pre-2005 EGI data format for subw=29'
      open(10,file=ifile,form='unformatted',access='direct',recl=2)
      open(20,file='readegi.coo',status='unknown')
c
      if (lvel) open(21,file='readegi.vel',status='unknown')
      if (lvel) open(22,file='test.vel',status='unknown')
c
c  constants
c  ---------
c  hdrsync = 5A A5 hex, corresponds to signed integer -23206
      hdrsync = -23206
      max4 = 2147483647
      max2 = 32767
      radeg = 180/3.141592653589d0
c  direction cosines - factor 2 seems necessary?
      fakc = 2.d0/max4
c  longitude
      faklon = 180.d0/max4
c  roll,heading,pitch
      fakr = 180.d0/max2
c  altitude 
      faka = 131068.d0*0.3048/max2
c  speed convert from ft/s to m/s
      fakv_old = 8192.d0*0.3048/max4
      fakv = 8192.d0*0.3048/max2
c  acc
      fakacc_old = 1023.97d0*0.3048/max2
      fakacc = 1023.97d0*0.3048/max4
c  utc
      faku = 131072.d0/max4
c  egi time tag 
c      faktt = 4.194240d0/(max2*2.0d0)
c      alternative factor:
       faktt = 2.0971520d0/max2
c  counters: irec is input record counter
c            nrec is record counter
c            nsub counts number of read wanted subrecords
c            nsubo counts number of output wanted subrecords
c
      maxt = 36.0d0
      irec = 0
      nrec = 0
      nsub = 0
      nsubo = 0
      nsub8 = 0
      n8 = 0
      n6 = 0
      n16 = 0
      n17 = 0
      n29 = 0
      lstart = .true.
      lalign = .true.
      thrp = -9999
      thrup = -9999
      dthr = (dt-0.001)/3600
      dthrmax = 0
      nsum = 0
      tsum = 0
      vxsum = 0
      vysum = 0
      vzsum = 0
      axsum = 0
      aysum = 0
      azsum = 0
      utcp = 0
      vzint = 0
      utc8p = 0
      tt8p = 0
      ttbias = 0
      ttudif = 0
c
c  -----------------------------
c  main loop reentry
c  read header block (in record)
c  -----------------------------
c
10    do j = 1,12
        irec = irec+1
        read(10,rec=irec,err=90) hdr(j)
      enddo
c
c     2 lines added to to handle midnight problem, avo Aug 05
      call split(hdr(10),iday,ihr)
      if (n17.lt.5.and.n29.lt.5) ifirstday = iday
c
c  check if header record is correct. If not skip forward to find header
c
      if (hdr(1).ne.hdrsync.or.hdr(2).ne.hdrsync) then
        write(*,*) '*** rechdr not ok, irec=',irec,', hdr=',
     .  hdr(1),hdr(2)
12      irec = irec+1
        read(10,rec=irec,err=90) hdr(1)
        if (hdr(1).ne.hdrsync) goto 12 
        irec = irec-1
        goto 10
      endif
c
      call split(hdr(9),iyr,imon)
      call split(hdr(10),iday,ihr)
      call split(hdr(11),imin,isec)
c     changed to handle midnight problem, avo Aug 05
      thr=(iday-ifirstday)*24.d0+ihr+imin/60.d0+isec/3600.d0+ms/3600000.d0
      if (thr.gt.maxt) thr = thr - 24.0d0
      if (lstart) then
        ihr1 = ihr
        imin1 = imin
        thr1 = thr
        thrp = thr
        write(*,14) iyr+2000,imon,iday,ifirstday,subw
14      format(/' hdr date: ',i4,i3,i3,' firstday',i3,
     .  ', wanted subadr:',i3)
      endif
c
c  read data block header (first 4 bytes)
c  --------------------------------------
c
c format: 1: dev  + type
c         2: subw + len
c         3: TTBC
c         4: status
c for more info see 'EGIcon.doc' by Kim Woelders
c
      do j = 1,4
        irec = irec+1
        read(10,rec=irec,err=90) dhdr(j)
      enddo
      type = dhdr(1)/256
      tchar='T'
      if (type.ne.1) tchar='R'
      call split(dhdr(2),sub,len)
c
c  data header output on screen - occasionally
c
c  counting records
c
      nrec = nrec+1
      if (sub.eq.8) n8 = n8+1
      if (sub.eq.6) n6 = n6+1
      if (sub.eq.16) n16 = n16+1
      if (sub.eq.17) n17 = n17+1
      if (sub.eq.29) n29 = n29+1
      lout = (mod(nsub,30000).eq.1)
      louth = (nrec.lt.20)
c
      if (louth) write(*,15) hdrseqn,datseq,
     .ihr,imin,isec,ms,tchar,sub,len,irec
15    format(' hdr',i8,' dat',i8,
     .' time ',3i3,'.',i3,' sub ',a1,i3,' len ',i2,
     .' rec ',i8)
c  
c  skip unwanted records - jump back for new record
c
      if (sub.ne.subw.and.sub.ne.8) then
        irec = irec+len
        goto 10
      endif
c
      IF (SUB.EQ.17.OR.SUB.EQ.29) THEN
c	
c  decode wanted subadr data
c  --------------------------
c
      nsub = nsub+1
      do j= 1,len  
        irec = irec+1
        read(10,rec=irec,err=90) w(j)
      enddo
c
c  sub 17 or 29
c
      cnexz = long(w(21),w(22))*fakc
      rlat = asin(cnexz)*radeg
      rlon = long(w(23),w(24))*faklon
      alt = iswap(w(25))*faka
      roll = iswap(w(10))*fakr
      pitch = iswap(w(11))*fakr
      hdg = iswap(w(12))*fakr
c      iutc = long(w(27),w(28))
c      utc = iutc*faku
c
      tt1 = real(iswap256(w(2)))
c  
      ttp = tt
      tt = tt1*faktt
c
      if (lold) then
        vz = long(w(7),w(8))*fakv_old
      else
        vz = iswap(w(16))*fakv
      endif
      mode2 = iswap(w(29))
      if (lvel) then
        azim = iswap(w(9))*fakr
        if (lold) then
          vx = long(w(3),w(4))*fakv_old
          vy = long(w(5),w(6))*fakv_old
          ax = iswap(w(14))*fakacc_old 
          ay = iswap(w(15))*fakacc_old 
          az = iswap(w(16))*fakacc_old
        else
          ax = long(w(3),w(4))*fakacc
          ay = long(w(5),w(6))*fakacc
          az = long(w(7),w(8))*fakacc 
          vx = iswap(w(14))*fakv 
          vy = iswap(w(15))*fakv 
        endif
c 
        if (dt.gt.0) then
          nsum = nsum+1
          tsum = tsum+thru
          vxsum = vxsum+vx
          vysum = vysum+vy
          vzsum = vzsum+vz
          axsum = axsum+ax
          aysum = aysum+ay
          azsum = azsum+az
        endif
      endif
c
c  update time info
c  internal egi clock reset after 4.2 sec
c
      if (tt-ttp.lt.0) ttbias = ttbias + 4.19436800d0
      te = tt + ttbias
      utc = te + ttudif
c
      ELSEIF (SUB.EQ.8) THEN
c
c       message 8 is GPS data: use this to update ttudif
c       UTC time tag: word (19/20)
c       and EGI time tag: word 2 (time of validity)
c
        tt8p = tt8
        utc8p = utc8
        nsub8 = nsub8+1
        do j = 1, len  
          irec = irec+1
          read(10,rec=irec) w8(j)
        enddo
c
        iutc8 = long(w8(19),w8(20))
        utc8 = iutc8*faku
        tt8 = real(iswap256(w8(2)))
        tt8 = tt8*faktt
        if (tt.lt.tt8) tt8 = tt8 - 4.19436800d0
        te8 = ttbias + tt8
        ttudif = utc8-te8
c        write(*,18) tt8,te8,utc8,ttudif
c18      format(' test tt8 te8 utc8 dif:',4f12.4)
c
c  warning for time gaps
c
        tgap = utc8-utc8p
        if (n8.gt.1.and.tgap.gt.3.5) then 
          ihrg = utc8/3600
          iming = (utc8-ihrg*3600)/60
          isecg = utc8-ihrg*3600-iming*60
          write(*,17) utc8/3600,ihrg,iming,isecg,tgap
17        format(' major gps time gap: ',f9.5,i4,i3.2,i3.2', sec:',f7.3)
        endif
        goto 10
      ENDIF
c
      thr = utc/3600.d0
      thru = utc/3600.0d0 + 24.0d0*(iday - ifirstday)
c
c  integrate vertical velocity
c  alfa is an empirical damping factor
c
      alfa = 0.998
      if (nsub8.ge.2) then
        if (nsub8.eq.2) vzint=alt
        if (thru.gt.maxt) thru = thru - 24.0d0
        dtu = utc-utcp
        if (dtu.gt.86400) dtu = dtu - 86400.0d0
        if (dtu.lt.-86400) dtu = dtu + 86400.0d0
        if (thru.lt.t1.or.utcp.le.0) then
          vzint = alt
        else
          vzint = alfa*(vzint + dtu*vz) + (1-alfa)*alt
        endif
      endif
      utcp = utc
c
c  output data on screen and file
c  ------------------------------
c
      if (lout) then
        mode = iswap(w(1))
        fom = iswap(w(26))
c
c  decode fom and mode2
c
        ifom3 = mod(fom,16)
        ifom2 = mod(fom/16,16)
        ifom1 = mod(fom/256,16)
        mode2char = 'unk'
        if (mod(mode2/16384,2).eq.1) mode2char = 'sby'
        if (mod(mode2/8192,2).eq.1) mode2char = 'ala'
        if (mod(mode2/4096,2).eq.1) mode2char = 'sha'
        if (mod(mode2/2048,2).eq.1) mode2char = 'alg'
        if (mod(mode2/128,2).eq.1) mode2char = 'nav'
        if (mod(mode2,2).eq.1) mode2char = 'nrf'
c
        if (thru.gt.t1) write(*,20) thru,rlat,rlon,alt,pitch,roll,hdg,
     .  mode2char,ifom1,ifom2,ifom3,mode,ttudif
20      format(' ',f9.6,f10.5,f11.5,f8.1,2f6.1,f7.1,' ',a3,' ',
     .  3i1,i5,f9.1)
      endif
c
c  output on files - position, pitch, roll, hdg
c
c  to avoid double pos at same time
      if ((thru-thrup).le.0.000001) goto 10
c
      if (thr-thrp.gt.dthr.and.thru.gt.thrup) then
        if (thru.gt.t1)
     .  write(20,21) thru,rlat,rlon,alt,pitch,roll,hdg,vzint
21      format(' ',f10.7,f12.7,f13.7,f9.1,2f8.3,f9.3,1x,f11.2)
c
c  velocity/acc output - average
c
        if (lvel) then
          alpha = azim-hdg
          if (alpha.lt.-180) alpha = alpha+360
          if (alpha.gt.180) alpha = alpha-360
c
          if (dt.gt.0) then
            if (nsum.ge.1) then
              thru = tsum/nsum
              vx = vxsum/nsum         
              vy = vysum/nsum         
              vz = vzsum/nsum         
              ax = axsum/nsum         
              ay = aysum/nsum         
              az = azsum/nsum         
              nsum = 0
              tsum = 0
              vxsum = 0
              vysum = 0
              vzsum = 0
              axsum = 0
              aysum = 0
              azsum = 0
            endif
          endif
c
          if (thru.gt.t1)
     .    write(21,22) thru,vx,vy,vz,ax,ay,az,azim-hdg
22        format(' ',f10.7,2f13.5,f11.5,3f9.4,f8.2)
c  test output 
          if (thru.gt.t1) then
            write(22,22) thru,alt,vzint,vz,az-9.82,testacc
            if (dt.eq.0) then
              testacc = 0
            else
              testacc = (vz-vzp)/dt 
            endif
            vzp = vz
          endif
        endif
c
        if (thr-thrp.gt.dthrmax.and.thru.gt.t1) then
          dthrmax = thr-thrp
          thrmax = thr
        endif
        thrp = thr
        thrup = thru
        nsubo = nsubo+1
      endif
c
c  end of record loop
c  ------------------
c       
      if (lalign) then
        if (mode2.eq.128) then
          lalign = .false.
          write(*,*)
          write(*,*)
     .    '   thr        lat        lon        h   pitch  roll',
     .    '   hdg     FOM mode  egi2utc'
          ihru2 = thru
          iminu2 = (thru-ihru2)*60
        endif
      endif
      if (lstart) lstart = .false.
      if (thru.gt.t2) goto 90
      goto 10 
c 
90    write(*,91) iyr+2000,imon,iday
91    format(/' Date: ',i4,i3,i3)
      write(*,92) ihr1,imin1,ihr,imin,(thr-thr1)*60,ihru2,iminu2
92    format(' Start eqi time  ',i2,':',i2,', stop ',i2,':',i2,
     .', duration:',f7.2,' min'/
     .' Start nav time  ',i2,':',i2)
      write(*,93) nrec,nsub,nsubo,n6,n8,n16,n17,n29
93    format(' Total records read:',i8,', wanted type read:',i8,
     .', output:',i8/' No of recs subadr 6,8,16,17,29: ',5i8)
      write(*,94) nsub/(thr-thr1)/3600,dthrmax*3600,thrmax
94    format(' Average logging rate:',f5.1,' Hz, max data time break:'
     .,f8.2,' sec at ',f9.6)
      end
c
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
      integer*2 function iswap(i)
c     ---------------------------
c  function swaps the two bytes in i     
c
      integer*2 i,k
      byte b(2),b1
      equivalence(b,k)
      k = i
      b1 = b(1)
      b(1) = b(2)
      b(2) = b1
      iswap = k
      return
      end      
c
c    ----------------------
      integer*4 function iswap256(i)
c    ----------------------------      
c     converts two bytes to an unsigned integer,
c     it must be an integer*4 becuase of the missing sign!
c
      integer*4 i,k
      byte b(4),b1
      equivalence(b,k)
      k = i
      b1 = b(1)
c      b(1) = b(2)*256
      b(1) = b(2)
      b(2) = b1
      b(3) = 0
      b(4) = 0
      iswap256 = k
      return
      end 
c    ----------------------
      integer*4 function iswap16(i)
c    ----------------------------      
c     converts one byte to an unsigned integer,
c     it must be an integer*4 becuase of the missing sign!
c
      integer*4 i,k
      byte b(4)
      equivalence(b,k)
      k = i
c      b1 = b(1)
      b(1) = b(2)
      b(2) = 0
      b(3) = 0
      b(4) = 0
      iswap16 = k
      return
      end 
c     -------------------------
c
      subroutine split(i,i1,i2)
c     -------------------------
c  splits i into first and second byte
c
      integer*2 i,k
      integer*1 b(2),i1,i2
      equivalence(b,k)
      k = i
      i1 = b(1)
      i2 = b(2)
      return
      end
c
      integer*4 function long(i1,i2)
c     ------------------------------
c  combines two EGI words into one integer*4
c
      integer*2 i1,i2,j(2)
      integer*4 k
      equivalence(j,k)
      j(2) = iswap(i1)
      j(1) = iswap(i2)
      long = k
      return
      end