Imports System.Math

Imports System.Drawing
Imports System.Windows.Forms
Imports System.Collections
Imports System.Threading


Public Class frmDebug




    Private Sub OK_Button_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles OK_Button.Click
        On Error Resume Next
        Me.Visible = False
        boolViewDebug = False
        Me.Close()
    End Sub

    Private Sub Exit_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Me.FormClosing
        On Error Resume Next
        Me.Visible = False
        boolViewDebug = False
    End Sub

    Private Sub Got_Resize(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Me.Paint
        Try
            rrtxtDebugLog.Width = Me.Width - 55
            rrtxtDebugLog.Height = Me.Height - 222
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in Got_Resize :  " & ex.ToString)
        End Try
    End Sub







    Private Sub butSend_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles butSend.Click
        Try
            Me.subWriteLogs(Now() & "****** Debug Window User Generated Command :   " & txtCommand.Text)
            Dim strCommandText As String = txtCommand.Text & " 0 0 0 0 0 0"
            Dim strParsedText As String() = strCommandText.Split(" "c)
            Dim tbpEstimatedPose As typeBotPose = tbpBotPose
            If CBool(InStr(txtCommand.Text, "move")) And CBool(InStr(txtCommand.Text, " x ")) Then
                tomTypeOfMovePlanned = TypeOfMove.CURVE
                Dim tbpTargetPose As typeBotPose
                tbpTargetPose.Location.X = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, Val(strParsedText(2))).X
                tbpTargetPose.Location.Y = ftnLocationFromEstimatedPose(tbpEstimatedPose, 90, Val(strParsedText(4))).Y
                tbpTargetPose.Orientation = ftnConvertTo360(tbpEstimatedPose.Orientation + Atan(Val(strParsedText(4)) / Val(strParsedText(2))))
                boolStop = False
                tbpBotPose = ftnExecuteMove(tbpBotPose, txtCommand.Text, tbpTargetPose)
            ElseIf CBool(InStr(txtCommand.Text, "move")) And CBool(InStr(txtCommand.Text, "curveto")) Then
                tomTypeOfMovePlanned = TypeOfMove.CURVE
                Dim tbpTargetPose As typeBotPose
                tbpTargetPose.Location.X = Val(strParsedText(2))
                tbpTargetPose.Location.Y = Val(strParsedText(3))
                tbpTargetPose.Orientation = ftnConvertTo360(tbpEstimatedPose.Orientation + Atan(Val(strParsedText(4)) / Val(strParsedText(2))))
                boolStop = False
                tbpBotPose = ftnExecuteMove(tbpBotPose, txtCommand.Text, tbpTargetPose)
            ElseIf CBool(InStr(txtCommand.Text, "move")) Then
                Dim tbpTargetPose As typeBotPose = tbpBotPose
                If CBool(InStr(txtCommand.Text, "degrees")) Then
                    tomTypeOfMovePlanned = TypeOfMove.TURN
                    tbpTargetPose.Orientation = ftnConvertTo360(tbpEstimatedPose.Orientation + Val(strParsedText(1))) '
                Else
                    tomTypeOfMovePlanned = TypeOfMove.FWD
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, Val(strParsedText(1))) '
                End If
                boolStop = False
                tbpBotPose = ftnExecuteMove(tbpBotPose, txtCommand.Text, tbpTargetPose)
            ElseIf CBool(InStr(txtCommand.Text, "dock")) Then
                boolStop = False
                frmDock.subDock()
            ElseIf CBool(InStr(txtCommand.Text, "stop")) Then
                boolStop = False
                ftnSendCommand("stop")
            Else
                ftnSendCommand(txtCommand.Text)
            End If
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in butSend_Click :  " & ex.ToString)
        End Try
    End Sub



    '====================================  Write Log Files  ===============================================================

    Public Sub subWriteLogs(ByRef strtext As String)
        Try
            If boolLogFileEnable Then queueLogEntries.Enqueue(strtext)
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in subWriteLogs :  " & ex.ToString)
            'boolQueueBusy = False
        End Try
    End Sub


    Dim strtext As String = ""
    Private Sub timerDebug_Tick(sender As Object, e As EventArgs) Handles timerDebug.Tick
        Try
            timerDebug.Enabled = False
            If boolLogFileEnable And (queueLogEntries.Count <> 0) Then 'And Not tmiMoveInfo.boolBotIsMoving Then
                strtext  = ""
                While (queueLogEntries.Count <> 0)
                    strtext &= queueLogEntries.Dequeue().ToString & vbCrLf
                End While
                My.Computer.FileSystem.WriteAllText(My.Application.Info.DirectoryPath & "\GotABot_Log.txt", strtext, True)
            End If
            timerDebug.Enabled = True
        Catch ex As Exception
            timerDebug.Enabled = True
            subWriteLogs(" EXCEPTION in timerDebug_Tick :  " & ex.ToString)
        End Try
    End Sub

    Dim intCourseCorrections As Integer = 0
    Public Sub subWriteStartMessage(ByRef strCommand As String, ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose)
        Try
            If boolLogFileEnable Then
                Dim strStartMessage As String = vbCrLf & vbCrLf & vbCrLf & vbCrLf & vbCrLf
                strStartMessage &= "===========  " & intDrivingSequenceNumber & " - " & Now() & "  =================================================" & vbCrLf
                If tmiMoveInfo.boolBotIsMoving Then
                    intCourseCorrections += 1
                    strStartMessage &= ftnElapsedTimeS() & "  ********** COURSE CORRECTION " & intCourseCorrections & " ************" & vbCrLf
                Else
                    intCourseCorrections = 0
                End If
                strStartMessage &= "Starting Pose = " & tbpStartingPose.Location.X & " " & tbpStartingPose.Location.Y & "  +/- " & tbpStartingPose.LocationUncertainty.X & " " & tbpStartingPose.LocationUncertainty.Y &
                             "      at " & Int(tbpStartingPose.Orientation) & "  +/- " & Int(tbpStartingPose.OrientationUncertainty) & " degrees" & vbCrLf
                strStartMessage &= "Driving Command =  " & strCommand & vbCrLf
                strStartMessage &= "Target = " & tbpTargetPose.Location.X & ", " & tbpTargetPose.Location.Y & " at " & tbpTargetPose.Orientation & " degrees." & vbCrLf & vbCrLf
                subWriteLogs(strStartMessage)
            End If
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in subWriteStartMessage :  " & ex.ToString)
        End Try
    End Sub



    Public Sub subWriteFinishedMessage()
        Try
            Dim strEndMessage As String = vbCrLf & vbCrLf
            strEndMessage &= "  ===============================================================================================  " & vbCrLf & vbCrLf & vbCrLf
            strEndMessage &= Now() & "  : " & intDrivingSequenceNumber & "    -----------------------------  Finished Driving.  ------------------------- " & vbCrLf
            strEndMessage &= "                          Elapsed Time = " & Now().Subtract(timeDrivingStarted).ToString & vbCrLf & vbCrLf & vbCrLf
            strEndMessage &= "  ===============================================================================================  " & vbCrLf
            subWriteLogs(strEndMessage)
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in subWriteFinishedMessage :  " & ex.ToString)
        End Try
    End Sub



    Public Sub subWritePose(ByRef strIdentify As String, ByRef tbpWritePose As typeBotPose)
        Try
            Me.subWriteLogs(strIdentify & " = " & tbpWritePose.Location.X & ", " & tbpWritePose.Location.Y _
                         & " +/- " & tbpWritePose.LocationUncertainty.X & ", " & tbpWritePose.LocationUncertainty.Y _
                         & " at " & CShort(tbpWritePose.Orientation) & " +/- " & CShort(tbpWritePose.OrientationUncertainty) & " degrees")
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in subWritePose :  " & ex.ToString)
        End Try
    End Sub

    Public Sub subWriteShortPose(ByRef strIdentify As String, ByRef tbpWritePose As typeBotPose)
        Try
            Me.subWriteLogs(strIdentify & " = " & tbpWritePose.Location.X & ", " & tbpWritePose.Location.Y _
                         & " at " & CShort(tbpWritePose.Orientation) & " degrees")
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in subWriteShortPose :  " & ex.ToString)
        End Try
    End Sub

    Public Sub subWriteCoordinate(ByRef strIdentify As String, ByRef tmcWriteCoordinate As typeMapCoordinate)
        Try
            Me.subWriteLogs(strIdentify & " = " & tmcWriteCoordinate.X & ", " & tmcWriteCoordinate.Y)
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in subWriteCoordinate :  " & ex.ToString)
        End Try
    End Sub

    Public Sub subWriteVector(ByRef strIdentify As String, ByRef trvWriteVector As typeRangeVector)
        Try
            Me.subWriteLogs(strIdentify & " = " & trvWriteVector.sngRange & " at " & trvWriteVector.sngAngle & " degrees.")
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in subWriteVector :  " & ex.ToString)
        End Try
    End Sub


    '================================  Debug Log Options ================================
    Private Sub subLogMenu(sender As Object, e As EventArgs) Handles mnuLogMenu.Click
        Try
            mnuDrive.Checked = boolDebugDrive
            mnuCC.Checked = boolDebugCC
            mnuObstacle.Checked = boolDebugObstacle
            mnuER1IF.Checked = boolDebugER1IF
            mnuHero.Checked = boolDebugHero
            mnuOdometry.Checked = boolDebugOdometry
            mnuLocalize2D.Checked = boolDebugLocalize2D
            mnuDock.Checked = boolDebugDock
            mnuPath.Checked = boolDebugPath
            mnuCollision.Checked = boolDebugCollision
            mnuSLAM.Checked = boolDebugSlam
            mnuRBPF.Checked = boolDebugRBPF
            mnuDR.Checked = boolDebugDR
            mnuAPI.Checked = boolDebugAPI
            mnuKinect.Checked = boolDebugKinect
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in subLogMenu :  " & ex.ToString)
        End Try
    End Sub

    Private Sub subUpdateLogMenu()
        Try
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in subUpdateLogMenu :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuDrive_Click(sender As Object, e As EventArgs) Handles mnuDrive.Click
        Try
            boolDebugDrive = Not boolDebugDrive
            mnuDrive.Checked = boolDebugDrive
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuDrive_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuCC_Click(sender As Object, e As EventArgs) Handles mnuCC.Click
        Try
            boolDebugCC = Not boolDebugCC
            mnuCC.Checked = boolDebugCC
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuCC_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuObstacle_Click(sender As Object, e As EventArgs) Handles mnuObstacle.Click
        Try
            boolDebugObstacle = Not boolDebugObstacle
            mnuObstacle.Checked = boolDebugObstacle
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuObstacle_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuER1IF_Click(sender As Object, e As EventArgs) Handles mnuER1IF.Click
        Try
            boolDebugER1IF = Not boolDebugER1IF
            mnuER1IF.Checked = boolDebugER1IF
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuER1IF_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuHero_Click(sender As Object, e As EventArgs) Handles mnuHero.Click
        Try
            boolDebugHero = Not boolDebugHero
            mnuHero.Checked = boolDebugHero
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuHero_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuAPI_Click(sender As Object, e As EventArgs) Handles mnuAPI.Click
        Try
            boolDebugAPI = Not boolDebugAPI
            mnuAPI.Checked = boolDebugAPI
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuAPI_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuOdometry_Click(sender As Object, e As EventArgs) Handles mnuOdometry.Click
        Try
            boolDebugOdometry = Not boolDebugOdometry
            mnuOdometry.Checked = boolDebugOdometry
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuOdometry_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuLocalize2D_Click(sender As Object, e As EventArgs) Handles mnuLocalize2D.Click
        Try
            boolDebugLocalize2D = Not boolDebugLocalize2D
            mnuLocalize2D.Checked = boolDebugLocalize2D
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuLocalize2D_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuKinect_Click(sender As Object, e As EventArgs) Handles mnuKinect.Click
        Try
            boolDebugKinect = Not boolDebugKinect
            mnuKinect.Checked = boolDebugKinect
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuKinect_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuDock_Click(sender As Object, e As EventArgs) Handles mnuDock.Click
        Try
            boolDebugDock = Not boolDebugDock
            mnuDock.Checked = boolDebugDock
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuDock_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuPath_Click(sender As Object, e As EventArgs) Handles mnuPath.Click
        Try
            boolDebugPath = Not boolDebugPath
            mnuPath.Checked = boolDebugPath
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuPath_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuCollision_Click(sender As Object, e As EventArgs) Handles mnuCollision.Click
        Try
            boolDebugCollision = Not boolDebugCollision
            mnuCollision.Checked = boolDebugCollision
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuCollision_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuSLAM_Click(sender As Object, e As EventArgs) Handles mnuSLAM.Click
        Try
            boolDebugSlam = Not boolDebugSlam
            mnuSLAM.Checked = boolDebugSlam
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuSLAM_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuRBPF_Click(sender As Object, e As EventArgs) Handles mnuRBPF.Click
        Try
            boolDebugRBPF = Not boolDebugRBPF
            mnuRBPF.Checked = boolDebugRBPF
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuRBPF_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuURG_Click(sender As Object, e As EventArgs) Handles mnuURG.Click
        Try
            boolDebugURG = Not boolDebugURG
            mnuURG.Checked = boolDebugURG
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuURG_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuDR_Click(sender As Object, e As EventArgs) Handles mnuDR.Click
        Try
            boolDebugDR = Not boolDebugDR
            mnuDR.Checked = boolDebugDR
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuDR_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub mnuArm_Click(sender As Object, e As EventArgs) Handles mnuArm.Click
        Try
            boolDebugArm = Not boolDebugArm
            mnuArm.Checked = boolDebugArm
            mnuLogMenu.ShowDropDown()
        Catch ex As Exception
            subWriteLogs(" EXCEPTION in mnuArm_Click :  " & ex.ToString)
        End Try
    End Sub
End Class