SUBROUTINE VelFiltTest

c Subroutine to do a filter on the vertical acceleration.
c   Calculates a vert vel based on change in altitude over the sec
c   Then updates a filtered vertvel for a long term average
c   Next, take the average vert vel previously computed by
c     integrating Vert Accel and update the 2nd long term filter
c For initial investigation, compute a 1 Hz avg error
c
c  Access to Common is for time stamp in diag print, and Init
c
c  Test version does printout to LU 26

      IMPLICIT none

      INCLUDE "FastCom.ftni"

C Local storage
      INTEGER i, NumGood
      REAL FiltLen,Vel_PA,Vel_Ac, Diff
      REAL PA_lcl(10),AC_lcl(10),DiffOld(45)
      REAL S_XiYi, S_Xi, S_Yi, S_Xi_2, RGood, RTmp
      REAL Deriv, Tweak, MaxTweak
      LOGICAL Init_lcl, First
      SAVE DiffOld

      DATA Init_lcl/.TRUE./
      DATA FiltLen/.9/  ! set length very long
      DATA Tweak/0.0/
      DATA MaxTweak/.005/

      IF (Init) Init_lcl = .TRUE.  ! if main system reset, so do we

C Do the Vert Vel from Altitude
      Vel_PA = PAlt(40) - PAlt(1)  ! in m/s because PAlt is meters
      IF (Vel_PA.LT.-100. .OR. Vel_PA.GT.100.) Vel_PA=-999.
C Vert Vel from Accel is computed, take the avg over the sec
      Vel_AC = (VVel(40)+VVel(1))/2.  ! avg, already m/s
      IF (Vel_AC.LT.-100. .OR. Vel_AC.GT.100.) Vel_AC=-999.

C Now do the filtering - boxcar avg
c      IF(Init_lcl) THEN ! 1st time, initialize array
c        Diff = 0.0
c        DO i = 1,15
c          PA_lcl(i) = Vel_PA
c          AC_lcl(i) = Vel_AC
c        END DO
c      ELSE  ! normal second
c        DO i = 1,9  ! move everything up
c          PA_lcl(i+1) = PA_lcl(i)
c          AC_lcl(i+1) = AC_lcl(i)
c        END DO
c        PA_lcl(1) = Vel_PA  ! fill in new values
c        AC_lcl(1) = Vel_AC
c      ENDIF  ! either way, arrays are full
c      Alt2VelFilt = 0.0  ! get ready to sum
c      Acc2VelFilt = 0.0
c      DO i = 1,10
c        Alt2VelFilt = Alt2VelFilt + PA_lcl(i)
c        Acc2VelFilt = Acc2VelFilt + AC_lcl(i)
c      END DO
c      Alt2VelFilt = Alt2VelFilt/10.  ! finish the avg
c      Acc2VelFilt = Acc2VelFilt/10.

c Autoregressive version
      IF (Init_lcl) THEN
        IF (Vel_PA.GT.-900. .AND. Vel_AC.GT.-900.) THEN  ! only if both good
          Alt2VelFilt = Vel_PA
          Acc2VelFilt = Vel_AC
        ELSE
          Alt2VelFilt = 0.  ! something safe
          Acc2VelFilt = 0.
        ENDIF

      ELSEIF (Vel_PA.GT.-900. .AND. Vel_AC.GT.-900.) THEN  ! only do the rest if both good
        Alt2VelFilt = FiltLen*Alt2VelFilt + (1-FiltLen)*Vel_PA
        Acc2VelFilt = FiltLen*Acc2VelFilt + (1-FiltLen)*Vel_AC

C Add this second's error to RMS sum
        RMS_Sum = RMS_Sum + (Vel_AC-Vel_PA)**2.
        RMS_Cnt = RMS_Cnt + 1

C This is where we adjust the offset of the Accel equation
        Diff = Acc2VelFilt-Alt2VelFilt

c Move new Diff into rolling archive
        DO i = 44,1,-1
          DiffOld(i+1) = DiffOld(i)
        END DO
        DiffOld(1) = Diff
c Calc Derivative by doing least squares fit to Diff array
C Do the initial summations
        NumGood = 0  ! start w/no good data points
        S_XiYi = 0.  ! getting ready for sums ! This is Sum(XiYi)
        S_Xi = 0.    ! Sum(Xi)                ! Xi's are the Freqs
        S_Yi = 0.    ! Sum(Yi)                ! Yi's are the tb's
        S_Xi_2 = 0.  ! Sum(Xi^2)
 
        DO i = 1,45    ! loop through all data points
          IF (DiffOld(i).GT.-900.) THEN  ! only sum in the acceptable ones
            NumGood = NumGood + 1   ! bump count of good samples
            S_XiYi = S_XiYi + Float(i)*DiffOld(i)
            S_Xi = S_Xi + Float(i)
            S_Yi = S_Yi + DiffOld(i)
            S_Xi_2 = S_Xi_2 + Float(i*i)
          ENDIF
        END DO

        RGood = Float(NumGood)  ! easier to do once
C Do the Slope Calc
        Deriv = RGood*S_XiYi - S_Xi*S_Yi    ! Numerator nS(XiYi)-S(Xi)S(Yi)
        RTmp = RGood*S_Xi_2 - S_Xi*S_Xi     ! Denominator nS(Xi^2)-S(Xi)^2
        IF (RTmp.NE.0.) THEN                ! guard against /0
          Deriv = Deriv/RTmp
        ELSE
          Deriv = 0.                        ! dummy value
        ENDIF

c        IF (DiffOld(1).GT.-900. .AND. DiffOld(2).GT.-900. .AND.
c     &      DiffOld(3).GT.-900. .AND. DiffOld(43).GT.-900. .AND.
c     &      DiffOld(44).GT.-900. .AND. DiffOld(45).GT.-900.)
c     &    Deriv = (DiffOld(1)+DiffOld(2)+DiffOld(3))/3. - 
c     &            (DiffOld(58)+DiffOld(59)+DiffOld(60))/3.
        IntegDiff = IntegDiff + Diff

c calc tweak
        Tweak = Kprop*Diff + Kinteg*IntegDiff + Kderiv*Deriv
        IF (Tweak.GT.MaxTweak) Tweak = MaxTweak
        IF (Tweak.LT.-MaxTweak) Tweak = -MaxTweak
        VAoff = VAoff_orig - Tweak
      ENDIF
      
c diag print
c      IF (Time_Now.LT.6000)
        WRITE(26,'(i6.6,4f9.2,2f11.5,2f11.3)')
     &     Time_Now,Vel_AC,Vel_PA,Acc2VelFilt,
     &     Alt2VelFilt,Diff,Tweak,IntegDiff,
     &     Deriv*10.

      Init_lcl = .FALSE.  ! if sys init, we'll set again next time

      RETURN

      END