

' This module returns a bot pose based upon Range Scan Data.
' It is used for devices which can return a 2D range scan such as a Kinect or Laser Range scanner
Imports System.Threading
Imports System.Math
Imports System.Drawing


Module modLocalize2Dscan
    Dim bool2DScanLocalizationInProgress As Boolean = False ' dont want conflict 

    Public Function ftnLocalizeFrom2DScan_Top(ByRef tbpEstimatedPose As typeBotPose, ByVal tbpReferencePose As typeBotPose, ByRef intParticleNumber As Integer, ByVal boolBotIsMoving As Boolean, _
                                              ByRef boolGood2DLocalization As Boolean, ByRef sngRMS_Error As Single, ByRef boolHighPrecision As Boolean) As typeBotPose
        Try
            ftnLocalizeFrom2DScan_Top = tbpEstimatedPose
            If boolDebugLocalize2D Then frmDebug.subWriteLogs(ftnElapsedTimeS() & "  ftnLocalizeFrom2DScan_Top:  START")
            boolGood2DLocalization = True
            If ftnScannerConnected() And Not boolStop And Not boolSimulatedMovement Then
                Dim tbpRawScanPose As typeBotPose
                sngRMS_Error = sngOneDotFiveSigma
                If bool2DScanLocalizationInProgress Then
                    If boolDebugLocalize2D Then frmDebug.subWriteLogs(ftnElapsedTimeS() & "  ftnLocalizeFrom2DScan_Top:  bool2DScanLocalizationInProgress")
                    Dim timeOut As DateTime = Now().AddMilliseconds(12000)
                    While bool2DScanLocalizationInProgress And Not boolStop And Now() < timeOut ' wait for previous scan to finish
                        Thread.Sleep(25)
                    End While
                    If Now() >= timeOut Then frmDebug.subWriteLogs("  ftnLocalizeFrom2DScan_Top timeOut ")
                End If
                bool2DScanLocalizationInProgress = True
                tbpRawScanPose = ftn2dLocalize(tbpEstimatedPose, intParticleNumber, boolGood2DLocalization, sngRMS_Error, boolHighPrecision)
                If Not boolGood2DLocalization Then
                    ftnLocalizeFrom2DScan_Top = tbpEstimatedPose
                Else
                    If (sngRMS_Error < sngOkError) Then
                        ftnLocalizeFrom2DScan_Top = tbpRawScanPose
                    ElseIf (sngRMS_Error > sngOkError) And tmiMoveInfo.boolBotIsMoving Then
                        ftnLocalizeFrom2DScan_Top = tbpEstimatedPose
                        boolGood2DLocalization = False
                    ElseIf (sngRMS_Error < sngUnacceptableError) Then
                        ftnLocalizeFrom2DScan_Top = ftnFindAvgPose(tbpEstimatedPose, tbpRawScanPose, intParticleNumber, 0)
                    Else
                        boolGood2DLocalization = False
                        ftnLocalizeFrom2DScan_Top = tbpEstimatedPose
                    End If
                    ftnLocalizeFrom2DScan_Top = ftnMinPoseUncertainty(ftnLocalizeFrom2DScan_Top)
                    If tmiMoveInfo.boolBotIsMoving Then tbpReferencePose = tbpEstimatedPose
                    If boolGood2DLocalization Then ftnLocalizeFrom2DScan_Top = ftnValidatePose(ftnLocalizeFrom2DScan_Top, tbpReferencePose, intParticleNumber, boolGood2DLocalization, Not boolBotIsMoving, "Scan2D")
                End If
                If boolDebugLocalize2D Then
                    Dim trvVector As typeRangeVector
                    Dim tmcVectorOrigin As typeMapCoordinate = ftnLocationFromEstimatedPose(ftnLocalizeFrom2DScan_Top, tmcScanVectorOffset)
                    Dim intDistanceFromWallErrorSquared, intDistanceFromWallErrorSquared_Sum, intValidBeamCounter As Integer
                    For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1 Step constMaxScannerBeams \ intNumberOfBeams
                        trvVector.sngRange = trvScanVectors(intBeamNumber).sngRange
                        If (trvVector.sngRange > 0) And (trvVector.sngRange < intScanVectorsMaxRangeInches) Then  ' 
                            trvVector.sngAngle = ftnConvertTo360(trvScanVectors(intBeamNumber).sngAngle + ftnLocalizeFrom2DScan_Top.Orientation)
                            intDistanceFromWallErrorSquared = ftnDistanceFromWallErrorSquared(intParticleNumber, tmcVectorOrigin, trvVector, False, False)
                            intDistanceFromWallErrorSquared_Sum += intDistanceFromWallErrorSquared
                            If (intDistanceFromWallErrorSquared < sngOneSigmaSquared) Then
                                intValidBeamCounter += 3
                            ElseIf (intDistanceFromWallErrorSquared < intOneDotFiveSigmaSquared) Then
                                intValidBeamCounter += 1
                            End If

                            frmDebug.subWriteLogs(" Beam " & intBeamNumber & ":  Range = " & trvVector.sngRange & ":  Error = " & intDistanceFromWallErrorSquared)
                        End If
                    Next intBeamNumber
                    frmDebug.subWriteShortPose(" ftnLocalizeFrom2DScan_Top = ", ftnLocalizeFrom2DScan_Top)
                    frmDebug.subWriteLogs(" intValidBeamCounter = " & intValidBeamCounter & ":  intDistanceFromWallErrorSquared_Sum = " & intDistanceFromWallErrorSquared_Sum)
                    frmDebug.subWriteLogs("ftnScanToMapRMS_Error = " & ftnScanToMapRMS_Error(ftnLocalizeFrom2DScan_Top, intParticleNumber, True))
                End If
            End If
            bool2DScanLocalizationInProgress = False
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocalizeFrom2DScan_Top ::  " & ex.ToString)
            ftnLocalizeFrom2DScan_Top = tbpBotPose
            sngRMS_Error = sngOneDotFiveSigma
        End Try
    End Function




    Function ftn2dLocalize(ByRef tbpEstimatedPose As typeBotPose, ByRef intParticleNumber As Integer,
                           ByRef boolGood2DLocalization As Boolean, ByRef sngRMS_Error As Single, ByRef boolHighPrecision As Boolean) As typeBotPose
        Try
            If boolDebugLocalize2D Then frmDebug.subWriteLogs(ftnElapsedTimeS() & "  ftn2dLocalize:  START")
            sngRMS_Error = sngOneDotFiveSigma
            boolGood2DLocalization = False
            If ftnNonFlatWall() Then
                ftn2dLocalize = ftnPoseFrom2DScan(tbpEstimatedPose, intParticleNumber, sngRMS_Error, boolHighPrecision) '
                boolGood2DLocalization = sngRMS_Error < sngUnacceptableError
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftn2dLocalize ::  " & ex.ToString)
            ftn2dLocalize = tbpEstimatedPose
            sngRMS_Error = sngOneDotFiveSigma
        End Try
    End Function




    Public Function ftnPoseFrom2DScan(ByRef tbpEstimatedPose As typeBotPose, ByRef intParticleNumber As Integer, ByRef sngRMS_Error As Single, ByRef boolHighPrecision As Boolean) As typeBotPose
        Try

            Dim boolValid As Boolean = True
            ftnPoseFrom2DScan = tbpEstimatedPose
            If tmiMoveInfo.boolBotIsMoving Then
                If tmiMoveInfo.tomTypeOfMove = TypeOfMove.TURN Then
                    ftnPoseFrom2DScan.OrientationUncertainty = 20
                Else
                    ftnPoseFrom2DScan.OrientationUncertainty = 12
                End If
                ftnPoseFrom2DScan.LocationUncertainty.X = 12
                ftnPoseFrom2DScan.LocationUncertainty.Y = 12
            End If
            ' was 4,4 ->175 iterations uncertainty of (3,3,7) then 2,2 ->128 iterations uncertainty of (2,2,3) then 1,1 175 iterations // 478
            ' iterations for uncertainties of 8,8,12
            If boolHighPrecision Then
                ftnPoseFrom2DScan = ftnPoseWithBestFit(ftnPoseFrom2DScan, intParticleNumber, sngRMS_Error, 2, 2)  ' 10x5x13 = 750
            Else
                ftnPoseFrom2DScan = ftnPoseWithBestFit(ftnPoseFrom2DScan, intParticleNumber, sngRMS_Error, 4, 2)  ' 5x5x13 = 325
            End If
            ftnPoseFrom2DScan = ftnValidatePose(ftnPoseFrom2DScan, tbpEstimatedPose, intParticleNumber, boolValid, boolDebugRBPF Or boolDebugLocalize2D, "ftnPoseFrom2DScan: 7 ")
            If (sngRMS_Error < sngOneDotFiveSigma) And boolValid Then
                ftnPoseFrom2DScan.LocationUncertainty.X = 5
                ftnPoseFrom2DScan.LocationUncertainty.Y = 5
                ftnPoseFrom2DScan.OrientationUncertainty = 3
                ftnPoseFrom2DScan = ftnPoseWithBestFit(ftnPoseFrom2DScan, intParticleNumber, sngRMS_Error, 2, 1) ' 7x7x7 = 343
            End If
            If boolValid Then ftnPoseFrom2DScan = ftnValidatePose(ftnPoseFrom2DScan, tbpEstimatedPose, intParticleNumber, boolValid, boolDebugRBPF Or boolDebugLocalize2D, "ftnPoseFrom2DScan: 7 ")
            If (sngRMS_Error < sngOneDotFiveSigma) And boolValid Then
                ftnPoseFrom2DScan.LocationUncertainty.X = 1
                ftnPoseFrom2DScan.LocationUncertainty.Y = 1
                ftnPoseFrom2DScan.OrientationUncertainty = 2
                ftnPoseFrom2DScan = ftnPoseWithBestFit(ftnPoseFrom2DScan, intParticleNumber, sngRMS_Error, 1, 1) ' 3x3x5 = 45  // 1150
            End If
            If boolValid Then ftnPoseFrom2DScan = ftnValidatePose(ftnPoseFrom2DScan, tbpEstimatedPose, intParticleNumber, True, boolDebugRBPF Or boolDebugLocalize2D, "ftnPoseFrom2DScan: 3 ")
            If Not boolValid Then sngRMS_Error = Max(sngRMS_Error, sngUnacceptableError * 1.1)

            ftnPoseFrom2DScan = ftnPoseUncertainty(tbpEstimatedPose, ftnPoseFrom2DScan, sngRMS_Error, (sngRMS_Error < sngUnacceptableError))
        Catch ex As Exception
            ftnPoseFrom2DScan = tbpEstimatedPose
            sngRMS_Error = sngOneDotFiveSigma
            frmDebug.subWriteLogs(" EXCEPTION in ftnPoseFrom2DScan :  " & ex.ToString)
        End Try
    End Function

    Function ftnPoseUncertainty(ByRef tbpEstimatedPose As typeBotPose, ByVal tbpLocalizedPose As typeBotPose, ByRef sngRMS_Error As Single, ByRef boolValidLocalization As Boolean) As typeBotPose
        Try
            If Not boolValidLocalization Then
                ftnPoseUncertainty = tbpEstimatedPose
            Else
                ftnPoseUncertainty = tbpLocalizedPose
                If (sngRMS_Error < sngLowError) Then
                    ftnPoseUncertainty.OrientationUncertainty = 10
                    ftnPoseUncertainty.LocationUncertainty.X = 10
                    ftnPoseUncertainty.LocationUncertainty.Y = 10
                ElseIf (sngRMS_Error < sngOkError) Then
                    ftnPoseUncertainty.OrientationUncertainty = 15
                    ftnPoseUncertainty.LocationUncertainty.X = 10
                    ftnPoseUncertainty.LocationUncertainty.Y = 10
                Else
                    ftnPoseUncertainty.OrientationUncertainty = 15
                    ftnPoseUncertainty.LocationUncertainty.X = 15
                    ftnPoseUncertainty.LocationUncertainty.Y = 15
                End If
            End If
            ftnPoseUncertainty.OrientationUncertainty = Min(ftnPoseUncertainty.OrientationUncertainty, tbpEstimatedPose.OrientationUncertainty)
            ftnPoseUncertainty.OrientationUncertainty = Min(ftnPoseUncertainty.LocationUncertainty.X, tbpEstimatedPose.LocationUncertainty.X)
            ftnPoseUncertainty.OrientationUncertainty = Min(ftnPoseUncertainty.LocationUncertainty.Y, tbpEstimatedPose.LocationUncertainty.Y)
        Catch ex As Exception
            ftnPoseUncertainty = tbpEstimatedPose
            frmDebug.subWriteLogs(" EXCEPTION in ftnPoseUncertainty :  " & ex.ToString)
        End Try
    End Function


    Function ftnPoseWithBestFit(ByRef tbpEstimatedPose As typeBotPose, ByRef intParticleNumber As Integer, ByRef sngRmsErrorBest As Single,
                                                 ByVal intLocationStep As Integer, ByVal intOrientationStep As Integer) As typeBotPose
        Try ' Check every pose within the bot's uncertainty region to find that which most closely resembles the scan data
            ftnPoseWithBestFit = tbpEstimatedPose
            Dim intRayCastCount As Integer = 0
            If Not boolStop And Not boolShutDown Then
                Dim tbpPossiblePose As typeBotPose = tbpEstimatedPose
                Dim sngRmsError As Single
                Dim intStartX As Integer = Max(tbpEstimatedPose.Location.X - tbpEstimatedPose.LocationUncertainty.X, 0)
                Dim intEndX As Integer = Min(tbpEstimatedPose.Location.X + tbpEstimatedPose.LocationUncertainty.X, intMapWidth - 1)
                Dim intStartY As Integer = Max(tbpEstimatedPose.Location.Y - tbpEstimatedPose.LocationUncertainty.Y, 0)
                Dim intEndY As Integer = Min(tbpEstimatedPose.Location.Y + tbpEstimatedPose.LocationUncertainty.Y, intMapHeight - 1)
                Dim intOrientationStart As Integer ' 
                Dim intOrientationEnd As Integer '
                subFindStartEndAngles(tbpEstimatedPose, intOrientationStart, intOrientationEnd)
                sngRmsErrorBest = ftnScanToMapRMS_Error(tbpEstimatedPose, intParticleNumber, False) ' Find  Ray Cast Error at estimated pose

                If boolDebugLocalize2D Then
                    frmDebug.subWriteShortPose(vbCrLf & vbCrLf & ftnElapsedTimeS() & " === ftnPoseWithBestFit:  Start  tbpEstimatedPose = ", tbpEstimatedPose)
                    frmDebug.subWriteLogs("        ftnPoseWithBestFit:  Ray Cast Error at Estimated Pose = " & sngRmsErrorBest)
                    frmDebug.subWriteLogs("        ftnPoseWithBestFit:  intStartX, intEndX " & intStartX & " to " & intEndX & " @ " & intLocationStep)
                    frmDebug.subWriteLogs("        ftnPoseWithBestFit:  intStartY, intEndY " & intStartY & " to " & intEndY & " @ " & intLocationStep)
                    frmDebug.subWriteLogs("        ftnPoseWithBestFit:  OrientaStart, Stop " & intOrientationStart & " to " & intOrientationEnd & " @ " & intOrientationStep)
                End If
                For tbpPossiblePose.Location.Y = intStartY To intEndY Step intLocationStep
                    For tbpPossiblePose.Location.X = intStartX To intEndX Step intLocationStep ' try to find a lower error pose 
                        If (intDistanceFromWallSquaredArray(intParticleNumber, tbpPossiblePose.Location.X, tbpPossiblePose.Location.Y) >= sngBotRadiusSquared) Then ' if not in a wall
                            For tbpPossiblePose.Orientation = intOrientationStart To intOrientationEnd Step intOrientationStep
                                'tbpPossiblePose.Orientation = ftnConvertTo360(tbpPossiblePose.Orientation)
                                sngRmsError = ftnScanToMapRMS_Error(tbpPossiblePose, intParticleNumber, True) ' Find End Point Model error at this pose
                                If boolDebugLocalize2D Then frmDebug.subWriteLogs("ftnPoseWithBestFit:  intRayCastCount = " & intOrientationStep & ", " & tbpPossiblePose.Orientation & ", " & sngRmsError)
                                If (sngRmsError < sngRmsErrorBest) Then ' good match but check for intervening walls
                                    sngRmsError = ftnScanToMapRMS_Error(tbpPossiblePose, intParticleNumber, False) ' Find Ray Cast Model error at this pose
                                    If (sngRmsError < sngRmsErrorBest) Then ' 
                                        sngRmsErrorBest = sngRmsError
                                        ftnPoseWithBestFit = tbpPossiblePose
                                        If boolDebugLocalize2D Then
                                            frmDebug.subWriteShortPose(" !!!!!!!!!!!!!!!!!!!  ftnPoseWithBestFit: New Best Error = " & sngRmsError & " @ ", tbpPossiblePose)
                                            intRayCastCount += 1
                                        End If
                                    End If
                                End If
                            Next tbpPossiblePose.Orientation
                        End If
                    Next tbpPossiblePose.Location.X
                Next tbpPossiblePose.Location.Y
            Else
                If boolDebugLocalize2D Then frmDebug.subWriteLogs(" >>> ftnPoseWithBestFit - boolStop Exit")
            End If
            If boolDebugLocalize2D Then
                frmDebug.subWriteShortPose(ftnElapsedTimeS() & " === ftnPoseWithBestFit: END ", ftnPoseWithBestFit)
                frmDebug.subWriteLogs("        ftnPoseWithBestFit:  intRayCastCount = " & intRayCastCount)
            End If
        Catch ex As Exception
            ftnPoseWithBestFit = tbpEstimatedPose
            sngRmsErrorBest = sngOneDotFiveSigma
            frmDebug.subWriteLogs(" EXCEPTION in ftnPoseWithBestFit :  " & ex.ToString)
        End Try
    End Function

    Function ftnScanToMapRMS_Error(ByRef tbpPossiblePose As typeBotPose, ByRef intParticleNumber As Integer, ByRef boolEndPointModel As Boolean) As Single
        ' Find the correlation of a particular pose to the range data 
        ' boolEndPointModel = Don't check for intervening walls
        ftnScanToMapRMS_Error = sngOneDotFiveSigma
        Dim trvVector As typeRangeVector
        Dim intDistanceFromWallErrorSquared As Integer
        Dim intValidBeamCounter As Integer = 0
        Dim intDistanceFromWallErrorSquaredSum As Integer = 0
        Dim boolEndPointInGreySpace As Boolean = False
        Try
            Dim tmcVectorOrigin As typeMapCoordinate = ftnLocationFromEstimatedPoseNoChk(tbpPossiblePose, tmcScanVectorOffset)
            For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1 Step constMaxScannerBeams \ intNumberOfBeams
                trvVector.sngRange = trvScanVectors(intBeamNumber).sngRange
                If (trvVector.sngRange > 0) And (trvVector.sngRange < intScanVectorsMaxRangeInches) Then  ' 
                    trvVector.sngAngle = ftnConvertTo360(trvScanVectors(intBeamNumber).sngAngle + tbpPossiblePose.Orientation)
                    intDistanceFromWallErrorSquared = ftnDistanceFromWallErrorSquared(intParticleNumber, tmcVectorOrigin, trvVector, boolEndPointModel, boolEndPointInGreySpace)
                    If (intDistanceFromWallErrorSquared < sngOneSigmaSquared) Then
                        intValidBeamCounter += 3
                    ElseIf (intDistanceFromWallErrorSquared < intOneDotFiveSigmaSquared) Then
                        intValidBeamCounter += 1
                    ElseIf boolEndPointInGreySpace Then
                        intValidBeamCounter += 1 ' Grey zone = cluttered environment / may have false readings
                    End If
                    intDistanceFromWallErrorSquaredSum += intDistanceFromWallErrorSquared
                End If
            Next intBeamNumber
            intValidBeamCounter = intValidBeamCounter \ 3
            If (intValidBeamCounter > intNumberOfBeams * 0.4) Then  ' looks like a valid correlation
                ftnScanToMapRMS_Error = Sqrt(intDistanceFromWallErrorSquaredSum / intValidBeamCounter)
                If boolKinectConnected Then
                    ftnScanToMapRMS_Error *= 0.85  ' 
                ElseIf boolURGConnected Then
                    ftnScanToMapRMS_Error *= 0.7  ' 
                ElseIf boolHeroSelected Then
                    ftnScanToMapRMS_Error *= 0.5
                End If
                If boolEnableRBPF And tmiMoveInfo.tomTypeOfMove = TypeOfMove.TURN Then ftnScanToMapRMS_Error *= 0.5 ' 
                ftnScanToMapRMS_Error = Min(ftnScanToMapRMS_Error, sngOneDotFiveSigma)
                ftnScanToMapRMS_Error = Max(ftnScanToMapRMS_Error, 0.25)
            Else
                ftnScanToMapRMS_Error = sngOneDotFiveSigma
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXECEPTION in ftnScanToMapRMS_Error :  " & ex.ToString)
            ftnScanToMapRMS_Error = sngOneDotFiveSigma
        End Try
    End Function



    '--------------------------------------  Non Flat Wall  ----------------------------------------------------------------------

    Public Function ftnNonFlatWall() As Boolean
        ' long flat walls which fill the Scanner's FOV can cause localization errors
        ftnNonFlatWall = False
        Try
            If boolURGConnected Then
                ftnNonFlatWall = True
            Else
                Dim sngScannerRange(constMaxScannerBeams) As Single
                Dim sngScannerSlope(constMaxScannerBeams) As Single
                Dim intBeamInterval As Integer = constMaxScannerBeams \ intNumberOfBeams
                Dim intTwoSamplesPerVector As Integer = intBeamInterval + intBeamInterval
                For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1 Step intBeamInterval
                    sngScannerRange(intBeamNumber) = trvScanVectors(intBeamNumber).sngRange
                Next intBeamNumber
                If (sngScannerRange(intTwoSamplesPerVector) = 0) Then sngScannerRange(intTwoSamplesPerVector) = sngScannerRange(3 * intBeamInterval)
                If (sngScannerRange(intBeamInterval) = 0) Then sngScannerRange(intBeamInterval) = sngScannerRange(intTwoSamplesPerVector)
                If (sngScannerRange(0) = 0) Then sngScannerRange(0) = sngScannerRange(intBeamInterval)
                sngScannerSlope(intBeamInterval) = sngScannerRange(intBeamInterval) - sngScannerRange(0)
                Dim intI_MinusSamplesPerVector As Integer
                Dim intSpikeCount As Integer = 0
                For I As Integer = intTwoSamplesPerVector To constMaxScannerBeams - 1 Step intBeamInterval
                    intI_MinusSamplesPerVector = I - intBeamInterval
                    If (sngScannerRange(I) = 0) Then sngScannerRange(I) = sngScannerRange(intI_MinusSamplesPerVector) ' get rid of range = 0 entries
                    sngScannerSlope(I) = sngScannerRange(I) - sngScannerRange(intI_MinusSamplesPerVector) ' calculate slopes
                    If (sngScannerRange(I) <> 0) And (sngScannerRange(intI_MinusSamplesPerVector) <> 0) Then
                        intSpikeCount += 1
                        If (Abs(sngScannerRange(I) - sngScannerRange(intI_MinusSamplesPerVector)) > 3) And ((sngScannerRange(I) > 40) Or (intSpikeCount > 0)) Then ' Exit if Spike (Discontinuity)
                            ftnNonFlatWall = True
                            Exit Function
                        End If
                    End If
                Next I
            End If
            If boolDebugLocalize2D Then frmDebug.subWriteLogs(" ftnNonFlatWall =  " & ftnNonFlatWall)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnNonFlatWall :  " & ex.ToString)
        End Try
    End Function




End Module
