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



'
Module modSLAM
    '==============================  AUTO MAP ================================================================
    Public Sub subAutoSlamTop()
        Try
            frmDebug.subWriteLogs(Now() & "  @@@@@@@@@@@ SLAM:  Map Making Start   @@@@@@@@@@@")
            ftnSetMoveInfo(TypeOfMove.NONE, tbpBotPose, tbpBotPose, False)
            Dim boolSlamComplete As Boolean = False
            Dim tbpTargetPose As typeBotPose
            ReDim intPathArrayCopy(intMapWidth, intMapHeight)   '
            boolCollisionDetectionEnable = False
            intDrivingSequenceNumber = 0
            Dim boolGoodLocalization As Boolean = False
            strStatusDriving = " Map Initialize "
            tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, boolGoodLocalization, (sngCcDelayS < 0.35))

            frmDock.subUnDock(tbpBotPose)
            ftnPirouette(tbpBotPose)
            Do  ' Move and explore
                strStatusDriving = "SLAM Mode"
                subCheckForPause()
                ftnSetMoveInfo(TypeOfMove.NONE, tbpBotPose, tbpBotPose, False)
                tbpTargetPose = ftnNextTargetPose(tbpBotPose) ' Find Target to move to
                While ftnPirouetteRequired(tbpBotPose, tbpTargetPose) And Not boolSlamComplete And Not boolStop
                    tbpBotPose = ftnPirouette(tbpBotPose)
                    boolSlamComplete = ftnPostSlamMove(tbpBotPose)
                    tbpTargetPose = ftnNextTargetPose(tbpBotPose)
                End While
                intMovesSincePirouette += 1
                If boolStop Then boolSlamComplete = True
                If Not boolSlamComplete Then  ' Move
                    strStatusDriving = intDrivingSequenceNumber & " : SLAM  Move "
                    If ftnTargetRequiresTurn(tbpBotPose, tbpTargetPose.Location) Then
                        tbpBotPose = ftnBotTurn(tbpBotPose, True, tbpTargetPose)
                    Else
                        tbpBotPose = ftnBotFwd(tbpBotPose, tbpTargetPose)
                    End If
                    boolSlamComplete = ftnPostSlamMove(tbpBotPose)  ' Localize and check status
                End If
                tbpTargetPose = tbpBotPose
            Loop Until boolStop Or boolSlamComplete ' 

            ftnSendCommand("play phrase 'Mapping complete'")
            frmDebug.subWriteLogs(Now() & "  @@@@@@@@@@@ SLAM:  Map Making Complete   @@@@@@@@@@@")
            strStatusDriving = "  Waiting For Command "
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subAutoSlamTop :  " & ex.ToString)
        End Try
    End Sub

    Function ftnPostSlamMove(ByVal tbpEstimatedPose As typeBotPose) As Boolean
        ' returns true if a collision detected
        Try
            ftnPostSlamMove = True
            Dim boolGoodLocalization As Boolean = False
            ftnSendCommand("battery")
            boolCollisionDetectionEnable = True
            boolCollisionDetectionEnable = False
            tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, False, (sngCcDelayS < 0.35))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnPostSlamMove :  " & ex.ToString)
        End Try
    End Function

    Function ftnPirouetteRequired(ByVal tbpEstimatedPose As typeBotPose, ByVal tbpTargetPose As typeBotPose) As Boolean
        Try
            tmiMoveInfo.tomTypeOfMove = TypeOfMove.FWD
            Dim trvObstacleVector As typeRangeVector = ftnObstacleVectorTop(tbpEstimatedPose, tmiMoveInfo.tomTypeOfMove, tbpEstimatedPose, tbpTargetPose)
            trvObstacleVector.sngRange = ftnObstacleForwardDistance(trvObstacleVector)
            ftnPirouetteRequired = (ftnPosesEqual(tbpBotPose, tmiMoveInfo.tbpTargetPose) Or
                                   (trvObstacleVector.sngRange <= 15) And (intMovesSincePirouette > 3)) And Not boolStop
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnPirouetteRequired :  " & ex.ToString)
        End Try
    End Function


    Public Function ftnPirouette() As typeBotPose
        Try
            ftnPirouette = ftnPirouette(tbpBotPose)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnPirouette :  " & ex.ToString)
        End Try
    End Function

    Dim boolPirouetteBusy As Boolean = False
    Dim intMovesSincePirouette As Integer = 0
    Public Function ftnPirouette(ByVal tbpEstimatedPose As typeBotPose) As typeBotPose
        ftnPirouette = tbpEstimatedPose
        Try
            If Not boolPirouetteBusy And Not tmiMoveInfo.boolBotIsMoving And Not boolDrivingToDestination Then
                boolPirouetteBusy = True
                If boolURGConnected Then
                    tbpTargetPosePlanned = tbpEstimatedPose
                    tbpStartingPosePlanned = tbpEstimatedPose
                    intDrivingSequenceNumber += 1
                    frmDebug.subWriteStartMessage("Pirouette", tbpEstimatedPose, tbpTargetPosePlanned)
                    tomTypeOfMovePlanned = TypeOfMove.TURN
                    ftnSendCommand("move 360 degrees")
                    ftnPirouette = ftnLocalizeRBPF_Top(tbpEstimatedPose, False)
                    tbpBotPose = ftnPirouette
                    ftnSendCommand("play phrase Beep")
                    intMovesSincePirouette = 0
                Else
                    Dim boolGoodLocalization As Boolean = False
                    Dim tbpTargetPose As typeBotPose
                    Dim intTurnAngle As Short = sngScanFOV * 0.5
                    For intTurn As Short = 1 To 360 / intTurnAngle
                        If boolStop Then Exit For
                        ftnSendCommand("battery")
                        strStatusDriving = intDrivingSequenceNumber & " : SLAM  Pirouette " & intTurn
                        frmDebug.subWriteLogs(" : SLAM  Pirouette " & intTurn)
                        tbpTargetPose = ftnPirouette
                        tbpTargetPose.Orientation = ftnConvertTo360(ftnPirouette.Orientation + intTurnAngle)
                        ftnPirouette = ftnBotTurn(ftnPirouette, False, tbpTargetPose)
                        tbpBotPose = ftnPirouette
                        ftnPirouette = ftnLocalize_Top(ftnPirouette, intBestParticle, boolGoodLocalization, (sngCcDelayS < 0.35))
                        tbpBotPose = ftnPirouette
                        subCheckForPause()
                    Next intTurn
                    ftnPirouette = ftnLocalize_Top(ftnPirouette, intBestParticle, boolGoodLocalization, (sngCcDelayS < 0.35))
                    ftnSendCommand("play phrase Beep")
                    intMovesSincePirouette = 0
                End If
                boolPirouetteBusy = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnPirouette - a :  " & ex.ToString)
        End Try
    End Function



    Function ftnNextTargetPose(ByVal tbpEstimatedPose As typeBotPose) As typeBotPose
        Try
            ftnNextTargetPose = tbpEstimatedPose
            tmcDestination = ftnNextDestination(tbpEstimatedPose)
            ftnNextTargetPose = ftnFindTargetPose(tbpEstimatedPose, tmcDestination)
            frmDebug.subWriteLogs(" Target Location = " & ftnNextTargetPose.Location.X & ", " & ftnNextTargetPose.Location.Y)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnNextTargetPose :  " & ex.ToString)
        End Try
    End Function


    Dim intPathArrayCopy(intMapWidth, intMapHeight) As Integer

    '

    Function ftnNextDestination(ByVal tbpEstimatedPose As typeBotPose) As typeMapCoordinate
        Try
            ftnNextDestination = tbpEstimatedPose.Location
            Dim boolFoundNextLocation As Boolean = False
            If ftnInWall_NoChk1(tbpEstimatedPose.Location) Then
                ftnNextDestination = ftnLocationFromEstimatedPose(tbpEstimatedPose, 180, 6)
                boolFoundNextLocation = True
            Else
                subGeneratePathArray()
                intPathArrayCopy = intPathArray
            End If
            If Not boolFoundNextLocation Then ftnNextDestination = ftnGeneralForwardLocation(tbpEstimatedPose, boolFoundNextLocation)
            If Not boolFoundNextLocation Then ftnNextDestination = ftnUnexploredLocation(tbpEstimatedPose, boolFoundNextLocation)
            If Not boolFoundNextLocation Then ftnNextDestination = tbpEstimatedPose.Location
            If boolDebugSlam And boolFoundNextLocation Then frmDebug.subWriteLogs(" Next Destination = " & ftnNextDestination.X & ", " & ftnNextDestination.Y)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnNextDestination :  " & ex.ToString)
        End Try
    End Function

    Function ftnForwardLocation(ByVal tbpEstimatedPose As typeBotPose, ByRef boolFoundNextLocation As Boolean) As typeMapCoordinate
        Try
            Dim tbpTargetPose As typeBotPose = tbpEstimatedPose
            tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, 2 * sngBotRadius)
            Dim trvObstacleVector As typeRangeVector = ftnObstacleVectorTop(tbpEstimatedPose, TypeOfMove.FWD, tbpEstimatedPose, tbpTargetPose)
            trvObstacleVector.sngRange = ftnObstacleForwardDistance(trvObstacleVector)
            If (trvObstacleVector.sngRange > (3 * sngBotRadius)) Then
                ftnForwardLocation = tbpTargetPose.Location
                intPathArray = intPathArrayCopy
                boolFoundNextLocation = ftnFindPathToDestination(ftnForwardLocation, tbpEstimatedPose.Location)
            Else
                boolFoundNextLocation = False
            End If
            If Not boolFoundNextLocation Then
                ftnForwardLocation = tbpEstimatedPose.Location
                If boolDebugSlam Then frmDebug.subWriteLogs(" No Direct Forward Location found")
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnForwardLocation :  " & ex.ToString)
        End Try
    End Function

    Function ftnGeneralForwardLocation(ByVal tbpEstimatedPose As typeBotPose, ByRef boolFoundNextLocation As Boolean) As typeMapCoordinate
        ' Returns an unexplored location reachable with a Turn then a Drive Forward
        Try
            ftnGeneralForwardLocation = tbpEstimatedPose.Location
            Dim tbpTargetPose As typeBotPose = tbpEstimatedPose
            Dim tbpAfterTurnPose As typeBotPose = tbpEstimatedPose
            For intOrientation As Integer = -60 To 60 Step 10 ' Find any unexplored locations which are a short distance in front (generally)
                If (Abs(intOrientation) > 25) Then
                    For intRange = (2 * sngBotRadius) To (4 * sngBotRadius)
                        tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpEstimatedPose, intOrientation, intRange)
                        tbpTargetPose.Location = ftnCheckMapArrayLimits(tbpTargetPose.Location)
                        If ftnInWall_NoChk1(tbpTargetPose.Location) Then Exit For ' Hit a wall - try a different orientation
                        If ftnLocationUnexploredAndReachable(tbpTargetPose.Location) And (intRange > sngBotDiameter) Then
                            tbpTargetPose.Orientation = ftnConvertTo360(tbpEstimatedPose.Orientation + intOrientation)
                            tbpAfterTurnPose.Orientation = tbpTargetPose.Orientation
                            If ftnForwardUnobstructed(tbpEstimatedPose, tbpTargetPose) Then
                                boolFoundNextLocation = True
                                ftnGeneralForwardLocation = tbpTargetPose.Location
                            End If
                            Exit For
                        End If
                    Next intRange
                End If
                If boolFoundNextLocation Then Exit For
            Next intOrientation
            If boolDebugSlam And Not boolFoundNextLocation Then frmDebug.subWriteLogs(" No General Forward Location found")
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGeneralForwardLocation :  " & ex.ToString)
        End Try
    End Function

    Function ftnUnexploredLocation(ByVal tbpEstimatedPose As typeBotPose, ByRef boolFoundNextLocation As Boolean) As typeMapCoordinate
        Try
            ftnUnexploredLocation = tbpEstimatedPose.Location
            Dim tmcDestination As typeMapCoordinate
            For intX As Integer = (sngBotRadius + 10) To intMapWidth Step sngBotRadius ' Look elsewhere for unexplorered locations
                For intY As Integer = (sngBotRadius + 10) To intMapHeight Step sngBotRadius
                    If Not boolFoundNextLocation Then
                        tmcDestination = ftnConvertToTMC(tbpEstimatedPose.Location.X + intX, tbpEstimatedPose.Location.Y + intY)
                        tmcDestination = ftnCheckMapArrayLimits(tmcDestination)
                        boolFoundNextLocation = ftnLocationUnexploredAndReachable(tmcDestination)
                    End If
                    If Not boolFoundNextLocation Then
                        tmcDestination = ftnConvertToTMC(tbpEstimatedPose.Location.X - intX, tbpEstimatedPose.Location.Y + intY)
                        tmcDestination = ftnCheckMapArrayLimits(tmcDestination)
                        boolFoundNextLocation = ftnLocationUnexploredAndReachable(tmcDestination)
                    End If
                    If Not boolFoundNextLocation Then
                        tmcDestination = ftnConvertToTMC(tbpEstimatedPose.Location.X + intX, tbpEstimatedPose.Location.Y - intY)
                        tmcDestination = ftnCheckMapArrayLimits(tmcDestination)
                        boolFoundNextLocation = ftnLocationUnexploredAndReachable(tmcDestination)
                    End If
                    If Not boolFoundNextLocation Then
                        tmcDestination = ftnConvertToTMC(tbpEstimatedPose.Location.X - intX, tbpEstimatedPose.Location.Y - intY)
                        tmcDestination = ftnCheckMapArrayLimits(tmcDestination)
                        boolFoundNextLocation = ftnLocationUnexploredAndReachable(tmcDestination)
                    End If
                    If boolFoundNextLocation Then Exit For
                Next intY
                If boolFoundNextLocation Then Exit For
            Next intX
            If boolFoundNextLocation Then ftnUnexploredLocation = tmcDestination
            If boolDebugSlam And Not boolFoundNextLocation Then frmDebug.subWriteLogs(" No Unexplored Location found ")
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnUnexploredLocation :  " & ex.ToString)
        End Try
    End Function


    Function ftnLocationUnexploredAndReachable(ByRef tmcDestination As typeMapCoordinate) As Boolean
        Try
            ftnLocationUnexploredAndReachable = False
            If boolGreySpace(tmcDestination.X, tmcDestination.Y) Then  ' unexplored
                If Not ftnInWall_NoChk1(tmcDestination) Then
                    intPathArray = intPathArrayCopy
                    ftnLocationUnexploredAndReachable = ftnFindPathToDestination(tmcDestination, tbpBotPose.Location) ' reachable
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocationUnexploredAndReachable :  " & ex.ToString)
            frmDebug.subWriteLogs("                 tmcDestination = " & tmcDestination.X & ", " & tmcDestination.Y)
        End Try
    End Function

    Function ftnForwardUnobstructed(ByVal tbpEstimatedPose As typeBotPose, ByVal tbpTargetPose As typeBotPose) As Boolean
        Try
            Dim sngDistanceToTarget As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tbpTargetPose.Location)
            Dim trvMapObstacle As typeRangeVector = ftnMapObstacleVector(tbpEstimatedPose, tbpTargetPose, tbpEstimatedPose, TypeOfMove.FWD)
            ftnForwardUnobstructed = (ftnObstacleForwardDistance(trvMapObstacle) < sngDistanceToTarget)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnForwardUnobstructed :  " & ex.ToString)
        End Try
    End Function


 

End Module
