
Imports System.Drawing
Imports System.Math



Partial Public Class frmArm
    'Module modArmDraw
    Delegate Sub DelegateDrawArm()
    Public Sub subDrawArm()
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegateDrawArm(AddressOf subDrawArm)
                Me.Invoke(newDelegate, New Object())
            Else
                pbxArm.CreateGraphics.Clear(Color.LightGray)
                subDrawArmVector(tjcJointCoordinates.tpcShoulder, tjcJointCoordinates.tpcShoulder, constShoulderSegment, False)
                subDrawArmVector(tjcJointCoordinates.tpcShoulder, tjcJointCoordinates.tpcElbow, constShoulderSegment, False)
                subDrawArmVector(tjcJointCoordinates.tpcElbow, tjcJointCoordinates.tpcWristRotateB4Offset, constElbowSegment, False)
                subDrawArmVector(tjcJointCoordinates.tpcWristRotateB4Offset, tjcJointCoordinates.tpcWrist, constWristRotateSegment, False)
                Dim tpcGripperStart As typePointCoordinate = ftnPointFromPoint(tjcJointCoordinates.tpcWrist, ftnGripperDegreesToWidth(taaArmAngles.sngGripperOpening) / 2, taaArmAngles.sngElbow + 90, 0)
                subDrawArmVector(tjcJointCoordinates.tpcWrist, tpcGripperStart, constGripperSegment, False)
                Dim tpcGripperTip As typePointCoordinate = ftnPointFromPoint(tpcGripperStart, constWristToGripper, taaArmAngles.sngElbow, 0)
                subDrawArmVector(tpcGripperStart, tpcGripperTip, constGripperSegment, False)
                tpcGripperStart = ftnPointFromPoint(tjcJointCoordinates.tpcWrist, ftnGripperDegreesToWidth(taaArmAngles.sngGripperOpening) / 2, taaArmAngles.sngElbow - 90, 0)
                subDrawArmVector(tjcJointCoordinates.tpcWrist, tpcGripperStart, constGripperSegment, False)
                tpcGripperTip = ftnPointFromPoint(tpcGripperStart, constWristToGripper, taaArmAngles.sngElbow, 0)
                subDrawArmVector(tpcGripperStart, tpcGripperTip, constGripperSegment, False)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawArm :  " & ex.ToString)
        End Try
    End Sub

    Delegate Sub DelegateDrawArmVector(ByVal tpcStartCoordinate As typePointCoordinate, ByVal tpcEndCoordinate As typePointCoordinate, ByVal intArmSegment As Integer, ByVal boolError As Boolean)
    Public Sub subDrawArmVector(ByVal tpcStartCoordinate As typePointCoordinate, ByVal tpcEndCoordinate As typePointCoordinate, ByVal intArmSegment As Integer, ByVal boolError As Boolean)    ' Draw a vector from point-point
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegateDrawArmVector(AddressOf subDrawArmVector)
                Me.Invoke(newDelegate, New Object() {tpcStartCoordinate, tpcEndCoordinate, intArmSegment, boolError})
            Else
                subDrawSmallCircleArm(tpcEndCoordinate, True)
                tpcStartCoordinate.X = pbxArm.Width * 0.25 + (tpcStartCoordinate.X - tjcJointCoordinates.tpcShoulderBase.X) / 2
                tpcStartCoordinate.Y = pbxArm.Width * 0.25 + (tpcStartCoordinate.Y - tjcJointCoordinates.tpcShoulderBase.Y) / 2
                tpcStartCoordinate.Z = pbxArm.Height * 0.5 + (tpcStartCoordinate.Z - tjcJointCoordinates.tpcShoulderBase.Z) / 2
                tpcStartCoordinate.Z = pbxArm.Height - tpcStartCoordinate.Z
                tpcEndCoordinate.X = pbxArm.Width * 0.25 + (tpcEndCoordinate.X - tjcJointCoordinates.tpcShoulderBase.X) / 2
                tpcEndCoordinate.Y = pbxArm.Width * 0.25 + (tpcEndCoordinate.Y - tjcJointCoordinates.tpcShoulderBase.Y) / 2
                tpcEndCoordinate.Z = pbxArm.Height * 0.5 + (tpcEndCoordinate.Z - tjcJointCoordinates.tpcShoulderBase.Z) / 2
                tpcEndCoordinate.Z = pbxArm.Height - tpcEndCoordinate.Z

                If boolError Then
                    Dim widePen As New Pen(Brushes.Crimson)
                    widePen.Width = 8.0F
                    pbxArm.CreateGraphics.DrawLine(widePen, tpcStartCoordinate.Y, tpcStartCoordinate.Z, tpcEndCoordinate.Y, tpcEndCoordinate.Z)
                ElseIf (intArmSegment = intArmSegmentSelect) Then
                    Dim widePen As New Pen(Brushes.Green)
                    widePen.Width = 8.0F
                    pbxArm.CreateGraphics.DrawLine(widePen, tpcStartCoordinate.Y, tpcStartCoordinate.Z, tpcEndCoordinate.Y, tpcEndCoordinate.Z)
                Else
                    Dim widePen As New Pen(Brushes.Black)
                    widePen.Width = 8.0F
                    pbxArm.CreateGraphics.DrawLine(widePen, tpcStartCoordinate.Y, tpcStartCoordinate.Z, tpcEndCoordinate.Y, tpcEndCoordinate.Z)
                End If
                pbxArm.CreateGraphics.Flush()
                pbxArm.CreateGraphics.Dispose()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subDrawArmVector :  " & ex.ToString)
        End Try
    End Sub

    Delegate Sub DelegateDrawSmallCircle(ByVal tpcCoordinate As typePointCoordinate, ByVal boolBlue As Boolean)
    Public Sub subDrawSmallCircleArm(ByVal tpcCoordinate As typePointCoordinate, ByVal boolBlue As Boolean)    ' Draw a small circle 
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegateDrawSmallCircle(AddressOf subDrawSmallCircleArm)
                Me.Invoke(newDelegate, New Object() {tpcCoordinate, boolBlue})
            Else
                tpcCoordinate.X = pbxArm.Width * 0.25 + (tpcCoordinate.X - tjcJointCoordinates.tpcShoulderBase.X) / 2
                tpcCoordinate.Y = pbxArm.Width * 0.25 + (tpcCoordinate.Y - tjcJointCoordinates.tpcShoulderBase.Y) / 2
                tpcCoordinate.Z = pbxArm.Height * 0.5 + (tpcCoordinate.Z - tjcJointCoordinates.tpcShoulderBase.Z) / 2
                tpcCoordinate.Z = pbxArm.Height - tpcCoordinate.Z
                If Not ((tpcCoordinate.Y = tjcJointCoordinates.tpcShoulderBase.Y) And (tpcCoordinate.Z = tjcJointCoordinates.tpcShoulderBase.Z)) Then
                    If boolBlue Then
                        pbxArm.CreateGraphics.DrawArc(Pens.Blue, tpcCoordinate.Y - 12, tpcCoordinate.Z - 12, 24, 24, 0, 360)
                        pbxArm.CreateGraphics.FillEllipse(Brushes.Blue, tpcCoordinate.Y - 12, tpcCoordinate.Z - 12, 24, 24)
                    Else
                        pbxArm.CreateGraphics.DrawArc(Pens.Green, tpcCoordinate.Y - 12, tpcCoordinate.Z - 12, 24, 24, 0, 360)
                        pbxArm.CreateGraphics.FillEllipse(Brushes.Green, tpcCoordinate.Y - 12, tpcCoordinate.Z - 12, 24, 24)
                    End If
                    pbxArm.CreateGraphics.Flush()
                    pbxArm.CreateGraphics.Dispose()
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subDrawSmallCircleArm :  " & ex.ToString)
        End Try
    End Sub



    'End Module
End Class