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


Module modMapArray

    ''''''''''''''''''''''''''''''''''''''''''''''
    '
    '     Map Array Module: 
    '
    '  This module is responsible for:
    '    - converting a map (bmpMap) to a byte array (byteNavigationArray)
    '              0 = free,  255 = occupied
    '    - adding a buffer space around occupied cells 
    '
    ''''''''''''''''''''''''''''''''''''''''''''''
    Public Sub subConvertMapToArray(ByRef intParticleNumber As Integer)
        ' Occupied = (byteMapGreenPixel = 0)  
        Try
            strStatusDriving = "Generating Map Array"
            ReDim byteNavigationArray(intMapWidth, intMapHeight)

            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
            If Not boolSlam Then
                Dim byteMapGreenPixel As Byte
                ReDim boolGreySpace(intMapWidth, intMapHeight)
                ReDim boolNoGo(intMapWidth, intMapHeight)

                For intX As Integer = 0 To intMapWidth - 1   ' Convert the map image into byteNavigationArray.   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
                            boolOccupiedCell(0, intX, intY) = True
                            byteNavigationArray(intX, intY) = 255
                        Else
                            boolGreySpace(intX, intY) = (byteMapGreenPixel = 223)
                            boolNoGo(intX, intY) = (byteMapGreenPixel = 1)
                        End If
                    Next intY
                Next intX
            Else
                For intX As Integer = 0 To intMapWidth - 1   ' Convert the map image into byteNavigationArray.   0 = free,  255 = occupied
                    For intY As Integer = 0 To intMapHeight - 1
                        intPixelIndex = intMapWidth * 4 * intY + intX * 4 + 1   ' Green Pixel
                        If (byteMapRGB(intPixelIndex) = 0) Then
                            boolOccupiedCell(0, intX, intY) = True
                            byteNavigationArray(intX, intY) = 255
                        End If
                    Next intY
                Next intX
            End If
            subUnLockMap(bmpMapData)    ' Unlock the bits.
            subGenerateDistanceFromWallSquaredArray(intParticleNumber)
            strStatusDriving = "Map Array Generated "
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subConvertMapToArray :  " & ex.ToString)
        End Try
    End Sub


    Public Sub subGenerateDistanceFromWallSquaredArray(ByRef intParticleNumber As Integer) ' TODO Set periphery = intOneDotFiveSigmaSquared.  Speeds up ftnDistanceFromWallErrorSquared as dont have to check for ftnLocationOnMapEdge
        Dim intDistanceFromWallSquared As Integer
        Dim tmcLocation1, tmcLocation2 As typeMapCoordinate
        Dim intTotallySafeDistanceFromWall As Integer = Sqrt(intTotallySafeDistanceFromWallSquared)
        Dim intX_Diff, intY_Diff, intX, intY As Integer
        Try
            For tmcLocation1.X = 1 To intMapWidth - 2 ' Mark wall edges as Distance = 0
                For tmcLocation1.Y = 1 To intMapHeight - 2
                    If boolOccupiedCell(0, tmcLocation1.X, tmcLocation1.Y) Then
                        If (Not boolOccupiedCell(0, tmcLocation1.X, tmcLocation1.Y + 1)) Then
                            intDistanceFromWallSquaredArray(intParticleNumber, tmcLocation1.X, tmcLocation1.Y) = 0
                        ElseIf (Not boolOccupiedCell(0, tmcLocation1.X, tmcLocation1.Y - 1)) Then
                            intDistanceFromWallSquaredArray(intParticleNumber, tmcLocation1.X, tmcLocation1.Y) = 0
                        ElseIf (Not boolOccupiedCell(0, tmcLocation1.X + 1, tmcLocation1.Y)) Then
                            intDistanceFromWallSquaredArray(intParticleNumber, tmcLocation1.X, tmcLocation1.Y) = 0
                        ElseIf (Not boolOccupiedCell(0, tmcLocation1.X - 1, tmcLocation1.Y)) Then
                            intDistanceFromWallSquaredArray(intParticleNumber, tmcLocation1.X, tmcLocation1.Y) = 0
                        Else
                            intDistanceFromWallSquaredArray(intParticleNumber, tmcLocation1.X, tmcLocation1.Y) = intTotallySafeDistanceFromWallSquared
                        End If
                    Else
                        intDistanceFromWallSquaredArray(intParticleNumber, tmcLocation1.X, tmcLocation1.Y) = intTotallySafeDistanceFromWallSquared
                    End If
                Next tmcLocation1.Y
            Next tmcLocation1.X

            For intX = 0 To intMapWidth - 1
                For intY = 0 To intMapHeight - 1
                    If (intDistanceFromWallSquaredArray(intParticleNumber, intX, intY) = 0) Then  ' location is a wall edge.  Populate nearby cells with distance from this location.
                        For tmcLocation2.X = Max(intX - intTotallySafeDistanceFromWall, 0) To Min(intX + intTotallySafeDistanceFromWall, intMapWidth - 1) ' left and right neighbors
                            intX_Diff = Abs(tmcLocation2.X - intX)
                            For tmcLocation2.Y = Max(intY - intTotallySafeDistanceFromWall, 0) To Min(intY + intTotallySafeDistanceFromWall, intMapHeight - 1) ' up and down neighbors
                                intY_Diff = Abs(tmcLocation2.Y - intY)
                                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
                Next intY
            Next intX
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subGenerateDistanceFromWallSquaredArray " & ex.Message)
            frmDebug.subWriteLogs("   intParticleNumber = " & intParticleNumber)
            frmDebug.subWriteLogs("    tmcLocation1:  " & tmcLocation1.X & ", " & tmcLocation1.Y)
            frmDebug.subWriteLogs("    tmcLocation2:  " & tmcLocation2.X & ", " & tmcLocation2.Y)
        End Try
    End Sub



End Module
