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

Module modAPICommands

    Public Function ftnAPICommands(ByVal strAPIMessage As String) As String
        ftnAPICommands = ""
        Try
            If Not frmGotABotTop.InvokeRequired Then
                Dim strReceivedMessage As String = strAPIMessage
                Dim strParsedMessage As String()
                strAPIMessage &= vbCr & vbCr & vbCr & vbCr
                strParsedMessage = strAPIMessage.Split(vbCr)
                strAPIMessage = strParsedMessage(0)
                If CBool(InStr(strAPIMessage, "POST")) Then ' message is from IFTTT webhooks
                    strAPIMessage = LCase(strAPIMessage)
                    strAPIMessage = Replace(strAPIMessage, "%20", "&=")
                    strAPIMessage = Replace(strAPIMessage, "=&=", "&=")
                    Dim i As Integer = 0
                    While CBool(InStr(strAPIMessage, "="))
                        strAPIMessage = Mid(strAPIMessage, InStr(strAPIMessage, "=") + 1)
                        strParsedMessage(i) = Left(strAPIMessage, Max(InStr(strAPIMessage, "&") - 1, 0))
                        i += 1
                    End While
                    While (i < strParsedMessage.GetUpperBound(0))
                        strParsedMessage(i) = "*"
                        i += 1
                    End While
                Else
                    strStatusAPIMessage = "API: " & strAPIMessage
                    strAPIMessage = String.Concat(strAPIMessage, " * ", "* ", "* ", "* ", "* ", "* ", "* ", "* ")
                    strParsedMessage = strAPIMessage.Split(vbCr)
                    strParsedMessage = strParsedMessage(0).Split(" "c)
                End If
                If (strParsedMessage(0) = strApiPassword) Then
                    If (strParsedMessage(1) = "API_Timeout") Then
                        strAPIMessage = "API Timeout" & vbCrLf
                    ElseIf (strParsedMessage(1) = "") Then
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "turn") And (strParsedMessage(2) = "right") Then
                        boolStop = False
                        tbpBotPose = ftnBotTurn(tbpBotPose, False, ftnConvertToPose(tbpBotPose.Location, ftnConvertTo360(tbpBotPose.Orientation - Convert.ToSingle(Val(strParsedMessage(3))))))
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "turn") And (strParsedMessage(2) = "left") Then
                        boolStop = False
                        tbpBotPose = ftnBotTurn(tbpBotPose, False, ftnConvertToPose(tbpBotPose.Location, ftnConvertTo360(tbpBotPose.Orientation + Convert.ToSingle(Val(strParsedMessage(3))))))
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "forward") Then
                        boolStop = False
                        tbpBotPose = ftnBotFwdDistance(tbpBotPose, Convert.ToSingle(Val(strParsedMessage(2))))
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "back") Then
                        boolStop = False
                        tbpBotPose = ftnBotFwdDistance(tbpBotPose, -Convert.ToSingle(Val(strParsedMessage(2))))
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "home") Then
                        boolStop = False
                        frmArm.boolHomeArmAPI = True
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "open") Then
                        boolStop = False
                        frmArm.boolOpenGripperAPI = True
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "close") Then
                        boolStop = False
                        frmArm.boolCloseGripperAPI = True
                        strAPIMessage = "OK"
                    ElseIf ((strParsedMessage(1) = "pick") And (strParsedMessage(2) = "up") And (Val(strParsedMessage(3)) > 0)) Then
                        boolStop = False
                        frmArm.intXAPI = Val(strParsedMessage(3))
                        frmArm.boolPickUpAPI = True
                        strAPIMessage = "OK"
                    ElseIf ((strParsedMessage(1) = "get") And (strParsedMessage(2) = "object") And (Val(strParsedMessage(3)) > 0)) Then
                        boolStop = False
                        frmArm.intXAPI = Val(strParsedMessage(3))
                        frmArm.boolPickUpAPI = True
                        strAPIMessage = "OK"
                    ElseIf ((strParsedMessage(1) = "up") And (Val(strParsedMessage(2)) > 0)) Then
                        boolStop = False
                        Dim tjcNewJointCoordinates As frmArm.typeJointCoordinate = frmArm.ftnForwardKinematicsCoordinates(frmArm.taaArmAngles)
                        tjcNewJointCoordinates.tpcGripperEnd.Z += Val(strParsedMessage(2))
                        frmArm.taaNewArmAngles = frmArm.ftnAnglesToPlaceGripperAtPoint(tjcNewJointCoordinates.tpcGripperEnd, True)
                        frmArm.boolMoveToNewAnglesAPI = True
                        strAPIMessage = "OK"
                    ElseIf ((strParsedMessage(1) = "down") And (Val(strParsedMessage(2)) > 0)) Then
                        boolStop = False
                        Dim tjcNewJointCoordinates As frmArm.typeJointCoordinate = frmArm.ftnForwardKinematicsCoordinates(frmArm.taaArmAngles)
                        tjcNewJointCoordinates.tpcGripperEnd.Z -= Val(strParsedMessage(2))
                        frmArm.taaNewArmAngles = frmArm.ftnAnglesToPlaceGripperAtPoint(tjcNewJointCoordinates.tpcGripperEnd, True)
                        frmArm.boolMoveToNewAnglesAPI = True
                        strAPIMessage = "OK"
                    ElseIf ((strParsedMessage(1) = "in") And (Val(strParsedMessage(2)) > 0)) Then
                        boolStop = False
                        Dim tjcNewJointCoordinates As frmArm.typeJointCoordinate = frmArm.ftnForwardKinematicsCoordinates(frmArm.taaArmAngles)
                        tjcNewJointCoordinates.tpcGripperEnd.Y -= Val(strParsedMessage(2))
                        frmArm.taaNewArmAngles = frmArm.ftnAnglesToPlaceGripperAtPoint(tjcNewJointCoordinates.tpcGripperEnd, True)
                        frmArm.boolMoveToNewAnglesAPI = True
                        strAPIMessage = "OK"
                    ElseIf ((strParsedMessage(1) = "out") And (Val(strParsedMessage(2)) > 0)) Then
                        boolStop = False
                        Dim tjcNewJointCoordinates As frmArm.typeJointCoordinate = frmArm.ftnForwardKinematicsCoordinates(frmArm.taaArmAngles)
                        tjcNewJointCoordinates.tpcGripperEnd.Y += Val(strParsedMessage(2))
                        frmArm.taaNewArmAngles = frmArm.ftnAnglesToPlaceGripperAtPoint(tjcNewJointCoordinates.tpcGripperEnd, True)
                        frmArm.boolMoveToNewAnglesAPI = True
                        strAPIMessage = "OK"
                    ElseIf ((strParsedMessage(1) = "set") And (strParsedMessage(2) = "diameter") And (Val(strParsedMessage(3)) > 0)) Then
                        sngBotDiameter = Convert.ToSingle(Val(strParsedMessage(3)))
                        subRecalculateVariables()
                        subConvertMapToArray(intBestParticle)
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "set") And (strParsedMessage(2) = "ltime") And (Val(strParsedMessage(3)) > 0) Then
                        sngLocalizationTime = Convert.ToSingle(Val(strParsedMessage(3)))
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "set") And (strParsedMessage(2) = "offset") And (Val(strParsedMessage(3)) > 0) Then
                        sngCameraOffset = Convert.ToSingle(Val(strParsedMessage(3)))
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "set") And (strParsedMessage(2) = "simulated") And ((strParsedMessage(3) = "true") Or (strParsedMessage(3) = "false")) Then
                        boolSimulatedMovement = strParsedMessage(3)
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "set") And (strParsedMessage(2) = "velocity") Then
                        sngBotLinearVelocity = Convert.ToSingle(Val(strParsedMessage(3)))
                        subRecalculateVariables()
                        ftnSendCommand("set linear velocity " & sngBotLinearVelocity)
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "set") And (strParsedMessage(2) = "angular") And (strParsedMessage(3) = "velocity") Then
                        sngBotAngularVelocity = Convert.ToSingle(Val(strParsedMessage(4)))
                        subRecalculateVariables()
                        ftnSendCommand("set angular velocity " & sngBotAngularVelocity)
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "set") And (strParsedMessage(2) = "pose") Then
                        If (Val(strParsedMessage(3)) >= 0) Then tbpBotPose.Location.X = Int(Val(strParsedMessage(3)))
                        If (Val(strParsedMessage(4)) >= 0) Then tbpBotPose.Location.Y = Int(Val(strParsedMessage(4)))
                        If (Val(strParsedMessage(5)) >= 0) Then tbpBotPose.LocationUncertainty.X = Int(Val(strParsedMessage(5)))
                        If (Val(strParsedMessage(6)) >= 0) Then tbpBotPose.LocationUncertainty.Y = Int(Val(strParsedMessage(6)))
                        If (Val(strParsedMessage(7)) >= 0) Then tbpBotPose.Orientation = Int(Val(strParsedMessage(7)))
                        If (Val(strParsedMessage(8)) >= 0) Then tbpBotPose.OrientationUncertainty = Int(Val(strParsedMessage(8)))
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "set") And (strParsedMessage(2) = "orientation") And (Val(strParsedMessage(3)) >= 0) And (Val(strParsedMessage(4)) >= 0) Then
                        Dim intApiUncertainty As Integer = Int(Val(strParsedMessage(4)))
                        If boolSlam Then
                            tbpBotPose.Orientation = ftnConvertTo360(Convert.ToInt16(Val(strParsedMessage(3))))
                            tbpBotPose.OrientationUncertainty = intApiUncertainty
                        End If
                        If Not boolDrivingToDestination And (Not boolDock Or boolDocked Or boolPause) Then
                            If (intApiUncertainty <= tbpBotPose.OrientationUncertainty) Then
                                tbpBotPose.OrientationUncertainty = intApiUncertainty
                                tbpBotPose.Orientation = ftnConvertTo360(Convert.ToInt16(Val(strParsedMessage(3))))
                            End If
                        End If
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(2) = "destination") Then
                        If (Val(strParsedMessage(3)) >= 0) Then tmcDestination.X = Int(Val(strParsedMessage(3)))
                        If (Val(strParsedMessage(4)) >= 0) Then tmcDestination.Y = Int(Val(strParsedMessage(4)))
                        tmcDestination = ftnCheckMapArrayLimits(tmcDestination)
                        boolStop = False
                        Dim threadDriveToDestination As New Thread(AddressOf ftnDriveToUserChosenDestination)
                        threadDriveToDestination.Start()
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "dock") Or (strParsedMessage(1) = "recharge") Then
                        boolDock = True
                        boolStop = False
                        Dim threadDock As New Thread(AddressOf frmDock.subDock)
                        threadDock.Start()
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "localize") Or (strParsedMessage(1) = "localized") Then
                        ftnSendCommand("events")
                        Dim boolGoodLocalization As Boolean = False
                        tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, boolGoodLocalization, (sngCcDelayS < 0.35))
                        strAPIMessage = "Pose = " & tbpBotPose.Location.X & " " & tbpBotPose.Location.Y & " " &
                                     tbpBotPose.LocationUncertainty.X & " " & tbpBotPose.LocationUncertainty.Y & " " &
                                     tbpBotPose.Orientation & " " & tbpBotPose.OrientationUncertainty & vbCrLf
                        strAPIMessage = strAPIMessage & "OK"
                    ElseIf (strParsedMessage(1) = "stop") Or (strParsedMessage(1) = "halt") Then
                        frmDebug.subWriteLogs("API Command Received: stop")
                        tbpBotPose = ftnBotStop(tbpBotPose, tmiMoveInfo.tbpTargetPose)
                        boolStop = True
                        boolDock = False
                        boolDrivingToDestination = False
                        boolLocalizationInProgress = False
                        strStatusDriving = "STOP mode"
                        ftnSendCommand("play phrase 'By your command.'")
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "pause") Then
                        frmDebug.subWriteLogs("API Command Received: pause")
                        boolPause = Not boolPause
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "bumper") Then
                        Dim byteCollisionCode As Byte = 7
                        Select Case strParsedMessage(2)
                            Case "left" : byteCollisionCode = 5
                            Case "right" : byteCollisionCode = 6
                            Case "front" : byteCollisionCode = 4
                            Case "rear" : byteCollisionCode = 3
                        End Select
                        ftnCollisionRecovery(tbpBotPose, byteCollisionCode)
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "load" And (strParsedMessage(2) = "settings") And (Dir(strParsedMessage(3)) <> "")) Then
                        LoadSettings(strParsedMessage(3))
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "load" And (strParsedMessage(2) = "map") And (Dir(strParsedMessage(3)) <> "")) Then
                        frmGotABotTop.subOpenMap(strParsedMessage(3))
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "save" And (strParsedMessage(2) = "settings")) Then
                        frmGotABotTop.mnuSaveSettings_Click(tlmLandMarkArray, Nothing)
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "get" And (strParsedMessage(2) = "pose")) Then
                        strAPIMessage = "POSE = " & tbpBotPose.Location.X & " " & tbpBotPose.Location.Y & " " &
                                         tbpBotPose.LocationUncertainty.X & " " & tbpBotPose.LocationUncertainty.Y & " " &
                                         tbpBotPose.Orientation & " " & tbpBotPose.OrientationUncertainty & vbCrLf
                        strAPIMessage = strAPIMessage & "OK"
                    ElseIf ((strParsedMessage(1) = "get") And (strParsedMessage(2) = "status") Or (strParsedMessage(1) = "status")) Then
                        Dim threadStatus As New Thread(AddressOf subStatus)
                        threadStatus.Start()
                        If boolDocked Then
                            strAPIMessage = "docked" & vbCrLf & "OK"
                        ElseIf Not tmiMoveInfo.boolBotIsMoving Then
                            strAPIMessage = "stopped" & vbCrLf & "OK"
                        Else
                            strAPIMessage = "moving" & vbCrLf & "OK"
                        End If
                    ElseIf (strParsedMessage(1) = "get" And (strParsedMessage(2) = "path")) Then
                        strAPIMessage = "PATH = " & vbCrLf
                        Dim tmcOldDestination As typeMapCoordinate
                        Dim tbpOldBotPose As typeBotPose = tbpBotPose
                        tmcOldDestination = tmcDestination
                        tmcDestination.X = Convert.ToSingle(Val(strParsedMessage(3)))
                        tmcDestination.Y = Convert.ToSingle(Val(strParsedMessage(4)))
                        tmcDestination = ftnCheckMapArrayLimits(tmcDestination)
                        subGeneratePathArray()
                        ftnFindPathToDestination(tmcDestination, tbpBotPose.Location)
                        While Not ftnLocationsEqual(tmcDestination, tbpBotPose.Location) And Not boolStop And Not boolShutDown
                            tbpBotPose = ftnFindTargetPose(tbpBotPose, tmcDestination)
                            strAPIMessage = strAPIMessage & " " & tbpBotPose.Location.X & "  " & tbpBotPose.Location.Y & vbCrLf
                        End While
                        tbpBotPose = tbpOldBotPose
                        tmcDestination = tmcOldDestination
                        strAPIMessage = strAPIMessage & "OK"
                    ElseIf (strParsedMessage(1) = "transmit") Then
                        If CBool(InStr(strAPIMessage, "transmit")) Then _
                           strAPIMessage = strAPIMessage.Replace("transmit ", "")
                        If CBool(InStr(strAPIMessage, "*")) Then _
                          strAPIMessage = strAPIMessage.Replace("*", "")
                        ftnSendCommand(strAPIMessage)
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "Receive") Then
                        strAPIMessage = strAPIMessage & "OK"
                    ElseIf (strParsedMessage(1) = "look") And (strParsedMessage(2) = "up") Then
                        If frmArm.boolEnableArm Then
                            Dim sngShoulder As Single = frmArm.taaArmAngles.sngShoulder
                            Dim sngElbow As Single = frmArm.taaArmAngles.sngElbow
                            frmArm.taaArmAngles.sngShoulder = frmArm.ftnJointAngleVerify(frmArm.taaArmAngles.sngShoulder + 10)
                            frmArm.taaArmAngles.sngElbow = frmArm.taaArmAngles.sngElbow + 20
                            sngShoulder = frmArm.taaArmAngles.sngShoulder
                            frmArm.subMoveArmToNewAngles(frmArm.taaArmAngles, False)
                        ElseIf boolKinectDriversInstalled Then  ' KLUDGE
                            If K_Sensor IsNot Nothing Then _
                               K_Sensor.ElevationAngle() = K_Sensor.ElevationAngle() + 15
                        End If
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "look") And (strParsedMessage(2) = "down") Then
                        If frmArm.boolEnableArm Then
                            frmArm.boolLookDownAPI = True
                        ElseIf boolKinectDriversInstalled Then  ' KLUDGE
                            If K_Sensor IsNot Nothing Then _
                               K_Sensor.ElevationAngle() = K_Sensor.ElevationAngle() - 15
                        End If
                        strAPIMessage = "OK"
                    ElseIf (strParsedMessage(1) = "who") And (strParsedMessage(2) = "am") And (strParsedMessage(3) = "i") Or _
                           (strParsedMessage(1) = "identify") Then
                        modFaceRecognition.subWhoAmI()
                        strAPIMessage = "OK"
                        ElseIf (strParsedMessage(1) = "remember") And (strParsedMessage(2) = "my") And (strParsedMessage(3) = "face") Or _
                               (strParsedMessage(1) = "remember") And (strParsedMessage(2) = "me") Then
                            If (strPersonName = "") Then
                                subSpeak("tell me your name first")
                                strAPIMessage = "OK"
                            Else
                                boolEnableFaceRecognition = True
                                boolEnableVideo = True
                                frmVideo.subVideoStart()
                                If Not boolTrained Then boolTrained = modFaceRecognition.ftnFaceRecognitionLoad()
                                subSpeak(" play phraseA 'OK'")
                                subTrainWebcam()
                                strAPIMessage = "OK"
                            End If
                        ElseIf (strParsedMessage(1) = "my") And (strParsedMessage(2) = "name") And (strParsedMessage(3) = "is") Then
                            strPersonName = strParsedMessage(4)
                            strAPIMessage = "OK"
                        ElseIf (strParsedMessage(1) = "look") And (strParsedMessage(2) = "at") And (strParsedMessage(3) = "me") Then
                            subLookAtMe()
                            strAPIMessage = "OK"
                        ElseIf ((strParsedMessage(1) = "exit") Or (strParsedMessage(1) = "quit")) And (strParsedMessage(2) = "application") Then
                            ftnSendCommand("set power stopped " & 0)
                            ftnSendCommand("play phrase 'Goodby'")
                            frmGotABotTop.Visible = True
                            boolShutDown = True
                            frmGotABotTop.subGotABotExit()
                            strAPIMessage = "OK"
                        ElseIf (strParsedMessage(1) = "batchfile") Or (strParsedMessage(1) = "file") Then
                            If (Dir(strParsedMessage(2) & ".bat") <> "") Then
                                ftnSendCommand("play phrase 'starting batch file' ")
                                Process.Start(strParsedMessage(2) & ".bat")
                            Else
                                ftnSendCommand("play phrase 'Batch file not found'")
                            End If
                            strAPIMessage = "OK"
                        ElseIf (strParsedMessage(1) = "help") Or (strParsedMessage(1) = "?") Then
                            strAPIMessage = "VALID COMMANDS :" & vbCrLf
                            strAPIMessage = "   (NOTE : Every command must be preceded by the password)" & vbCrLf
                            strAPIMessage = strAPIMessage & "  " & vbCrLf
                            strAPIMessage = strAPIMessage & "    turn right xx" & vbCrLf
                            strAPIMessage = strAPIMessage & "    turn left xx" & vbCrLf
                            strAPIMessage = strAPIMessage & "    forward xx" & vbCrLf
                            strAPIMessage = strAPIMessage & "    back xx" & vbCrLf
                            strAPIMessage = strAPIMessage & "    open" & vbCrLf
                            strAPIMessage = strAPIMessage & "    close" & vbCrLf
                            strAPIMessage = strAPIMessage & "    home" & vbCrLf
                            strAPIMessage = strAPIMessage & "    pick up xx" & vbCrLf
                            strAPIMessage = strAPIMessage & "    set orientation xx xx" & vbCrLf
                            strAPIMessage = strAPIMessage & "    set destination xxxx xxx" & vbCrLf
                            strAPIMessage = strAPIMessage & "    localize" & vbCrLf
                            strAPIMessage = strAPIMessage & "    stop" & vbCrLf
                            strAPIMessage = strAPIMessage & "    dock" & vbCrLf
                            strAPIMessage = strAPIMessage & "    get pose" & vbCrLf
                            strAPIMessage = strAPIMessage & "    get path xxx xxx" & vbCrLf
                            strAPIMessage = strAPIMessage & "    get status" & vbCrLf
                            strAPIMessage = strAPIMessage & "    look up" & vbCrLf
                            strAPIMessage = strAPIMessage & "    look down" & vbCrLf
                            strAPIMessage = strAPIMessage & "    look at me" & vbCrLf
                            strAPIMessage = strAPIMessage & "    remember me" & vbCrLf
                            strAPIMessage = strAPIMessage & "    who am i" & vbCrLf
                            strAPIMessage = strAPIMessage & "    batchfile ssssss" & vbCrLf
                            strAPIMessage = strAPIMessage & "    exit application" & vbCrLf
                            strAPIMessage = strAPIMessage & "OK"
                        Else
                            strAPIMessage = "ERROR  UNRECOGNIZED COMMAND "
                            subSpeak("play phrase error")
                            frmDebug.subWriteLogs(" ftnAPICommands :  ERROR  UNRECOGNIZED COMMAND " & strReceivedMessage)
                        End If   '
                        strAPIMessage = strAPIMessage & " >"
                        ftnAPICommands = strAPIMessage
                        If boolDebugAPI Then frmDebug.subWriteLogs(" \\\\\  API Response: " & strAPIMessage)
                Else
                    strAPIMessage = "ERROR  Wrong Password " & strReceivedMessage
                    ftnSendCommand("play phrase 'ERROR  Wrong Password'")
                    frmDebug.subWriteLogs(" ftnAPICommands :  ERROR Wrong Password " & strReceivedMessage)
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnAPICommands :  " & ex.ToString)
        End Try
    End Function

    Public Sub subStatus()
        Try
            ftnSendCommand("battery") ' 
            If boolBotConnected And Not boolMute Then
                Dim sngLaptopBatteryPercent As Single = Int(SystemInformation.PowerStatus.BatteryLifePercent * 100)
                If (sngBotBatteryVoltage < 11.4) Or (sngLaptopBatteryPercent < 40) Then
                    ftnSendCommand("play phrase 'Battery level is low'")
                ElseIf (sngBotBatteryVoltage < 11.6) Or (sngLaptopBatteryPercent < 75) Then
                    ftnSendCommand("play phrase 'Battery level is OK'")
                ElseIf (sngBotBatteryVoltage < 11.9) Or (sngLaptopBatteryPercent < 90) Then
                    ftnSendCommand("play phrase 'Battery level is good'")
                Else
                    ftnSendCommand("play phrase 'Battery is full'")
                End If
                subWait(300)
                If ftnLocationsEqual(tbpBotPose.Location, tmcDock) Or boolDocked Then
                    ftnSendCommand("play phrase 'I am at my docking station'")
                ElseIf boolDock Then
                    ftnSendCommand("play phrase 'I am docking'")
                ElseIf frmArm.boolGetObjectBusy Then
                    ftnSendCommand("play phrase 'I am picking up an object'")
                ElseIf boolDrivingToDestination Then
                    ftnSendCommand("play phrase 'I am driving to destination'")
                End If
                If ftnIAmLost(tbpBotPose) Then _
                    ftnSendCommand("play phrase 'I am lost'")
            Else
                ftnSendCommand("play phrase 'I am not connected to my hardware'")
            End If
            If boolPause Then ftnSendCommand("play phrase 'I am in pause mode'")

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


End Module

