
Imports System.Math
Imports System.Threading


Partial Public Class frmArm


    ' ============================ Higher Level Arm Control  ==========================================
    Delegate Function DelegateAimGripper(ByVal tpcAimPoint As typePointCoordinate) As Boolean
    Public Function ftnAimGripperAtPoint(ByVal tpcAimPoint As typePointCoordinate) As Boolean
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegateAimGripper(AddressOf ftnAimGripperAtPoint)
                Me.Invoke(newDelegate, New Object() {tpcAimPoint})
            Else
                ftnAimGripperAtPoint = False
                subDrawSmallCircleArm(tpcAimPoint, False)
                Dim taaArmAnglesTrial As typeArmAngles = ftnAnglesToAimGripperAtPoint(tpcAimPoint)
                ftnAimGripperAtPoint = ftnValidKinematics(taaArmAnglesTrial)
                If ftnAimGripperAtPoint Then subMoveArmToNewAngles(taaArmAnglesTrial, True)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnAimGripperAtPoint :  " & ex.ToString)
        End Try
    End Function

    Function ftnAnglesToAimGripperAtPoint(ByVal tpcAimPoint As typePointCoordinate) As typeArmAngles
        Try
            ftnAnglesToAimGripperAtPoint = taaArmAngles
            Dim taaArmAnglesTrial As typeArmAngles = taaArmAngles
            Dim tjcJointCoordinatesTrial As typeJointCoordinate
            For taaArmAnglesTrial.sngShoulder = 115 To -70 Step -5
                tjcJointCoordinatesTrial = ftnForwardKinematicsCoordinates(taaArmAnglesTrial)
                taaArmAnglesTrial.sngElbow = ftnConvertTo180(ftnRadianToDegree(Atan((tpcAimPoint.Z - tjcJointCoordinatesTrial.tpcElbow.Z) / Max((tpcAimPoint.Y - tjcJointCoordinatesTrial.tpcElbow.Y), 0.001))))
                tjcJointCoordinatesTrial = ftnForwardKinematicsCoordinates(taaArmAnglesTrial)
                If ftnValidKinematics(taaArmAnglesTrial) Then
                    ftnAnglesToAimGripperAtPoint.sngElbow = taaArmAnglesTrial.sngElbow
                    ftnAnglesToAimGripperAtPoint.sngShoulder = taaArmAnglesTrial.sngShoulder
                    Exit For
                End If
            Next taaArmAnglesTrial.sngShoulder
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnAnglesToAimGripperAtPoint :  " & ex.ToString)
        End Try
    End Function

    Delegate Function DelegatePlaceGripperAtPoint(ByVal tpcTarget As typePointCoordinate, ByRef boolWaitTillComplete As Boolean) As Boolean
    Public Function ftnPlaceGripperAtPoint(ByVal tpcTarget As typePointCoordinate, ByRef boolWaitTillComplete As Boolean) As Boolean
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegatePlaceGripperAtPoint(AddressOf ftnPlaceGripperAtPoint)
                Me.Invoke(newDelegate, New Object() {tpcTarget, boolWaitTillComplete})
            Else
                ftnPlaceGripperAtPoint = False
                subDrawSmallCircleArm(tpcTarget, False)
                If Not boolStop And Not boolShutDown Then
                    ' If boolDebugArm Then
                    frmDebug.subWriteLogs(" ==========================  ftnPlaceGripperAtPoint : (x,y,z) = " & tpcTarget.X & ", " & tpcTarget.Y & ", " & tpcTarget.Z)
                    Dim boolCanPlace As Boolean
                    Dim taaTrialArmAngles As typeArmAngles = ftnAnglesToPlaceGripperAtPoint(tpcTarget, boolCanPlace)

                    If ftnValidKinematics(taaTrialArmAngles) And boolCanPlace Then
                        'taaArmAngles = taaTrialArmAngles
                        subMoveArmToNewAngles(taaTrialArmAngles, boolWaitTillComplete)
                        ftnPlaceGripperAtPoint = True
                        If boolDebugArm Then
                            frmDebug.subWriteLogs(" ftnPlaceGripperAtPoint : Valid taaTrialArmAngles(sngShoulder,sngElbow) = " & taaTrialArmAngles.sngShoulder & ", " & taaTrialArmAngles.sngElbow)
                            frmDebug.subWriteLogs(" ftnPlaceGripperAtPoint :  tjcJointCoordinates.tpcCamera = " & tjcJointCoordinates.tpcCamera.X & ", " & tjcJointCoordinates.tpcCamera.Y & ", " & tjcJointCoordinates.tpcCamera.Z)
                            frmDebug.subWriteLogs(" ftnPlaceGripperAtPoint :  tjcJointCoordinates.tpcGripperEnd = " & tjcJointCoordinates.tpcGripperEnd.X & ", " & tjcJointCoordinates.tpcGripperEnd.Y & ", " & tjcJointCoordinates.tpcGripperEnd.Z)
                        End If
                    Else
                        ftnPlaceGripperAtPoint = False
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " ======================  Failed to place Gripper at point(x,y,z) = " & tpcTarget.X & ", " & tpcTarget.Y & ", " & tpcTarget.Z)
                    End If
                End If
            subDrawSmallCircleArm(tpcTarget, False)
            End If
        Catch ex As Exception
            ftnSendCommand("play phrase Beep")
            frmDebug.subWriteLogs(" EXCEPTION in ftnPlaceGripperAtPoint :  " & ex.ToString)
        End Try
    End Function

    Function ftnAnglesToPlaceGripperAtPoint(ByVal tpcTarget As typePointCoordinate, ByRef boolCanPlace As Boolean) As typeArmAngles
        Try
            ftnAnglesToPlaceGripperAtPoint = taaArmAngles
            ftnAnglesToPlaceGripperAtPoint.sngShoulder = 0
            ftnAnglesToPlaceGripperAtPoint.sngElbow = 0
            Dim tjcTrialJointCoordinates As typeJointCoordinate = ftnForwardKinematicsCoordinates(ftnAnglesToPlaceGripperAtPoint)
            Dim sngDistanceShoulderToTarget_mm As Single = ftnDistanceBetweenPoints_mm(tjcTrialJointCoordinates.tpcShoulderBase, tpcTarget)

            If (sngDistanceShoulderToTarget_mm <= constArmLength_mm) Then
                Dim sngDistaceElbowToGripperEnd_mm As Single = ftnDistanceBetweenPoints_mm(tjcTrialJointCoordinates.tpcElbow, tjcTrialJointCoordinates.tpcGripperEnd)
                Dim sngAngleShoulderBaseToTarget As Single = ftnRadianToDegree(Atan((tpcTarget.Z - tjcTrialJointCoordinates.tpcShoulderBase.Z) _
                                                                   / Max(Abs(tjcTrialJointCoordinates.tpcShoulderBase.Y - tpcTarget.Y), 0.001)))
                If (tjcTrialJointCoordinates.tpcShoulderBase.Y > tpcTarget.Y) And (sngAngleShoulderBaseToTarget >= 0) Then
                    sngAngleShoulderBaseToTarget += 90
                ElseIf (tjcTrialJointCoordinates.tpcShoulderBase.Y > tpcTarget.Y) Then
                    sngAngleShoulderBaseToTarget -= 90
                End If
                Dim sngAngleElbowToGripperEnd As Single = ftnRadianToDegree(Atan((tjcTrialJointCoordinates.tpcElbow.Z - tjcTrialJointCoordinates.tpcGripperEnd.Z) _
                                                                   / Max(Abs(tjcTrialJointCoordinates.tpcElbow.Y - tjcTrialJointCoordinates.tpcGripperEnd.Y), 0.001)))
                If (tjcTrialJointCoordinates.tpcElbow.Y > tjcTrialJointCoordinates.tpcGripperEnd.Y) And (sngAngleElbowToGripperEnd >= 0) Then
                    sngAngleElbowToGripperEnd += 90
                ElseIf (tjcTrialJointCoordinates.tpcElbow.Y > tjcTrialJointCoordinates.tpcGripperEnd.Y) Then
                    sngAngleElbowToGripperEnd -= 90
                End If

                'The Law of Cosines (also called the Cosine Rule) says: c2 = a2 + b2 − 2ab cos(C)  or C = Acos((a2 + b2 - c2) / 2ab)
                ftnAnglesToPlaceGripperAtPoint.sngShoulder = ftnRadianToDegree(Acos((constShoulderToElbow ^ 2 + sngDistanceShoulderToTarget_mm ^ 2 - sngDistaceElbowToGripperEnd_mm ^ 2) _
                                                                         / (2 * constShoulderToElbow * sngDistanceShoulderToTarget_mm)))
                ftnAnglesToPlaceGripperAtPoint.sngShoulder = ftnConvertTo180(ftnAnglesToPlaceGripperAtPoint.sngShoulder + sngAngleShoulderBaseToTarget)
                ftnAnglesToPlaceGripperAtPoint.sngElbow = ftnRadianToDegree(Acos((sngDistaceElbowToGripperEnd_mm ^ 2 + constShoulderToElbow ^ 2 - sngDistanceShoulderToTarget_mm ^ 2) _
                                                                         / (2 * sngDistaceElbowToGripperEnd_mm * constShoulderToElbow)))
                ftnAnglesToPlaceGripperAtPoint.sngElbow = ftnConvertTo180(ftnAnglesToPlaceGripperAtPoint.sngShoulder - 180 + ftnAnglesToPlaceGripperAtPoint.sngElbow + sngAngleElbowToGripperEnd)
                ftnAnglesToPlaceGripperAtPoint.sngWrist = ftnAnglesToPlaceGripperAtPoint.sngWrist
                ftnAnglesToPlaceGripperAtPoint.sngWristRotate = ftnAnglesToPlaceGripperAtPoint.sngWristRotate
                boolCanPlace = True
            Else
                ftnAnglesToPlaceGripperAtPoint = taaArmAngles
                frmDebug.subWriteLogs(ftnElapsedTimeS() & " ====================== ftnAnglesToPlaceGripperAtPoint: Distance too great for Gripper at point(x,y,z) = " & tpcTarget.X & ", " & tpcTarget.Y & ", " & tpcTarget.Z)
                boolCanPlace = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnAnglesToPlaceGripperAtPoint :  " & ex.ToString)
        End Try
    End Function



    Sub subWaitForTimers()
        Try
            Dim timeout As DateTime = Now().AddMilliseconds(200)
            While (Not timerArmWatchDog.Enabled Or Not timerArmUpdate.Enabled) And (Now() < timeout)
                subWait(25)
            End While
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subWaitForTimers :  " & ex.ToString)
        End Try
    End Sub

    Dim boolMoveArmToNewAnglesBusy As Boolean = False
    Delegate Sub DelegateMoveArm(ByVal taaArmAngles As typeArmAngles, ByVal boolWaitTillComplete As Boolean)
    Sub subMoveArmToNewAngles(ByVal taaNewArmAngles As typeArmAngles, ByVal boolWaitTillComplete As Boolean)
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegateMoveArm(AddressOf subMoveArmToNewAngles)
                Me.Invoke(newDelegate, New Object() {taaNewArmAngles, boolWaitTillComplete})
            Else
                If Not boolMoveArmToNewAnglesBusy And Not boolStop And Not boolShutDown Then
                    boolMoveArmToNewAnglesBusy = True
                    subWaitForTimers()
                    Dim taaArmAnglesCompensated As typeArmAngles = ftnArmAnglesCompensatedForDeflection(taaNewArmAngles)
                    ftnSetShoulderAngle(taaArmAnglesCompensated.sngShoulder)
                    ftnSetElbowAngle(taaArmAnglesCompensated.sngElbow - taaNewArmAngles.sngShoulder)
                    ftnSetWristRotateAngle(taaArmAnglesCompensated.sngWristRotate)
                    ftnSetGripperPosition(ftnDegreeToPosition(taaArmAnglesCompensated.sngGripperOpening))
                    If boolDebugArm Then
                        frmDebug.subWriteLogs(" ------------subMoveArmToNewAngles : sngShoulder =  " & taaNewArmAngles.sngShoulder & ", " & taaArmAnglesCompensated.sngShoulder)
                        frmDebug.subWriteLogs(" ------------subMoveArmToNewAngles : sngElbow =  " & taaNewArmAngles.sngElbow & ", " & taaArmAnglesCompensated.sngElbow)
                        frmDebug.subWriteLogs(" ------------subMoveArmToNewAngles : sngGripperOpening =  " & taaNewArmAngles.sngGripperOpening)
                        frmDebug.subWriteLogs(" ------------subMoveArmToNewAngles : sngGripperPosition =  " & ftnDegreeToPosition(taaArmAnglesCompensated.sngGripperOpening))
                    End If
                    If boolWaitTillComplete Then subWaitForArmToStopMoving()
                    taaArmAngles = ftnGetArmAngles()
                    boolMoveArmToNewAnglesBusy = False
                End If
            End If
        Catch ex As Exception
            boolMoveArmToNewAnglesBusy = False
            frmDebug.subWriteLogs(" EXCEPTION in subMoveArmToNewAngles :  " & ex.ToString)
        End Try
    End Sub

    Sub subWaitForArmToStopMoving()
        Try
            If boolArmConnected And Not boolShutDown Then
                Dim timeOut As DateTime = Now().AddMilliseconds(6000)
                While (Now() < timeOut) And Not boolShutDown And _
                   ((UnSafeNativeMethods.dxl_read_word(constShoulderServo, constMoving) = 1) Or _
                   (UnSafeNativeMethods.dxl_read_word(constGripperServo, constMoving) = 1) Or _
                   (UnSafeNativeMethods.dxl_read_word(constElbowServo, constMoving) = 1))
                    subWait(100)
                End While
                subWait(500)
                If boolDebugArm Then
                    frmDebug.subWriteLogs(ftnElapsedTimeS() & " subWaitForArmToStopMoving:   tjcJointCoordinates.tpcGripperEnd (x,y,z) = " & tjcJointCoordinates.tpcGripperEnd.X & ",  " & tjcJointCoordinates.tpcGripperEnd.Y & ",  " & tjcJointCoordinates.tpcGripperEnd.Z & ", " & (UnSafeNativeMethods.dxl_read_word(constGripperServo, constMoving) = 1))
                    frmDebug.subWriteLogs(ftnElapsedTimeS() & " subWaitForArmToStopMoving:   taaArmAngles(sngShoulder,sngElbow) = " & taaArmAngles.sngShoulder & ", " & (UnSafeNativeMethods.dxl_read_word(constShoulderServo, constMoving) = 1) & ", " & taaArmAngles.sngElbow & ", " & (UnSafeNativeMethods.dxl_read_word(constElbowServo, constMoving) = 1))
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subWaitForArmToStopMoving :  " & ex.ToString)
        End Try
    End Sub

    Function ftnArmAnglesCompensatedForDeflection(ByVal taaDesiredArmAngles As typeArmAngles) As typeArmAngles
        Try ' Real world servo angles will be less than commanded due to deflection due to load.  
            ' This function converts desired arm angles to angles required to achieve them
            ftnArmAnglesCompensatedForDeflection = taaDesiredArmAngles
            If Not boolSimulatedMovement Then
                ftnArmAnglesCompensatedForDeflection.sngShoulder = ftnJointAngleVerify(taaDesiredArmAngles.sngShoulder + 5)  ' + Abs(Cos(ftnDegreeToRadian(taaDesiredArmAngles.sngShoulder))))
                ftnArmAnglesCompensatedForDeflection.sngElbow = ftnJointAngleVerify(taaDesiredArmAngles.sngElbow + 4)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnArmAnglesCompensatedForDeflection :  " & ex.ToString)
        End Try
    End Function

    Function ftnArmAnglesCorrectedForDeflection(ByVal taaArmAnglesReadFromServos As typeArmAngles) As typeArmAngles
        Try ' Convert angles read from sevos to actual angles due to deflection
            ftnArmAnglesCorrectedForDeflection = taaArmAnglesReadFromServos
            If Not boolSimulatedMovement Then
                ftnArmAnglesCorrectedForDeflection.sngShoulder = ftnJointAngleVerify(ftnArmAnglesCorrectedForDeflection.sngShoulder - 5)
                ftnArmAnglesCorrectedForDeflection.sngElbow = ftnJointAngleVerify(ftnArmAnglesCorrectedForDeflection.sngElbow - 4)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnArmAnglesCorrectedForDeflection :  " & ex.ToString)
        End Try
    End Function


    Sub subArmApiControl()
        Try
            If boolCloseGripperAPI Then
                boolCloseGripperAPI = False
                Dim threadClose As New Thread(AddressOf subCloseGripper)
                threadClose.Start()
            ElseIf boolOpenGripperAPI Then
                boolOpenGripperAPI = False
                Dim threadOpen As New Thread(AddressOf subOpenGripper)
                threadOpen.Start()
            ElseIf boolHomeArmAPI Then
                boolHomeArmAPI = False
                Dim threadHome As New Thread(AddressOf subHome)
                threadHome.Start(False)
            ElseIf boolPickUpAPI Then
                boolPickUpAPI = False
                Dim threadPickUp As New Thread(AddressOf ftnGetObjectAuto)
                threadPickUp.Start(intXAPI)
            ElseIf boolMoveToNewAnglesAPI Then
                boolMoveToNewAnglesAPI = False
                subMoveArmToNewAngles(taaNewArmAngles, False)
            ElseIf boolLookUpAPI Then
                boolLookUpAPI = False
                Dim taaNewArmAngles As typeArmAngles = taaArmAngles
                taaNewArmAngles.sngShoulder = ftnJointAngleVerify(frmArm.taaArmAngles.sngShoulder + 10)
                taaNewArmAngles.sngElbow = frmArm.taaArmAngles.sngElbow + 20
                subMoveArmToNewAngles(taaNewArmAngles, True)
            ElseIf boolLookDownAPI Then
                boolLookDownAPI = False
                Dim taaNewArmAngles As typeArmAngles = taaArmAngles
                taaNewArmAngles.sngShoulder = ftnJointAngleVerify(frmArm.taaArmAngles.sngShoulder - 10)
                Dim sngElbowJointAngle As Single = ftnJointAngleVerify(taaNewArmAngles.sngElbow - taaNewArmAngles.sngShoulder - 10)
                taaNewArmAngles.sngElbow = sngElbowJointAngle + taaNewArmAngles.sngShoulder
                subMoveArmToNewAngles(taaNewArmAngles, True)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subArmApiControl :  " & ex.ToString)
        End Try
    End Sub

    Delegate Sub DelegateCloseGripper()
    Public Sub subCloseGripper()
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegateCloseGripper(AddressOf subCloseGripper)
                Me.Invoke(newDelegate, New Object() {})
            Else
                frmArm.taaArmAngles.sngGripperOpening = Me.ftnPositionToDegree(frmArm.constGripperClosedPosition)
                Me.subMoveArmToNewAngles(frmArm.taaArmAngles, False)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subCloseGripper :  " & ex.ToString)
        End Try
    End Sub

    Delegate Sub DelegateOpenGripper()
    Public Sub subOpenGripper()
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegateOpenGripper(AddressOf subOpenGripper)
                Me.Invoke(newDelegate, New Object() {})
            Else
                frmArm.taaArmAngles.sngGripperOpening = Me.ftnPositionToDegree(frmArm.constGripperOpenPosition)
                Me.subMoveArmToNewAngles(frmArm.taaArmAngles, False)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subOpenGripper :  " & ex.ToString)
        End Try
    End Sub



End Class
