param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
$ErrorActionPreference = 'Stop'

$plannerRoot = Join-Path $PSScriptRoot '..\ParkrobTrajplanner'
$coarsePathReadme = Join-Path $plannerRoot 'CoarsePath\README.md'
$legacyRootReadme = Join-Path $plannerRoot 'README.md'
if (-not (Test-Path -LiteralPath $coarsePathReadme -PathType Leaf)) {
    throw 'CoarsePath module README must be located inside the CoarsePath directory.'
}
if (Test-Path -LiteralPath $legacyRootReadme -PathType Leaf) {
    throw 'The coarse-path-only README must not remain at the ParkrobTrajplanner root.'
}
$coarsePathReadmeContent = Get-Content -LiteralPath $coarsePathReadme -Raw
if (-not $coarsePathReadmeContent.Contains('## 总预算与取消')) {
    throw 'CoarsePath README must document total timeout and cancellation semantics.'
}
if (-not $coarsePathReadmeContent.Contains('PlanningMapBuildStatus')) {
    throw 'CoarsePath README must document map-stage termination status handling.'
}

$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-Equal($Expected, $Actual, [string]$Message) {
    if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
}

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

function Find-Method($Type, [string]$Name, [Type[]]$ParameterTypes) {
    foreach ($candidate in $Type.GetMethods()) {
        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
}

function New-EmptyPlanningMap {
    $mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
    $boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
    $requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
    $factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
    $request = [Activator]::CreateInstance($requestType)
    $request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
    $request.ResolutionMm = [single]50
    $request.AllowExplicitEmptyMap = $true
    $result = [Activator]::CreateInstance($factoryType).Create($request)
    Assert-True $result.Succeeded 'Empty integration map must be created.'
    Assert-True $result.Map.PlanningReady 'Explicit empty integration map must be ready.'
    return $result.Map
}

$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
$plannerType = $assembly.GetType($coarsePath + 'HybridAStarPlanner', $true)
$planner = [Activator]::CreateInstance($plannerType)
$requestType = $assembly.GetType($coarsePath + 'PlanningRequest', $true)
$poseType = $assembly.GetType($coarsePath + 'Pose2D', $true)
$vehicleType = $assembly.GetType($coarsePath + 'VehicleParameters', $true)
$configurationType = $assembly.GetType($coarsePath + 'HybridAStarConfiguration', $true)
$goalDirectionType = $assembly.GetType($coarsePath + 'GoalDirectionConstraint', $true)
$plan = Find-Method $plannerType 'Plan' @($requestType, [Threading.CancellationToken])
Assert-True ($plan -ne $null) 'HybridAStarPlanner must expose Plan(request, cancellationToken).'

$defaultConfiguration = [Activator]::CreateInstance($configurationType)
Assert-Near 0.15 $defaultConfiguration.GoalPositionToleranceMeters 'Default goal-position tolerance must retain the documented 0.15 m terminal acceptance radius.'

$request = [Activator]::CreateInstance($requestType)
$request.Map = New-EmptyPlanningMap
$request.Start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
$request.Goal = [Activator]::CreateInstance($poseType, @(1.20, 1.0, 0.0))
$request.Vehicle = [Activator]::CreateInstance($vehicleType)
$request.Vehicle.LengthMeters = 0.20
$request.Vehicle.WidthMeters = 0.20
$request.Vehicle.SafetyMarginMeters = 0.0
$request.Vehicle.MaximumCurvaturePerMeter = 1.0
$request.Configuration = [Activator]::CreateInstance($configurationType)
$request.Configuration.MaximumExpandedNodes = 10000
$request.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(2.0)
$request.Configuration.GoalPositionToleranceMeters = 0.001
$request.Configuration.GoalHeadingToleranceRadians = 0.001
$request.Configuration.AllowReverse = $false
$request.GoalDirection = [Enum]::Parse($goalDirectionType, 'Forward')
$request.StartVehicleCurvature = [double]0.20

$result = $plan.Invoke($planner, @($request, [Threading.CancellationToken]::None))
Assert-Equal 'Success' $result.Status.ToString() 'Planner must publish a validated direct path on an empty map.'
Assert-True ($result.Path.Count -ge 2) 'Validated path must retain its start and goal points.'
Assert-Near 0.0 $result.Path[0].ArcLength 'The first path point must have zero arc length.'
Assert-Near 0.20 $result.Path[0].VehicleCurvature `
    'The assembled start point must retain the requested physical vehicle curvature.'
Assert-Equal 'Start' $result.Path[0].Source.ToString() 'The first path point must retain the start source.'
Assert-Equal 'GoalTruncation' $result.Path[-1].Source.ToString() 'The terminal truncated point must retain its source.'
Assert-True ($result.Diagnostics.PeakOpenListCount -ge 1) 'Planner diagnostics must retain the actual Open List peak count.'
Assert-True ($result.Diagnostics.StaleOpenListEntryCount -ge 0) 'Planner diagnostics must retain a non-negative stale Open List count.'
Assert-True ($result.Diagnostics.GetType().GetProperty('PathSearchElapsed') -ne $null) 'Planner diagnostics must expose path-search elapsed time.'
Assert-True ($result.Diagnostics.PathSearchElapsed -ge [TimeSpan]::Zero) 'Successful planning must retain non-negative path-search time.'
Assert-True ($result.Diagnostics.PathSearchElapsed -le $result.Diagnostics.Elapsed) 'Path-search time must not exceed total elapsed time.'

$nodeLimitedRequest = [Activator]::CreateInstance($requestType)
$nodeLimitedRequest.Map = $request.Map
$nodeLimitedRequest.Start = $request.Start
$nodeLimitedRequest.Goal = $request.Goal
$nodeLimitedRequest.Vehicle = $request.Vehicle
$nodeLimitedRequest.Configuration = [Activator]::CreateInstance($configurationType)
$nodeLimitedRequest.Configuration.MaximumExpandedNodes = 0
$nodeLimitedRequest.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(2)
$nodeLimitedRequest.Configuration.GoalPositionToleranceMeters = 0.001
$nodeLimitedRequest.Configuration.GoalHeadingToleranceRadians = 0.001
$nodeLimitedRequest.Configuration.AllowReverse = $false
$nodeLimitedRequest.GoalDirection = [Enum]::Parse($goalDirectionType, 'Forward')
$nodeLimitedResult = $plan.Invoke($planner, @($nodeLimitedRequest, [Threading.CancellationToken]::None))
Assert-Equal 'SearchNodeLimitExceeded' $nodeLimitedResult.Status.ToString() 'A zero node limit must fail after planner preflight.'
Assert-True ($nodeLimitedResult.Diagnostics.PathSearchElapsed -ge [TimeSpan]::Zero) 'Search-stage node-limit failure must retain path-search time.'
Assert-True ($nodeLimitedResult.Diagnostics.PathSearchElapsed -le $nodeLimitedResult.Diagnostics.Elapsed) 'Failed path-search time must not exceed total elapsed time.'
Assert-True (-not [string]::IsNullOrWhiteSpace($nodeLimitedResult.Diagnostics.TerminationReason)) 'Planner diagnostics must retain a node-limit reason.'

for ($index = 1; $index -lt $result.Path.Count; $index++) {
    $previous = $result.Path[$index - 1]
    $current = $result.Path[$index]
    Assert-True ($current.ArcLength -ge $previous.ArcLength) 'Path arc length must be non-decreasing.'
    Assert-True ([Math]::Abs($current.UnwrappedHeading - $previous.UnwrappedHeading) -le ([Math]::PI + 0.000001)) 'Unwrapped heading must remain continuous between adjacent points.'
    $samePose = ([Math]::Abs($current.X - $previous.X) -le 0.000001) -and
        ([Math]::Abs($current.Y - $previous.Y) -le 0.000001) -and
        ([Math]::Abs($current.Heading - $previous.Heading) -le 0.000001) -and
        ([Math]::Abs($current.ArcLength - $previous.ArcLength) -le 0.000001)
    if ($samePose) {
        Assert-True (($current.Direction -ne $previous.Direction) -and $current.IsGearSwitchPoint) 'Adjacent duplicate points are permitted only as a marked gear-switch pair.'
    }
}

Assert-True ($result.Segments.Count -ge 1) 'Validated path must include inclusive direction segments.'
Assert-Equal 0 $result.Segments[0].StartIndex 'The first segment must include the first path point.'
Assert-Equal ($result.Path.Count - 1) $result.Segments[-1].EndIndex 'The final segment must include the final path point.'
for ($index = 0; $index -lt $result.Segments.Count; $index++) {
    $segment = $result.Segments[$index]
    Assert-Equal $index $segment.SegmentIndex 'Segment indexes must be contiguous.'
    Assert-True ($segment.StartIndex -le $segment.EndIndex) 'Each segment must use inclusive ordered indexes.'
    if ($index -gt 0) {
        $previousSegment = $result.Segments[$index - 1]
        Assert-Equal ($previousSegment.EndIndex + 1) $segment.StartIndex 'Segments must cover each path point exactly once.'
    }
}

Write-Output 'Coarse path integration checks passed.'

# Facade checks intentionally use only ASCII text so the script can also run in legacy PowerShell hosts.
$facade = $coarsePath + 'Facade.'
$serviceType = $assembly.GetType($facade + 'CoarsePathPlanningService', $true)
$jobType = $assembly.GetType($facade + 'CoarsePathPlanningJob', $true)
$jobResultType = $assembly.GetType($facade + 'CoarsePathPlanningJobResult', $true)
$debugOptionsType = $assembly.GetType($facade + 'PlanningDebugOptions', $true)
$debugSinkType = $assembly.GetType($facade + 'IPlanningDebugSink', $true)
$mapRequestType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest', $true)
$boundsType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm', $true)
$servicePlan = Find-Method $serviceType 'Plan' @($jobType, [Threading.CancellationToken])
Assert-True ($servicePlan -ne $null) 'CoarsePathPlanningService must expose Plan(job, cancellationToken).'
Assert-True ($jobResultType.GetProperty('MapResult') -ne $null) 'Facade result must retain the map result.'
Assert-True ($jobResultType.GetProperty('PlanningResult') -ne $null) 'Facade result must retain the planning result.'
Assert-True ($jobResultType.GetProperty('DebugDiagnostics') -ne $null) 'Facade result must retain debug diagnostics.'

function New-FacadeJob {
    $job = [Activator]::CreateInstance($jobType)
    $mapRequest = [Activator]::CreateInstance($mapRequestType)
    $mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
    $mapRequest.ResolutionMm = [single]50
    $mapRequest.AllowExplicitEmptyMap = $true
    $job.MapRequest = $mapRequest
    $job.Start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
    $job.Goal = [Activator]::CreateInstance($poseType, @(1.20, 1.0, 0.0))
    $job.Vehicle = [Activator]::CreateInstance($vehicleType)
    $job.Vehicle.LengthMeters = 0.20
    $job.Vehicle.WidthMeters = 0.20
    $job.Vehicle.SafetyMarginMeters = 0.0
    $job.Vehicle.MaximumCurvaturePerMeter = 1.0
    $job.Configuration = [Activator]::CreateInstance($configurationType)
    $job.Configuration.MaximumExpandedNodes = 10000
    $job.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(2.0)
    $job.Configuration.GoalPositionToleranceMeters = 0.001
    $job.Configuration.GoalHeadingToleranceRadians = 0.001
    $job.Configuration.AllowReverse = $false
    $job.GoalDirection = [Enum]::Parse($goalDirectionType, 'Forward')
    return $job
}

$service = [Activator]::CreateInstance($serviceType)
$firstFacadeResult = $servicePlan.Invoke($service, @((New-FacadeJob), [Threading.CancellationToken]::None))
Assert-True $firstFacadeResult.MapResult.Succeeded 'Facade must build the requested map first.'
Assert-Equal 'Success' $firstFacadeResult.PlanningResult.Status.ToString() 'Facade must pass a successful map to the planner.'
Assert-Equal 0 $firstFacadeResult.DebugDiagnostics.Count 'Default debug sink must not add diagnostics.'

$secondFacadeResult = $servicePlan.Invoke($service, @((New-FacadeJob), [Threading.CancellationToken]::None))
Assert-True $secondFacadeResult.MapResult.Succeeded 'Repeated facade request must retain a map result.'
Assert-Equal 'Input' $secondFacadeResult.MapResult.CacheHit.ToString() 'One facade service must retain its map factory cache.'
Assert-Equal $firstFacadeResult.MapResult.Map.InputFingerprint $secondFacadeResult.MapResult.Map.InputFingerprint 'Map fingerprint must remain stable for equal input.'
Assert-Equal $firstFacadeResult.PlanningResult.Status $secondFacadeResult.PlanningResult.Status 'Map cache reuse must not change planning status.'
Assert-Equal $firstFacadeResult.PlanningResult.Path.Count $secondFacadeResult.PlanningResult.Path.Count 'Map cache reuse must not change path point count.'

$invalidJob = [Activator]::CreateInstance($jobType)
$invalidJob.MapRequest = [Activator]::CreateInstance($mapRequestType)
$mapFailureResult = $servicePlan.Invoke($service, @($invalidJob, [Threading.CancellationToken]::None))
Assert-False $mapFailureResult.MapResult.Succeeded 'Invalid map input must be returned as a map failure.'
Assert-Equal 'InvalidMap' $mapFailureResult.PlanningResult.Status.ToString() 'Map failure must return an empty non-search planning result.'
Assert-Equal 0 $mapFailureResult.PlanningResult.Diagnostics.ExpandedNodeCount 'Map failure must not start the search.'
Assert-Equal 0 $mapFailureResult.PlanningResult.Path.Count 'Map failure must not publish a path.'
Assert-Equal ([TimeSpan]::Zero) $mapFailureResult.PlanningResult.Diagnostics.PathSearchElapsed 'Map failure must report zero path-search time.'

$cancelledFacadeSource = New-Object Threading.CancellationTokenSource
$cancelledFacadeSource.Cancel()
$cancelledFacadeResult = $servicePlan.Invoke($service, @((New-FacadeJob), $cancelledFacadeSource.Token))
Assert-Equal 'Cancelled' $cancelledFacadeResult.MapResult.Status.ToString() 'Facade must retain a cancelled map result.'
Assert-Equal 'Cancelled' $cancelledFacadeResult.PlanningResult.Status.ToString() 'Facade must map map-stage cancellation to planning cancellation.'
Assert-Equal 0 $cancelledFacadeResult.PlanningResult.Path.Count 'Cancelled facade planning must publish no path.'
Assert-Equal 0 $cancelledFacadeResult.PlanningResult.Segments.Count 'Cancelled facade planning must publish no segments.'

$timedOutFacadeJob = New-FacadeJob
$timedOutFacadeJob.Configuration.SearchTimeout = [TimeSpan]::Zero
$timedOutFacadeResult = $servicePlan.Invoke($service, @($timedOutFacadeJob, [Threading.CancellationToken]::None))
Assert-Equal 'TimedOut' $timedOutFacadeResult.MapResult.Status.ToString() 'Facade total timeout must stop before publishing a map.'
Assert-Equal 'SearchTimeout' $timedOutFacadeResult.PlanningResult.Status.ToString() 'Facade must map map-stage timeout to planning timeout.'
Assert-Equal 0 $timedOutFacadeResult.PlanningResult.Path.Count 'Timed out facade planning must publish no path.'

$defaultDebugOptions = [Activator]::CreateInstance($debugOptionsType)
Assert-False $defaultDebugOptions.Enabled 'Debug output must be opt-in.'
Assert-True ($defaultDebugOptions.Sink -ne $null) 'Debug options must default to a no-op sink.'

$debugSinkSource = @'
using System;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;

public sealed class FacadeRecordingDebugSink : IPlanningDebugSink
{
    public int PublishCount { get; private set; }
    public void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult)
    {
        PublishCount++;
    }
}

public sealed class FacadeThrowingDebugSink : IPlanningDebugSink
{
    public void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult)
    {
        throw new InvalidOperationException("debug sink failure");
    }
}
'@
$runtimeDirectory = [Runtime.InteropServices.RuntimeEnvironment]::GetRuntimeDirectory()
$debugSinkReferences = @(
    (Join-Path $runtimeDirectory 'mscorlib.dll'),
    (Join-Path $runtimeDirectory 'System.dll'),
    (Join-Path $runtimeDirectory 'System.Core.dll'),
    $AssemblyPath,
    'C:\Program Files (x86)\Reference Assemblies\Microsoft\Framework\.NETFramework\v4.8\Facades\netstandard.dll'
)
Add-Type -TypeDefinition $debugSinkSource -ReferencedAssemblies $debugSinkReferences
$recordingSinkType = 'FacadeRecordingDebugSink' -as [type]
$throwingSinkType = 'FacadeThrowingDebugSink' -as [type]

$disabledDebugOptions = [Activator]::CreateInstance($debugOptionsType)
$recordingSink = [Activator]::CreateInstance($recordingSinkType)
$disabledDebugOptions.Sink = $recordingSink
$disabledJob = New-FacadeJob
$disabledJob.DebugOptions = $disabledDebugOptions
$disabledResult = $servicePlan.Invoke($service, @($disabledJob, [Threading.CancellationToken]::None))
Assert-Equal 0 $recordingSink.PublishCount 'Disabled debug output must not publish to the sink.'
Assert-Equal $firstFacadeResult.PlanningResult.Status $disabledResult.PlanningResult.Status 'Disabled debug output must not change planning status.'

$throwingDebugOptions = [Activator]::CreateInstance($debugOptionsType)
$throwingDebugOptions.Enabled = $true
$throwingDebugOptions.Sink = [Activator]::CreateInstance($throwingSinkType)
$throwingJob = New-FacadeJob
$throwingJob.DebugOptions = $throwingDebugOptions
$throwingResult = $servicePlan.Invoke($service, @($throwingJob, [Threading.CancellationToken]::None))
Assert-Equal $firstFacadeResult.MapResult.Map.InputFingerprint $throwingResult.MapResult.Map.InputFingerprint 'Debug sink errors must not change the map fingerprint.'
Assert-Equal $firstFacadeResult.PlanningResult.Status $throwingResult.PlanningResult.Status 'Debug sink errors must not change planning status.'
Assert-Equal $firstFacadeResult.PlanningResult.Path.Count $throwingResult.PlanningResult.Path.Count 'Debug sink errors must not change the path.'
Assert-Equal 1 $throwingResult.DebugDiagnostics.Count 'Debug sink errors must be retained as diagnostics.'

Write-Output 'Coarse path facade checks passed.'

# P1 UI scenario factory contract checks. These run against the same built assembly
# and intentionally fail until the factory is introduced.
$testNamespace = $coarsePath + 'Test.'
$scenarioEnumType = $assembly.GetType($testNamespace + 'CoarsePathTestScenario', $false)
$scenarioFactoryType = $assembly.GetType($testNamespace + 'CoarsePathScenarioFactory', $false)
Assert-True ($scenarioEnumType -ne $null) 'P1 scenario enum must exist.'
Assert-True ($scenarioFactoryType -ne $null) 'P1 scenario factory must exist.'

$factoryCreate = Find-Method $scenarioFactoryType 'Create' @($scenarioEnumType)
$factoryCreateAtAmr = Find-Method $scenarioFactoryType 'Create' @(
    $scenarioEnumType, [double], [double], [double])
$factoryManual = Find-Method $scenarioFactoryType 'CreateManualGoalDemo' @(
    [double], [double], [double], [double], [double], [double])
Assert-True ($factoryCreate -ne $null) 'P1 scenario factory must expose Create(scenario).'
Assert-True ($factoryCreateAtAmr -ne $null) 'Scenario factory must expose Create(scenario, amrX, amrY, amrHeading).'
Assert-True ($factoryManual -ne $null) 'P1 scenario factory must expose CreateManualGoalDemo with six doubles.'

$scenarioNames = @('ExplicitEmpty', 'RectangleDetour', 'ManualAndTwoLeg', 'CacheHit', 'ReverseGearSwitch', 'NoFeasiblePath')
foreach ($scenarioName in $scenarioNames) {
    $scenario = [Enum]::Parse($scenarioEnumType, $scenarioName)
    $jobA = $factoryCreate.Invoke($null, @($scenario))
    $jobB = $factoryCreate.Invoke($null, @($scenario))
    Assert-True ($jobA -ne $null) "Scenario $scenarioName must return a job."
    Assert-False ([object]::ReferenceEquals($jobA, $jobB)) "Scenario $scenarioName must return a new job per call."
}

foreach ($scenarioName in $scenarioNames) {
    $scenario = [Enum]::Parse($scenarioEnumType, $scenarioName)
    $liveJob = $factoryCreateAtAmr.Invoke($null, @($scenario, 12345.0, -6789.0, 135.0))
    Assert-Near 12.345 $liveJob.Start.X "Live $scenarioName start X must equal the AMR X."
    Assert-Near -6.789 $liveJob.Start.Y "Live $scenarioName start Y must equal the AMR Y."
    Assert-Near (3.0 * [Math]::PI / 4.0) $liveJob.Start.Heading "Live $scenarioName heading must equal the AMR heading."
}

$rectangleScenario = [Enum]::Parse($scenarioEnumType, 'RectangleDetour')
$liveRectangle = $factoryCreateAtAmr.Invoke($null, @($rectangleScenario, 12000.0, -3000.0, 90.0))
Assert-Near 12.0 $liveRectangle.Start.X 'Live rectangle start X must equal the AMR X.'
Assert-Near -3.0 $liveRectangle.Start.Y 'Live rectangle start Y must equal the AMR Y.'
Assert-Near ([Math]::PI / 2.0) $liveRectangle.Start.Heading 'Live rectangle start heading must equal the AMR heading.'
Assert-Near 16.0 $liveRectangle.Goal.X 'Live rectangle goal X must preserve the four-metre relative offset.'
Assert-Near -3.0 $liveRectangle.Goal.Y 'Live rectangle goal Y must preserve the relative offset.'
Assert-Near ([Math]::PI / 2.0) $liveRectangle.Goal.Heading 'Live rectangle goal heading must preserve the zero baseline heading delta.'
Assert-Near 11000.0 $liveRectangle.MapRequest.Bounds.XMin 'Live rectangle map X minimum must translate with the AMR.'
Assert-Near -1000.0 $liveRectangle.MapRequest.Bounds.YMax 'Live rectangle map Y maximum must translate with the AMR.'
$rectangleProjection = $liveRectangle.MapRequest.ObstacleSources[0].ProjectToWorld()
$rectangleObstacle = $rectangleProjection.Obstacles[0]
Assert-Near 13700.0 $rectangleObstacle.XMin 'Live rectangle obstacle X minimum must translate with the AMR.'
Assert-Near -2200.0 $rectangleObstacle.YMax 'Live rectangle obstacle Y maximum must translate with the AMR.'

$multiScenario = [Enum]::Parse($scenarioEnumType, 'ManualAndTwoLeg')
$baselineMulti = $factoryCreate.Invoke($null, @($multiScenario))
$liveMulti = $factoryCreateAtAmr.Invoke($null, @($multiScenario, 7000.0, 8000.0, 45.0))
Assert-Near 7.0 $liveMulti.Start.X 'Live multi-source start X must equal AMR X.'
Assert-Near 8.0 $liveMulti.Start.Y 'Live multi-source start Y must equal AMR Y.'
Assert-Near ([Math]::PI / 4.0) $liveMulti.Goal.Heading 'Live multi-source goal heading must follow AMR heading.'
$baselineTwoLeg = $baselineMulti.MapRequest.ObstacleSources[1].ProjectToWorld().Obstacles
$liveTwoLeg = $liveMulti.MapRequest.ObstacleSources[1].ProjectToWorld().Obstacles
Assert-Near ($baselineTwoLeg[0].CenterX + 6000.0) $liveTwoLeg[0].CenterX 'TwoLeg X must translate without rotation.'
Assert-Near ($baselineTwoLeg[0].CenterY + 7000.0) $liveTwoLeg[0].CenterY 'TwoLeg Y must translate without rotation.'

$noPathScenario = [Enum]::Parse($scenarioEnumType, 'NoFeasiblePath')
$liveNoPath = $factoryCreateAtAmr.Invoke($null, @($noPathScenario, 9000.0, -1000.0, -180.0))
Assert-Near 9.0 $liveNoPath.Start.X 'Live infeasible scenario must use AMR X.'
Assert-Near -1.0 $liveNoPath.Start.Y 'Live infeasible scenario must use AMR Y.'
Assert-Near 8000.0 $liveNoPath.MapRequest.Bounds.XMin 'Live infeasible map must translate with its baseline start.'

$invalidLivePoseRejected = $false
try { $null = $factoryCreateAtAmr.Invoke($null, @($rectangleScenario, [double]::NaN, 0.0, 0.0)) }
catch [ArgumentOutOfRangeException] {
    $invalidLivePoseRejected = $true
}
catch [Reflection.TargetInvocationException] {
    $invalidLivePoseRejected = $_.Exception.InnerException -is [ArgumentOutOfRangeException]
}
Assert-True $invalidLivePoseRejected 'Live factory must reject non-finite AMR coordinates.'

$manualJob = $factoryManual.Invoke($null, @(1000.0, 2000.0, 90.0, 4000.0, 2000.0, 0.0))
Assert-Near 1.0 $manualJob.Start.X 'Manual AMR X must convert mm to m.'
Assert-Near 2.0 $manualJob.Start.Y 'Manual AMR Y must convert mm to m.'
Assert-Near ([Math]::PI / 2.0) $manualJob.Start.Heading 'Manual AMR heading must convert degrees to radians.'
Assert-Near 4.0 $manualJob.Goal.X 'Manual goal X must convert mm to m.'
Assert-Near 0.0 $manualJob.Goal.Heading 'Manual goal heading must convert degrees to radians.'
Assert-True $manualJob.MapRequest.AllowExplicitEmptyMap 'Manual goal demo must declare its empty map explicitly.'

$slowFeasibleJob = $factoryManual.Invoke($null, @(1000.0, 2000.0, 0.0, 1500.0, 2500.0, 90.0))
$slowFeasibleJob.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(30)
$slowFeasibleJob.Configuration.MaximumExpandedNodes = 1000000
$slowFeasibleService = [Activator]::CreateInstance($serviceType)
$slowFeasibleResult = $servicePlan.Invoke($slowFeasibleService, @($slowFeasibleJob, [Threading.CancellationToken]::None))
Assert-Equal 'Success' $slowFeasibleResult.PlanningResult.Status.ToString() 'The previously five-second-limited feasible pose must succeed with a caller-selected longer budget.'
Assert-True ($slowFeasibleResult.PlanningResult.Diagnostics.PathSearchElapsed -le $slowFeasibleResult.PlanningResult.Diagnostics.Elapsed) 'Slow feasible path-search time must remain within total elapsed time.'

$manualObstacleKindType = $assembly.GetType($testNamespace + 'ManualCoarsePathObstacleKind', $false)
$manualObstacleType = $assembly.GetType($testNamespace + 'ManualCoarsePathObstacle', $false)
Assert-True ($manualObstacleKindType -ne $null) 'Manual obstacle kind enum must exist.'
Assert-True ($manualObstacleType -ne $null) 'Manual obstacle value type must exist.'

$manualCircle = Find-Method $manualObstacleType 'Circle' @([double], [double], [double])
$manualRectangle = Find-Method $manualObstacleType 'AxisAlignedRectangle' @([double], [double], [double], [double])
$manualObstacleFactory = $scenarioFactoryType.GetMethods() | Where-Object {
    $_.Name -eq 'CreateManualObstacleDemo' -and $_.GetParameters().Length -eq 8
} | Select-Object -First 1
Assert-True ($manualCircle -ne $null) 'Manual obstacle type must create circles from center and radius.'
Assert-True ($manualRectangle -ne $null) 'Manual obstacle type must create rectangles from center and X/Y dimensions.'
Assert-True ($manualObstacleFactory -ne $null) 'Scenario factory must expose CreateManualObstacleDemo with six poses, obstacles and version.'

$manualObstacles = [Array]::CreateInstance($manualObstacleType, 2)
$manualObstacles.SetValue($manualCircle.Invoke($null, @([double]6500, [double]2000, [double]200)), 0)
$manualObstacles.SetValue($manualRectangle.Invoke($null, @([double]-2000, [double]500, [double]600, [double]400)), 1)
$manualObstacleJob = $manualObstacleFactory.Invoke($null, @(
    1000.0, 2000.0, 0.0, 4000.0, 2000.0, 0.0, $manualObstacles, [long]77))
Assert-False $manualObstacleJob.MapRequest.AllowExplicitEmptyMap 'Manual obstacles must disable the explicit-empty-map mode.'
Assert-Equal 1 $manualObstacleJob.MapRequest.ObstacleSources.Count 'Manual obstacles must create one unified source.'
Assert-Equal 'manual-user-input' $manualObstacleJob.MapRequest.ObstacleSources[0].SourceId 'Manual source ID must be stable.'
Assert-Equal 77 $manualObstacleJob.MapRequest.ObstacleSources[0].SourceVersion 'Manual source version must be preserved.'
Assert-True ($manualObstacleJob.MapRequest.Bounds.XMin -le -4300.0) 'Manual map must include the rectangle outline and padding.'
Assert-True ($manualObstacleJob.MapRequest.Bounds.XMax -ge 8700.0) 'Manual map must include the circle outline and padding.'

$emptyManualObstacles = [Array]::CreateInstance($manualObstacleType, 0)
$emptyManualJob = $manualObstacleFactory.Invoke($null, @(
    1000.0, 2000.0, 0.0, 4000.0, 2000.0, 0.0, $emptyManualObstacles, [long]0))
Assert-True $emptyManualJob.MapRequest.AllowExplicitEmptyMap 'Zero manual obstacles must retain explicit empty-map mode.'
Assert-Equal 0 $emptyManualJob.MapRequest.ObstacleSources.Count 'Zero manual obstacles must not create a fake source.'

$invalidGeometryRejected = $false
try {
    $null = $manualCircle.Invoke($null, @([double]1000, [double]1000, [double]0))
}
catch [ArgumentOutOfRangeException] { $invalidGeometryRejected = $true }
catch [Reflection.TargetInvocationException] {
    $invalidGeometryRejected = $_.Exception.InnerException -is [ArgumentOutOfRangeException]
}
Assert-True $invalidGeometryRejected 'Invalid manual geometry must report argument range.'

$scenarioService = [Activator]::CreateInstance($serviceType)
foreach ($scenarioName in @('ExplicitEmpty', 'RectangleDetour', 'ManualAndTwoLeg', 'ReverseGearSwitch')) {
    $job = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, $scenarioName)))
    $result = $servicePlan.Invoke($scenarioService, @($job, [Threading.CancellationToken]::None))
    Assert-Equal 'Success' $result.PlanningResult.Status.ToString() "Scenario $scenarioName must succeed."
}

$reverseJob = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'ReverseGearSwitch')))
$reverseResult = $servicePlan.Invoke($scenarioService, @($reverseJob, [Threading.CancellationToken]::None))
Assert-True (($reverseResult.PlanningResult.Path | Where-Object { $_.IsGearSwitchPoint }).Count -ge 1) 'Reverse scenario must expose a gear-switch point.'

$noPathJob = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'NoFeasiblePath')))
$noPathResult = $servicePlan.Invoke($scenarioService, @($noPathJob, [Threading.CancellationToken]::None))
Assert-Equal 'NoFeasiblePath' $noPathResult.PlanningResult.Status.ToString() 'Barrier scenario must be infeasible.'
Assert-Equal 0 $noPathResult.PlanningResult.Path.Count 'Infeasible scenario must not publish a path.'
Assert-True ($noPathResult.PlanningResult.Diagnostics.TerminationReason.Contains('Open List')) 'No-path diagnostics must retain the exact search exhaustion reason.'
Assert-True (-not [string]::IsNullOrWhiteSpace($noPathResult.PlanningResult.Diagnostics.TerminationReason)) 'No-path diagnostics must retain a reason.'

$cacheJobA = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'CacheHit')))
$cacheJobB = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'CacheHit')))
$cacheFirst = $servicePlan.Invoke($scenarioService, @($cacheJobA, [Threading.CancellationToken]::None))
$cacheSecond = $servicePlan.Invoke($scenarioService, @($cacheJobB, [Threading.CancellationToken]::None))
Assert-Equal 'Input' $cacheSecond.MapResult.CacheHit.ToString() 'Cache-hit scenario must reuse the complete map input.'
Assert-Equal $cacheFirst.PlanningResult.Status $cacheSecond.PlanningResult.Status 'Map cache reuse must not change planning status.'

Write-Output 'Coarse path P1 scenario checks passed.'