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" } }
function Assert-False($Actual, [string]$Message) { if ($Actual) { throw $Message } }
function Assert-Null($Actual, [string]$Message) { if ($null -ne $Actual) { throw $Message } }
function Find-NonPublicInstanceMethod($Type, [string]$Name, [Type[]]$ParameterTypes) {
    foreach ($candidate in $Type.GetMethods([Reflection.BindingFlags]'Instance,NonPublic')) {
        if ($candidate.Name -ne $Name) { continue }
        $parameters = $candidate.GetParameters()
        if ($parameters.Length -ne $ParameterTypes.Length) { continue }
        $matches = $true
        for ($index = 0; $index -lt $parameters.Length; $index++) {
            if ($parameters[$index].ParameterType -ne $ParameterTypes[$index]) { $matches = $false; break }
        }
        if ($matches) { return $candidate }
    }
    return $null
}
$ns = 'MultiWheelC.TrajectoryPlanning.Mapping.'
$boundsType = $assembly.GetType($ns + 'MapBoundsMm', $true)
$obstacleType = $assembly.GetType($ns + 'IMapObstacle', $true)
$sourceType = $assembly.GetType($ns + 'IMapObstacleSource', $true)
$circleType = $assembly.GetType($ns + 'CircleObstacle', $true)
$manualType = $assembly.GetType($ns + 'ManualObstacleSource', $true)
$twoLegInputType = $assembly.GetType($ns + 'TwoLegProjectionInput', $true)
$twoLegType = $assembly.GetType($ns + 'TwoLegObstacleSource', $true)
$requestType = $assembly.GetType($ns + 'PlanningMapRequest', $true)
$factoryType = $assembly.GetType($ns + 'PlanningMapFactory', $true)
$mapResultType = $assembly.GetType($ns + 'PlanningMapBuildResult', $true)
$mapBuildStatusType = $assembly.GetType($ns + 'PlanningMapBuildStatus', $true)
$operationBudgetType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Utils.PlanningOperationBudget', $false)
$operationStopReasonType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Utils.PlanningOperationStopReason', $false)
Assert-True ($operationBudgetType -ne $null) 'PlanningOperationBudget must exist for shared planning cancellation and timeout handling.'
Assert-True ($operationStopReasonType -ne $null) 'PlanningOperationStopReason must exist for shared planning cancellation and timeout handling.'
Assert-True $operationStopReasonType.IsEnum 'PlanningOperationStopReason must be an enum.'
foreach ($expectedStopReason in @('None', 'Cancelled', 'TimedOut')) {
    Assert-True ($operationStopReasonType.GetEnumNames() -contains $expectedStopReason) "PlanningOperationStopReason must contain $expectedStopReason."
}
Assert-True ($mapResultType.GetProperty('Status') -ne $null) 'PlanningMapBuildResult must expose an explicit build status.'
foreach ($expectedMapStatus in @('Success', 'Failed', 'Cancelled', 'TimedOut')) {
    Assert-True ($mapBuildStatusType.GetEnumNames() -contains $expectedMapStatus) "PlanningMapBuildStatus must contain $expectedMapStatus."
}
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]2000, [single]0, [single]2000))
$obstacles = [Array]::CreateInstance($obstacleType, 1)
$obstacles.SetValue([Activator]::CreateInstance($circleType, @([single]500, [single]500, [single]100)), 0)
$manual = [Activator]::CreateInstance($manualType, @('manual', [long]1, $true, $obstacles))
$twoLegInput = [Activator]::CreateInstance($twoLegInputType, @($true, [single]1000, [single]1000, [double]([Math]::PI / 2), [single]100, [single]0, [single]-100, [single]0, [single]40, 'test snapshot'))
$twoLeg = [Activator]::CreateInstance($twoLegType, @('two-leg', [long]1, $false, $twoLegInput))
$sources = [Array]::CreateInstance($sourceType, 2); $sources.SetValue($twoLeg, 0); $sources.SetValue($manual, 1)
function New-Request($sourceArray, [bool]$allowEmpty) { $request = [Activator]::CreateInstance($requestType); $request.Bounds = $bounds; $request.ResolutionMm = [single]50; $request.ObstacleSources = $sourceArray; $request.AllowExplicitEmptyMap = $allowEmpty; return $request }
$factory = [Activator]::CreateInstance($factoryType)
$budgetConstructor = $operationBudgetType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
    @([Threading.CancellationToken], [TimeSpan]), $null)
Assert-True ($budgetConstructor -ne $null) 'PlanningOperationBudget must expose its internal cancellation and timeout constructor.'
$createWithBudget = Find-NonPublicInstanceMethod $factoryType 'Create' @($requestType, $operationBudgetType)
Assert-True ($createWithBudget -ne $null) 'PlanningMapFactory must expose an internal budget-aware Create overload.'
$cancelledSource = New-Object Threading.CancellationTokenSource
$cancelledSource.Cancel()
$cancelledBudget = $budgetConstructor.Invoke(@($cancelledSource.Token, [TimeSpan]::FromSeconds(1)))
$cancelled = $createWithBudget.Invoke($factory, @((New-Request $sources $false), $cancelledBudget))
Assert-Equal 'Cancelled' $cancelled.Status.ToString() 'Cancelled map construction must retain the cancellation status.'
Assert-False $cancelled.Succeeded 'Cancelled map construction must not succeed.'
Assert-Null $cancelled.Map 'Cancelled map construction must not publish a map.'
Assert-Equal 'None' $cancelled.CacheHit.ToString() 'Cancelled map construction must not publish a cache hit.'
$first = $factory.Create((New-Request $sources $false))
Assert-True $first.Succeeded 'Mixed source request must build.'
Assert-True $first.Map.PlanningReady 'Applied source geometry must make the map ready.'
Assert-True ($first.Map.IsOccupiedWorld(1.0, 1.1)) 'TwoLeg must project detection-time local coordinates into world metres.'
$second = $factory.Create((New-Request $sources $false))
Assert-Equal 'Input' $second.CacheHit.ToString() 'Same snapshot must hit complete input cache.'
Assert-True ([object]::ReferenceEquals($first.Map, $second.Map)) 'Same snapshot must return the exact immutable map object.'
$manualVersionTwo = [Activator]::CreateInstance($manualType, @('manual', [long]2, $true, $obstacles))
$sourcesVersionTwo = [Array]::CreateInstance($sourceType, 2); $sourcesVersionTwo.SetValue($manualVersionTwo, 0); $sourcesVersionTwo.SetValue($twoLeg, 1)
$third = $factory.Create((New-Request $sourcesVersionTwo $false))
Assert-Equal 'Occupancy' $third.CacheHit.ToString() 'Version change with equal occupancy must reuse occupancy buffers.'
Assert-True ($third.Map.SnapshotId -gt $first.Map.SnapshotId) 'Occupancy reuse must still issue a fresh snapshot id.'
$emptySources = [Array]::CreateInstance($sourceType, 0)
$emptyBlocked = $factory.Create((New-Request $emptySources $false))
Assert-True (-not $emptyBlocked.Map.PlanningReady) 'Implicit empty map must be blocked.'
$emptyAllowed = $factory.Create((New-Request $emptySources $true))
Assert-True $emptyAllowed.Map.PlanningReady 'Explicit empty map must be accepted.'
$invalidManual = [Activator]::CreateInstance($manualType, @('invalid', [long]0, $true, $null))
$invalidSources = [Array]::CreateInstance($sourceType, 1); $invalidSources.SetValue($invalidManual, 0)
$invalid = $factory.Create((New-Request $invalidSources $false))
Assert-True (-not $invalid.Succeeded) 'Required invalid source must reject the full build.'
Write-Output 'Planning map factory checks passed.'