Imports System.Drawing
Imports System.Windows.Forms


Module modLocalizeTop

    Public Sub subLocalizeTop()
        Try
            tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, True, (sngCcDelayS < 0.35))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subLocalizeTop :  " & ex.ToString)
        End Try
    End Sub

    Public Function ftnLocalize_Top(ByVal tbpEstimatedPose As typeBotPose, ByRef intParticleNumber As Integer, ByRef boolGoodLocalization As Boolean, ByRef boolHighPrecision As Boolean) As typeBotPose
        Try
            ftnLocalize_Top = tbpEstimatedPose
            'If boolDebugLocalize2D Then 
            boolGoodLocalization = False
            If boolLocalizationInProgress Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnLocalize_Top:  WARNING boolLocalizationInProgress")
            Dim timeOut As DateTime = Now().AddMilliseconds(3000)
            While boolLocalizationInProgress And (Now() < timeOut)  ' wait for localize to complete
                subWait(25)
            End While
            If boolLocalizationInProgress Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnLocalize_Top:  ERROR boolLocalizationInProgress  ")
            If Not boolSimulatedMovement And Not boolShutDown And Not boolLocalizationInProgress Then
                boolLocalizationInProgress = True
                tbpEstimatedPose.Orientation = ftnConvertTo360(tbpEstimatedPose.Orientation)
                Dim tbp2DPose, tbpVisualPose As typeBotPose
                Dim boolGood2DLocalization As Boolean = False
                Dim boolGoodVisualLocalization As Boolean = False
                Dim sngRMSError As Single = 0
                strStatusDriving = intDrivingSequenceNumber & ":  Localize = "

                If ftnScannerConnected() Then
                    If boolHeroSelected Then ftnGetHeroScanVectors()
                    If boolEnableRBPF Then
                        tbp2DPose = ftnLocalizeRBPF_Top(tbpEstimatedPose, boolGood2DLocalization)
                    Else
                        tbp2DPose = ftnLocalizeFrom2DScan_Top(ftnLocalize_Top, tbpEstimatedPose, intParticleNumber, tmiMoveInfo.boolBotIsMoving, boolGood2DLocalization, sngRMSError, boolHighPrecision)
                    End If
                    subWriteScanResult(boolGood2DLocalization, tbp2DPose, sngRMSError)
                End If

                If ftnVisualRecognizerEnabled() And Not (ftnScannerConnected() And (boolSlam Or frmArm.Visible)) Then
                    If (Not boolGood2DLocalization Or (ftnPoseUncertaintyFactor(tbp2DPose) > 442)) Then  '(442 = (13 inches + 13 inches) * 17 degrees)
                        subStartImageRecognition()
                        tbpVisualPose = ftnLocalizeUsingVisualLMsTop(ftnLocalize_Top, tbpEstimatedPose, boolGoodVisualLocalization)
                    End If
                End If

                boolGoodLocalization = boolGood2DLocalization Or boolGoodVisualLocalization
                If boolGood2DLocalization And boolGoodVisualLocalization Then
                    ftnLocalize_Top = ftnFindAvgPose(tbp2DPose, tbpVisualPose, intParticleNumber, 0)
                ElseIf boolGood2DLocalization Then
                    ftnLocalize_Top = tbp2DPose
                ElseIf boolGoodVisualLocalization Then
                    ftnLocalize_Top = tbpVisualPose
                End If
                If boolGoodLocalization Then boolShowGreenVectors = True

                frmDebug.subWritePose(ftnElapsedTimeS() & "       Pose Derived From All Data      ", ftnLocalize_Top)
                If Not tmiMoveInfo.boolBotIsMoving Then tbpBotPose = ftnLocalize_Top
            End If
            boolLocalizationInProgress = False
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocalize_Top :  " & ex.ToString)
            ftnLocalize_Top = tbpEstimatedPose
            boolLocalizationInProgress = False
        End Try
    End Function


    Public Sub subWriteScanResult(ByRef boolGoodLocalization As Boolean, ByRef tbpPose As typeBotPose, ByRef sngRMS_Error As Single)
        Try
            If Not tmiMoveInfo.boolBotIsMoving Then
                strStatusDriving = " Scan Error=" & FormatNumber(sngRMS_Error, 2)
                'If boolDebugRBPF Then frmDebug.subWriteLogs("subWriteScanResult:   intBestParticle = " & intBestParticle)
                If boolGoodLocalization Then
                    frmDebug.subWritePose(ftnElapsedTimeS() & "       Pose Derived From Scanner  ", tbpPose)
                    frmDebug.subWriteLogs("                         Scan Error = " & FormatNumber(sngRMS_Error, 2) & "   (localization success)")
                Else
                    frmDebug.subWritePose(ftnElapsedTimeS() & "       Pose Derived From Scanner  ", tbpPose)
                    frmDebug.subWriteLogs("                           Scan Error = " & FormatNumber(sngRMS_Error, 2) & "   (localization failed)")
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subWriteScanResult :  " & ex.ToString)
        End Try
    End Sub




    Public Function ftnAccelerometerCheck(ByVal tbpReferencePose As typeBotPose) As typeBotPose ' check for collision using Kinect Accelerometer 
        ftnAccelerometerCheck = tbpReferencePose
#If constCompile2dAccelerometer Then
        If (tmiMoveInfo.tomTypeOfMove = TypeOfMove.FWD) Then
            ftnTransmitGotAKinect("get accelerometer distance ", "OK", 500)
            frmDebug.subWriteLogs( "Kinect Accelerometer Distances: " & tmcAccelerometerDistance.X & ", " & tmcAccelerometerDistance.Y)
            Dim tmcAccelerometer As typeMapCoordinate = ftnLocationFromLocation(tmiMoveInfo.tbpStartingPose.Location, tmiMoveInfo.tbpStartingPose.Orientation, tmcAccelerometerDistance.X)
            frmDebug.subWriteLogs( "Kinect Accelerometer Pose: " & tmcAccelerometer.X & ", " & tmcAccelerometer.Y)
            If (ftnDistanceBetweenLocations(tbpReferencePose.Location, tmcAccelerometer) > 12) Then
                'tbpReferencePose.Location = tmiMoveInfo.tbpStartingPose.Location
                'ftnAccelerometerCheck.Location = ftnBotOffsetLocation(tmcAccelerometerDistance)
                'ftnAccelerometerCheck.LocationUncertainty.X += ftnInchesToMapUnits(9)
                'ftnAccelerometerCheck.LocationUncertainty.X += ftnInchesToMapUnits(9)
                'ftnAccelerometerCheck.OrientationUncertainty += 15
                frmDebug.subWritePose(" Pose after Collision Detected by Kinect sensor", ftnAccelerometerCheck)
            End If
        End If
#End If
    End Function




    Function ftnFindPoseOverlap(ByVal tbpNewPoseReading As typeBotPose, ByVal tbpReferencePose As typeBotPose) As typeBotPose
        Try
            ' Use the existing pose to reduce the uncertainties of a new pose reading
            Dim intMaxX, intMaxY, intMinX, intMinY As Integer
            ftnFindPoseOverlap = tbpNewPoseReading
            If (tbpNewPoseReading.Location.X + tbpNewPoseReading.LocationUncertainty.X) > (tbpReferencePose.Location.X + tbpReferencePose.LocationUncertainty.X) Then
                intMaxX = (tbpReferencePose.Location.X + tbpReferencePose.LocationUncertainty.X)
            Else
                intMaxX = (tbpNewPoseReading.Location.X + tbpNewPoseReading.LocationUncertainty.X)
            End If
            If (tbpNewPoseReading.Location.Y + tbpNewPoseReading.LocationUncertainty.Y) > (tbpReferencePose.Location.Y + tbpReferencePose.LocationUncertainty.Y) Then
                intMaxY = (tbpReferencePose.Location.Y + tbpReferencePose.LocationUncertainty.Y)
            Else
                intMaxY = (tbpNewPoseReading.Location.Y + tbpNewPoseReading.LocationUncertainty.Y)
            End If
            If (tbpNewPoseReading.Location.X - tbpNewPoseReading.LocationUncertainty.X) < (tbpReferencePose.Location.X - tbpReferencePose.LocationUncertainty.X) Then
                intMinX = (tbpReferencePose.Location.X - tbpReferencePose.LocationUncertainty.X)
            Else
                intMinX = (tbpNewPoseReading.Location.X - tbpNewPoseReading.LocationUncertainty.X)
            End If
            If (tbpNewPoseReading.Location.Y - tbpNewPoseReading.LocationUncertainty.Y) < (tbpReferencePose.Location.Y - tbpReferencePose.LocationUncertainty.Y) Then
                intMinY = (tbpReferencePose.Location.Y - tbpReferencePose.LocationUncertainty.Y)
            Else
                intMinY = (tbpNewPoseReading.Location.Y - tbpNewPoseReading.LocationUncertainty.Y)
            End If
            ftnFindPoseOverlap.LocationUncertainty.X = (intMaxX - intMinX) \ 2
            ftnFindPoseOverlap.Location.X = intMaxX - ftnFindPoseOverlap.LocationUncertainty.X
            ftnFindPoseOverlap.LocationUncertainty.Y = (intMaxY - intMinY) \ 2
            ftnFindPoseOverlap.Location.Y = intMaxY - ftnFindPoseOverlap.LocationUncertainty.Y
            ftnFindPoseOverlap = ftnMinPoseUncertainty(ftnFindPoseOverlap)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnFindPoseOverlap :  " & ex.ToString)
        End Try
    End Function


End Module
