
Imports System.Math
Imports System.Threading
Imports System.Drawing

Public Module modObstacle

    Dim boolCollisionPredictAndDetectBusy As Boolean = False
    Dim sngLoopTimeS As Single = 0.45
    Public Sub subCollisionPredictAndDetect()
        Try
            If boolDebugObstacle Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ======================= subCollisionPredictAndDetect:  boolCollisionPredictAndDetectBusy = " & boolCollisionPredictAndDetectBusy)
            If boolCollisionDetectionEnable And Not boolCollisionPredictAndDetectBusy Then
                boolCollisionPredictAndDetectBusy = True
                Dim trvObstacleVector As typeRangeVector
                Dim sngDistanceToObstacle As Single
                Dim sngSafeDistance As Single = 6 * ftnCmToInches(sngBotLinearVelocity * sngLoopTimeS) + sngBotRadius
                Dim boolValidLocation As Boolean = False
                Dim sngMaxDistanceToTravel As Single
                Dim tmiMoveInfoLocal As typeMoveInfo
                Dim timeLoopStart As DateTime = Now()
                timeLoopStart = Now()
                tmiMoveInfoLocal = ftnGetMoveInfo()
                sngMaxDistanceToTravel = ftnDistanceBetweenLocations(tmiMoveInfoLocal.tbpTargetPose.Location, tmiMoveInfoLocal.tbpStartingPose.Location)
                Dim sngDistanceLeftToTravel As Single = Min(ftnDistanceBetweenLocations(tmiMoveInfoLocal.tbpTargetPose.Location, tbpBotPose.Location), sngMaxDistanceToTravel)
                trvObstacleVector = ftnObstacleVectorTop(tbpBotPose, tmiMoveInfoLocal.tomTypeOfMove, tmiMoveInfoLocal.tbpStartingPose, tmiMoveInfoLocal.tbpTargetPose)
                sngDistanceToObstacle = Min(ftnObstacleForwardDistance(trvObstacleVector), sngMaxDistanceToTravel)
                If boolDebugObstacle And tmiMoveInfoLocal.boolBotIsMoving Then
                    frmDebug.subWriteVector("     subCollisionPredictAndDetect : trvObstacleVector = ", trvObstacleVector)
                    frmDebug.subWriteLogs("     subCollisionPredictAndDetect:  - sngDistanceToObstacle " & sngDistanceToObstacle)
                    frmDebug.subWriteLogs("     subCollisionPredictAndDetect:  - sngDistanceLeftToTravel " & sngDistanceLeftToTravel)
                    frmDebug.subWriteLogs("     subCollisionPredictAndDetect:  - sngSafeDistance " & sngSafeDistance)
                End If
                If (trvObstacleVector.sngRange < sngBotRadius) And boolCollisionDetectionEnable Then ' Collision!
                    'Case "left" : byteCollisionCode = 5
                    'Case "right" : byteCollisionCode = 6
                    'Case "front" : byteCollisionCode = 4
                    'Case "rear" : byteCollisionCode = 3
                    If (trvObstacleVector.sngAngle >= -20) And (trvObstacleVector.sngAngle <= 20) Then
                        tbpBotPose = ftnCollisionRecovery(tbpBotPose, 4)
                    ElseIf (trvObstacleVector.sngAngle >= 0) And (trvObstacleVector.sngAngle <= 90) Then
                        tbpBotPose = ftnCollisionRecovery(tbpBotPose, 5)
                    ElseIf (trvObstacleVector.sngAngle <= 0) And (trvObstacleVector.sngAngle >= -90) Then
                        tbpBotPose = ftnCollisionRecovery(tbpBotPose, 6)

                        'ElseIf (Abs(trvVectorFromBotCenter.sngAngle) > 90) And (ftnScannerCollisionCode = 7) Then
                        '    ftnScannerCollisionCode = 3
                    End If
                ElseIf ftnScannerConnected() And (tmiMoveInfoLocal.tomTypeOfMove = TypeOfMove.FWD) Or (tmiMoveInfoLocal.tomTypeOfMove = TypeOfMove.CURVE) Then
                    If (sngDistanceToObstacle < (sngDistanceLeftToTravel - 2)) And (sngDistanceToObstacle < sngSafeDistance) Then ' We're going to hit an obstacle
                        subCollisionPrevent()
                        sngLoopTimeS = (sngLoopTimeS + Now().Subtract(timeLoopStart).TotalSeconds()) / 2
                        sngSafeDistance = 6 * ftnCmToInches(sngBotLinearVelocity * sngLoopTimeS) + sngBotRadius
                        While tmiMoveInfo.boolBotIsMoving And Not boolStop
                            Thread.Sleep(25)
                        End While
                        frmDebug.subWriteVector(ftnElapsedTimeS() & " /// Collision was Predicted: Range = ", trvObstacleVector)
                        If boolDebugObstacle Then
                            frmDebug.subWriteLogs(Now() & " subCollisionPredictAndDetect: sngLoopTimeS = " & sngLoopTimeS)
                            frmDebug.subWriteLogs(Now() & " subCollisionPredictAndDetect: sngDistanceLeftToTravel = " & sngDistanceLeftToTravel & " sngDistanceToObstacle =  " & sngDistanceToObstacle)
                            frmDebug.subWriteLogs(Now() & " subCollisionPredictAndDetect:  sngMaxDistanceToTravel =  " & sngMaxDistanceToTravel & " sngSafeDistance =  " & sngSafeDistance)
                        End If
                    End If
                ElseIf boolER1Selected And boolEnableBumperSwitches_ER1 Then
                    tbpBotPose = ftnCollisionRecovery(tbpBotPose, Val(ftnSendCommand("bumper")))
                End If
                boolCollisionPredictAndDetectBusy = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subCollisionPredictAndDetect :  " & ex.ToString)
        End Try
    End Sub


    Delegate Sub DelegateCollisionPredicted()
    Sub subCollisionPrevent()  ' Stop if collision predicted
        Try
            If frmGotABotTop.InvokeRequired Then
                Dim newDelegate As New DelegateCollisionPredicted(AddressOf subCollisionPrevent)
                frmGotABotTop.Invoke(newDelegate)
            Else
                If tmiMoveInfo.boolBotIsMoving Then
                    tbpBotPose = ftnLocalizeUsingDeadReckoning(tbpBotPose, -0.2, True)
                    If tmiMoveInfo.boolBotIsMoving Then tbpBotPose.OrientationUncertainty += 15
                    ftnBotStop(tbpBotPose, tbpBotPose)
                End If
            End If ' GotABotClass.InvokeRequired
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subCollisionPrevent :  " & ex.ToString)
        End Try
    End Sub

    '============================================   Obstacle Detection ===================all=======================================================
    Public Function ftnObstacleDetected(ByRef tbpEstimatedPose As typeBotPose, ByRef tomTypeOfMove As TypeOfMove, ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As Boolean
        Try
            ftnObstacleDetected = False
            If ((tomTypeOfMove = TypeOfMove.FWD) Or (tomTypeOfMove = TypeOfMove.CURVE)) And Not boolSimulatedMovement Then  '
                Dim sngFwdDistance As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tbpTargetPose.Location)
                Dim trvObstacleVector As typeRangeVector = ftnObstacleVectorTop(tbpEstimatedPose, tomTypeOfMove, tbpStartingPose, tbpTargetPose)
                Dim sngObstacleFwdDistance As Single = ftnObstacleForwardDistance(trvObstacleVector)
                ftnObstacleDetected = (sngObstacleFwdDistance < sngFwdDistance)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnObstacleDetected :  " & ex.ToString)
        End Try
    End Function



    Public Function ftnObstacleVectorTop(ByRef tbpEstimatedPose As typeBotPose, ByRef tomTypeOfMove As TypeOfMove, ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As typeRangeVector ' returns a vector ( relative to bot center and orientation) of nearest obstacle.
        '  checksScanners and the map for obstacles
        Try
            ftnObstacleVectorTop.sngAngle = 0
            ftnObstacleVectorTop.sngRange = 99999
            If boolDebugObstacle And tmiMoveInfo.boolBotIsMoving Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " =========================== ftnObstacleVectorTop:  START")
            If (tomTypeOfMove <> TypeOfMove.TURN) Then  ' 
                Dim sngFwdDistance As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tbpTargetPose.Location) - sngBotRadius
                If ftnScannerConnected() Then
                    Dim trvScannerVector As typeRangeVector = ftnScannerObstacleVector(tbpEstimatedPose, tbpStartingPose, tbpTargetPose, tomTypeOfMove)
                    If (ftnObstacleForwardDistance(trvScannerVector) < sngFwdDistance) Then
                        ftnObstacleVectorTop = trvScannerVector
                        subDebugObstacleVector(tbpEstimatedPose, "Scanner", ftnObstacleVectorTop, tomTypeOfMove, tbpStartingPose, tbpTargetPose)
                    End If
                Else
                    Dim trvMapVector As typeRangeVector = ftnMapObstacleVector(tbpEstimatedPose, tbpTargetPose, tbpStartingPose, tomTypeOfMove)
                    If (ftnObstacleForwardDistance(trvMapVector) < sngFwdDistance) Then
                        ftnObstacleVectorTop = trvMapVector
                        subDebugObstacleVector(tbpEstimatedPose, "Map", ftnObstacleVectorTop, tomTypeOfMove, tbpStartingPose, tbpTargetPose)
                    End If
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnObstacleVectorTop :  " & ex.ToString)
            ftnObstacleVectorTop.sngAngle = 0
            ftnObstacleVectorTop.sngRange = 99999
        End Try
    End Function

    Sub subDebugObstacleVector(ByRef tbpEstimatedPose As typeBotPose, ByRef strSource As String, ByRef trvObstacleVector As typeRangeVector, ByRef tomTypeOfMove As TypeOfMove, ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose)
        Try
            If boolDebugObstacle Then
                frmDebug.subWriteLogs("   OBSTACLE DETECTED By " & strSource)
                frmDebug.subWritePose("       tbpEstimatedPose = ", tbpEstimatedPose)
                frmDebug.subWritePose("       tbpStartingPose = ", tbpStartingPose)
                frmDebug.subWritePose("       tbpTargetPose = ", tbpTargetPose)
                frmDebug.subWriteLogs("       tomTypeOfMove = " & tomTypeOfMove.ToString)
                frmDebug.subWriteVector("        Obstacle Vector  = ", trvObstacleVector)
                Dim tmcObstacleLocation As typeMapCoordinate = ftnLocationFromEstimatedPose(tbpEstimatedPose, trvObstacleVector.sngAngle, trvObstacleVector.sngRange)
                frmDebug.subWriteCoordinate("           Obstacle Location = ", tmcObstacleLocation)
                Dim sngFwdDistance As Single = ftnObstacleForwardDistance(trvObstacleVector)
                frmDebug.subWriteLogs("           sngFwdDistance = " & sngFwdDistance)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDebugObstacleVector :  " & ex.ToString)
        End Try
    End Sub

    Function ftnMapObstacleVector(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpTargetPose As typeBotPose, ByRef tbpStartingPose As typeBotPose, ByRef tomTypeOfMove As TypeOfMove) As typeRangeVector  ' Check the map for obstacles
        Try
            ftnMapObstacleVector.sngRange = 9999
            ftnMapObstacleVector.sngAngle = 0
            If Not boolSlam Then
                Dim tmcCheckLocationCenter As typeMapCoordinate
                Dim sngDistanceToTarget As Single = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tbpTargetPose.Location) - sngBotRadius
                Dim sngDistanceFromStart As Single = sngBotRadius
                ftnMapObstacleVector.sngAngle = 0
                Do
                    If (tomTypeOfMove = TypeOfMove.FWD) Then
                        tmcCheckLocationCenter = ftnLocationFromEstimatedPose(tbpEstimatedPose, 0, sngDistanceFromStart)
                    ElseIf (tomTypeOfMove = TypeOfMove.CURVE) Then
                        tmcCheckLocationCenter = ftnCurve_DR(tbpStartingPose, sngDistanceFromStart, tbpTargetPose, False).Location
                    End If
                    If ftnInWall_NoChk1(tmcCheckLocationCenter) Then ftnMapObstacleVector.sngRange = sngDistanceFromStart
                    sngDistanceFromStart += 2
                Loop Until (sngDistanceFromStart > sngDistanceToTarget) Or (ftnMapObstacleVector.sngRange <> 9999) Or boolStop
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnMapObstacleVector :  " & ex.ToString)
            ftnMapObstacleVector.sngRange = 9999
            ftnMapObstacleVector.sngAngle = 0
        End Try
    End Function


    Public Function ftnScannerObstacleVector(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose, ByRef tomTypeOfMove As TypeOfMove) As typeRangeVector ' returns a vector ( relative to bot center and orientation) of nearest obstacle. 
        Try
            ftnScannerObstacleVector.sngRange = 9999
            ftnScannerObstacleVector.sngAngle = 0
            If (tomTypeOfMove <> TypeOfMove.TURN) Then
                Dim sngFwdDistanceToObstacle As Single = 9999
                Dim sngLowestFwdDistanceToObstacle As Single = 9999
                Dim tbpScannerPose As typeBotPose = tbpEstimatedPose
                Dim trvVectorFromBotCenter As typeRangeVector
                tbpScannerPose.Location = ftnLocationFromEstimatedPose(tbpEstimatedPose, tmcScanVectorOffset)
                Dim tmcObstacleLocation As typeMapCoordinate
                For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1 Step 4   ' 
                    If (Abs(trvScanVectors(intBeamNumber).sngAngle) < 90) Then ' 
                        tmcObstacleLocation = ftnLocationFromEstimatedPose(tbpScannerPose, trvScanVectors(intBeamNumber))
                        trvVectorFromBotCenter.sngAngle = ftnTurnAngle(tbpEstimatedPose, tmcObstacleLocation)
                        If (Abs(trvVectorFromBotCenter.sngAngle) <= 90) Then
                            trvVectorFromBotCenter.sngRange = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tmcObstacleLocation)
                            sngFwdDistanceToObstacle = ftnObstacleForwardDistance(trvVectorFromBotCenter)
                            If (sngFwdDistanceToObstacle < sngLowestFwdDistanceToObstacle) Then
                                sngLowestFwdDistanceToObstacle = sngFwdDistanceToObstacle
                                ftnScannerObstacleVector = trvVectorFromBotCenter
                            End If
                        End If
                    End If
                Next intBeamNumber
                If (tomTypeOfMove = TypeOfMove.CURVE) Then ftnScannerObstacleVector = ftnCurveObstacleVector(tbpEstimatedPose, tbpStartingPose, tbpTargetPose, ftnScannerObstacleVector)
                If boolDebugObstacle Then frmDebug.subWriteVector(ftnElapsedTimeS() & "  ftnScannerObstacleVector:       Obstacle Vector  = ", ftnScannerObstacleVector)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnScannerObstacleVector :  " & ex.ToString)
            ftnScannerObstacleVector.sngRange = 9999
            ftnScannerObstacleVector.sngAngle = 0
        End Try
    End Function

    Function ftnCurveObstacleVector(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose, ByVal trvObstacleVector As typeRangeVector) As typeRangeVector
        Try
            ftnCurveObstacleVector = trvObstacleVector
            Dim sngRadius, sngCentralAngle, sngArcLength As Single
            Dim boolClockwise As Boolean
            Dim tmoCenterOfRotation As typeMapOffset
            subDeriveCurveVariables(tbpStartingPose, tbpTargetPose, sngRadius, boolClockwise, tmoCenterOfRotation, sngCentralAngle, sngArcLength)
            Dim tbpCheckPose As typeBotPose = ftnCurveMotionDR(tbpStartingPose, trvObstacleVector.sngRange, sngRadius, boolClockwise, tmoCenterOfRotation, sngCentralAngle, sngArcLength, False)
            Dim tmcObstacleLocation As typeMapCoordinate = ftnLocationFromEstimatedPose(tbpEstimatedPose, trvObstacleVector)
            If boolDebugObstacle Then
                frmDebug.subWritePose(" ftnCurveObstacleVector:  tbpCheckPose = ", tbpCheckPose)
                frmDebug.subWriteCoordinate(" ftnCurveObstacleVector:  tmcObstacleLocation = ", tmcObstacleLocation)
            End If
            If (ftnDistanceBetweenLocations(tbpCheckPose.Location, tmcObstacleLocation) > sngBotRadius) Then
                If boolDebugObstacle Then frmDebug.subWriteLogs(" ftnCurveObstacleVector:  No obstacle")
                ftnCurveObstacleVector.sngRange = 9999
                ftnCurveObstacleVector.sngAngle = 0
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCurveObstacleVector :  " & ex.ToString)
            ftnCurveObstacleVector.sngRange = 9999
            ftnCurveObstacleVector.sngAngle = 0
        End Try
    End Function

     Public Function ftnObstacleForwardDistance(ByRef trvObstacleVector As typeRangeVector) As Single
        Try
            If ((trvObstacleVector.sngRange * Sin(ftnDegreeToRadian(Abs(trvObstacleVector.sngAngle)))) < sngBotRadius) Then
                ftnObstacleForwardDistance = trvObstacleVector.sngRange * Cos(ftnDegreeToRadian(Abs(trvObstacleVector.sngAngle))) - sngBotRadius
            Else
                ftnObstacleForwardDistance = 9999
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnObstacleForwardDistance :  " & ex.ToString)
            ftnObstacleForwardDistance = 9999
        End Try
    End Function

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

    Function ftnAddObstacleToMap(ByRef tbpEstimatedPose As typeBotPose, ByRef tomTypeOfMove As TypeOfMove, ByRef tbpStartingPose As typeBotPose, ByRef tbpTargetPose As typeBotPose) As typeBotPose
        ' add obstacle (temporary) to map
        Try
            ftnAddObstacleToMap = tbpEstimatedPose
            If Not boolEnableRBPF And ftnScannerConnected() Then
                Dim sngFwdDistance As Single = ftnDistanceCommanded(tbpEstimatedPose, tbpTargetPose)
                Dim boolGoodLocalization As Boolean = False
                ftnAddObstacleToMap = ftnLocalize_Top(ftnAddObstacleToMap, intBestParticle, boolGoodLocalization, True)
                Dim trvObstacleVector As typeRangeVector = ftnObstacleVectorTop(tbpEstimatedPose, tomTypeOfMove, tbpStartingPose, tbpTargetPose)
                Dim sngObstacleFwdDistance As Single = ftnObstacleForwardDistance(trvObstacleVector)
                If (sngObstacleFwdDistance <= sngFwdDistance) Then
                    Dim tmcObstacleLocation As typeMapCoordinate = ftnLocationFromEstimatedPose(tbpEstimatedPose, trvObstacleVector)
                    frmDebug.subWriteLogs(" =============================  Adding Obstacle to Map")
                    For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1 Step constMaxScannerBeams \ intNumberOfBeams
                        trvObstacleVector = ftnScannerToBotCenterVector(ftnAddObstacleToMap, trvScanVectors(intBeamNumber))
                        sngObstacleFwdDistance = ftnObstacleForwardDistance(trvObstacleVector)
                        If (sngObstacleFwdDistance <= sngFwdDistance) And (Abs(trvObstacleVector.sngAngle) < 90) Then
                            tmcObstacleLocation = ftnLocationFromEstimatedPose(ftnAddObstacleToMap, trvObstacleVector)
                            byteNavigationArray(tmcObstacleLocation.X, tmcObstacleLocation.Y) = 255
                            boolOccupiedCell(0, tmcObstacleLocation.X, tmcObstacleLocation.Y) = True
                            frmDebug.subWriteLogs(" Obstacle Added at " & tmcObstacleLocation.X & ", " & tmcObstacleLocation.Y)
                        End If
                    Next intBeamNumber
                    subGenerateDistanceFromWallSquaredArray(intBestParticle)
                    strStatusDriving = "New Map Array Generated "
                    subGeneratePathArray()
                    ftnFindPathToDestination(tmcDestination, ftnAddObstacleToMap.Location)
                    boolMapUpdatedWithObstacle = True
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnAddObstacleToMap :  " & ex.ToString)
        End Try
    End Function

    Function ftnScannerToBotCenterVector(ByVal tbpEstimatedPose As typeBotPose, ByRef trvScannerVector As typeRangeVector) As typeRangeVector
        Try
            ftnScannerToBotCenterVector = trvScannerVector
            Dim tbpScannerPose As typeBotPose = tbpEstimatedPose
            tbpScannerPose.Location = ftnLocationFromEstimatedPose(tbpEstimatedPose, tmcScanVectorOffset)
            Dim tmcVectorEnd As typeMapCoordinate = ftnLocationFromEstimatedPose(tbpScannerPose, trvScannerVector)
            ftnScannerToBotCenterVector.sngAngle = ftnTurnAngle(tbpEstimatedPose, tmcVectorEnd)
            ftnScannerToBotCenterVector.sngRange = ftnDistanceBetweenLocations(tbpEstimatedPose.Location, tmcVectorEnd)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnScannerToBotCenterVector :  " & ex.ToString)
        End Try
    End Function



End Module



