Imports System.Drawing.Imaging
Imports System.Drawing
Imports System.Threading
Imports System.Windows.Forms


Public Class frmSlam
    Dim boolEnableRBPF_Old As Boolean
    Dim sngMaxTravel_Old As Single


    Delegate Sub DelegateSlamInit()
    Sub subSlamInit()
        Try
            If InvokeRequired Then
                Dim newDelegate As New DelegateSlamInit(AddressOf subSlamInit)
                BeginInvoke(newDelegate, New Object() {})
            Else
                boolSlam = True
                subRecalculateVariables()
                subConvertMapToArray(intBestParticle)
                boolEnableRBPF_Old = boolEnableRBPF
                boolEnableRBPF = True
                sngMaxTravel_Old = sngMaxTravel
                sngMaxTravel = 40
                intNumberOfParticles = 32
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subSlamInit :  " & ex.ToString)
        End Try
    End Sub

    Private Sub butAutoMap_Click(sender As System.Object, e As System.EventArgs) Handles butAutoMap.Click
        boolStop = False
        boolSlam = True
        Dim threadAutoSlamTop As New Thread(AddressOf subAutoSlamTop)
        threadAutoSlamTop.Start()
        Me.Refresh()
    End Sub

    Private Sub subSlamQuit(sender As System.Object, e As System.EventArgs) Handles Me.FormClosed
        Try
            boolSlam = False
            boolEnableRBPF = False
            sngMaxTravel = sngMaxTravel_Old
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subSlamQuit :  " & ex.ToString)
        End Try
    End Sub

    Private Sub butOK_Click(sender As System.Object, e As System.EventArgs) Handles butOK.Click
        boolSlam = False
        Me.Close()
    End Sub




    Private Sub butPirouette_Click(sender As Object, e As EventArgs) Handles butPirouette.Click
        Try
            Dim threadPirouette As New Thread(AddressOf ftnPirouette)
            threadPirouette.Start()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in butPiroette_Click :  " & ex.ToString)
        End Try
    End Sub



    Dim boolMovementInProgress As Boolean = False
    Private Sub butFWD_Click(sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles butFWD.MouseDown, butFWD.Click
        Try
            If Not boolMovementInProgress And Not boolLocalizationInProgress Then
                Dim tbpTargetPose As typeBotPose = tbpBotPose
                boolMovementInProgress = True
                boolStop = False
                frmDebug.subWriteLogs("))))))  Manual Control:  Forward")
                If (e.Button = MouseButtons.Left) Then  ' small move
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 0, 10)
                    tbpBotPose = ftnBotFwd(tbpBotPose, tbpTargetPose)
                ElseIf (e.Button = MouseButtons.Middle) Then ' 
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 0, 20)
                    tbpBotPose = ftnBotFwd(tbpBotPose, tbpTargetPose)
                ElseIf (e.Button = MouseButtons.Right) Then ' big move
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 0, 40)
                    tbpBotPose = ftnBotFwd(tbpBotPose, tbpTargetPose)
                End If
                subWait(100)
                boolMovementInProgress = False
                Dim threadLocalizeTop As New Thread(AddressOf subLocalizeTop)
                threadLocalizeTop.Start()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in butFWD_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub butBACK_Click(sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles butBACK.MouseDown, butBACK.Click
        Try
            If Not boolMovementInProgress And Not boolLocalizationInProgress Then
                Dim tbpTargetPose As typeBotPose = tbpBotPose
                boolMovementInProgress = True
                boolStop = False
                frmDebug.subWriteLogs("))))))  Manual Control:  Back")
                If (e.Button = MouseButtons.Left) Then  ' small move
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 180, 10)
                    tbpBotPose = ftnBotFwd(tbpBotPose, tbpTargetPose)
                ElseIf (e.Button = MouseButtons.Middle) Then ' 
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 180, 20)
                    tbpBotPose = ftnBotFwd(tbpBotPose, tbpTargetPose)
                ElseIf (e.Button = MouseButtons.Right) Then ' big move
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 180, 40)
                    tbpBotPose = ftnBotFwd(tbpBotPose, tbpTargetPose)
                End If
                subWait(100)
                boolMovementInProgress = False
                Dim threadLocalizeTop As New Thread(AddressOf subLocalizeTop)
                threadLocalizeTop.Start()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in butBACK_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub butLEFT_Click(sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles butLEFT.MouseDown
        Try
            If Not boolMovementInProgress And Not boolLocalizationInProgress Then
                Dim tbpTargetPose As typeBotPose = tbpBotPose
                boolMovementInProgress = True
                boolStop = False
                frmDebug.subWriteLogs("))))))  Manual Control:  Left")
                If (e.Button = MouseButtons.Left) Then  ' small move
                    tbpTargetPose = tbpBotPose
                    tbpTargetPose.Orientation = ftnConvertTo360(tbpTargetPose.Orientation + 15)
                    tbpBotPose = ftnBotTurn(tbpBotPose, False, tbpTargetPose)
                    subWait(100)
                ElseIf (e.Button = MouseButtons.Middle) Then  ' 
                    tbpTargetPose = tbpBotPose
                    tbpTargetPose.Orientation = ftnConvertTo360(tbpTargetPose.Orientation + 45)
                    tbpBotPose = ftnBotTurn(tbpBotPose, False, tbpTargetPose)
                    subWait(100)
                ElseIf (e.Button = MouseButtons.Right) Then  ' big move
                    tbpTargetPose = tbpBotPose
                    tbpTargetPose.Orientation = ftnConvertTo360(tbpTargetPose.Orientation + 90)
                    tbpBotPose = ftnBotTurn(tbpBotPose, False, tbpTargetPose)
                    subWait(100)
                End If
                boolMovementInProgress = False
                Dim threadLocalizeTop As New Thread(AddressOf subLocalizeTop)
                threadLocalizeTop.Start()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in butLEFT_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub butRight_Click(sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles butRight.MouseDown
        Try
            If Not boolMovementInProgress And Not boolLocalizationInProgress Then
                Dim tbpTargetPose As typeBotPose = tbpBotPose
                boolMovementInProgress = True
                boolStop = False
                frmDebug.subWriteLogs("))))))  Manual Control:  Right")
                If (e.Button = MouseButtons.Left) Then  ' small move
                    tbpTargetPose = tbpBotPose
                    tbpTargetPose.Orientation = ftnConvertTo360(tbpTargetPose.Orientation - 15)
                    tbpBotPose = ftnBotTurn(tbpBotPose, False, tbpTargetPose)
                    subWait(100)
                ElseIf (e.Button = MouseButtons.Middle) Then  ' 
                    tbpTargetPose = tbpBotPose
                    tbpTargetPose.Orientation = ftnConvertTo360(tbpTargetPose.Orientation - 45)
                    tbpBotPose = ftnBotTurn(tbpBotPose, False, tbpTargetPose)
                    subWait(100)
                ElseIf (e.Button = MouseButtons.Right) Then  ' big move
                    tbpTargetPose = tbpBotPose
                    tbpTargetPose.Orientation = ftnConvertTo360(tbpTargetPose.Orientation - 90)
                    tbpBotPose = ftnBotTurn(tbpBotPose, False, tbpTargetPose)
                    subWait(100)
                End If
                boolMovementInProgress = False
                Dim threadLocalizeTop As New Thread(AddressOf subLocalizeTop)
                threadLocalizeTop.Start()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in butRight_Click :  " & ex.ToString)
        End Try
    End Sub


    Private Sub butLocalize_Click(sender As Object, e As EventArgs) Handles butLocalize.Click
        Try
            If Not boolLocalizationInProgress Then
                frmDebug.subWriteLogs(ftnElapsedTimeS() & "                  Starting Localization ")
                Dim threadLocalizeTop As New Thread(AddressOf subLocalizeTop)
                threadLocalizeTop.Start()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in butLocalize_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub updnMapSelect_ValueChanged(sender As Object, e As EventArgs) Handles updnMapSelect.ValueChanged
        Try
            If updnMapSelect.Focused Then
                subConvertOccupencyOddsToMap(updnMapSelect.Value)  ' Mar2018
                'If (frmGotABotTop.myMap.Image IsNot Nothing) Then frmGotABotTop.myMap.Image.Dispose()
                'frmGotABotTop.myMap.Image = bmpMap.Clone
                'frmGotABotTop.myMap.Refresh()
                subRecalculateVariables()
                subConvertMapToArray(updnMapSelect.Value)
                If boolDrivingToDestination Then subGeneratePathArray()
                tbpBotPose = tbpParticlePose(updnMapSelect.Value)
                subDrawMapPixel(tbpParticlePose(updnMapSelect.Value).Location.X, tbpParticlePose(updnMapSelect.Value).Location.Y, Color.Red)
            End If
            txtParticlePose.Text = tbpParticlePose(updnMapSelect.Value).Location.X & ", " & tbpParticlePose(updnMapSelect.Value).Location.Y & " at " & tbpParticlePose(updnMapSelect.Value).Orientation
            txtError.Text = sngRMS_Error(updnMapSelect.Value).ToString & ",  " & sngRMS_ErrorAvg(updnMapSelect.Value).ToString
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in updnMapSelect_ValueChanged :  " & ex.ToString)
        End Try
    End Sub
End Class