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

function Assert-Throws([scriptblock]$Action, [string]$Message) {
    try {
        & $Action
    }
    catch {
        return
    }

    throw $Message
}

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 Find-NonPublicStaticMethod($Type, [string]$Name, [Type[]]$ParameterTypes) {
    foreach ($candidate in $Type.GetMethods([Reflection.BindingFlags]'Static,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
}

function New-TestMap {
    $mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
    $boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
    $requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
    $factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
    $bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
    $request = [Activator]::CreateInstance($requestType)
    $request.Bounds = $bounds
    $request.ResolutionMm = [single]50
    $request.AllowExplicitEmptyMap = $true
    $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-CornerBlockedMap {
    $mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
    $boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
    $requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
    $factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
    $obstacleType = $assembly.GetType($mapping + 'IMapObstacle', $true)
    $circleType = $assembly.GetType($mapping + 'CircleObstacle', $true)
    $manualType = $assembly.GetType($mapping + 'ManualObstacleSource', $true)
    $sourceType = $assembly.GetType($mapping + 'IMapObstacleSource', $true)
    $bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]150, [single]0, [single]150))
    $obstacles = [Array]::CreateInstance($obstacleType, 2)
    $obstacles.SetValue([Activator]::CreateInstance($circleType, @([single]75, [single]25, [single]0)), 0)
    $obstacles.SetValue([Activator]::CreateInstance($circleType, @([single]25, [single]75, [single]0)), 1)
    $sources = [Array]::CreateInstance($sourceType, 1)
    $sources.SetValue([Activator]::CreateInstance($manualType, @('corner-blocks', [long]1, $true, $obstacles)), 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 'Corner-blocked map must be created.'
    Assert-True $result.Map.PlanningReady 'Corner-blocked map must be ready.'
    return $result.Map
}

$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
$search = $coarsePath + 'Search.'
$primitiveType = $assembly.GetType($search + 'MotionPrimitive', $true)
$generatorType = $assembly.GetType($search + 'MotionPrimitiveGenerator', $true)
$goalCheckerType = $assembly.GetType($search + 'GoalToleranceChecker', $true)

Assert-True ($primitiveType.GetProperty('ActualLengthMeters') -ne $null) 'MotionPrimitive must expose actual length.'
Assert-True ($primitiveType.GetProperty('Points') -ne $null) 'MotionPrimitive must expose integration points.'
Assert-True ($primitiveType.GetProperty('IsGoalTruncation') -ne $null) 'MotionPrimitive must expose goal truncation state.'

$poseType = $assembly.GetType($coarsePath + 'Pose2D', $true)
$requestType = $assembly.GetType($coarsePath + 'PlanningRequest', $true)
$configurationType = $assembly.GetType($coarsePath + 'HybridAStarConfiguration', $true)
$vehicleType = $assembly.GetType($coarsePath + 'VehicleParameters', $true)
$directionType = $assembly.GetType($coarsePath + 'TravelDirection', $true)
$goalDirectionType = $assembly.GetType($coarsePath + 'GoalDirectionConstraint', $true)
$generate = Find-Method $generatorType 'Generate' @($poseType, [double], $directionType, $requestType)
Assert-True ($generate -ne $null) 'MotionPrimitiveGenerator must expose Generate(start, curvature, direction, request).'
$isSatisfied = Find-Method $goalCheckerType 'IsSatisfied' @($poseType, $poseType, $configurationType, $directionType, $goalDirectionType)
Assert-True ($isSatisfied -ne $null) 'GoalToleranceChecker must expose the planned goal check.'

$map = New-TestMap
$vehicle = [Activator]::CreateInstance($vehicleType)
$vehicle.LengthMeters = 0.20
$vehicle.WidthMeters = 0.20
$vehicle.SafetyMarginMeters = 0.0
$vehicle.MaximumCurvaturePerMeter = 1.0
$config = [Activator]::CreateInstance($configurationType)
$config.PrimitiveLengthMeters = 0.50
$config.IntegrationStepMeters = 0.05
$config.MaximumCollisionCheckStepMeters = 0.025
$config.GoalPositionToleranceMeters = 0.001
$config.GoalHeadingToleranceRadians = 0.001
$config.CurvatureLevelCount = 5
$forward = [Enum]::Parse($directionType, 'Forward')
$reverse = [Enum]::Parse($directionType, 'Reverse')
$anyDirection = [Enum]::Parse($goalDirectionType, 'Any')
$forwardOnly = [Enum]::Parse($goalDirectionType, 'Forward')
$start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
$generator = [Activator]::CreateInstance($generatorType)

$request = [Activator]::CreateInstance($requestType)
$request.Map = $map
$request.Vehicle = $vehicle
$request.Configuration = $config
$request.Goal = [Activator]::CreateInstance($poseType, @(3.0, 1.0, 0.0))
$request.GoalDirection = $anyDirection
$straight = $generate.Invoke($generator, @($start, 0.0, $forward, $request))
Assert-True ($straight -ne $null) 'Straight primitive must be generated.'
Assert-Near 0.50 $straight.ActualLengthMeters 'Straight primitive must use the configured cap.'
Assert-Near 1.50 $straight.End.X 'Straight primitive must use analytic integration.'
Assert-Near 1.00 $straight.End.Y 'Straight primitive must not drift laterally.'
Assert-Near 0.00 $straight.End.Heading 'Straight primitive must keep heading.'
Assert-True ($straight.Points.Count -ge 20) 'Point spacing must honor the collision check step.'
$previous = $start
foreach ($point in $straight.Points) {
    $distance = [Math]::Sqrt(($point.X - $previous.X) * ($point.X - $previous.X) + ($point.Y - $previous.Y) * ($point.Y - $previous.Y))
    Assert-True ($distance -le 0.025001) 'Point spacing must not exceed the map-safe collision step.'
    $previous = $point
}

$curve = $generate.Invoke($generator, @($start, 1.0, $forward, $request))
Assert-True ($curve -ne $null) 'Arc primitive must be generated.'
Assert-Near ([Math]::Sin(0.50) + 1.0) $curve.End.X 'Arc X must use analytic integration.'
Assert-Near (1.0 - [Math]::Cos(0.50) + 1.0) $curve.End.Y 'Arc Y must use analytic integration.'
Assert-Near 0.50 $curve.End.Heading 'Arc heading must use signed distance.'

$reversePrimitive = $generate.Invoke($generator, @($start, 0.0, $reverse, $request))
Assert-True ($reversePrimitive -ne $null) 'Reverse primitive must be generated.'
Assert-Near 0.50 $reversePrimitive.End.X 'Reverse straight primitive must move behind the vehicle.'

$request.Goal = [Activator]::CreateInstance($poseType, @(1.30, 1.0, 0.0))
$truncated = $generate.Invoke($generator, @($start, 0.0, $forward, $request))
Assert-True ($truncated -ne $null) 'Goal-truncated primitive must be generated.'
Assert-Near 0.30 $truncated.ActualLengthMeters 'A 0.30m goal must truncate at its first internal point.'
Assert-True $truncated.IsGoalTruncation 'Truncated primitive must record its goal source.'
Assert-Near 1.30 $truncated.End.X 'Truncated primitive must end at the goal point.'

$request.Goal = $start
$zeroLength = $generate.Invoke($generator, @($start, 0.0, $forward, $request))
Assert-True ($zeroLength -ne $null) 'Start-at-goal must produce a zero-length candidate.'
Assert-Near 0.0 $zeroLength.ActualLengthMeters 'Start-at-goal candidate must have zero length.'
Assert-True $zeroLength.IsGoalTruncation 'Start-at-goal candidate must have goal truncation source.'

$wrapPose = [Activator]::CreateInstance($poseType, @(1.0, 1.0, (-[Math]::PI + 0.0005)))
$wrapGoal = [Activator]::CreateInstance($poseType, @(1.0, 1.0, ([Math]::PI - 0.0005)))
$config.GoalHeadingToleranceRadians = 0.002
Assert-True $isSatisfied.Invoke($null, @($wrapPose, $wrapGoal, $config, $forward, $forwardOnly)) 'Goal heading tolerance must wrap across pi.'
Assert-False $isSatisfied.Invoke($null, @($wrapPose, $wrapGoal, $config, $reverse, $forwardOnly)) 'Goal direction constraint must reject reverse entry.'

$levelsMethod = Find-Method $generatorType 'GetCurvatureLevels' @($vehicleType, $configurationType)
Assert-True ($levelsMethod -ne $null) 'MotionPrimitiveGenerator must expose curvature levels.'
$levels = $levelsMethod.Invoke($generator, @($vehicle, $config))
Assert-True ($levels.Count -eq 5) 'Five configured curvature levels must be generated.'
Assert-Near -1.0 $levels[0] 'First curvature level must be negative maximum.'
Assert-Near 0.0 $levels[2] 'Middle curvature level must be straight.'
Assert-Near 1.0 $levels[4] 'Last curvature level must be positive maximum.'
$adjacent = Find-Method $generatorType 'AreCurvatureLevelsAdjacent' @([int], [int])
Assert-True ($adjacent -ne $null) 'MotionPrimitiveGenerator must expose curvature adjacency.'
Assert-True $adjacent.Invoke($null, @(2, 3)) 'Neighboring curvature levels must be allowed.'
Assert-False $adjacent.Invoke($null, @(2, 4)) 'Non-neighboring curvature levels must be rejected.'

$heapGenericType = $assembly.GetType($search + 'BinaryMinHeap`1', $true)
$heapType = $heapGenericType.MakeGenericType([string])
$heap = [Activator]::CreateInstance($heapType)
$push = Find-Method $heapType 'Push' @([string], [double], [double], [double])
$pop = Find-Method $heapType 'Pop' @()
Assert-True ($push -ne $null) 'BinaryMinHeap must expose Push(item, f, h, g).'
Assert-True ($pop -ne $null) 'BinaryMinHeap must expose Pop().'
$push.Invoke($heap, @('node-a', 8.0, 2.0, 1.0))
$push.Invoke($heap, @('node-b', 8.0, 1.0, 1.0))
$push.Invoke($heap, @('node-c', 8.0, 1.0, 3.0))
$push.Invoke($heap, @('node-d', 7.0, 9.0, 0.0))
$push.Invoke($heap, @('node-e', 8.0, 1.0, 3.0))
Assert-Equal 'node-d' $pop.Invoke($heap, @()) 'Lower F must win.'
Assert-Equal 'node-c' $pop.Invoke($heap, @()) 'Equal F and H must prefer larger G.'
Assert-Equal 'node-e' $pop.Invoke($heap, @()) 'Equal F, H and G must preserve insertion order.'
Assert-Equal 'node-b' $pop.Invoke($heap, @()) 'Equal F must prefer lower H.'
Assert-Equal 'node-a' $pop.Invoke($heap, @()) 'Remaining item must be returned last.'
$firstDeterministicHeap = [Activator]::CreateInstance($heapType)
$secondDeterministicHeap = [Activator]::CreateInstance($heapType)
foreach ($deterministicHeap in @($firstDeterministicHeap, $secondDeterministicHeap)) {
    $push.Invoke($deterministicHeap, @('repeat-a', 5.0, 2.0, 1.0))
    $push.Invoke($deterministicHeap, @('repeat-b', 5.0, 1.0, 1.0))
    $push.Invoke($deterministicHeap, @('repeat-c', 5.0, 1.0, 3.0))
}
$firstDeterministicOrder = @($pop.Invoke($firstDeterministicHeap, @()), $pop.Invoke($firstDeterministicHeap, @()), $pop.Invoke($firstDeterministicHeap, @())) -join ','
$secondDeterministicOrder = @($pop.Invoke($secondDeterministicHeap, @()), $pop.Invoke($secondDeterministicHeap, @()), $pop.Invoke($secondDeterministicHeap, @())) -join ','
Assert-Equal $firstDeterministicOrder $secondDeterministicOrder 'Equivalent heap inputs must have deterministic pop order.'

$calculatorType = $assembly.GetType($search + 'SearchCostCalculator', $true)
$calculator = [Activator]::CreateInstance($calculatorType)
$calculate = Find-Method $calculatorType 'Calculate' @([double], $directionType, [bool], [double], [double], [int], [double], $configurationType)
Assert-True ($calculate -ne $null) 'SearchCostCalculator must expose the planned equivalent-meter calculation.'
$costConfig = [Activator]::CreateInstance($configurationType)
$cost = $calculate.Invoke($calculator, @(2.0, $reverse, $true, 0.5, 1.0, 2, 0.25, $costConfig))
Assert-Near 4.55 $cost 'Reverse, gear, curvature and clearance costs must be accumulated.'
$negativeWeightConfig = [Activator]::CreateInstance($configurationType)
$negativeWeightConfig.CurvatureMagnitudeWeight = -0.01
Assert-Throws { $calculate.Invoke($calculator, @(1.0, $forward, $false, 0.0, 1.0, 0, 1.0, $negativeWeightConfig)) } 'Negative cost weights must be rejected.'
$nonFiniteWeightConfig = [Activator]::CreateInstance($configurationType)
$nonFiniteWeightConfig.ClearanceCostWeight = [double]::NaN
Assert-Throws { $calculate.Invoke($calculator, @(1.0, $forward, $false, 0.0, 1.0, 0, 1.0, $nonFiniteWeightConfig)) } 'Non-finite cost weights must be rejected.'

$dijkstraType = $assembly.GetType($search + 'GridDijkstraHeuristic', $true)
$dijkstraConstructor = $dijkstraType.GetConstructor(@($map.GetType(), [int], [int]))
Assert-True ($dijkstraConstructor -ne $null) 'GridDijkstraHeuristic must expose the planned grid constructor.'
$getCost = Find-Method $dijkstraType 'GetCost' @([int], [int])
Assert-True ($getCost -ne $null) 'GridDijkstraHeuristic must expose GetCost(row, col).'
$openHeuristic = $dijkstraConstructor.Invoke(@($map, 2, 2))
$expectedDiagonalCost = 2.0 * [Math]::Sqrt(2.0) * $map.ResolutionMeters
Assert-Near $expectedDiagonalCost $getCost.Invoke($openHeuristic, @(0, 0)) 'Open-grid diagonal movement must cost sqrt(2) per cell.'
$cornerBlockedMap = New-CornerBlockedMap
$blockedHeuristic = $dijkstraConstructor.Invoke(@($cornerBlockedMap, 1, 1))
Assert-True ([double]::IsPositiveInfinity($getCost.Invoke($blockedHeuristic, @(0, 0)))) 'Diagonal corner cutting through two occupied orthogonal cells must be forbidden.'

$operationBudgetType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Utils.PlanningOperationBudget', $true)
$operationStopReasonType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Utils.PlanningOperationStopReason', $true)
$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.'
$tryCreateDijkstra = Find-NonPublicStaticMethod $dijkstraType 'TryCreate' @($map.GetType(), [int], [int], $operationBudgetType,
    $dijkstraType.MakeByRefType(), $operationStopReasonType.MakeByRefType())
Assert-True ($tryCreateDijkstra -ne $null) 'GridDijkstraHeuristic must expose an internal budget-aware TryCreate method.'
$expiredBudget = $budgetConstructor.Invoke(@([Threading.CancellationToken]::None, [TimeSpan]::Zero))
$dijkstraArguments = [object[]]@($map, 2, 2, $expiredBudget, $null, $null)
$dijkstraCreated = $tryCreateDijkstra.Invoke($null, $dijkstraArguments)
Assert-False $dijkstraCreated 'An expired total budget must stop Dijkstra construction.'
Assert-Null $dijkstraArguments[4] 'Stopped Dijkstra construction must not publish a partial heuristic.'
Assert-Equal 'TimedOut' $dijkstraArguments[5].ToString() 'Expired Dijkstra construction must report timeout.'

$largeBounds = [Activator]::CreateInstance($assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm', $true),
    @([single]0, [single]40000, [single]0, [single]20000))
$largeMapRequest = [Activator]::CreateInstance($assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest', $true))
$largeMapRequest.Bounds = $largeBounds
$largeMapRequest.ResolutionMm = [single]20
$largeMapRequest.AllowExplicitEmptyMap = $true
$largeMap = [Activator]::CreateInstance($assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory', $true)).Create($largeMapRequest)
Assert-True $largeMap.Succeeded 'Large empty map for Dijkstra cancellation must be created before cancellation starts.'
$midDijkstraCancellation = New-Object Threading.CancellationTokenSource
$midDijkstraBudget = $budgetConstructor.Invoke(@($midDijkstraCancellation.Token, [TimeSpan]::FromSeconds(2)))
$midDijkstraCancellation.CancelAfter(1)
$midDijkstraTimer = [Diagnostics.Stopwatch]::StartNew()
$midDijkstraArguments = [object[]]@($largeMap.Map, 500, 1000, $midDijkstraBudget, $null, $null)
$midDijkstraCreated = $tryCreateDijkstra.Invoke($null, $midDijkstraArguments)
$midDijkstraTimer.Stop()
Assert-False $midDijkstraCreated 'Cancellation during Dijkstra initialization must stop construction.'
Assert-Null $midDijkstraArguments[4] 'Mid-Dijkstra cancellation must not publish a partial heuristic.'
Assert-Equal 'Cancelled' $midDijkstraArguments[5].ToString() 'Mid-Dijkstra cancellation must retain cancellation status.'
Assert-True ($midDijkstraTimer.Elapsed -lt [TimeSpan]::FromSeconds(2)) 'Dijkstra cancellation must return within the configured response bound.'

Write-Output 'Coarse path search primitive checks passed.'

$nodeType = $assembly.GetType($search + 'HybridAStarNode', $true)
$nodeKeyType = $assembly.GetType($search + 'HybridAStarNodeKey', $true)
$searchType = $assembly.GetType($search + 'HybridAStarSearch', $true)
$searchResultType = $assembly.GetType($search + 'HybridAStarSearchResult', $true)
$searchMethod = Find-Method $searchType 'Search' @($requestType, [Threading.CancellationToken])
Assert-True ($searchMethod -ne $null) 'HybridAStarSearch must expose Search(request, cancellationToken).'
Assert-True ($searchResultType.GetProperty('Status') -ne $null) 'Search result must expose planning status.'
Assert-True ($searchResultType.GetProperty('TerminationReason') -ne $null) 'Search result must expose its original termination reason.'
Assert-True ($searchResultType.GetProperty('SuccessNodeIndex') -ne $null) 'Search result must expose the successful node index.'
Assert-True ($searchResultType.GetProperty('ReopenedNodeCount') -ne $null) 'Search result must expose reopened node count.'
Assert-True ($searchResultType.GetProperty('StaleOpenListEntryCount') -ne $null) 'Search result must expose stale Open List entry count.'
Assert-True ($searchResultType.GetProperty('PeakOpenListCount') -ne $null) 'Search result must expose Open List peak count.'
Assert-True ($nodeType.GetProperty('ParentNodeIndex') -ne $null) 'Search node must expose its parent node index.'
Assert-True ($nodeKeyType.GetProperty('HeadingIndex') -ne $null) 'Search node key must contain a heading index.'
Assert-True ($nodeKeyType.GetProperty('Direction') -ne $null) 'Search node key must contain a travel direction.'
Assert-True ($nodeKeyType.GetProperty('CurvatureLevelIndex') -ne $null) 'Search node key must contain a curvature level index.'

$goalCandidateProperty = $nodeType.GetProperty('IsGoalCandidate')
Assert-True ($goalCandidateProperty -ne $null) 'Search node must mark a goal-truncated candidate explicitly.'
$nodeConstructor = $nodeType.GetConstructor(@([int], $nodeKeyType, $poseType, [int], $primitiveType, [double], [double], [double], [bool]))
Assert-True ($nodeConstructor -ne $null) 'Search node must accept the explicit goal-candidate marker.'
$admissionMethod = $searchType.GetMethod('ShouldEnqueueSuccessor', [Reflection.BindingFlags]'NonPublic,Static')
Assert-True ($admissionMethod -ne $null) 'Search must expose its private successor admission policy for reflection regression coverage.'

$candidateStart = [Activator]::CreateInstance($poseType, @(1.000, 1.000, 0.0))
$candidateGoal = [Activator]::CreateInstance($poseType, @(1.024, 1.000, 0.0))
$candidatePoints = [Array]::CreateInstance($poseType, 1)
$candidatePoints.SetValue($candidateGoal, 0)
$candidatePrimitive = [Activator]::CreateInstance($primitiveType, @($candidateStart, $forward, 0.0, 0.024, $candidatePoints, [double[]]@(1.0), $true))
$sharedKey = [Activator]::CreateInstance($nodeKeyType, @(20, 20, 0, $forward, 2))
$normalNode = $nodeConstructor.Invoke(@(10, $sharedKey, $candidateStart, 0, $null, 0.0, 0.250, 0.0, $false))
$goalCandidateNode = $nodeConstructor.Invoke(@(11, $sharedKey, $candidateGoal, 10, $candidatePrimitive, 0.0, 0.300, 0.0, $true))
$dominanceConfiguration = [Activator]::CreateInstance($configurationType)
$dominanceConfiguration.GoalPositionToleranceMeters = 0.001
$dominanceConfiguration.GoalHeadingToleranceRadians = 0.001
Assert-True $candidatePrimitive.IsGoalTruncation 'Regression setup must use a goal-truncated motion primitive.'
Assert-True $normalNode.Key.Equals($goalCandidateNode.Key) 'Regression setup must share one discrete state key.'
Assert-False $isSatisfied.Invoke($null, @($normalNode.Pose, $candidateGoal, $dominanceConfiguration, $forward, $forwardOnly)) 'Lower-G normal node must remain outside the continuous goal tolerance.'
Assert-True $isSatisfied.Invoke($null, @($goalCandidateNode.Pose, $candidateGoal, $dominanceConfiguration, $forward, $forwardOnly)) 'Goal candidate must satisfy the continuous goal tolerance.'
Assert-False $admissionMethod.Invoke($null, @($normalNode, 0.250)) 'Equal-cost normal state must be dominated by the best same-key label.'
Assert-True $admissionMethod.Invoke($null, @($goalCandidateNode, 0.250)) 'Goal candidate must enter Open List even when a lower-G normal state has the same key.'

function New-SearchRequest([object]$Goal, [object]$GoalDirection, [bool]$AllowReverse) {
    $searchRequest = [Activator]::CreateInstance($requestType)
    $searchRequest.Map = $map
    $searchRequest.Start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
    $searchRequest.Goal = $Goal
    $searchRequest.Vehicle = $vehicle
    $searchConfiguration = [Activator]::CreateInstance($configurationType)
    $searchConfiguration.PrimitiveLengthMeters = 0.50
    $searchConfiguration.IntegrationStepMeters = 0.05
    $searchConfiguration.MaximumCollisionCheckStepMeters = 0.025
    $searchConfiguration.GoalPositionToleranceMeters = 0.001
    $searchConfiguration.GoalHeadingToleranceRadians = 0.001
    $searchConfiguration.CurvatureLevelCount = 5
    $searchConfiguration.MaximumExpandedNodes = 10000
    $searchConfiguration.SearchTimeout = [TimeSpan]::FromSeconds(2.0)
    $searchConfiguration.AllowReverse = $AllowReverse
    $searchRequest.Configuration = $searchConfiguration
    $searchRequest.GoalDirection = $GoalDirection
    return $searchRequest
}

$searcher = [Activator]::CreateInstance($searchType)
$searchGoal = [Activator]::CreateInstance($poseType, @(1.20, 1.0, 0.0))
$searchRequest = New-SearchRequest $searchGoal $forwardOnly $false
$searchResult = $searchMethod.Invoke($searcher, @($searchRequest, [Threading.CancellationToken]::None))
Assert-Equal 'Success' $searchResult.Status.ToString() 'Empty map forward search must succeed.'
Assert-True ($null -ne $searchResult.SuccessNodeIndex) 'Success must be reported only with a popped goal candidate node.'
Assert-True ($searchResult.ExpandedNodeCount -gt 0) 'A successful search must expand the selected goal candidate.'

$atGoal = New-SearchRequest $searchRequest.Start $forwardOnly $false
$atGoalResult = $searchMethod.Invoke($searcher, @($atGoal, [Threading.CancellationToken]::None))
Assert-Equal 'Success' $atGoalResult.Status.ToString() 'A zero-length goal candidate must enter and leave the open list successfully.'
Assert-True ($null -ne $atGoalResult.SuccessNodeIndex) 'A zero-length goal candidate must have a node index.'

$cancelledRequest = New-SearchRequest $searchGoal $forwardOnly $false
$cancellationSource = New-Object Threading.CancellationTokenSource
$cancellationSource.Cancel()
$cancelledResult = $searchMethod.Invoke($searcher, @($cancelledRequest, $cancellationSource.Token))
Assert-Equal 'Cancelled' $cancelledResult.Status.ToString() 'Cancellation must be observed before node expansion.'
Assert-True (-not [string]::IsNullOrWhiteSpace($cancelledResult.TerminationReason)) 'Cancelled search must retain a non-empty reason.'

$limitedRequest = New-SearchRequest $searchGoal $forwardOnly $false
$limitedRequest.Configuration.MaximumExpandedNodes = 0
$limitedResult = $searchMethod.Invoke($searcher, @($limitedRequest, [Threading.CancellationToken]::None))
Assert-Equal 'SearchNodeLimitExceeded' $limitedResult.Status.ToString() 'Node limit must be checked before expansion.'
Assert-True (-not [string]::IsNullOrWhiteSpace($limitedResult.TerminationReason)) 'Node-limited search must retain a non-empty reason.'

$timedOutRequest = New-SearchRequest $searchGoal $forwardOnly $false
$timedOutRequest.Configuration.SearchTimeout = [TimeSpan]::Zero
$timedOutResult = $searchMethod.Invoke($searcher, @($timedOutRequest, [Threading.CancellationToken]::None))
Assert-Equal 'SearchTimeout' $timedOutResult.Status.ToString() 'Timeout must be checked before expansion.'
Assert-True (-not [string]::IsNullOrWhiteSpace($timedOutResult.TerminationReason)) 'Timed-out search must retain a non-empty reason.'
Assert-False ($cancelledResult.TerminationReason -eq $limitedResult.TerminationReason) 'Cancelled and node-limited searches must retain different reasons.'
Assert-False ($limitedResult.TerminationReason -eq $timedOutResult.TerminationReason) 'Node-limited and timed-out searches must retain different reasons.'

$internalSearchMethod = $searchType.GetMethods([Reflection.BindingFlags]'Instance,NonPublic') |
    Where-Object {
        $_.Name -eq 'Search' -and
        $_.GetParameters().Length -eq 2 -and
        $_.GetParameters()[1].ParameterType -eq $operationBudgetType
    } |
    Select-Object -First 1
Assert-True ($internalSearchMethod -ne $null) 'Search must retain its internal shared-budget overload.'
$internalErrorResult = $internalSearchMethod.Invoke($searcher, @($searchRequest, $null))
Assert-Equal 'InternalError' $internalErrorResult.Status.ToString() 'A missing internal budget must be mapped to InternalError.'
Assert-True ($internalErrorResult.TerminationReason.Contains('ArgumentNullException')) 'Internal search errors must retain the exception type.'

Write-Output 'Coarse path Hybrid A star search checks passed.'