
Imports System.Math
Imports System.Drawing


Module modLocalizeDeadReckoning


    Public Function ftnLocalizeUsingDeadReckoning(ByRef tbpEstimatedPose As typeBotPose, ByRef sngSensorDelay_seconds As Single, ByRef boolCalculateUncertainties As Boolean) As typeBotPose
        ' sngSensorDelay_seconds is used to calculate the pose at the time sensor readings (Kinect, IR, camera, whatever) were measured
        Try
            Dim sngDistanceTravelled, sngElapsedTime As Single
            Dim intAngleTraveled As Integer
            Dim tmiMoveInfoLocal As typeMoveInfo = ftnGetMoveInfo()
            sngElapsedTime = ftnElapsedTimeS(tmiMoveInfoLocal.timeMoveCommandSent) - sngSensorDelay_seconds  ' in Seconds
            Select Case tmiMoveInfoLocal.tomTypeOfMove
                Case TypeOfMove.TURN
                    intAngleTraveled = ftnAngleTravelled(tmiMoveInfoLocal.tbpStartingPose, tmiMoveInfoLocal.tbpTargetPose, sngElapsedTime, tmiMoveInfoLocal.boolCourseCorrection)
                    ftnLocalizeUsingDeadReckoning = ftnTurnDR(tmiMoveInfoLocal.tbpStartingPose, intAngleTraveled, boolCalculateUncertainties)
                Case TypeOfMove.FWD
                    sngDistanceTravelled = ftnDistanceTravelled(tmiMoveInfoLocal.tbpStartingPose, tmiMoveInfoLocal.tbpTargetPose, sngElapsedTime, tmiMoveInfoLocal.tomTypeOfMove, tmiMoveInfoLocal.boolCourseCorrection)
                    ftnLocalizeUsingDeadReckoning = ftnFwdDR(tmiMoveInfoLocal.tbpStartingPose, sngDistanceTravelled, boolCalculateUncertainties)
                Case TypeOfMove.CURVE
                    sngDistanceTravelled = ftnDistanceTravelled(tmiMoveInfoLocal.tbpStartingPose, tmiMoveInfoLocal.tbpTargetPose, sngElapsedTime, tmiMoveInfoLocal.tomTypeOfMove, tmiMoveInfoLocal.boolCourseCorrection)
                    ftnLocalizeUsingDeadReckoning = ftnCurve_DR(tmiMoveInfoLocal.tbpStartingPose, sngDistanceTravelled, tmiMoveInfoLocal.tbpTargetPose, boolCalculateUncertainties)
                Case TypeOfMove.HALT
                    ftnLocalizeUsingDeadReckoning = tmiMoveInfo.tbpTargetPose
                Case Else
                    ftnLocalizeUsingDeadReckoning = tbpEstimatedPose
            End Select
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocalizeUsingDeadReckoning :  " & ex.ToString)
        End Try
    End Function

 
    Public Function ftnFwdDR(ByRef tbpStartingPose As typeBotPose, ByRef sngDistanceTravelled As Single, ByRef boolCalculateUncertainties As Boolean) As typeBotPose
        Try
            ftnFwdDR = tbpStartingPose
            Dim sngOrientationRadians As Single = ftnDegreeToRadian(tbpStartingPose.Orientation)
            Dim intDeltaX As Integer = Round(sngDistanceTravelled * System.Math.Cos(sngOrientationRadians))
            Dim intDeltaY As Integer = Round(sngDistanceTravelled * System.Math.Sin(-sngOrientationRadians))
            ftnFwdDR.Location.X += intDeltaX
            ftnFwdDR.Location.Y += intDeltaY
            If boolCalculateUncertainties Then
                Dim sngUncertaintyFactor As Single = sngBotMovementAccuracy * 10
                ftnFwdDR.LocationUncertainty.X = tbpStartingPose.LocationUncertainty.X + Abs(intDeltaX / sngUncertaintyFactor)
                ftnFwdDR.LocationUncertainty.Y = tbpStartingPose.LocationUncertainty.Y + Abs(intDeltaY / sngUncertaintyFactor)
                ftnFwdDR.OrientationUncertainty = tbpStartingPose.OrientationUncertainty + Abs((sngDistanceTravelled) / (sngBotMovementAccuracy * 20))
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnFwdDR :  " & ex.ToString)
        End Try
    End Function

    Function ftnCurve_DR(ByRef tbpStartingPose As typeBotPose, ByRef sngDistanceTravelled As Single, ByRef tbpTargetPose As typeBotPose, ByRef boolCalculateUncertainties As Boolean) As typeBotPose
        ' For Direct mode Curve movement
        Try
            ftnCurve_DR = tbpStartingPose
            If tmiMoveInfo.boolMoveCommandInProgress Then
                ftnCurve_DR = tbpStartingPosePlanned
            Else
                Dim sngRadius, sngCentralAngle, sngArcLength As Single
                Dim boolClockwise As Boolean
                Dim tmoCenterOfRotation As typeMapOffset
                subDeriveCurveVariables(tbpStartingPose, tbpTargetPose, sngRadius, boolClockwise, tmoCenterOfRotation, sngCentralAngle, sngArcLength)
                If (sngDistanceTravelled > sngArcLength) Then
                    ftnCurve_DR = tbpTargetPose
                    If boolClockwise Then
                        ftnCurve_DR.Orientation = ftnConvertTo360(Round(tbpStartingPose.Orientation - sngCentralAngle))
                    Else
                        ftnCurve_DR.Orientation = ftnConvertTo360(Round(tbpStartingPose.Orientation + sngCentralAngle))
                    End If
                Else
                    ftnCurve_DR = ftnCurveMotionDR(tbpStartingPose, sngDistanceTravelled, sngRadius, boolClockwise, tmoCenterOfRotation, sngCentralAngle, sngArcLength, boolCalculateUncertainties)
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCurve_DR :  " & ex.ToString)
        End Try
    End Function

    Function ftnCurveMotionDR(ByRef tbpStartingPose As typeBotPose, ByRef sngDistanceTravelled As Single,
                              ByRef sngRadius As Single, ByRef boolClockwise As Boolean, ByRef tmoCenterOfRotation As typeMapOffset,
                              ByRef sngCentralAngle As Single, ByRef sngArcLength As Single, ByRef boolCalculateUncertainties As Boolean) As typeBotPose
        'http://www.mathopenref.com/circlecentral.html
        'http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm
        'https://www.doc.ic.ac.uk/~ajd/Robotics/RoboticsResources/lecture2.pdf
        Try
            ftnCurveMotionDR = tbpStartingPose
            Dim tmoLocation As typeMapOffset
            Dim intAngleTravelled As Integer = Round(sngDistanceTravelled / sngArcLength * sngCentralAngle)
            If boolClockwise Then
                ftnCurveMotionDR.Orientation = ftnConvertTo360(tbpStartingPose.Orientation - intAngleTravelled)
                tmoLocation = ftnLocationFromEstimatedPose(tmoCenterOfRotation, ftnCurveMotionDR.Orientation, 90, sngRadius)
            Else
                ftnCurveMotionDR.Orientation = ftnConvertTo360(tbpStartingPose.Orientation + intAngleTravelled)
                tmoLocation = ftnLocationFromEstimatedPose(tmoCenterOfRotation, ftnCurveMotionDR.Orientation, -90, sngRadius)
            End If
            ftnCurveMotionDR.Location.X = Round(tmoLocation.X)
            ftnCurveMotionDR.Location.Y = Round(tmoLocation.Y)
            If boolCalculateUncertainties Then
                Dim intLocUncertainty As Integer = Round(sngDistanceTravelled / 10)
                ftnCurveMotionDR.LocationUncertainty.X += intLocUncertainty
                ftnCurveMotionDR.LocationUncertainty.Y += intLocUncertainty
                ftnCurveMotionDR.OrientationUncertainty += Round(intAngleTravelled / 10)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCurveMotionDR :  " & ex.ToString)
        End Try
    End Function



    Function ftnTurnDR(ByRef tbpStartingPose As typeBotPose, ByRef intTurnAngle As Integer, ByRef boolCalculateUncertainties As Boolean) As typeBotPose  '
        Try
            ftnTurnDR = tbpStartingPose
            ftnTurnDR.Orientation = ftnConvertTo360(tbpStartingPose.Orientation + intTurnAngle)
            If boolCalculateUncertainties Then
                ftnTurnDR.OrientationUncertainty += Abs(ftnConvertTo180(intTurnAngle) / (sngBotMovementAccuracy * 20))
                'If boolER1Selected And (sngBotWheelSeparation < 33) Then
                '    ftnTurnDR.OrientationUncertainty += Abs(ftnConvertTo180(intTurnAngle) / (sngBotMovementAccuracy * 12))
                'Else
                '    ftnTurnDR.OrientationUncertainty += Abs(ftnConvertTo180(intTurnAngle) / (sngBotMovementAccuracy * 20))
                'End If
                ftnTurnDR.LocationUncertainty.X += 1
                ftnTurnDR.LocationUncertainty.Y += 1
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnTurnDR :  " & ex.ToString)
        End Try
    End Function


    Function ftnDistanceTravelled(ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose, ByRef sngElapsedTime As Single, ByRef tomTypeOfMove As TypeOfMove, ByRef boolCourseCorrection As Boolean) As Single
        ' d = vt + (1/2)at^2 
        ' v = a t  
        Try
            Dim sngDistanceCommanded As Single
            If (tomTypeOfMove = TypeOfMove.CURVE) Then
                sngDistanceCommanded = ftnCurveArcLength(tbpStartingPose, tbpTargetPose) * 1.1
            Else
                sngDistanceCommanded = ftnDistanceCommanded(tbpStartingPose, tbpTargetPose)
            End If
            If boolCourseCorrection Then
                ftnDistanceTravelled = ftnCmToInches(sngElapsedTime * sngBotLinearVelocity)
            ElseIf (sngElapsedTime > sngBotLinearAccelerationTime) Then
                ftnDistanceTravelled = sngBotLinearAccelerationDistance + ftnCmToInches((sngElapsedTime - sngBotLinearAccelerationTime) * sngBotLinearVelocity)
            Else ' TODO Add deacceleration distance
                ftnDistanceTravelled = ftnCmToInches(sngBotLinearAcceleration * sngElapsedTime * sngElapsedTime * 0.5)
            End If
            If (sngDistanceCommanded < 0) Then ftnDistanceTravelled = -ftnDistanceTravelled
            If (Abs(ftnDistanceTravelled) > Abs(sngDistanceCommanded)) Then ftnDistanceTravelled = sngDistanceCommanded
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnDistanceTravelled :  " & ex.ToString)
        End Try
    End Function

    Function ftnAngleTravelled(ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose, ByRef sngElapsedTime As Single, ByRef boolCourseCorrection As Boolean) As Integer
        Try
            Dim intTurnAngleCommanded As Integer = ftnAngleDifference(tbpTargetPose.Orientation, tbpStartingPose.Orientation)
            If boolSlam Then _
                If (intTurnAngleCommanded = 0) Then intTurnAngleCommanded = 360 'Pirouette
            If boolCourseCorrection Then
                ftnAngleTravelled = Round(sngElapsedTime * sngBotAngularVelocity)
            ElseIf (sngElapsedTime > sngBotAngularAccelerationTime) Then
                ftnAngleTravelled = Round(sngBotAngularAccelerationDistance + (sngElapsedTime - sngBotAngularAccelerationTime) * sngBotAngularVelocity)
            Else
                ftnAngleTravelled = Round(sngBotAngularAcceleration * sngElapsedTime * sngElapsedTime * 0.5)
            End If
            If (ftnAngleTravelled < 0) Then ftnAngleTravelled = 0
            If (ftnAngleTravelled > Abs(intTurnAngleCommanded)) Then ftnAngleTravelled = Abs(intTurnAngleCommanded)
            If (intTurnAngleCommanded < 0) Then ftnAngleTravelled = -ftnAngleTravelled
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnAngleTravelled :  " & ex.ToString)
        End Try
    End Function



End Module



