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-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)
}

$detectorType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.CurvatureTransitionDetector'
$hooksType = $detectorType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
Assert-True ($null -ne $hooksType) 'CurvatureTransitionDetector must expose its narrowly scoped nested TestHooks helper.'
$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
Assert-True ($null -ne $executeMethod) 'TestHooks must expose deterministic scenario execution for reflection tests.'

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

# Same direction: 0 -> 0.4167 is detected once.
$singleTransition = Invoke-Scenario 'SingleTransition'
Assert-Equal 1 $singleTransition.TransitionCount 'One primitive curvature jump must be detected.'
Assert-Near 0.4167 $singleTransition.MaximumJump 0.0001 'The jump magnitude must be retained.'

# Same pose at a forward/reverse boundary: no event crosses the stop.
$gearSwitch = Invoke-Scenario 'GearSwitch'
Assert-Equal 0 $gearSwitch.TransitionCount 'A stopped gear switch must not be a smoothing event.'

# A 0.01 1/m numerical change is below max(0.001, 5% of 0.8333).
$noise = Invoke-Scenario 'Noise'
Assert-Equal 0 $noise.TransitionCount 'Sub-threshold curvature noise must be ignored.'

# Two 0.50 m windows whose ranges overlap are merged.
$overlap = Invoke-Scenario 'Overlap'
Assert-Equal 1 $overlap.RegionCount 'Overlapping windows must form one joint region.'
Assert-Equal 2 $overlap.TransitionCountInFirstRegion 'The merged region must retain both events.'

# Near a segment start, the window becomes asymmetric without crossing the hard boundary.
$nearStart = Invoke-Scenario 'NearStart'
Assert-Near 0.0 $nearStart.StartArcLength 0.000000001 'A start window must be clamped to the segment.'
Assert-True ($nearStart.RightWindowLength -gt $nearStart.LeftWindowLength) `
    'Unavailable left length must be shifted to the right.'

# Finite endpoint curvatures may still overflow during subtraction; detection must fail rather than publish infinity.
$coarsePathPointType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.CoarsePathPoint'
$pathSegmentType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.PathSegment'
$directionType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.TravelDirection'
$coarseSourceType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.CoarsePathPointSource'
$requestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.PathSmoothingRequest'
$configurationType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.PathSmoothingConfiguration'
$optionsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2OptionsSnapshot'
$forward = [Enum]::Parse($directionType, 'Forward')
$motionPrimitive = [Enum]::Parse($coarseSourceType, 'MotionPrimitive')
$overflowPoints = [Array]::CreateInstance($coarsePathPointType, 2)
$overflowPoints.SetValue([Activator]::CreateInstance($coarsePathPointType, @(
    [double]0.0, [double]0.0, [double]0.0, [double]0.0, [double]0.0, $forward,
    -[double]::MaxValue, [double]1.0, $false, $motionPrimitive)), 0)
$overflowPoints.SetValue([Activator]::CreateInstance($coarsePathPointType, @(
    [double]0.1, [double]0.0, [double]0.0, [double]0.0, [double]0.1, $forward,
    [double]::MaxValue, [double]1.0, $false, $motionPrimitive)), 1)
$overflowSegments = [Array]::CreateInstance($pathSegmentType, 1)
$overflowSegments.SetValue([Activator]::CreateInstance($pathSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
$configuration = [Activator]::CreateInstance($configurationType)
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
Assert-True ($null -ne $optionsConstructor) 'Local G2 options must be constructible for detector reflection tests.'
$options = $optionsConstructor.Invoke(@($configuration))
$overflowRequest = [Activator]::CreateInstance($requestType, @($overflowPoints, $overflowSegments, $null, $null, $configuration))
$detector = [Activator]::CreateInstance($detectorType, $true)
$tryDetect = $detectorType.GetMethod('TryDetect', [Reflection.BindingFlags]'Instance,NonPublic')
Assert-True ($null -ne $tryDetect) 'CurvatureTransitionDetector must retain its internal TryDetect contract.'
$overflowArguments = [object[]]@($overflowRequest, [double]0.8333, $options, $null, $null)
$overflowAccepted = $tryDetect.Invoke($detector, $overflowArguments)
Assert-False $overflowAccepted 'Curvature subtraction overflow must reject detection.'
Assert-True (-not [string]::IsNullOrWhiteSpace([string]$overflowArguments[4])) `
    'Rejected overflow detection must provide a stable reason.'
Assert-Equal 0 $overflowArguments[3].Count 'Rejected overflow detection must not publish a curvature event.'

$plannerType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2WindowPlanner'
$plannerHooksType = $plannerType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
Assert-True ($null -ne $plannerHooksType) 'LocalG2WindowPlanner must expose narrow deterministic TestHooks.'
$planScenario = $plannerHooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')

$separated = $planScenario.Invoke($null, @('SeparatedByOneMeter'))
Assert-Equal 2 $separated.RegionCount 'Events 1.0 m apart cannot share a 0.80 m total window.'
Assert-Equal '1,1' $separated.TransitionCounts 'Separated events must remain one event per region.'

$mergeable = $planScenario.Invoke($null, @('Mergeable'))
Assert-Equal 1 $mergeable.RegionCount 'Events covered by one legal total window must merge.'
Assert-Equal '2' $mergeable.TransitionCounts 'The merged region must retain both events.'

$partition = $planScenario.Invoke($null, @('ThreeEventPartition'))
Assert-Equal 2 $partition.RegionCount 'Three events must split at the first infeasible joint window.'
Assert-Equal '2,1' $partition.TransitionCounts 'Only a feasible consecutive subgroup may merge.'

$boundary = $planScenario.Invoke($null, @('NearBoundary'))
Assert-True ($boundary.FirstRightLength -gt $boundary.FirstLeftLength) `
    'A boundary-clamped total window must transfer missing length to the available side.'
Assert-True ($boundary.MaximumWindowLength -le 0.80 + 1e-9) `
    'No candidate window may exceed 0.80 m total length.'

$coverage = $planScenario.Invoke($null, @('InteriorCoverage'))
Assert-True $coverage.RepresentativeTargetsFirst `
    'Preferred, minimum, and maximum balanced targets must precede asymmetric variants.'
Assert-True $coverage.HasAsymmetricVariant `
    'Default window planning must retain a legal asymmetric variant after balanced coverage.'
Assert-True ($coverage.FirstRegionVariantCount -le 12) `
    'Window planning must obey the configured default cap.'

foreach ($snapshot in @($separated, $mergeable, $partition, $boundary, $coverage)) {
    Assert-True $snapshot.ExactEnvelope 'Region envelope must equal the extrema of actual legal variants.'
    Assert-True ($snapshot.MaximumWindowLength -le 0.80 + 1e-9) `
        'MaximumWindowLengthMeters is a total, not a per-side length.'
}

$repeat = $planScenario.Invoke($null, @('ThreeEventPartition'))
Assert-Equal $partition.Signature $repeat.Signature `
    'Repeated planning must preserve grouping, candidate numbering and variant order.'

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