Imports System.Math
Imports System.Drawing


Public Module modLocalizeOdometry
    ' This module is responsible for robot localization through odometry data.


    Public Function ftnLocalizeUsingOdometry(ByVal tbpEstimatedPose As typeBotPose) As typeBotPose
        ftnLocalizeUsingOdometry = tbpEstimatedPose
        Try  '
            If Not boolSimulatedMovement Then
                Dim timeMoveCommandSent As DateTime = tmiMoveInfo.timeMoveCommandSent
                If Not tmiMoveInfo.boolMoveCommandInProgress Then
                    Dim strRecv As String = ftnSendCommand("pose")
                    Dim strParsedText As String() = strRecv.Split(" "c)
                    If (UBound(strParsedText) = 3) And CBool(InStr(strRecv, "OK")) Then
                        ftnLocalizeUsingOdometry = ftnLocalizeUsingDeadReckoning(tbpEstimatedPose, 0, True) ' calculate uncertainties
                        ftnLocalizeUsingOdometry.Location.X = Round(Val(strParsedText(1)))
                        ftnLocalizeUsingOdometry.Location.Y = Round(Val(strParsedText(2)))
                        ftnLocalizeUsingOdometry.Orientation = Round(Val(strParsedText(3)))
                    Else  ' Error 
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " ============  ftnLocalizeUsingOdometry: ERROR ==========  " & strRecv)
                        frmDebug.subWriteShortPose(ftnElapsedTimeS() & " ftnLocalizeUsingOdometry: ERROR ftnLocalizeUsingOdometry =   ", tbpEstimatedPose)
                        ftnLocalizeUsingOdometry = tbpEstimatedPose  '
                    End If
                End If
                If tmiMoveInfo.boolMoveCommandInProgress Or (timeMoveCommandSent <> tmiMoveInfo.timeMoveCommandSent) Then
                    ftnLocalizeUsingOdometry = ftnLocalizeUsingDeadReckoning(tbpStartingPosePlanned, 0, False)
                    If boolDebugOdometry Or boolDebugCC Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnLocalizeUsingOdometry:   boolMoveCommandInProgress")
                End If
            End If
            If boolDebugOdometry Then _
                frmDebug.subWritePose(ftnElapsedTimeS() & "ftnLocalizeUsingOdometry:     Pose Derived From Odometry Data ", ftnLocalizeUsingOdometry)
        Catch ex As Exception
            ftnLocalizeUsingOdometry = tbpEstimatedPose
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocalizeUsingOdometry :  " & ex.ToString)
        End Try
    End Function




End Module
