program readegi6
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c   VERSION TO GET EGI TIME TAG FROM GPS word 2
c   UPDATED BY K.KELLER MAY 2003
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)
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  -----  to run past midnight, avo Aug 05
c   info about EGI data: 'EGIcon.doc' June 20, 2000 by Kim Woelders
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      implicit real*8(a-h,o-z)
      logical lalign,lstart,lout,louth,lvel
      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
      write(*,*) 'input: EGI file / subw,dt,t1,t2,lvel'
      read(*,'(a)') ifile
      read(*,*) subw, dt, t1, t2, lvel
c
      write(*,*)dt
      open(10,file=ifile,form='unformatted',access='direct',recl=2)
      open(20,file='readegi.coo',status='unknown')
c   new tt test file 
      open(30,file='tt.txt',status='unknown')
c   new tt word 8 test file
      open(40,file='tt8.txt',status='unknown')
c   new utc time check file
      open(50,file='utc.txt',status='unknown')
      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 = 8192.d0*0.3048/max4
c  acc
      fakacc = 1023.97d0*0.3048/max2
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
c      utc8p = 0
c      tt8p = 0
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     2 lines added to to handle midnight problem, avo Aug 05
      call split(hdr(10),iday,ihr)
      if (n17.lt.3.and.n29.lt.3) 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
c      thr = ihr + imin/60.d0 + isec/3600.d0 + ms/3600000.d0
      if (lstart) then
        ihr1 = ihr
        imin1 = imin
        thr1 = thr
        thrp = thr
        write(*,14) iyr+2000,imon,iday,ifirstday,subw
14      format(' --- READEGI ---'/' Date: ',i4,i3,i3,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       write(*,*)'sub,len',sub, len
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
c  * omitted due to tt test sub 8 - moved to end of 2*ENDIF
c
      if (sub.ne.subw.and.sub.ne.8) then
        irec = irec+len
        goto 10
      endif
c     if (sub.ne.subw) then
c        irec = irec+len
c        goto 10
c     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
      iutc = long(w(27),w(28))
      utc = iutc*faku
c
      tt1 = real(iswap256(w(2)))
c  
      ttp=tt
      tt = tt1*faktt
c   test of UTC and EGI Time tag
c      write(*,*)utc, tt
c
      vz = long(w(7),w(8))*fakv
      mode2 = iswap(w(29))
      if (lvel) then
        azim = iswap(w(9))*fakr
        vx = long(w(3),w(4))*fakv
        vy = long(w(5),w(6))*fakv
        ax = iswap(w(14))*fakacc 
        ay = iswap(w(15))*fakacc 
        az = iswap(w(16))*fakacc 
        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     
      ENDIF
c
c     message 8 is GPS data:
c     UTC time tag: word (19/20)
c     and EGI time tag: word 2 (time of validity)
c
      IF (SUB.EQ.8) THEN
c      tt8p = tt8
c      utc8p = utc8
c       write(*,*)'sub=8  ',sub,len
       nsub8 = nsub8+1
       do j= 1,len  
        irec = irec+1
        read(10,rec=irec,err=90) w8(j)
       enddo
       iutc8 = long(w8(19),w8(20))
       utc8 = iutc8*faku
       tt8 = real(iswap256(w8(2)))
       tt8 = tt8*faktt
       imw2 = real(iswap16(w8(1)))
       write(40,*)utc8,tt8,imw2
      ENDIF
c
c  Correction of time done by EGI time tag:
c     utc(17/29) = utc(8) + (EGItt(17/29) - EGItt(8)) + bias?
c
      bias = 0.0d0
      if (tt-tt8.lt.0) bias = 4.194240d0  
      utcc = utc8 + tt - tt8 + bias
      write(50,55) utc,utcc,utc-utcc,tt - tt8
55    format(4(f12.5,2x))
c     thr=utcc/3600.0d0 
      utc=utcc
c      thru = utc/3600.0d0 + 24.0d0*(iday - ifirstday)
c  integrate vertical velocity
c
      IF (nsub8.ge.2) THEN
       if (nsub8.eq.2) vzint=alt
       thru = utc/3600.0d0 + 24.0d0*(iday - ifirstday)
       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 = vzint + dtu*vz
       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
        write( *,20) thru,rlat,rlon,alt,pitch,roll,hdg,
     .  mode2char,ifom1,ifom2,ifom3,mode
20      format(' ',f10.7,f12.7,f13.7,f9.1,2f7.2,f8.2,' ',a3,' ',3i1,i5)
      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
      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,2x,f10.2)
c         thrup=thru
C   test output
c       if (tt-ttp.lt.-4.0) write(40,*)utc,tt-ttp
       write(30,*) utc,tt,tt-ttp
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,3f11.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) then
          dthrmax = thr-thrp
          ihrmax = ihr
          iminmax = imin
        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(*,*)
     .    '   thr         lat         lon          h     pitch   roll',
     .    '    hdg      FOM mode'
          ihru2 = thru
          iminu2 = (thru-ihru2)*60
        endif
      endif
      if (lstart) then
        ihru1 = thru
        iminu1 = (thru-ihru1)*60
        lstart = .false.
      endif
      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,
     .ihru1,iminu1,ihru2,iminu2
92    format(' Start pc time  ',i2,':',i2,', stop ',i2,':',i2,
     .', duration:',f7.2,' min'/
     .' Start GPS time ',i2,':',i2,', nav ',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: ',4i8)
      write(*,94) nsub/(thr-thr1)/3600,dthrmax*3600,ihrmax,iminmax
94    format(' Average logging rate:',f5.1,' Hz, max time break:'
     .,f8.2,' sec at ',i2,':',i2)
      write(*,*) ifirstday
      write(*,*) iday
      write(*,*) thru
      write(*,*) thrup
      write(*,*) t2
      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