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

$ErrorActionPreference = 'Stop'
$newtonsoft = Join-Path $env:USERPROFILE '.nuget\packages\newtonsoft.json\13.0.4\lib\netstandard2.0\Newtonsoft.Json.dll'
if (Test-Path $newtonsoft) { $null = [Reflection.Assembly]::LoadFrom($newtonsoft) }
$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-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
    if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
        throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
    }
}

function Get-RequiredType([string]$Name) {
    return $assembly.GetType($Name, $true)
}

function Get-InternalProperty($Instance, [string]$Name) {
    return $Instance.GetType().GetProperty(
        $Name,
        [Reflection.BindingFlags]'Public,NonPublic,Instance').GetValue($Instance)
}

function Get-InternalMethod($Type, [string]$Name) {
    return @($Type.GetMethods([Reflection.BindingFlags]'Public,NonPublic,Instance,Static') |
        Where-Object Name -eq $Name)[0]
}

function New-InternalInstance($Type) {
    return [Activator]::CreateInstance(
        $Type,
        [Reflection.BindingFlags]'Instance,NonPublic,Public',
        $null,
        @(),
        $null)
}

$builderType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2CandidateBuilder'
$hooksType = $builderType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
Assert-True ($null -ne $hooksType) 'LocalG2CandidateBuilder must expose narrowly scoped deterministic TestHooks.'
$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
Assert-True ($null -ne $executeMethod) 'TestHooks must execute deterministic candidate scenarios.'

function Invoke-Scenario([string]$Scenario) {
    return $executeMethod.Invoke($null, @($Scenario))
}

$isolated = Invoke-Scenario 'Isolated'
Assert-True ($isolated.CandidateCount -gt 0 -and $isolated.CandidateCount -le 12) `
    'An isolated event must produce a bounded non-empty candidate set.'
Assert-Near 0.0 $isolated.StartPositionError 1e-9 'Window start position must match.'
Assert-Near 0.0 $isolated.EndPositionError 1e-9 'Window end position must match.'
Assert-Near 0.0 $isolated.StartCurvatureError 1e-8 'Window start curvature must match.'
Assert-Near 0.0 $isolated.EndCurvatureError 1e-8 'Window end curvature must match.'
Assert-True $isolated.ContainsLocalG2Source 'Generated samples must identify their source.'

$cluster = Invoke-Scenario 'Cluster'
Assert-Equal 1 $cluster.OutputRegionCount 'Overlapping events must be built as one region.'
Assert-True $cluster.InternalConnectionsAreG2 'Internal anchors must share tangent and curvature.'

$reverse = Invoke-Scenario 'Reverse'
Assert-Equal 'Reverse' $reverse.Direction 'Reverse candidates must preserve segment direction.'
Assert-True $reverse.VehicleAndGeometricCurvatureSignsAreOpposite `
    'Reverse candidates must convert vehicle curvature to geometric curvature exactly once.'

$spliced = Invoke-Scenario 'Spliced'
Assert-True $spliced.NoDuplicateNonGearPoints 'Splicing must remove duplicate boundary samples.'
Assert-True $spliced.EndpointsUnchanged 'Splicing must preserve the complete segment endpoints.'

$startBoundary = Invoke-Scenario 'StartBoundary'
Assert-True ($startBoundary.CandidateCount -gt 0) 'A start-boundary event must build a one-sided candidate.'
Assert-Near 0.0 $startBoundary.StartCurvatureError 1e-8 `
    'A start-boundary candidate must use the preserved physical start vehicle curvature.'

$interiorDerivative = Invoke-Scenario 'InteriorStationaryCurve'
Assert-True $interiorDerivative.Rejected `
    'A curve with a stationary interior derivative must be rejected even when its endpoint chord is short.'

$constantVelocity = Invoke-Scenario 'ConstantVelocityCurve'
Assert-True $constantVelocity.Accepted `
    'A nonstationary constant-velocity curve with collinear derivative controls must be accepted.'

$exactSplice = Invoke-Scenario 'ExactSpliceEndpoints'
Assert-True $exactSplice.EndpointsAreExact `
    'Splicing must write interpolated exact endpoints instead of accepting approximate candidate endpoints.'

$gearBoundary = Invoke-Scenario 'GearBoundary'
Assert-True $gearBoundary.GearBoundaryMarkerPreserved `
    'A window touching a gear-switch segment boundary must preserve its point-level gear marker.'

$twoRegions = Invoke-Scenario 'TwoRegionWorkOrder'
Assert-True $twoRegions.WorkOrderDescending `
    'Same-segment regions must be processed from larger original local arc to smaller local arc.'
Assert-True $twoRegions.FrontArcPreservedAfterBackReplacement `
    'Replacing the back region must preserve the front region original arc coordinates.'
Assert-True $twoRegions.BothReplacementsRetained `
    'Back-then-front replacement must retain the exact LocalG2Transition interiors at (0.75, 0.20) and (2.25, 0.20).'
Assert-True $twoRegions.ForwardOrderRejected `
    'The regression fixture must prove that front-first invalidates the original back absolute arc.'
Assert-True $twoRegions.DeterministicWorkOrder `
    'Work ordering must repeat exactly without mutating report order.'
Assert-True $twoRegions.DeterministicReplacementGeometry `
    'Repeated back-then-front replacement must preserve count and every point X, Y, source, and direction.'
Assert-True $twoRegions.InvalidWorkOrderRejected `
    'Work ordering must reject null regions and cross-segment event contents.'

$ordering = Invoke-Scenario 'MultiSegmentWorkOrder'
Assert-True $ordering.SegmentOrderAscending `
    'Region work ordering must process segment indices in ascending order.'
Assert-True $ordering.EqualArcUsesReportOrder `
    'Equal first-local-arc regions must retain their original report-index order.'

$evaluatorType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2CandidateEvaluator'
$evaluatorHooksType = $evaluatorType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
Assert-True ($null -ne $evaluatorHooksType) 'LocalG2CandidateEvaluator must expose narrowly scoped deterministic TestHooks.'
$evaluateMethod = $evaluatorHooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
Assert-True ($null -ne $evaluateMethod) 'TestHooks must execute deterministic candidate quality-gate scenarios.'

function Invoke-EvaluationScenario([string]$Scenario) {
    return $evaluateMethod.Invoke($null, @($Scenario))
}

$tooFar = Invoke-EvaluationScenario 'TooFar'
Assert-Equal 'DeviationExceeded' $tooFar.FailureReason `
    'A collision-free candidate more than 0.10 m from the raw window must be rejected.'

$overshoot = Invoke-EvaluationScenario 'Overshoot'
Assert-Equal 'CurvatureOvershoot' $overshoot.FailureReason `
    'A candidate outside the raw regional curvature range must be rejected.'

$noOp = Invoke-EvaluationScenario 'NoOp'
Assert-Equal 'InsufficientImprovement' $noOp.FailureReason `
    'A safe no-op must not be accepted.'

$oscillating = Invoke-EvaluationScenario 'Oscillating'
Assert-Equal 'VariationCostRegression' $oscillating.FailureReason `
    'Repeated curvature oscillation must fail the variation-cost gate.'

$lowClearance = Invoke-EvaluationScenario 'LowClearance'
Assert-Equal 'InsufficientClearance' $lowClearance.FailureReason `
    'A path with less than 0.02 m checked clearance must be rejected.'

$improved = Invoke-EvaluationScenario 'Improved'
Assert-Equal 'Accepted' $improved.Status `
    'A safe candidate with at least 20 percent peak improvement must be accepted.'

$smallestDeviation = Invoke-EvaluationScenario 'SmallestDeviation'
$best = Invoke-EvaluationScenario 'Best'
Assert-Equal $smallestDeviation.CandidateIndex $best.CandidateIndex `
    'Among sufficient candidates, minimum deviation must win before extra smoothness.'

# Exercise the real builder → evaluator seam that a hand-built TestHook candidate does not cover.
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
$factoryType = Get-RequiredType ($root + 'Test.SmoothingScenarioFactory')
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
$preprocessorType = Get-RequiredType ($root + 'Processing.PathSmoothingPreprocessor')
$optionsType = Get-RequiredType ($root + 'LocalG2.LocalG2OptionsSnapshot')
$detectorType = Get-RequiredType ($root + 'LocalG2.CurvatureTransitionDetector')
$plannerType = Get-RequiredType ($root + 'LocalG2.LocalG2WindowPlanner')
$fixturePath = Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\Fixtures\path-smoothing-fixtures.json'
$fixtures = (Get-InternalMethod $factoryType 'CreateFixtureRequests').Invoke($null, @((Resolve-Path $fixturePath).Path))
$baseRequest = $fixtures[1].SmoothingRequest # single-turn
$configuration = [Activator]::CreateInstance($configurationType)
$configuration.Method = [Enum]::Parse($methodType, 'LocalG2Quintic')
$singleTurnRequest = [Activator]::CreateInstance($requestType, @(
    $baseRequest.CoarsePath, $baseRequest.Segments, $baseRequest.Map, $baseRequest.Vehicle, $configuration))
$preprocessor = [Activator]::CreateInstance($preprocessorType)
$prepareArgs = [object[]]@($singleTurnRequest, $null, $null)
Assert-True ((Get-InternalMethod $preprocessorType 'TryPrepare').Invoke($preprocessor, $prepareArgs)) `
    'SingleTurn must prepare before evaluating Local G2 candidates.'
$preparedPath = $prepareArgs[1]
$options = [Activator]::CreateInstance($optionsType, [Reflection.BindingFlags]'Instance,NonPublic,Public', $null, @($configuration), $null)
$detector = New-InternalInstance $detectorType
$detectArgs = [object[]]@($singleTurnRequest, [double]$singleTurnRequest.Vehicle.MaximumCurvaturePerMeter, $options, $null, $null)
Assert-True ((Get-InternalMethod $detectorType 'TryDetect').Invoke($detector, $detectArgs)) `
    'SingleTurn must detect its Local G2 transition.'
$planner = New-InternalInstance $plannerType
$planArgs = [object[]]@($preparedPath, $detectArgs[3], $options, $null, $null)
Assert-True ((Get-InternalMethod $plannerType 'TryPlan').Invoke($planner, $planArgs)) `
    'SingleTurn must plan a Local G2 region.'
$region = $null
foreach ($plannedRegion in $planArgs[3]) { $region = $plannedRegion; break }
$preparedSegment = (Get-InternalProperty $preparedPath 'Segments')[(Get-InternalProperty $region 'SegmentIndex')]
$realBuilder = New-InternalInstance $builderType
$realCandidates = (Get-InternalMethod $builderType 'Build').Invoke($realBuilder, @(
    $preparedSegment, $region, [double]$configuration.OutputSpacingMeters, $options, [Threading.CancellationToken]::None))
Assert-True ($realCandidates.Count -gt 0) 'SingleTurn must build real Local G2 candidates.'
$realEvaluator = New-InternalInstance $evaluatorType
$duplicateFailures = @()
foreach ($realCandidate in $realCandidates) {
    $evaluation = (Get-InternalMethod $evaluatorType 'Evaluate').Invoke($realEvaluator, @(
        $preparedPath, $preparedPath, $region, $realCandidate, $singleTurnRequest, $options, [Threading.CancellationToken]::None))
    $failureReason = Get-InternalProperty $evaluation 'FailureReason'
    if ($failureReason -eq 2) {
        $candidateIndex = Get-InternalProperty $realCandidate 'CandidateIndex'
        $duplicateFailures += $candidateIndex
    }
}
Assert-Equal 0 $duplicateFailures.Count `
    'SingleTurn builder candidates must not fail evaluator window analysis due to duplicate or degenerate points.'

Write-Output 'Path smoothing Local G2 candidate checks passed.'
