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-False($Actual, [string]$Message) {
    if ($Actual) { throw $Message }
}

function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
    if ([Math]::Abs($Expected - $Actual) -gt 0.000001) { throw "$Message Expected=$Expected Actual=$Actual" }
}

function New-TestMap {
    $mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
    $boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
    $obstacleType = $assembly.GetType($mapping + 'IMapObstacle', $true)
    $rectangleType = $assembly.GetType($mapping + 'AxisAlignedRectangleObstacle', $true)
    $sourceType = $assembly.GetType($mapping + 'IMapObstacleSource', $true)
    $manualType = $assembly.GetType($mapping + 'ManualObstacleSource', $true)
    $requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
    $factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
    $bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]3000, [single]0, [single]3000))
    $obstacle = [Activator]::CreateInstance($rectangleType, @([single]1500, [single]1550, [single]1200, [single]1800))
    $obstacles = [Array]::CreateInstance($obstacleType, 1)
    $obstacles.SetValue($obstacle, 0)
    $source = [Activator]::CreateInstance($manualType, @('manual', [long]1, $true, $obstacles))
    $sources = [Array]::CreateInstance($sourceType, 1)
    $sources.SetValue($source, 0)
    $request = [Activator]::CreateInstance($requestType)
    $request.Bounds = $bounds
    $request.ResolutionMm = [single]50
    $request.ObstacleSources = $sources
    $request.AllowExplicitEmptyMap = $false
    $result = [Activator]::CreateInstance($factoryType).Create($request)
    Assert-True $result.Succeeded 'Test map must be created.'
    Assert-True $result.Map.PlanningReady 'Test map must be ready.'
    return $result.Map
}

function New-DiagonalCellMap {
    $mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
    $boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
    $obstacleType = $assembly.GetType($mapping + 'IMapObstacle', $true)
    $circleType = $assembly.GetType($mapping + 'CircleObstacle', $true)
    $sourceType = $assembly.GetType($mapping + 'IMapObstacleSource', $true)
    $manualType = $assembly.GetType($mapping + 'ManualObstacleSource', $true)
    $requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
    $factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
    $bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]3000, [single]0, [single]3000))
    $obstacle = [Activator]::CreateInstance($circleType, @([single]1525, [single]1525, [single]0))
    $obstacles = [Array]::CreateInstance($obstacleType, 1)
    $obstacles.SetValue($obstacle, 0)
    $source = [Activator]::CreateInstance($manualType, @('diagonal-cell', [long]1, $true, $obstacles))
    $sources = [Array]::CreateInstance($sourceType, 1)
    $sources.SetValue($source, 0)
    $request = [Activator]::CreateInstance($requestType)
    $request.Bounds = $bounds
    $request.ResolutionMm = [single]50
    $request.ObstacleSources = $sources
    $request.AllowExplicitEmptyMap = $false
    $result = [Activator]::CreateInstance($factoryType).Create($request)
    Assert-True $result.Succeeded 'Diagonal cell map must be created.'
    Assert-True $result.Map.PlanningReady 'Diagonal cell map must be ready.'
    Assert-True $result.Map.IsOccupied(30, 30) 'Diagonal cell obstacle must occupy its 50mm cell.'
    return $result.Map
}

$map = New-TestMap
$vehicle = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters
$vehicle.LengthMeters = 0.20
$vehicle.WidthMeters = 0.20
$vehicle.SafetyMarginMeters = 0.0
$checker = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle.FootprintCollisionChecker

$diagonalCellMap = New-DiagonalCellMap
$diagonalCellVehicle = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters
$diagonalCellVehicle.LengthMeters = 0.10
$diagonalCellVehicle.WidthMeters = 0.10
$diagonalCellVehicle.SafetyMarginMeters = 0.0
$diagonalCellPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.299, 1.299, 0.0)
$diagonalCellClearance = 0.0
$diagonalCellSafe = $checker.IsPoseCollisionFree($diagonalCellPose, $diagonalCellMap, $diagonalCellVehicle, 0.20, [ref]$diagonalCellClearance)
Assert-False $diagonalCellSafe 'Expanded diagonal footprint must not bypass an occupied 50mm cell.'

$edgePose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.35, 1.50, 0.0)
$edgeClearance = 0.0
$edgeSafe = $checker.IsPoseCollisionFree($edgePose, $map, $vehicle, 0.0, [ref]$edgeClearance)
Assert-False $edgeSafe 'Touching an occupied cell must be a collision.'

$farPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(0.50, 0.50, 0.0)
$farClearance = 0.0
$farSafe = $checker.IsPoseCollisionFree($farPose, $map, $vehicle, 0.0, [ref]$farClearance)
Assert-True $farSafe 'Strict distance field clearance must allow the distant pose.'
Assert-True ($farClearance -gt 0.0) 'Distant pose must report positive body clearance.'

$from = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.20, 1.50, 0.0)
$to = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.80, 1.50, 0.0)
$sweepClearance = 0.0
$sweepSafe = $checker.IsSweptMotionCollisionFree($from, $to, $map, $vehicle, 0.50, [ref]$sweepClearance)
Assert-False $sweepSafe 'A swept vehicle must not pass through an occupied cell.'

$gridCenterPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.475, 1.475, 0.0)
$gridCenterClearance = 0.0
Assert-False $checker.IsPoseCollisionFree($gridCenterPose, $map, $vehicle, 0.0, [ref]$gridCenterClearance) 'A pose at an occupied grid center must collide.'

$subGridPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.481, 1.463, 0.37)
$subGridClearance = 0.0
Assert-False $checker.IsPoseCollisionFree($subGridPose, $map, $vehicle, 0.0, [ref]$subGridClearance) 'An arbitrary sub-grid heading must collide with the thin obstacle.'

$diagonalPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.32, 1.50, ([Math]::PI / 4.0))
$diagonalClearance = 0.0
Assert-False $checker.IsPoseCollisionFree($diagonalPose, $map, $vehicle, 0.0, [ref]$diagonalClearance) 'A 45 degree footprint overlap must collide.'

$outsidePose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(0.05, 0.05, 0.0)
$outsideClearance = 0.0
Assert-False $checker.IsPoseCollisionFree($outsidePose, $map, $vehicle, 0.0, [ref]$outsideClearance) 'Any footprint corner outside the map must collide.'

$curveVehicle = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters
$curveVehicle.MaximumCurvaturePerMeter = 0.80
$curveVehicle.MinimumTurningRadiusMeters = 2.0
$maximumCurvature = 0.0
$hasMaximumCurvature = [MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle.VehicleKinematics]::TryGetMaximumCurvaturePerMeter($curveVehicle, [ref]$maximumCurvature)
Assert-True $hasMaximumCurvature 'Vehicle curvature constraints must be accepted.'
Assert-Near 0.50 $maximumCurvature 'Both curvature constraints must use the conservative minimum.'

Write-Output 'Coarse path collision checks passed.'