      PROGRAM AccelTestLong

C Program to read a fast tape file and extract data into a comma
C   delimited file for Lockheed fatigue life analysis study
C Long Accel Filter Test version - do whole flight

      IMPLICIT none

      INCLUDE "FastCom.ftni"

      INTEGER InLu, OutLu, ios, BufSize, i,j,PCnt,NzIdx
      INTEGER Buf_Cnt,Vel_Cnt, NumBad
      REAL NzMax, Start_Alt
      CHARACTER*64 InFile, OutFile
      LOGICAL Prelim

      PARAMETER (InLu=35)  ! arbitrary #, Out will be 33

C Do some setup stuff
      DO i = 1,10
        INE_Over1(i) = 0  ! init to no overrun
        INE_Over2(i) = 0
      END DO
      VzLast = -999.  ! flag for no last vert speed
      NzLast = 0.     ! init vert accel
      VzJerk = 0.     ! init change in accel (jerk)
      NVLast = -999.  ! same for other 'last' values
      NALast = 0.
      NJerk = 0.
      EVLast = -999.
      EALast = 0.
      EJerk = 0.
      VVLast = -999.
      VALast = 0.
      VJerk = 0.
      Time_Offset = 0  ! show that we haven't started data timing

C ask for and open the Setup file
      CALL GetSetupFile(InFile)
      VAoff_orig = VAoff  ! save so VelFilt can modify VAoffset

      OPEN(InLu,FILE=InFile,form='unformatted',
     &     access='direct',recl=1,ERR=999,IOSTAT=ios)
      RecNum_com = 1  ! pointer into input file

      Prelim = .TRUE.  ! need to wait for taxi
      CALL ReadBuf(InLu,BufSize)  ! so we can enter the do loop

C Loop on each 1 sec record
      DO WHILE (BufSize.GT.0)

        Bad_Sec = '      '   ! always assume it will be good

C Do some stuff at the start for each record
        RA_OK = .FALSE.   ! will be set by INE_cnvt or Extract_RA
        IF (Prelim) Init = .TRUE.  ! always do init stuff if not flying
C calc new time in seconds, see if we skipped
        Time_Now = IB(6)*3600 + IB(7)*60 + IB(8) - Time_Offset
        IF (Time_Now.LT.0) Time_Now = Time_Now + 86400  ! next day rollover

        IF (.NOT.Prelim .AND.               ! only if we're flying
     &      (Time_Now.GT.Time_Last+1 .OR.   ! more than 1 sec jump
     &      Time_Now.LT.Time_Last)) THEN    ! or neg time jump
          WRITE(*,'("Time Jump - Last:"i7" Current:"i7)')
     &              Time_Last, Time_Now
          Init = .TRUE.  ! so we reset all filters during this record
        ENDIF

C Decode the pointers and starting loc for each data type
        CALL Decode_Sec

C Extract INE data, specifically 3D speeds and 3D attitude
        CALL Extract_INE  ! from main buf to raw, with 39/40 adj
        CALL Cnvt_INE     ! from raw to engr units, fill NVel, etc

C If we want APN232 Radar Altimeter, now's the time
        IF (RA_Sel.EQ.3) CALL Extract_RA232  ! overwrites Alt()
        IF (RA_Sel.EQ.6) CALL Extract_RA159S  ! overwrites Alt()

C We have enough decoded to check for movement
        IF (Prelim) THEN  ! still waiting for aircraft movement
          IF (SQRT(NVel(1)**2.+EVel(1)**2.).GT.30.) THEN  ! we're moving

            VSum1 = 0.0  ! init Vert vel for AVx->Vvel
            VSum2 = 0.0  ! this is for Vz summation
            Alt2VelFilt = 0.  ! vel trend filter from delta alt
            Acc2VelFilt = 0.  ! vel trend filter from integrated Vaccel
            RMS_Sum = 0.  ! init RMS error variables
            RMS_Cnt = 0   
            Buf_Cnt = 0  ! # of seconds we've processed
            Vel_Cnt = 0
            IF (RA_Sel.EQ.0) THEN  ! save PAlt
              Start_Alt = PAlt(1)  ! for end printout
            ELSE  ! save RA Alt
              Start_Alt = Alt(1)
            ENDIF

c diag
            OPEN(unit=26,file='VelList.txt',status='new')
            WRITE(26,'("Time  AC_vel  PA_vel  ACFilt   ",
     &                 "PAFilt  Diff  Tweak  Integr  Deriv")') 

            CALL Create_OutFile(OutLu)  ! create & open an output file
            CALL Init_Hdr(OutLu)  ! write hdr lines, do other init
            Prelim = .FALSE.  ! only do it once, keep init for 1 more sec
          ENDIF
        ENDIF  ! done if we're not flying
        IF (.NOT.Prelim) THEN  ! we're moving, do everything else

C Extract PQ and PS, then calc the corrected values & IAS
          CALL Extract_ADC(PQchan,PQoff,PQslp,PQarray)
          CALL LimitTest(PQarray,150.,-1.,NumBad,'    PQ')
          IF (NumBad.LE.30)
     &      CALL LinFilt(PQarray,6.0,'    PQ')

          CALL Extract_ADC(PSchan,PSoff,PSslp,PSarray)
          CALL LimitTest(PSarray,1050.,100.,NumBad,'    PS')
          IF (NumBad.LE.30)
     &      CALL LinFilt(PSarray,5.0,'    PS')

          CALL Calc_IAS  ! from PQ, PS and the error constants
          CALL LinFilt(PAlt,100.,'  PAlt')
          IF (RA_Sel.EQ.0) RA_OK = .TRUE.  ! want to use PAlt

C Calculate the aircraft-referenced vert velocity, then horiz accels
          IF (VAchan.GT.0) THEN  ! want to use old analog accel
            CALL Extract_ADC(VAchan,VAoff,VAslp,VAcc)  ! get the accel data
            CALL LimitTest(VAcc,7.,-5.,NumBad,'  VAcc')
            IF (NumBad.LE.30)
     &        CALL LinFilt(VAcc,.6,'  VAcc')

            IF (.NOT.Init)
     &        CALL Accel2Vel(VSum1,VAcc,VVel)  ! convert to VVel, overwrite INE
            Vel_Cnt = Vel_Cnt + 40  ! # of samples added to running total
            CALL VelFiltTest  ! adjust Accel cal dynamically - test version
          ELSE  ! need to fill in VAcc
            CALL Vel2Accel(VVLast,VJerk,VALast,VVel,VAcc)
          ENDIF
          CALL Vel2Accel(NVLast,NJerk,NALast,NVel,NAcc)  ! Always calc these
          CALL Vel2Accel(EVLast,EJerk,EALast,EVel,EAcc)

C Calc Nz
          IF (VAchan.GT.0) THEN  ! calc Nz directly from Accels
            CALL Earth2Acft(Hdg,Pitch,Roll,NAcc,EAcc,VAcc,Nz)
c           CALL Accel2Vel(VSum2,Nz,Vz)  ! does integration, so spike is forever
c until I get spikes removed, need to calc Vz from earth velocities
             CALL Earth2Acft(Hdg,Pitch,Roll,NVel,EVel,VVel,Vz)
          ELSE  ! RINU - calc Vz 1st, then Nz
            CALL Earth2Acft(Hdg,Pitch,Roll,NVel,EVel,VVel,Vz)
            CALL Vel2Accel(VzLast,VzJerk,NzLast,Vz,Nz)
          ENDIF

C Diag print, in case we want to abort
          IF (Buf_Cnt.LT.50)
     &      WRITE(*,'(i2,":",i2.2,":",i2.2" Nv:"f6.1" Ev:"f6.1
     &              " Vv:"f6.2" PQ:"f5.1" PS:"f7.1" Hdg:"f6.1
     &              " Pitch:"f5.1" Roll:"f5.1" Vacc:"f6.3
     &              " Vz:"f6.2" Nz:"f6.3)') 
     &       IB(6),IB(7),IB(8),NVel(1),EVel(1),VVel(1),
     &       PQArray(1),PSArray(1),Hdg(1)*57.29,Pitch(1)*57.29,
     &       Roll(1)*57.29,VAcc(1),Vz(1),Nz(1)

c          IF (Buf_Cnt.GT.100) STOP

          CALL Write1Sec(OutLu)  ! write 1 sec of data, this tests init & Ra_Ok

        ENDIF

        Init = .FALSE.  ! start clean next sec, unless...

C Things we found this second that makes us want to do init next sec
C If we don't have 40 samples, or something flags the sec to not print
        IF ((INE_Sel.EQ.1.AND.INE_Cnt1.LT.40) .OR.
     &      (INE_Sel.EQ.2.AND.INE_Cnt2.LT.40) .OR.
     &      (.NOT.RA_OK).OR.Bad_Sec.NE.'      ')
     &    Init = .TRUE.

        Time_Last = Time_Now  ! always do this just before next read

        CALL ReadBuf(InLu,BufSize)  ! always try for next read
        Buf_Cnt = Buf_Cnt + 1  ! bump # of seconds

c Diag - quit after 6000 seconds
c        IF (Buf_Cnt.GE.6000) BufSize = 0
      END DO  ! drop through at EOF

      CLOSE(InLu)  ! clean up file access
      CLOSE(OutLu)

c do closeout printing
      WRITE(*,'(i2,":",i2.2,":",i2.2" Nv:"f6.1" Nacc:"f6.3
     &          " Ev:"f6.1" Eacc:"f6.3" Vv:"f7.1" Vacc:"f6.3
     &          " Hdg:"f6.1" Ptch:"f5.1" Roll:"f5.1
     &          " Vz:"f7.1" Nz:"f6.3)') 
     &   IB(6),IB(7),IB(8),NVel(1),NAcc(1),EVel(1),EAcc(1),
     &   VVel(1),VAcc(1),Hdg(1)*57.29,Pitch(1)*57.29,
     &   Roll(1)*57.29,Vz(1),Nz(1)
      IF (RA_Sel.EQ.0) THEN  ! display PAlt
        WRITE(*,'("Start PAlt:"f6.1" End PAlt:"f6.1
     &            " Vel_Cnt:"i8" VelCorr:"f12.9)') Start_Alt,
     &            PAlt(1), Vel_Cnt, -VVel(40)/Float(Vel_Cnt)
      ELSE
        WRITE(*,'("Start Alt:"f6.1" End Alt:"f6.1
     &            " Vel_Cnt:"i8" VelCorr:"f12.9)') Start_Alt,
     &            Alt(1), Vel_Cnt, -VVel(40)/Float(Vel_Cnt)
      ENDIF
      WRITE(*,'("VVel RMS Error:"f15.4)') SQRT(RMS_Sum/Float(RMS_Cnt))

      STOP

999   WRITE(*,'("Error while opening file: ",a," IOSTAT="*)')
     & InFile, ios
      STOP

      END

      INCLUDE "GetSetupFile.ftni"
      INCLUDE "Create_OutFile.ftni"
      INCLUDE "ReadBuf.ftni"
      INCLUDE "Decode_Sec.ftni"
      INCLUDE "Extract_INE.ftni"
      INCLUDE "Cnvt_INE.ftni"
      INCLUDE "PairCheck.ftni"
      INCLUDE "LimitTest.ftni"
      INCLUDE "Extract_ADC.ftni"
      INCLUDE "Accel2Vel.ftni"
      INCLUDE "Calc_IAS.ftni"
      INCLUDE "Vel2Accel.ftni"
      INCLUDE "Earth2Acft.ftni"
      INCLUDE "Init_Hdr.ftni"
      INCLUDE "Write1Sec.ftni"
      INCLUDE "Extract_RA232.ftni"
      INCLUDE "Extract_RA159S.ftni"
      INCLUDE "LinFilt.ftni"
      INCLUDE "VelFiltTest.ftni"


