Imports System.Math


Module modAdjustPose

    Public Function ftnAdjustPose(ByVal tbpEstimatedPose As typeBotPose, ByVal tbpDestination As typeBotPose, ByVal intError_i As Integer) As typeBotPose
        Try ' Fine Adjustment to reach Destination Pose ( a Turn to the desired destination, followed by a short drive forward, followed by a final Turn to the desired orientation)
            tbpEstimatedPose = ftnLocalize_Top(tbpEstimatedPose, intBestParticle, False, True)
            ftnAdjustPose = tbpEstimatedPose
            frmDebug.subWritePose(vbCrLf & ftnElapsedTimeS() & " ====================  Adjust Pose To ", tbpDestination)
            strStatusDriving = "Pose Adjustment"
            If Not ftnLocationsEqual(tbpEstimatedPose.Location, tbpDestination.Location) Then
                subCheckForPause()
                If (tbpEstimatedPose.Orientation <> tbpDestination.Orientation) Then ftnAdjustPose = ftnBotTurn(tbpEstimatedPose, False, tbpDestination)
                If Not ftnLocationsEqual(ftnAdjustPose.Location, tbpDestination.Location) Then ftnAdjustPose = ftnBotFwd(ftnAdjustPose, tbpDestination) ' Goto tbpDestination Location
                ftnAdjustPose = ftnLocalize_Top(ftnAdjustPose, intBestParticle, False, True)
            End If
            If boolDebugArm Then frmDebug.subWritePose(ftnElapsedTimeS() & " ====================  ftnDriveCloserToDestination : At destination ", ftnAdjustPose)
            If Not ftnPosesEqual(ftnAdjustPose, tbpDestination) Then ftnAdjustPose = ftnTurnToFinalOrientation(ftnAdjustPose, tbpDestination)
            strStatusDriving = "Pose Adjusted"
            frmDebug.subWritePose(vbCrLf & ftnElapsedTimeS() & " ====================  Adjust Pose COMPLETE ", ftnAdjustPose)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnAdjustPose :  " & ex.ToString)
        End Try
    End Function

    Function ftnTurnToFinalOrientation(ByVal tbpEstimatedPose As typeBotPose, ByVal tbpDestination As typeBotPose) As typeBotPose
        Try
            ftnTurnToFinalOrientation = tbpEstimatedPose
            If Not boolStop And Not boolPause And Not boolShutDown Then
                Dim intTries As Integer = 0
                While (Abs(ftnAngleDifference(tbpDestination.Orientation, ftnTurnToFinalOrientation.Orientation)) > 2) And (intTries < 3) And Not boolStop And Not boolShutDown
                    tbpDestination.Location = ftnTurnToFinalOrientation.Location
                    intTries += 1
                    frmDebug.subWritePose(" ============================= ftnTurnToFinalOrientation : tbpDestination " & intTries & " ", tbpDestination)
                    ftnTurnToFinalOrientation = ftnBotTurn(ftnTurnToFinalOrientation, False, tbpDestination)
                    ftnTurnToFinalOrientation = ftnLocalize_Top(ftnTurnToFinalOrientation, intBestParticle, True, True)
                    subCheckForPause()
                End While
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnTurnToFinalOrientation :  " & ex.ToString)
        End Try
    End Function


End Module
