
Imports System.Math
Imports System.Threading


Module modDriveToDestination

    ' This module determines the driving commands required for the Bot
    ' to reach a specific destination

    ' Examples of  driving commands:
    '      move 27 inches
    '      move -12 cm
    '      move 27 degrees
    '      stop


    Public Function ftnDriveToUserChosenDestination() As Boolean
        Try
            ftnDriveToUserChosenDestination = False
            Thread.CurrentThread.Name = "threadDrive"
            If ftnInWall_NoChk(tmcDestination, intBestParticle) Or (intDistanceFromWallSquaredArray(intBestParticle, tmcDestination.X, tmcDestination.Y) < intSafeDistanceFromWallSquared) And Not boolSlam Then
                strStatusDriving = "In Wall"
                ftnSendCommand("play phrase ' I'm sorry Dave. I'm afraid I can't do that '")
            Else
                boolStop = False
                boolDock = False
                frmDebug.subWriteLogs(vbCrLf & vbCrLf & vbCrLf & " ******************************************************************************************" & vbCrLf)
                frmDebug.subWriteLogs(" GOTABOT       (Version " & My.Application.Info.Version.ToString(3) & ")")
                ftnSendCommand("play phraseA OK")
                ftnDriveToUserChosenDestination = ftnDriveToDestination_Top(tbpBotPose, tmcDestination) ' Drive to destination
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnDriveToUserChosenDestination :  " & ex.ToString)
        End Try
    End Function


    Public Function ftnDriveToDestination_Top(ByVal tbpEstimatedPose As typeBotPose, ByVal tmcDestination As typeMapCoordinate) As Boolean
        Try
            ftnDriveToDestination_Top = False
            If Not ftnCheckForStopMode() And Not boolDrivingToDestination Then
                subCheckForPause()
                frmDebug.subWriteLogs(Now() & " Drive to new destination: " & tmcDestination.X & ", " & tmcDestination.Y)
                strStatusDriving = "Drive to " & tmcDestination.X & ", " & tmcDestination.Y
                tbpBotPose = tbpEstimatedPose
                If tmiMoveInfo.boolBotIsMoving Then
                    frmDebug.subWriteLogs(Now() & " ftnDriveToDestination_Top:  WARNING: tmiMoveInfo.boolBotIsMoving = TRUE when called")
                    tbpBotPose = ftnLocalizeUsingDeadReckoning(tbpBotPose, 0, True)
                    If tmiMoveInfo.boolBotIsMoving Then tbpBotPose.OrientationUncertainty += 15
                    tbpBotPose = ftnBotStop(tbpBotPose, tmiMoveInfo.tbpTargetPose)
                End If
                subInitializeDrivingControl(tbpBotPose)  ' initialize
                timeDrivingStarted = Now()
                ftnSendCommand("stop")
                intDrivingSequenceNumber = 0
                frmDock.subUnDock(tbpBotPose)
                If boolMapUpdatedWithObstacle Then ' remove obstacle
                    subConvertMapToArray(intBestParticle)
                    boolMapUpdatedWithObstacle = False
                End If
                subGeneratePathArray()
                ftnFindPathToDestination(tmcDestination, tbpBotPose.Location)
                boolDrivingToDestination = True
                subDriveToDestinationLoop(tmcDestination)
                ftnDriveToDestination_Top = (ftnDistanceBetweenLocations(tbpBotPose.Location, tmcDestination) <= sngBotDiameter / 3)
                ftnBotStop(tbpBotPose, tbpBotPose)
                If Not boolSlam And Not ftnDriveToDestination_Top Then
                    frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ===========================  FAILED to reach destination  ")
                    ftnSendCommand("play phraseA 'warning.  failed to reach destination'")
                ElseIf Not boolDock And Not frmArm.boolGetObjectBusy Then
                    frmDebug.subWriteFinishedMessage()
                    strStatusDriving = intDrivingSequenceNumber & " : Finished Driving "
                    If Not boolSlam And (ftnDistanceBetweenLocations(tbpBotPose.Location, tmcDestination) <= (sngBotDiameter / 6)) Then
                        ftnSendCommand("play phraseA 'nailed it.'")
                    ElseIf Not boolSlam Then
                        ftnSendCommand("play phraseA 'close enough'")
                    End If
                End If
                ftnSendCommand("set power stopped " & intPowerStopped / 3)
            End If
            boolDrivingToDestination = False
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnDriveToDestination_Top :  " & ex.ToString)
        End Try
    End Function

    Sub subDriveToDestinationLoop(ByVal tmcDestination As typeMapCoordinate)
        Try
            Dim boolGoodPath As Boolean = True
            Dim tbpTargetPose As typeBotPose
            Dim sngDestinationError As Single
            If boolSlam Then
                sngDestinationError = sngBotDiameter * 0.5
            Else
                sngDestinationError = sngBotDiameter * 0.33333
            End If
            Do While ((ftnDistanceBetweenLocations(tbpBotPose.Location, tmcDestination) > sngDestinationError) _
                             And Not boolStop And boolGoodPath And boolDrivingToDestination)
                If boolDebugDrive Then frmDebug.subWriteLogs(ftnElapsedTimeS() & "          <<<<<  subDriveToDestinationLoop  Do START      ")
                subCheckForPause()

                If boolDebugDrive Then frmDebug.subWritePose(ftnElapsedTimeS() & " subDriveToDestinationLoop: " & " tbpBotPose  ", tbpBotPose)
                tbpTargetPose = ftnFindTargetPose(tbpBotPose, tmcDestination) ' 
                If boolDebugDrive Or boolDebugPath Then frmDebug.subWritePose(ftnElapsedTimeS() & " subDriveToDestinationLoop: " & "  Target Pose  ", tbpTargetPose)
                If Not tmiMoveInfo.boolBotIsMoving Then tbpBotPose = ftnControlDriving(tbpBotPose, tbpTargetPose) '  
                If Not (boolSlam And Not boolDrivingToDestination) Then tbpBotPose = ftnPostMoveLocalizeAndPath(tbpBotPose, tmiMoveInfo.tomTypeOfMove, tmiMoveInfo.tbpTargetPose)
            Loop  ' 
            If boolDebugDrive Then

            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDriveToDestinationLoop :  " & ex.ToString)
        End Try
    End Sub


    Function ftnControlDriving(ByVal tbpEstimatedPose As typeBotPose, ByVal tbpTargetPose As typeBotPose) As typeBotPose
        Try
            ftnControlDriving = tbpEstimatedPose
            If boolDebugDrive Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnControlDriving  START  ")
            Dim intTurnAngle As Integer = ftnTurnAngleV2(tbpEstimatedPose, tbpTargetPose)
            Dim tomTypeOfMove As TypeOfMove = ftnTypeOfMove(tbpEstimatedPose, tbpTargetPose)
            If boolDebugDrive Or boolDebugPath Then frmDebug.subWriteLogs(" ftnControlDriving: tomTypeOfMove = " & tomTypeOfMove.ToString)
            If boolStop Then
                ftnControlDriving = ftnLocalizeUsingDeadReckoning(tbpEstimatedPose, 0, True)
                If tmiMoveInfo.boolBotIsMoving Then ftnControlDriving.OrientationUncertainty += 15
                ftnControlDriving = ftnBotStop(ftnControlDriving, tbpTargetPose)
            ElseIf Not tmiMoveInfo.boolBotIsMoving And (tomTypeOfMove <> TypeOfMove.TURN) And ftnObstacleDetected(tbpEstimatedPose, tomTypeOfMove, tbpBotPose, tbpTargetPose) Then  ' 
                Dim timeOut As DateTime = Now().AddMilliseconds(6000)
                While (Now() < timeOut) And ftnObstacleDetected(tbpEstimatedPose, tomTypeOfMove, tbpBotPose, tbpTargetPose)
                    subWait(1000)
                    ftnSendCommand("play phrase notify")
                End While
                If ftnObstacleDetected(tbpEstimatedPose, tomTypeOfMove, tbpBotPose, tbpTargetPose) Then
                    ftnAddObstacleToMap(tbpEstimatedPose, tomTypeOfMove, tbpEstimatedPose, tbpTargetPose)
                    ftnSendCommand("play phrase prompt")
                End If
            ElseIf Not boolStop Then
                Select Case tomTypeOfMove
                    Case TypeOfMove.TURN
                        Dim sngDistanceToTarget As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tbpTargetPose.Location)
                        If (Abs(intTurnAngle) < 135) Or (sngDistanceToTarget > 10) Or boolSlam Then
                            tbpTargetPose = tbpEstimatedPose
                            tbpTargetPose.Orientation = ftnConvertTo360(ftnControlDriving.Orientation + intTurnAngle)
                            ftnControlDriving = ftnBotTurn(ftnControlDriving, True, tbpTargetPose)
                        Else
                            frmDebug.subWriteLogs(ftnElapsedTimeS() & " ***** Turn and Backup: . ***************************")
                            tbpTargetPose.Orientation = ftnConvertTo360(ftnControlDriving.Orientation + intTurnAngle - 180)
                            ftnControlDriving = ftnBotTurn(ftnControlDriving, False, tbpTargetPose)  ' prepare to backup
                            ftnControlDriving = ftnBotFwd(ftnControlDriving, tbpTargetPose) ' backup
                        End If
                    Case TypeOfMove.FWD
                        ftnControlDriving = ftnBotFwd(ftnControlDriving, tbpTargetPose)
                    Case TypeOfMove.CURVE
                        tbpTargetPose = ftnCurve_DR(tbpEstimatedPose, 99999, tbpTargetPose, False) ' Calculate the orientation
                        ftnControlDriving = ftnBotCurve(ftnControlDriving, tbpTargetPose)
                    Case TypeOfMove.HALT
                        ftnControlDriving = ftnLocalizeUsingDeadReckoning(ftnControlDriving, 0, True)
                        If tmiMoveInfo.boolBotIsMoving Then ftnControlDriving.OrientationUncertainty += 15
                        ftnControlDriving = ftnBotStop(ftnControlDriving, tbpTargetPose)
                    Case Else
                        frmDebug.subWriteLogs(" **********************  ftnControlDriving:  Unexpected TypeOfMove")
                        ftnControlDriving = ftnLocalizeUsingDeadReckoning(ftnControlDriving, 0, True)
                        If tmiMoveInfo.boolBotIsMoving Then ftnControlDriving.OrientationUncertainty += 15
                        ftnControlDriving = ftnBotStop(ftnControlDriving, tbpTargetPose)
                End Select
            End If
            If boolDebugDrive Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnControlDriving  END  ")
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnControlDriving :  " & ex.ToString)
        End Try
    End Function



End Module
