

'=============================================================================================================================================================

' NOTE:  This project requires that drivers for Phidgets (http://www.phidgets.com/docs/OS_-_Windows#Quick_Downloads) 
' and Kinect (http://www.microsoft.com/en-us/kinectforwindows/develop/) be installed on the PC

' Rcc Port  = 9000
' API_Port = 9100
' 2nd API_Port = 9400

'=============================================================================================================================================================


Imports System.Net
Imports System.Drawing
Imports System.Threading
Imports System.Globalization
Imports System.Text
Imports System.Math
Imports System.Windows.Forms

Public Class frmGotABotTop

    Dim boolMouseDown As Boolean = False
    Dim boolSettingsChangedTemp As Boolean = False
    Dim boolPlaceBot As Boolean = False
    Dim boolPlaceDestination As Boolean = False

  

    Private Sub subGotABotLoad(ByVal sender As Object, ByVal e As System.EventArgs) Handles Me.Load
        Try
            Thread.CurrentThread.Name = "threadGotABotTop"
            Dim infoReader As System.IO.FileInfo = My.Computer.FileSystem.GetFileInfo("GotABot_Log.txt")
            If (Dir("GotABot_Log.txt") <> "") Then
                If infoReader.Length > 10000000 Then frmDebug.subWriteLogs(" GotABot_Log.txt is growing excessively large.  Please start a new log file")
            End If

            LoadSettings("GotABot_Settings.xml") ' load settings
            boolEnableFaceRecognition = False
            subOpenMap("GotABot_Map.bmp")
            Me.Text = "GOTABOT       (Version " & My.Application.Info.Version.ToString(3) & ")"
            Me.StatusDriving.ForeColor = Color.Black
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subGotABotLoad :  " & ex.ToString)
        End Try
    End Sub

    Sub subInitialize(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Me.Shown
        Try
            boolStop = True
            ftnSetMoveInfo(TypeOfMove.HALT, tbpBotPose, tbpBotPose, False)
            frmDebug.subWriteLogs(vbCrLf & Now() & " ************************ GOTABOT       (Version " & My.Application.Info.Version.ToString(3) & ")  ********************************" & vbCrLf)
            If (ticCameraResolution.X = 0) Then
                ticCameraResolution.X = 640
                ticCameraResolution.Y = 480
            End If
            AboutBox1.LabelVer.Text = "Version " & My.Application.Info.Version.ToString
            subConfigureMenu(sender, e)
            tmiMoveInfo.timeMoveCommandSent = Now()
            If boolHeroSelected Then
                subHeroConfigure()
            ElseIf mnuER1Direct.Checked Then
                ftnER1_Config()
            End If
            ftnKinectInitializeTop()
            If Not boolKinectConnected Then ftnURGInitializeTop()
            subStartThreads()
            subRecalculateVariables()
            subConvertMapToArray(intBestParticle)
            subInitParticles(tbpBotPose, intNumberOfParticles)
            boolSettingsChanged = False
            subEndImageRecognition()
            timerWatchDog.Interval = 60000
            boolDrivingToDestination = False
            subInitializeDrivingControl(tbpBotPose)
            ftnRcc_Communicate("events" & vbCrLf, "OK", 1000)
            strStatusDriving = "Checking Bot Coms "
            ftnSendCommand("stop")
            ftnSendCommand("stop")
            ftnSendCommand("stop")
            tmiMoveInfo.boolMoveCommandInProgress = False
            ftnSendCommand("battery")
            If boolEnableVideo Then frmVideo.subVideoStart()
            If frmArm.boolEnableArm Then
                frmArm.ftnArmLoad()
                frmArm.subHome(True)
            End If
            'subGuiUpdate(sender, e)
            boolStop = True
            timerWatchDog.Enabled = True
            strStatusDriving = "Waiting for command "
            boolSlam = False
            boolEnableRBPF = False
            subResizeMap()
            If boolHeroSelected Then
                intNumberOfBeams = 40   '  
            Else
                intNumberOfBeams = 64
            End If
            ftnSendCommand("play phraseA 'Ready'")
            subDrawBot(tbpBotPose, tbpBotPose.Location)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subInitialize :  " & ex.ToString)
        End Try
    End Sub

    Dim tbpPreviousPose As typeBotPose
    Sub subGuiUpdate(sender As Object, e As EventArgs) Handles timerGUI.Tick
        Try
            If timerGUI.Enabled And (bmpMap IsNot Nothing) And Not boolShutDown Then
                If boolShutDown Then Me.Close()
                timerGUI.Enabled = False
                If Not boolMapLocked And (boolSlam Or Not boolDrivingToDestination) Then  'YUK
                    'If (boolSlam Or Not boolDrivingToDestination) Then  'YUK
                    boolMapLocked = True
                    myMap.Image = bmpMap.Clone
                    boolMapLocked = False
                    Refresh()
                End If
                subButtonGui()
                'If Not ftnPosesEqual(tbpBotPose, tbpPreviousPose) And Not tmiMoveInfo.boolBotIsMoving Then subPathGUI()
                subPathGUI()
                subDrawBot(tbpBotPose, tbpPreviousPose.Location)
                tbpPreviousPose = tbpBotPose
                subVectorGUI()
                subArmGUI()
                subLmGUI()
                If boolDock Then subDrawSmallCircle(tmcDock, Color.Purple)
                subRBPFGUI()
                If (boolPlaceDestination And boolMouseDown) Then subDrawDestination(tmcDestination)
                subUpdateAllText(tbpBotPose)
                timerGUI.Enabled = True
            End If
        Catch ex As Exception
            timerGUI.Enabled = True
            frmDebug.subWriteLogs(" EXCEPTION in subGuiUpdate :  " & ex.ToString)
        End Try
    End Sub

    Sub subButtonGui()
        Try
            If boolStop Then
                Me.picStop.BorderStyle = FormBorderStyle.Fixed3D
            Else
                Me.picStop.BorderStyle = FormBorderStyle.None
            End If
            If boolPlaceBot Then
                Me.picBot.BorderStyle = FormBorderStyle.Fixed3D
            Else
                Me.picBot.BorderStyle = FormBorderStyle.None
            End If
            If boolPlaceDestination Then
                Me.picDestination.BorderStyle = FormBorderStyle.Fixed3D
            Else
                Me.picDestination.BorderStyle = FormBorderStyle.None
            End If
            If boolPause Then
                Me.picPause.BorderStyle = FormBorderStyle.Fixed3D
            Else
                Me.picPause.BorderStyle = FormBorderStyle.None
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subButtonGui :  " & ex.ToString)
        End Try
    End Sub

    Dim boolPathDisplayed As Boolean = False
    Sub subPathGUI()
        Try
            If boolDrivingToDestination And Not boolPathDisplayed Then
                subDrawPath(tmcDestination)
                boolPlaceDestination = False
                boolPathDisplayed = True
            ElseIf Not boolDrivingToDestination Then
                boolPathDisplayed = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subPathGUI :  " & ex.ToString)
        End Try
    End Sub

    Sub subRBPFGUI()
        Try
            If boolEnableRBPF Then
                If Not tmiMoveInfo.boolBotIsMoving And Not boolSimulatedMovement Then
                    For intParticleNumber As Integer = 0 To intNumberOfParticles - 1 Step Max(intNumberOfParticles / 128, 1)
                        subDrawMapPixel(tbpParticlePose(intParticleNumber).Location.X, tbpParticlePose(intParticleNumber).Location.Y, Color.CornflowerBlue)
                    Next
                End If
                If Not boolSimulatedMovement Then
                    frmSlam.txtParticlePose.Text = tbpParticlePose(frmSlam.updnMapSelect.Value).Location.X & "' " & tbpParticlePose(frmSlam.updnMapSelect.Value).Location.Y & " at " & tbpParticlePose(frmSlam.updnMapSelect.Value).Orientation
                    frmSlam.txtError.Text = sngRMS_Error(frmSlam.updnMapSelect.Value).ToString & ",  " & sngRMS_ErrorAvg(frmSlam.updnMapSelect.Value).ToString
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subRBPFGUI :  " & ex.ToString)
        End Try
    End Sub



    Sub subArmGUI()
        Try
            If frmArm.Visible Then
                If Not boolLocalizationInProgress Then  '  
                    subDrawSmallCircle(ftnConvertToTMC(frmArm.updnX.Value, frmArm.updnY.Value), Color.HotPink)
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subArmGUI :  " & ex.ToString)
        End Try
    End Sub

    Sub subLmGUI()
        Try
            If frmLandMarkEdit.Visible Then
                If Not boolLocalizationInProgress Then  '  
                    subDrawLandMark()
                    myMap.CreateGraphics.DrawRectangle(Pens.CornflowerBlue, tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkValidRegion.X * sngZoom, tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkValidRegion.Y * sngZoom,
                                                                           tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkValidRegion.Width * sngZoom, tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkValidRegion.Height * sngZoom)
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subLmGUI :  " & ex.ToString)
        End Try
    End Sub

    Sub subVectorGUI()
        Try
            If boolLocalizationInProgress And Not tmiMoveInfo.boolBotIsMoving Or classScanner.chkContinuous.Checked Then
                subLmGUI()
                subDrawScanVectors(tbpBotPose, boolShowGreenVectors)
            End If
            If Not boolLocalizationInProgress Then
                If boolShowGreenVectors Then
                    subDrawScanVectors(tbpBotPose, boolShowGreenVectors)
                    boolShowGreenVectors = False
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subVectorGUI :  " & ex.ToString)
        End Try
    End Sub

    Sub subUpdateAllText(ByRef tbpBotPose As typeBotPose)
        Try
            If Not Me.InvokeRequired Then
                If Not updnOrientation.Focused Then updnOrientation.Value = ftnConvertTo360(tbpBotPose.Orientation)
                subUpdateStatusLine()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subUpdateAllText :  " & ex.ToString)
        End Try
    End Sub

    Sub subUpdateStatusLine()
        Try
            If Not Me.InvokeRequired Then
                Dim sngLaptopBatteryPercent As Single = Int(SystemInformation.PowerStatus.BatteryLifePercent * 100)
                If (sngLaptopBatteryPercent <> 999) Then _
                        strStatusLaptopVoltage = "Laptop: " & sngLaptopBatteryPercent.ToString & "%"
                booRccConnected = (boolEnableRcc And classRccClient.Connected)
                strStatusCommunications = "Comms: "
                If boolBotConnected Then strStatusCommunications += "Bot "
                If boolKinectConnected Then _
                       strStatusCommunications += "Kinect "
                If boolURGConnected Then _
                        strStatusCommunications += "URG04 "
                If frmArm.boolArmConnected Then strStatusCommunications += "Arm "
                If booRccConnected Then strStatusCommunications += "RCC "
                If classAPI1Server.Connected Then strStatusCommunications += "API1 "
                If classAPI2Server.Connected Then strStatusCommunications += "API2 "

                Dim stbStatus As StringBuilder = New StringBuilder(strStatusDriving)
                Dim cap As Int16 = stbStatus.EnsureCapacity(191)
                stbStatus.Append("                                             ")
                stbStatus.Remove(40, stbStatus.Length - 40)
                stbStatus.Append(strStatusCommunications)
                stbStatus.Append("                                   ")
                stbStatus.Remove(70, stbStatus.Length - 70)
                stbStatus.Append(strStatusBotVoltage)
                stbStatus.Append("                 ")
                stbStatus.Remove(85, stbStatus.Length - 85)
                stbStatus.Append(strStatusLaptopVoltage)
                stbStatus.Append("                         ")
                stbStatus.Remove(105, stbStatus.Length - 105)
                stbStatus.Append(strStatusMouseCoordinates)
                stbStatus.Append("                    ")
                stbStatus.Remove(120, stbStatus.Length - 120)
                stbStatus.Append(strStatusMapContents)

                Me.StatusDriving.Text = stbStatus.ToString
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subUpdateStatusLine :  " & ex.ToString)
        End Try
    End Sub


    Private Sub subConfigureMenu(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuConfigure.Click
        Try
            If boolER1Selected Then
                mnuAngularVelocity.Visible = True
                mnuAngularAcceleration.Visible = True
                mnuVelocity.Visible = True
                mnuBotAcceleration.Visible = True
                mnuBotDiameter.Visible = True
                mnuBotSelect.Text = "Bot Select                   (ER1)"
                mnuSteeringBiasCorrection.Visible = False
                mnuDistanceCompensation.Visible = False
                mnuPowerMoving.Visible = True
                mnuPowerStopped.Visible = True
            ElseIf boolHeroSelected Then
                mnuAngularVelocity.Visible = False
                mnuAngularAcceleration.Visible = False
                mnuVelocity.Visible = False
                mnuBotAcceleration.Visible = False
                mnuBotDiameter.Visible = False
                mnuBotSelect.Text = "Bot Select                   (Hero)"
                mnuDistanceCompensation.Visible = True
                mnuSteeringBiasCorrection.Visible = True
                mnuPowerMoving.Visible = False
                mnuPowerStopped.Visible = False
            Else
                mnuSteeringBiasCorrection.Visible = False
                mnuPowerMoving.Visible = False
                mnuPowerStopped.Visible = False
                mnuDistanceCompensation.Visible = False
            End If
            mnuBotComPort.Visible = boolHeroSelected
            If (comBot IsNot Nothing) Then mnuBotComPort.Text = "Bot Com Port           (" & comBot.PortName & ")"
            If boolBotConnected And boolHeroSelected Then
                mnuBotComPort.Text = "ER1          (" & comBot.PortName & ")"
            Else
                mnuBotComPort.Text = "ER1"
            End If
            mnuHeroSelected.Checked = boolHeroSelected
            mnuER1Direct.Checked = boolER1Selected
            If boolBotConnected And boolER1Selected Then
                mnuER1Direct.Text = "ER1          (" & comBot.PortName & ")"
            Else
                mnuER1Direct.Text = "ER1"
            End If
            mnuFaceRecognition.Checked = boolEnableFaceRecognition
            mnuVideo.Checked = boolEnableVideo
            mnuArmControl.Checked = frmArm.boolEnableArm
            mnuDock.Checked = boolDock
            mnuWheelSeparation.Visible = boolER1Selected
            mnuWheelSeparation.Text = "Wheel Separation           (" & sngBotWheelSeparation.ToString & ")"
            mnuEnableBumperSwitches.Visible = boolER1Selected
            mnuSimulatedMovement.Checked = boolSimulatedMovement
            mnuMute.Checked = boolMute
            mnuLogFileEnable.Checked = boolLogFileEnable
            mnuHeroSelected.Checked = boolHeroSelected
            mnuEnableBumperSwitches.Checked = boolEnableBumperSwitches_ER1
            mnuBotPose.Text = "Pose                      (" & tbpBotPose.Location.X & ", " & tbpBotPose.Location.Y & "    " & tbpBotPose.Orientation & ")"
            mnuBotDiameter.Text = "Bot Diameter                (" & sngBotDiameter & ")"
            mnuBotAcceleration.Text = "Acceleration                  (" & sngBotLinearAcceleration & ")"
            mnuVelocity.Text = "Velocity                          (" & sngBotLinearVelocity & ")"
            mnuAngularVelocity.Text = "Angular Velocity             (" & sngBotAngularVelocity & ")"
            mnuAngularAcceleration.Text = "Angular Acceleration      (" & sngBotAngularAcceleration & ")"
            mnuSteeringBiasCorrection.Text = "Steering Bias Correction      (" & sngSteeringBiasOffsetCorrection & ")"
            mnuPowerMoving.Text = "Power Moving            (" & intPowerMoving & ")"
            mnuPowerStopped.Text = "Power Stopped            (" & intPowerStopped & ")"
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subConfigure_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuTools_Click(sender As Object, e As EventArgs) Handles mnuTools.Click
        subConfigureMenu(sender, e)
    End Sub


    Sub subSizeChanged(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Me.SizeChanged
        Try
            If (MouseButtons = MouseButtons.Left) Then subResizeMap() ' synth.SpeakAsync caused problems with me.height
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subSizeChanged :  " & ex.ToString)
        End Try
    End Sub


    Private Sub subResizeMap()
        Try
            If (Me.WindowState <> FormWindowState.Minimized) Then
                sngZoom = (CSng(Me.Height - 150) / intMapHeight)
                myMap.Height = CInt(sngZoom * intMapHeight)
                myMap.Width = CInt(sngZoom * intMapWidth)
                Me.Width = myMap.Width + 80
                If Me.Width > Screen.PrimaryScreen.Bounds.Width Then _
                            Me.Width = Screen.PrimaryScreen.Bounds.Width
                If Me.Height > Screen.PrimaryScreen.Bounds.Height Then _
                        Me.Height = Screen.PrimaryScreen.Bounds.Height
            End If
            subRecalculateVariables()
            Me.Refresh()
            myMap.Refresh()
            myMap.Focus()
            Application.DoEvents()
            If boolDrivingToDestination Then subDrawPath(tmcDestination)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subResizeMap :  " & ex.ToString)
        End Try
    End Sub



    Dim boolPlacementActive As Boolean = False
    Dim tmcMouseDown As typeMapCoordinate
    Public Sub myMapNouseDown(ByVal sender As System.Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles myMap.MouseDown
        Try
            Dim tmcMapPoint As typeMapCoordinate = ftnMouseToMapPosition()
            boolMouseDown = (e.Button = MouseButtons.Left)

            If e.Button = MouseButtons.Right Then
                boolMouseDown = False
                MapContextMenu.Visible = True
                MapContextMenu.Left = MousePosition.X
                MapContextMenu.Top = MousePosition.Y
            ElseIf (boolPlaceBot And boolMouseDown) Then
                If ((Control.MouseButtons & MouseButtons.Left) <> MouseButtons.Left) And Not ftnInWall_NoChk(tmcMapPoint, intBestParticle) Then
                    frmDebug.subWriteLogs(vbCrLf & vbCrLf & vbCrLf & " ******************************************************************************************" & vbCrLf)
                    frmDebug.subWriteLogs(" ************  New Bot Location selected: Location = " & tmcMapPoint.X & ", " & tmcMapPoint.Y & vbCrLf)
                    subChangeParticlePose(tmcMapPoint.X - tbpBotPose.Location.X, tmcMapPoint.Y - tbpBotPose.Location.Y, 0)
                    tbpBotPose.Location = tmcMapPoint 'Place the bot icon
                    If Not boolSlam Then subInitParticles(tbpBotPose, intNumberOfParticles)
                    boolPlacementActive = True
                End If
            ElseIf (boolPlaceDestination And boolMouseDown) Then
                tmcDestination = tmcMapPoint
            ElseIf (frmLandMarkEdit.Visible And frmLandMarkEdit.boolLocationFocused And boolMouseDown) Then ' Enter a Landmark Location
                tmcMapPoint = ftnMouseToMapPosition()
                tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkLocation.X = tmcMapPoint.X
                tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkLocation.Y = tmcMapPoint.Y
            ElseIf (frmArm.Visible And frmArm.boolLocationFocused And boolMouseDown) Then ' Enter an object Location
                tmcMapPoint = ftnMouseToMapPosition()
                frmArm.subUpdateObjectLocation(tmcMapPoint)
            ElseIf frmLandMarkEdit.Visible And frmLandMarkEdit.boolValidtyRegionFocused And boolMouseDown Then  ' Determine LandMark Validity area
                tmcMouseDown = ftnMouseToMapPosition()
            End If
            boolPlacementActive = ((Control.MouseButtons & MouseButtons.Left) <> MouseButtons.Left)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in myMapNouseDown :  " & ex.ToString)
        End Try
    End Sub

    Public Sub myMapNouseUp(ByVal sender As System.Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles myMap.MouseUp
        Try
            If boolPlaceDestination Then
                tmcDestination = ftnMouseToMapPosition()
                Refresh()
                'subGuiUpdate(sender, e)
                Dim threadDriveToDestination As New Thread(AddressOf ftnDriveToUserChosenDestination)
                threadDriveToDestination.Start()
            ElseIf boolPlaceBot Then
                tbpBotPose.Location = ftnMouseToMapPosition()
                Refresh()
                'subGuiUpdate(sender, e)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in myMapNouseUp :  " & ex.ToString)
        End Try
    End Sub


    Dim boolMap_MouseMoveBusy As Boolean = False
    Public Sub myMap_MouseMove(ByVal sender As System.Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles myMap.MouseMove
        Try
            If Not boolMap_MouseMoveBusy Then
                boolMap_MouseMoveBusy = True
                If boolLocalizationInProgress Then
                    Me.Cursor = Cursors.WaitCursor
                Else
                    Me.Cursor = Cursors.Arrow
                End If
                Dim tmcMapPoint As typeMapCoordinate = ftnMouseToMapPosition()
                myMap.Select()
                strStatusMouseCoordinates = tmcMapPoint.X.ToString("0000") & ", " & tmcMapPoint.Y.ToString("0000")
                strStatusMapContents = byteNavigationArray(tmcMapPoint.X, tmcMapPoint.Y).ToString & ", " & intPathArray(tmcMapPoint.X, tmcMapPoint.Y) & ", " &
                                               intDistanceFromWallSquaredArray(intBestParticle, tmcMapPoint.X, tmcMapPoint.Y).ToString & ", " & boolOccupiedCell(0, tmcMapPoint.X, tmcMapPoint.Y).ToString

                If e.Button <> MouseButtons.Left Then boolMouseDown = False
                If (boolPlaceBot And boolMouseDown And boolPlacementActive) Then    'Drag the bot icon
                    'subChangeParticlePose(tmcMapPoint.X - tbpBotPose.Location.X, tmcMapPoint.Y - tbpBotPose.Location.Y, 0)
                    If Not ftnInWall_NoChk(tmcMapPoint, intBestParticle) Then
                        tbpBotPose.Location = tmcMapPoint
                        If Not boolSlam Then subInitParticles(tbpBotPose, intNumberOfParticles)
                        boolSettingsChanged = True
                    End If
                ElseIf (boolPlaceDestination And boolMouseDown And boolPlacementActive) Then
                    tmcDestination = tmcMapPoint  'Place the Destination icon
                ElseIf frmLandMarkEdit.Visible And boolMouseDown And frmLandMarkEdit.boolValidtyRegionFocused Then
                    subLMValidityRegion(tmcMouseDown)
                ElseIf frmLandMarkEdit.Visible And boolMouseDown And frmLandMarkEdit.boolLocationFocused Then
                    tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkLocation.X = tmcMapPoint.X
                    tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkLocation.Y = tmcMapPoint.Y
                End If
                boolMap_MouseMoveBusy = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in myMap_MouseMove :  " & ex.ToString)
        End Try
    End Sub


    Sub subLMValidityRegion(ByVal tmcMouseDown As typeMapCoordinate)
        Try
            If boolPlaceDestination = False And boolPlaceBot = False And boolMouseDown _
                    And frmLandMarkEdit.Visible And frmLandMarkEdit.boolValidtyRegionFocused Then  ' Determine LandMark Validity area
                Dim intWidth, intHeight, intX, intY As Integer
                If (ftnMouseToMapPosition().Y >= tmcMouseDown.Y) Then
                    intHeight = ftnMouseToMapPosition().Y - tmcMouseDown.Y
                    intY = tmcMouseDown.Y
                Else
                    intHeight = (tmcMouseDown.Y - ftnMouseToMapPosition().Y)
                    intY = ftnMouseToMapPosition().Y
                End If
                If (ftnMouseToMapPosition().X >= tmcMouseDown.X) Then
                    intWidth = ftnMouseToMapPosition().X - tmcMouseDown.X
                    intX = tmcMouseDown.X
                Else
                    intWidth = (tmcMouseDown.X - ftnMouseToMapPosition().X)
                    intX = ftnMouseToMapPosition().X
                End If
                tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkValidRegion.X = intX
                tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkValidRegion.Y = intY
                tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkValidRegion.Width = intWidth
                tlmLandMarkArray(frmLandMarkEdit.UpDnLM.Value).LandMarkValidRegion.Height = intHeight
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subLMValidityRegion :  " & ex.ToString)
        End Try
    End Sub



    Private Sub updnOrientation_ValueChanged(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles updnOrientation.ValueChanged
        Try
            If Not Me.InvokeRequired And updnOrientation.Focused And updnOrientation.Enabled Then
                updnOrientation.Enabled = False
                updnOrientation.Value = ftnConvertTo360(Int(updnOrientation.Value))
                If (tbpBotPose.Orientation <> updnOrientation.Value) Then
                    subChangeParticlePose(0, 0, ftnAngleDifference(Convert.ToInt16(updnOrientation.Value), tbpBotPose.Orientation))
                    tbpBotPose.Orientation = updnOrientation.Value
                    tmiMoveInfo.tbpStartingPose.Orientation = updnOrientation.Value
                    frmDebug.subWriteLogs(" ************  New Bot Orientation selected:  = " & tbpBotPose.Orientation & " Degrees")
                    updnOrientation.Enabled = True
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in updnOrientation_ValueChanged :  " & ex.ToString)
            updnOrientation.Enabled = True
            updnOrientation.Value = ftnConvertTo360(tbpBotPose.Orientation)
        End Try
    End Sub



    Private Sub picPause_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles picPause.DoubleClick
        On Error Resume Next
        boolPause = Not boolPause
        If boolPause Then frmDebug.subWriteLogs(Now() & "    >>>>  PAUSE button was pushed  >>> ")
    End Sub

    Private Function ftnMouseToMapPosition() As typeMapCoordinate
        Try
            ftnMouseToMapPosition.X = (MousePosition.X - Me.Location.X - myMap.Location.X - myMap.Padding.Left - myMap.Margin.Left - 5) / sngZoom
            ftnMouseToMapPosition.Y = (MousePosition.Y - Me.Location.Y - myMap.Location.Y - myMap.Padding.Top - myMap.Margin.Top - 27) / sngZoom
            ftnMouseToMapPosition = ftnCheckMapArrayLimits(ftnMouseToMapPosition)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnMouseToMapPosition :  " & ex.ToString)
        End Try
    End Function









    '==========================  MENUS  ==================================================



    '==========================  File Menu  ==================================================

    Delegate Sub DelegateLoadSettings(ByVal sender As System.Object, ByVal e As System.EventArgs)
    Public Sub mnuLoadSettings_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuLoadSettings.Click
        Try
            If InvokeRequired Then
                Dim newDelegate As New DelegateLoadSettings(AddressOf mnuLoadSettings_Click)
                Invoke(newDelegate, New Object() {sender, e})
            Else
                Me.StatusDriving.ForeColor = Color.Black
                LoadSettings("GotABot_Settings.xml")
                boolSettingsChanged = False
                subResizeMap()
                subConvertMapToArray(intBestParticle)
                strStatusDriving = "Waiting for command "
                subInitParticles(tbpBotPose, intNumberOfParticles)
                ftnSendCommand("play phraseA 'Ready'")
                Me.Refresh()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuLoadSettings_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuLoadSettingsFileSelect_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuLoadSettingsFileSelect.Click
        On Error Resume Next
        Me.StatusDriving.ForeColor = Color.Black
        OpenFD.Title = "Load GotABot Settings"
        OpenFD.Filter = "xml Files(*.xml)|*.xml"
        Dim DidWork As Integer = OpenFD.ShowDialog()
        If DidWork <> DialogResult.Cancel Then _
            LoadSettings(OpenFD.FileName)
        boolSettingsChanged = False
        subResizeMap()
        subConvertMapToArray(intBestParticle)
        subInitParticles(tbpBotPose, intNumberOfParticles)
        If boolHeroSelected Then
            ftnHeroConfig()
        ElseIf mnuER1Direct.Checked Then
            ftnER1_Config()
        ElseIf boolER1Selected Then
            ftnSendCommand("stop") ' establish connection to the ER1  
        End If
        Me.Refresh()
        strStatusDriving = "Waiting for command "
        ftnSendCommand("play phraseA 'Ready'")
    End Sub


    Public Sub mnuSaveSettings_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuSaveSettings.Click
        On Error Resume Next
        SaveSettings("GotABot_Settings.xml")
        boolSettingsChanged = False
    End Sub

    Private Sub mnuSaveSettingsFileSelect_Click() Handles mnuSaveSettingsFileSelect.Click
        On Error Resume Next
        Dim saveFD As New SaveFileDialog()
        saveFD.Title = "GotABot Settings Changed - Save?"
        saveFD.Filter = "xml Files(*.xml)|*.xml"
        Dim DidWork As Integer = saveFD.ShowDialog()
        If DidWork <> DialogResult.Cancel Then _
            SaveSettings(saveFD.FileName)
        boolSettingsChanged = False
    End Sub


    Private Sub mnuNewLog_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuNewLog.Click
        On Error Resume Next
        If (Dir("GotABot_Log.txt") <> "") Then My.Computer.FileSystem.DeleteFile("GotABot_Log.txt",
                    FileIO.UIOption.AllDialogs, FileIO.RecycleOption.DeletePermanently, FileIO.UICancelOption.DoNothing)
    End Sub

    Private Sub mnuOpenMapLog_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuOpenMapLog.Click
        On Error Resume Next
        Process.Start("notepad", "GotABot_Log.txt")
    End Sub





    Private Sub mnuExit_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuExit.Click
        Me.Close()
    End Sub


    Dim boolAlreadyClosed As Boolean = False
    Delegate Sub DelegateGotABotExit()
    Public Sub subGotABotExit() Handles Me.FormClosed, Me.FormClosing
        Try
            If Me.InvokeRequired And Not boolAlreadyClosed Then
                Dim newDelegate As New DelegateGotABotExit(AddressOf subGotABotExit)
                Me.Invoke(newDelegate, New Object() {})
            ElseIf Not boolAlreadyClosed Then
                boolAlreadyClosed = True
                timerWatchDog.Enabled = False
                If boolBotConnected Then tbpBotPose = modBotMovementControl.ftnBotStop(tbpBotPose, tmiMoveInfo.tbpTargetPose)
                boolStop = True
                ftnSendCommand("set power stopped " & 0)
                timerGUI.Enabled = False
                If boolSettingsChanged Then _
                         mnuSaveSettingsFileSelect_Click()
                boolKinectConnected = False
                ftnUrgClose()
                modIF_Kinect.ftnKinectStop()
                ftnSendCommand("play phrase 'Goodby'")
                frmArm.subArmExit()
                ftnER1_Close()
                subWait(2000)
                boolShutDown = True
                subWait(2000)
                Me.Close()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subGotABotExit :  " & ex.ToString)
        End Try
    End Sub









    '==========================  Configure Menu  ==================================================



    Private Sub mnuEditMap_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuEditMap.Click
        Try
            OpenFD.Title = "Edit a Map"
            OpenFD.Filter = "BitMap Files(*.bmp)|*.bmp"
            Dim DidWork As Integer = OpenFD.ShowDialog()
            If DidWork <> DialogResult.Cancel Then
                bmpMap.Dispose()

                Dim editMap As Process = Process.Start("mspaint", OpenFD.SafeFileName)
                editMap.WaitForExit()
                subWait(500)
                subOpenMap(OpenFD.FileName)
            End If
            subResizeMap()
            Me.Refresh()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuEditMap_Click :  " & ex.ToString)
        End Try
    End Sub

    Public Sub mnuOpenMap_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuOpenMap.Click
        Try
            OpenFD.Title = "Open a New Map"
            OpenFD.Filter = "BitMap Files(*.bmp)|*.bmp"
            Dim DidWork As Integer = OpenFD.ShowDialog()
            If DidWork <> DialogResult.Cancel Then subOpenMap(OpenFD.FileName)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuOpenMap_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuSaveMap_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuSaveMap.Click
        Try
            Dim saveFD As New SaveFileDialog()
            saveFD.Title = "Save a copy of the map"
            saveFD.Filter = "BitMap Files(*.bmp)|*.bmp"
            Dim DidWork As Integer = saveFD.ShowDialog()
            If DidWork <> DialogResult.Cancel Then _
                  bmpMap.Save(saveFD.FileName)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuSaveMap_Click :  " & ex.ToString)
        End Try
    End Sub


    Delegate Sub DelegateOpenMap(ByVal strFileName As String)
    Public Sub subOpenMap(ByVal strFileName As String)
        Try
            If InvokeRequired Then
                Dim newDelegate As New DelegateOpenMap(AddressOf subOpenMap)
                BeginInvoke(newDelegate, New Object() {strFileName})
            Else
                bmpMap.Dispose()
                bmpMap = New Bitmap(System.Drawing.Image.FromFile(strFileName))
                intMapHeight = bmpMap.Height
                intMapWidth = bmpMap.Width
                subResizeMap()
                If (myMap.Image IsNot Nothing) Then myMap.Image.Dispose()
                Me.myMap.Image = bmpMap.Clone
                subRecalculateVariables()
                subConvertMapToArray(intBestParticle) ' Convert the Map from .bmp format to a byte array
                subInitParticles(tbpBotPose, intNumberOfParticles)
                Me.Refresh()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subOpenMap :  " & ex.ToString)
        End Try
    End Sub


    Private Sub mnuViewLM_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
        On Error Resume Next
        frmLandMarkEdit.Visible = Not frmLandMarkEdit.Visible
        boolViewLM = frmLandMarkEdit.Visible
        frmLandMarkEdit.Focus()
    End Sub



    Private Sub subScannerConfigure(sender As Object, e As EventArgs) Handles mnuScannerConfigure.Click
        On Error Resume Next
        classScanner.Visible = True
        classScanner.Focus()
    End Sub


    Private Sub mnuDistanceCompensation_Click(sender As Object, e As EventArgs) Handles mnuDistanceCompensation.Click
        On Error Resume Next
        frmDistanceComp.Visible = True
        frmDistanceComp.Focus()
    End Sub

    Private Sub mnuER1Direct_Click(sender As Object, e As EventArgs) Handles mnuER1Direct.Click
        Try
            mnuER1Direct.Checked = Not mnuER1Direct.Checked
            boolER1Selected = mnuER1Direct.Checked
            If boolER1Selected Then
                mnuHeroSelected.Checked = False
                boolHeroSelected = False
                intNumberOfBeams = 64
                sngBotMovementAccuracy = 0.7
                sngMaxTravel = 111
                ftnER1_Config()
            End If
            boolSettingsChanged = True
            subConfigureMenu(sender, e)
            mnuConfigure.ShowDropDown()
            mnuBotConfigure.ShowDropDown()
            mnuBotSelect.ShowDropDown()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuER1Direct_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuHeroSelected_Click(sender As Object, e As EventArgs) Handles mnuHeroSelected.Click
        On Error Resume Next
        mnuHeroSelected.Checked = Not mnuHeroSelected.Checked
        boolHeroSelected = mnuHeroSelected.Checked
        If boolHeroSelected Then
            subHeroConfigure()
        End If
        boolSettingsChanged = True
        subConfigureMenu(sender, e)
        mnuConfigure.ShowDropDown()
        mnuBotConfigure.ShowDropDown()
        mnuBotSelect.ShowDropDown()
    End Sub

    Sub subHeroConfigure()
        Try
            boolER1Selected = False
            mnuER1Direct.Checked = False
            sngBotAngularAcceleration = 999
            sngBotAngularVelocity = 999
            sngBotLinearVelocity = 40
            sngBotLinearAcceleration = 40
            sngBotDiameter = 14.5
            sngBotMovementAccuracy = 0.4
            sngMaxTravel = 48
            If Not boolURGConnected And Not boolKinectConnected Then
                sngScanFOV = 350
                tmcScanVectorOffset.X = 0
                tmcScanVectorOffset.Y = 0
                intScanOffsetDegrees = 0
                intScanVectorsMaxRangeInches = 100
                intScanVectorsMinRangeInches = 0
                intNumberOfBeams = 40
                subCalculateScanBeamAngles()
                subCalculateScanCalCoefficients()
            End If
            subRecalculateVariables()
            boolStop = True
            strStatusDriving = "Hero Configured "
            ftnHeroConfig()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subHeroConfigure :  " & ex.ToString)
        End Try
    End Sub


    Private Sub mnuDownloadHeroS19_Click(sender As Object, e As EventArgs)
        subHeroS19Download()
        Me.Refresh()
    End Sub


    Private Sub mnuEnableBumperSwitches_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuEnableBumperSwitches.Click
        On Error Resume Next
        mnuEnableBumperSwitches.Checked = Not mnuEnableBumperSwitches.Checked
        boolEnableBumperSwitches_ER1 = mnuEnableBumperSwitches.Checked
        boolSettingsChanged = True
        subConfigureMenu(sender, e)
        mnuConfigure.ShowDropDown()
        mnuOptions.ShowDropDown()
    End Sub

    Private Sub mnuBotComPort_Click(sender As Object, e As EventArgs) Handles mnuBotComPort.Click
        Try
            If boolHeroSelected Then
                comBot.PortName = InputBox("Enter Bot Com Port (ie COM1 or COM2 or ... )", "Bot Com Port Name", comBot.PortName)
                boolSettingsChanged = True
                subConfigureMenu(sender, e)
                mnuConfigure.ShowDropDown()
                mnuBotConfigure.ShowDropDown()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuBotComPort_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuBotDiameter_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuBotDiameter.Click
        On Error Resume Next
        Dim strBotDiameter As String
        strBotDiameter = InputBox("Enter Bot Diameter (in inches) ", "Bot Diameter ", Str(sngBotDiameter))
        If strBotDiameter <> "" And IsNumeric(strBotDiameter) Then _
                sngBotDiameter = Convert.ToSingle(strBotDiameter)
        subRecalculateVariables()
        subConvertMapToArray(intBestParticle)
        boolSettingsChanged = True
        subConfigureMenu(sender, e)
        mnuConfigure.ShowDropDown()
        mnuBotConfigure.ShowDropDown()
    End Sub

    Private Sub mnuPowerMoving_Click(sender As Object, e As EventArgs) Handles mnuPowerMoving.Click
        Try
            Dim strPowerMoving As String
            strPowerMoving = InputBox("Enter Power Moving (0-100) ", "Power Moving", Str(intPowerMoving))
            If strPowerMoving <> "" And IsNumeric(strPowerMoving) Then
                intPowerMoving = Convert.ToSingle(strPowerMoving)
                If (intPowerMoving > 100) Or (intPowerMoving < 0) Then
                    intPowerMoving = 60
                    ftnSendCommand("play phrase Beep")
                End If
                ftnSendCommand("set power moving " & intPowerMoving)
            End If
            subConfigureMenu(sender, e)
            mnuConfigure.ShowDropDown()
            mnuBotConfigure.ShowDropDown()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuPowerMoving_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuPowerStopped_Click(sender As Object, e As EventArgs) Handles mnuPowerStopped.Click
        Try
            Dim strPowerStopped As String
            strPowerStopped = InputBox("Enter Power Stopped (0-100) ", "Power Stopped", Str(intPowerStopped))
            If strPowerStopped <> "" And IsNumeric(strPowerStopped) Then
                intPowerStopped = Convert.ToSingle(strPowerStopped)
                If (intPowerStopped > 100) Or (intPowerStopped < 0) Then
                    intPowerStopped = 6
                    ftnSendCommand("play phrase Beep")
                End If
                ftnSendCommand("set power stopped " & intPowerStopped)
            End If
            ftnSendCommand("stop")
            subConfigureMenu(sender, e)
            mnuConfigure.ShowDropDown()
            mnuBotConfigure.ShowDropDown()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuPowerStopped_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuWheelSeparation_Click(sender As Object, e As EventArgs) Handles mnuWheelSeparation.Click
        Try
            Dim strWheelSeparation As String
            strWheelSeparation = InputBox("Enter WheelSeparation in cm ", "Wheel Separation", Str(sngBotWheelSeparation))
            If strWheelSeparation <> "" And IsNumeric(strWheelSeparation) Then
                sngBotWheelSeparation = Convert.ToSingle(strWheelSeparation)
                ftnSendCommand("set power stopped " & intPowerStopped)
            End If
            subConfigureMenu(sender, e)
            mnuConfigure.ShowDropDown()
            mnuBotConfigure.ShowDropDown()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuWheelSeparation_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuAcceleration_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuBotAcceleration.Click
        On Error Resume Next
        Dim strAcceleration As String
        strAcceleration = InputBox("Enter the Bot Acceleration in centimeters per second /second ", "Acceleration", Str(sngBotLinearAcceleration))
        If strAcceleration <> "" And IsNumeric(strAcceleration) Then
            sngBotLinearAcceleration = Convert.ToSingle(strAcceleration)
            If (sngBotLinearAcceleration <= 0) Then sngBotLinearAcceleration = 1
        End If
        subRecalculateVariables()
        boolSettingsChanged = True
        subConfigureMenu(sender, e)
        mnuConfigure.ShowDropDown()
        mnuBotConfigure.ShowDropDown()
    End Sub

    Private Sub mnuVelocity_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuVelocity.Click
        On Error Resume Next
        Dim strVelocity As String
        strVelocity = InputBox("Enter the Bot Linear Velocity in centimeters per second  ", "Velocity", Str(sngBotLinearVelocity))
        If strVelocity <> "" And IsNumeric(strVelocity) Then
            sngBotLinearVelocity = Convert.ToSingle(strVelocity)
            sngBotLinearVelocityMax = sngBotLinearVelocity
        End If
        subRecalculateVariables()
        boolSettingsChanged = True
        ftnSendCommand("set linear velocity " & Int(sngBotLinearVelocity).ToString)
        subConfigureMenu(sender, e)
        mnuConfigure.ShowDropDown()
        mnuBotConfigure.ShowDropDown()
    End Sub

    Private Sub mnuAngularVelocity_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuAngularVelocity.Click
        On Error Resume Next
        Dim strAngularVelocity As String
        strAngularVelocity = InputBox("Enter the Bot Angular Velocity in degrees per second  ", "AngularVelocity", Str(sngBotAngularVelocity))
        If strAngularVelocity <> "" And IsNumeric(strAngularVelocity) Then
            sngBotAngularVelocity = Convert.ToSingle(strAngularVelocity)
        End If
        subRecalculateVariables()
        boolSettingsChanged = True
        ftnSendCommand("set angular velocity  " & Int(sngBotAngularVelocity).ToString)
        subConfigureMenu(sender, e)
        mnuConfigure.ShowDropDown()
        mnuBotConfigure.ShowDropDown()
    End Sub

    Private Sub mnuAngularAcceleration_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuAngularAcceleration.Click
        On Error Resume Next
        Dim strAngularAcceleration As String
        strAngularAcceleration = InputBox("Enter the Bot Angular Acceleration in degrees per second per second ", "AngularAcceleration", Str(sngBotAngularAcceleration))
        If strAngularAcceleration <> "" And IsNumeric(strAngularAcceleration) Then
            sngBotAngularAcceleration = Convert.ToSingle(strAngularAcceleration)
            If (sngBotAngularAcceleration <= 0) Then sngBotAngularAcceleration = 1
        End If
        subRecalculateVariables()
        boolSettingsChanged = True
        subConfigureMenu(sender, e)
        mnuConfigure.ShowDropDown()
        mnuBotConfigure.ShowDropDown()
    End Sub

    Private Sub mnuSteeringBias_Click(sender As Object, e As EventArgs) Handles mnuSteeringBiasCorrection.Click
        Try
            Dim strSteeringBiasOffsetCorrection As String
            strSteeringBiasOffsetCorrection = InputBox("If the robot does not steer straight ahead then enter an offset correction" & vbCrLf &
                                                  " '-ve offset' = robot will steer more left" & vbCrLf &
                                                  " '+ve offset' = robot will steer more right", "SteeringBiasCorrection", Str(sngSteeringBiasOffsetCorrection))
            If strSteeringBiasOffsetCorrection <> "" And IsNumeric(strSteeringBiasOffsetCorrection) Then
                sngSteeringBiasOffsetCorrection = Convert.ToSingle(strSteeringBiasOffsetCorrection)
            End If
            boolSettingsChanged = True
            subConfigureMenu(sender, e)
            mnuConfigure.ShowDropDown()
            mnuBotConfigure.ShowDropDown()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuSteeringBias_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuViewDebug_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuViewDebug.Click
        On Error Resume Next
        boolViewDebug = True
        frmDebug.Visible = boolViewDebug
        frmDebug.Focus()
    End Sub


    Private Sub mnuToolsOptions_Click(sender As System.Object, e As System.EventArgs)
        On Error Resume Next
    End Sub

    Function ProcessesRunning(ByVal ProcessName As String) As Integer
        '  Generalized Function to Check if an Instance of Application is running in the machine
        Try
            Return Process.GetProcessesByName(ProcessName).GetUpperBound(0) + 1
        Catch ex As Exception
            Return 0
            frmDebug.subWriteLogs(" EXCEPTION in ProcessesRunning :  " & ex.ToString)
        End Try
    End Function

    Private Sub mnuSimulatedMovement_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuSimulatedMovement.Click
        On Error Resume Next
        If (boolSimulatedMovement = False) Then
            boolSimulatedMovement = True
            mnuSimulatedMovement.Checked = True
        Else
            boolSimulatedMovement = False
            mnuSimulatedMovement.Checked = False
        End If
        boolSettingsChanged = True
        subConfigureMenu(sender, e)
        mnuConfigure.ShowDropDown()
        mnuOptions.ShowDropDown()
    End Sub

    Private Sub mnuLocalize_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuLocalize.Click
        Try
            boolStop = False
            If Not tmiMoveInfo.boolBotIsMoving Then tmiMoveInfo.timeMoveCommandSent = Now()
            Dim threadLocalize As New Thread(AddressOf subLocalizeTop)
            threadLocalize.Start()
            subWait(250)
            Dim timeOut As DateTime = Now().AddMilliseconds(3000)
            While boolLocalizationInProgress And (Now() < timeOut)  ' wait for localize to complete
                subWait(25)
            End While
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuLocalize_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuSLAM_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuSLAM.Click
        Try
            boolStop = False
            mnuOpenMap_Click(sender, e)
            boolSlam = True
            frmSlam.subSlamInit()
            frmSlam.Visible = True
            subInitParticles(tbpBotPose, intNumberOfParticles)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuSLAM_Click :  " & ex.ToString)
        End Try
    End Sub


    Private Sub mnuDock_Click(sender As System.Object, e As System.EventArgs) Handles mnuDock.Click
        On Error Resume Next
        boolDock = Not boolDock
        If boolDock Then
            boolStop = False
            mnuTools.HideDropDown()
            Dim threadDock As New Thread(AddressOf frmDock.subDock)
            threadDock.Start()
        End If
    End Sub

    Private Sub mnuDockConfigure_Click(sender As System.Object, e As System.EventArgs) Handles mnuDockConfigure.Click
        On Error Resume Next
        frmDock.Visible = True
        frmDock.Focus()
    End Sub


    Private Sub mnuFaceRecognition_Click(sender As Object, e As EventArgs) Handles mnuFaceRecognition.Click
        Try
            boolEnableFaceRecognition = Not boolEnableFaceRecognition
            mnuFaceRecognition.Checked = boolEnableFaceRecognition
            If boolEnableFaceRecognition Then frmVideo.subVideoStart()
            boolSettingsChanged = True
            subConfigureMenu(sender, e)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuFaceRecognition_Click :  " & ex.ToString)
        End Try
    End Sub


    Private Sub mnuArmControl_Click(sender As Object, e As EventArgs) Handles mnuArmControl.Click
        Try
            frmArm.boolEnableArm = Not frmArm.boolEnableArm
            mnuArmControl.Checked = frmArm.boolEnableArm
            subConfigureMenu(sender, e)
            mnuTools.ShowDropDown()
            If frmArm.boolEnableArm Then
                frmArm.ftnArmLoad()
                frmArm.subHome(True)
            Else
                frmArm.subArmExit()
            End If
            boolSettingsChanged = True
            subConfigureMenu(sender, e)
            mnuTools.ShowDropDown()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuArmControl_Click :  " & ex.ToString)
        End Try
    End Sub


    Private Sub mnuVideo_Click(sender As Object, e As EventArgs) Handles mnuVideo.Click
        Try
            boolEnableVideo = Not boolEnableVideo
            mnuVideo.Checked = boolEnableVideo
            If boolEnableVideo Then
                frmVideo.subVideoStart()
            Else
                frmVideo.exitToolStripMenuItem_Click()
            End If
            boolSettingsChanged = True
            subConfigureMenu(sender, e)
            mnuTools.ShowDropDown()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuVideo_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuMute_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuMute.Click
        On Error Resume Next
        boolMute = Not boolMute
        mnuMute.Checked = boolMute
        If boolMute Then
            ftnSendCommand("clear")
            ftnSendCommand("speak off")
        Else
            ftnSendCommand("clear")
            ftnSendCommand("speak on")
        End If
        subConfigureMenu(sender, e)
        mnuConfigure.ShowDropDown()
        mnuOptions.ShowDropDown()
    End Sub

    Private Sub mnuApiPassword_Click(sender As Object, e As EventArgs) Handles mnuApiPassword.Click
        Try
            strApiPassword = InputBox("Enter the Password ", " Password", strApiPassword)
            boolSettingsChanged = True
            subConfigureMenu(sender, e)
            mnuConfigure.HideDropDown()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in mnuApiPassword_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuBotPose_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuBotPose.Click
        On Error Resume Next
        frmPose.Visible = True
        frmPose.Focus()
    End Sub




    Private Sub mnuLogFileEnable_Click(sender As Object, e As EventArgs) Handles mnuLogFileEnable.Click
        On Error Resume Next
        mnuLogFileEnable.Checked = Not mnuLogFileEnable.Checked
        boolLogFileEnable = mnuLogFileEnable.Checked
        boolSettingsChanged = True
        subConfigureMenu(sender, e)
        mnuConfigure.ShowDropDown()
        mnuOptions.ShowDropDown()
    End Sub


    '==========================  Tool Strip  ==================================================


    Private Sub OpenToolStripButton_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles OpenToolStripButton.Click
        Try
            If NativeMethods.GetAsyncKeyState(Keys.LShiftKey) Then   ' Test Code
                '    timerGUI.Enabled = False

                tbpBotPose.Location = ftnConvertToTMC(117, 166)
                tbpBotPose.LocationUncertainty = ftnConvertToTMC(13, 13)
                tbpBotPose.Orientation = 183
                tbpBotPose.OrientationUncertainty = 13
                intNumberOfBeams = 32
                intNumberOfParticles = 32
                frmDebug.subWriteLogs("OpenToolStripButton_Click:  intNumberOfBeams =  " & intNumberOfBeams)
                frmDebug.subWriteLogs("OpenToolStripButton_Click:  intNumberOfParticles =  " & intNumberOfParticles)
                tmiMoveInfo.timeMoveCommandSent = Now()
                mnuLocalize_Click(sender, e)
                boolLocalizationInProgress = False
                tmiMoveInfo.boolMoveCommandInProgress = False
                tmiMoveInfo.boolBotIsMoving = False
                tmiMoveInfo.timeMoveCommandSent = Now()
                For i = 1 To 100
                    subConvertMapToArray(intBestParticle)
                Next
                frmDebug.subWriteLogs(" subConvertMapToArray  Time = " & ftnElapsedTimeS() / 100)
                tmiMoveInfo.timeMoveCommandSent = Now()
                For i = 1 To 1000
                    subGeneratePathArray()
                Next
                frmDebug.subWriteLogs(" subGeneratePathArray  Time = " & ftnElapsedTimeS() / 1000)
                tmiMoveInfo.timeMoveCommandSent = Now()
                For i = 1 To 10
                    ftnFindPathToDestination(ftnConvertToTMC(150, 65), tbpBotPose.Location)
                Next
                frmDebug.subWriteLogs(" ftnFindPathToDestination Time = " & ftnElapsedTimeS() / 10)
                tmiMoveInfo.timeMoveCommandSent = Now()
                For i = 1 To 100
                    ftnFindTargetPose(tbpBotPose, ftnConvertToTMC(150, 65))
                Next
                frmDebug.subWriteLogs(" ftnFindTargetPose  Time = " & ftnElapsedTimeS() / 100)
                tmiMoveInfo.timeMoveCommandSent = Now()
                boolEnableRBPF = True
                subInitParticles(tbpBotPose, intNumberOfParticles)
                tmiMoveInfo.timeMoveCommandSent = Now()
                ftnLocalizeRBPF_Top(tbpBotPose, False)
                frmDebug.subWriteLogs(" ftnLocalizeRBPF_Top  Time = " & ftnElapsedTimeS())
                tmiMoveInfo.timeMoveCommandSent = Now()
                For i = 1 To 10
                    subConvertScanToOccupencyOdds(32)
                Next
                frmDebug.subWriteLogs(" subConvertScanToOccupencyOdds   Time = " & ftnElapsedTimeS() / 10)
                tmiMoveInfo.timeMoveCommandSent = Now()
                For i = 1 To 10
                    subGuiUpdate(sender, e)
                Next
                frmDebug.subWriteLogs(" subGuiUpdate   Time = " & ftnElapsedTimeS() / 10)
                boolLocalizationInProgress = False
                boolEnableRBPF = False
                intNumberOfBeams = 32
                boolEnableRBPF = False
                boolLocalizationInProgress = False
                tmiMoveInfo.timeMoveCommandSent = Now()
                mnuLocalize_Click(sender, e)
                boolLocalizationInProgress = False

                timerGUI.Enabled = True
            ElseIf NativeMethods.GetAsyncKeyState(Keys.LControlKey) Then   ' Test Code
                Beep()
                tbpBotPose.Location = ftnConvertToTMC(175, 183)
                tbpBotPose.LocationUncertainty = ftnConvertToTMC(12, 12)
                tbpBotPose.Orientation = 65
                tbpBotPose.OrientationUncertainty = 10
                'subGuiUpdate(sender, e)

                subConvertMapToArray(intBestParticle)
                subGeneratePathArray()
                ftnFindPathToDestination(ftnConvertToTMC(150, 65), tbpBotPose.Location)
                ftnFindTargetPose(tbpBotPose, ftnConvertToTMC(150, 65))
            Else
                mnuOpenMap_Click(sender, e)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in OpenToolStripButton_Click :  " & ex.ToString)
        End Try
    End Sub


    Private Sub SaveToolStripButton_Click(sender As Object, e As EventArgs) Handles SaveToolStripButton.Click
        Try
            If NativeMethods.GetAsyncKeyState(Keys.LShiftKey) Then  ' Test Code
                tbpBotPose.Location = ftnConvertToTMC(114, 168)
                tbpBotPose.Orientation = 178
                Dim tbpTarget As typeBotPose = tbpBotPose
                tmcDestination = ftnConvertToTMC(128, 193)
                Dim threadDriveToDestination As New Thread(AddressOf ftnDriveToUserChosenDestination)
                threadDriveToDestination.Start()
            Else
                mnuSaveSettings_Click(sender, e)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in SaveToolStripButton_Click :  " & ex.ToString)
        End Try
    End Sub


    '==========================  Map Context Menu  ==================================================

    Private Sub ContextMenuStrip1_Opening(ByVal sender As System.Object, ByVal e As System.ComponentModel.CancelEventArgs) Handles MapContextMenu.Opening
        MapContextMenu.Visible = True
    End Sub

    Private Sub mnuLocalizeContext_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuLocalizeContext.Click
        mnuLocalize_Click(sender, e)
    End Sub


    Private Sub mnuAbout_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuAbout.Click
        AboutBox1.Visible = True
    End Sub



    '==================================== Keyboard Manual Control  =============================================
    Dim boolManualControl As Boolean = False
    Public boolCtrlKey As Boolean = False
    Private Sub Form_KeyDown(ByVal sender As Object, ByVal e As System.Windows.Forms.KeyEventArgs) Handles MyBase.KeyDown
        Try
            Dim boolGoodLocalization As Boolean = False
            boolCtrlKey = (e.KeyCode = Keys.ControlKey)
            If Not boolManualControl And Not boolLocalizationInProgress And Not boolSlam Then
                boolManualControl = True
                boolDocked = False
                boolDock = False
                Dim tbpTargetPose As typeBotPose = tbpBotPose
                If e.KeyCode = Keys.Down Then
                    boolStop = False
                    frmDebug.subWriteLogs("))))))  Manual Control:  BACK")
                    If boolSlam Or boolHeroSelected Then
                        While NativeMethods.GetAsyncKeyState(Keys.Down)
                            tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 180, 10)
                            tbpBotPose = ftnBotFwd(tbpBotPose, tbpTargetPose)
                            subWait(1000)
                            NativeMethods.GetAsyncKeyState(Keys.Down)
                        End While
                    Else
                        tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 180, sngMaxTravel)
                        tbpBotPose = ftnBotFwd(tbpBotPose, tbpTargetPose)
                    End If
                    If boolSlam Then
                        Dim threadLocalize As New Thread(AddressOf subLocalizeTop)
                        threadLocalize.Start()
                    End If
                End If
                If e.KeyCode = Keys.Up Then
                    boolStop = False
                    frmDebug.subWriteLogs("))))))  Manual Control:  FWD")
                    If boolSlam Or boolHeroSelected Then
                        While NativeMethods.GetAsyncKeyState(Keys.Up)
                            tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 0, 10)
                            tbpBotPose = ftnBotFwd(tbpBotPose, tbpTargetPose)
                            subWait(1000)
                            NativeMethods.GetAsyncKeyState(Keys.Up)
                        End While
                    Else
                        tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 0, sngMaxTravel)
                        tbpBotPose = ftnBotFwd(tbpBotPose, tbpTargetPose)
                    End If
                    If boolSlam Then
                        Dim threadLocalize As New Thread(AddressOf subLocalizeTop)
                        threadLocalize.Start()
                    End If
                End If
                If e.KeyCode = Keys.Right Then
                    boolStop = False
                    frmDebug.subWriteLogs("))))))  Manual Control:  TURN RIGHT")
                    If boolSlam Or boolHeroSelected Then
                        While NativeMethods.GetAsyncKeyState(Keys.Right)
                            tbpTargetPose.Orientation = ftnConvertTo360(tbpTargetPose.Orientation - 15)
                            tbpBotPose = ftnBotTurn(tbpBotPose, False, tbpTargetPose)
                            subWait(1000)
                            NativeMethods.GetAsyncKeyState(Keys.Right)
                        End While
                    Else
                        tbpTargetPose.Orientation = ftnConvertTo360(tbpTargetPose.Orientation - 135)
                        tbpBotPose = ftnBotTurn(tbpBotPose, False, tbpTargetPose)
                    End If
                    If boolSlam Then
                        Dim threadLocalize As New Thread(AddressOf subLocalizeTop)
                        threadLocalize.Start()
                    End If
                End If
                If e.KeyCode = Keys.Left Then
                    boolStop = False
                    frmDebug.subWriteLogs("))))))  Manual Control:  TURN LEFT")
                    If boolSlam Or boolHeroSelected Then
                        While NativeMethods.GetAsyncKeyState(Keys.Left)
                            tbpTargetPose.Orientation = ftnConvertTo360(tbpTargetPose.Orientation + 10)
                            tbpBotPose = ftnBotTurn(tbpBotPose, False, tbpTargetPose)
                            subWait(1000)
                            NativeMethods.GetAsyncKeyState(Keys.Left)
                        End While
                    Else
                        tbpTargetPose.Orientation = ftnConvertTo360(tbpTargetPose.Orientation + 135)
                        tbpBotPose = ftnBotTurn(tbpBotPose, False, tbpTargetPose)
                    End If
                    If boolSlam Or boolHeroSelected Then
                        While boolManualControl
                            subWait(100)
                        End While
                        If boolSlam Then
                            Dim threadLocalize As New Thread(AddressOf subLocalizeTop)
                            threadLocalize.Start()
                        End If
                    End If
                End If
                If boolHeroSelected And boolManualControl Then
                    subWait(500)
                    boolManualControl = False
                    boolCtrlKey = False
                End If
                'ElseIf boolManualControl And boolLocalizationInProgress Then
                '    MsgBox(" Wait for Localization to complete")
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in Form_KeyDown :  " & ex.ToString)
        End Try
    End Sub

    Private Sub Form_KeyUp(ByVal sender As Object, ByVal e As System.Windows.Forms.KeyEventArgs) Handles MyBase.KeyUp
        Try
            If boolManualControl And Not boolHeroSelected Then
                If (boolManualControl And (boolBotConnected Or (tmiMoveInfo.tomTypeOfMove = TypeOfMove.TURN) Or (tmiMoveInfo.tomTypeOfMove = TypeOfMove.FWD))) Then
                    If Not boolEnableRBPF Then
                        tbpBotPose = ftnBotStop(tbpBotPose, tmiMoveInfo.tbpTargetPose)
                        tmiMoveInfo.tomTypeOfMove = TypeOfMove.NONE
                    End If
                End If
                subWait(500)
                boolManualControl = False
                boolCtrlKey = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in Form_KeyUp :  " & ex.ToString)
        End Try
    End Sub


    '==================================== Tool Strip  =============================================

    Private Sub picBot_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles picBot.Click
        boolPlaceBot = Not boolPlaceBot
        If boolPlaceBot Then boolPlaceDestination = False
    End Sub

    Private Sub picDestination_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles picDestination.Click
        Try
            boolPlaceDestination = Not boolPlaceDestination
            If boolPlaceDestination Then boolPlaceBot = False
            If boolPlaceDestination Then boolStop = False
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in picDestination_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub picStop_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles picStop.DoubleClick
        Try
            boolStop = Not boolStop
            If boolStop Then
                Me.picStop.BorderStyle = FormBorderStyle.Fixed3D
                tbpBotPose = ftnBotStop(tbpBotPose, tmiMoveInfo.tbpTargetPose)
                boolStop = True
                boolDock = False
                boolDrivingToDestination = False
                boolLocalizationInProgress = False
                strStatusDriving = "STOP mode"
                frmDebug.subWriteLogs(Now() & "    >>>>  STOP button was pushed  >>> ")
                ftnSendCommand("play phrase 'By your command.'")
            Else
                Me.picStop.BorderStyle = FormBorderStyle.None
                strStatusDriving = ""
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in picStop_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub StatusDriving_Click(sender As Object, e As EventArgs) Handles StatusDriving.Click
        Try
            ftnSendCommand("battery")
            Dim byteCollisionCode1 As Byte = Val(ftnSendCommand("bumper"))
            Select Case byteCollisionCode1
                Case 4
                    subDrawBumperSwitch(tbpBotPose, 0)
                Case 6
                    subDrawBumperSwitch(tbpBotPose, -45)
                Case 5
                    subDrawBumperSwitch(tbpBotPose, 45)
                Case 3
                    subDrawBumperSwitch(tbpBotPose, 180)
                Case Else
            End Select
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in StatusDriving_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuHelpContents_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles mnuHelpContents.Click
        Process.Start("GotABot User Manual.pdf")
    End Sub

    Private Sub HelpToolStripButton_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles HelpToolStripButton.Click
        If NativeMethods.GetAsyncKeyState(Keys.RShiftKey) Then   ' Test Code
            Dim sngmax As Single = 0
            Dim sngmin As Single = 0
            tmiMoveInfo.timeMoveCommandSent = Now()
            For i = 1 To 1000000
                Dim sngResult As Single = ftnGenerateGaussian()
                If (sngResult > sngmax) Then sngmax = sngResult
                If (sngResult < sngmin) Then sngmin = sngResult
            Next
            frmDebug.subWriteLogs(" ftnGenerateGaussian Time = " & ftnElapsedTimeS() / 1000000)
            frmDebug.subWriteLogs(" ftnGenerateGaussian max = " & sngmax)
            frmDebug.subWriteLogs(" ftnGenerateGaussian min = " & sngmin)
        ElseIf NativeMethods.GetAsyncKeyState(Keys.LShiftKey) Then   ' Test Code
        ElseIf NativeMethods.GetAsyncKeyState(Keys.RShiftKey) Then   ' Test Code
        Else
            Process.Start("GotABot User Manual.pdf")
        End If
    End Sub



    '==================================== WatchDog - Dock, API Disconnect  =============================================
    Dim boolHeroDisplay1 As Boolean = False
    Private Sub timerWatchDog_Tick(sender As System.Object, e As System.EventArgs) Handles timerWatchDog.Tick
        ' check if APIs are communicating
        ' check battery voltages and Dock if required
        Try
            If Not tmiMoveInfo.boolBotIsMoving And Not boolLocalizationInProgress And Not boolShutDown And Not boolDock Then
                timerWatchDog.Enabled = False
                subCheckForAPITimeout1()
                subCheckForAPITimeout2()
                Dim sngLaptopBatteryPercent As Single = Int(SystemInformation.PowerStatus.BatteryLifePercent * 100)
                ftnSendCommand("battery")  'Recheck
                If boolHeroSelected And boolHeroDisplay1 And Not tmiMoveInfo.boolBotIsMoving Then
                    ftnHero_Communicate("GD08374F051D88", ">", 1000) ' Write "-Hero-" to robot's display
                    boolHeroDisplay1 = False
                ElseIf boolHeroSelected And Not tmiMoveInfo.boolBotIsMoving Then
                    ftnHero_Communicate("GD085E1D707D88", ">", 1000) ' Write "-Gota-" to robot's display
                    boolHeroDisplay1 = True
                End If
                If (strStatusBotVoltage = "   ??   ") Or CBool(InStr(strStatusBotVoltage, "--")) Then sngBotBatteryVoltage = sngDockTriggerBot
                If boolDock And ((sngBotBatteryVoltage > sngDockTriggerBot * 1.05) And ((sngLaptopBatteryPercent > intDockTriggerPC * 1.1) Or (SystemInformation.PowerStatus.BatteryChargeStatus = BatteryChargeStatus.Charging))) Then
                    boolDock = False
                ElseIf Not boolDock And ((sngBotBatteryVoltage < sngDockTriggerBot) Or ((sngLaptopBatteryPercent < intDockTriggerPC) And Not (SystemInformation.PowerStatus.BatteryChargeStatus = BatteryChargeStatus.Charging))) Then
                    If Not tmiMoveInfo.boolBotIsMoving And ((sngBotBatteryVoltage < sngDockTriggerBot) Or
                           (sngLaptopBatteryPercent < intDockTriggerPC)) And Not (SystemInformation.PowerStatus.BatteryChargeStatus = BatteryChargeStatus.Charging) And Not boolSimulatedMovement Then
                        subWait(1000)
                        ftnSendCommand("battery")  'Recheck
                        If (strStatusBotVoltage = "   ??   ") Then sngBotBatteryVoltage = sngDockTriggerBot
                        If boolBotConnected And Not tmiMoveInfo.boolBotIsMoving And ((sngBotBatteryVoltage < sngDockTriggerBot) Or (sngLaptopBatteryPercent < intDockTriggerPC)) Then
                            subWait(1000)
                            ftnSendCommand("battery")  'Recheck
                            If (strStatusBotVoltage = "   ??   ") Then sngBotBatteryVoltage = sngDockTriggerBot
                            If Not boolDock And ((sngBotBatteryVoltage < sngDockTriggerBot) Or (sngLaptopBatteryPercent < intDockTriggerPC)) Then
                                mnuDock.Checked = True
                                boolStop = False
                                boolPause = False
                                Dim threadDock As New Thread(AddressOf frmDock.subDock)
                                threadDock.Start()
                            End If
                        End If
                    End If
                End If
                If mnuER1Direct.Checked And Not boolBotConnected Then ftnER1_Config()
                If boolURGConnected Then
                    Dim availableSerialPorts As System.Collections.ObjectModel.ReadOnlyCollection(Of String) = My.Computer.Ports.SerialPortNames
                    If Not availableSerialPorts.Contains(comURG.PortName) Then boolURGConnected = False
                    If boolURGConnected Then classScanner.subUpdateScannerStatusLabel("URG Connected,  " & comURG.PortName.ToString, Color.Black)
                    'If Not boolUrgAccessed Then
                    '    subUrgLaserOff()
                    '    classScanner.subUpdateScannerStatusLabel("URG Connected, Laser OFF " & comURG.PortName.ToString, Color.Black)
                    'Else
                    '    classScanner.subUpdateScannerStatusLabel("URG Connected, Laser ON " & comURG.PortName.ToString, Color.Black)
                    'End If
                ElseIf boolKinectConnected Then
                    classScanner.subUpdateScannerStatusLabel("Kinect Connected", Color.Black)
                ElseIf boolHeroSelected Then
                    classScanner.subUpdateScannerStatusLabel("Hero Selected", Color.Black)
                Else
                    classScanner.subUpdateScannerStatusLabel("No Scanner", Color.Red)
                End If
                If frmArm.boolEnableArm And Not frmArm.boolArmConnected Then frmArm.ftnArmLoad()
                If (ftnElapsedTimeS() > 90) Then ftnSendCommand("set power stopped " & 0)
                timerWatchDog.Interval = 60000
                If Not boolLocalizationInProgress And Not boolDrivingToDestination And Not boolSlam Then GC.Collect()
                timerWatchDog.Enabled = True
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in timerWatchDog_Tick: :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuLandmarkSetup_Click(sender As Object, e As EventArgs) Handles mnuLandmarkSetup.Click
        On Error Resume Next
        frmLandmarkConfiguration.frmVideoConfigure_Load()
        frmLandmarkConfiguration.Focus()
    End Sub
End Class

Class NativeMethods
    Declare Function GetAsyncKeyState Lib "user32" (ByVal vKey As Integer) As Short
End Class



