' This module contains subroutine and functions which are used by multiple modules

Imports System.Drawing
Imports System.Math
Imports System.Threading
Imports System.Windows.Forms


Public Module modUtilities


    ' =========================================   Angles  ===================================================

 
    Public Function ftnConvertTo360(ByRef intAngle As Integer) As Integer
        ftnConvertTo360 = intAngle
        While ftnConvertTo360 >= 360
            ftnConvertTo360 -= 360
        End While
        While ftnConvertTo360 < 0
            ftnConvertTo360 += 360
        End While
    End Function

    Public Function ftnConvertTo360(ByRef sngAngle As Single) As Single
        ftnConvertTo360 = sngAngle
        While ftnConvertTo360 >= 360
            ftnConvertTo360 -= 360
        End While
        While ftnConvertTo360 < 0
            ftnConvertTo360 += 360
        End While
    End Function

    Public Function ftnConvertTo360(ByRef dblAngle As Double) As Double
        ftnConvertTo360 = dblAngle
        While ftnConvertTo360 >= 360
            ftnConvertTo360 -= 360
        End While
        While ftnConvertTo360 < 0
            ftnConvertTo360 += 360
        End While
    End Function

 
    Public Function ftnConvertTo180(ByRef intAngle As Integer) As Integer
        ftnConvertTo180 = intAngle
        While ftnConvertTo180 > 180
            ftnConvertTo180 -= 360
        End While
        While ftnConvertTo180 < -180
            ftnConvertTo180 += 360
        End While
        If ftnConvertTo180 = 360 Then ftnConvertTo180 = 0
    End Function

    Public Function ftnConvertTo180(ByRef sngAngle As Single) As Single
        ftnConvertTo180 = sngAngle
        While ftnConvertTo180 > 180
            ftnConvertTo180 -= 360
        End While
        While ftnConvertTo180 < -180
            ftnConvertTo180 += 360
        End While
        If ftnConvertTo180 = 360 Then ftnConvertTo180 = 0
    End Function

    Public Function ftnConvertTo180(ByRef sngAngle As Double) As Double
        ftnConvertTo180 = sngAngle
        While ftnConvertTo180 > 180
            ftnConvertTo180 -= 360
        End While
        While ftnConvertTo180 < -180
            ftnConvertTo180 += 360
        End While
        If ftnConvertTo180 = 360 Then ftnConvertTo180 = 0
    End Function

    Public Function ftnP1ToP2Angle(ByRef tmcP1 As typeMapCoordinate, ByRef tmcP2 As typeMapCoordinate) As Single
        Try
            If (tmcP1.X = tmcP2.X) Then  ' don't want a divide by zero
                ftnP1ToP2Angle = 90
            Else
                ftnP1ToP2Angle = ftnRadianToDegree(Atan(Abs((tmcP1.Y - tmcP2.Y) / (tmcP1.X - tmcP2.X))))
            End If  ' only valid for quadrant 1
            If (tmcP1.X > tmcP2.X) Then ftnP1ToP2Angle = 180 - ftnP1ToP2Angle
            If (tmcP1.Y < tmcP2.Y) Then ftnP1ToP2Angle = 360 - ftnP1ToP2Angle
            ftnP1ToP2Angle = ftnConvertTo360(ftnP1ToP2Angle + 0.01)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnP1ToP2Angle :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnP1ToP2Angle(ByRef tmoP1 As typeMapOffset, ByRef tmcP2 As typeMapCoordinate) As Single
        Try
            If (tmoP1.X = tmcP2.X) Then  ' don't want a divide by zero
                ftnP1ToP2Angle = 90
            Else
                ftnP1ToP2Angle = ftnRadianToDegree(Atan(Abs((tmoP1.Y - tmcP2.Y) / (tmoP1.X - tmcP2.X))))
            End If  ' only valid for quadrant 1
            If (tmoP1.X > tmcP2.X) Then ftnP1ToP2Angle = 180 - ftnP1ToP2Angle
            If (tmoP1.Y < tmcP2.Y) Then ftnP1ToP2Angle = 360 - ftnP1ToP2Angle
            ftnP1ToP2Angle = ftnConvertTo360(ftnP1ToP2Angle + 0.01)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnP1ToP2Angle :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnP1ToP2RelativeAngle(ByRef tmcP1 As typeMapCoordinate, ByRef tmcP2 As typeMapCoordinate) As Single
        Try
            If (tmcP1.X = tmcP2.X) Then  ' don't want a divide by zero
                ftnP1ToP2RelativeAngle = 90
            Else
                ftnP1ToP2RelativeAngle = ftnRadianToDegree(Atan(Abs((tmcP1.Y - tmcP2.Y) / (tmcP1.X - tmcP2.X))))
            End If  ' 
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnP1ToP2RelativeAngle :  " & ex.ToString)
        End Try
    End Function

    'Public Function ftnTurnAngle(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As Single  ' 
    '    Try ' Returns the degrees required for the Bot to turn and point at tbpTargetPose 
    '        If ftnPosesEqual(tbpEstimatedPose, tbpTargetPose) Then
    '            ftnTurnAngle = ftnP1ToP2Angle(tbpEstimatedPose.Location, ftnLowestValueNeighbor(tbpEstimatedPose.Location)) - tbpEstimatedPose.Orientation
    '        ElseIf ftnLocationsEqual(tbpEstimatedPose.Location, tbpTargetPose.Location) Then
    '            ftnTurnAngle = ftnAngleDifference(tbpTargetPose.Orientation, tbpEstimatedPose.Orientation)
    '        Else
    '            ftnTurnAngle = ftnP1ToP2Angle(tbpEstimatedPose.Location, tbpTargetPose.Location) - tbpEstimatedPose.Orientation
    '        End If
    '        ftnTurnAngle = ftnConvertTo180(ftnTurnAngle)
    '    Catch ex As Exception
    '        frmDebug.subWriteLogs(" EXCEPTION in ftnTurnAngle :  " & ex.ToString)
    '    End Try
    'End Function

    Public Function ftnTurnAngleV2(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As Single  ' 
        Try ' Returns the degrees required for the Bot to turn and point at tbpTargetPose 
            If ftnLocationsEqual(tbpEstimatedPose.Location, tbpTargetPose.Location) Then
                ftnTurnAngleV2 = ftnAngleDifference(tbpTargetPose.Orientation, tbpEstimatedPose.Orientation)
            Else
                ftnTurnAngleV2 = ftnP1ToP2Angle(tbpEstimatedPose.Location, tbpTargetPose.Location) - tbpEstimatedPose.Orientation
            End If
            ftnTurnAngleV2 = ftnConvertTo180(ftnTurnAngleV2)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnTurnAngleV2 :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnTurnAngle(ByRef tbpEstimatedPose As typeBotPose, ByRef tmcTargetLocation As typeMapCoordinate) As Single  ' 
        Try ' Returns the degrees required for the Bot to turn and point at tmcTargetLocation 
            If ftnLocationsEqual(tbpEstimatedPose.Location, tmcTargetLocation) Then
                ftnTurnAngle = 0
            Else
                ftnTurnAngle = ftnP1ToP2Angle(tbpEstimatedPose.Location, tmcTargetLocation) - tbpEstimatedPose.Orientation
                ftnTurnAngle = ftnConvertTo180(ftnTurnAngle)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnTurnAngle :  " & ex.ToString)
        End Try
    End Function


    Public Function ftnAngleDifference(ByRef intAngle1 As Integer, ByRef intAngle2 As Integer) As Integer
        ftnAngleDifference = (intAngle1 - intAngle2)
        ftnAngleDifference = ftnConvertTo180(ftnAngleDifference)
    End Function

    Public Function ftnAngleDifference(ByRef sngAngle1 As Single, ByRef sngAngle2 As Single) As Single
        ftnAngleDifference = (sngAngle1 - sngAngle2)
        ftnAngleDifference = ftnConvertTo180(ftnAngleDifference)
    End Function

    Public Function ftnAngleDifference(ByRef intAngle1 As Integer, ByRef sngAngle2 As Single) As Single
        ftnAngleDifference = (intAngle1 - sngAngle2)
        ftnAngleDifference = ftnConvertTo180(ftnAngleDifference)
    End Function

    Public Function ftnDegreeToRadian(ByRef sngDegrees As Single) As Single
        ftnDegreeToRadian = sngDegrees * 0.017453
    End Function

    Public Function ftnRadianToDegree(ByRef sngDegress As Single) As Single
        ftnRadianToDegree = sngDegress * 57.29578
    End Function


    Public Function ftnAnglesWithinUncertainty(ByRef intAngle1 As Integer, ByRef intAngle2 As Integer, ByRef intMaxDifference As Integer) As Boolean
        Try
            ftnAnglesWithinUncertainty = (Abs(ftnAngleDifference(intAngle1, intAngle2)) <= intMaxDifference)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnAnglesWithinUncertainty :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnAnglesWithinUncertainty(ByRef sngAngle1 As Single, ByRef sngAngle2 As Single, ByRef intMaxDifference As Integer) As Boolean
        Try
            ftnAnglesWithinUncertainty = (Abs(ftnAngleDifference(sngAngle1, sngAngle2)) <= intMaxDifference)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnAnglesWithinUncertainty :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnAnglesWithinUncertainty(ByRef sngAngle1 As Single, ByRef sngAngle2 As Single, ByRef sngMaxDifference As Single) As Boolean
        Try
            ftnAnglesWithinUncertainty = (Abs(ftnAngleDifference(sngAngle1, sngAngle2)) <= sngMaxDifference)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnAnglesWithinUncertainty :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnOrientationPointsRight(ByVal intAngle As Integer) As Boolean
        intAngle = ftnConvertTo360(intAngle)
        ftnOrientationPointsRight = (intAngle < 90) Or (intAngle > 270)
    End Function

    Public Function ftnOrientationPointsRight(ByVal sngAngle As Single) As Boolean
        sngAngle = ftnConvertTo360(sngAngle)
        ftnOrientationPointsRight = (sngAngle < 90) Or (sngAngle > 270)
    End Function

    Public Function ftnOrientationPointsUp(ByVal sngAngle As Single) As Boolean
        sngAngle = ftnConvertTo180(sngAngle)
        ftnOrientationPointsUp = (sngAngle > 0)
    End Function


    ' =========================================   Distances  ===================================================


    Public Function ftnDistanceBetweenLocations(ByRef tmcLocation1 As typeMapCoordinate, ByRef tmcLocation2 As typeMapCoordinate) As Single
        Dim intDistanceX As Integer = tmcLocation2.X - tmcLocation1.X
        Dim intDistanceY As Integer = tmcLocation2.Y - tmcLocation1.Y
        ftnDistanceBetweenLocations = Sqrt(intDistanceX * intDistanceX + intDistanceY * intDistanceY)
    End Function

    Public Function ftnDistanceBetweenLocations(ByRef tmoLocation1 As typeMapOffset, ByRef tmoLocation2 As typeMapOffset) As Single
        Dim intDistanceX As Integer = tmoLocation2.X - tmoLocation1.X
        Dim intDistanceY As Integer = tmoLocation2.Y - tmoLocation1.Y
        ftnDistanceBetweenLocations = Sqrt(intDistanceX * intDistanceX + intDistanceY * intDistanceY)
    End Function

    Public Function ftnInchesToCm(ByRef sngDistanceInInches As Single) As Single
        ftnInchesToCm = sngDistanceInInches * 2.54
    End Function



    Public Function ftnCmToInches(ByRef sngDistanceInCm As Single) As Single
        ftnCmToInches = sngDistanceInCm * 0.3937
    End Function




    ' =========================================   Map Limits  ===================================================


    Public Function ftnLocationOnMapEdge(ByRef tmcLocation As typeMapCoordinate) As Boolean
        Try
            ftnLocationOnMapEdge = (tmcLocation.Y = 0) Or (tmcLocation.Y = intMapHeight - 1) Or
                                   (tmcLocation.X = 0) Or (tmcLocation.X = intMapWidth - 1)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocationOnMapEdge :  " & ex.ToString)
        End Try
    End Function



    Public Sub subCheckMapArrayLimits(ByRef intX_Min As Integer, ByRef intX_Max As Integer, ByRef intY_Min As Integer, ByRef intY_Max As Integer)
        If intX_Min < 0 Then intX_Min = 0
        If intX_Max >= intMapWidth Then intX_Max = intMapWidth - 1
        If intY_Min < 0 Then intY_Min = 0
        If intY_Max >= intMapHeight Then intY_Max = intMapHeight - 1
    End Sub

    Public Sub subCheckMapArrayLimits(ByRef intX As Integer, ByRef intY As Integer)
        If intY < 0 Then intY = 0
        If intY >= intMapHeight Then intY = intMapHeight - 1
        If intX < 0 Then intX = 0
        If intX >= intMapWidth Then intX = intMapWidth - 1
    End Sub



    Public Function ftnCheckMapArrayLimits(ByRef tmcMapCoordinate As typeMapCoordinate) As typeMapCoordinate
        Try
            ftnCheckMapArrayLimits = tmcMapCoordinate
            If ftnCheckMapArrayLimits.Y < 0 Then ftnCheckMapArrayLimits.Y = 0
            If ftnCheckMapArrayLimits.Y >= intMapHeight Then ftnCheckMapArrayLimits.Y = intMapHeight - 1
            If ftnCheckMapArrayLimits.X < 0 Then ftnCheckMapArrayLimits.X = 0
            If ftnCheckMapArrayLimits.X >= intMapWidth Then ftnCheckMapArrayLimits.X = intMapWidth - 1
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCheckMapArrayLimits :  " & ex.ToString)
        End Try
    End Function


    Public Function ftnGetLocalIPAddress() As String
        ' Author = Sam Malone 
        On Error Resume Next
        Dim h As System.Net.IPHostEntry = System.Net.Dns.GetHostEntry(System.Net.Dns.GetHostName)
        ftnGetLocalIPAddress = CType(h.AddressList.GetValue(0), System.Net.IPAddress).ToString
    End Function



    '=============================================  Map Locations  ====================================================================

    Public Function ftnLocationFromLocation(ByRef tmcStartingLocation As typeMapCoordinate, ByRef sngAngle As Single, ByVal sngDistance As Single) As typeMapCoordinate
        Try
            sngDistance = sngDistance
            ftnLocationFromLocation = tmcStartingLocation
            Dim sngAngleRadians = sngAngle * 0.017453
            ftnLocationFromLocation.X += Round(sngDistance * Math.Cos(sngAngleRadians))
            ftnLocationFromLocation.Y += Round(sngDistance * -Math.Sin(sngAngleRadians))
            ftnLocationFromLocation = ftnCheckMapArrayLimits(ftnLocationFromLocation)
        Catch ex As Exception
            ftnLocationFromLocation = tmcStartingLocation
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocationFromLocation")
            frmDebug.subWriteLogs(" tmcStartingLocation =  " & tmcStartingLocation.X & ",  " & tmcStartingLocation.Y)
            frmDebug.subWriteLogs(" sngDistance = " & sngDistance)
            frmDebug.subWriteLogs(" sngAngle = " & sngAngle)
        End Try
    End Function

    Public Function ftnLocationFromEstimatedPose(ByRef tbpEstimatedPose As typeBotPose, ByRef tmcRelativeOffset As typeMapCoordinate) As typeMapCoordinate
        Try
            Dim sngOrientationInRadians As Single = tbpEstimatedPose.Orientation * 0.017453
            Dim sngCos As Single = Math.Cos(sngOrientationInRadians)
            Dim sngSin As Single = Math.Sin(sngOrientationInRadians)
            ftnLocationFromEstimatedPose.X = tbpEstimatedPose.Location.X + Round(tmcRelativeOffset.X * sngCos - tmcRelativeOffset.Y * sngSin)
            ftnLocationFromEstimatedPose.Y = tbpEstimatedPose.Location.Y - Round(tmcRelativeOffset.Y * sngCos + tmcRelativeOffset.X * sngSin)
            ftnLocationFromEstimatedPose = ftnCheckMapArrayLimits(ftnLocationFromEstimatedPose)
        Catch ex As Exception
            ftnLocationFromEstimatedPose = tbpEstimatedPose.Location
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocationFromEstimatedPose  - tmcRelativeOffset:  " & ex.ToString)
        End Try
    End Function

    Public Function ftnLocationFromEstimatedPoseNoChk(ByRef tbpEstimatedPose As typeBotPose, ByRef tmcRelativeOffset As typeMapCoordinate) As typeMapCoordinate
        Try
            Dim sngOrientationInRadians As Single = tbpEstimatedPose.Orientation * 0.017453
            Dim sngCos As Single = Math.Cos(sngOrientationInRadians)
            Dim sngSin As Single = Math.Sin(sngOrientationInRadians)
            ftnLocationFromEstimatedPoseNoChk.X = tbpEstimatedPose.Location.X + Round(tmcRelativeOffset.X * sngCos - tmcRelativeOffset.Y * sngSin)
            ftnLocationFromEstimatedPoseNoChk.Y = tbpEstimatedPose.Location.Y - Round(tmcRelativeOffset.Y * sngCos + tmcRelativeOffset.X * sngSin)
        Catch ex As Exception
            ftnLocationFromEstimatedPoseNoChk = tbpEstimatedPose.Location
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocationFromEstimatedPoseNoChk :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnLocationFromEstimatedPose(ByRef tbpEstimatedPose As typeBotPose, ByRef trvRangeVector As typeRangeVector) As typeMapCoordinate
        Try
            ftnLocationFromEstimatedPose = tbpEstimatedPose.Location
            Dim sngRange As Single = trvRangeVector.sngRange
            Dim sngAbsoluteAngle As Single = ftnConvertTo180(tbpEstimatedPose.Orientation + trvRangeVector.sngAngle)
            Dim sngAbsoluteAngleRadians As Single = sngAbsoluteAngle * 0.017453
            ftnLocationFromEstimatedPose.X += Round(sngRange * Math.Cos(sngAbsoluteAngleRadians))
            ftnLocationFromEstimatedPose.Y += Round(sngRange * -Math.Sin(sngAbsoluteAngleRadians))
            ftnLocationFromEstimatedPose = ftnCheckMapArrayLimits(ftnLocationFromEstimatedPose)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocationFromEstimatedPose :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnLocationFromEstimatedPose(ByRef tbpEstimatedPose As typeBotPose, ByRef sngRelativeAngle As Single, ByRef sngDistance As Single) As typeMapCoordinate
        Try
            ftnLocationFromEstimatedPose = tbpEstimatedPose.Location
            Dim sngAbsoluteAngle As Single = ftnConvertTo180(tbpEstimatedPose.Orientation + sngRelativeAngle)
            Dim sngAbsoluteAngleRadians As Single = sngAbsoluteAngle * 0.017453
            ftnLocationFromEstimatedPose.X += Round(sngDistance * Math.Cos(sngAbsoluteAngleRadians))
            ftnLocationFromEstimatedPose.Y += Round(sngDistance * -Math.Sin(sngAbsoluteAngleRadians))
            ftnLocationFromEstimatedPose = ftnCheckMapArrayLimits(ftnLocationFromEstimatedPose)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocationFromEstimatedPose :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnLocationFromEstimatedPoseNC(ByRef tbpEstimatedPose As typeBotPose, ByRef sngRelativeAngle As Single, ByRef sngDistance As Single) As typeMapCoordinate
        Try
            ftnLocationFromEstimatedPoseNC = tbpEstimatedPose.Location
            Dim sngAbsoluteAngle As Single = ftnConvertTo180(tbpEstimatedPose.Orientation + sngRelativeAngle)
            Dim sngAbsoluteAngleRadians As Single = sngAbsoluteAngle * 0.017453
            ftnLocationFromEstimatedPoseNC.X += Round(sngDistance * Math.Cos(sngAbsoluteAngleRadians))
            ftnLocationFromEstimatedPoseNC.Y += Round(sngDistance * -Math.Sin(sngAbsoluteAngleRadians))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocationFromEstimatedPoseNC :  " & ex.ToString)
        End Try
    End Function


    Public Function ftnTmoLocationFromEstimatedPose(ByRef tbpEstimatedPose As typeBotPose, ByRef sngRelativeAngle As Single, ByRef sngDistance As Single) As typeMapOffset
        Try
            Dim sngAbsoluteAngle As Single = ftnConvertTo180(tbpEstimatedPose.Orientation + sngRelativeAngle)
            Dim sngAbsoluteAngleRadians As Single = sngAbsoluteAngle * 0.017453
            ftnTmoLocationFromEstimatedPose.X = Round(tbpEstimatedPose.Location.X + sngDistance * Math.Cos(sngAbsoluteAngleRadians))
            ftnTmoLocationFromEstimatedPose.Y = Round(tbpEstimatedPose.Location.Y + sngDistance * -Math.Sin(sngAbsoluteAngleRadians))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocationFromEstimatedPose :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnLocationFromEstimatedPose(ByRef tmoEstimatedLocation As typeMapOffset, ByRef sngOrientation As Single, ByRef sngRelativeAngle As Single, ByRef sngDistance As Single) As typeMapOffset
        Try
            ftnLocationFromEstimatedPose.X = tmoEstimatedLocation.X
            ftnLocationFromEstimatedPose.Y = tmoEstimatedLocation.Y
            Dim sngAbsoluteAngle As Single = ftnConvertTo180(sngOrientation + sngRelativeAngle)
            Dim sngAbsoluteAngleRadians As Single = sngAbsoluteAngle * 0.017453
            ftnLocationFromEstimatedPose.X += Round(sngDistance * Math.Cos(sngAbsoluteAngleRadians))
            ftnLocationFromEstimatedPose.Y += Round(sngDistance * -Math.Sin(sngAbsoluteAngleRadians))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocationFromEstimatedPose :  " & ex.ToString)
        End Try
    End Function


    Public Function ftnLocationsEqual(ByRef tmcLocation1 As typeMapCoordinate, ByRef tmcLocation2 As typeMapCoordinate) As Boolean
        ftnLocationsEqual = ((tmcLocation1.X = tmcLocation2.X) And (tmcLocation1.Y = tmcLocation2.Y))
    End Function

    Public Function ftnPosesEqual(ByRef tbpBotPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As Boolean
        Try
            ftnPosesEqual = ftnLocationsEqual(tbpBotPose.Location, tbpTargetPose.Location) And (tbpBotPose.Orientation = tbpTargetPose.Orientation)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnPosesEqual :  " & ex.ToString)
        End Try
    End Function


    Public Function ftnConvertToTMC(ByRef intX As Integer, ByRef intY As Integer) As typeMapCoordinate
        ftnConvertToTMC.X = intX
        ftnConvertToTMC.Y = intY
    End Function






    '====================================     Pose  =====================================================================

    Public Function ftnPoseUncertaintyFactor(ByRef tbpPose As typeBotPose) As Integer
        ftnPoseUncertaintyFactor = ((tbpPose.LocationUncertainty.X + tbpPose.LocationUncertainty.Y) * tbpPose.OrientationUncertainty)
    End Function

    Public Function ftnMinPoseUncertainty(ByRef tbpPoseIn As typeBotPose) As typeBotPose
        ftnMinPoseUncertainty = tbpPoseIn
        Try
            If boolHeroSelected Then
                If tbpPoseIn.OrientationUncertainty < 10 Then ftnMinPoseUncertainty.OrientationUncertainty = 15
                If tbpPoseIn.LocationUncertainty.Y < 10 Then ftnMinPoseUncertainty.LocationUncertainty.Y = 10
                If tbpPoseIn.LocationUncertainty.X < 10 Then ftnMinPoseUncertainty.LocationUncertainty.X = 10
            Else
                If tbpPoseIn.OrientationUncertainty < 10 Then ftnMinPoseUncertainty.OrientationUncertainty = 10
                If tbpPoseIn.LocationUncertainty.Y < 10 Then ftnMinPoseUncertainty.LocationUncertainty.Y = 10
                If tbpPoseIn.LocationUncertainty.X < 10 Then ftnMinPoseUncertainty.LocationUncertainty.X = 10
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnMinPoseUncertainty :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnRangeCorrectForHeight(ByRef tlmLandMark As typeLandMark, ByRef tirLandMark As typeImageRecognized) As Single
        On Error Resume Next
        ftnRangeCorrectForHeight = tirLandMark.sngRange
        If (tlmLandMark.intLandMarkHeight <> 0) Then  '  Correct range due to height difference between camera and LandMark
            ftnRangeCorrectForHeight = Sqrt(tirLandMark.sngRange * tirLandMark.sngRange - tlmLandMark.intLandMarkHeight * tlmLandMark.intLandMarkHeight)
        End If
    End Function


    Dim dblAvgPoseX As Double  ' used to keep a floating point representation of the average location.  Otherwise, the normal integer representation does not allow fine resolution with the averaging algorithm used  
    Dim dblAvgPoseY As Double
    Public Function ftnFindAvgPose(ByRef tbpAveragePose As typeBotPose,
                                   ByRef tbpNewEstimatedPose As typeBotPose, ByRef intParticleNumber As Integer,
                                   ByRef intPosesAveraged As Integer) As typeBotPose
        Try
            Dim sngEstimateAccuracyFactor As Single
            sngEstimateAccuracyFactor = (tbpAveragePose.LocationUncertainty.X + tbpAveragePose.LocationUncertainty.Y) /
                                        (tbpNewEstimatedPose.LocationUncertainty.X + tbpNewEstimatedPose.LocationUncertainty.Y)
            If (sngEstimateAccuracyFactor > 1) Then sngEstimateAccuracyFactor = 1
            intPosesAveraged += 1
            If (intPosesAveraged = 1) Then
                dblAvgPoseX = tbpAveragePose.Location.X
                dblAvgPoseY = tbpAveragePose.Location.Y
            End If
            Dim sngCurrentPoseFactor As Single = 1 + intPosesAveraged - sngEstimateAccuracyFactor
            dblAvgPoseX = (dblAvgPoseX * sngCurrentPoseFactor + tbpNewEstimatedPose.Location.X * sngEstimateAccuracyFactor) / (intPosesAveraged + 1)
            dblAvgPoseY = (dblAvgPoseY * sngCurrentPoseFactor + tbpNewEstimatedPose.Location.Y * sngEstimateAccuracyFactor) / (intPosesAveraged + 1)
            ftnFindAvgPose.Location.X = Round(dblAvgPoseX)
            ftnFindAvgPose.Location.Y = Round(dblAvgPoseY)
            ftnFindAvgPose.Location = ftnCheckMapArrayLimits(ftnFindAvgPose.Location)
            If (ftnInWall_NoChk(ftnFindAvgPose.Location, intParticleNumber)) Then ftnFindAvgPose.Location = tbpAveragePose.Location

            tbpAveragePose.Orientation = ftnConvertTo360(tbpAveragePose.Orientation)
            tbpNewEstimatedPose.Orientation = ftnConvertTo360(tbpNewEstimatedPose.Orientation)
            If ftnOrientationPointsRight(tbpAveragePose.Orientation) Then
                ftnFindAvgPose.Orientation = ftnConvertTo360((ftnConvertTo180(tbpAveragePose.Orientation) * sngCurrentPoseFactor + ftnConvertTo180(tbpNewEstimatedPose.Orientation) * sngEstimateAccuracyFactor) / (intPosesAveraged + 1))
            Else
                ftnFindAvgPose.Orientation = ftnConvertTo360((tbpAveragePose.Orientation * sngCurrentPoseFactor + ftnConvertTo360(tbpNewEstimatedPose.Orientation) * sngEstimateAccuracyFactor) / (intPosesAveraged + 1))
            End If

            ftnFindAvgPose.OrientationUncertainty = ftnConvertTo360((tbpAveragePose.OrientationUncertainty * sngCurrentPoseFactor + tbpNewEstimatedPose.OrientationUncertainty * sngEstimateAccuracyFactor) / (intPosesAveraged + 1))
            ftnFindAvgPose.LocationUncertainty.X = (tbpAveragePose.LocationUncertainty.X * sngCurrentPoseFactor + tbpNewEstimatedPose.LocationUncertainty.X * sngEstimateAccuracyFactor) / (intPosesAveraged + 1)
            ftnFindAvgPose.LocationUncertainty.Y = (tbpAveragePose.LocationUncertainty.Y * sngCurrentPoseFactor + tbpNewEstimatedPose.LocationUncertainty.Y * sngEstimateAccuracyFactor) / (intPosesAveraged + 1)
            ftnFindAvgPose = ftnMinPoseUncertainty(ftnFindAvgPose)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnFindAvgPose :  " & ex.ToString)
        End Try
    End Function




    '========================================================   Misc  =================================================================

    Public Function ftnVisualRecognizerEnabled() As Boolean
        ftnVisualRecognizerEnabled = boolEnableRcc
    End Function

    Public Function ftnVisualRecognizerConnected() As Boolean
        ftnVisualRecognizerConnected = boolEnableRcc And booRccConnected
    End Function

    Public Function ftnScannerConnected() As Boolean
        ftnScannerConnected = boolKinectConnected Or boolURGConnected Or boolBotConnected And boolHeroSelected
    End Function

    Public Function ftnConvertToTRV(ByRef sngRange As Single, ByRef sngAngle As Single) As typeRangeVector
        ftnConvertToTRV.sngRange = sngRange
        ftnConvertToTRV.sngAngle = sngAngle
    End Function

    Public Function ftnConvertToPose(ByRef tmcLocation As typeMapCoordinate, ByRef sngOrientation As Single) As typeBotPose
        ftnConvertToPose = tbpBotPose
        ftnConvertToPose.Location = tmcLocation
        ftnConvertToPose.Orientation = sngOrientation
    End Function

    Public Sub subFindStartEndAngles(ByRef tbpEstimatedPose As typeBotPose, ByRef intStartAngle As Integer, ByRef intEndAngle As Integer)
        Try
            intStartAngle = ftnConvertTo180(tbpEstimatedPose.Orientation - tbpEstimatedPose.OrientationUncertainty)
            intEndAngle = ftnConvertTo180(tbpEstimatedPose.Orientation + tbpEstimatedPose.OrientationUncertainty)
            If (intEndAngle < intStartAngle) Then intEndAngle += 360
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subFindStartEndAngles :  " & ex.ToString)
        End Try
    End Sub



    Public Sub subRecalculateVariables()
        Try
            sngBotRadius = sngBotDiameter / 2
            sngBotRadiusSquared = sngBotRadius * sngBotRadius
            intBotRadiusZoom = sngBotRadius * sngZoom
            intBotDiameterZoom = sngBotDiameter * sngZoom
            sngOneSigma = 6
            sngOneSigmaSquared = sngOneSigma * sngOneSigma
            sngOneDotFiveSigma = 1.5 * sngOneSigma
            intOneDotFiveSigmaSquared = sngOneDotFiveSigma * sngOneDotFiveSigma
            sngUnacceptableError = sngOneSigma * 0.6
            sngOkError = sngOneSigma * 0.4
            sngLowError = sngOneSigma * 0.25
            If boolHeroSelected Then
                intTotallySafeDistanceFromWallSquared = Round(sngBotRadius + 12) ^ 2
                intSafeDistanceFromWallSquared = Round(sngBotRadius + 7) ^ 2
                intSaferDistanceFromWallSquared = Round(sngBotRadius + 8) ^ 2
            ElseIf boolURGConnected Or boolKinectConnected Then
                intTotallySafeDistanceFromWallSquared = Round(sngBotRadius + 7) ^ 2
                intSafeDistanceFromWallSquared = Round(sngBotRadius + 5) ^ 2
                intSaferDistanceFromWallSquared = Round(sngBotRadius + 6) ^ 2
            Else
                intTotallySafeDistanceFromWallSquared = Round(sngBotRadius + 9) ^ 2
                intSafeDistanceFromWallSquared = Round(sngBotRadius + 6) ^ 2
                intSaferDistanceFromWallSquared = Round(sngBotRadius + 7) ^ 2
            End If
            subGenerateSquaredDistanceArray()
            sngMinCurveDistance = Max(1, 1.5 * sngCcDelayS * sngBotLinearVelocity / 2.54)
            subRecalculateBotSpeeds()
            subCalculateScanBeamAngles()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subRecalculateVariables :  " & ex.ToString)
        End Try
    End Sub

    Sub subGenerateSquaredDistanceArray()
        Dim intTotallySafeDistanceFromWall As Integer = Sqrt(intTotallySafeDistanceFromWallSquared)
        ReDim intSquareDistanceArray(intTotallySafeDistanceFromWall, intTotallySafeDistanceFromWall)
        Try
            For intX As Integer = 0 To intTotallySafeDistanceFromWall
                For intY As Integer = 0 To intTotallySafeDistanceFromWall
                    intSquareDistanceArray(intX, intY) = (intX * intX + intY * intY)
                Next intY
            Next intX
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subGenerateSquaredDistanceArray :  " & ex.ToString)
        End Try
    End Sub





    '----------------------------------------------- Line Calculations ------------------------------------------------------------------------------------------

    Dim dx, dy, e2, sx, sy, Err As Integer
    Public Function ftnFirstPointOnLine(ByRef tmcStartingPoint As typeMapCoordinate, ByRef tmcEndPoint As typeMapCoordinate) As typeMapCoordinate
        'Bresenham's line algorithm - see http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
        ' return the first point on a line defined by tmcStartingPoint and tmcEndPoint
        Try
            ftnFirstPointOnLine = tmcStartingPoint
            dx = Abs(tmcEndPoint.X - tmcStartingPoint.X)
            dy = Abs(tmcEndPoint.Y - tmcStartingPoint.Y)
            If tmcStartingPoint.X < tmcEndPoint.X Then sx = 1 Else sx = -1
            If tmcStartingPoint.Y < tmcEndPoint.Y Then sy = 1 Else sy = -1
            Err = dx - dy

            e2 = 2 * Err
            If e2 > -dy Then
                Err = Err - dy
                ftnFirstPointOnLine.X += sx
            End If
            If e2 < dx Then
                Err = Err + dx
                ftnFirstPointOnLine.Y += sy
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnFirstPointOnLine :  " & ex.ToString)
        End Try
    End Function


    Public Function ftnNextPointOnLine(ByRef tmcCurrentPoint As typeMapCoordinate) As typeMapCoordinate
        'Bresenham's line algorithm - see http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
        ' return the next point on a line defined by tmcCurrentPoint and tmcEndPoint
        Try
            ftnNextPointOnLine = tmcCurrentPoint
            e2 = 2 * Err
            If e2 > -dy Then
                Err = Err - dy
                ftnNextPointOnLine.X += sx
            End If
            If e2 < dx Then
                Err = Err + dx
                ftnNextPointOnLine.Y += sy
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnNextPointOnLine :  " & ex.ToString)
        End Try
    End Function



    ' ================================   Wall Calculations  =========================================================

    Public Function ftnDistanceFromWallErrorSquared(ByRef intParticleNumber As Integer, ByRef tmcVectorOrigin As typeMapCoordinate, ByRef trvVector As typeRangeVector,
                                             ByRef boolEndPointModel As Boolean, ByRef boolEndPointInGreySpace As Boolean) As Integer
        ' returns the square of the error between where the vector says there is a wall vs where the map says there is a wall
        '  NOTE:  boolEndPointModel = does not check for intervening walls
        Dim tmcCheckLocation, tmcEndPoint As typeMapCoordinate
        Try
            boolEndPointInGreySpace = False
            Dim sngRange As Single = trvVector.sngRange
            tmcEndPoint = tmcVectorOrigin
            Dim sngAngleRadians As Single = trvVector.sngAngle * 0.017453
            tmcEndPoint.X += Round(sngRange * Math.Cos(sngAngleRadians))
            tmcEndPoint.Y += Round(sngRange * -Math.Sin(sngAngleRadians))
            If tmcEndPoint.Y < 0 Or tmcEndPoint.Y >= intMapHeight Or tmcEndPoint.X < 0 Or tmcEndPoint.X >= intMapWidth Then
                ftnDistanceFromWallErrorSquared = sngOneSigmaSquared
            Else
                If boolEndPointModel Then
                    ftnDistanceFromWallErrorSquared = Min(intDistanceFromWallSquaredArray(intParticleNumber, tmcEndPoint.X, tmcEndPoint.Y), intOneDotFiveSigmaSquared) ' wall distance vs distance reported - squared
                Else ' Perform Ray Cast to check for intervening walls
                    tmcCheckLocation = ftnFirstPointOnLine(tmcVectorOrigin, tmcEndPoint)
                    While (Not ftnLocationsEqual(tmcCheckLocation, tmcEndPoint)) And
                          Not (boolOccupiedCell(0, tmcCheckLocation.X, tmcCheckLocation.Y))
                        tmcCheckLocation = ftnNextPointOnLine(tmcCheckLocation)
                    End While
                    If (boolOccupiedCell(0, tmcCheckLocation.X, tmcCheckLocation.Y)) Then
                        ftnDistanceFromWallErrorSquared = Min(Round((ftnDistanceBetweenLocations(tmcVectorOrigin, tmcCheckLocation) - trvVector.sngRange) ^ 2), intOneDotFiveSigmaSquared) ' 
                    Else
                        ftnDistanceFromWallErrorSquared = Min(intDistanceFromWallSquaredArray(intParticleNumber, tmcCheckLocation.X, tmcCheckLocation.Y), intOneDotFiveSigmaSquared) ' wall distance vs distance reported - squared
                    End If
                End If
                If Not boolSlam Then boolEndPointInGreySpace = boolGreySpace(tmcEndPoint.X, tmcEndPoint.Y)
                If boolEndPointInGreySpace Then
                    ftnDistanceFromWallErrorSquared *= 0.65
                ElseIf (sngRange > 65) Then
                    ftnDistanceFromWallErrorSquared *= 0.7
                End If
            End If
        Catch ex As Exception
            ftnDistanceFromWallErrorSquared = sngOneDotFiveSigma
            frmDebug.subWriteLogs(" EXCEPTION in ftnDistanceFromWallErrorSquared :  " & ex.ToString)
            frmDebug.subWriteLogs(" intParticleNumber =  " & intParticleNumber)
            frmDebug.subWriteCoordinate(" tmcVectorOrigin =  ", tmcVectorOrigin)
            frmDebug.subWriteVector(" trvVector = ", trvVector)
            frmDebug.subWriteCoordinate(" tmcEndPoint =  ", tmcEndPoint)
            frmDebug.subWriteCoordinate(" tmcCheckLocation =  ", tmcCheckLocation)
        End Try
    End Function

    Public Function ftnInWall_NoChk(ByRef tmcLocation As typeMapCoordinate, ByRef intParticleNumber As Integer) As Boolean
        Try
            ftnInWall_NoChk = (intDistanceFromWallSquaredArray(intParticleNumber, tmcLocation.X, tmcLocation.Y) < sngBotRadiusSquared)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnInWall_NoChk :  " & ex.ToString)
            frmDebug.subWriteLogs("                 tmcLocation = " & tmcLocation.X & ", " & tmcLocation.Y)
            ftnInWall_NoChk = True
        End Try
    End Function

    Public Function ftnInWall_NoChk1(ByRef tmcLocation As typeMapCoordinate) As Boolean
        Try
            ftnInWall_NoChk1 = (intDistanceFromWallSquaredArray(intBestParticle, tmcLocation.X, tmcLocation.Y) < sngBotRadiusSquared)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnInWall_NoChk :  " & ex.ToString)
            frmDebug.subWriteLogs("                 tmcLocation = " & tmcLocation.X & ", " & tmcLocation.Y)
            ftnInWall_NoChk1 = True
        End Try
    End Function







    '================================================  Movement ===========================================================

    Public Sub subRecalculateBotSpeeds()
        Try
            sngBotLinearAccelerationTime = sngBotLinearVelocity / sngBotLinearAcceleration
            sngBotLinearAccelerationDistance = ftnCmToInches(sngBotLinearAcceleration * sngBotLinearAccelerationTime * sngBotLinearAccelerationTime * 0.5)
            sngBotAngularAccelerationTime = sngBotAngularVelocity / sngBotAngularAcceleration
            sngBotAngularAccelerationDistance = (sngBotAngularAcceleration * sngBotAngularAccelerationTime * sngBotAngularAccelerationTime * 0.5)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subRecalculateBotSpeeds :  " & ex.ToString)
        End Try
    End Sub

    Function ftnCheckForStopMode() As Boolean
        Try
            ftnCheckForStopMode = boolStop
            If boolStop Then
                ftnSendCommand("play phrase Beep")
                frmDebug.subWriteLogs(" STOP mode ")
                strStatusDriving = " STOP mode"
                boolDock = False
                ftnSendCommand("play phraseA 'Please exit STOP mode first.'")
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCheckForStopMode :  " & ex.ToString)
        End Try
    End Function

    Public Sub subCheckForPause()
        Try
            While boolPause And Not boolStop
                strStatusDriving = " PAUSED"
                Thread.Sleep(250)
            End While
            strStatusDriving = " "
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subCheckForPause :  " & ex.ToString)
        End Try
    End Sub

    Public Function ftnDistanceCommanded(ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As Single
        Try
            ftnDistanceCommanded = ftnDistanceBetweenLocations(tbpStartingPose.Location, tbpTargetPose.Location)
            Dim intTurnAngle As Integer = ftnTurnAngleV2(tbpStartingPose, tbpTargetPose)
            Dim boolBackards As Boolean = (Abs(intTurnAngle) > 145)
            If boolBackards Then ftnDistanceCommanded = -ftnDistanceCommanded
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnDistanceCommanded :  " & ex.ToString)
        End Try
    End Function



    '================================================  Movement - New Move Variables ===========================================================

    Public Function ftnSetMoveInfo(ByRef tomTypeOfMove As TypeOfMove, ByRef tbpStartingPose As typeBotPose,
                                        ByRef tbpTargetPose As typeBotPose, ByRef boolNewMove As Boolean) As Boolean
        Try
            Dim timeOut As DateTime = Now().AddMilliseconds(2000)
            While boolSetMoveInfoBusy And (Now() < timeOut) And Not boolShutDown
                Thread.Sleep(15)
            End While
            If (Now() > timeOut) Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ********************  ftnSetMoveInfo: ERROR ERROR ERROR - Timeout")
            boolSetMoveInfoBusy = True
            If boolNewMove Then
                tmiMoveInfo.boolMoveCommandInProgress = True
                tmiMoveInfo.tbpStartingPose = tbpStartingPose
                tmiMoveInfo.tbpTargetPose = tbpTargetPose
                tmiMoveInfo.tomTypeOfMove = tomTypeOfMove

                tmiMoveInfo.boolCourseCorrection = tmiMoveInfo.boolBotIsMoving
                If tmiMoveInfo.boolBotIsMoving Then tbpBotPose = tbpStartingPose
                If boolEnableRBPF And Not tmiMoveInfo.boolCourseCorrection Then subUpdateStartPose()
                tmiMoveInfo.timeMoveCommandSent = Now()
                tmiMoveInfo.boolBotIsMoving = (tomTypeOfMove <> TypeOfMove.HALT)
            Else
                tmiMoveInfo.boolBotIsMoving = False  ' triggered by subBackgroundMonitor
            End If
            boolSetMoveInfoBusy = False
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnSetMoveInfo :  " & ex.ToString)
        End Try
        boolSetMoveInfoBusy = False
    End Function


    Public Function ftnGetMoveInfo() As typeMoveInfo
        Try
            Dim timeOut As DateTime = Now().AddMilliseconds(2000)
            While boolSetMoveInfoBusy And (Now() < timeOut) And Not boolShutDown
                Thread.Sleep(15)
            End While
            If (Now() >= timeOut) Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ********************  ftnGetMoveInfo: ERROR ERROR ERROR - Timeout")
            ftnGetMoveInfo = tmiMoveInfo
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnGetMoveInfo :  " & ex.ToString)
        End Try
    End Function

    Public Sub subDebugTmi()
        Try
            frmDebug.subWriteLogs(" subDebugTmi: tomTypeOfMove = " & tmiMoveInfo.tomTypeOfMove.ToString)
            frmDebug.subWriteLogs(" subDebugTmi: boolBotIsMoving = " & tmiMoveInfo.boolBotIsMoving.ToString)
            frmDebug.subWriteLogs(" subDebugTmi: boolMoveCommandInProgress = " & tmiMoveInfo.boolMoveCommandInProgress.ToString)
            frmDebug.subWriteLogs(" subDebugTmi: boolCourseCorrection = " & tmiMoveInfo.boolCourseCorrection.ToString)
            frmDebug.subWriteLogs(" subDebugTmi: timeMoveCommandSent = " & tmiMoveInfo.timeMoveCommandSent.ToString)
            frmDebug.subWriteShortPose("subDebugTmi: tbpStartingPose  ", tmiMoveInfo.tbpStartingPose)
            frmDebug.subWriteShortPose("subDebugTmi: tbpTargetPose  ", tmiMoveInfo.tbpTargetPose)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDebugTmi :  " & ex.ToString)
        End Try
    End Sub




    '================================================  Time  ===========================================================

    Delegate Sub DelegateWait(ByRef intMillseconds As Integer)
    Public Sub subWait(ByRef intMillseconds As Integer)
        Try
            If frmGotABotTop.InvokeRequired Then
                Dim newDelegate As New DelegateWait(AddressOf subWait)
                frmGotABotTop.Invoke(newDelegate, New Object() {intMillseconds})
            Else
                Dim TimeEnd As DateTime = Now().AddMilliseconds(intMillseconds)
                Do
                    Thread.Sleep(15)
                    subCheckForPause()
                    Application.DoEvents()
                Loop Until Now() > TimeEnd Or boolShutDown Or boolStop
                TimeEnd = Nothing
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subWait :  " & ex.ToString)
        End Try
    End Sub


    Public Function ftnElapsedTimeS(ByRef timeMoveCommandSent As DateTime) As Single ' returns elapsed time since timeMoveCommandSent in seconds
        Try
            ftnElapsedTimeS = Now().Subtract(timeMoveCommandSent).TotalSeconds()
            If (ftnElapsedTimeS < 0) Then ftnElapsedTimeS = 0
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnElapsedTimeS :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnElapsedTimeS() As Single ' returns elapsed time since start of move in seconds
        Try
            ftnElapsedTimeS = Now().Subtract(tmiMoveInfo.timeMoveCommandSent).TotalSeconds()
            If (ftnElapsedTimeS < 0) Then ftnElapsedTimeS = 0
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnElapsedTimeS :  " & ex.ToString)
        End Try
    End Function





    '================================================ Camera Calculations  ===========================================================

    Public Function ftnCameraAngleX(ByRef tirObject As typeImageRecognized) As Single
        ' Get the angle of an object relative to the camera. 
        ' EXAMPLE:  For a camera with a resolution of 640 x 480
        '   If intX = 320 then the angle TO THE CAMERA is 0 degrees
        '    intX = 0                     FOV/2 degrees
        '    intX = 640                  -FOV/2 degrees
        Try
            ftnCameraAngleX = Convert.ToSingle((ticCameraResolution.X \ 2 - tirObject.intPixelX) / ticCameraResolution.X * sngCameraFOV) ' Angle to Camera
        Catch ex As Exception
            ftnCameraAngleX = 0
            frmDebug.subWriteLogs(" EXCEPTION in ftnCameraAngleX :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnCameraAngleY(ByRef tirObject As typeImageRecognized) As Single
        ' Get the angle of an object relative to the camera. 
        ' EXAMPLE:  For a camera with a resolution of 640 x 480
        '   If intY = 240 then the angle TO THE CAMERA is 0 degrees
        '    intY = 0                     FOV/2 degrees
        '    intY = 480                  -FOV/2 degrees
        Try
            ftnCameraAngleY = Convert.ToSingle((ticCameraResolution.Y \ 2 - tirObject.intPixelY) / ticCameraResolution.X * sngCameraFOV) ' Angle to Camera
        Catch ex As Exception
            ftnCameraAngleY = 0
            frmDebug.subWriteLogs(" EXCEPTION in ftnCameraAngleY :  " & ex.ToString)
        End Try
    End Function

    Public boolMapLocked As Boolean = False
    Function ftnLockMap() As System.Drawing.Imaging.BitmapData
        Try
            Dim rect As New Rectangle(0, 0, intMapWidth, intMapHeight)
            Dim timeOut As DateTime = Now().AddMilliseconds(5000)
            While boolMapLocked And (Now() < timeOut) And Not boolShutDown
                Thread.Sleep(15)
            End While
            boolMapLocked = True
            If (Now() > timeOut) Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ********************  ftnLockMap: ERROR ERROR ERROR - Timeout")
            ftnLockMap = bmpMap.LockBits(rect, Drawing.Imaging.ImageLockMode.ReadWrite, bmpMap.PixelFormat)    ' Lock the bitmap's bits. 
        Catch ex As Exception
            boolMapLocked = False
            frmDebug.subWriteLogs(" EXCEPTION in ftnLockMap :  " & ex.ToString)
            Dim rect As New Rectangle(0, 0, intMapWidth, intMapHeight)
            ftnLockMap = bmpMap.LockBits(rect, Drawing.Imaging.ImageLockMode.ReadWrite, bmpMap.PixelFormat)    ' Lock the bitmap's bits. 
            boolMapLocked = True
        End Try
    End Function

    Sub subUnLockMap(ByRef bmpMapData As System.Drawing.Imaging.BitmapData)
        Try
            Dim timeOut As DateTime = Now().AddMilliseconds(5000)
            While Not boolMapLocked And (Now() < timeOut) And Not boolShutDown
                Thread.Sleep(15)
            End While
            If (Now() > timeOut) Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ********************  subUnLockMap: ERROR ERROR ERROR - Timeout")
            bmpMap.UnlockBits(bmpMapData)    ' Unlock the bits.
            boolMapLocked = False
        Catch ex As Exception
            boolMapLocked = True
            frmDebug.subWriteLogs(" EXCEPTION in subUnLockMap :  " & ex.ToString)
        End Try
    End Sub


End Module
