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 } }
function Assert-Equal($Expected, $Actual, [string]$Message) { if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" } }
$boundsType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm', $true)
$gridType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.EnvironmentGridMap', $true)
$circleType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.CircleObstacle', $true)
$rectangleType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.AxisAlignedRectangleObstacle', $true)
$rasterizer = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapObstacleRasterizer', $true)
$adapter = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapAdapter', $true)
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]105, [single]0, [single]105))
$grid = [Activator]::CreateInstance($gridType, @($bounds, [single]20))
Assert-Equal $true ($grid.IsWorldInBounds([single]104.9, [single]104.9)) 'Last partial cell must be inside.'
Assert-Equal $false ($grid.IsWorldInBounds([single]105, [single]50)) 'XMax must be exclusive.'
Assert-Equal $true ($grid.IsOccupiedWorld([single]-0.1, [single]20)) 'Outside world must be occupied.'
$circle = [Activator]::CreateInstance($circleType, @([single]40, [single]40, [single]0))
$rasterizer.GetMethod('Rasterize').Invoke($null, @($grid, $circle)) | Out-Null
Assert-True ($grid.OccupiedCount -ge 4) 'Circle touching a grid intersection must conservatively occupy adjacent cells.'
$rectangle = [Activator]::CreateInstance($rectangleType, @([single]80, [single]100, [single]80, [single]100))
$rasterizer.GetMethod('Rasterize').Invoke($null, @($grid, $rectangle)) | Out-Null
Assert-True ($grid.IsOccupiedWorld([single]90, [single]90)) 'Rectangle must occupy its intersecting cells.'
$outside = [Activator]::CreateInstance($circleType, @([single]500, [single]500, [single]10))
$before = $grid.OccupiedCount
$rasterizer.GetMethod('Rasterize').Invoke($null, @($grid, $outside)) | Out-Null
Assert-Equal $before $grid.OccupiedCount 'Outside obstacle must not mark cells.'
$planning = $adapter.GetMethod('Create').Invoke($null, @($grid))
Assert-True ($planning.IsOccupiedWorld(0.04, 0.04)) 'Planning map must use metre world coordinates.'
Assert-Equal 0.0 ($planning.GetConservativeObstacleDistanceMeters(0.04, 0.04)) 'Occupied cell clearance must be zero.'
Assert-Equal 0.0 ($planning.GetConservativeObstacleDistanceMeters(-1.0, 0.0)) 'Outside map clearance must be zero.'
$singleBounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]3000, [single]0, [single]3000))
$singleGrid = [Activator]::CreateInstance($gridType, @($singleBounds, [single]50))
$singleCircle = [Activator]::CreateInstance($circleType, @([single]1525, [single]1525, [single]0))
$rasterizer.GetMethod('Rasterize').Invoke($null, @($singleGrid, $singleCircle)) | Out-Null
Assert-Equal 1 $singleGrid.OccupiedCount 'Zero radius circle at a cell center must occupy exactly one cell.'
$singlePlanning = $adapter.GetMethod('Create').Invoke($null, @($singleGrid))
$singleDistance = $singlePlanning.GetConservativeObstacleDistanceMeters(1.275, 1.275)
$singleNearestCellCenterDistance = [Math]::Sqrt(0.25 * 0.25 + 0.25 * 0.25)
Assert-True ((-not [double]::IsNaN($singleDistance)) -and (-not [double]::IsInfinity($singleDistance))) 'Single occupied cell clearance must be finite.'
Assert-True ($singleDistance -le $singleNearestCellCenterDistance) 'Single occupied cell clearance must not exceed nearest occupied cell geometry distance.'
$emptyGrid = [Activator]::CreateInstance($gridType, @($bounds, [single]20))
$emptyPlanning = $adapter.GetMethod('Create').Invoke($null, @($emptyGrid))
Assert-True ([double]::IsPositiveInfinity($emptyPlanning.GetConservativeObstacleDistanceMeters(0.01, 0.01))) 'Empty map clearance must be positive infinity.'
$invalidResolutionRejected = $false
try { [Activator]::CreateInstance($gridType, @($bounds, [single]10)) | Out-Null } catch { $invalidResolutionRejected = $true }
Assert-True $invalidResolutionRejected 'Resolution below 20 mm must fail.'
Write-Output 'Planning map adapter checks passed.'