
Imports System.Math
Imports System.Threading
Imports System.EventArgs
Imports System.Drawing


Module modCurve
    ' Curve movement code


    Public Sub subDeriveCurveVariables(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpTargetPose As typeBotPose,
                                 ByRef sngRadius As Single, ByRef boolClockwise As Boolean, ByRef tmoCenterOfRotation As typeMapOffset,
                                 ByRef sngCentralAngle As Single, ByRef sngArcLength As Single)
        Try
            sngRadius = ftnCurveRadius(tbpEstimatedPose, tbpTargetPose)
            boolClockwise = ftnCurveClockwise(tbpEstimatedPose, tbpTargetPose)
            If boolClockwise Then
                tmoCenterOfRotation = ftnTmoLocationFromEstimatedPose(tbpEstimatedPose, -90, sngRadius)
            Else
                tmoCenterOfRotation = ftnTmoLocationFromEstimatedPose(tbpEstimatedPose, 90, sngRadius)
            End If
            sngCentralAngle = ftnCurveCentralAngle(tbpEstimatedPose.Location, tbpTargetPose.Location, tmoCenterOfRotation)
            sngArcLength = ftnCurveArcLength(sngRadius, sngCentralAngle)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDeriveCurveVariables :  " & ex.ToString)
        End Try
    End Sub

    Public Function ftnCurveRadius(ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As Single
        '  TODO Optimize this code
        Try
            Dim sngCenterToTarget As Single = ftnDistanceBetweenLocations(tbpStartingPose.Location, tbpTargetPose.Location)
            ftnCurveRadius = sngCenterToTarget * 0.5
            Dim boolClockwise As Boolean = ftnCurveClockwise(tbpStartingPose, tbpTargetPose)
            Dim tmoCenterOfRotation As typeMapOffset
            Dim sngMaxRadius As Single = 500 '(100)
            Dim sngRadius As Single = ftnCurveRadius
            Do
                ftnCurveRadius = sngRadius
                If (sngRadius < sngCenterToTarget) Then
                    sngRadius += Max(Abs(sngRadius - sngCenterToTarget) / 2, 0.5)
                Else
                    sngRadius -= Max(Abs(sngRadius - sngCenterToTarget) / 2, 0.5)
                End If
                If boolClockwise Then
                    tmoCenterOfRotation = ftnTmoLocationFromEstimatedPose(tbpStartingPose, -90, sngRadius)
                Else
                    tmoCenterOfRotation = ftnTmoLocationFromEstimatedPose(tbpStartingPose, 90, sngRadius)
                End If
                sngCenterToTarget = Sqrt((tmoCenterOfRotation.X - tbpTargetPose.Location.X) ^ 2 + (tmoCenterOfRotation.Y - tbpTargetPose.Location.Y) ^ 2) 'ftnDistanceBetweenLocations(tmcCenterOfRotation, tbpTargetPose.Location)
            Loop Until (sngRadius > sngMaxRadius) Or (Abs(sngRadius - sngCenterToTarget) < 1)
            ftnCurveRadius = (sngCenterToTarget + ftnCurveRadius) / 2
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCurveRadius :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnCurveCentralAngle(ByRef tmcStartLocation As typeMapCoordinate, ByRef tmcEndLocation As typeMapCoordinate, ByRef tmoCenterOfRotation As typeMapOffset) As Single
        Try
            Dim sngAngleToStart As Single = ftnP1ToP2Angle(tmoCenterOfRotation, tmcStartLocation)
            Dim sngAngleToTarget As Single = ftnP1ToP2Angle(tmoCenterOfRotation, tmcEndLocation)
            ftnCurveCentralAngle = Abs(ftnAngleDifference(sngAngleToStart, sngAngleToTarget))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCurveCentralAngle :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnCurveArcLength(ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As Single
        Try
            Dim sngRadius As Single = ftnCurveRadius(tbpStartingPose, tbpTargetPose)
            Dim tmoCenterOfRotation As typeMapOffset
            Dim boolClockwise As Boolean = ftnCurveClockwise(tbpStartingPose, tbpTargetPose)
            If boolClockwise Then
                tmoCenterOfRotation = ftnTmoLocationFromEstimatedPose(tbpStartingPose, -90, sngRadius)
            Else
                tmoCenterOfRotation = ftnTmoLocationFromEstimatedPose(tbpStartingPose, 90, sngRadius)
            End If
            Dim sngCentralAngle As Single = ftnCurveCentralAngle(tbpStartingPose.Location, tbpTargetPose.Location, tmoCenterOfRotation)
            ftnCurveArcLength = ftnCurveArcLength(sngRadius, sngCentralAngle)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCurveArcLength :  " & ex.ToString)
        End Try
    End Function


    Public Function ftnCurveArcLength(ByRef sngRadius As Single, ByRef sngCentralAngle As Single) As Single
        Try
            ftnCurveArcLength = 0.01745329 * sngRadius * sngCentralAngle
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCurveArcLength :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnCurveClockwise(ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As Boolean
        Try
            ftnCurveClockwise = (ftnTurnAngleV2(tbpStartingPose, tbpTargetPose) < 0)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCurveClockwise :  " & ex.ToString)
        End Try
    End Function


    

End Module
