Imports System.Math
Imports System.Threading
Imports System.Drawing

Public Module modCourseCorrection

    Dim boolCourseCorrectionBusy As Boolean = False
    Public Sub subCourseCorrection_Top()
        '  localize while moving and issue course corrections
        Try
            If boolDebugCC Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ======================= subCourseCorrection_Top:  boolCourseCorrectionBusy = " & boolCourseCorrectionBusy)
            If tmiMoveInfo.boolBotIsMoving And Not boolCourseCorrectionBusy And Not boolSimulatedMovement And boolER1Selected And _
                        (boolURGConnected Or boolKinectConnected) And Not boolStop And Not boolEnableRBPF And Not boolSlam And Not boolPause Then
                boolCourseCorrectionBusy = True
                Dim boolValidLocation As Boolean
                Dim timeLocalizationStart As DateTime
                Dim tbpScanPose As typeBotPose
                Dim tmiMoveInfoLocal As typeMoveInfo = ftnGetMoveInfo()
                If tmiMoveInfoLocal.boolBotIsMoving And (Abs(ftnTurnAngleV2(tmiMoveInfoLocal.tbpStartingPose, tmiMoveInfoLocal.tbpTargetPose)) < 145) _
                    And (tmiMoveInfoLocal.tomTypeOfMove <> TypeOfMove.TURN) Then 'Moving, but not backwards or turning
                    If boolDebugCC Then frmDebug.subWriteLogs(ftnElapsedTimeS() & "               subCourseCorrection_Top:  Valid Move")
                    If (ftnDistanceBetweenLocations(tmiMoveInfoLocal.tbpStartingPose.Location, tbpBotPose.Location) > sngBotLinearAccelerationDistance) Then '  
                        timeLocalizationStart = Now().AddMilliseconds(-70)  ' 70 ms represents the average age of the scan vectors (100 ms scan rate + 16 ms tick time ++)
                        Dim tbpDR As typeBotPose = ftnLocalizeUsingDeadReckoning(tbpBotPose, 0, False)
                        tbpScanPose = ftnLocalizeFrom2DScan_Top(tbpBotPose, tbpBotPose, intBestParticle, True, boolValidLocation, 0, (sngCcDelayS < 0.35))
                        If boolValidLocation Then subCourseCorrect(timeLocalizationStart, tbpScanPose, tbpDR)
                    End If
                End If
                boolCourseCorrectionBusy = False
            End If  ' 
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subCourseCorrection_Top :  " & ex.ToString)
            boolCourseCorrectionBusy = False
        End Try
    End Sub



    Sub subCourseCorrect(ByRef timeLocalizationStart As DateTime, ByRef tbpScanPose As typeBotPose, ByRef tbpDR As typeBotPose) ' perform the course correction
        Try
            Thread.CurrentThread.Priority = ThreadPriority.Highest
            If boolDebugCC Then
                frmDebug.subWriteShortPose(ftnElapsedTimeS() & "subCourseCorrect:    tbpDR       ", tbpDR)
                frmDebug.subWriteShortPose(ftnElapsedTimeS() & "subCourseCorrect:    tbpScanPose ", tbpScanPose)
            End If
            If tmiMoveInfo.boolBotIsMoving And Not boolStop And Not boolShutDown And Not tmiMoveInfo.boolMoveCommandInProgress Then ' 
                If (ftnDistanceBetweenLocations(tmiMoveInfo.tbpTargetPose.Location, tbpBotPose.Location) > (sngBotLinearAccelerationDistance + ftnCmToInches(sngBotLinearVelocity) * sngCcDelayS)) Then ' course correction is possible 
                    Dim tbpStartingPoseDR As typeBotPose = ftnLocalizeUsingDeadReckoning(tbpBotPose, -sngCcDelayS, False) ' Predict where the Bot will be when the move correction is issued
                    Dim tbpStartingPosePlanned As typeBotPose = tbpScanPose
                    tbpStartingPosePlanned.Location.X += tbpStartingPoseDR.Location.X - tbpDR.Location.X
                    tbpStartingPosePlanned.Location.Y += tbpStartingPoseDR.Location.Y - tbpDR.Location.Y
                    tbpStartingPosePlanned.Orientation = ftnConvertTo360(tbpScanPose.Orientation + tbpStartingPoseDR.Orientation - tbpDR.Orientation)
                    ftnFindPathToDestination(tmcDestination, tbpStartingPosePlanned.Location)  '
                    Dim tbpNexttarget As typeBotPose = ftnFindTargetPose(tbpStartingPosePlanned, tmcDestination)
                    Dim tomTypeOfMoveCorrection As TypeOfMove = ftnTypeOfMove(tbpStartingPosePlanned, tbpNexttarget)
                    If (ftnDistanceBetweenLocations(tbpNexttarget.Location, tbpStartingPosePlanned.Location) > sngBotLinearAccelerationDistance) And
                        Not tmiMoveInfo.boolMoveCommandInProgress And
                        Not ftnLocationsEqual(tbpStartingPosePlanned.Location, tmiMoveInfo.tbpStartingPose.Location) Then ' perform course correction
                        Select Case tomTypeOfMoveCorrection  ' start move correction
                            Case TypeOfMove.FWD
                                ftnBotFwd(tbpStartingPosePlanned, tbpNexttarget)
                                sngCcDelayS = (sngCcDelayS + Now().Subtract(timeLocalizationStart).TotalSeconds() - ftnElapsedTimeS()) / 2
                            Case TypeOfMove.CURVE
                                tbpNexttarget = ftnCurve_DR(tbpStartingPosePlanned, 99999, tbpNexttarget, False) ' Calculate the orientation
                                ftnBotCurve(tbpStartingPosePlanned, tbpNexttarget)
                                sngCcDelayS = (sngCcDelayS + Now().Subtract(timeLocalizationStart).TotalSeconds() - ftnElapsedTimeS()) / 2
                        End Select
                        If boolDebugCC Then
                            frmDebug.subWriteLogs(ftnElapsedTimeS() & " ##############  Course Corrected:  ##############  ")
                            frmDebug.subWriteLogs("       tbpDR =                    " & tbpDR.Location.X & ", " & tbpDR.Location.Y & " at " & tbpDR.Orientation & " degrees.")
                            frmDebug.subWriteLogs("       ScanPose =                 " & tbpScanPose.Location.X & ", " & tbpScanPose.Location.Y & " at " & tbpScanPose.Orientation & " degrees.")
                            frmDebug.subWriteLogs("       tbpStartingPoseDR =  " & tbpStartingPoseDR.Location.X & ", " & tbpStartingPoseDR.Location.Y & " at " & tbpStartingPoseDR.Orientation & " degrees.")
                            frmDebug.subWriteLogs("       tbpStartingPosePlanned =   " & tbpStartingPosePlanned.Location.X & ", " & tbpStartingPosePlanned.Location.Y & " at " & tbpStartingPosePlanned.Orientation & " degrees.")
                            frmDebug.subWriteLogs("       tbpNexttarget =            " & tbpNexttarget.Location.X & ", " & tbpNexttarget.Location.Y & " at " & tbpNexttarget.Orientation & " degrees.")
                            frmDebug.subWriteLogs("       sngCcDelayS = " & sngCcDelayS)
                            frmDebug.subWriteLogs("       tomTypeOfMoveCorrection = " & tomTypeOfMoveCorrection.ToString)
                        End If
                    Else
                        If boolDebugCC Then
                            frmDebug.subWriteLogs(ftnElapsedTimeS() & " -------------  No Course Correction:  ")
                            frmDebug.subWriteLogs("       tbpBotPose =  " & tbpBotPose.Location.X & ", " & tbpBotPose.Location.Y & " at " & tbpBotPose.Orientation & " degrees.")
                            frmDebug.subWriteLogs("       tbpNexttarget =  " & tbpNexttarget.Location.X & ", " & tbpNexttarget.Location.Y & " at " & tbpNexttarget.Orientation & " degrees.")
                            frmDebug.subWriteLogs("       tomTypeOfMoveCorrection = " & tomTypeOfMoveCorrection.ToString)
                        End If
                    End If
                End If
            End If
            Thread.CurrentThread.Priority = ThreadPriority.Normal
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subCourseCorrect :  " & ex.ToString)
        End Try
    End Sub





End Module
