Imports System.Math


Partial Public Class frmArm

    Public boolGetObjectBusy As Boolean = False
    Delegate Function DelegateGetObject(ByVal intObjectNumber As Integer) As Boolean
    Public Function ftnGetObjectAuto(ByVal intObjectNumber As Integer) As Boolean
        ftnGetObjectAuto = False
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegateGetObject(AddressOf ftnGetObjectAuto)
                Me.Invoke(newDelegate, New Object() {intObjectNumber})
            Else
                If Not boolShutDown And Not boolGetObjectBusy Then
                    boolStop = False
                    boolGetObjectBusy = True
                    Dim tbpObjectPose As typeBotPose ' use Scan Vectors or LMs to get close to object
                    tbpObjectPose.Location = taoObjectArray(intObjectNumber).tmcLocation
                    tbpObjectPose.Orientation = ftnConvertTo360(taoObjectArray(intObjectNumber).intOrientation + 180)
                    tmcDestination = ftnLocationFromEstimatedPose(tbpObjectPose, 0, 18)  ' Destination is 18" from the object and facing it
                    frmDebug.subWriteCoordinate(ftnElapsedTimeS() & "================ Pick up " & taoObjectArray(intObjectNumber).strName & " at ", tmcDestination)
                    ftnGetObjectAuto = ftnDriveToDestination_Top(tbpBotPose, tmcDestination)

                    If ftnGetObjectAuto Then '  use vision to locate object  
                        tbpBotPose = ftnAdjustPose(tbpBotPose, ftnConvertToPose(tmcDestination, taoObjectArray(intObjectNumber).intOrientation), 2)
                        Dim tpcObject As typePointCoordinate = ftnConvertLocationToPointCoordinate(tbpBotPose, taoObjectArray(intObjectNumber).tmcLocation, taoObjectArray(intObjectNumber).intObjectHeight)
                        subHome(True)
                        UnSafeNativeMethods.dxl_write_word(254, frmArm.constTorqueLimit, 1023) '   
                        subHome(True)
                        tpcObject = ftnMoveBotToPickUpPoint(tpcObject, intObjectNumber, False)
                        ftnGetObjectAuto = ftnPickUpObject(tpcObject, intObjectNumber)
                    End If
                    If ftnGetObjectAuto Then
                        ftnSendCommand("play phrase tada")
                    Else
                        ftnSendCommand("play phrase fail")
                    End If
                    frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ===============================  Pick up complete")
                    boolGetObjectBusy = False
                End If  'If Not boolShutDown
            End If  '
        Catch ex As Exception
            ftnGetObjectAuto = False
            boolGetObjectBusy = False
            frmDebug.subWriteLogs(" EXCEPTION In ftnGetObjectAuto :  " & ex.ToString)
        End Try
    End Function

    Function ftnMoveBotToPickUpPoint(ByVal tpcObject As typePointCoordinate, ByVal intObjectNumber As Integer, ByRef boolLocated As Boolean) As typePointCoordinate
        Try ' Center the object and approach until within arm's length.  Repeat until achieved.  Return the objects visually measured point coordinates
            ftnMoveBotToPickUpPoint = tpcObject
            tpcObject = ftnPointAndLocate(tbpBotPose, tpcObject, intObjectNumber, boolLocated, False) '  point at object and visually determine its coordinates
            While ((Abs(ftnRadianToDegree(Atan(tpcObject.X / tpcObject.Y))) > 1) Or (tpcObject.Y > (ftnPickUpDistance_mm(intObjectNumber) + 5)))
                tpcObject = ftnPointAndLocate(tbpBotPose, tpcObject, intObjectNumber, boolLocated, True) '  point at object and visually determine its coordinates
                tpcObject = ftnMoveForward(tpcObject, intObjectNumber, boolLocated)
            End While
            ftnMoveBotToPickUpPoint = tpcObject
            If boolDebugArm Then frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & "  =====================ftnMoveBotToPickUpPoint:  tpcObject at " & tpcObject.X & ",  " & tpcObject.Y & ",  " & tpcObject.Z)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnMoveBotToPickUpPoint :  " & ex.ToString)
        End Try
    End Function


    Function ftnPickUpObject(ByRef tpcObject As typePointCoordinate, ByVal intObjectNumber As Integer) As Boolean
        Try
            ftnPickUpObject = False
            frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & "  =====================ftnPickUpObject:  object at " & tpcObject.X & ",  " & tpcObject.Y & ",  " & tpcObject.Z)
            boolCollisionDetectionEnable = False
            If Not boolStop And Not boolShutDown Then
                Dim tpcObjectPrevious As typePointCoordinate = tpcObject
                ftnPickUpObject = ftnPlaceGripperNearObject(tpcObject, intObjectNumber, 0)
                If ftnPickUpObject Then
                    For i = 1 To 3 ' Position gripper near object and visually relocate it
                        If ((Abs(ftnRadianToDegree(Atan(tpcObject.X / tpcObject.Y))) > 1) Or _
                            (Abs(tpcObject.Y - tpcObjectPrevious.Y) > 10) Or _
                            (Abs(tpcObject.Z - tpcObjectPrevious.Z) > 10)) Then
                            tpcObjectPrevious = tpcObject
                            ftnPickUpObject = ftnPlaceGripperNearObject(tpcObject, intObjectNumber, i)  ' Position gripper near object
                            tpcObject = ftnVisuallyLocateObjectCoordinate(tbpBotPose, tpcObject, intObjectNumber, False, False) ' 
                            tbpBotPose = ftnTurnToObject(tbpBotPose, tpcObject)
                            ftnMoveForward(tpcObject, intObjectNumber, False)
                            tpcObject = ftnVisuallyLocateObjectCoordinate(tbpBotPose, tpcObject, intObjectNumber, False, False) ' 
                        End If
                    Next
                End If

                ftnPickUpObject = ftnPickUpObject And ((tpcObject.Z - constShoulderBaseZ) < 350)  ' max height

                If ftnPickUpObject Then
                    Dim taaActualArmAngles As typeArmAngles = ftnGetArmAngles()
                    tjcJointCoordinates = ftnForwardKinematicsCoordinates(taaActualArmAngles)
                    Dim sngDistanceShoulderToObject_mm As Single = ftnDistanceBetweenPoints_mm(tjcJointCoordinates.tpcShoulderBase, tpcObject)
                    If (sngDistanceShoulderToObject_mm > constArmLength_mm) And (sngDistanceShoulderToObject_mm < constArmLength_mm + 10) Then
                        subMoveGripperOutOfWay()
                        tbpBotPose = ftnBotFwdDistance(tbpBotPose, 20 / 25.4) '  Move closer to object
                        tpcObject.Y -= 20  ' TODO - change to use odometry data
                    End If

                    taaArmAngles.sngWristRotate = taoObjectArray(intObjectNumber).intRotation ' Rotate gripper if required
                    subMoveArmToNewAngles(taaArmAngles, True)
                    If (taoObjectArray(intObjectNumber).intObjectHeight > 150) Then ' Aim for just beyond object
                        tpcObject.Y += 15
                        tpcObject.Z += taoObjectArray(intObjectNumber).intGripOffset_mm - 20
                    Else
                        tpcObject.Y += taoObjectArray(intObjectNumber).intGripOffset_mm - 30
                        'tpcObject.Z -= 25
                    End If
                End If


                frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ===============================  Position gripper around object")
                ftnPickUpObject = ftnPlaceGripperAtPoint(tpcObject, True)  ' position gripper around object
                If ftnPickUpObject Then
                    frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ===============================  Close Gripper")
                    taaArmAngles.sngGripperOpening = ftnPositionToDegree(constGripperClosedPosition)
                    subMoveArmToNewAngles(taaArmAngles, True)
                    frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ===============================  Lift up and Move back")
                    subMoveElbowUp(30)
                    tbpBotPose = ftnBotFwdDistance(tbpBotPose, -6)
                    If boolDebugArm Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ===============================  ftnPickUpObject : Home")
                    subHome(False)
                    ftnPickUpObject = ftnPickUpObject And (UnSafeNativeMethods.dxl_read_word(constGripperServo, constPresentPosition) < (constGripperClosedPosition - 15))   ' does not work
                Else
                    frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & "  ===============================   FAILED - Could not grip object at " & tpcObject.X & ",  " & tpcObject.Y & ",  " & tpcObject.Z)
                    ftnPickUpObject = False
                    subHome(True)
                End If
            End If
            boolCollisionDetectionEnable = True
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnPickUpObject :  " & ex.ToString)
            ftnPickUpObject = False
            boolCollisionDetectionEnable = True
        End Try
    End Function

    Function ftnPlaceGripperNearObject(ByVal tpcNearObject As typePointCoordinate, ByVal intObjectNumber As Integer, ByVal i As Integer) As Boolean
        Try
            frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " =============  Position gripper near object at (xyz)  " & tpcNearObject.X & ", " & tpcNearObject.Y & ", " & tpcNearObject.Z & " : intObjectNumber = " & intObjectNumber & " : iteration = " & i)
            subMoveElbowUp(15)
            If (taoObjectArray(intObjectNumber).intObjectHeight > 150) Then ' Aim for just below object
                tpcNearObject.Z -= 20
            Else
                tpcNearObject.Y -= 30
            End If
            ftnPlaceGripperNearObject = ftnPlaceGripperAtPoint(tpcNearObject, True)  ' 
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnPlaceGripperNearObject :  " & ex.ToString)
        End Try
    End Function


    Function ftnPointAndLocate(ByRef tbpEstimatedPose As typeBotPose, ByRef tpcObject As typePointCoordinate, ByVal intObjectNumber As Integer, _
                               ByRef boolLocated As Boolean, ByRef boolPerformSanityCheck As Boolean) As typePointCoordinate
        Try
            ftnPointAndLocate = tpcObject
            frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ===============================  ftnPointAndLocate :  tpcObject(x,y,z) " & tpcObject.X & ", " & tpcObject.Y & ", " & tpcObject.Z)
            If Not boolStop And Not boolShutDown Then
                tbpEstimatedPose = ftnPointAtObject(tbpEstimatedPose, tpcObject)  ' point at object's assumed position
                ftnPointAndLocate = ftnVisuallyLocateObjectCoordinate(tbpEstimatedPose, tpcObject, intObjectNumber, boolLocated, boolPerformSanityCheck) ' visually locate object and find its location
            End If
            If boolDebugArm Then frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ===============================  ftnPointAndLocate :  ftnPointAndLocate(x,y,z) " & ftnPointAndLocate.X & ", " & ftnPointAndLocate.Y & ", " & ftnPointAndLocate.Z)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnPointAndLocate :  " & ex.ToString)
        End Try
    End Function

    Function ftnPointAtObject(ByRef tbpEstimatedPose As typeBotPose, ByRef tpcObject As typePointCoordinate) As typeBotPose  ' point at object
        Try
            ftnPointAtObject = tbpEstimatedPose
            ftnPointAtObject = ftnTurnToObject(tbpEstimatedPose, tpcObject)
            If boolDebugArm Then frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ===============================  Point gripper at object (x,y,z) " & tpcObject.X & ", " & tpcObject.Y & ", " & tpcObject.Z)
            ftnAimGripperAtPoint(tpcObject) ' Point gripper at object
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnPointAtObject :  " & ex.ToString)
        End Try
    End Function

    Function ftnMoveForward(ByRef tpcObject As typePointCoordinate, ByVal intObjectNumber As Integer, ByRef boolLocated As Boolean) As typePointCoordinate
        Try
            ftnMoveForward = tpcObject
            If (tpcObject.Y > (ftnPickUpDistance_mm(intObjectNumber) + 5)) Then
                Dim sngFwdDistance_i As Single = Min(4.5, (tpcObject.Y - ftnPickUpDistance_mm(intObjectNumber)) / 25.4)
                If ((tpcObject.Y - ftnPickUpDistance_mm(intObjectNumber)) < 125) Then subMoveGripperOutOfWay()
                frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & "   ======================== Move closer to object " & sngFwdDistance_i)
                tpcObject.Y -= sngFwdDistance_i * 25.4  ' TODO - change to use odometry data
                If (tpcObject.Y < (sngBotRadius * 25.4 + 20)) Then boolCollisionDetectionEnable = False
                tbpBotPose = ftnBotFwdDistance(tbpBotPose, sngFwdDistance_i) '  Move closer to object
                tpcObject = ftnPointAndLocate(tbpBotPose, tpcObject, intObjectNumber, boolLocated, False) '  point at object and visually determine its coordinates
            End If
            ftnMoveForward = tpcObject
            If boolDebugArm Then frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ===============================  ftnMoveForward: at object (x,y,z) " & tpcObject.X & ", " & tpcObject.Y & ", " & tpcObject.Z)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnMoveForward :  " & ex.ToString)
        End Try
    End Function

    Function ftnTurnToObject(ByRef tbpEstimatedPose As typeBotPose, ByRef tpcObject As typePointCoordinate) As typeBotPose
        Try
            ftnTurnToObject = tbpEstimatedPose
            Dim sngAngleXY As Single = ftnRadianToDegree(Atan(tpcObject.X / tpcObject.Y))
            If (Abs(sngAngleXY) > 1) Then
                If boolDebugArm Then frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ===============================  Turn towards object (x,y,z) " & tpcObject.X & ", " & tpcObject.Y & ", " & tpcObject.Z)
                ftnTurnToObject = ftnBotTurnToOrientation(tbpEstimatedPose, ftnConvertTo360(tbpEstimatedPose.Orientation - sngAngleXY)) '   Turn towards object
                tpcObject.X = 0
            End If

        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnTurnToObject :  " & ex.ToString)
        End Try
    End Function

    Function ftnPickUpDistance_mm(ByVal intObjectNumber As Integer) As Single
        Try
            Dim intObjectRelativeHeight As Integer = taoObjectArray(intObjectNumber).intObjectHeight - tjcJointCoordinates.tpcShoulderBase.Z
            If (Abs(intObjectRelativeHeight) < 150) Then
                ftnPickUpDistance_mm = 350
            ElseIf (intObjectRelativeHeight <= -250) Then
                ftnPickUpDistance_mm = 200
            ElseIf (intObjectRelativeHeight <= -150) Then
                ftnPickUpDistance_mm = 180
            ElseIf (intObjectRelativeHeight >= 300) Then
                ftnPickUpDistance_mm = 200
            ElseIf (intObjectRelativeHeight >= 250) Then
                ftnPickUpDistance_mm = 220
            Else
                ftnPickUpDistance_mm = 270
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnPickUpDistance_mm :  " & ex.ToString)
        End Try
    End Function

    Sub subMoveElbowUp(ByRef sngAngle As Single)
        Try
            If boolDebugArm Then frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " subMoveElbowUp:  ======================== Move elbow up ")
            Dim taaNewArmAngles As typeArmAngles = taaArmAngles
            taaNewArmAngles.sngElbow = Min(taaNewArmAngles.sngElbow + sngAngle, 90)
            subMoveArmToNewAngles(taaNewArmAngles, True)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subMoveElbowUp :  " & ex.ToString)
        End Try
    End Sub

    Sub subMoveGripperOutOfWay()
        Try
            If boolDebugArm Then frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " subMoveGripperOutOfWay:  ======================== Move gripper out of the way ")
            Dim taaNewArmAngles As typeArmAngles = taaArmAngles
            taaNewArmAngles.sngElbow = Min(taaNewArmAngles.sngElbow + 50, 90)
            taaNewArmAngles.sngShoulder = Min(taaNewArmAngles.sngShoulder + 25, 115)
            subMoveArmToNewAngles(taaNewArmAngles, True)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subMoveGripperOutOfWay :  " & ex.ToString)
        End Try
    End Sub

    Function ftnVisuallyLocateObjectCoordinate(ByRef tbpEstimatedPose As typeBotPose, ByVal tpcObject As typePointCoordinate, ByVal intObjectNumber As Integer, _
                                               ByRef boolLocated As Boolean, ByRef boolPerformSanityCheck As Boolean) As typePointCoordinate ' visually locate object and find its location
        Try
            ftnVisuallyLocateObjectCoordinate = tpcObject
            frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ===============================  Visually locate object  ")
            boolLocated = False
            Dim sngCameraToObjectAngleYZ, sngCameraToObjectAngleYX As Single
            If boolDebugArm Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ===============================  ftnVisuallyLocateObjectCoordinate :  START")
            If ftnVisualRecognizerConnected() Then
                taaArmAngles = ftnGetArmAngles()
                tjcJointCoordinates = ftnForwardKinematicsCoordinates(taaArmAngles)
                Dim tirArmObject As typeImageRecognized = ftnFindObjectVisually(tbpEstimatedPose, tpcObject, intObjectNumber)
                boolLocalizationInProgress = False     '??????????????????????????????????????????????????????????????
                If CBool(InStr(tirArmObject.Name, taoObjectArray(intObjectNumber).strName)) Then '  OK - we've  found the object
                    boolLocated = True
                    sngCameraToObjectAngleYZ = (ticCameraResolution.Y / 2 - tirArmObject.intPixelY) / ticCameraResolution.X * sngCameraFOV + taaArmAngles.sngElbow
                    sngCameraToObjectAngleYX = ftnPixelX_ToDegrees(tirArmObject.intPixelX)
                    ftnVisuallyLocateObjectCoordinate = ftnPointFromPoint(tjcJointCoordinates.tpcCamera, tirArmObject.sngRange * 25.4, sngCameraToObjectAngleYZ, sngCameraToObjectAngleYX)
                    If boolDebugArm Then
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnVisuallyLocateObjectCoordinate :  tirArmObject.Name = " & tirArmObject.Name & " :   (x, y) " & tirArmObject.intPixelX & ", " & tirArmObject.intPixelY)
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnVisuallyLocateObjectCoordinate :  tirArmObject.sngRange = " & tirArmObject.sngRange)
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnVisuallyLocateObjectCoordinate :  sngRange_mm = " & tirArmObject.sngRange * 25.4)
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnVisuallyLocateObjectCoordinate :  tirArmObject.intPixelX = " & tirArmObject.intPixelX)
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnVisuallyLocateObjectCoordinate :  tirArmObject.intPixelY = " & tirArmObject.intPixelY)
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnVisuallyLocateObjectCoordinate :  tjcJointCoordinates.tpcCamera = " & tjcJointCoordinates.tpcCamera.X & ", " & tjcJointCoordinates.tpcCamera.Y & ", " & tjcJointCoordinates.tpcCamera.Z)
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnVisuallyLocateObjectCoordinate :  taaArmAngles.sngElbow = " & taaArmAngles.sngElbow)
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnVisuallyLocateObjectCoordinate :  sngCameraToObjectAngleYZ = " & sngCameraToObjectAngleYZ)
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnVisuallyLocateObjectCoordinate :  sngCameraToObjectAngleYX = " & sngCameraToObjectAngleYX)
                    End If
                End If
            End If
            Dim boolSanityCheckPass As Boolean = (Abs(ftnVisuallyLocateObjectCoordinate.Y - tpcObject.Y) < 100) And (Abs(ftnVisuallyLocateObjectCoordinate.Z - tpcObject.Z) < 100) ' Check for really bad localizations
            If (boolLocated And Not boolPerformSanityCheck) Or (boolLocated And boolSanityCheckPass) Then
                frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ========================================= Object visually located at (xyz)  " & ftnVisuallyLocateObjectCoordinate.X & ", " & ftnVisuallyLocateObjectCoordinate.Y & ", " & ftnVisuallyLocateObjectCoordinate.Z)
            ElseIf boolLocated Then
                frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ========================================= Object  localization rejected (xyz)  " & ftnVisuallyLocateObjectCoordinate.X & ", " & ftnVisuallyLocateObjectCoordinate.Y & ", " & ftnVisuallyLocateObjectCoordinate.Z)
                frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ========================================= Object  expected at (xyz)  " & tpcObject.X & ", " & tpcObject.Y & ", " & tpcObject.Z)
                frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ========================================= sngCameraToObjectAngleYZ, sngCameraToObjectAngleYX " & sngCameraToObjectAngleYZ & ", " & sngCameraToObjectAngleYX)
                ftnVisuallyLocateObjectCoordinate = tpcObject
            Else
                frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ========================================= Object not seen ")
                ftnVisuallyLocateObjectCoordinate = tpcObject
            End If
            frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & " ========================================= Object located at (xyz)  " & ftnVisuallyLocateObjectCoordinate.X & ", " & ftnVisuallyLocateObjectCoordinate.Y & ", " & ftnVisuallyLocateObjectCoordinate.Z)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnVisuallyLocateObjectCoordinate :  " & ex.ToString)
        End Try
    End Function


    Private Function ftnFindObjectVisually(ByRef tbpEstimatedPose As typeBotPose, ByVal tpcObject As typePointCoordinate, ByVal intObjectNumber As Integer) As typeImageRecognized
        ftnFindObjectVisually.Name = ""
        ftnFindObjectVisually.intPixelX = 0
        ftnFindObjectVisually.intPixelY = 0
        ftnFindObjectVisually.sngRange = 0
        Try
            Dim strObjectRecognized As String = ""
            strStatusDriving = " Searching for Object  ..."
            If boolSimulatedMovement Then
                ftnFindObjectVisually.Name = taoObjectArray(intObjectNumber).strName
                ftnFindObjectVisually.sngRange = tpcObject.Y
                ftnFindObjectVisually.intPixelX = ticCameraResolution.X / 2
                ftnFindObjectVisually.intPixelY = ticCameraResolution.Y / 2
                ftnFindObjectVisually.sngYRotation = 0
            Else
                If boolDebugArm And (strObjectRecognized <> "") Then frmDebug.subWriteLogs(" ftnFindObjectVisually : ftnVisualRecognizerConnected = " & ftnVisualRecognizerConnected())
                If ftnVisualRecognizerConnected() Then
                    subStartImageRecognition()
                    Dim intValidFinds As Integer = 0
                    Dim timeOut As DateTime = Now().AddSeconds(sngLocalizationTime)
                    Dim tirVisuallyLocatedObject As typeImageRecognized
                    tirVisuallyLocatedObject.Name = ""
                    Do
                        Do
                            strObjectRecognized = ftnGetNextObject()
                            strStatusDriving = " Recognized Object : " & strObjectRecognized
                            If CBool(InStr(strObjectRecognized, taoObjectArray(intObjectNumber).strName)) Then ' Is it the object we're looking for?
                                frmDebug.subWriteLogs(ftnElapsedTimeS() & " ========================================= Object found  " & strObjectRecognized)
                                strStatusDriving = " Object Found = " & ftnFindObjectVisually.Name
                                tirVisuallyLocatedObject = ftnDecodeRecognitionString(strObjectRecognized, False)  '(tpcObject.Y > (ftnPickUpDistance_mm(intObjectNumber) + 125)))    ' Convert the image recognition string to useable units
                            End If
                            subWait(25)
                        Loop Until ((Now() > timeOut) Or CBool(InStr(strObjectRecognized, taoObjectArray(intObjectNumber).strName)) Or boolStop Or boolShutDown)
                        If CBool(InStr(strObjectRecognized, taoObjectArray(intObjectNumber).strName)) Then '  get an average of visual sightings
                            intValidFinds += 1
                            ftnFindObjectVisually.intPixelX = (ftnFindObjectVisually.intPixelX * (intValidFinds - 1) + tirVisuallyLocatedObject.intPixelX) / intValidFinds
                            ftnFindObjectVisually.intPixelY = (ftnFindObjectVisually.intPixelY * (intValidFinds - 1) + tirVisuallyLocatedObject.intPixelY) / intValidFinds
                            ftnFindObjectVisually.sngRange = (ftnFindObjectVisually.sngRange * (intValidFinds - 1) + tirVisuallyLocatedObject.sngRange) / intValidFinds
                            ftnFindObjectVisually.Name = tirVisuallyLocatedObject.Name
                            If boolDebugArm Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ========================================= Object x, y, range  " & ftnFindObjectVisually.intPixelX & ", " & ftnFindObjectVisually.intPixelY & ", " & ftnFindObjectVisually.sngRange)
                        End If
                    Loop Until ((Now() > timeOut) Or (intValidFinds > 4) Or boolStop Or boolShutDown)
                    subEndImageRecognition()
                End If
            End If

        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnFindObjectVisually :  " & ex.ToString)
        End Try
    End Function



    Function ftnPixelX_ToDegrees(ByRef intPixelX As Integer) As Single
        Try
            ftnPixelX_ToDegrees = (intPixelX - ticCameraResolution.X / 2) / ticCameraResolution.X * sngCameraFOV
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnPixelX_ToDegrees :  " & ex.ToString)
            ftnPixelX_ToDegrees = 0
        End Try
    End Function

End Class


