
' Module which controls the driving of the robot. 

Imports System.Math
Imports System.Threading
Imports System.Drawing
Imports System.Windows.Forms


Module modBotMovementControl

    Dim tbpLastLocalizedPose As typeBotPose
    Dim intConsecutiveTurnCounter As Integer = 0  ' Counts the consecutive number of "Turn" moves.
    '                                                  Used to prevent excessive localization and new path generation 


    Public Sub subInitializeDrivingControl(ByRef tbpEstimatedPose As typeBotPose)
        Try
            tbpLastLocalizedPose = tbpEstimatedPose
            ftnSetMoveInfo(TypeOfMove.NONE, tbpEstimatedPose, tbpEstimatedPose, False)
            intMovementsSinceLocalization = 0
            boolCollisionDetectionEnable = True
            intConsecutiveTurnCounter = 0
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subInitializeDrivingControl :  " & ex.ToString)
        End Try
    End Sub

    Public Function ftnBotStop(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As typeBotPose
        Try
            strStatusDriving = intDrivingSequenceNumber & " : Stop: "
            tomTypeOfMovePlanned = TypeOfMove.HALT
            ftnBotStop = tbpEstimatedPose
            ftnExecuteMove(tbpEstimatedPose, "stop", tbpTargetPose)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnBotStop :  " & ex.ToString)
        End Try
    End Function

    Function ftnBotFwdToTarget(ByRef tbpEstimatedPose As typeBotPose, ByRef tmcTargetLocation As typeMapCoordinate) As typeBotPose
        Try
            Dim tbpTargetPose As typeBotPose = tbpEstimatedPose  ' Turn towards Target
            tbpTargetPose.Location = tmcTargetLocation
            ftnBotFwdToTarget = ftnBotFwd(tbpBotPose, tbpTargetPose)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnBotFwdToTarget :  " & ex.ToString)
        End Try
    End Function

    Function ftnBotFwdDistance(ByRef tbpEstimatedPose As typeBotPose, ByRef sngDistance As Single) As typeBotPose
        Try
            ftnBotFwdDistance = tbpEstimatedPose
            If Not boolStop Then
                Dim tbpTargetPose As typeBotPose = tbpEstimatedPose  ' drive forward sngDistance
                tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, sngDistance)
                tomTypeOfMovePlanned = TypeOfMove.FWD
                intConsecutiveTurnCounter = 0
                ftnBotFwdDistance = ftnExecuteMove(tbpEstimatedPose, "move " & sngDistance & " i", tbpTargetPose)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnBotFwdDistance :  " & ex.ToString)
        End Try
    End Function


    Function ftnBotFwd(ByRef tbpEstimatedPose As typeBotPose, ByVal tbpTargetPose As typeBotPose) As typeBotPose
        Try
            ftnBotFwd = tbpEstimatedPose
            If boolDebugDrive Then
                frmDebug.subWriteLogs(ftnElapsedTimeS() & "        : ftnBotFwd   tbpEstimatedPose =: " & tbpEstimatedPose.Location.X & ", " & tbpEstimatedPose.Location.Y & "  at " & tbpEstimatedPose.Orientation)
                frmDebug.subWriteLogs(ftnElapsedTimeS() & "        : ftnBotFwd   tbpTargetPose =: " & tbpTargetPose.Location.X & ", " & tbpTargetPose.Location.Y & "  at " & tbpTargetPose.Orientation)
            End If
            If Not boolStop Then
                tomTypeOfMovePlanned = TypeOfMove.FWD
                Dim sngDistanceToTarget As Single = Min(ftnDistanceCommanded(tbpEstimatedPose, tbpTargetPose), sngMaxTravel)
                If sngDistanceToTarget = 0 Then
                    sngDistanceToTarget = 2 'Error Recovery
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, sngDistanceToTarget)
                    frmDebug.subWriteLogs(ftnElapsedTimeS() & " ***** Path Failure:  Fwd 2 commanded ***************************")
                End If
                intConsecutiveTurnCounter = 0
                ftnBotFwd = ftnExecuteMove(tbpEstimatedPose, "move " & sngDistanceToTarget & " i", tbpTargetPose)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnBotFwd :  " & ex.ToString)
        End Try
    End Function

    Function ftnBotCurve(ByRef tbpEstimatedPose As typeBotPose, ByVal tbpTargetPose As typeBotPose) As typeBotPose
        Try
            intConsecutiveTurnCounter = 0
            tomTypeOfMovePlanned = TypeOfMove.CURVE
            If boolDebugDrive Then
                frmDebug.subWriteLogs(ftnElapsedTimeS() & "        : ftnBotCurve   tbpEstimatedPose =: " & tbpEstimatedPose.Location.X & ", " & tbpEstimatedPose.Location.Y & "  at " & tbpEstimatedPose.Orientation)
                frmDebug.subWriteLogs(ftnElapsedTimeS() & "        : ftnBotCurve   tbpTargetPose =: " & tbpTargetPose.Location.X & ", " & tbpTargetPose.Location.Y & "  at " & tbpTargetPose.Orientation)
            End If
            If Not boolStop Then ftnBotCurve = ftnExecuteMove(tbpEstimatedPose, "move curveto " & tbpTargetPose.Location.X & ", " & tbpTargetPose.Location.Y, tbpTargetPose)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnBotCurve :  " & ex.ToString)
        End Try
    End Function


    Function ftnBotTurnToOrientation(ByVal tbpEstimatedPose As typeBotPose, ByRef sngOrientation As Single) As typeBotPose
        Try
            ftnBotTurnToOrientation = ftnBotTurn(tbpEstimatedPose, False, ftnConvertToPose(tbpEstimatedPose.Location, sngOrientation))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnBotTurnToOrientation :  " & ex.ToString)
        End Try
    End Function

    Function ftnBotTurnToTarget(ByVal tbpEstimatedPose As typeBotPose, ByRef tmcTargetLocation As typeMapCoordinate) As typeBotPose
        Try
            ftnBotTurnToTarget = tbpEstimatedPose
            Dim tbpTargetPose As typeBotPose = tbpEstimatedPose  ' Turn towards Target
            tbpTargetPose.Orientation = ftnConvertTo360(tbpTargetPose.Orientation + ftnTurnAngle(tbpEstimatedPose, tmcTargetLocation))
            ftnBotTurnToTarget = ftnBotTurn(tbpEstimatedPose, False, tbpTargetPose)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnBotTurnToTarget :  " & ex.ToString)
        End Try
    End Function



    Function ftnBotTurn(ByRef tbpEstimatedPose As typeBotPose, ByRef boolRestrictConsecutiveTurns As Boolean,
                         ByVal tbpTargetPose As typeBotPose) As typeBotPose
        Try
            ftnBotTurn = tbpEstimatedPose
            If boolDebugDrive Then
                frmDebug.subWriteLogs(ftnElapsedTimeS() & "        : ftnBotTurn   tbpEstimatedPose =: " & tbpEstimatedPose.Location.X & ", " & tbpEstimatedPose.Location.Y & "  at " & tbpEstimatedPose.Orientation)
                frmDebug.subWriteLogs(ftnElapsedTimeS() & "        : ftnBotTurn   tbpTargetPose =: " & tbpTargetPose.Location.X & ", " & tbpTargetPose.Location.Y & "  at " & tbpTargetPose.Orientation)
            End If
            If Not boolStop Then
                Dim sngTurnAngle As Single = ftnTurnAngleV2(tbpEstimatedPose, tbpTargetPose)
                tomTypeOfMovePlanned = TypeOfMove.TURN
                If boolRestrictConsecutiveTurns And (intConsecutiveTurnCounter >= constMaxTurnsWithLocalization) Then ' intConsecutiveTurnCounter keeps track of turns and small movements and is used to limit unproductive localizations and path regeneration
                    frmDebug.subWriteLogs(ftnElapsedTimeS() & " *********** Too many consecutive Turns - Moving forward instead ***************************")
                    tbpTargetPose = tbpEstimatedPose
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, 2)
                    ftnBotTurn = ftnBotFwd(tbpEstimatedPose, tbpTargetPose)
                Else
                    If (Abs(sngTurnAngle) < 1) Then
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " *********** Path Failure Turn - Moving forward instead ***************************")
                        If boolDebugPath Then
                            frmDebug.subWriteLogs("  :    tbpEstimatedPose =: " & tbpEstimatedPose.Location.X & ", " & tbpEstimatedPose.Location.Y & "  at " & tbpEstimatedPose.Orientation)
                            frmDebug.subWriteLogs("  :    tbpTargetPose =: " & tbpTargetPose.Location.X & ", " & tbpTargetPose.Location.Y & "  at " & tbpTargetPose.Orientation)
                            frmDebug.subWriteLogs("  :    tmcDestination =: " & tmcDestination.X & ", " & tmcDestination.Y)
                        End If
                        Dim tbpFindTargetPose As typeBotPose = ftnFindTargetPose(tbpEstimatedPose, tmcDestination)
                        frmDebug.subWriteLogs("  :    tbpFindTargetPose =: " & tbpFindTargetPose.Location.X & ", " & tbpFindTargetPose.Location.Y & "  at " & tbpFindTargetPose.Orientation)
                        tbpTargetPose = tbpEstimatedPose
                        tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, 2)
                        ftnBotTurn = ftnBotFwd(tbpEstimatedPose, tbpTargetPose)
                    Else
                        If (Abs(sngTurnAngle) > 135) Then sngTurnAngle = ftnTurnWithoutHittingTail(tbpEstimatedPose, sngTurnAngle)
                        tbpTargetPose.Orientation = ftnConvertTo360(tbpEstimatedPose.Orientation + sngTurnAngle)
                        ftnBotTurn = ftnExecuteMove(tbpEstimatedPose, "move " & sngTurnAngle & " degrees", tbpTargetPose)
                        If boolRestrictConsecutiveTurns Then intConsecutiveTurnCounter += 1
                    End If
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnBotTurn :  " & ex.ToString)
        End Try
    End Function

    Function ftnTurnWithoutHittingTail(ByRef tbpEstimatedPose As typeBotPose, ByRef sngTurnAngle As Single) As Single
        ' This routine assumes that the robot is not perfectly round but rather that the robot's tail extends
        ' furthur from the center of movement than the head or sides.  To lessen the chances of collision between
        ' the tail and a nearby wall while turning, this routine will reverse the turn direction when near a wall 
        ' and when the direction of turn moves the tail towards the wall.
        Try
            ftnTurnWithoutHittingTail = sngTurnAngle
            If Not boolSlam Then
                Dim boolNearWallLeft As Boolean = False
                Dim boolNearWallRight As Boolean = False
                Dim boolNearWallRear As Boolean = False
                Dim tmcCheckLocation As typeMapCoordinate
                tmcCheckLocation = ftnLocationFromLocation(tmcCheckLocation, ftnConvertTo360(tbpEstimatedPose.Orientation - 180), sngBotRadius)
                boolNearWallRear = boolOccupiedCell(0, tmcCheckLocation.X, tmcCheckLocation.Y)
                If Not boolNearWallRear Then
                    Dim sngDistance As Single = sngBotRadius
                    While Not boolNearWallLeft And Not boolNearWallRight And (sngDistance < (sngBotRadius + 6)) ' Find if there is a wall near the sides of the bot
                        tmcCheckLocation = ftnLocationFromEstimatedPose(tbpEstimatedPose, 90, sngDistance)
                        boolNearWallLeft = boolOccupiedCell(0, tmcCheckLocation.X, tmcCheckLocation.Y)
                        tmcCheckLocation = ftnLocationFromEstimatedPose(tbpEstimatedPose, -90, sngDistance)
                        boolNearWallRight = boolOccupiedCell(0, tmcCheckLocation.X, tmcCheckLocation.Y)
                        sngDistance += 2
                    End While
                    If boolNearWallLeft And Not boolNearWallRight And (sngTurnAngle < 0) Then
                        ftnTurnWithoutHittingTail = 360 + sngTurnAngle
                    ElseIf boolNearWallRight And Not boolNearWallLeft And (sngTurnAngle > 0) Then
                        ftnTurnWithoutHittingTail = -(360 - sngTurnAngle)
                    Else
                        ftnTurnWithoutHittingTail = sngTurnAngle
                    End If
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnTurnWithoutHittingTail :  " & ex.ToString)
        End Try
    End Function

    Function ftnExecuteMove(ByVal tbpEstimatedPose As typeBotPose, ByRef strCommand As String, ByVal tbpTargetPose As typeBotPose) As typeBotPose ' Execute move and update pose using dead reckoning and odometry
        Try
            ftnExecuteMove = tbpEstimatedPose
            If Not boolShutDown Then
                ftnSendCommand("set power stopped " & intPowerStopped)
                tbpTargetPosePlanned = tbpTargetPose
                tbpStartingPosePlanned = tbpEstimatedPose
                If boolSimulatedMovement Then
                    ftnExecuteMove = ftnSimulatedExecuteMove(tbpEstimatedPose, strCommand, tbpTargetPose)
                ElseIf (tmiMoveInfo.boolBotIsMoving And (boolStop Or (String.CompareOrdinal(strCommand, "stop") = 0))) Then   ' Stop movement
                    ftnExecuteMove = ftnExecuteStopCommand(tbpEstimatedPose, tbpEstimatedPose)
                ElseIf Not boolStop And Not (boolSlam And boolLocalizationInProgress) Then
                    ftnExecuteMove = ftnExecuteMoveCommand(tbpEstimatedPose, tbpTargetPose, strCommand)
                    If Not boolStop Then ftnExecuteMove = ftnExecuteMoveComplete(ftnExecuteMove)
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnExecuteMove :  " & ex.ToString)
        End Try
    End Function

    Function ftnExecuteMoveCommand(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpTargetPose As typeBotPose, ByRef strCommand As String) As typeBotPose
        Try
            ftnExecuteMoveCommand = tbpEstimatedPose
            If Not tmiMoveInfo.boolBotIsMoving Then ftnBatteryDisconnected()
            If Not boolStop Then
                intDrivingSequenceNumber += 1
                frmDebug.subWriteStartMessage(strCommand, tbpEstimatedPose, tbpTargetPose)
                strStatusDriving = intDrivingSequenceNumber & " : Driving: " & strCommand

                Dim strResponse As String = ftnSendCommand(strCommand)
                ftnExecuteMoveCommand = ftnLocalizeUsingDeadReckoning(tbpBotPose, -999, True)
                ftnExecuteMoveCommand = ftnCheckForMoveComError(ftnExecuteMoveCommand, strResponse, tbpTargetPose)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnExecuteMoveCommand :  " & ex.ToString)
        End Try
    End Function

    Function ftnExecuteMoveComplete(ByRef tbpEstimatedPose As typeBotPose) As typeBotPose '  Dead Reckoning and Odometry localization
        Try
            If Not tmiMoveInfo.boolBotIsMoving Then ' Don't execute for course corrections
                Dim timeOut As DateTime = Now().AddMilliseconds(sngMaxTravel / ftnCmToInches(sngBotLinearVelocity))
                While tmiMoveInfo.boolBotIsMoving And (Now() < timeOut)  ' wait for move to complete
                    Thread.Sleep(25)
                End While
                If boolDebugDR Then
                    frmDebug.subWriteLogs(ftnElapsedTimeS() & ":      Now() =  " & Now())
                    frmDebug.subWriteShortPose(" tbpEstimatedPose ", tbpEstimatedPose)
                    frmDebug.subWriteLogs("      tmiMoveInfo.tomTypeOfMove = " & tmiMoveInfo.tomTypeOfMove)
                    frmDebug.subWriteLogs("      tmiMoveInfo.timeMoveCommandSent = " & tmiMoveInfo.timeMoveCommandSent.ToString)
                    frmDebug.subWriteShortPose("            tmiMoveInfo.tbpStartingPose ", tmiMoveInfo.tbpStartingPose)
                    frmDebug.subWriteShortPose("            tmiMoveInfo.tbpTargetPose ", tmiMoveInfo.tbpTargetPose)
                End If
                If tmiMoveInfo.boolMoveCommandInProgress Then
                    ftnExecuteMoveComplete = tbpStartingPosePlanned
                Else
                    ftnExecuteMoveComplete = ftnLocalizeUsingDeadReckoning(tbpEstimatedPose, 0, True)
                End If
                frmDebug.subWritePose(ftnElapsedTimeS() & "       Pose Derived From Dead Reckoning", ftnExecuteMoveComplete)
                Dim tbpOdometryPose As typeBotPose = ftnLocalizeUsingOdometry(ftnExecuteMoveComplete)
                ftnExecuteMoveComplete = ftnValidatePose(tbpOdometryPose, ftnExecuteMoveComplete, intBestParticle, True)
                frmDebug.subWritePose(ftnElapsedTimeS() & "       Pose Derived From Odometry", ftnExecuteMoveComplete)
                If boolDebugDrive Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnExecuteMove Move complete:  ")
            Else
                ftnExecuteMoveComplete = tbpBotPose ' 
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subExecuteMoveComplete :  " & ex.ToString)
        End Try
    End Function

    Function ftnExecuteStopCommand(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As typeBotPose
        Try
            If boolDebugDrive Then frmDebug.subWriteLogs(ftnElapsedTimeS() & "  ftnExecuteStopCommand Start:  ")
            ftnExecuteStopCommand = tbpEstimatedPose
            Dim boolBotIsMoving As Boolean = tmiMoveInfo.boolBotIsMoving
            ftnSetMoveInfo(tomTypeOfMovePlanned, tbpEstimatedPose, tbpEstimatedPose, True)
            Dim strResponse As String = ftnSendCommand("stop")
            frmDebug.subWriteLogs(ftnElapsedTimeS() & " ************************ STOP commanded  ********************************")
            If boolDebugDrive Then frmDebug.subWritePose(ftnElapsedTimeS() & "       tbpEstimatedPose", tbpEstimatedPose)
            If boolDebugDrive Then frmDebug.subWritePose(ftnElapsedTimeS() & "       tbpTargetPose", tbpTargetPose)
            If boolBotIsMoving Then
                ftnExecuteStopCommand = ftnCheckForMoveComError(tbpEstimatedPose, strResponse, tbpEstimatedPose)
                ftnExecuteStopCommand.LocationUncertainty.X += 4
                ftnExecuteStopCommand.LocationUncertainty.Y += 4
                ftnExecuteStopCommand.OrientationUncertainty += 8
                intMovementsSinceLocalization = 99  ' force localization 
            End If
            If boolDebugDrive Then frmDebug.subWritePose(ftnElapsedTimeS() & "       ftnExecuteStopCommand", ftnExecuteStopCommand)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnExecuteStopCommand :  " & ex.ToString)
        End Try
    End Function



    Public Function ftnPostMoveLocalizeAndPath(ByRef tbpEstimatedPose As typeBotPose, ByRef tomTypeOfMove As TypeOfMove,
                                    ByRef tbpTargetPose As typeBotPose) As typeBotPose
        'localizes, finds new path if required, collision recovery if required
        Try
            ftnPostMoveLocalizeAndPath = tbpEstimatedPose
            If Not boolShutDown And Not boolSimulatedMovement Then
                If boolDebugDrive Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnPostMoveLocalizeAndPath   START")
                Dim boolGoodLocalization As Boolean = False
                If ftnLocalizationNeeded(tbpEstimatedPose, tbpTargetPose) Then ' Fewer localizations result in fewer driving corrections, but also greater chance of collisions - its a tradeoff
                    ftnPostMoveLocalizeAndPath = ftnLocalize_Top(ftnPostMoveLocalizeAndPath, intBestParticle, boolGoodLocalization, (sngCcDelayS < 0.35))
                    If boolGoodLocalization Then tbpLastLocalizedPose = ftnPostMoveLocalizeAndPath
                ElseIf boolSlam Then
                    subGeneratePathArray()
                End If
                ftnPostMoveLocalizeAndPath = ftnLostRecoveryTop(ftnPostMoveLocalizeAndPath)
                If (intPathArray(ftnPostMoveLocalizeAndPath.Location.X, ftnPostMoveLocalizeAndPath.Location.Y) < constDestinationCost) Then ' only required if the Bot is too far off the path
                    subGeneratePathArray() ' Bot is too far off the original path, must regenerate path
                    ftnFindPathToDestination(tmcDestination, ftnPostMoveLocalizeAndPath.Location)
                End If
                If boolDebugDrive Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnPostMoveLocalizeAndPath   END" & vbCrLf & vbCrLf & vbCrLf)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnPostMoveLocalizeAndPath :  " & ex.ToString)
        End Try
    End Function

    Function ftnCheckForMoveComError(ByRef tbpEstimatedPose As typeBotPose, ByRef strResponse As String, ByRef tbpTargetPose As typeBotPose) As typeBotPose
        Try
            ftnCheckForMoveComError = tbpEstimatedPose
            If CBool(InStr(strResponse, "COM ERROR")) Then ' some type of error was detected so increase uncertainties  UNUSED
                If (tmiMoveInfo.tomTypeOfMove = TypeOfMove.FWD) Or (tmiMoveInfo.tomTypeOfMove = TypeOfMove.CURVE) Then
                    Dim sngDistanceToTarget As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tbpTargetPose.Location)
                    ftnCheckForMoveComError.LocationUncertainty.X += sngDistanceToTarget / 2
                    ftnCheckForMoveComError.LocationUncertainty.Y += sngDistanceToTarget / 2
                    ftnCheckForMoveComError.OrientationUncertainty += Abs(ftnAngleDifference(tbpEstimatedPose.Orientation, tbpTargetPose.Orientation) / 2)
                ElseIf (tmiMoveInfo.tomTypeOfMove = TypeOfMove.TURN) Then
                    ftnCheckForMoveComError.OrientationUncertainty += Abs(ftnAngleDifference(tbpEstimatedPose.Orientation, tbpTargetPose.Orientation) / 2)
                End If
                ftnSendCommand("play phrase 'Communication error.  '")
                intMovementsSinceLocalization = 99  ' force localization updates
                frmDebug.subWriteLogs("  ")
                frmDebug.subWriteLogs(ftnElapsedTimeS() & " **************  Communication Error during movement.  ' *****************************")
                frmDebug.subWriteLogs("  ")
            ElseIf CBool(InStr(strResponse, "com timeout")) Then ' some type of error was detected so increase uncertainties  used for HERO only
                ftnCheckForMoveComError = tbpTargetPose
                ftnCheckForMoveComError.LocationUncertainty.X = ftnCheckForMoveComError.LocationUncertainty.X + 10
                ftnCheckForMoveComError.LocationUncertainty.Y = ftnCheckForMoveComError.LocationUncertainty.Y + 10
                ftnCheckForMoveComError.OrientationUncertainty = Max(ftnCheckForMoveComError.OrientationUncertainty, 17)
                intMovementsSinceLocalization = 99  ' force localization updates
                frmDebug.subWriteLogs("  ")
                frmDebug.subWriteLogs(ftnElapsedTimeS() & " **************  Communication Timeout during movement.  ' *****************************")
                frmDebug.subWriteLogs("  ")
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCheckForMoveComError :  " & ex.ToString)
        End Try
    End Function

    Function ftnSimulatedExecuteMove(ByVal tbpEstimatedPose As typeBotPose, ByVal strCommand As String, ByVal tbpTargetPose As typeBotPose) As typeBotPose
        Try
            ftnSimulatedExecuteMove = tbpEstimatedPose
            If (tomTypeOfMovePlanned = TypeOfMove.HALT) Then
                ftnSetMoveInfo(tomTypeOfMovePlanned, ftnSimulatedExecuteMove, ftnSimulatedExecuteMove, False)
                tmiMoveInfo.tomTypeOfMove = TypeOfMove.NONE
            Else
                ftnSetMoveInfo(tomTypeOfMovePlanned, ftnSimulatedExecuteMove, tbpTargetPose, True)
                Dim sngStartingDistanceToTarget As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tbpTargetPose.Location)
                Dim sngRemainingDistanceToTarget As Single = sngStartingDistanceToTarget
                frmDebug.subWriteStartMessage(strCommand, tbpEstimatedPose, tbpTargetPose)
                tmiMoveInfo.boolMoveCommandInProgress = False
                While (((tmiMoveInfo.tomTypeOfMove = TypeOfMove.TURN) And ((Abs(ftnConvertTo180(ftnAngleDifference(tbpEstimatedPose.Orientation, tbpTargetPose.Orientation))) - 0.5) >= Abs(ftnAngleDifference(tmiMoveInfo.tbpStartingPose.Orientation, ftnSimulatedExecuteMove.Orientation)))) Or
                     ((tmiMoveInfo.tomTypeOfMove = TypeOfMove.FWD) And Not (sngRemainingDistanceToTarget <= 3) And (sngStartingDistanceToTarget >= ftnDistanceBetweenLocations(tmiMoveInfo.tbpStartingPose.Location, ftnSimulatedExecuteMove.Location)) And Not ftnLocationsEqual(ftnSimulatedExecuteMove.Location, tbpTargetPose.Location)) Or
                     ((tmiMoveInfo.tomTypeOfMove = TypeOfMove.CURVE) And Not (sngRemainingDistanceToTarget < 3) And (sngRemainingDistanceToTarget > 1))) And
                     Not boolStop And Not boolShutDown And Not ftnPosesEqual(ftnSimulatedExecuteMove, tbpTargetPose) ' 
                    ftnSimulatedExecuteMove = ftnLocalizeUsingDeadReckoning(ftnSimulatedExecuteMove, 0, False)
                    'If (tmiMoveInfo.tomTypeOfMove <> TypeOfMove.TURN) and (ftnElapsedTimeS > ftnDistanceBetweenLocations() / 
                    sngRemainingDistanceToTarget = ftnDistanceBetweenLocations(tbpTargetPose.Location, ftnSimulatedExecuteMove.Location)
                    tbpBotPose = ftnSimulatedExecuteMove
                End While
                subWait(sngRemainingDistanceToTarget * 100)
                If (tmiMoveInfo.tomTypeOfMove = TypeOfMove.CURVE) Then
                    ftnSimulatedExecuteMove = ftnLocalizeUsingDeadReckoning(ftnSimulatedExecuteMove, 0, False)
                ElseIf (tmiMoveInfo.tomTypeOfMove = TypeOfMove.TURN) Then
                    ftnSimulatedExecuteMove.Location = tbpEstimatedPose.Location
                Else
                    ftnSimulatedExecuteMove.Location = tbpTargetPose.Location
                End If
            End If
            intDrivingSequenceNumber += 1
            ftnSetMoveInfo(tmiMoveInfo.tomTypeOfMove, ftnSimulatedExecuteMove, ftnSimulatedExecuteMove, False)
            tmiMoveInfo.boolBotIsMoving = False
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnSimulatedExecuteMove :  " & ex.ToString)
        End Try
    End Function



    Public Function ftnLocalizationNeeded(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As Boolean
        Try
            ftnLocalizationNeeded = False
            If boolSimulatedMovement Or boolShutDown Then
                ftnLocalizationNeeded = False
            ElseIf (boolKinectConnected Or boolURGConnected) And _
                ((tbpEstimatedPose.LocationUncertainty.X > 11) Or (tbpEstimatedPose.LocationUncertainty.Y > 11) Or (tbpEstimatedPose.OrientationUncertainty > 11)) Then
                ftnLocalizationNeeded = True
            ElseIf (ftnPoseUncertaintyFactor(tbpEstimatedPose) > 480) Then '(15+15)*16
                ftnLocalizationNeeded = True
            ElseIf (Abs(ftnAngleDifference(tbpLastLocalizedPose.Orientation, tbpEstimatedPose.Orientation)) >= 25) Then
                ftnLocalizationNeeded = True
            ElseIf (ftnDistanceBetweenLocations(tbpLastLocalizedPose.Location, tbpEstimatedPose.Location) > 6) Then
                ftnLocalizationNeeded = True
            ElseIf (intMovementsSinceLocalization > 2) Then
                ftnLocalizationNeeded = True
            ElseIf (intConsecutiveTurnCounter <= constMaxTurnsWithLocalization) Then ' This prevents Bot from turning back and forth repeatedly
                ftnLocalizationNeeded = (intMovementsSinceLocalization > 2)
            ElseIf boolSlam Or boolEnableRBPF Then
                ftnLocalizationNeeded = True
            End If
            If ftnLocalizationNeeded Then
                intMovementsSinceLocalization = 0
            Else
                intMovementsSinceLocalization += 1
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocalizationNeeded :  " & ex.ToString)
        End Try
    End Function


    Function ftnBatteryDisconnected() As Boolean
        Try
            ftnBatteryDisconnected = False
            If Not boolSimulatedMovement Then
                Do
                    ftnSendCommand("battery") '   
                    If (strStatusBotVoltage = "   ??   ") Or CBool(InStr(strStatusBotVoltage, "--")) Or (sngBotBatteryVoltage < (sngDockTriggerBot * 0.7)) Then
                        ftnBatteryDisconnected = True
                        strStatusDriving = intDrivingSequenceNumber & " : BATTERY DISCONNECTED! "
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & "    |||||||||||  BATTERY DISCONNECT DETECTED  |||||||||||")
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & "                   Battery Voltage = " & sngBotBatteryVoltage)
                        ftnSendCommand("play phrase 'BATTERY DISCONNECTED'")
                        subWait(1500)
                    End If
                Loop Until (sngBotBatteryVoltage > (sngDockTriggerBot * 0.7)) Or boolStop Or boolShutDown
                ftnBatteryDisconnected = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnBatteryDisconnected :  " & ex.ToString)
        End Try
    End Function

    Function ftnImStuck(ByRef tbpEstimatedPose As typeBotPose, ByRef tomTypeOfMove As TypeOfMove, ByRef tbpTargetPose As typeBotPose) As Boolean
        On Error Resume Next
        ftnImStuck = False
        If Not boolEnableRBPF And Not boolSlam And (tomTypeOfMove = TypeOfMove.TURN) Then
            If (Abs(ftnAngleDifference(tbpTargetPose.Orientation, tbpEstimatedPose.Orientation)) > 20) And Not boolStop Then
                ftnImStuck = True
                ftnSendCommand("play phraseA 'I am stuck'")
                frmDebug.subWriteLogs(Now() & "    |||||||||||  I'm stuck  |||||||||||")
            End If
        End If
    End Function

    Public Function ftnGetUnStuck(ByRef tbpEstimatedPose As typeBotPose, ByRef tomTypeOfMove As TypeOfMove) As typeBotPose
        Try
            ftnGetUnStuck = tbpEstimatedPose
            If boolDebugDrive Then frmDebug.subWriteLogs(" tbpEstimatedPose:  START " & tomTypeOfMove.ToString)
            If Not boolStop And Not boolShutDown And boolDrivingToDestination Then
                Dim tbpTargetPose As typeBotPose = tbpEstimatedPose
                If (tomTypeOfMove = TypeOfMove.TURN) Then
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, 6)
                    tbpBotPose = ftnBotFwd(tbpEstimatedPose, tbpTargetPose)
                    ftnGetUnStuck = ftnLocalize_Top(tbpBotPose, intBestParticle, False, (sngCcDelayS < 0.35))
                    tbpBotPose = ftnGetUnStuck
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 0, -2)
                    ftnGetUnStuck = ftnBotFwd(tbpBotPose, tbpTargetPose)
                ElseIf (tomTypeOfMove = TypeOfMove.FWD) Or (tomTypeOfMove = TypeOfMove.CURVE) Then
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, -6)
                    tbpBotPose = ftnBotFwd(tbpEstimatedPose, tbpTargetPose)
                    ftnGetUnStuck = ftnLocalize_Top(tbpBotPose, intBestParticle, False, (sngCcDelayS < 0.35))
                    tbpBotPose = ftnGetUnStuck
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 0, 2)
                    ftnGetUnStuck = ftnBotFwd(tbpBotPose, tbpTargetPose)
                End If
                ftnGetUnStuck = ftnLocalize_Top(tbpBotPose, intBestParticle, False, (sngCcDelayS < 0.35))
            End If
            tbpBotPose = ftnGetUnStuck
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetUnStuck :  " & ex.ToString)
        End Try
    End Function

End Module
