param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
$ErrorActionPreference = 'Stop'
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
function Assert-True($Actual, [string]$Message) { if (-not $Actual) { throw $Message } }
$ns = 'MultiWheelC.TrajectoryPlanning.Mapping.'
$boundsType = $assembly.GetType($ns + 'MapBoundsMm', $true)
$sourceType = $assembly.GetType($ns + 'IMapObstacleSource', $true)
$requestType = $assembly.GetType($ns + 'PlanningMapRequest', $true)
$factoryType = $assembly.GetType($ns + 'PlanningMapFactory', $true)
$imageRequestType = $assembly.GetType($ns + 'PlanningMapImageExportRequest', $true)
$exporterType = $assembly.GetType($ns + 'PlanningMapImageExporter', $true)
$mapRequest = [Activator]::CreateInstance($requestType)
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]1000, [single]0, [single]1000))
$mapRequest.ResolutionMm = [single]50
$mapRequest.ObstacleSources = [Array]::CreateInstance($sourceType, 0)
$mapRequest.AllowExplicitEmptyMap = $true
$map = [Activator]::CreateInstance($factoryType).Create($mapRequest).Map
$imageRequest = [Activator]::CreateInstance($imageRequestType)
$imageRequest.Map = $map
$imageRequest.OutputRootDirectory = (Join-Path $PSScriptRoot '..\obj\planning_map_image_test')
$export = $exporterType.GetMethod('ExportIfEnabled').Invoke($null, @($true, $imageRequest))
Assert-True $export.Saved ('Image export failed: ' + $export.Message)
Assert-True (Test-Path -LiteralPath $export.FilePath -PathType Leaf) 'Image output is missing.'
$png = [IO.File]::ReadAllBytes($export.FilePath)
Assert-True ($png.Length -gt 45) 'PNG is too small.'
Assert-True ($png[0] -eq 137 -and $png[1] -eq 80 -and $png[2] -eq 78 -and $png[3] -eq 71) 'PNG signature is invalid.'
Assert-True ([Text.Encoding]::ASCII.GetString($png, 12, 4) -eq 'IHDR') 'PNG must begin with IHDR.'
Assert-True ([Text.Encoding]::ASCII.GetString($png, $png.Length - 8, 4) -eq 'IEND') 'PNG must end with IEND.'
$source = Get-Content -LiteralPath (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\Map\Test\Visualization\PlanningMapImageExporter.cs') -Raw -Encoding UTF8
Assert-True ($source -notmatch 'GridMapData|TrapMapVehiclePose|TwoLegDetect') 'New exporter must consume only PlanningGridMap.'
Write-Output 'Planning map image checks passed.'