Imports System.Math


Partial Public Class frmArm

    Function ftnPositionToDegree(ByVal intPosition As Short) As Single
        On Error Resume Next
        ftnPositionToDegree = ftnConvertTo180(intPosition * 300.0 / 1024 - 150)  ' 
    End Function

    Function ftnDegreeToPosition(ByVal sngDegree As Single) As Short
        On Error Resume Next
        ftnDegreeToPosition = Round((ftnConvertTo180(sngDegree) + 150) / 300.0 * 1024)   ' 
    End Function

    Function ftnJointAngleVerify(ByVal sngAngle As Single) As Single
        Try
            sngAngle = ftnConvertTo180(sngAngle)
            sngAngle = Max(sngAngle, -constMaxJointAngle)
            ftnJointAngleVerify = Min(sngAngle, constMaxJointAngle)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnJointAngleVerify :  " & ex.ToString)
        End Try
    End Function

    '================================================ Point Calculations  ===========================================================
    ' Points are in mm

    Public Function ftnPointFromPoint(ByVal tpcOrigin As typePointCoordinate, ByVal sngDistance_mm As Single, ByVal sngAngleYZ As Single, ByVal sngAngleYX As Single) As typePointCoordinate
        Try '
            Dim sngDistanceY As Single  = (sngDistance_mm * Cos(ftnDegreeToRadian(sngAngleYZ)) * Cos(ftnDegreeToRadian(sngAngleYX)))
            ftnPointFromPoint.Y = tpcOrigin.Y + sngDistanceY
            ftnPointFromPoint.X = tpcOrigin.X + sngDistance_mm * Sin(ftnDegreeToRadian(sngAngleYX))
            ftnPointFromPoint.Z = tpcOrigin.Z + sngDistance_mm * Sin(ftnDegreeToRadian(sngAngleYZ))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnPointFromPoint :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnDistanceBetweenPoints_mm(ByVal tpcCoordinate1 As typePointCoordinate, ByVal tpcCoordinate2 As typePointCoordinate) As Single
        Try
            ftnDistanceBetweenPoints_mm = Sqrt((tpcCoordinate2.X - tpcCoordinate1.X) ^ 2 + (tpcCoordinate2.Y - tpcCoordinate1.Y) ^ 2 + (tpcCoordinate2.Z - tpcCoordinate1.Z) ^ 2)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnDistanceBetweenPoints_mm :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnPointsAreSame(ByVal tpcCoordinate1 As typePointCoordinate, ByVal tpcCoordinate2 As typePointCoordinate) As Boolean
        On Error Resume Next
        ftnPointsAreSame = (tpcCoordinate2.X = tpcCoordinate1.X) And (tpcCoordinate2.Y = tpcCoordinate1.Y) And (tpcCoordinate2.Z = tpcCoordinate1.Z)
    End Function

    Function ftnConvertPointToMapCoordinate(ByRef tbpEstimatedPose As typeBotPose, ByRef tpcObjectCoordinate As typePointCoordinate) As typeMapCoordinate ' 
        Try
            Dim sngDistanceToObject As Single = ftnCmToInches(Sqrt(tpcObjectCoordinate.X ^ 2 + tpcObjectCoordinate.Y ^ 2) / 10)
            Dim sngAngleYX As Single
            If (tpcObjectCoordinate.Y = 0) Then
                sngAngleYX = 90
            Else
                sngAngleYX = ftnRadianToDegree(Atan(tpcObjectCoordinate.X / tpcObjectCoordinate.Y))
            End If
            ftnConvertPointToMapCoordinate = ftnLocationFromEstimatedPose(tbpEstimatedPose, -sngAngleYX, sngDistanceToObject)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnConvertPointToMapCoordinate :  " & ex.ToString)
        End Try
    End Function

    Function ftnConvertLocationToPointCoordinate(ByRef tbpEstimatedPose As typeBotPose, ByRef tmcObjectLocation As typeMapCoordinate, ByRef intObjectHeight_mm As Integer) As typePointCoordinate ' assumes Bot pointed at object
        Try
            ftnConvertLocationToPointCoordinate.X = 0
            ftnConvertLocationToPointCoordinate.Y = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tmcObjectLocation) * 25.4
            ftnConvertLocationToPointCoordinate.Z = intObjectHeight_mm   ' 
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnConvertLocationToPointCoordinate :  " & ex.ToString)
        End Try
    End Function

    Function ftnConvertToTPC(ByVal intX As Integer, ByVal intY As Integer, ByVal intZ As Integer) As typePointCoordinate
        Try
            ftnConvertToTPC.X = intX
            ftnConvertToTPC.Y = intY
            ftnConvertToTPC.Z = intZ
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnConvertToTPC :  " & ex.ToString)
        End Try
    End Function

End Class
