' scan 360 degrees and look for landmarks in order to localize
Imports System.Math
Imports System.Drawing
Imports System.Windows.Forms


Module modLocalizeLost

    Public Function ftnLostRecoveryTop(ByVal tbpEstimatedPose As typeBotPose) As typeBotPose
        Try
            ftnLostRecoveryTop = tbpEstimatedPose
            If ftnIAmLost(tbpEstimatedPose) And Not boolStop And Not boolShutDown Then
                If boolHeroSelected Then
                    ftnLostRecoveryTop.OrientationUncertainty = Max(35, ftnLostRecoveryTop.OrientationUncertainty + 10)
                    ftnLostRecoveryTop.LocationUncertainty.X += 10
                    ftnLostRecoveryTop.LocationUncertainty.Y += 10
                    ftnLostRecoveryTop = ftnGetUnStuck(ftnLostRecoveryTop, TypeOfMove.FWD)
                    Dim boolGoodLocalization As Boolean = False
                    ftnLostRecoveryTop = ftnLocalize_Top(tbpEstimatedPose, intBestParticle, boolGoodLocalization, (sngCcDelayS < 0.35))
                Else
                    ftnLostRecoveryTop.OrientationUncertainty = 60
                    ftnLostRecoveryTop.LocationUncertainty.X = 40
                    ftnLostRecoveryTop.LocationUncertainty.Y = ftnLostRecoveryTop.LocationUncertainty.X
                    ftnLostRecoveryTop = ftnLostRecovery(ftnLostRecoveryTop)
                    If ftnIAmLost(ftnLostRecoveryTop) And Not boolStop And Not boolShutDown Then
                        subWait(1500)
                        ftnLostRecoveryTop.OrientationUncertainty = 140
                        ftnLostRecoveryTop.LocationUncertainty.X = 65
                        ftnLostRecoveryTop.LocationUncertainty.Y = ftnLostRecoveryTop.LocationUncertainty.X
                        ftnLostRecoveryTop = ftnLostRecovery(ftnLostRecoveryTop)
                    End If
                    If ftnIAmLost(ftnLostRecoveryTop) And Not boolStop And Not boolShutDown Then
                        subWait(1500)
                        ftnLostRecoveryTop.OrientationUncertainty = 170
                        ftnLostRecoveryTop.LocationUncertainty.X = 120
                        ftnLostRecoveryTop.LocationUncertainty.Y = ftnLostRecoveryTop.LocationUncertainty.X
                        ftnLostRecoveryTop = ftnLostRecovery(ftnLostRecoveryTop)
                    End If
                    If ftnIAmLost(ftnLostRecoveryTop) And Not boolStop And Not boolShutDown Then
                        frmDebug.subWriteLogs(intDrivingSequenceNumber & " : HELP!  I'm lost - ")
                        strStatusDriving = intDrivingSequenceNumber & " : HELP!  I'm lost -"
                        If Not boolStop Then ftnSendCommand("play phrase 'I give up'")
                    ElseIf Not boolStop And Not boolShutDown Then
                        ftnSendCommand("play phraseA 'Ok. I know where I am.'")
                    End If
                End If

            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLostRecoveryTop :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnIAmLost(ByVal tbpEstimatedPose As typeBotPose) As Boolean
        ftnIAmLost = False
        If Not boolSlam Then
            If ((ftnPoseUncertaintyFactor(tbpEstimatedPose) > 1200) _
            Or (tbpEstimatedPose.OrientationUncertainty > 40) _
            Or (((tbpEstimatedPose.LocationUncertainty.X + tbpEstimatedPose.LocationUncertainty.Y)) > 50)) Then
                ftnIAmLost = True
            Else
                ftnIAmLost = False
            End If
        End If
    End Function


    Private Function ftnLostRecovery(ByVal tbpEstimatedPose As typeBotPose) As typeBotPose   'Rotate 360 degrees in steps and attempt to localize
        ftnLostRecovery = tbpEstimatedPose
        Try
            ftnLostRecovery = tbpEstimatedPose
            If Not boolStop Then
                Dim intMaxLostTurns As Short
                Dim intTurnAngle As Short
                Dim tbpTargetPose As typeBotPose
                If ftnScannerConnected() Then
                    intMaxLostTurns = 360 / sngScanFOV
                    intTurnAngle = sngScanFOV
                Else
                    intMaxLostTurns = 360 / sngCameraFOV
                    intTurnAngle = sngCameraFOV
                End If
                ftnSendCommand("play phraseA 'i am lost'")
                frmDebug.subWriteLogs("@@@@@@@@@@@@@@@@ I'm lost, Attempting recovery........@@@@@@@@@@@@@@@@@@ ")
                ftnLostRecovery = ftnGetUnStuck(ftnLostRecovery, TypeOfMove.FWD)
                ftnLostRecovery.LocationUncertainty = tbpEstimatedPose.LocationUncertainty
                ftnLostRecovery.OrientationUncertainty = tbpEstimatedPose.OrientationUncertainty
                ftnLostRecovery = ftnLocalize_Top(ftnLostRecovery, intBestParticle, False, (sngCcDelayS < 0.35))
                Dim intTurn As Short = 0
                Do While (ftnIAmLost(ftnLostRecovery) And (intTurn < intMaxLostTurns) And Not boolStop _
                          And (boolDrivingToDestination Or boolDock))
                    subCheckForPause()
                    tbpTargetPose = ftnLostRecovery
                    tbpTargetPose.Orientation = ftnConvertTo360(ftnLostRecovery.Orientation + intTurnAngle)
                    ftnLostRecovery = ftnBotTurn(ftnLostRecovery, False, tbpTargetPose)
                    tbpBotPose = ftnLostRecovery
                    subWait(500)
                    ftnLostRecovery.LocationUncertainty = tbpEstimatedPose.LocationUncertainty
                    ftnLostRecovery.OrientationUncertainty = tbpEstimatedPose.OrientationUncertainty
                    tbpBotPose = ftnLostRecovery
                    ftnLostRecovery = ftnLocalize_Top(ftnLostRecovery, intBestParticle, False, (sngCcDelayS < 0.35))
                    tbpBotPose = ftnLostRecovery
                    ftnLostRecovery = ftnRefineLocalization(ftnLostRecovery)
                    tbpBotPose = ftnLostRecovery
                    intTurn += 1
                Loop
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLostRecovery :  " & ex.ToString)
        End Try
    End Function

    Function ftnRefineLocalization(ByVal tbpEstimatedPose As typeBotPose) As typeBotPose
        Try
            ftnRefineLocalization = tbpEstimatedPose
            If ftnVisualRecognizerEnabled() And Not ftnScannerConnected() And Not boolStop And Not boolShutDown Then
                Dim boolGoodLocalization As Boolean = False
                For I As Integer = 1 To 3
                    If (ftnRefineLocalization.OrientationUncertainty > 20) And (ftnRefineLocalization.OrientationUncertainty < 55) And Not boolStop Then
                        ftnRefineLocalization = ftnLocalize_Top(ftnRefineLocalization, intBestParticle, boolGoodLocalization, (sngCcDelayS < 0.35))
                    End If
                Next
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnRefineLocalization :  " & ex.ToString)
        End Try
    End Function


End Module
