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

Module modPath
    Const constNearWall = 512

    '=================================================

    '  This module is responsible for:
    '    - operates on map array generated by the modNavigationArray module
    '    - Generates a path (shortest distance) between the bot and the destination
    '

    '=================================================

    '  Convert the map array to a Path Matrix where the Destination is assigned the lowest value (ie Cost) 
    '  and the robot's location is assigned a much higher value.  The shortest path between robot and Destination may 
    ' be found by following the gradient.  i.e. Starting at the Robot's location, find the neighboring cell with the lowest value.
    ' This will be the next cell on the path to the destination.  Repeat until the destination is reached.


    Sub subGeneratePathArray()
        '  This routine generates a matrix representing the obstacles in the map
        Try
            ReDim intPathArray(intMapWidth, intMapHeight)
            strStatusDriving = intDrivingSequenceNumber & " : Generating Path Matrix "
            Dim intX, intY As Integer
            Dim intDistanceFromWallSquaredCell As Integer
            For intX = 0 To intMapWidth - 0
                For intY = 0 To intMapHeight - 0
                    intDistanceFromWallSquaredCell = intDistanceFromWallSquaredArray(intBestParticle, intX, intY)
                    If boolOccupiedCell(0, intX, intY) Then
                        intPathArray(intX, intY) = 65535
                    ElseIf (intDistanceFromWallSquaredCell < sngBotRadiusSquared) Then ' 
                        intPathArray(intX, intY) = constMaxPathCost - 4096   '    ' 
                    ElseIf (intDistanceFromWallSquaredCell < intTotallySafeDistanceFromWallSquared) Then ' 
                        intPathArray(intX, intY) = Sqrt(intTotallySafeDistanceFromWallSquared - intDistanceFromWallSquaredCell)   '    ' 
                    ElseIf boolGreySpace(intX, intY) Then  ' 
                        intPathArray(intX, intY) = 8
                    Else
                        intPathArray(intX, intY) = 0
                    End If
                Next intY
            Next intX
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subGeneratePathArray :  " & ex.ToString)
        End Try
    End Sub

    Dim boolCostIsUsed(constMaxPathCost) As Boolean  ' speed up ftnFindPathToDestination by identifying unused intNextLocationCost values
    Dim intMaxThreshold As Integer
    Function ftnFindPathToDestination(ByRef tmcDestination As typeMapCoordinate, ByRef tmcStartLocation As typeMapCoordinate) As Boolean
        ' Find path to destination.  Returns true if a good path is found
        ftnFindPathToDestination = False
        Try
            strStatusDriving = intDrivingSequenceNumber & " : Finding Path to Destination "
            Dim intCurrentLocationCost As Integer = constDestinationCost + 1 ' the destination is the lowest cost. constDestinationCost = 4096
            intMaxThreshold = constNearWall
            If (ftnInWall_NoChk1(tbpBotPose.Location)) Then intMaxThreshold += 1
            ReDim boolCostIsUsed(constMaxPathCost) ' set all elements to False
            intPathArray(tmcDestination.X, tmcDestination.Y) = intCurrentLocationCost ' Start path at destination = lowest cost

            While (intPathArray(tmcStartLocation.X, tmcStartLocation.Y) < constDestinationCost) And Not boolStop _
                 And (intCurrentLocationCost <> constMaxPathCost)
                subUpdatePathStep(intCurrentLocationCost)
            End While
            If (intCurrentLocationCost = constMaxPathCost) Then '
                strStatusDriving = intDrivingSequenceNumber & " : Invalid Destination. -"
                If Not boolSlam Then ftnSendCommand("play phrase 'ERROR! Will Robinson, ERROR!'")
                ftnFindPathToDestination = False
            Else ' define more steps incase Bot goes far off path
                Dim i As Integer = 0
                While (intCurrentLocationCost <> constMaxPathCost) And (i < 64)
                    i += 1
                    subUpdatePathStep(intCurrentLocationCost)
                End While
                ftnFindPathToDestination = True
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnFindPathToDestination :  " & ex.ToString)
        End Try
    End Function

    Sub subUpdatePathStep(ByRef intCurrentLocationCost As Integer)
        Try
            Dim intNextLocationCost As Integer = intCurrentLocationCost + 1
            For intX As Integer = 1 To intMapWidth - 2 ' Work backward from the destination to the bot.  Each iteration (Location), add to the cost
                For intY As Integer = 1 To intMapHeight - 2
                    If ((intPathArray(intX, intY) = intCurrentLocationCost)) Then _
                        subUpdateCellNeighbors(intX, intY, intNextLocationCost)
                Next intY
            Next intX
            intCurrentLocationCost += 1
            While Not boolCostIsUsed(intCurrentLocationCost) And (intCurrentLocationCost <> constMaxPathCost)
                intCurrentLocationCost += 1
            End While
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subUpdatePathStep :  " & ex.ToString)
        End Try
    End Sub

    Sub subUpdateCellNeighbors(ByRef intX As Integer, ByRef intY As Integer, ByVal intNextLocationCost As Integer)
        '  Up/Dn and Left/Right neighbors of the selected cell are incremented by +1.
        '  Diagonal Neighbors are incremented by +2
        Try
            Dim intCellX, intCellY As Integer

            intCellX = intX
            For intCellY = (intY - 1) To (intY + 1) Step 2 ' up and down neighbors
                subUpdateCell(intCellX, intCellY, intNextLocationCost)
            Next intCellY
            intCellY = intY
            For intCellX = (intX - 1) To (intX + 1) Step 2 ' left and right neighbors
                subUpdateCell(intCellX, intCellY, intNextLocationCost)
            Next intCellX

            intNextLocationCost += 1  ' cost = 2 for diagonal neighbors
            For intCellX = (intX - 1) To (intX + 1) Step 2 ' diagonals
                For intCellY = (intY - 1) To (intY + 1) Step 2 ' 
                    subUpdateCell(intCellX, intCellY, intNextLocationCost)
                Next intCellY
            Next intCellX
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subUpdateCellNeighbors :  " & ex.ToString)
        End Try
    End Sub


    Private Sub subUpdateCell(ByRef intCellX As Integer, ByRef intCellY As Integer, ByRef intNextLocationCost As Integer)
        Try
            If (intPathArray(intCellX, intCellY) <= intMaxThreshold) Then  ' if intPathArray has not been previously set
                intPathArray(intCellX, intCellY) += intNextLocationCost
                boolCostIsUsed(intPathArray(intCellX, intCellY)) = True
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subUpdateCell :  " & ex.ToString)
            frmDebug.subWriteLogs(" intCellX =  " & intCellX)
            frmDebug.subWriteLogs(" intCellY =  " & intCellY)
        End Try
    End Sub

    Public Function ftnLowestValueNeighbor(ByRef tmcCurrentLocation As typeMapCoordinate) As typeMapCoordinate
        ' This function finds the next Location on the path as we move
        ' from bot to destination.  The next Location is the neighbor of tmcCurrentLocation
        ' which has the lowest intPathArray value).
        Try
            ftnLowestValueNeighbor = tmcCurrentLocation
            Dim tmcCandidateLocation As typeMapCoordinate
            Dim intLowestValue As Integer = intPathArray(ftnLowestValueNeighbor.X, ftnLowestValueNeighbor.Y)
            Dim intCandidateValue As Integer
            For tmcCandidateLocation.X = (tmcCurrentLocation.X - 1) To (tmcCurrentLocation.X + 1)
                For tmcCandidateLocation.Y = (tmcCurrentLocation.Y - 1) To (tmcCurrentLocation.Y + 1)
                    intCandidateValue = intPathArray(tmcCandidateLocation.X, tmcCandidateLocation.Y)
                    If (intCandidateValue < intLowestValue) Then  ' 
                        ftnLowestValueNeighbor = tmcCandidateLocation
                        intLowestValue = intCandidateValue
                    End If
                Next tmcCandidateLocation.Y
            Next tmcCandidateLocation.X
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLowestValueNeighbor :  " & ex.ToString)
            frmDebug.subWriteCoordinate("     tmcCurrentLocation  ", tmcCurrentLocation)
        End Try
    End Function






    '================================================================================================================================================================================
    '   The following functions are used to find the intermediate steps along the path from a start location
    '    to the Destination
    '================================================================================================================================================================================

    Function ftnFindTargetPose(ByVal tbpEstimatedPose As typeBotPose, ByRef tmcDestination As typeMapCoordinate) As typeBotPose
        ' This function searches for the furthest point on the path that it can safely reach using a 'MoveCurve' command or 
        ' an optional 'turn' command followed by a move 'fwd'.  
        ftnFindTargetPose = tbpEstimatedPose
        Try
            Dim tbpTargetPoseTurnAndFwd As typeBotPose = ftnFindTargetPoseTurnAndFwd(tbpEstimatedPose, tmcDestination) ' Find furthest target using turn and fwd moves
            Dim sngDistanceTurnAndFwd As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tbpTargetPoseTurnAndFwd.Location)
            Dim sngAngleTurnAndFwd As Single = ftnTurnAngle(tbpEstimatedPose, tbpTargetPoseTurnAndFwd.Location)
            Dim tmcTargetAfterTurnAndFwd As typeMapCoordinate = ftnLowestValueNeighbor(tbpTargetPoseTurnAndFwd.Location) ' 
            Dim sngAngleTurnAfterFwd As Single = ftnTurnAngle(tbpTargetPoseTurnAndFwd, tmcTargetAfterTurnAndFwd)

            Dim tbpTargetPoseCurve As typeBotPose = ftnFindTargetPoseCurve(tbpEstimatedPose, tmcDestination, tbpTargetPoseTurnAndFwd.Location) ' Find furthust target using curve moves
            Dim sngDistanceCurve As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tbpTargetPoseCurve.Location)
            Dim tmcTargetAfterCurve As typeMapCoordinate = ftnLowestValueNeighbor(tbpTargetPoseCurve.Location) ' 
            Dim sngAngleTurnAftercurve As Single = ftnTurnAngle(tbpTargetPoseCurve, tmcTargetAfterCurve)

            Dim boolCurveHasLessTurn As Boolean = (Abs(sngAngleTurnAftercurve) < 1.2 * (Abs(sngAngleTurnAndFwd) + Abs(sngAngleTurnAfterFwd)))
            If tmiMoveInfo.boolBotIsMoving And (sngDistanceCurve > 0) And ftnTargetRequiresTurn(tbpEstimatedPose, tbpTargetPoseTurnAndFwd.Location) Then
                ftnFindTargetPose = tbpTargetPoseCurve
            ElseIf (sngDistanceCurve > Min((sngDistanceTurnAndFwd + 18), 1.5 * sngDistanceTurnAndFwd)) Then
                ftnFindTargetPose = tbpTargetPoseCurve
            ElseIf (sngDistanceCurve > sngDistanceTurnAndFwd) And boolCurveHasLessTurn Then
                ftnFindTargetPose = tbpTargetPoseCurve
            ElseIf (sngDistanceCurve > (sngDistanceTurnAndFwd * 0.75)) _
                   And (ftnCurveArcLength(tbpEstimatedPose, tbpTargetPoseTurnAndFwd) < (sngDistanceTurnAndFwd * 1.5)) _
                   And boolCurveHasLessTurn Then
                ftnFindTargetPose = tbpTargetPoseCurve
            Else
                ftnFindTargetPose = tbpTargetPoseTurnAndFwd
            End If

            If boolDebugPath Then
                frmDebug.subWriteLogs(ftnElapsedTimeS() & " ======================= ftnFindTargetPose ==============================================")
                frmDebug.subWritePose(" ftnFindTargetPose: tbpEstimatedPose ", tbpEstimatedPose)
                frmDebug.subWriteLogs(" ftnFindTargetPose: sngDistanceTurnAndFwd " & sngDistanceTurnAndFwd)
                frmDebug.subWritePose(" ftnFindTargetPose:  tbpTargetPoseCurve", tbpTargetPoseCurve)
                frmDebug.subWriteLogs(" ftnFindTargetPose: sngDistanceCurve " & sngDistanceCurve)
                frmDebug.subWritePose(" ftnFindTargetPose:  ", ftnFindTargetPose)
                If (ftnTypeOfMove(tbpEstimatedPose, ftnFindTargetPose) = TypeOfMove.CURVE) Then
                    Dim sngRadius, sngCentralAngle, sngArcLength As Single
                    Dim boolClockwise As Boolean
                    Dim tmoCenterOfRotation As typeMapOffset
                    subDeriveCurveVariables(tbpEstimatedPose, ftnFindTargetPose, sngRadius, boolClockwise, tmoCenterOfRotation, sngCentralAngle, sngArcLength)
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnFindTargetPose :  " & ex.ToString)
        End Try
    End Function


    Function ftnFindTargetPoseTurnAndFwd(ByVal tbpEstimatedPose As typeBotPose, ByRef tmcDestination As typeMapCoordinate) As typeBotPose
        ftnFindTargetPoseTurnAndFwd = tbpEstimatedPose ' Find the furthust location reachable with a Turn followed by a Forward command
        Try
            Dim boolClearPath As Boolean = False
            Dim tbpFurthestSafePose As typeBotPose = tbpEstimatedPose
            Dim tbpPoseAfterTurn As typeBotPose = tbpEstimatedPose
            Do           'see if we can go furthur without hitting a wall
                tbpFurthestSafePose = ftnFindTargetPoseTurnAndFwd
                ftnFindTargetPoseTurnAndFwd.Location = ftnLowestValueNeighbor(ftnFindTargetPoseTurnAndFwd.Location)

                tbpPoseAfterTurn.Orientation = ftnP1ToP2Angle(tbpEstimatedPose.Location, ftnFindTargetPoseTurnAndFwd.Location) ' Orientation required to drive from tbpEstimatedPose.Location to ftnFindTargetPoseTurnAndFwd
                ftnFindTargetPoseTurnAndFwd.Orientation = tbpPoseAfterTurn.Orientation
                boolClearPath = ftnFwdSafePath(tbpPoseAfterTurn, ftnFindTargetPoseTurnAndFwd.Location) ' can we drive forward to this location without coming too close to a wall?
            Loop Until boolStop Or Not boolClearPath Or ftnLocationsEqual(ftnFindTargetPoseTurnAndFwd.Location, tmcDestination)
            ftnFindTargetPoseTurnAndFwd = tbpFurthestSafePose
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnFindTargetPoseTurnAndFwdTurnAndFwd :  " & ex.ToString)
        End Try
    End Function

    Function ftnFindTargetPoseCurve(ByVal tbpEstimatedPose As typeBotPose, ByRef tmcDestination As typeMapCoordinate, ByRef tmcTurnAndFwd As typeMapCoordinate) As typeBotPose
        ftnFindTargetPoseCurve = tbpEstimatedPose
        Try
            Dim boolClearPath As Boolean = False
            Dim tbpFurthestSafePose As typeBotPose = tbpEstimatedPose
            Dim tmcCheck As typeMapCoordinate = tmcTurnAndFwd  ' Point on the shortest path to destination.  This function checks if a Curve path can reach it
            boolClearPath = False
            tbpFurthestSafePose = tbpEstimatedPose
            Dim sngCheckDistance As Single = 0

            boolClearPath = ftnCurveSafePath(tbpEstimatedPose, tmcTurnAndFwd)
            If Not boolClearPath Then tmcCheck = tbpEstimatedPose.Location
            Do
                If boolClearPath Then tbpFurthestSafePose.Location = tmcCheck
                tmcCheck = ftnLowestValueNeighbor(tmcCheck)
                If Not ftnLocationsEqual(tmcCheck, tmcDestination) Then _
                    tmcCheck = ftnLowestValueNeighbor(tmcCheck)
                boolClearPath = ftnCurveSafePath(tbpEstimatedPose, ftnFindTargetPoseCurve.Location)
                If boolClearPath Then
                    sngCheckDistance = 0
                Else
                    sngCheckDistance += 1
                End If
            Loop Until (Not boolClearPath And (sngCheckDistance > 15) Or ftnLocationsEqual(tmcCheck, tmcDestination))
            ftnFindTargetPoseCurve = tbpFurthestSafePose
            If boolDebugPath Then
                frmDebug.subWriteLogs(" ===================================  ftnFindTargetPoseCurve End ============================")
                frmDebug.subWriteLogs(": : tbpEstimatedPose= " & tbpEstimatedPose.Location.X & ", " & tbpEstimatedPose.Location.Y & "  at " & tbpEstimatedPose.Orientation)
                frmDebug.subWriteLogs(": : tmcDestination= " & tmcDestination.X & ", " & tmcDestination.Y)
                frmDebug.subWriteLogs(": : ftnFindTargetPoseCurve= " & ftnFindTargetPoseCurve.Location.X & ", " & ftnFindTargetPoseCurve.Location.Y & "  at " & ftnFindTargetPoseCurve.Orientation)
                frmDebug.subWriteLogs(": : ftnCurveSafePath = " & ftnCurveSafePath(tbpEstimatedPose, ftnFindTargetPoseCurve.Location))
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnFindTargetPoseCurve :  " & ex.ToString)
        End Try
    End Function


    Function ftnTypeOfMove(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As TypeOfMove
        Try
            If ftnFwdSafePath(tbpEstimatedPose, tbpTargetPose.Location) Then
                ftnTypeOfMove = TypeOfMove.FWD
            ElseIf ftnCurveSafePath(tbpEstimatedPose, tbpTargetPose.Location) Then
                ftnTypeOfMove = TypeOfMove.CURVE
            ElseIf Not tmiMoveInfo.boolBotIsMoving And (Abs(ftnTurnAngle(tbpEstimatedPose, tbpTargetPose.Location)) > 1) Then
                ftnTypeOfMove = TypeOfMove.TURN
            Else
                ftnTypeOfMove = TypeOfMove.HALT
            End If
        Catch ex As Exception
            ftnTypeOfMove = TypeOfMove.TURN
            frmDebug.subWriteLogs(" EXCEPTION in ftnTypeOfMove :  " & ex.ToString)
        End Try
    End Function


    Function ftnTargetRequiresTurn(ByRef tbpEstimatedPose As typeBotPose, ByRef tmcTargetLocation As typeMapCoordinate) As Boolean
        Try
            ftnTargetRequiresTurn = False
            Dim tbpTargetPose As typeBotPose = tbpEstimatedPose
            tbpTargetPose.Location = tmcTargetLocation
            Dim sngTurnAngle As Single = ftnTurnAngleV2(tbpEstimatedPose, tbpTargetPose)
            If ftnLocationsEqual(tbpEstimatedPose.Location, tmcTargetLocation) Then
                ftnTargetRequiresTurn = True
            Else
                Dim sngDistanceToTarget As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tmcTargetLocation)
                Dim sngOffsetDistance As Single = sngDistanceToTarget * Abs(Sin(ftnDegreeToRadian(sngTurnAngle)))
                If (sngOffsetDistance > 3) Then
                    ftnTargetRequiresTurn = True
                ElseIf Abs(sngTurnAngle) > 10 Then
                    ftnTargetRequiresTurn = True
                ElseIf Not boolSlam And (sngOffsetDistance > 2) Then
                    ftnTargetRequiresTurn = True
                ElseIf Not boolSlam And (sngOffsetDistance >= 1) Then
                    Dim tmcNewTargetLocation As typeMapCoordinate = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, sngDistanceToTarget)
                    If Not ftnFwdSafePathNoTurnCheck(tbpEstimatedPose, tmcNewTargetLocation) Then _
                      ftnTargetRequiresTurn = True
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnTargetRequiresTurn :  " & ex.ToString)
        End Try
    End Function


    Function ftnFwdSafePath(ByRef tbpEstimatedPose As typeBotPose, ByRef tmcTargetLocation As typeMapCoordinate) As Boolean
        ' this function returns true if the path between tbpEstimatedPose.Location and tmcTargetLocation is clear of obstructions
        ' and not near a wall
        Try
            ftnFwdSafePath = True
            If ftnTargetRequiresTurn(tbpEstimatedPose, tmcTargetLocation) Then
                ftnFwdSafePath = False
            Else
                Dim sngDistanceToTarget As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tmcTargetLocation)
                If (sngDistanceToTarget > (sngBotRadius + 2)) Then
                    Dim tmcCheckLocation As typeMapCoordinate = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, sngBotRadius) ' tbpEstimatedPose.Location 
                    tmcCheckLocation = ftnFirstPointOnLine(tmcCheckLocation, tmcTargetLocation)
                    Do
                        tmcCheckLocation = ftnNextPointOnLine(tmcCheckLocation)
                        ftnFwdSafePath = ftnSafeLocationForMove(tbpEstimatedPose, tmcCheckLocation, sngDistanceToTarget, (ftnPoseUncertaintyFactor(tbpEstimatedPose) > 300))
                    Loop Until ftnLocationsEqual(tmcCheckLocation, tmcTargetLocation) Or boolStop Or Not ftnFwdSafePath
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnFwdSafePath :  " & ex.ToString)
        End Try
    End Function

    Function ftnFwdSafePathNoTurnCheck(ByRef tbpEstimatedPose As typeBotPose, ByRef tmcTargetLocation As typeMapCoordinate) As Boolean
        ' this function returns true if the path between tbpEstimatedPose.Location and tmcTargetLocation is clear of obstructions
        ' and not near a wall
        Try
            Dim sngDistanceToTarget As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tmcTargetLocation)
            ftnFwdSafePathNoTurnCheck = (sngDistanceToTarget < sngMaxTravel)
            If ftnFwdSafePathNoTurnCheck And (sngDistanceToTarget > sngBotRadius) Then
                Dim tmcCheckLocation As typeMapCoordinate = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, sngBotRadius) ' tbpEstimatedPose.Location 
                tmcCheckLocation = ftnFirstPointOnLine(tmcCheckLocation, tmcTargetLocation)
                Do
                    tmcCheckLocation = ftnNextPointOnLine(tmcCheckLocation)
                    tmcCheckLocation = ftnNextPointOnLine(tmcCheckLocation)
                    ftnFwdSafePathNoTurnCheck = ftnSafeLocationForMove(tbpEstimatedPose, tmcCheckLocation, sngDistanceToTarget, False)
                Loop Until ftnLocationsEqual(tmcCheckLocation, tmcTargetLocation) Or boolStop Or Not ftnFwdSafePathNoTurnCheck
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnFwdSafePathNoTurnCheck :  " & ex.ToString)
        End Try
    End Function



    Function ftnCurveSafePath(ByRef tbpStartingPose As typeBotPose, ByRef tmcTargetLocation As typeMapCoordinate) As Boolean
        ' this function returns true if the path between tbpStartingPose and tmcTargetLocation is clear of obstructions
        ' and not near a wall
        Try
            ftnCurveSafePath = False
            If boolER1Selected And Not boolSlam Then
                Dim sngDistance As Single = ftnDistanceBetweenLocations(tbpStartingPose.Location, tmcTargetLocation)
                Dim intAbsTurnAngle As Integer = Max(Abs(ftnTurnAngle(tbpStartingPose, tmcTargetLocation)), 0.01)
                If ((sngDistance > sngMinCurveDistance) And intAbsTurnAngle <= 90) Then
                    Dim tbpTargetPose As typeBotPose = tbpStartingPose
                    tbpTargetPose.Location = tmcTargetLocation
                    Dim sngRadius, sngCentralAngle, sngArcLength As Single
                    Dim boolClockwise As Boolean
                    Dim tmoCenterOfRotation As typeMapOffset
                    Dim tbpCheckPose As typeBotPose
                    Dim boolClearPath As Boolean = False
                    subDeriveCurveVariables(tbpStartingPose, tbpTargetPose, sngRadius, boolClockwise, tmoCenterOfRotation, sngCentralAngle, sngArcLength)
                    tbpTargetPose = ftnCurveMotionDR(tbpStartingPose, sngArcLength, sngRadius, boolClockwise, tmoCenterOfRotation, sngCentralAngle, sngArcLength, False)
                    tbpTargetPose.Location = tmcTargetLocation
                    If (Abs(ftnAngleDifference(tbpStartingPose.Orientation, tbpTargetPose.Orientation)) / sngDistance < 5) Then  ' max allowable orientation change of 5 degrees per inch of travel
                        If (sngArcLength < sngMaxTravel) And (Abs(ftnAngleDifference(tbpStartingPose.Orientation, tbpTargetPose.Orientation)) < 120) Then
                            Dim sngDistanceFromStart As Single = sngMinCurveDistance
                            Do
                                tbpCheckPose = ftnCurveMotionDR(tbpStartingPose, sngDistanceFromStart, sngRadius, boolClockwise, tmoCenterOfRotation, sngCentralAngle, sngArcLength, False)
                                boolClearPath = ftnSafeLocationForMove(tbpStartingPose, tbpCheckPose.Location, sngDistanceFromStart, tmiMoveInfo.boolBotIsMoving)
                                sngDistanceFromStart = Min(sngDistanceFromStart + 4, sngArcLength)
                            Loop Until (sngDistanceFromStart >= sngArcLength) Or boolStop Or Not boolClearPath
                            ftnCurveSafePath = boolClearPath
                        End If
                    End If
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCurveSafePath :  " & ex.ToString)
        End Try
    End Function



    Function ftnSafeLocationForMove(ByRef tbpStartPose As typeBotPose, ByRef tmcCheckLocation As typeMapCoordinate, ByRef sngDistanceFromStart As Single, ByRef boolUseSaferCriteria As Boolean) As Boolean
        Try
            ftnSafeLocationForMove = Not boolNoGo(tmcCheckLocation.X, tmcCheckLocation.Y)  ' boolNoGo set by purple map pixels
            If ftnSafeLocationForMove Then
                Dim intCellDistanceFromWallSquared As Integer = intDistanceFromWallSquaredArray(intBestParticle, tmcCheckLocation.X, tmcCheckLocation.Y)
                If boolURGConnected Or boolKinectConnected Then
                    If (intCellDistanceFromWallSquared < intSaferDistanceFromWallSquared) Then
                        If (intCellDistanceFromWallSquared >= intSafeDistanceFromWallSquared) Then
                            ftnSafeLocationForMove = True
                        ElseIf (sngDistanceFromStart < sngBotDiameter) Then
                            ftnSafeLocationForMove = (intCellDistanceFromWallSquared >= intDistanceFromWallSquaredArray(intBestParticle, tbpStartPose.Location.X, tbpStartPose.Location.Y))
                        Else
                            ftnSafeLocationForMove = False
                        End If
                    End If
                Else
                    If (intCellDistanceFromWallSquared < intSaferDistanceFromWallSquared) Then
                        If (sngDistanceFromStart >= 60) Then
                            ftnSafeLocationForMove = False
                        ElseIf (intCellDistanceFromWallSquared >= intSaferDistanceFromWallSquared) Then
                            ftnSafeLocationForMove = True
                        ElseIf (sngDistanceFromStart < 36) And (intCellDistanceFromWallSquared >= intSafeDistanceFromWallSquared) Then
                            ftnSafeLocationForMove = True
                        ElseIf (sngDistanceFromStart < sngBotDiameter) Then
                            ftnSafeLocationForMove = (intCellDistanceFromWallSquared >= intDistanceFromWallSquaredArray(intBestParticle, tbpStartPose.Location.X, tbpStartPose.Location.Y))
                        Else
                            ftnSafeLocationForMove = False
                        End If
                    End If
                End If
            End If
        Catch ex As Exception
            ftnSafeLocationForMove = False
            frmDebug.subWriteLogs(" EXCEPTION in ftnSafeLocationForMove :  " & ex.ToString)
            frmDebug.subWriteLogs(":  tbpStartPose= " & tbpStartPose.Location.X & ", " & tbpStartPose.Location.Y & "  at " & tbpStartPose.Orientation)
            frmDebug.subWriteLogs(":  tmcCheckLocation= " & tmcCheckLocation.X & ", " & tmcCheckLocation.Y)
            frmDebug.subWriteLogs(":  sngDistanceFromStart= " & sngDistanceFromStart)
        End Try
    End Function

End Module
