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

Module modCollision
    ' bumper switches are active low
    ' rear = DI2
    ' left front = DI1
    ' right front = DI0


    Public Function ftnCollisionRecovery(ByVal tbpEstimatedPose As typeBotPose, ByRef byteCollisionCode As Byte) As typeBotPose
        Try
            ftnCollisionRecovery = tbpEstimatedPose
            If (byteCollisionCode <> 7) Then
                boolCollisionDetectionEnable = False
                tbpBotPose = ftnLocalizeUsingDeadReckoning(tbpBotPose, 0.2, True) '  ' assume that Collision was detected / communicated 0.2 seconds after the event  
                If tmiMoveInfo.boolBotIsMoving Then tbpBotPose = ftnBotStop(tbpBotPose, tmiMoveInfo.tbpTargetPose) ' ISSUE STOP COMMAND
                ftnSendCommand("play phrase doh")
                tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, False, True)
                ftnCollisionRecovery = tbpBotPose
                If boolSlam Then
                    boolStop = True
                    boolDrivingToDestination = False
                    byteCollisionCode = 7
                    MsgBox(" There was a collision and GotABot cannot determine the resulting pose accurately!" & vbCrLf &
                           "Move the Bot away from whatever it collided with and adjust the Pose " & vbCrLf &
                           "(Press Configure->Pose) to reflect the actual Pose.  " & vbCrLf &
                           "Press Tools->Localization to perform a final correction.")
                Else
                    If Not boolStop And Not boolShutDown Then
                        frmDebug.subWriteLogs(" }}}}}}}}}}  WARNING: Collision Recovery Initiated")
                        subWait(250)
                        Dim tbpTargetPose As typeBotPose = tbpEstimatedPose
                        ftnCollisionRecovery = ftnPoseAfterCollision(tbpEstimatedPose)
                        tbpBotPose = ftnCollisionRecovery
                        Select Case byteCollisionCode
                            Case 6  ' Right Front
                                frmDebug.subWriteLogs(" }}}}}}}}}}  WARNING: Collision Detected - Right Front")
                                strStatusDriving = intDrivingSequenceNumber & ": COLLISION  - Right Front"
                                ftnCollisionRecovery = ftnBotFwdDistance(ftnCollisionRecovery, -6) ' back up
                                ftnSendCommand("play phrase 'it jumped in front of me!'")
                            Case 5 '  Left Front
                                frmDebug.subWriteLogs(" }}}}}}}}}}  WARNING: Collision Detected - Left Front")
                                strStatusDriving = intDrivingSequenceNumber & ": COLLISION  - Left Front"
                                ftnCollisionRecovery = ftnBotFwdDistance(ftnCollisionRecovery, -6) ' back up
                                ftnSendCommand("play phrase 'pardon me!'")
                            Case 4 ' Front
                                frmDebug.subWriteLogs(" }}}}}}}}}}  WARNING: Collision Detected -  Front")
                                strStatusDriving = intDrivingSequenceNumber & ": COLLISION  -  Front"
                                ftnCollisionRecovery = ftnBotFwdDistance(ftnCollisionRecovery, -6) ' back up
                                ftnSendCommand("play phrase 'get out of my way!'")
                            Case 3 ' Rear
                                frmDebug.subWriteLogs(" }}}}}}}}}}  WARNING: Collision Detected - Rear")
                                strStatusDriving = intDrivingSequenceNumber & ": COLLISION  - Rear"
                                ftnCollisionRecovery = ftnBotFwdDistance(ftnCollisionRecovery, 6) 'go forward
                                ftnSendCommand("play phrase 'it snuck up on me!'")
                            Case Else
                                frmDebug.subWriteLogs(" }}}}}}}}}}  No Action taken:  no collision detected")
                        End Select
                    End If
                    ftnCollisionRecovery.Location = ftnCheckMapArrayLimits(ftnCollisionRecovery.Location)
                    If ftnInWall_NoChk(ftnCollisionRecovery.Location, intBestParticle) Then ftnCollisionRecovery.Location = tbpEstimatedPose.Location
                    tbpBotPose = ftnLocalize_Top(ftnCollisionRecovery, intBestParticle, False, True)
                    ftnCollisionRecovery = tbpBotPose
                    ftnSendCommand("battery")
                End If
                boolCollisionDetectionEnable = True
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCollisionRecovery :  " & ex.ToString)
        End Try
    End Function

    Private Function ftnPoseAfterCollision(ByVal tbpEstimatedPose As typeBotPose) As typeBotPose
        Try
            If boolURGConnected Or boolKinectConnected Then
                tbpEstimatedPose.OrientationUncertainty += 30
                tbpEstimatedPose.LocationUncertainty.X += 6
                tbpEstimatedPose.LocationUncertainty.Y += 6
                ftnPoseAfterCollision = ftnLocalize_Top(tbpEstimatedPose, intBestParticle, False, True)
            Else ' force lost recovery
                tbpEstimatedPose.OrientationUncertainty = 60
                tbpEstimatedPose.LocationUncertainty.X = 40
                tbpEstimatedPose.LocationUncertainty.Y = 40
                ftnPoseAfterCollision = ftnLocalize_Top(tbpEstimatedPose, intBestParticle, False, True)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnPoseAfterCollision :  " & ex.ToString)
        End Try
    End Function


End Module
