c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: A general purpose routine to digitally filter a set of data.                  
c              Zero Pad data so that the number of data points is a power of 2, apply
c              a window function to the data since it is assumed to be nonperiodic,
c              take the fourier transform of the result then multiply it with the filter
c              function, and take the inverse transform. Note that the filter function
c              must have been previously built using the routine BuildFilter.
c
c     Routines called:
c       realft
c
      subroutine DigFilterDataSet( Data,NumPnts,
     &                             NumFilPnts,NumBeginZero,NumEndZero,
     &                             Filter,DoWindow,BlankEnds,NumBlank )
      implicit none
               
      real*4    Data(*),Filter(*)
      integer*4 NumPnts,NumFilPnts,NumBeginZero,NumEndZero,NumBlank
      logical*4 DoWindow,BlankEnds

      real*4    Data2(2048),pi,w(2048)
      integer*4 i,j
                                 
      pi= 4.0*atan(1.0)
c
c     Apply window function to data- at present only a Hanning window is
c     supported.  Other window functions may be added as command options
c     later on...  
c    
      if ( DoWindow ) then     
        do i= 1, NumPnts
          w(i)= 0.5*(1.0-cos((2.0*pi*i)/float(NumPnts-1)))
          Data(i)= Data(i)*w(i)                           
        end do              
      end if
c
c     Zero pad data
c                              
      i= 1            
      do j= 1, NumFilPnts         ! create zero padded data array
        if ( j .le. NumBeginZero ) then
           Data2(j)= 0.0      
        else if ( j .gt. (NumBeginZero+NumPnts) ) then
           Data2(j)= 0.0
        else              
           Data2(j)= Data(i)
           i= i + 1
        end if
      end do ! j              
c         
c     Now do the filtering operation finally.  Take the FFT of the zero
c     padded and windowed data, multiply it with the filter function and
c     then inverse FFT the result and extract the filtered data.
c                              
      call realft( Data2,NumFilPnts/2,1 )
      do i= 1, NumFilPnts/2
        do j= 1, 2
          Data2(2*(i-1)+j)= Data2(2*(i-1)+j)*Filter(i)
        end do                       
      end do ! i
      call realft( Data2,NumFilPnts/2,-1 )
      if ( DoWindow ) then     ! the 2.0* is to correct for the fact that the
        do i= 1, NumPnts       ! after filtering has 1/2 the magnitude that it
          j= i + NumBeginZero  ! originally had
          if ( w(i) .gt. 1.0e-5 ) then
            Data(i)= 2.0*Data2(j)/(float(NumFilPnts)*w(i))
          else                       
            Data(i)= 2.0*100000.*Data2(j)/float(NumFilPnts)
          end if 
        end do ! i        
      else                         
        do i= 1, NumPnts
          Data(i)= 2.0*Data2(i+NumBeginZero)/float(NumFilPnts)
        end do          
      end if ! DoWindow                                            
      if ( BlankEnds ) then
        do i= 1, NumBlank
          Data(i)= Data(NumBlank+1)
          Data(NumPnts+1-i)= Data(NumPnts-NumBlank)
        end do
      end if
      return      
      end
c
c
c     Purpose: Provide a shell to branch to digital or time based convolution
c              filtering techniques
c
      subroutine FilterEMG( CMND,EF,OF )
      implicit none      

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
                                           
      record /CmndLine/    CMND
      record /EMG_flag/    EF
      record /option_flag/ OF

      integer*4 i,optnum
      logical*4 UseTCV

      if ( .not. EF.read ) goto 1000
      if ( EP.SampleRate .lt. 1.0e-5 ) goto 1010 ! error
      if ( EP.NumSamples .lt. 2 ) goto 1020 ! error
      UseTCV= .false.
      do i= 1, CMND.NumOpt(2)
        if ( CMND.OptText(i,2) .eq. 'TCV' ) then
          UseTCV= .true.
          optnum= i
        endif           
      end do
      if ( UseTCV ) then
        call RemoveCMNDOpt( CMND,2,optnum )
        call ConvolveEMG( CMND,EF,OF )
      else
        call DigitalFilterEMG( CMND,EF,OF )
      end if
      return

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- EMG data has not been read in yet')
      return

1010  write(6,1015) EP.SampleRate
      if ( OF.ToLog ) write(3,1015) EP.SampleRate
1015  format(' Error- EMG sample rate is ',f10.6,' Hz which is too small.'/
     &       '        Something has gone wrong in reading the EMG file')
      return
      
1020  write(6,1025) EP.NumSamples
      if ( OF.ToLog ) write(3,1025) EP.NumSamples
1025  format(' Error- There are only ',i4,' samples of data in each EMG channel'/
     &       '        There must be at least 2 samples to perform filtering')
      return
      end
c
c
c     Purpose: Carry out a time based convolution on the emg signals
c
      subroutine ConvolveEMG( CMND,EF,OF )
      implicit none      

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_emg.inc'
                                           
      record /CmndLine/    CMND
      record /EMG_flag/    EF
      record /option_flag/ OF

      integer*4 ftype,tcvorder,sftype,stcvorder,SFNumChnl,SFChnlNum(MAX_EMG),
     &          SFCmndNum(MAX_EMG),i,NumPnts,chnl,cn,SFChnl,conv_fltr,ierr
      real*4    sf,lf,hf,slf,shf
      logical*4 SpecialFilter,error

      sf= EP.SampleRate
      lf= 0.0
      hf= 0.0                                             
      ftype= 0
      tcvorder= 1
c
c     check for additional entires in the command line. If there are more
c     than 2 commands then the remaining commands are presumed to be filter
c     parameters for specific channels.
c                                  
      SFNumChnl= 0         
      SpecialFilter= .false.
      if ( CMND.NumCmnd .gt. 2 ) then                       
        do i= 1, CMND.NumCmnd-2       
          if ( .not. CMND.CmndNumOK(i+2) ) goto 2000        
          if ( (CMND.CmndIntVal(i+2) .lt. 1) .or.
     &         (CMND.CmndIntVal(i+2) .gt. EP.NumChnl) ) goto 2010
          SpecialFilter= .true.
          SFNumChnl= SFNumChnl + 1
          SFChnlNum(SFNumChnl)= CMND.CmndIntVal(i+2)
          SFCmndNum(SFNumChnl)= i+2
        end do              
      end if      
c
c     Setup the convolution filter parameters
c
      if ( .not. SpecialFilter ) then
        do i= 1, CMND.NumOpt(2)
          call CheckTcvOpt( CMND,2,i,lf,hf,tcvorder,ftype,OF.ToLog,error )
          if ( error ) goto 2030
        end do
      end if
c
c     Convolve each channel using command line parameters
c                        
      NumPnts= EP.NumSamples
      do chnl= 1, EP.NumChnl

        if ( OF.UpDate .and. (.not. SpecialFilter) ) 
     &    write(6,100) Chnl,EP.NumChnl
100     format('+ Filtering EMG Channel # ',i2,'/',i2,'               ')

        if ( SFNumChnl .gt. 0 ) then
          cn= 0
          do i= 1, SFNumChnl
            if ( chnl .eq. SFChnlNum(i) ) then
              cn= SFCmndNum(i)
              SFChnl= SFChnlNum(i)
            end if
          end do
          if ( cn .ne. 0 ) then
            if ( OF.UpDate ) write(6,101) Chnl,EP.NumChnl
101         format('+ Filtering EMG Channel # ',i2,'/',i2,' using special')
            slf= lf
            shf= hf
            stcvorder= tcvorder
            sftype= ftype
            do i= 1, CMND.NumOpt(cn)
              call CheckTcvOpt( CMND,cn,i,slf,shf,stcvorder,sftype,
     &                          OF.ToLog,error )
              if ( error ) goto 2040
            end do
            ierr= conv_fltr( EMGData(1,chnl),NumPnts,sf,slf,shf,sftype,stcvorder )
            if ( ierr .ne. 0 ) goto 1050 ! error
          else
            if ( OF.UpDate ) write(6,100) Chnl,EP.NumChnl
            ierr= conv_fltr( EMGData(1,chnl),NumPnts,sf,lf,hf,ftype,tcvorder )
            if ( ierr .ne. 0 ) goto 1050 ! error
          end if

        else
          ierr= conv_fltr( EMGData(1,chnl),NumPnts,sf,lf,hf,ftype,tcvorder )
          if ( ierr .ne. 0 ) goto 1050 ! error
        end if

      end do                                           
c
c     Finish up
c
      EP.Filter= CMND.Text(1)(CMND.OptStart(1,2):
     &                             CMND.OptStart(1,2)+39)
      if ( EF.NumProcOps .lt. 5 ) then
        EF.NumProcOps= EF.NumProcOps + 1
        EF.ProcOp(EF.NumProcOps)= CMND.Text(1)(1:40)
      end if
      EF.filtered= .true.
      return

1050  write(6,1055) chnl
      if ( OF.ToLog ) write(3,1055) chnl
1055  format(' Error- Convolution filter failed at EMG channel ',i2)
      call conv_fltr_err( ierr,OF.ToLog )
      return

2000  write(6,2005) CMND.CmndNumOK(i+2),EP.NumChnl
      if ( OF.ToLog ) write(3,2005) CMND.CmndNumOK(i+2),EP.NumChnl
2005  format(' Error- ',a3,' is not a valid channel number'
     &       '        Valid channels are 1 to ',i2)
      return
   
2010  write(6,2015) CMND.CmndIntVal(i+2),EP.NumChnl
      if ( OF.ToLog ) write(3,2015) CMND.CmndIntVal(i+2),EP.NumChnl
2015  format(' Error- ',i3,' is not a valid channel number'
     &       '        Valid channels are 1 to ',i2)
      return

2030  write(6,2035) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,2035) CMND.OptText(i,2)
2035  format(' Error - ',a3,' is not a valid time convolution filter option'/
     &       '         Valid options are: /LOW=#  /HIGh=#  /ORDer=#'/
     &       '                            /BUTterworth /CHEbeyschev')
      return                        

2040  write(6,2045) CMND.OptText(i,cn),SFChnl
      if ( OF.ToLog ) write(3,2045) CMND.OptText(i,cn),SFChnl
2045  format(' Error - ',a3,' is not a valid time convolution filter option'/
     &       '         for special filter EMG channel # ',i2/
     &       '         Valid options are: /LOW=#  /HIGh=#  /ORDer=#'/
     &       '                            /BUTterworth /CHEbeyschev')
      return                        
      end
c
c
c                            
c                          
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Digitally filter all the EMG data in the frequency domain. The
c              process followed to do this is:
c              Zero Pad data so that the number of data points is a power of 2, apply
c              a window function to the data since it is assumed to be nonperiodic,
c              take the fourier transform of the result then multiply it with the filter
c              function, and take the inverse transform. Note that the frequency domain
c              filter function is built up in the routine BuildFilter from the
c              command options. Also, normally the same window function is applied
c              to all channels of EMG but it is possible to apply a specially filter
c              function to selected channels by adding the channel numbers after the
c              EMG FIL command and indicating the filter function as an option to
c              the channel number: For example, a 20 Hz low cutoff and 250 Hz high 
c              cutoff to all channels would be (good settings for surface EMG):
c                EMG FIL/LOW=20/HIG=250
c              A special case where there were wire EMG's on channels 5 and 7 and 
c              standard surface electrodes on the other channels would be :
c                EMG FIL/LOW=20/HIG=250 5/LOW=20/HIG=1000 7/LOW=20/HIG=1000
c
c     Routines called:
c       BuildFIlter
c       realft
c
      subroutine DigitalFilterEMG( CMND,EF,OF )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_emg.inc'

      record /CmndLine/    CMND
      record /EMG_flag/    EF
      record /option_flag/ OF
                  
      integer*4  MAX_PNT   
      parameter (MAX_PNT= 8192 )
                     
      integer*4 i,j,n,NumPnts,NumBeginZero,NumEndZero,Chnl,SFNumChnl,
     &          SFChnlNum(MAX_EMG),sfn,NumBlank,SFNumBlank(MAX_EMG)
      real*4    Data2(2*MAX_PNT),Filter(MAX_PNT),pi,smplfreq,w(2*MAX_PNT),
     &          SfFilter(MAX_PNT,MAX_EMG)
      logical*4 Error,DoWindow,SfDoWindow(MAX_EMG),SpecialFilter,BlankEnds,
     &          SFBlankEnds(MAX_EMG)
                           
c
c     check for additional entires in the command line. If there are more
c     than 2 commands then the remaining commands are presumed to be filter
c     parameters for specific channels.
c
      SFNumChnl= 0         
      SpecialFilter= .false.
      if ( CMND.NumCmnd .gt. 2 ) then                       
        do i= 1, CMND.NumCmnd-2       
          if ( .not. CMND.CmndNumOK(i+2) ) goto 2000        
          if ( (CMND.CmndIntVal(i+2) .lt. 1) .or.
     &         (CMND.CmndIntVal(i+2) .gt. EP.NumChnl) ) goto 2010
          SpecialFilter= .true.
          SFNumChnl= SFNumChnl + 1
          SFChnlNum(SFNumChnl)= CMND.CmndIntVal(i+2)
        end do              
      end if      
c
c     Figure out how many zeros must be added to data.  Zeros are added
c     symmetrically to the beginning and end of the array.  This should
c     take care of most of the end effects resulting from fourier transform
c     process.             
c                               
      pi= 4.0*atan(1.0)
      n= EP.NumSamples
      i= 1
      do while( (n - 2**i) .gt. 0 )
        i= i + 1
      end do         
      i= i + 1 ! extra padding added because of end effects, July 1992 DAM
      if ( i .gt. 14 ) goto 1030 ! error- too many samples, must be <=16384
      NumPnts= 2**i                                                
      NumBeginZero= (NumPnts-n)/2
      NumEndZero=   (NumPnts-n) - NumBeginZero                          
c                                                   
c     Build filter function
c                        
      smplfreq= float(EP.SampleRate)
      if ( SpecialFilter ) then
        do i= 1, SFNumChnl
          call BuildFilter( CMND,i+2,NumPnts,smplfreq,
     &                      SfFilter(1,i),SfDoWindow(i),
     &                      SFBlankEnds(i),SFNumBlank(i),Error,OF )
          if ( Error ) return
        end do
      end if
      call BuildFilter( CMND,2,NumPnts,smplfreq,
     &                  Filter,DoWindow,BlankEnds,NumBlank,Error,OF )
      if ( Error ) return        
c
c     Do Filtering of each EMG channel
c                                                                  
      EP.Filter= CMND.Text(1)(CMND.OptStart(1,2):
     &                             CMND.OptStart(1,2)+39)
      do Chnl= 1, EP.NumChnl
        SpecialFilter= .false.
        if ( SFNumChnl .gt. 0 ) then
          do i= 1, SFNumChnl
            if ( Chnl .eq. SFChnlNum(i) ) then
              SpecialFilter= .true.
              sfn= i
            end if
          end do
        end if
        if ( OF.UpDate ) then 
          if ( SpecialFilter ) then
            write(6,101) Chnl,EP.NumChnl
101         format('+ Filtering EMG Channel # ',i2,'/',i2,' using special')
          else
            write(6,100) Chnl,EP.NumChnl
100         format('+ Filtering EMG Channel # ',i2,'/',i2,'               ')
          end if
        end if
c
c       Zero pad data
c
        i= 1
        do j= 1, NumPnts         ! create zero padded data array
          if ( j .le. NumBeginZero ) then
             Data2(j)= 0.0      
          else if ( j .gt. NumBeginZero+n ) then
             Data2(j)= 0.0
          else
            Data2(j)= EMGData(i,Chnl)
            i= i + 1
          end if
        end do ! j              
c
c       Apply window function to data- at present only a Hanning window is
c       supported.  Other window functions may be added as command options
c       later on...               
c
        if ( SpecialFilter ) then
          if ( SfDoWindow(sfn) ) then
            do i= 1, EP.NumSamples
              w(i)= 0.5*(1.0-cos((2.0*pi*i)/(EP.NumSamples-1)))
              Data2(NumBeginZero+i)= Data2(NumBeginZero+i)*w(i)
            end do              
          end if
        else
          if ( DoWindow ) then
            do i= 1, EP.NumSamples
              w(i)= 0.5*(1.0-cos((2.0*pi*i)/(EP.NumSamples-1)))
              Data2(NumBeginZero+i)= Data2(NumBeginZero+i)*w(i)
            end do              
          end if
        end if
c           
c       Now do the filtering operation finally.  Take the FFT of the zero
c       padded and windowed data, multiply it with the filter function and
c       then inverse FFT the result and extract the filtered data.
c
        call realft( Data2,NumPnts/2,1 )
        if ( SpecialFilter ) then
          do i= 1, NumPnts/2
            do j= 1, 2
              Data2(2*(i-1)+j)= Data2(2*(i-1)+j)*SfFilter(i,sfn)
            end do
          end do ! i
        else
          do i= 1, NumPnts/2
            do j= 1, 2
              Data2(2*(i-1)+j)= Data2(2*(i-1)+j)*Filter(i)
            end do
          end do ! i
        end if
        call realft( Data2,NumPnts/2,-1 )
c
c       Filtering is done, now cleanup
c
        if ( SpecialFilter ) then
          if ( SfDoWindow(sfn) ) then              ! NOTE the 2.0* for scaling
            do i= 1, EP.NumSamples
              j= i + NumBeginZero
              if ( w(i) .gt. 1.0e-5 ) then
                EMGData(i,Chnl)= 2.0*Data2(j)/(float(NumPnts)*w(i))
              else
                EMGData(i,Chnl)= 2.0*100000.*Data2(j)/float(NumPnts)
              end if
            end do ! i                                                 
          else
            do i= 1, EP.NumSamples
              EMGData(i,Chnl)= 2.0*Data2(i+NumBeginZero)/float(NumPnts)
            end do
          end if ! DoWindow
        else
          if ( DoWindow ) then              ! NOTE the 2.0* for scaling
            do i= 1, EP.NumSamples
              j= i + NumBeginZero
              if ( w(i) .gt. 1.0e-5 ) then
                EMGData(i,Chnl)= 2.0*Data2(j)/(float(NumPnts)*w(i))
              else
                EMGData(i,Chnl)= 2.0*100000.*Data2(j)/float(NumPnts)
              end if
            end do ! i                                                 
          else
            do i= 1, EP.NumSamples
              EMGData(i,Chnl)= 2.0*Data2(i+NumBeginZero)/float(NumPnts)
            end do
          end if ! DoWindow
        end if
        if ( BlankEnds ) then
          do i= 1, NumBlank
            EMGData(i,chnl)= EMGData(NumBlank+1,chnl)
            EMGData(n+1-i,chnl)= EMGData(n-NumBlank,chnl)
          end do
        end if
      end do ! Chnl

      if ( EF.NumProcOps .lt. 5 ) then
        EF.NumProcOps= EF.NumProcOps + 1
        EF.ProcOp(EF.NumProcOps)= CMND.Text(1)(1:40)
      end if
      EF.filtered= .true.
      return
                                             
1030  write(6,1035) EP.NumSamples
      if ( OF.ToLog ) write(3,1035) EP.NumSamples
1035  format(' Error- There are ',i6,' samples per EMG channel, but at most'/
     &       '        16384 samples/channel can be filtered')
      return

2000  write(6,2005) CMND.CmndNumOK(i+2),EP.NumChnl
      if ( OF.ToLog ) write(3,2005) CMND.CmndNumOK(i+2),EP.NumChnl
2005  format(' Error- ',a3,' is not a valid channel number'
     &       '        Valid channels are 1 to ',i2)
      return
   
2010  write(6,2015) CMND.CmndIntVal(i+2),EP.NumChnl
      if ( OF.ToLog ) write(3,2015) CMND.CmndIntVal(i+2),EP.NumChnl
2015  format(' Error- ',i3,' is not a valid channel number'
     &       '        Valid channels are 1 to ',i2)
      return
      end   
c                    
c
c
      subroutine FilterFRC( CMND,FF,OF )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'

      record /CmndLine/    CMND
      record /force_flag/    FF
      record /option_flag/ OF

      integer*4 i,optnum
      logical*4 UseTCV

      if ( .not. FF.read ) goto 1000
      if ( FP.frm_rate .lt. 1.0e-5 ) goto 1010 ! error
      if ( FP.nfrm .lt. 2 ) goto 1020 ! error
      UseTCV= .false.
      do i= 1, CMND.NumOpt(2)
        if ( CMND.OptText(i,2) .eq. 'TCV' ) then
          UseTCV= .true.
          optnum= i
        endif           
      end do
      if ( UseTCV ) then
        call RemoveCMNDOpt( CMND,2,optnum )
        call ConvolveFRC( CMND,FF,OF )
      else
        call DigitalFilterFRC( CMND,FF,OF )
      end if
      return

1000  write(6,1005)                                     
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Force plate data has not been read in yet')
      return

1010  write(6,1015) FP.frm_rate
      if ( OF.ToLog ) write(3,1015) FP.frm_rate
1015  format(' Error- Force plate sample rate is ',f10.6,' Hz which is too small.'/
     &       '        Something has gone wrong in reading the force plate file')
      return                     
      
1020  write(6,1025) FP.nfrm
      if ( OF.ToLog ) write(3,1025) FP.nfrm
1025  format(' Error- There are only ',i4,' samples of data in each force channel'/
     &       '        There must be at least 2 samples to perform filtering')
      return
      end
c
c
c     Purpose: Carry out a time based convolution on the emg signals
c
      subroutine ConvolveFRC( CMND,FF,OF )
      implicit none      

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_gls2.inc'
      include 'a_frc.inc'
                                           
      record /CmndLine/    CMND
      record /force_flag/    FF
      record /option_flag/ OF

      integer*4 ftype,tcvorder,i,NumPnts,chnl,plt,conv_fltr,ierr
      real*4    sf,lf,hf,data(MAX_ELC)
      logical*4 error

      sf= FP.frm_rate
      lf= 0.0
      hf= 0.0                                             
      ftype= 0
      tcvorder= 1
c
c     Setup the convolution filter parameters
c
      do i= 1, CMND.NumOpt(2)
        call CheckTcvOpt( CMND,2,i,lf,hf,tcvorder,ftype,OF.ToLog,error )
        if ( error ) goto 2030
      end do
c
c     Convolve each channel using command line parameters
c                        
      NumPnts= FP.nfrm
      do plt= 1, FP.num_plt
        do chnl= 1, 6
          if ( OF.UpDate ) write(6,100) chnl,plt,FP.num_plt
100       format('+ Filtering Force plate channel # ',i1,'/6',
     &            '  Plate # ',i1,'/',i1)
          do i= 1, NumPnts
            data(i)= FpData(chnl,i,plt)
          end do
          ierr= conv_fltr( data,NumPnts,sf,lf,hf,ftype,tcvorder )
          if ( ierr .ne. 0 ) goto 1050 ! error
          do i= 1, NumPnts
            FpData(chnl,i,plt)= data(i)
          end do
        end do
      end do                                           
c
c     Finish up
c
      FF.filtered= .true. 
      FF.FiltOp= CMND.Text(1)(1:40) 
      return

1050  write(6,1055) chnl
      if ( OF.ToLog ) write(3,1055) chnl
1055  format(' Error- Convolution filter failed at EMG channel ',i2)
      call conv_fltr_err( ierr,OF.ToLog )
      return

2000  write(6,2005) CMND.CmndNumOK(i+2),EP.NumChnl
      if ( OF.ToLog ) write(3,2005) CMND.CmndNumOK(i+2),EP.NumChnl
2005  format(' Error- ',a3,' is not a valid channel number'
     &       '        Valid channels are 1 to ',i2)
      return
   
2010  write(6,2015) CMND.CmndIntVal(i+2),EP.NumChnl
      if ( OF.ToLog ) write(3,2015) CMND.CmndIntVal(i+2),EP.NumChnl
2015  format(' Error- ',i3,' is not a valid channel number'
     &       '        Valid channels are 1 to ',i2)
      return

2030  write(6,2035) CMND.OptText(i,2)
      if ( OF.ToLog ) write(3,2035) CMND.OptText(i,2)
2035  format(' Error - ',a3,' is not a valid time convolution filter option'/
     &       '         Valid options are: /LOW=#  /HIGh=#  /ORDer=#'/
     &       '                            /BUTterworth /CHEbeyschev')
      return                        
      end
c                            
c
c                            
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Digitally filter the force plate information in the frequency
c              domain using the following the process:
c              Zero Pad data so that the number of data points is a power of 2, apply
c              a window function to the data since it is assumed to be nonperiodic,
c              take the fourier transform of the result then multiply it with the filter
c              function, and take the inverse transform.
c
c     Routines called:
c       BuildFIlter
c       realft
c
      subroutine DigitalFilterFRC( CMND,FF,OF )
      implicit none

      include 'a_const.inc'
      include 'a_struct.inc'
      include 'a_prm.inc'                        
      include 'a_gls2.inc'
      include 'a_frc.inc'

      record /CmndLine/    CMND
      record /force_flag/  FF
      record /option_flag/ OF
                     
      integer*4 i,j,n,NumPnts,NumBeginZero,NumEndZero,Chnl,plt,NumBlank
      real*4    Data2(16384),Filter(8192),pi,smplfreq,w(16384)
      logical*4 Error,DoWindow,BlankEnds
                           
c
c     Figure out how many zeros must be added to data.  Zeros are added
c     symmetrically to the beginning and end of the array.  This should
c     take care of most of the end effects resulting from fourier transform
c     process.             
c                               
      pi= 4.0*atan(1.0)
      n= FP.nfrm
      i= 1
      do while( (n - 2**i) .gt. 0 )
        i= i + 1
      end do         
      i= i + 1 ! *2 just in case
      if ( i .gt. 14 ) goto 1030 ! error- too many samples, must be <=16384
      NumPnts= 2**i                                           
      NumBeginZero= (NumPnts-n)/2
      NumEndZero=   (NumPnts-n) - NumBeginZero                          
c                                       
c     Build filter function
c
      smplfreq= float(FP.frm_rate)
      call BuildFilter( CMND,2,NumPnts,smplfreq,Filter,DoWindow,
     &                  BlankEnds,NumBlank,Error,OF )
      if ( Error ) return       
c
c     Do Filtering of each force plate channel
c
      do Plt= 1, FP.num_plt
        do chnl= 1, 6
          if ( OF.UpDate ) write(6,100) chnl,plt,FP.num_plt
100       format('+ Filtering Force plate channel # ',i1,'/6',
     &            '  Plate # ',i1,'/',i1)
c                   
c         Zero pad data                
c
          i= 1
          do j= 1, NumPnts         ! create zero padded data array
            if ( j .le. NumBeginZero ) then
               Data2(j)= 0.0      
            else if ( j .gt. (NumBeginZero+n) ) then
               Data2(j)= 0.0
            else
              Data2(j)= FpData(chnl,i,plt)
              i= i + 1
            end if   
          end do ! j                           
c                   
c       Apply window function to data- at present only a Hanning window is
c       supported.  Other window functions may be added as command options
c       later on...  
c
          if ( DoWindow ) then
            do i= 1, FP.nfrm
              w(i)= 0.5*(1.0-cos((2.0*pi*i)/(FP.nfrm-1)))
              Data2(NumBeginZero+i)= Data2(NumBeginZero+i)*w(i)
            end do              
          end if
c    
c         Now do the filtering operation finally.  Take the FFT of the zero
c         padded and windowed data, multiply it with the filter function and
c         then inverse FFT the result and extract the filtered data.
c
          call realft( Data2,NumPnts/2,1 )
          do i= 1, NumPnts/2
            do j= 1, 2
              Data2(2*(i-1)+j)= Data2(2*(i-1)+j)*Filter(i)
            end do    
          end do ! i
          call realft( Data2,NumPnts/2,-1 )
          if ( DoWindow ) then             ! NOTE the 2.0* for scaling
            do i= 1, FP.nfrm
              j= i + NumBeginZero
              if ( w(i) .gt. 1.0e-5 ) then
                FpData(chnl,i,plt)= 2.0*Data2(j)/(float(NumPnts)*w(i))
              else 
                FpData(chnl,i,plt)= 2.0*100000.*Data2(j)/float(NumPnts)
              end if
            end do ! i
          else          
            do i= 1, n                     
              FpData(chnl,i,plt)= 2.0*Data2(i+NumBeginZero)/float(NumPnts)
            end do  
          end if ! DoWindow
          if ( BlankEnds ) then
            do i= 1, NumBlank
              FpData(chnl,i,plt)= FpData(chnl,NumBlank+1,plt)
              FpData(chnl,n+1-i,plt)= FpData(chnl,n-NumBlank,plt)
            end do
          end if
        end do ! Chnl   
      end do ! plt
      FF.filtered= .true. 
      FF.FiltOp= CMND.Text(1)(1:40) 
      return
                                                   
1030  write(6,1035) FP.nfrm
      if ( OF.ToLog ) write(3,1035) FP.nfrm
1035  format(' Error- There are ',i6,' samples per force channel, but at most'/
     &       '        16384 samples/channel can be filtered')
      return
      end                  
c           
c                
c                          
c
c     Copyright by Dwight Meglan 1990
c
c     Purpose: Build a frequency domain filter function based upon the command
c              line options. This filter function will applied the frequency
c              domaim version of a set of data as a window function. This is
c              similar in concept to the time domain windowing functions used to
c              compensate for the nonperiodic nature of data that is FFTed.
c
c     NOTE: there must be a LOW, HIG, or NOT present for this routine to
c     be executed.
c     Available options are:
c     /LOW=  - low frequency cutoff (Hz)
c     /LSL=  - slope of low cutoff in %/Hz
c     /HIG=  - high frequency cutoff (Hz)
c     /HSL=  - slope of high cutoff in %/Hz
c     /NOT=  - center frequency of notch filter (Hz)
c     /NWI=  - notch filter width (Hz)
c     /NSL=  - slope of notch filter in %/Hz
c     /WIN=  - use Hanning window
c     /NWN=  - don't use Hanning window
c
      subroutine BuildFilter( CMND,cn,NumPnts,SmplFreq,Filter,DoWindow,
     &                        BlankEnds,NumBlank,Error,OF )
      implicit none                                     

      include 'a_struct.inc'

      record /CmndLine/    CMND
      record /option_flag/ OF

      real*4    Filter(*),SmplFreq
      integer*4 NumPnts,cn,NumBlank
      logical*4 Error,DoWindow,BlankEnds

      real*4    DelFreq,MinFreq,MaxFreq,LowCutOff,HighCutOff,LowSlope,
     &          HighSlope,NotchFreq,NotchWidth,NotchSlope,Percent
      integer*4 i,LowCutPnt,LowCutWidth,HighCutPnt,HighCutWidth,NotchPnt,
     &          NotchWidthPnt,NotchSlopeWidth,NotchBegin,NotchEnd
      logical*4 DoLowCut,DoHighCut,DoNotch

      Error= .true.               
      DelFreq= SmplFreq/float(NumPnts)
      MinFreq= DelFreq
      MaxFreq= SmplFreq/2.0
      LowCutOff=  MinFreq ! Hz
      LowSlope=   100.0   ! %/Hz
      HighCutOff= MaxFreq ! Hz
      HighSlope=  100.0   ! %/Hz
      NotchFreq=  0.0     ! Hz
      NotchWidth= 1.0     ! Hz
      NotchSlope= 100.0   ! %/Hz
      DoLowCut=  .false.
      DoHighCut= .false.
      DoNotch=   .false.
      DoWindow=  .true.
      BlankEnds= .true.
      NumBlank=   10
      if ( CMND.NumOpt(cn) .le. 0 ) goto 1000 ! error
      do i= 1, CMND.NumOpt(cn)
        if ( CMND.OptText(i,cn) .eq. 'LOW' ) then
          if ( (.not. CMND.OptNumOK(i,2)) .or. 
     &         (CMND.OptRealVal(i,cn) .lt. MinFreq) .or.
     &         (CMND.OptRealVal(i,cn) .gt. MaxFreq) ) goto 1010 ! error
          DoLowCut= .true.
          LowCutOff= CMND.OptRealVal(i,cn)
        else if ( CMND.OptText(i,cn) .eq. 'LSL' ) then
          if ( (.not. CMND.OptNumOK(i,cn)) .or. 
     &         (CMND.OptRealVal(i,cn) .lt. 0.0) .or.
     &         (CMND.OptRealVal(i,cn) .gt. 100.0) ) goto 1020 ! error
          LowSlope= CMND.OptRealVal(i,cn) 
        else if ( CMND.OptText(i,cn) .eq. 'HIG' ) then
          if ( (.not. CMND.OptNumOK(i,cn)) .or. 
     &         (CMND.OptRealVal(i,cn) .lt. MinFreq) .or.
     &         (CMND.OptRealVal(i,cn) .gt. MaxFreq) ) goto 1030 ! error
          HighCutOff= CMND.OptRealVal(i,cn)
          DoHighCut= .true.
        else if ( CMND.OptText(i,cn) .eq. 'HSL' ) then
          if ( (.not. CMND.OptNumOK(i,cn)) .or. 
     &         (CMND.OptRealVal(i,cn) .lt. 0.0) .or.
     &         (CMND.OptRealVal(i,cn) .gt. 100.0) ) goto 1040 ! error
          HighSlope= CMND.OptRealVal(i,cn) 
        else if ( CMND.OptText(i,cn) .eq. 'NOT' ) then
          if ( (.not. CMND.OptNumOK(i,cn)) .or. 
     &         (CMND.OptRealVal(i,cn) .lt. MinFreq) .or.
     &         (CMND.OptRealVal(i,cn) .gt. MaxFreq) ) goto 1050 ! error
          NotchFreq= CMND.OptRealVal(i,cn)
          DoNotch= .true.
        else if ( CMND.OptText(i,cn) .eq. 'NSL' ) then
          if ( (.not. CMND.OptNumOK(i,cn)) .or. 
     &         (CMND.OptRealVal(i,cn) .lt. 0.0) .or.
     &         (CMND.OptRealVal(i,cn) .gt. 100.0) ) goto 1060 ! error
          NotchSlope= CMND.OptRealVal(i,cn)
        else if ( CMND.OptText(i,cn) .eq. 'NWI' ) then
          if ( (.not. CMND.OptNumOK(i,cn)) .or.       
     &         (CMND.OptRealVal(i,cn) .lt. 0.0) .or.
     &         (CMND.OptRealVal(i,cn) .gt. MaxFreq) ) goto 1070 ! error
          NotchWidth= CMND.OptRealVal(i,2)
        else if ( CMND.OptText(i,cn) .eq. 'WIN' ) then
          DoWindow= .true.
        else if ( CMND.OptText(i,cn) .eq. 'NWN' ) then
          DoWindow= .false.
        else if ( CMND.OptText(i,cn) .eq. 'BLA' ) then
          BlankEnds= .true.
          if ( (.not. CMND.OptNumOK(i,cn)) .or.
     &         (CMND.OptIntVal(i,cn) .lt. 1) .or.
     &         (CMND.OptIntVal(i,cn) .gt. NumPnts) ) goto 2010 ! error
          NumBlank= CMND.OptIntVal(i,cn)
        else if ( CMND.OptText(i,cn) .eq. 'NBL' ) then
          BlankEnds= .false.
        else                                         
          goto 1080 ! error- bad option...
        end if
      end do ! option number
      if ( (DoLowCut .and. DoHighCut) .and.
     &     (LowCutOff .ge. HighCutOff)      ) goto 1090 ! error
      if ( (DoLowCut .or. DoHighCut) .and. DoNotch ) then
        if ( (NotchFreq .lt. LowCutOff) .or.           
     &       (NotchFreq .gt. HighCutOff)     ) goto 2000 ! error
      end if                                                    
c
c     Now that the filter information has been read and interpreted build
c     the filter function in the frequency domain.
c                               
      do i= 1, NumPnts/2                                         
        Filter(i)= 1.0
      end do
      if ( DoLowCut ) then
        LowCutPnt= LowCutOff/DelFreq
        LowCutWidth= 100./(LowSlope*DelFreq)
        if ( LowCutPnt-LowCutWidth .gt. 1 ) then     
          do i= 1, LowCutPnt-LowCutWidth
            Filter(i)= 0.0
          end do
        end if
        i= LowCutPnt
        Percent= 1.0
        do while ( (i .gt. 1) .and. ((LowCutPnt-i) .le. LowCutWidth) )
          Filter(i)= Filter(i)*Percent
          if ( Filter(i) .lt. 0 ) Filter(i)= 0.0 ! just in case...
          Percent= Percent - LowSlope*DelFreq/100.0
          i= i - 1
        end do
      end if ! LowCut filter present
      if ( DoHighCut ) then 
        HighCutPnt= HighCutOff/DelFreq
        HighCutWidth= 100./(HighSlope*DelFreq)
        if ( (HighCutPnt+HighCutWidth) .lt. NumPnts/2 ) then
          do i= HighCutPnt+HighCutWidth, NumPnts/2
            Filter(i)= 0.0
          end do
        end if
        i= HighCutPnt
        Percent= 1.0
        do while ( (i .lt. NumPnts/2) .and. 
     &             ((i-HighCutPnt) .le. HighCutWidth) )
          Filter(i)= Filter(i)*Percent
          if ( Filter(i) .lt. 0 ) Filter(i)= 0.0 ! just in case...
          Percent= Percent - HighSlope*DelFreq/100.0
          i= i + 1
        end do
      end if ! HighCut filter present
      if ( DoNotch ) then
        NotchWidthPnt= NotchWidth/DelFreq 
        NotchSlopeWidth= 100./(NotchSlope*DelFreq)
        NotchPnt=   NotchFreq/DelFreq
        NotchBegin= NotchPnt - NotchWidthPnt/2
        if ( NotchBegin .lt. 1 ) NotchBegin= 1
        NotchEnd=   NotchBegin + NotchWidthPnt
        if ( NotchEnd .gt. NumPnts/2 ) NotchEnd= NumPnts/2
        do i= NotchBegin, NotchEnd
          Filter(i)= 0.0
        end do                     
        i= NotchBegin
        Percent= 0.0
        do while ( (i .gt. 1) .and. 
     &             ((NotchBegin-i) .le. NotchSlopeWidth) )
          Filter(i)= Filter(i)*Percent              
          if ( Filter(i) .gt. 1.0 ) Filter(i)= 1.0 ! just in case...
          Percent= Percent + NotchSlope*DelFreq/100.0
          i= i - 1
        end do
        i= NotchEnd
        Percent= 0.0
        do while ( (i .lt. NumPnts/2) .and. 
     &             ((i-NotchEnd) .le. NotchSlopeWidth) )
          Filter(i)= Filter(i)*Percent
          if ( Filter(i) .gt. 1.0 ) Filter(i)= 1.0 ! just in case...
          Percent= Percent + NotchSlope*DelFreq/100.0
          i= i + 1   
        end do
      end if ! Notch filter present
      Error= .false.
      return                               

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- There must be a high and/or low cutoff and/or notch'/
     &       '        frequency given as an option for performing the filter'/
     &       '        operation.'/
     &       '        Valid options are: /LOW=  /HIGh=  /NOTch=  /NWI='/
     &       '                           /LSL=  /HSL=   /NSL= '/
     &       '                           /WIN   /NWN')
      return                                              
                                                            
1010  write(6,1015) CMND.OptRealVal(i,cn),MinFreq,MaxFreq
      if ( OF.ToLog ) write(3,1015) CMND.OptRealVal(i,cn),MinFreq,MaxFreq
1015  format(' Error- ',f8.3,' Hz is an invalid low cutoff frequency.'/
     &       '        Valid frequencies are ',f8.3,'> and <',f8.3,' Hz')
      return               
                                         
1020  write(6,1025) CMND.OptRealVal(i,cn)
      if ( OF.ToLog ) write(3,1025) CMND.OptRealVal(i,cn)
1025  format(' Error- ',f8.3',is an invalid low cutoff slope'/
     &       '        Valid slopes are from 0 to 100 %/Hz')
      return

1030  write(6,1035) CMND.OptRealVal(i,cn),MinFreq,MaxFreq
      if ( OF.ToLog ) write(3,1035) CMND.OptRealVal(i,cn),MinFreq,MaxFreq
1035  format(' Error- ',f8.3,' Hz is an invalid high cutoff frequency.'/
     &       '        Valid frequencies are ',f8.3,'> and <',f8.3,' Hz')
      return
                                         
1040  write(6,1045) CMND.OptRealVal(i,cn)
      if ( OF.ToLog ) write(3,1045) CMND.OptRealVal(i,cn)
1045  format(' Error- ',f8.3',is an invalid high cutoff slope'/
     &       '        Valid slopes are from 0 to 100 %/Hz')
      return                                                  

1050  write(6,1055) CMND.OptRealVal(i,cn),MinFreq,MaxFreq
      if ( OF.ToLog ) write(3,1055) CMND.OptRealVal(i,cn),MinFreq,MaxFreq
1055  format(' Error- ',f8.3,' Hz is an invalid notch frequency.'/
     &       '        Valid frequencies are ',f8.3,'> and <',f8.3,' Hz')
      return
                                         
1060  write(6,1065) CMND.OptRealVal(i,cn)
      if ( OF.ToLog ) write(3,1065) CMND.OptRealVal(i,cn)
1065  format(' Error- ',f8.3',is an invalid notch slope'/
     &       '        Valid slopes are from 0 to 100 %/Hz')
      return                                                  

1070  write(6,1075) CMND.OptRealVal(i,cn),MaxFreq     
      if ( OF.ToLog ) write(3,1075) CMND.OptRealVal(i,cn),MaxFreq
1075  format(' Error- ',f8.3',is an invalid notch width'/
     &       '        Valid widths are from 0 to ',f8.3,' Hz')
      return                                                  

1080  write(6,1085) CMND.OptText(i,cn)
      if ( OF.ToLog ) write(3,1085) CMND.OptText(i,cn)
1085  format(' Error- ',a3,' is an invalid digital filter option'/
     &       '        Valid options are: /LOW=  /HIGh=  /NOTch=  /NWI='/
     &       '                           /LSL=  /HSL=   /NSL= '/
     &       '                           /WIN   /NWN    /BLAnk=  /NoBLank')
      return

1090  write(6,1095) LowCutOff,HighCutOff
      if ( OF.ToLog ) write(3,1095) LowCutOff,HighCutOff
1095  format(' Error- Low CutOff is ',f8.3,' and the High CutOff is ',f8.3/
     &       '        but the Low CutOff must be greater than the High CutOff')
      return
      
2000  write(6,2005) LowCutOff,HighCutOff,NotchFreq
      if ( OF.ToLog ) write(3,2005) LowCutOff,HighCutOff,NotchFreq
2005  format(' Error- Low CutOff is ',f8.3,' Hz, the High CutOff is ',f8.3,' Hz'/
     &       '        and the notch frequency is ',f8.3' Hz'/
     &       '        but the Notch Frequency must be >Low CutOff and <High CutOff')
      return

2010  write(6,2015) CMND.OptText(i,cn),NumPnts
      if ( OF.ToLog ) write(3,2015) CMND.OptText(i,cn),NumPnts
2015  format(' Error- ',a3,' does not have valid number of points'/
     &       '        Valid numbers of points are from 1 to ',i5)
      return                                                  
      end
      
c     Hanning window      
      
      real*4 function HanWin( i,n )                        
      implicit none                             

      integer*4 i,n

      HanWin= 0.5*(1.0-cos( (8.0*atan(1.0)*i)/(n-1) ))
      return
      end
c
c
c                                                                      
      subroutine realft( data,n,isign )
      implicit none
c
c     Calculate the fourier transform of a set of 2n real-valued data points.
c     Replaces this data (which is stored in data(1...2n)) by the positive
c     frequency half of its complex fourier transform.  The real valued first
c     last components of the complex transform are returned as elements data(1)
c     and data(2) respectively.  n must be a power of 2.  This routine also
c     also calculates the inverse transform of a complex data array if it is
c     the transform of real data.  Inverse is taken when isign= -1, forward 
c     when isign= 1.  The inverse must be multiplied by 1/n for proper 
c     scaling.
c     Routine was extracted from Numerical Recipes in C and converted to
c     fortran- seems a kind of backward going from C to fortran, Oh well...
c     I only had the C version of the book at the time, but I have since 
c     gotten the FORTRAN version and checked my conversion- it's OK...
c
      integer*4 n,isign
      real*4    data(1)

      integer*4 i,i1,i2,i3,i4,n2p3
      real*4    c1,c2,h1r,h1i,h2r,h2i,wrs,wis
      real*8    wr,wi,wpr,wpi,wtemp,theta

      c1= 0.5
      theta= 4.0d0*datan(1.0d0)/dble(n)
      if ( isign .eq. 1 ) then   ! forward transform
        c2= -0.5
        call four1( data,n,1 )
      else  ! set up for inverse transform
        c2= 0.5
        theta= -theta
      end if
      wpr= -2.0d0*dsin(0.5d0*theta)**2
      wpi= dsin(theta)
      wr= 1.0d0 + wpr
      wi= wpi
      n2p3= 2*n + 3
      do i= 2, n/2                    ! case of i= 1 done separately below
        i1= 2*i - 1  
        i2= 1 + i1
        i3= n2p3 - i2
        i4= 1 + i3
        wrs= sngl(wr)
        wis= sngl(wi)
        h1r=  c1*( data(i1)+data(i3) )  ! the two separate transforms are
        h1i=  c1*( data(i2)-data(i4) )  ! separated out of the data
        h2r= -c2*( data(i2)+data(i4) )
        h2i=  c2*( data(i1)-data(i3) )
        data(i1)=  h1r + wrs*h2r - wis*h2i  ! recombine them to form the true
        data(i2)=  h1i + wrs*h2i + wis*h2r  ! transform of the original real data
        data(i3)=  h1r - wrs*h2r + wis*h2i
        data(i4)= -h1i + wrs*h2i + wis*h2r
        wtemp= wr
        wr= wr*wpr - wi*wpi + wr
        wi= wi*wpr + wtemp*wpi + wi
      end do
      if ( isign .eq. 1 ) then          
        h1r= data(1)                    ! squeeze the first and last data
        data(1)= h1r + data(2)          ! together to get them all in the array
        data(2)= h1r - data(2)
      else                              ! do the inverse transform
        h1r= data(1)
        data(1)= c1*( h1r + data(2) )
        data(2)= c1*( h1r - data(2) )
        call four1(data,n,-1)
      end if       
      return
      end
c
c
c
      subroutine four1( data,nn,isign )
      implicit none      
c
c     Take the discrete fourier transform of a set of complex number data.
c     If isign= 1 then take the forward transform, if isign= -1 then take 
c     the inverse transform times nn.  Input data is replaced with the result.
c     NOTE that nn must be a power of 2
c     From Numerical Recipes
c
      integer*4 nn,isign
      real*4    data(1)

      integer*4 n,mmax,m,j,istep,i
      real*4    tempr,tempi
      real*8    wtemp,wr,wpr,wpi,wi,theta
c
c     Bit reversal section
c
      n= 2*nn       ! left bit shift 1 place
      j= 1                  ! could also use n= 2*nn
      do i= 1, n, 2
        if ( j .gt. i ) then
           call swapr( data(j),data(i) )
           call swapr( data(j+1),data(i+1) )
        end if
        m= n/2  ! right bit shift 1 place 
        do while ( (m .ge. 2) .and. (j .gt. m) ) 
          j= j - m    
          m= m/2  ! right bit shift 1 place 
        end do  
        j= j + m
      end do
c
c     Danielson-Lanczos section
c
      mmax= 2
      do while ( n .gt. mmax )
        istep= 2*mmax
        theta= 8.0d0*datan(1.0d0)/dble(isign*mmax)
        wpr= -2.0d0*dsin(0.5d0*theta)**2
        wpi= dsin(theta)
        wr= 1.0d0
        wi= 0.0d0   
        do m= 1, mmax, 2
          do i= m, n, istep
            j= i + mmax
            tempr= sngl(wr)*data(j) - sngl(wi)*data(j+1)
            tempi= sngl(wr)*data(j+1) + sngl(wi)*data(j)
            data(j)=   data(i) - tempr
            data(j+1)= data(i+1) - tempi
            data(i)=   data(i) + tempr
            data(i+1)= data(i+1) + tempi
          end do ! i
          wtemp= wr  
          wr= wr*wpr - wi*wpi + wr
          wi= wi*wpr + wtemp*wpi + wi
        end do ! m
        mmax= istep
      end do ! while
      return
      end
c             
c
c
      subroutine swapr( a,b )
      real*4 a,b,temp

      temp= a
      a= b
      b= temp
      return
      end
