module TopDriveMain
    use CLog4
    implicit none
    public
    contains
    
    subroutine TopDrive_Setup()
        use CSimulationVariables
        implicit none
        call OnSimulationStop%Add(TopDrive_Stop)
        call OnTopDriveStart%Add(TopDrive_Start)
        call OnTopDriveStep%Add(TopDrive_Step)
        call OnTopDriveMain%Add(TopDriveMainBody)
    end subroutine
    
    subroutine TopDrive_Stop
        implicit none
        call Log_4('TopDrive_Stop')
    end subroutine TopDrive_Stop
    
    subroutine TopDrive_Start
        implicit none
        call Log_4('TopDrive_Start')
    end subroutine TopDrive_Start
    
    subroutine TopDrive_Step
        implicit none
        call Log_4('TopDrive_Step')
    end subroutine TopDrive_Step
    
    
    
    
    subroutine TopDriveMainBody
       
        !use CDataDisplayConsoleVariables
        !use CDrillingConsoleVariables
        use CSimulationVariables
        use TopDrive_VARIABLES
        use CDrillWatchVariables
        use CWarningsVariables
        use CSounds
        use CTopDrivePanelVariables
        
        implicit none
        
        integer,dimension(8) :: TDS_START_TIME, TDS_END_TIME
        INTEGER :: TDS_SolDuration
        
        call Log_4('TopDriveMainBody')
        
        Call TopDrive_StartUp
        loopTopDrivestart : do
        
        call sleepqq(10)
        
        !if ( (TopDriveTdsPowerState==-1) .and. (RpmKnob==0.) ) then  !FWD
        if ( (TopDriveTdsPowerState==-1) ) then  !FWD
            
            
            TDS%SoundBlower = .true.
            !Call SetSoundBlowerRT(TDS%SoundBlower)
            TopDriveTdsPowerLed = 1 
            
            loopTopDriveswitchFWD: do
                
                CALL DATE_AND_TIME(values=TDS_START_TIME)
                
                !IF ( RTTransmissionLever /=0 .and. RotaryGearsAbuse==0 ) THEN  !be in clutch mode  ????
                    TDS%N_new = (RpmKnob/250.d0)*965.d0          ! 0<RpmKnob<250  , 0<TDS%N_ref(truction motor)<965
                    !print*, 'TDSN=', RpmKnob , TDS%N_new
                    
                    !===> Top Drive Malfunction ----> Drive Motor Failure
                    call TopDrive_Malfunction_MotorFailure
                    
                    if (((TDS%N_new-TDS%N_old)/TDS%time_step)>193.) then
                        TDS%N_ref = (193.*TDS%time_step)+TDS%N_old
                    else if (((TDS%N_old-TDS%N_new)/TDS%time_step)>193.) then
                        TDS%N_ref = (-193.*TDS%time_step)+TDS%N_old
                    else
                        TDS%N_ref = TDS%N_new
                    end if
                    CALL TopDrive_INPUTS
                    CALL TopDrive_Solver
                    TDS%N_old = TDS%N_ref
               
                
                if ( (TopDriveTdsPowerState/=-1) .or. (IsStopped == .true.) ) then
                    TDS%SoundBlower = .false.
                    !Call SetSoundBlowerRT(TDS%SoundBlower)
                    TopDriveTdsPowerLed = 0
                    Call TopDrive_OffMode
                    exit loopTopDriveswitchFWD
                end if
                
                CALL DATE_AND_TIME(values=TDS_END_TIME)
                TDS_SolDuration = 100-(TDS_END_TIME(5)*3600000+TDS_END_TIME(6)*60000+TDS_END_TIME(7)*1000+TDS_END_TIME(8)-TDS_START_TIME(5)*3600000-TDS_START_TIME(6)*60000-TDS_START_TIME(7)*1000-TDS_START_TIME(8))
                !print*, 'TDStime=', TDS_SolDuration
                if(TDS_SolDuration > 0.0) then
                    CALL sleepqq(TDS_SolDuration)
                end if
            
            end do  loopTopDriveswitchFWD
            
            
        !else if ( (TopDriveTdsPowerState==1) .and. (RpmKnob==0.) ) then   !REV
            else if ( (TopDriveTdsPowerState==1) ) then   !REV
            
            
            TDS%SoundBlower = .true.
            !Call SetSoundBlowerRT(TDS%SoundBlower)
            TopDriveTdsPowerLed = 1
            
            loopTopDriveswitchREV: do
                
                CALL DATE_AND_TIME(values=TDS_START_TIME)
                
                    TDS%N_new = (RpmKnob/250.d0)*965.d0
                    
                    !===> Top Drive Malfunction ----> Drive Motor Failure
                    call TopDrive_Malfunction_MotorFailure
                    
                    if (((TDS%N_new-TDS%N_old)/TDS%time_step)>193.) then
                        TDS%N_ref = (193.*TDS%time_step)+TDS%N_old
                    else if (((TDS%N_old-TDS%N_new)/TDS%time_step)>193.) then
                        TDS%N_ref = (-193.*TDS%time_step)+TDS%N_old
                    else
                        TDS%N_ref = TDS%N_new
                    end if
                    CALL TopDrive_INPUTS
                    CALL TopDrive_Solver
                    TDS%N_old = TDS%N_ref
             
                
                if ( (TopDriveTdsPowerState/=1) .or. (IsStopped == .true.) ) then
                    TDS%SoundBlower = .false.
                    !Call SetSoundBlowerRT(TDS%SoundBlower)
                    TopDriveTdsPowerLed = 0
                    Call TopDrive_OffMode
                    exit loopTopDriveswitchREV
                end if
                
                CALL DATE_AND_TIME(values=TDS_END_TIME)
                TDS_SolDuration = 100-(TDS_END_TIME(5)*3600000+TDS_END_TIME(6)*60000+TDS_END_TIME(7)*1000+TDS_END_TIME(8)-TDS_START_TIME(5)*3600000-TDS_START_TIME(6)*60000-TDS_START_TIME(7)*1000-TDS_START_TIME(8))
                !print*, 'TDStime=', TDS_SolDuration
                if(TDS_SolDuration > 0.0) then
                    CALL sleepqq(TDS_SolDuration)
                end if
            
            end do  loopTopDriveswitchREV
        
        
        else
            if( TopDriveTdsPowerState /= 0 ) then
                TDS%SoundBlower = .true.
                !Call SetSoundBlowerRT(TDS%SoundBlower)
                TopDriveTdsPowerLed = 1
            else
                TDS%SoundBlower = .false.
                !Call SetSoundBlowerRT(TDS%SoundBlower)
                TopDriveTdsPowerLed = 0
            end if
            Call TopDrive_OffMode
            !exit loopTopDriveswitchFWD
        
        
        end if
        
        if (IsStopped == .true.) then
            exit loopTopDrivestart
        end if
    
        end do loopTopDrivestart
        
    end subroutine TopDriveMainBody



end module TopDriveMain