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


Public Module modLocalizeRBPF

    '  StochasticUniversalSampling() taken from http://mnemstudio.org/genetic-algorithms-simple-example-4.htm

    Dim sngParticleWeightNormalized(intNumberOfParticles) As Single
    Dim tbpParticleStartPose(intNumberOfParticles) As typeBotPose
    Dim tbpParticleProposalPose(intNumberOfParticles) As typeBotPose
    Dim byteParticleMapOdds(intNumberOfParticles, intMapWidth, intMapHeight) As Byte  ' 16 = 1:1 odds
    Dim boolOccupiedCellRBPF(intNumberOfParticles, intMapWidth, intMapHeight) As Boolean



 
    Public Sub subInitParticles(ByVal tbpEstimatedPose As typeBotPose, ByRef intNumberOfParticles As Integer)
        Dim intParticleNumber As Integer
        Try
            If boolEnableRBPF Or boolSlam Then
                If boolDebugRBPF Then frmDebug.subWriteLogs("//  subInitParticles  //")
                If boolSlam Then boolEnableRBPF = True
                frmSlam.updnMapSelect.Maximum = intNumberOfParticles - 1
                frmSlam.updnMapSelect.Refresh()
                ReDim byteParticleMapOdds(intNumberOfParticles, intMapWidth, intMapHeight)
                ReDim intDistanceFromWallSquaredArray(intNumberOfParticles, intMapWidth, intMapHeight)
                ReDim boolOccupiedCellRBPF(intNumberOfParticles, intMapWidth, intMapHeight)
                ReDim sngParticleWeightNormalized(intNumberOfParticles)
                ReDim tbpParticleStartPose(intNumberOfParticles)
                ReDim sngRMS_Error(intNumberOfParticles)
                ReDim sngRMS_ErrorAvg(intNumberOfParticles)
                ReDim tbpParticleProposalPose(intNumberOfParticles)
                ReDim tbpParticlePose(intNumberOfParticles)

                Dim bmpMapData As System.Drawing.Imaging.BitmapData = ftnLockMap()    ' Lock the bitmap's bits. 
                Dim ptr As IntPtr = bmpMapData.Scan0  ' Get the address of the first line.
                Dim bytes As Integer = Abs(bmpMapData.Stride) * intMapHeight   ' Declare an array to hold the bytes of the bitmap.
                Dim byteMapRGB(bytes - 1) As Byte
                System.Runtime.InteropServices.Marshal.Copy(ptr, byteMapRGB, 0, bytes)  ' Copy the RGB values into the array.
                Dim intPixelIndex As Integer
                Dim byteMapGreenPixel As Byte
                For intX As Integer = 0 To intMapWidth - 1   ' Convert the map image into byteParticleMapOdds.   0 = free,  255 = occupied
                    For intY As Integer = 0 To intMapHeight - 1
                        intPixelIndex = intMapWidth * 4 * intY + intX * 4 + 1   ' Green Pixel
                        byteMapGreenPixel = byteMapRGB(intPixelIndex)
                        If (byteMapGreenPixel = 0) Then
                            byteParticleMapOdds(0, intX, intY) = 255
                        ElseIf (byteMapGreenPixel = 255) Then
                            byteParticleMapOdds(0, intX, intY) = 0
                        Else
                            byteParticleMapOdds(0, intX, intY) = 16
                        End If
                    Next intY
                Next intX
                subUnLockMap(bmpMapData)    ' Unlock the bits.

                subGenerateDistanceFromWallSquaredArray(0)
                For intParticleNumber = 1 To intNumberOfParticles - 1
                    For intX As Integer = 0 To intMapWidth
                        For intY As Integer = 0 To intMapHeight
                            intDistanceFromWallSquaredArray(intParticleNumber, intX, intY) = intDistanceFromWallSquaredArray(0, intX, intY)
                        Next intY
                    Next intX
                Next intParticleNumber

                tbpParticlePose(0) = tbpEstimatedPose
                tbpParticlePose(0).OrientationUncertainty = 12
                tbpParticlePose(0).LocationUncertainty = ftnConvertToTMC(8, 8)
                sngRMS_Error(0) = sngLowError * 0.9

                tbpParticleStartPose(0) = tbpParticlePose(0)
                tbpParticleProposalPose(0) = tbpParticlePose(0)
                For intParticleNumber = 1 To intNumberOfParticles - 1
                    subCopyParticle(0, intParticleNumber)
                    sngRMS_Error(intParticleNumber) = sngRMS_Error(0)
                    sngRMS_ErrorAvg(intParticleNumber) = sngRMS_Error(0)
                Next intParticleNumber
                intBestParticle = 0
                subConvertOccupencyOddsToMap(intBestParticle)
                subConvertMapToArray(intBestParticle)
                subGeneratePathArray()
                tmiMoveInfo.timeMoveCommandSent = Now()
                ftnLocalizeRBPF_Top(tbpBotPose, False)  ' initial scans
                For intParticleNumber = 0 To intNumberOfParticles - 1
                    sngRMS_Error(intParticleNumber) = sngLowError * 0.9
                    sngRMS_ErrorAvg(intParticleNumber) = sngLowError
                Next
                subScanToMaps(intNumberOfParticles, intBestParticle)
                tbpBotPose = ftnLocalizeRBPF_Top(tbpBotPose, False)
                tbpBotPose = ftnLocalizeRBPF_Top(tbpBotPose, False)
                tmiMoveInfo.timeMoveCommandSent = Now()  ' Measure Processor speed so as to calculate  Bot Speeds
                For i = 1 To 100000 * intNumberOfParticles / 32
                    subRecalculateVariables()
                Next
                'frmDebug.subWriteLogs("  Measurement Time = " & ftnElapsedTimeS())
                sngBotAngularVelocity = Min(0.1 / ftnElapsedTimeS() * 18, sngBotAngularVelocity)
                sngBotLinearVelocity = Min(0.1 / ftnElapsedTimeS() * 12, sngBotLinearVelocity)
                sngBotAngularAcceleration = 40
                sngBotLinearAcceleration = 40
                subRecalculateVariables()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subInitParticles :  " & ex.ToString)
            frmDebug.subWriteLogs("     intParticleNumber = " & intParticleNumber)
            frmDebug.subWriteLogs("     tbpParticlePose(intParticleNumber).Location = " & tbpParticlePose(intParticleNumber).Location.X & ", " & tbpParticlePose(intParticleNumber).Location.Y)
        End Try
    End Sub



    Function ftnLocalizeRBPF_Top(ByVal tbpEstimatedPose As typeBotPose, ByRef boolGoodLocalization As Boolean) As typeBotPose
        Try
            ftnLocalizeRBPF_Top = tbpEstimatedPose
            boolLocalizationInProgress = True
            If boolDebugRBPF Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ============= ftnLocalizeRBPF_Top:  START ===============================")
            boolGoodLocalization = False
            subGenerateProposal(intNumberOfParticles) '
            subLocalizeParticles(intNumberOfParticles)  ' 
            If Not tmiMoveInfo.boolBotIsMoving Then
                subStochasticSampling(intNumberOfParticles)     '
                intBestParticle = ftnBestParticle(intNumberOfParticles)
            End If
            ftnLocalizeRBPF_Top = tbpParticlePose(intBestParticle)
            subScanToMaps(intNumberOfParticles, intBestParticle)
            boolGoodLocalization = (sngRMS_Error(intBestParticle) < sngUnacceptableError)

            If boolDebugRBPF Then '
                frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnLocalizeRBPF_Top:  tmiMoveInfo.boolBotIsMoving " & tmiMoveInfo.boolBotIsMoving)
                For intParticleNumber = 0 To intNumberOfParticles - 1
                    frmDebug.subWriteShortPose("ftnLocalizeRBPF_Top: " & intParticleNumber & ": " & FormatNumber(sngRMS_Error(intParticleNumber), 2) & " / " & FormatNumber(sngRMS_ErrorAvg(intParticleNumber), 2) & ":  tbpParticlePose ", tbpParticlePose(intParticleNumber))
                Next intParticleNumber
                frmDebug.subWritePose(ftnElapsedTimeS() & " ftnLocalizeRBPF_Top:   Best tbpParticleProposalPose " & intBestParticle & ": " & tmiMoveInfo.boolBotIsMoving & " - " & sngRMS_Error(intBestParticle).ToString & " /  " & _
                                      sngRMS_ErrorAvg(intBestParticle).ToString, tbpParticleProposalPose(intBestParticle)) '
                frmDebug.subWritePose(ftnElapsedTimeS() & " ftnLocalizeRBPF_Top:   Best tbpParticlePose " & intBestParticle & ": " & tmiMoveInfo.boolBotIsMoving & " - " & sngRMS_Error(intBestParticle).ToString & " /  " & _
                                      sngRMS_ErrorAvg(intBestParticle).ToString, tbpParticlePose(intBestParticle))
            End If
            If Not tmiMoveInfo.boolBotIsMoving Then subWriteScanResult(boolGoodLocalization, ftnLocalizeRBPF_Top, sngRMS_Error(intBestParticle))
            boolLocalizationInProgress = False
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocalizeRBPF_Top :  " & ex.ToString)
            boolLocalizationInProgress = False
        End Try
    End Function

    Sub subUpdateStartPose()
        ' Update the Start Pose 
        Try
            'Array.Copy(tbpParticlePose, tbpParticleStartPose, intNumberOfParticles) 'Check
            For intParticleNumber = 0 To intNumberOfParticles - 1
                tbpParticleStartPose(intParticleNumber) = tbpParticlePose(intParticleNumber)
            Next
            'tbpParticleStartPose = tbpParticlePose
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subUpdateStartPose :  " & ex.ToString)
        End Try
    End Sub

    Public Sub subScanToMaps(ByRef intNumberOfParticles As Integer, ByRef intBestParticle As Integer)
        Try
            subConvertScanToOccupencyOdds(intNumberOfParticles)
            subConvertOccupencyOddsToMap(intBestParticle)
            If boolDrivingToDestination Then subGeneratePathArray()
            frmSlam.updnMapSelect.Value = intBestParticle
            frmSlam.updnMapSelect.Refresh()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subScanToMaps :  " & ex.ToString)
        End Try
    End Sub


    Sub subGenerateProposal(ByRef intNumberOfParticles As Integer)
        '  Calculate tbpParticleProposalPose, the assumed pose of each particle as predicted by odometry or by dead reckoning
        Try
            Dim tmiMoveInfoLocal As typeMoveInfo = ftnGetMoveInfo()
            Dim tbpEstimatedPose As typeBotPose = ftnLocalizeUsingDeadReckoning(tmiMoveInfoLocal.tbpStartingPose, 0, True)
            tbpEstimatedPose = ftnLocalizeUsingOdometry(tbpEstimatedPose) '           
            Select Case tmiMoveInfoLocal.tomTypeOfMove
                Case TypeOfMove.TURN
                    Dim intTurnAngle As Integer = ftnAngleDifference(tbpEstimatedPose.Orientation, tmiMoveInfoLocal.tbpStartingPose.Orientation)
                    Dim intOrientationUncertainty As Integer = Abs(intTurnAngle) / 15 ' additional uncertainty due to incorrect modeling of bot velocity
                    For intParticleNumber As Integer = 0 To intNumberOfParticles - 1  '
                        tbpParticleProposalPose(intParticleNumber) = ftnTurnDR(tbpParticleStartPose(intParticleNumber), intTurnAngle, True)
                        tbpParticleProposalPose(intParticleNumber).OrientationUncertainty += intOrientationUncertainty
                    Next intParticleNumber
                Case TypeOfMove.FWD
                    Dim sngDistance As Single = ftnDistanceBetweenLocations(tmiMoveInfoLocal.tbpStartingPose.Location, tbpEstimatedPose.Location)
                    Dim intLocationUncertainty As Integer = Abs(sngDistance) / 15 ' additional uncertainty due to incorrect modeling of bot velocity
                    For intParticleNumber As Integer = 0 To intNumberOfParticles - 1
                        tbpParticleProposalPose(intParticleNumber) = ftnFwdDR(tbpParticleStartPose(intParticleNumber), sngDistance, True)
                        tbpParticleProposalPose(intParticleNumber).LocationUncertainty.X += Abs(sngDistance) / 15
                        tbpParticleProposalPose(intParticleNumber).LocationUncertainty.Y += Abs(sngDistance) / 15
                    Next intParticleNumber
                Case TypeOfMove.CURVE
                    Dim sngTargetRange As Single = ftnDistanceBetweenLocations(tmiMoveInfoLocal.tbpStartingPose.Location, tmiMoveInfoLocal.tbpTargetPose.Location)
                    Dim sngTargetAngle As Single = ftnTurnAngle(tmiMoveInfoLocal.tbpStartingPose, tmiMoveInfoLocal.tbpTargetPose.Location)
                    Dim sngDistance As Single = ftnDistanceTravelled(tmiMoveInfoLocal.tbpStartingPose, tmiMoveInfoLocal.tbpTargetPose, ftnElapsedTimeS(), tmiMoveInfoLocal.tomTypeOfMove, tmiMoveInfoLocal.boolCourseCorrection)
                    Dim tbpParticleTargetPose As typeBotPose
                    Dim intTurnAngle As Integer = ftnAngleDifference(tbpEstimatedPose.Orientation, tmiMoveInfoLocal.tbpStartingPose.Orientation)
                    Dim intOrientationUncertainty As Integer = Abs(intTurnAngle) / 15 ' additional uncertainty due to incorrect modeling of bot velocity
                    Dim intLocationUncertainty As Integer = Abs(sngDistance) / 15 ' additional uncertainty due to incorrect modeling of bot velocity
                    For intParticleNumber As Integer = 0 To intNumberOfParticles - 1
                        tbpParticleTargetPose = tbpParticleStartPose(intParticleNumber)
                        tbpParticleTargetPose.Location = ftnLocationFromEstimatedPose(tbpParticleStartPose(intParticleNumber), sngTargetAngle, sngTargetRange)
                        tbpParticleProposalPose(intParticleNumber) = ftnCurve_DR(tbpParticleStartPose(intParticleNumber), sngDistance, tbpParticleTargetPose, True)
                        tbpParticleProposalPose(intParticleNumber).OrientationUncertainty += intOrientationUncertainty
                        tbpParticleProposalPose(intParticleNumber).LocationUncertainty.X += Abs(sngDistance) / 15
                        tbpParticleProposalPose(intParticleNumber).LocationUncertainty.Y += Abs(sngDistance) / 15
                    Next intParticleNumber
                Case Else
                    If boolDebugRBPF Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " subGenerateProposal:  Unexpected type of Move " & tmiMoveInfo.tomTypeOfMove.ToString)
                    For intParticleNumber As Integer = 0 To intNumberOfParticles - 1  'TODO Change to array.copy
                        tbpParticleProposalPose(intParticleNumber) = tbpParticlePose(intParticleNumber)
                    Next
            End Select
            subAddPropsalNoiseAll(intNumberOfParticles) '  Add noise to each Particle's movement

            If boolDebugRBPF Then
                frmDebug.subWriteLogs(ftnElapsedTimeS() & " subGenerateProposal:  END =============================== " & tmiMoveInfo.tomTypeOfMove.ToString)
                frmDebug.subWritePose(ftnElapsedTimeS() & " subGenerateProposal:   tmiMoveInfoLocal.tbpStartingPose ", tmiMoveInfoLocal.tbpStartingPose)  '
                frmDebug.subWritePose(ftnElapsedTimeS() & "  subGenerateProposal:        Pose Derived From Odometry ", tbpEstimatedPose)  ' 
                frmDebug.subWritePose(ftnElapsedTimeS() & " subGenerateProposal:       BEST tbpParticleProposalPose ", tbpParticleProposalPose(intBestParticle))  '
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subGenerateProposal :  " & ex.ToString)
        End Try
    End Sub



    Sub subAddPropsalNoiseAll(ByRef intNumberOfParticles As Integer)
        Try
            If Not tmiMoveInfo.boolBotIsMoving Then
                Randomize()
                For intParticleNumber As Integer = 0 To intNumberOfParticles - 1
                    tbpParticleProposalPose(intParticleNumber).Location.X += ftnGenerateGaussian() ' 
                    tbpParticleProposalPose(intParticleNumber).Location.Y += ftnGenerateGaussian()
                    tbpParticleProposalPose(intParticleNumber).Location = ftnCheckMapArrayLimits(tbpParticleProposalPose(intParticleNumber).Location)
                    tbpParticleProposalPose(intParticleNumber).Orientation = ftnConvertTo360(tbpParticleProposalPose(intParticleNumber).Orientation + ftnGenerateGaussian())
                Next intParticleNumber
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subAddPropsalNoiseAll :  " & ex.ToString)
        End Try
    End Sub



    Sub subLocalizeParticles(ByRef intNumberOfParticles As Integer)
        Try
            If ftnNonFlatWall() Then  '
                Dim boolValid As Boolean
                For intParticleNumber As Integer = 0 To intNumberOfParticles - 1
                    tbpParticlePose(intParticleNumber) = ftnPoseFrom2DScan(tbpParticleProposalPose(intParticleNumber), intParticleNumber, sngRMS_Error(intParticleNumber), False)

                    tbpParticlePose(intParticleNumber) = ftnValidatePose(tbpParticlePose(intParticleNumber), tbpParticleProposalPose(intParticleNumber), intParticleNumber, boolValid, boolDebugRBPF, "subLocalizeParticles: " & intParticleNumber)
                    If (intDistanceFromWallSquaredArray(intParticleNumber, tbpParticlePose(intParticleNumber).Location.X, tbpParticlePose(intParticleNumber).Location.Y) < sngBotRadiusSquared) Then sngRMS_Error(intParticleNumber) = sngUnacceptableError '
                    If (sngRMS_Error(intParticleNumber) > sngOkError) Then  ' use the Proposal if there is a weak localization
                        tbpParticlePose(intParticleNumber) = tbpParticleProposalPose(intParticleNumber)
                        sngRMS_Error(intParticleNumber) = Max(sngRMS_Error(intParticleNumber), sngOkError)
                    End If
                    sngRMS_ErrorAvg(intParticleNumber) = (sngRMS_ErrorAvg(intParticleNumber) * 3 + sngRMS_Error(intParticleNumber)) / 4  ' Running average
                    tbpParticlePose(intParticleNumber).OrientationUncertainty = Min(tbpParticlePose(intParticleNumber).OrientationUncertainty, 20)
                    tbpParticlePose(intParticleNumber).LocationUncertainty.X = Min(tbpParticlePose(intParticleNumber).LocationUncertainty.X, 20)
                    tbpParticlePose(intParticleNumber).LocationUncertainty.Y = Min(tbpParticlePose(intParticleNumber).LocationUncertainty.Y, 20)
                Next intParticleNumber
            Else
                If boolDebugRBPF Then frmDebug.subWriteLogs("***********************  subLocalizeParticles :  ftnNonFlatWall ") ' 
                subSetPoseEqProposal()
            End If
            If boolDebugRBPF Then frmDebug.subWriteShortPose(ftnElapsedTimeS() & " subLocalizeParticles: " & intBestParticle & ": " & ":  tbpParticlePose ", tbpParticlePose(intBestParticle))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subLocalizeParticles :  " & ex.ToString)
        End Try
    End Sub





    Sub subSetPoseEqProposal()
        Try
            Array.Copy(tbpParticleProposalPose, tbpParticlePose, intNumberOfParticles)  'Check
            For intParticleNumber As Integer = 0 To intNumberOfParticles - 1    'TODO Change to array.copy 
                sngRMS_Error(intParticleNumber) = Max(sngRMS_Error(intParticleNumber), sngOkError)
            Next intParticleNumber
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subSetPoseEqProposal :  " & ex.ToString)
        End Try
    End Sub

    Dim colorGrey As Color = ColorTranslator.FromOle(RGB(223, 223, 223))
    Public Sub subConvertOccupencyOddsToMap(ByRef intParticleNumber As Integer)
        Try
            'If boolDebugRBPF Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ============= subConvertOccupencyOddsToMap:  START ==========" & intParticleNumber & ", " & sngRMS_Error(intParticleNumber))
            Dim bytePixelOdds As Byte

            Dim bmpMapData As System.Drawing.Imaging.BitmapData = ftnLockMap()    ' Lock the bitmap's bits. 
            Dim ptr As IntPtr = bmpMapData.Scan0  ' Get the address of the first line.
            Dim bytes As Integer = Math.Abs(bmpMapData.Stride) * intMapHeight   ' Declare an array to hold the bytes of the bitmap.
            Dim byteMapRGB(bytes - 1) As Byte
            System.Runtime.InteropServices.Marshal.Copy(ptr, byteMapRGB, 0, bytes)  ' Copy the RGB values into the array.

            Dim intPixelIndex As Integer
            For intX As Integer = 0 To intMapWidth - 1   ' Convert byteParticleMapOdds into bmpMap.   white = free,  black = occupied
                For intY As Integer = 0 To intMapHeight - 1
                    intPixelIndex = intMapWidth * 4 * intY + intX * 4
                    bytePixelOdds = byteParticleMapOdds(intParticleNumber, intX, intY)
                    If (bytePixelOdds = 16) Then
                        byteMapRGB(intPixelIndex) = colorGrey.B
                        byteMapRGB(intPixelIndex + 1) = colorGrey.G
                        byteMapRGB(intPixelIndex + 2) = colorGrey.R
                        byteMapRGB(intPixelIndex + 3) = colorGrey.A
                    ElseIf (bytePixelOdds < 16) Then
                        byteMapRGB(intPixelIndex) = Color.White.B
                        byteMapRGB(intPixelIndex + 1) = Color.White.G
                        byteMapRGB(intPixelIndex + 2) = Color.White.R
                        byteMapRGB(intPixelIndex + 3) = Color.White.A
                    Else
                        byteMapRGB(intPixelIndex) = Color.Black.B
                        byteMapRGB(intPixelIndex + 1) = Color.Black.G
                        byteMapRGB(intPixelIndex + 2) = Color.Black.R
                        byteMapRGB(intPixelIndex + 3) = Color.Black.A
                    End If
                Next intY
            Next intX
            System.Runtime.InteropServices.Marshal.Copy(byteMapRGB, 0, ptr, bytes)    ' Copy the RGB values back to bmpMap
            frmGotABotTop.myMap.Image = bmpMap.Clone
            subUnLockMap(bmpMapData)    ' Unlock the bits.
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subConvertOccupencyOddsToMap :  " & ex.ToString)
        End Try
    End Sub

    Function ftnGoodScan(ByRef intParticleNumber As Integer) As Boolean
        Try
            ftnGoodScan = (sngRMS_Error(intParticleNumber) < sngUnacceptableError) And (sngRMS_ErrorAvg(intParticleNumber) < sngUnacceptableError)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGoodScan :  " & ex.ToString)
        End Try
    End Function

    Sub subConvertScanToOccupencyOdds(ByRef intNumberOfParticles As Integer)
        Try
            Dim tbpThisParticlesPose As typeBotPose
            Dim trvVector As typeRangeVector
            Dim tmcEndPoint, tmcCheckPoint As typeMapCoordinate
            Dim boolRangeExceedsMapLimits As Boolean = False
            Dim tmcVectorOrigin As typeMapCoordinate
            For intParticleNumber As Integer = 0 To intNumberOfParticles - 1
                If boolDebugRBPF Then frmDebug.subWriteLogs(" subConvertScanToOccupencyOdds:  " & intParticleNumber & ": sngRMS_ErrorAvg, sngRMS_Error  " & sngRMS_ErrorAvg(intParticleNumber) & ",  " & sngRMS_Error(intParticleNumber))
                tbpThisParticlesPose = tbpParticlePose(intParticleNumber)
                If ftnGoodScan(intParticleNumber) Then  '
                    tmcVectorOrigin = ftnLocationFromEstimatedPose(tbpThisParticlesPose, tmcScanVectorOffset)
                    For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1 '
                        boolRangeExceedsMapLimits = False
                        If boolKinectConnected Then  ' Find the longest range of 4 samples
                            For i = 1 To 3
                                If (trvScanVectors(intBeamNumber + i).sngRange > trvScanVectors(intBeamNumber).sngRange) Then trvScanVectors(intBeamNumber).sngRange = trvScanVectors(intBeamNumber + i).sngRange
                            Next
                        End If
                        If (trvScanVectors(intBeamNumber).sngRange > sngBotRadius) Then
                            trvVector.sngRange = trvScanVectors(intBeamNumber).sngRange
                            trvVector.sngAngle = ftnConvertTo360(trvScanVectors(intBeamNumber).sngAngle + tbpThisParticlesPose.Orientation)
                            tmcEndPoint = ftnLocationFromLocation(tmcVectorOrigin, trvVector.sngAngle, trvVector.sngRange)
                            While ftnLocationOnMapEdge(tmcEndPoint) And (trvVector.sngRange > sngBotRadius) ' Vector extends beyond map limits,  reduce it
                                boolRangeExceedsMapLimits = True
                                trvVector.sngRange -= 1
                                tmcEndPoint = ftnLocationFromLocation(tmcVectorOrigin, trvVector.sngAngle, trvVector.sngRange) ' 
                            End While
                            tmcCheckPoint = ftnFirstPointOnLine(tmcVectorOrigin, tmcEndPoint)
                            While Not ftnLocationsEqual(tmcCheckPoint, tmcEndPoint)
                                If boolOccupiedCellRBPF(intParticleNumber, tmcCheckPoint.X, tmcCheckPoint.Y) Then
                                    subUpdateCellOccupencyEmpty(tmcCheckPoint, intParticleNumber)  ' empty space
                                    If (ftnDistanceBetweenLocations(tmcCheckPoint, tmcEndPoint) > 4) Then Exit While ' hit a wall - don't continue beyond
                                End If
                                subUpdateCellOccupencyEmpty(tmcCheckPoint, intParticleNumber)  ' empty space
                                tmcCheckPoint = ftnNextPointOnLine(tmcCheckPoint)
                            End While
                            If (trvVector.sngRange < intScanVectorsMaxRangeInches) And Not boolRangeExceedsMapLimits And ftnLocationsEqual(tmcCheckPoint, tmcEndPoint) Then _
                              subUpdateCellOccupencyFull(tmcCheckPoint, intParticleNumber) ' wall
                        End If
                        If boolKinectConnected Then intBeamNumber += 3 ' with Kinect, we only sample the longest beam of 4
                    Next intBeamNumber
                End If
            Next intParticleNumber
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subConvertScanToOccupencyOdds :  " & ex.ToString)
        End Try
    End Sub


    Sub subUpdateCellOccupencyEmpty(ByRef tmcLocation As typeMapCoordinate, ByRef intParticleNumber As Integer)  ' 
        Try ' byteParticleMapOdds :  0 = Empty,  16 = 50% Odds of Empty or Occupied,  255 = Occupied
            If (byteParticleMapOdds(intParticleNumber, tmcLocation.X, tmcLocation.Y) = 16) Then    '
                byteParticleMapOdds(intParticleNumber, tmcLocation.X, tmcLocation.Y) = 0 '
                boolOccupiedCellRBPF(intParticleNumber, tmcLocation.X, tmcLocation.Y) = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subUpdateCellOccupencyEmpty :  " & ex.ToString)
        End Try
    End Sub


    Sub subUpdateCellOccupencyFull(ByRef tmcLocation As typeMapCoordinate, ByRef intParticleNumber As Integer)  ' 
        Dim intX_Diff, intY_Diff, intDistanceFromWallSquared As Integer  ' update intDistanceFromWallSquaredArray
        Try ' byteParticleMapOdds :  0 = Empty,  16 = 50% Odds of Empty or Occupied,  255 = Occupied
            If (byteParticleMapOdds(intParticleNumber, tmcLocation.X, tmcLocation.Y) = 16) Then    ' Update intDistanceFromWallSquaredArray
                byteParticleMapOdds(intParticleNumber, tmcLocation.X, tmcLocation.Y) = 255 '
                boolOccupiedCellRBPF(intParticleNumber, tmcLocation.X, tmcLocation.Y) = True
                Dim tmcLocation2 As typeMapCoordinate
                For tmcLocation2.X = Max(tmcLocation.X - sngOneDotFiveSigma, 0) To Min(tmcLocation.X + sngOneDotFiveSigma, intMapWidth - 1) ' left and right neighbors
                    intX_Diff = Abs(tmcLocation2.X - tmcLocation.X)
                    For tmcLocation2.Y = Max(tmcLocation.Y - sngOneDotFiveSigma, 0) To Min(tmcLocation.Y + sngOneDotFiveSigma, intMapHeight - 1) ' up and down neighbors
                        intY_Diff = Abs(tmcLocation2.Y - tmcLocation.Y)
                        intDistanceFromWallSquared = intSquareDistanceArray(intX_Diff, intY_Diff)
                        intDistanceFromWallSquaredArray(intParticleNumber, tmcLocation2.X, tmcLocation2.Y) = Min(intDistanceFromWallSquaredArray(intParticleNumber, tmcLocation2.X, tmcLocation2.Y), intDistanceFromWallSquared)
                    Next tmcLocation2.Y
                Next tmcLocation2.X
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subUpdateCellOccupencyFull :  " & ex.ToString)
            frmDebug.subWriteLogs(" intParticleNumber =  " & intParticleNumber)
            frmDebug.subWriteCoordinate(" tmcLocation =  ", tmcLocation)
            frmDebug.subWriteLogs(" intX_Diff =  " & intX_Diff)
            frmDebug.subWriteLogs(" intY_Diff =  " & intY_Diff)
            frmDebug.subWriteLogs(" intSquareDistanceArray.GetUpperBound(0) =  " & intSquareDistanceArray.GetUpperBound(0))
            frmDebug.subWriteLogs(" intDistanceFromWallSquaredArray.Rank =  " & intDistanceFromWallSquaredArray.Rank)
            frmDebug.subWriteLogs(" intDistanceFromWallSquaredArray.GetUpperBound(0) =  " & intDistanceFromWallSquaredArray.GetUpperBound(0))
            frmDebug.subWriteLogs(" intDistanceFromWallSquaredArray.GetUpperBound(1) =  " & intDistanceFromWallSquaredArray.GetUpperBound(1))
            frmDebug.subWriteLogs(" intDistanceFromWallSquaredArray.GetUpperBound(2) =  " & intDistanceFromWallSquaredArray.GetUpperBound(2))
        End Try
    End Sub


    Public Function ftnGenerateGaussian() As Single
        Try ' Generate gaussian number with sigma = 0.5.  Range = +/- 3
            ftnGenerateGaussian = 0
            For i As Integer = 1 To 12   ' 
                ftnGenerateGaussian += Rnd()
            Next i
            ftnGenerateGaussian -= 6
            If ftnGenerateGaussian > 3 Then ftnGenerateGaussian = 3
            If ftnGenerateGaussian < -3 Then ftnGenerateGaussian = -3
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGenerateGaussian :  " & ex.ToString)
        End Try
    End Function

    Function ftnResampleRequired(ByRef intNumberOfParticles As Integer) As Boolean
        ' Normalize the weight array then calculate the Resample Factor
        Try
            subNormalizeWeights(intNumberOfParticles)
            Dim sngResampleFactor As Single = 0
            For intParticleNumber As Integer = 0 To intNumberOfParticles - 1
                sngResampleFactor += sngParticleWeightNormalized(intParticleNumber) * sngParticleWeightNormalized(intParticleNumber)
            Next intParticleNumber
            sngResampleFactor = 1 / sngResampleFactor
            ftnResampleRequired = (sngResampleFactor < (intNumberOfParticles * 0.85))
            If boolDebugRBPF And ftnResampleRequired Then frmDebug.subWriteLogs("ftnResampleRequired:   =================== sngResampleFactor = " & sngResampleFactor)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnResampleRequired :  " & ex.ToString)
        End Try
    End Function

    Sub subNormalizeWeights(ByRef intNumberOfParticles As Integer)
        Try
            Dim sngParticleWeight(intNumberOfParticles) As Single
            For intParticleNumber As Integer = 0 To intNumberOfParticles - 1
                sngParticleWeight(intParticleNumber) = 1 / sngRMS_ErrorAvg(intParticleNumber)
            Next intParticleNumber
            Dim sngWeightTotal As Single = sngParticleWeight.Sum
            For intParticleNumber As Integer = 0 To intNumberOfParticles - 1
                sngParticleWeightNormalized(intParticleNumber) = sngParticleWeight(intParticleNumber) / sngWeightTotal
            Next intParticleNumber
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subNormalizeWeights :  " & ex.ToString)
        End Try
    End Sub

    Public Sub subStochasticSampling(ByVal intNumberOfParticles As Integer)
        ' sample tpParticles using Stochastic Universal Sampling
        ' complexity of O(N)
        ' Stochastic Universal Sampling is an elaborately-named variation of roulette wheel selection. 
        ' Stochastic Universal Sampling ensures that the observed selection frequencies of each individual are in line 
        ' with the expected frequencies. So if we have an individual that occupies 4.5% of the wheel and we select 100 
        ' individuals, we would expect on average for that individual to be selected between four and five times.
        Try
            If Not tmiMoveInfo.boolBotIsMoving Then
                If ftnResampleRequired(intNumberOfParticles) Then
                    frmDebug.subWriteLogs(" ================================================  subStochasticSampling: ======== ")
                    Dim intOrignalParticle As Integer = 0
                    Dim dblCumulativeSum As Double = sngParticleWeightNormalized(0)
                    Dim dblSelectionSum As Double = 0
                    Dim tbpNewParticlePoses(intNumberOfParticles) As typeBotPose
                    Dim tbpNewParticleStartPose(intNumberOfParticles) As typeBotPose
                    Dim tbpNewParticleProposalPose(intNumberOfParticles) As typeBotPose
                    Dim byteNewParticleOdds(intNumberOfParticles, intMapWidth, intMapHeight) As Byte
                    Dim sngNewRMS_Error(intNumberOfParticles) As Single
                    Dim sngNewRMS_ErrorAvg(intNumberOfParticles) As Single
                    Dim intNewDistanceFromWallSquaredArrayRBPF(intNumberOfParticles, intMapWidth, intMapHeight) As Integer
                    Dim boolNewOccupiedCellRBPF(intNumberOfParticles, intMapWidth, intMapHeight) As Boolean
                    subReplaceWorstParticle(intNumberOfParticles)
                    For intNewSample As Integer = 0 To intNumberOfParticles - 1
                        dblSelectionSum = intNewSample / intNumberOfParticles
                        While (dblSelectionSum > dblCumulativeSum)
                            intOrignalParticle += 1
                            If (intOrignalParticle = intNumberOfParticles) Then intOrignalParticle = 0
                            dblCumulativeSum += sngParticleWeightNormalized(intOrignalParticle)
                        End While
                        If boolDebugRBPF Then frmDebug.subWriteLogs(" ====  subStochasticSampling: ======== Copy " & intOrignalParticle & " (" & FormatNumber(sngRMS_ErrorAvg(intOrignalParticle), 2) & ") to " & intNewSample & " (" & FormatNumber(sngRMS_ErrorAvg(intNewSample), 2) & ")")
                        'Dim intParticleLength As Integer = intDistanceFromWallSquaredArray.Length
                        'Array.Copy(byteParticleMapOdds, intOrignalParticle * intParticleLength, byteNewParticleOdds, intNewSample * intParticleLength, intParticleLength)
                        For x As Integer = 0 To intMapWidth - 1
                            For y As Integer = 0 To intMapHeight - 1
                                byteNewParticleOdds(intNewSample, x, y) = byteParticleMapOdds(intOrignalParticle, x, y)
                                intNewDistanceFromWallSquaredArrayRBPF(intNewSample, x, y) = intDistanceFromWallSquaredArray(intOrignalParticle, x, y)
                                boolNewOccupiedCellRBPF(intNewSample, x, y) = boolOccupiedCellRBPF(intOrignalParticle, x, y)
                            Next
                        Next
                        tbpNewParticlePoses(intNewSample) = tbpParticlePose(intOrignalParticle)
                        tbpNewParticleStartPose(intNewSample) = tbpParticleStartPose(intOrignalParticle)
                        tbpNewParticleProposalPose(intNewSample) = tbpParticleProposalPose(intOrignalParticle)
                        sngNewRMS_ErrorAvg(intNewSample) = sngRMS_ErrorAvg(intOrignalParticle)
                        sngNewRMS_Error(intNewSample) = sngRMS_Error(intOrignalParticle)
                    Next intNewSample
                    Array.Copy(tbpNewParticlePoses, tbpParticlePose, intNumberOfParticles) 'Check
                    Array.Copy(tbpNewParticleProposalPose, tbpParticleProposalPose, intNumberOfParticles) 'Check
                    Array.Copy(tbpNewParticleStartPose, tbpParticleStartPose, intNumberOfParticles) 'Check
                    Array.Copy(sngNewRMS_ErrorAvg, sngRMS_ErrorAvg, intNumberOfParticles) 'Check
                    Array.Copy(sngNewRMS_Error, sngRMS_Error, intNumberOfParticles) 'Check

                    For p As Integer = 0 To intNumberOfParticles - 1
                        For x As Integer = 0 To intMapWidth - 1
                            For y As Integer = 0 To intMapHeight - 1
                                byteParticleMapOdds(p, x, y) = byteNewParticleOdds(p, x, y)
                                intDistanceFromWallSquaredArray(p, x, y) = intNewDistanceFromWallSquaredArrayRBPF(p, x, y)
                                boolOccupiedCellRBPF(p, x, y) = boolNewOccupiedCellRBPF(p, x, y)
                            Next
                        Next
                    Next
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subStochasticSampling :  " & ex.ToString)
        End Try
    End Sub

    Sub subReplaceWorstParticle(ByVal intNumberOfParticles As Integer)
        Try
            Dim intWorstParticle As Integer = ftnWorstParticle(intNumberOfParticles)
            subCopyParticle(ftnBestParticle(intNumberOfParticles), intWorstParticle)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subReplaceWorstParticle :  " & ex.ToString)
        End Try
    End Sub


    Public Sub subCopyParticle(ByRef intSourceParticle As Integer, ByRef intDestinationParticle As Integer)
        Try
            If boolDebugRBPF Then frmDebug.subWriteLogs(" =====================================  subCopyParticle: ==================== " & intSourceParticle & " to " & intDestinationParticle)
            'Dim intParticleLength As Integer = intDistanceFromWallSquaredArray.Length
            'Array.Copy(byteParticleMapOdds, intSourceParticle * intParticleLength, byteParticleMapOdds, intDestinationParticle * intParticleLength, intParticleLength)

            sngRMS_Error(intDestinationParticle) = sngRMS_Error(intSourceParticle)
            sngRMS_ErrorAvg(intDestinationParticle) = sngRMS_ErrorAvg(intSourceParticle)
            'subAddPropsalNoise(intDestinationParticle)
            tbpParticlePose(intDestinationParticle) = tbpParticlePose(intSourceParticle)
            tbpParticleStartPose(intDestinationParticle) = tbpParticleStartPose(intSourceParticle)
            tbpParticleProposalPose(intDestinationParticle) = tbpParticleProposalPose(intSourceParticle)
            'Array.Copy(intDistanceFromWallSquaredArray, intSourceParticle * intParticleLength, intDistanceFromWallSquaredArray, intDestinationParticle * intParticleLength, intParticleLength)
            For x As Integer = 0 To intMapWidth - 1
                For y As Integer = 0 To intMapHeight - 1
                    intDistanceFromWallSquaredArray(intDestinationParticle, x, y) = intDistanceFromWallSquaredArray(intSourceParticle, x, y)
                    byteParticleMapOdds(intDestinationParticle, x, y) = byteParticleMapOdds(intSourceParticle, x, y)
                    boolOccupiedCellRBPF(intDestinationParticle, x, y) = boolOccupiedCellRBPF(intSourceParticle, x, y)
                Next
            Next
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subCopyParticle :  " & ex.ToString)
        End Try
    End Sub



    Function ftnBestParticle(ByRef intNumberOfParticles As Integer) As Integer
        Try
            ftnBestParticle = 0
            If Not boolDrivingToDestination And Not tmiMoveInfo.boolBotIsMoving Then
                For intParticleNumber As Integer = 1 To intNumberOfParticles - 1      ' 
                    If (sngRMS_ErrorAvg(intParticleNumber) < sngRMS_ErrorAvg(ftnBestParticle)) Then _
                        ftnBestParticle = intParticleNumber
                Next intParticleNumber
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnBestParticle :  " & ex.ToString)
        End Try
    End Function

    Function ftnWorstParticle(ByRef intNumberOfParticles As Integer) As Integer
        Try
            ftnWorstParticle = 0
            For intParticleNumber As Integer = 1 To intNumberOfParticles - 1
                If (sngRMS_ErrorAvg(intParticleNumber) > sngRMS_ErrorAvg(ftnWorstParticle)) Then
                    ftnWorstParticle = intParticleNumber
                End If
            Next intParticleNumber
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnWorstParticle :  " & ex.ToString)
        End Try
    End Function


    Public Sub subChangeParticlePose(ByVal intOffsetX As Integer, intOffsetY As Integer, ByVal intOffsetOrientation As Integer)
        Try
            If boolEnableRBPF Or boolSlam Then  '
                frmDebug.subWriteLogs(" subChangeParticlePose :  " & intOffsetX & ", " & intOffsetY & " at " & intOffsetOrientation)
                For intParticleNumber As Integer = 0 To intNumberOfParticles - 1
                    frmDebug.subWritePose(ftnElapsedTimeS() & " subChangeParticlePose:    old " & intParticleNumber, tbpParticlePose(intParticleNumber))
                    tbpParticlePose(intParticleNumber).Orientation = ftnConvertTo360(tbpParticlePose(intParticleNumber).Orientation + intOffsetOrientation)
                    tbpParticlePose(intParticleNumber).Location.X += intOffsetX
                    tbpParticlePose(intParticleNumber).Location.Y += intOffsetY
                    frmDebug.subWritePose(ftnElapsedTimeS() & " subChangeParticlePose:   new " & intParticleNumber, tbpParticlePose(intParticleNumber))
                    tbpParticleStartPose(intParticleNumber) = tbpParticlePose(intParticleNumber)
                    tbpParticleProposalPose(intParticleNumber) = tbpParticlePose(intParticleNumber)
                Next intParticleNumber
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subChangeParticlePose :  " & ex.ToString)
        End Try
    End Sub





End Module

