!!Stack= 0

c                                        
c	do_CYC_calc  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Controlling routine that calls the gait cycle calculation 
c				routines
c
c	Routines called:
c
c	CalcGaitStatistics 
c	InputGaitEvents
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	None
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	None
c
      subroutine do_CYC_calc( CMND,MF,SF,CF,OF )
      implicit none
                             
      include 'a_const.inc'                 
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'

      record /CmndLine/      CMND
      record /marker_flag/   MF
      record /segment_flag/  SF
      record /cycle_flag/    CF
      record /option_flag/   OF
                             
      if ( CMND.CmndText(2) .eq. 'EVE' ) then
        call InputGaitEvents( CMND,CF,OF )

      else if ( CMND.CmndText(2) .eq. 'STA' ) then
        if ( (.not. SF.position) .and. 
     &       (GP.Analysis_Type .ne. TDP_ANL) ) goto 1000
        call CalcGaitStatistics( CMND,MF,CF,OF )

      else if ( CMND.CmndText(2) .eq. 'AUT' ) then
        write(6,100)                       
        if ( OF.ToLog ) write(3,100)
100     format(' This function, the automatic detection of heel strikes and'/
     &         ' toe offs from foot motion data, has not been implemented yet...')

      else
        write(6,110) CMND.CmndText(2)
        if ( OF.ToLog ) write(3,110) CMND.CmndText(2)
110     format(' Error- ',a3,' is an invalid CYCle command.'/
     &         '        Valid commands are: EVEent STAtistics AUTomatic')
      end if
      return                                                 

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Segment position has not been calculated yet')
      return

1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Marker position data has not been read yet')
      return
      end
c                                        
c	InputGaitEvents  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Get gait events from command line and once they have
c				all been input, check over the ordering of the events
c				to see if normal walking gait cycle has been input.
c
c	Routines called:
c
c	CheckGaitPattern 
c	ParseLine
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			MP.first_frm
c	Param			MP.nfrm
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Joint			event
c	Param			MP.lhs_frm
c	Param			MP.lto_frm
c	Param			MP.num_lhs
c	Param			MP.num_lto
c	Param			MP.num_rhs
c	Param			MP.num_rto
c	Param			MP.rhs_frm
c	Param			MP.rto_frm
c
      subroutine InputGaitEvents( CMND,CF,OF )
      implicit none
      include 'a_const.inc'

       include 'a_struct.inc'
       include 'a_prm.inc'
       include 'a_names.inc' 
       include 'a_gls2.inc'
       include 'a_frc.inc'
       include 'a_jnt.inc'

       record /CmndLine/     CMND
       record /cycle_flag/   CF
       record /option_flag/  OF

       integer*4 num_event,i,frm,prev_frm,side
       character input*80
       logical*4 error,CheckGaitPattern,UseAbsFrm
c
c      Determine whether relative or absolute frame numbers will be used
c      for gait cycle event frames
c
       UseAbsFrm= .true.
       CF.event= .false.
       if ( CMND.NumOpt(2) .gt. 0 ) then
         if ( CMND.OptText(1,2) .eq. 'ABS' ) then
           UseAbsFrm= .true.
         else if ( CMND.OptText(1,2) .eq. 'REL' ) then
           UseAbsFrm= .false.
         else
           goto 1020 ! error
         end if
       end if
       write(6,10)           
       if ( OF.ToLog ) write(3,10)
10     format('  Input gait event and frame number (counting from 1)'/
     &        '  Gait events are:  RHS LHS RTO LTO '//
     &        '  Type both gait event and frame # on same line, e.g. RHS 23<RET>'/
     &        '  After last event, enter "done" on next line'//)
      if ( UseAbsFrm ) then
        write(6,15) MP.first_frm
15      format(' Frame numbers are absolute, rather than relative'/
     &         ' to the first frame (',i3,') in the data file'//)
      else
        write(6,17) MP.first_frm
17      format(' Frame numbers are relative to the first frame (',i3,
     &         ') in the data file'//)
      end if
      num_event= 1            
      prev_frm= 0
      CMND.CmndText(1)= '   '
      MP.num_rhs= 0
      MP.num_lhs= 0
      MP.num_rto= 0
      MP.num_lto= 0
      do frm= 1, MP.nfrm
        GaitEvent(frm)= 0
      end do
      do while ( CMND.CmndText(1) .ne. 'DON' )
        write(6,25) num_event                        
        if ( OF.ToLog ) write(3,25) num_event
25      format(' event number',i2,': ',$)        
        read(5,30,end=999) input
        if ( OF.ToLog ) write(3,30) input
30      format(a80)
        call ParseLine( input,CMND )
        if ( CMND.CmndText(1) .ne. 'DON' ) then
          error= .not. CMND.CmndNumOK(2)
          if ( .not. error ) then
            frm= CMND.CmndIntVal(2)
            if ( .not. UseAbsFrm ) frm= frm + MP.first_frm - 1
            if ( (frm .lt. MP.first_frm) .or.
     &           (frm .gt. MP.first_frm+MP.nfrm) ) error= .true.
            if ( frm .le. prev_frm ) error= .true.
          end if
          if ( .not. error ) then
            if ( CMND.CmndText(1) .eq. 'RHS' ) then           
              if ( MP.num_rhs .le. 4 ) then
                MP.num_rhs= MP.num_rhs + 1
                MP.rhs_frm(MP.num_rhs)= frm - MP.first_frm + 1
                num_event= num_event + 1
                prev_frm= frm
                GaitEvent(MP.rhs_frm(MP.num_rhs))= 1
              else
                write(6,32)             
                if ( OF.ToLog ) write(3,32)
32              format(' Maximum number of RHS''s has been reached.'/
     &                 ' Only five RHS''s are allowed...')
              end if
              
            else if ( CMND.CmndText(1) .eq. 'LHS' ) then
              if ( MP.num_lhs .le. 4 ) then
                MP.num_lhs= MP.num_lhs + 1
                MP.lhs_frm(MP.num_lhs)= frm - MP.first_frm + 1
                num_event= num_event + 1
                prev_frm= frm
                GaitEvent(MP.lhs_frm(MP.num_lhs))= 2
              else
                write(6,34)
                if ( OF.ToLog ) write(3,34)
34              format(' Maximum number of LHS''s has been reached.'/        
     &                 ' Only five LHS''s are allowed...')
              end if
                                        
            else if ( CMND.CmndText(1) .eq. 'RTO' ) then
              if ( MP.num_rto .le. 4 ) then
                MP.num_rto= MP.num_rto + 1
                MP.rto_frm(MP.num_rto)= frm - MP.first_frm + 1
                num_event= num_event + 1
                prev_frm= frm
                GaitEvent(MP.rto_frm(MP.num_rto))= 3
              else
                write(6,36)
                if ( OF.ToLog ) write(3,36)
36              format(' Maximum number of RTO''s has been reached.'/
     &                 ' Only five RTO''s are allowed...')
              end if
                                                
            else if ( CMND.CmndText(1) .eq. 'LTO' ) then
              if ( MP.num_lto .le. 4 ) then
                MP.num_lto= MP.num_lto + 1
                MP.lto_frm(MP.num_lto)= frm - MP.first_frm + 1
                num_event= num_event + 1
                prev_frm= frm
                GaitEvent(MP.lto_frm(MP.num_lto))= 4
              else
                write(6,38)                          
                if ( OF.ToLog ) write(3,38)
38              format(' Maximum number of LTO''s has been reached.'/
     &                 ' Only five LTO''s are allowed...')
              end if
                                        
            else
              write(6,40)                         
              if ( OF.ToLog ) write(3,40)
40            format(' Incorrect event name.  Try again...')
            end if
          else                                                  
            WRITE(6,50) CMND.CmndText(2),prev_frm,MP.nfrm
            if ( OF.ToLog ) write(3,50) CMND.CmndText(2),      
     &                                          prev_frm,MP.nfrm
50          format(' Invalid frame #: ',a3,'.  The previous frame # is ',i3'.'/
     &             ' There are ',i3,' frames of data.')
          end if ! good frame number
        end if ! event type not DONE    
      end do ! while event type not DONE
c
c     Now check over the gait events and see whether a proper cycle order
c     has been typed in
c
999   if ( (GP.analysis_type .eq. FULL_ANL) .or.
     &     (GP.analysis_type .eq. HH_ANL)   .or.
     &     (GP.analysis_type .eq. CC_ANL)   .or.
     &     (GP.analysis_type .eq. MAYO_ANL) .or.
     &     (GP.analysis_type .eq. TDP_ANL)       ) then
        if ( CheckGaitPattern(OF) )  CF.event= .true.
      else
        do side= rt, lf
          do frm= 1, MP.nfrm
            FlrCntct(frm,side)= .false.
          end do
        end do
        if ( (MP.rhs_frm(1) .gt. 0) .and. (MP.rto_frm(1) .gt. 0) ) then
          do frm= MP.rhs_frm(1), MP.rto_frm(1)
            FlrCntct(frm,rt)= .true.
          end do          
        end if
        if ( (MP.lhs_frm(1) .gt. 0) .and. (MP.lto_frm(1) .gt. 0) ) then
          do frm= MP.lhs_frm(1), MP.lto_frm(1)
            FlrCntct(frm,lf)= .true.
          end do          
        end if
        MP.first_ft= rt
        if ( (MP.rhs_frm(1) .gt. 0) .and. (MP.lhs_frm(1) .gt. 0) .and.
     &       (MP.lhs_frm(1) .gt. MP.rhs_frm(1)) ) MP.first_ft= lf
        CF.event= .true.
      end if
      return

1020  write(6,1025) CMND.OptText(1,2)
      if ( OF.ToLog ) write(3,1025) CMND.OptText(1,2)
1025  format('Error- ',a3,' is not a valid CYCle EVEnt option'/
     &       '       Valid options are: /RELative /ABSolute')
      return
      end
c                                        
c	CheckGaitPattern  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Check the pattern of the gait events and see if they
c				forma normal walking sequence.  If so then calculate
c				the length of the cycle in data frames and set the 
c				array FlrCntct to indicate when the feet are on
c				the ground.  Also, note which is the first foot to
c				have a heelstrike in the sequence.  There are two valid
c 				sequences: RHS LTO LHS RTO RHS
c						   LHS RTO RHS LTO LHS
c
c	Routines called:
c
c	None 
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			MP.lhs_frm
c	Param			MP.lto_frm
c	Param			MP.nfrm
c	Param			MP.num_lhs
c	Param			MP.num_lto
c	Param			MP.num_rhs
c	Param			MP.num_rto
c	Param			MP.rhs_frm
c	Param			MP.rto_frm
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Force			FlrCntct
c	Param			MP.cycle_first_frm
c	Param			MP.cycle_last_frm
c	Param			MP.cycle_length
c	Param			MP.first_ft
c	Param			MP.num_cycle
c
      logical*4 function CheckGaitPattern(OF) 
      implicit none
      include 'a_const.inc'

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

      record /option_flag/ OF

      integer*4 frm,side
               
      CheckGaitPattern= .false.
      if ( (MP.num_rhs .eq. 0) .or. (MP.num_rto .eq. 0)
     & .or.(MP.num_rto .eq. 0) .or. (MP.num_lto .eq. 0) ) goto 1000
      if ( MP.rhs_frm(1) .lt. MP.lhs_frm(1) ) then
        if ( MP.num_rhs .lt. 2 ) goto 1000
        if ( (MP.lto_frm(1) .lt. MP.rhs_frm(1)) .or.
     &       (MP.lhs_frm(1) .lt. MP.lto_frm(1)) .or.
     &       (MP.rto_frm(1) .lt. MP.lhs_frm(1)) .or.
     &       (MP.rhs_frm(2) .lt. MP.rto_frm(1))      ) goto 1010
        MP.first_ft= rt
        MP.num_cycle= 1
        MP.cycle_length(1)= MP.rhs_frm(2) - MP.rhs_frm(1) + 1
        MP.cycle_first_frm(1)= MP.rhs_frm(1)
        MP.cycle_last_frm(1)= MP.rhs_frm(2)
        do frm= 1, MP.nfrm              ! set up ground contact array
          do side= rt, lf
            FlrCntct(frm,side)= .false.
          end do ! side
        end do ! side
        do frm= MP.rhs_frm(1), MP.rto_frm(1)
          FlrCntct(frm,rt)= .true.
        end do                                                             
        FlrCntct(MP.rhs_frm(2),rt)= .true.
        do frm= MP.rhs_frm(1), MP.lto_frm(1)
          FlrCntct(frm,lf)= .true.
        end do
        do frm= MP.lhs_frm(1), MP.rhs_frm(2)
          FlrCntct(frm,lf)= .true.
        end do
      else    
        if ( MP.num_lhs .lt. 2 ) goto 1000
        if ( (MP.rto_frm(1) .lt. MP.lhs_frm(1)) .or.
     &       (MP.rhs_frm(1) .lt. MP.rto_frm(1)) .or.
     &       (MP.lto_frm(1) .lt. MP.rhs_frm(1)) .or.
     &       (MP.lhs_frm(2) .lt. MP.lto_frm(1))      ) goto 1000
        MP.first_ft= lf
        MP.num_cycle= 1
        MP.cycle_length(1)= MP.lhs_frm(2) - MP.lhs_frm(1) + 1
        MP.cycle_first_frm(1)= MP.lhs_frm(1)
        MP.cycle_last_frm(1)= MP.lhs_frm(2)
        do frm= 1, MP.nfrm              ! set up ground contact array
          do side= rt, lf                           
            FlrCntct(frm,side)= .false.
          end do ! side
        end do ! side
        do frm= MP.lhs_frm(1), MP.lto_frm(1)
          FlrCntct(frm,lf)= .true.
        end do
        FlrCntct(MP.lhs_frm(2),lf)= .true.
        do frm= MP.lhs_frm(1), MP.rto_frm(1)
          FlrCntct(frm,rt)= .true.
        end do
        do frm= MP.rhs_frm(1), MP.lhs_frm(2)
          FlrCntct(frm,rt)= .true.
        end do
      end if            
      CheckGaitPattern= .true.
      return
      
1000  write(6,1005) MP.num_rhs,MP.num_lhs,
     &              MP.num_rto,MP.num_lto         
      if ( OF.ToLog ) write(3,1005) MP.num_rhs,MP.num_lhs,
     &                                      MP.num_rto,MP.num_lto
1005  format(' Error- Invalid number of gait events'/
     &       '        RHS: ',i3/
     &       '        LHS: ',i3/
     &       '        RTO: ',i3/
     &       '        LTO: ',i3  )
      return

1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format('Error- Gait event frame numbers are not in the proper order.')
      return
      end
c                                        
c	CalcGaitStatistics  
c
c	Copyright by Dwight Meglan 1990
c
c	Purpose:	Calculate the time-distance parameters which describe
c				the walking gait cycle.  These are the classic data
c				used for clinical gait evaluation.  A set of options
c				is available here to indicate which marker should be
c				used to calculate the step length and width.  This may
c				be necessary because normally the foot LCS position is
c				used but if one of the marker's trajectories was obstructed
c				at the beginning or end of the gait cycle (unfortunately
c				a rather common occurence in pathological gait cases)
c				then marker position has been extrapolated and there is
c				a good chance that the foot LCS position is messed up 
c				there.  Since these are the critical data frames for
c				calculating the step data, provision has been made to
c				use the individual markers on the foot instead.  In this
c				way if one of the markers had good trajectory data during
c				the entire gait cycle, good step data can be calculated.
c				NOTE: If a negative step width occurs than one or more of
c				marker trajectories is probably messed up at the beginning
c				or end of the gait cycle.  There is however the possiblity
c				that a pathological gait in which the legs have actually
c				crossed has occurred.
c
c	Routines called:
c
c	VecDot
c	VecMag
c	VecSub 
c
c	Common Block Variables USED by routine:
c	Block Name		Variable Name
c
c	Param			GP.num_seg
c	Param			GP.seg_num
c	Param			MRK_NUM.ankle
c	Param			MRK_NUM.heel
c	Param			MRK_NUM.toe
c	Param			MP.cycle_length
c	Param			MP.first_ft
c	Param			rMtnFrmRate
c	Param			MP.lhs_frm
c	Param			MP.lto_frm
c	Param			MP.nfrm
c	Param			MP.num_lhs
c	Param			MP.num_lto
c	Param			MP.num_rhs
c	Param			MP.num_rto
c	Param			MP.rhs_frm
c	Param			MP.rto_frm
c	Marker			MrkData
c	Segment			SgpGCS
c
c	Common Block Variables MODIFIED by routine:
c	Block Name		Variable Name
c
c	Param			CP.cadence
c	Param			CP.cycle_time
c	Param			CP.dbl_time
c	Param			CP.dbl_percent
c	Param			CP.first_side
c	Param			CP.HS_percent
c	Param			CP.sgl_time
c	Param			CP.sgl_percent
c	Param			CP.step_length
c	Param			CP.step_width
c	Param			CP.stride_length
c	Param			CP.total_percent
c	Param			CP.total_time
c	Param			CP.TO_percent
c	Param			CP.velocity
c
      subroutine CalcGaitStatistics( CMND,MF,CF,OF )
      implicit none
      include 'a_const.inc'
                                      
      include 'a_struct.inc'
      include 'a_prm.inc'
      include 'a_names.inc'                 
      include 'a_mrk.inc'
      include 'a_seg.inc'

      record /CmndLine/      CMND
      record /marker_flag/   MF
      record /cycle_flag/    CF
      record /option_flag/   OF

      real*4    vecA(3),vecB(3),vecC(3),vecD(3),vecE(3),vecF(3),vecG(3),
     &          VecMag,VecDot,temp,feet,FtPosGCS(3,2),NormVecA(3),inch,
     &          NormCycleTime,NormAge(21),NormVel(21),NormCadence(21),
     &          NormStep(21),NormWidth(21),
     &          NormTotal,NormDouble,NormSingle,
     &          NormOTO,NormOFS,NormTO    
      integer*4 HS_frm(4),TO_frm(3),side_1st,side_2nd,ft_1st,ft_2nd,i,seg,side,
     &          toe_1st,toe_2nd,ankle_1st,ankle_2nd,heel_1st,heel_2nd
      logical*4 rtft_OK,lfft_OK,pelv_OK,UseToeMrk,UseFtLCS,UseHeelMrk,UseAklMrk,
     &          VecNorm,error

      data feet,inch / 3.2808,39.3696 / ! conversion ft/m, in/m
                                                                  
      data NormAge     / 1.0,  1.5,  2.0,  2.5,  3.0,  3.5,  4.0,  5.0,
     &                   6.0,  7.0,  8.0,  9.0, 10.0, 11.0, 12.0, 13.0,
     &                  16.0, 30.0, 40.0, 50.0, 60.0 /
      data NormVel     /0.62, 0.75, 0.78, 0.82, 0.86, 1.01, 1.03, 1.07,
     &                  1.27, 1.25, 1.33, 1.40, 1.47, 1.50, 1.47, 1.50,
     &                 1.523,1.451,1.584,1.552,1.466 /
      data NormCadence / 179,  180,  165,  159,  152,  168,  158,  153,
     &                   146,  144,  140,  140,  135,  135,  132,  116,
     &                   115,  111,  122,  118,  115 /
      data NormStep    /0.20, 0.25, 0.28, 0.31, 0.34, 0.36, 0.39, 0.42,
     &                  0.48, 0.52, 0.57, 0.60, 0.65, 0.66, 0.66, 0.78,
     &                  0.795,0.784,0.779,0.789,0.765 /
      data NormWidth   /0.07, 0.07, 0.07, 0.07, 0.07, 0.07, 0.07, 0.07,
     &                  0.07, 0.07, 0.07, 0.07, 0.07, 0.07, 0.07, 0.07,
     &                  0.072,0.079,0.096,0.082,0.071 /
                              
      data NormTotal,NormDouble,NormSingle / 62.0,12.0,38.0 /
      data NormOTO,NormOFS,NormTO / 12.0,50.0,62.0 /
c                                                         
c    Check for flags indicating that the toe, ankle or heel marker should be 
c    used to calculate the gait stats rather than the foot LCS center
c
      if ( GP.Analysis_Type .ne. TDP_ANL ) then
        UseFtLCS=   .true.
        UseToeMrk=  .false.
        UseHeelMrk= .false.
        UseAklMrk=  .false.
      else
        UseFtLCS=   .false.
        UseToeMrk=  .false.
        UseHeelMrk= .false.
        UseAklMrk=  .true.
      end if
      if ( CMND.NumOpt(2) .gt. 0 ) then
        if ( CMND.OptText(1,2) .eq. 'TOE' ) then
          UseToeMrk= .true.
          UseFtLCS=  .false.
        else if ( CMND.OptText(1,2) .eq. 'HEE' ) then
          UseHeelMrk= .true.
          UseFtLCS=  .false.
        else if ( CMND.OptText(1,2) .eq. 'ANK' ) then
          UseAklMrk= .true.
          UseFtLCS=  .false.
        else
          write(6,101) CMND.OptText(1,2)
101       format( ' Error- ',a3,' is not a valid option for CYCle STAtistics'/
     &            '        Valid options are : TOE ANKle HEEl')
        end if
      end if
c
c     Check whether gait events and segment data are present
c
      if ( .not. CF.event ) goto 1000 ! error
      if ( (GP.Analysis_Type .ne. FULL_ANL) .and.
     &     (GP.Analysis_Type .ne. HH_ANL)   .and.
     &     (GP.Analysis_Type .ne. CC_ANL)   .and.
     &     (GP.Analysis_Type .ne. MAYO_ANL) .and.
     &     (GP.Analysis_Type .ne. TDP_ANL)        ) goto 1080
      if ( GP.Analysis_Type .ne. TDP_ANL ) then
        rtft_OK= .false.
        lfft_OK= .false.
        pelv_OK= .false.                                                   
        do i= 1, GP.num_seg
          seg= GP.seg_num(i)
          if ( seg .eq. rtft ) rtft_OK= .true.         
          if ( seg .eq. lfft ) lfft_OK= .true.
          if ( seg .eq. pelv ) pelv_OK= .true.
        end do ! i => seg                
        if ( (.not. rtft_OK) .or. (.not. lfft_OK) .or. (.not. pelv_OK) ) goto 1010
      else
        if ( MP.num_mrk .ne. 6 ) goto 1060
        if ( .not. MF.read ) goto 1070
      end if
c                                     
c     set up data for analysis
c
      CP.first_side= MP.first_ft
      if ( MP.first_ft .eq. rt ) then
        side_1st= rt
        side_2nd= lf
        ft_1st=   rtft
        ft_2nd=   lfft
        heel_1st=  MRK_NUM(rt).heel
        heel_2nd=  MRK_NUM(lf).heel
        ankle_1st= MRK_NUM(rt).ankle
        ankle_2nd= MRK_NUM(lf).ankle
        toe_1st=   MRK_NUM(rt).toe
        toe_2nd=   MRK_NUM(lf).toe
        HS_frm(1)= MP.rhs_frm(1)
        HS_frm(2)= MP.lhs_frm(1)
        HS_frm(3)= MP.rhs_frm(2)
        TO_frm(1)= MP.lto_frm(1)
        TO_frm(2)= MP.rto_frm(1)
      else
        side_1st= lf
        side_2nd= rt
        ft_1st=   lfft
        ft_2nd=   rtft
        heel_1st=  MRK_NUM(lf).heel
        heel_2nd=  MRK_NUM(rt).heel
        ankle_1st= MRK_NUM(lf).ankle
        ankle_2nd= MRK_NUM(rt).ankle
        toe_1st=   MRK_NUM(lf).toe
        toe_2nd=   MRK_NUM(rt).toe
        HS_frm(1)= MP.lhs_frm(1)
        HS_frm(2)= MP.rhs_frm(1)
        HS_frm(3)= MP.lhs_frm(2)
        TO_frm(1)= MP.rto_frm(1)
        TO_frm(2)= MP.lto_frm(1)
      end if
c
c     Calculate stride length based upon the position of the foot LCS at
c     heel strike                         
c
      if ( UseFtLCS ) then
        call VecSub( SgpGCS(x,cntr,HS_frm(3),ft_1st),
     &               SgpGCS(x,cntr,HS_frm(1),ft_1st), vecA )
      else if ( UseToeMrk ) then
        call VecSub( MrkData(x,HS_frm(3),toe_1st),
     &               MrkData(x,HS_frm(1),toe_1st), vecA )
      else if ( UseAklMrk ) then
        call VecSub( MrkData(x,HS_frm(3),ankle_1st),
     &               MrkData(x,HS_frm(1),ankle_1st), vecA )
      else if ( UseHeelMrk ) then
        call VecSub( MrkData(x,HS_frm(3),heel_1st),          
     &               MrkData(x,HS_frm(1),heel_1st), vecA )
      end if
      CP.stride_length= VecMag( vecA )
      error= VecNorm(vecA,NormVecA)
      if ( error ) goto 1020
      if ( CP.stride_length .lt. 0.0001 ) goto 1020
c                                         
c     Calculate step length.  Step length is basically defined as the distance
c     along the vector from first to third heel strikes split at the point
c     where a line passing through the second heel strike and perpendicular
c     to the first-third line.
c
      if ( UseFtLCS ) then
        call VecSub( SgpGCS(x,cntr,HS_frm(2),ft_2nd), 
     &               SgpGCS(x,cntr,HS_frm(1),ft_1st), vecB )
        call VecSub( SgpGCS(x,cntr,HS_frm(3),ft_1st),
     &               SgpGCS(x,cntr,HS_frm(2),ft_2nd), vecC )
      else if ( UseToeMrk ) then
        call VecSub( MrkData(x,HS_frm(2),toe_2nd), 
     &               MrkData(x,HS_frm(1),toe_1st), vecB )
        call VecSub( MrkData(x,HS_frm(3),toe_1st),
     &               MrkData(x,HS_frm(2),toe_2nd), vecC )
      else if ( UseAklMrk ) then
        call VecSub( MrkData(x,HS_frm(2),ankle_2nd), 
     &               MrkData(x,HS_frm(1),ankle_1st), vecB )
        call VecSub( MrkData(x,HS_frm(3),ankle_1st),
     &               MrkData(x,HS_frm(2),ankle_2nd), vecC )
      else if ( UseHeelMrk ) then
        call VecSub( MrkData(x,HS_frm(2),heel_2nd), 
     &               MrkData(x,HS_frm(1),heel_1st), vecB )
        call VecSub( MrkData(x,HS_frm(3),heel_1st),
     &               MrkData(x,HS_frm(2),heel_2nd), vecC )
      end if
      CP.step_length(side_2nd)= abs(VecDot( vecB,NormVecA ))
      CP.step_length(side_1st)= abs(VecDot( vecC,NormVecA ))
c                                                    
c     Calculate step width.  This is the distance along the perpendicular
c     used in calculating step length from the second heel strike point
c     to the stride length line (first-third heel strike line).
c
      temp= VecMag(vecB)**2 - CP.step_length(side_2nd)**2
c      if ( temp .lt. 0.0 ) goto 1030 ! error    
      if ( temp .gt. 0 ) then
        CP.step_width= sqrt( temp )
      else
        CP.step_width= -sqrt( abs(temp) ) ! indicates trouble
      end if
c
c     Calculate cycle time
c
      if ( rMtnFrmRate .lt. 0.0001 ) goto 1040 ! error
      CP.cycle_time= float( HS_frm(3)-HS_frm(1)+1 )/rMtnFrmRate
c
c     Calculate cadence           
c
      if ( CP.cycle_time .lt. 0.0001 ) goto 1050 ! error
      CP.cadence= 60.0*2.0/CP.cycle_time
c
c     Calculate average velocity of pelvis based upon the position of its
c     LCS at heel strike
c
      if ( GP.Analysis_Type .ne. TDP_ANL ) then
        call VecSub( SgpGCS(x,cntr,HS_frm(3),pelv),
     &               SgpGCS(x,cntr,HS_frm(1),pelv), vecF )
      else  
        if ( UseToeMrk ) then
          call VecSub( MrkData(x,HS_frm(3),toe_1st),
     &                 MrkData(x,HS_frm(1),toe_1st), vecF )
        else if ( UseHeelMrk ) then
          call VecSub( MrkData(x,HS_frm(3),heel_1st),
     &                 MrkData(x,HS_frm(1),heel_1st), vecF )
        else
          call VecSub( MrkData(x,HS_frm(3),ankle_1st),
     &                 MrkData(x,HS_frm(1),ankle_1st), vecF )
        end if
      end if
      CP.velocity= abs(VecMag(vecF))/CP.cycle_time
c                                     
c     Calculate single and double stance and swing times
c             
      CP.dbl_time(side_1st)=    float(TO_frm(1)-HS_frm(1)+1)/rMtnFrmRate
      CP.dbl_percent(side_1st)= ( CP.dbl_time(side_1st)/
     &                                  CP.cycle_time )*100.
      CP.sgl_time(side_1st)=    float(HS_frm(2)-TO_frm(1)+1)/rMtnFrmRate
      CP.sgl_percent(side_1st)= ( CP.sgl_time(side_1st)/
     &                                  CP.cycle_time )*100.
      CP.dbl_time(side_2nd)=    float(TO_frm(2)-HS_frm(2)+1)/rMtnFrmRate
      CP.dbl_percent(side_2nd)= ( CP.dbl_time(side_2nd)/
     &                                  CP.cycle_time )*100.
      CP.sgl_time(side_2nd)=    float(HS_frm(3)-TO_frm(2)+1)/rMtnFrmRate
      CP.sgl_percent(side_2nd)= ( CP.sgl_time(side_2nd)/
     &                                  CP.cycle_time )*100.
      do side= rt, lf
        CP.total_time(side)= CP.sgl_time(side) + 
     &                             CP.dbl_time(1) + CP.dbl_time(2)
        CP.total_percent(side)= CP.sgl_percent(side) + 
     &                                CP.dbl_percent(1) +
     &                                CP.dbl_percent(2)
      end do ! side
      CP.hs_percent(1)= 0.0
      do i= 1, 2
        CP.HS_percent(i+1)= ( (HS_frm(i+1)-HS_frm(1)+1)/
     &                              float(MP.cycle_length(1)) )*100.
        CP.TO_percent(i)=   ( (TO_frm(i)-HS_frm(1)+1)/
     &                              float(MP.cycle_length(1)) )*100.
      end do                            
c                                                                 
c     Do percent normal according to age of patient
c                                             
      i= 21                        
      do while ( (GP.Age .lt. NormAge(i)) .and. (i .ge. 1) )
        i= i - 1
      end do                                               
      if ( i .le. 0 ) i= 1                 
      CP.NormStrideLength=   2.0*NormStep(i)
      CP.NormStepLength(rt)= NormStep(i)
      CP.NormStepLength(lf)= NormStep(i)
      CP.NormStepWidth=      NormWidth(i)
      CP.NormCadence=        NormCadence(i)
      CP.NormVelocity=       NormVel(i)
      NormCycleTime=               2.0*NormStep(i)/NormVel(i)
      CP.NormCycleTime=      NormCycleTime
      CP.PctNormStrideLength=   0.5*CP.stride_length/NormStep(i)*100.0
      CP.PctNormStepLength(rt)= CP.step_length(rt)/NormStep(i)*100.0
      CP.PctNormStepLength(lf)= CP.step_length(lf)/NormStep(i)*100.0
      CP.PctNormStepWidth=      CP.step_width/NormWidth(i)*100.0
      CP.PctNormCadence=        CP.cadence/NormCadence(i)*100.0
      CP.PctNormVelocity=       CP.velocity/NormVel(i)*100.0
      CP.PctNormCycleTime=      CP.cycle_time/NormCycleTime*100.0
      do side= rt, lf
        CP.NormSglPct(side)=   NormSingle
        CP.NormDblPct(side)=   NormDouble
        CP.NormTotalPct(side)= NormTotal
      end do 
      CP.NormHsPct(1)= 0.0
      CP.NormHsPct(2)= NormOFS
      CP.NormHsPct(3)= 100.0
      CP.NormToPct(1)= NormOTO
      CP.NormToPct(2)= NormTO
c                                                              
c     write out the gait cycle parameters just calculated
c                                                          
      write(6,6) NormAge(i)
6     format(/' Gait cycle parameters'//
     &        ' Using normal data for ',f6.2,' years old'/)                   
      write(6,8) CP.velocity,          CP.velocity*feet,
     &           CP.velocity*inch,
     &           CP.NormVelocity,      CP.PctNormVelocity,
     &           CP.stride_length,     CP.stride_length*feet,
     &           CP.stride_length*inch,
     &           CP.NormStrideLength,  CP.PctNormStrideLength,
     &           CP.step_length(rt),   CP.step_length(rt)*feet,
     &           CP.step_length(rt)*inch,
     &           CP.NormStepLength(rt),CP.PctNormStepLength(rt),
     &           CP.step_length(lf),   CP.step_length(lf)*feet,
     &           CP.step_length(lf)*inch,
     &           CP.NormStepLength(lf),CP.PctNormStepLength(lf),
     &           CP.step_width,        CP.step_width*feet,
     &           CP.step_width*inch,
     &           CP.NormStepWidth,     CP.PctNormStepWidth,
     &           CP.cadence,        
     &           CP.NormCadence,       CP.PctNormCadence
8     format(16x,'meters',8x,'feet',8x,'inches',6x,'Normal(m)',3x,'% Normal'//
     &       ' Velocity   : ',5(f8.3,5x)/
     &       ' Stride     : ',5(f8.3,5x)/
     &       ' R Step     : ',5(f8.3,5x)/
     &       ' L Step     : ',5(f8.3,5x)/          
     &       ' Step Width : ',5(f8.3,5x)//
     &       ' Cadence    : ',f8.3,' step/min',22x,2(f8.3,5x)/)
      write(6,10) CP.total_time(rt),CP.total_percent(rt),
     &            CP.NormTotalPct(rt),
     &            CP.total_time(lf),CP.total_percent(lf),
     &            CP.NormTotalPct(lf),
     &            CP.sgl_time(rt),  CP.sgl_percent(rt),
     &            CP.NormSglPct(rt),
     &            CP.sgl_time(lf),  CP.sgl_percent(lf),
     &            CP.NormSglPct(lf),
     &            CP.dbl_time(rt),  CP.dbl_percent(rt),
     &            CP.NormDblPct(rt),
     &            CP.dbl_time(lf),  CP.dbl_percent(lf),
     &            CP.NormDblPct(lf)
10    format('            Time (s)        Percent Cycle        Normal'/
     &       ' R TLST  ',3(f8.3,11x)/
     &       ' L TLST  ',3(f8.3,11x)/
     &       ' R SLST  ',3(f8.3,11x)/
     &       ' L SLST  ',3(f8.3,11x)/
     &       ' R DLS   ',3(f8.3,11x)/
     &       ' L DLS   ',3(f8.3,11x)/)       
      if ( CP.first_side .eq. rt ) then
        write(6,12) CP.HS_percent(1),CP.NormHsPct(1),
     &              CP.TO_percent(1),CP.NormToPct(1),
     &              CP.HS_percent(2),CP.NormHsPct(2),
     &              CP.TO_percent(2),CP.NormToPct(2),
     &              CP.HS_percent(3),CP.NormHsPct(3)
12    format(' RHS                        ',f8.3,11x,f8.3/
     &       ' LTO                        ',f8.3,11x,f8.3/
     &       ' LHS                        ',f8.3,11x,f8.3/
     &       ' RTO                        ',f8.3,11x,f8.3/
     &       ' RHS                        ',f8.3,11x,f8.3/ )
      else
        write(6,14) CP.HS_percent(1),CP.NormHsPct(1),
     &              CP.TO_percent(1),CP.NormToPct(1),
     &              CP.HS_percent(2),CP.NormHsPct(2),
     &              CP.TO_percent(2),CP.NormToPct(2),
     &              CP.HS_percent(3),CP.NormHsPct(3)
14    format(' LHS                        ',f8.3,11x,f8.3/
     &       ' RTO                        ',f8.3,11x,f8.3/
     &       ' RHS                        ',f8.3,11x,f8.3/
     &       ' LTO                        ',f8.3,11x,f8.3/
     &       ' LHS                        ',f8.3,11x,f8.3/ )
      end if ! first_side
      if ( OF.ToLog ) then
        write(3,6) NormAge(i)
        write(3,8) CP.velocity,          CP.velocity*feet,
     &             CP.velocity*inch,
     &             CP.NormVelocity,      CP.PctNormVelocity,
     &             CP.stride_length,     CP.stride_length*feet,
     &             CP.stride_length*inch,
     &             CP.NormStrideLength,  CP.PctNormStrideLength,
     &             CP.step_length(rt),   CP.step_length(rt)*feet,
     &             CP.step_length(rt)*inch,
     &             CP.NormStepLength(rt),CP.PctNormStepLength(rt),
     &             CP.step_length(lf),   CP.step_length(lf)*feet,
     &             CP.step_length(lf)*inch,
     &             CP.NormStepLength(lf),CP.PctNormStepLength(lf),
     &             CP.step_width,        CP.step_width*feet,
     &             CP.step_width*inch,
     &             CP.NormStepWidth,     CP.PctNormStepWidth,
     &             CP.cadence,        
     &             CP.NormCadence,       CP.PctNormCadence
        write(3,10) CP.total_time(rt),CP.total_percent(rt),
     &              CP.NormTotalPct(rt),
     &              CP.total_time(lf),CP.total_percent(lf),
     &              CP.NormTotalPct(lf),
     &              CP.sgl_time(rt),  CP.sgl_percent(rt),
     &              CP.NormSglPct(rt),
     &              CP.sgl_time(lf),  CP.sgl_percent(lf),
     &              CP.NormSglPct(lf),
     &              CP.dbl_time(rt),  CP.dbl_percent(rt),
     &              CP.NormDblPct(rt),
     &              CP.dbl_time(lf),  CP.dbl_percent(lf),
     &              CP.NormDblPct(lf),
     &              CP.dbl_time(lf),  CP.dbl_percent(lf)
        if ( CP.first_side .eq. rt ) then
          write(3,12) CP.HS_percent(1),CP.NormHsPct(1),
     &                CP.TO_percent(1),CP.NormToPct(1),
     &                CP.HS_percent(2),CP.NormHsPct(2),
     &                CP.TO_percent(2),CP.NormToPct(2),
     &                CP.HS_percent(3),CP.NormHsPct(3)
        else
          write(3,14) CP.HS_percent(1),CP.NormHsPct(1),
     &                CP.TO_percent(1),CP.NormToPct(1),
     &                CP.HS_percent(2),CP.NormHsPct(2),
     &                CP.TO_percent(2),CP.NormToPct(2),
     &                CP.HS_percent(3),CP.NormHsPct(3)
        end if
      end if ! log_file                     
      CF.statistics= .true.
      return

1000  write(6,1005)
      if ( OF.ToLog ) write(3,1005)
1005  format(' Error- Gait cycle events have not been input yet.')
      return
      
1010  write(6,1015)
      if ( OF.ToLog ) write(3,1015)
1015  format(' Error- Improper segment position data present.'/
     &       '        The rtft, lfft, & pelv segment positions must be present')
      return
      
1020  write(6,1025)
      if ( OF.ToLog ) write(3,1025)
1025  format(' Error- Stride length has been computed to be less or equal to 0.')
      return
      
1030  write(6,1035)
      if ( OF.ToLog ) write(3,1035)
1035  format(' Error- Step width calculation invalid.')
      return
      
1040  write(6,1045)
      if ( OF.ToLog ) write(3,1045)
1045  format(' Error- Motion data frame rate less than or equal to 0.')
      return
      
1050  write(6,1055)
      if ( OF.ToLog ) write(3,1055)
1055  format(' Error- Cycle time calculated to be 0.')
      return

1060  write(6,1065) MP.num_mrk
      if ( OF.ToLog ) write(3,1065) MP.num_mrk
1065  format(' Error- To use the TimeDistanceAnalysis there must be 6 markers'/
     &       '        The present number of markers is ',i2)
      return

1070  write(6,1075)
      if ( OF.ToLog ) write(3,1075)
1075  format(' Error- Analysis Type is set to time distance parameters,'/
     &       '        but the positions of the 6 markers have not been read yet')
      return

1080  write(6,1085)
      if ( OF.ToLog ) write(3,1085)
1085  format(' Error- Analysis Type must set to OSU Full Body, Helen Hayes, or'/
     &       '        time distance foot marker set for gait cycle analysis to'/
     &       '        be valid')
      return
      end                                                                      
