ParkingRobot/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1

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-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 Assert-False($Actual, [string]$Message) {
    if ($Actual) { throw $Message }
}

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

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

function New-GeometryPoint(
    [double]$X,
    [double]$Y,
    [double]$ArcLength,
    [double]$Heading,
    [double]$UnwrappedHeading,
    [bool]$IsGearSwitch = $false) {
    return [Activator]::CreateInstance($pointType, @(
        $X, $Y, $ArcLength, $Heading, $UnwrappedHeading,
        [double]1.0, $IsGearSwitch, $anchor))
}

function New-DirectionSegment(
    [int]$Index,
    $Direction,
    [object[]]$Points,
    [bool]$StartsAtGearSwitch = $false,
    [bool]$EndsAtGearSwitch = $false) {
    $typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
    for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
        $typedPoints.SetValue($Points[$pointIndex], $pointIndex)
    }

    return [Activator]::CreateInstance($segmentType, @(
        $Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch))
}

function New-DirectionSegmentWithStartCurvature(
    [int]$Index,
    $Direction,
    [double]$StartVehicleCurvature,
    [object[]]$Points,
    [bool]$StartsAtGearSwitch = $false,
    [bool]$EndsAtGearSwitch = $false) {
    $typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
    for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
        $typedPoints.SetValue($Points[$pointIndex], $pointIndex)
    }

    return [Activator]::CreateInstance($segmentType, @(
        $Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch, $StartVehicleCurvature))
}

function New-CoarsePoint(
    [double]$X,
    [double]$Y,
    [double]$ArcLength,
    $Direction,
    [bool]$IsGearSwitch = $false) {
    return [Activator]::CreateInstance($coarsePointType, @(
        $X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
        [double]0.0, [double]1.0, $IsGearSwitch, $coarseAnchor))
}

function Invoke-Analysis([object[]]$Segments, [double]$Spacing = 0.05) {
    $typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
    for ($index = 0; $index -lt $Segments.Count; $index++) {
        $typedSegments.SetValue($Segments[$index], $index)
    }

    $arguments = [object[]]@($typedSegments, $Spacing, $null, $null)
    $accepted = $analyzeMethod.Invoke($analyzer, $arguments)
    $description = [string]::Join(',', @($Segments | ForEach-Object {
        "index=$($_.SegmentIndex);direction=$($_.Direction);points=$($_.Points.Count)"
    }))
    Assert-True $accepted ("Geometry analysis must accept the analytic candidate. Reason=" + $arguments[3] + '; Segments=' + $description)
    Assert-True ($null -ne $arguments[2]) 'Successful geometry analysis must return PathGeometryAnalysis.'
    return $arguments[2]
}

function Assert-AnalysisRejected([object[]]$Segments, [string]$Message) {
    $typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
    for ($segmentIndex = 0; $segmentIndex -lt $Segments.Count; $segmentIndex++) {
        $typedSegments.SetValue($Segments[$segmentIndex], $segmentIndex)
    }

    $arguments = [object[]]@($typedSegments, [double]0.05, $null, $null)
    $accepted = $analyzeMethod.Invoke($analyzer, $arguments)
    Assert-True (-not $accepted) ($Message + '; Reason=' + $arguments[3])
}

function Invoke-RejectedAnalysis([object[]]$Segments, [string]$Message) {
    $typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
    for ($index = 0; $index -lt $Segments.Count; $index++) {
        $typedSegments.SetValue($Segments[$index], $index)
    }

    $arguments = [object[]]@($typedSegments, [double]0.05, $null, $null)
    $accepted = $analyzeMethod.Invoke($analyzer, $arguments)
    Assert-False $accepted ($Message + '; Reason=' + $arguments[3])
}

function New-CoarsePathPoint(
    [double]$X,
    [double]$Y,
    [double]$ArcLength,
    $Direction,
    [bool]$IsGearSwitch = $false,
    [string]$SourceName = 'MotionPrimitive') {
    return [Activator]::CreateInstance($coarsePointType, @(
        $X, $Y, [double]0.0, [double]0.0, $ArcLength,
        $Direction, [double]0.0, [double]1.0, $IsGearSwitch,
        [Enum]::Parse($coarsePointSourceType, $SourceName)))
}

function New-EmptyGeometryMap {
    $mapRequest = [Activator]::CreateInstance($mapRequestType)
    $mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
    $mapRequest.ResolutionMm = [single]50
    $mapRequest.AllowExplicitEmptyMap = $true
    $map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
    Assert-True ($null -ne $map) 'Geometry test must create an explicit empty planning map.'
    return $map
}

function Invoke-GeometryValidation($Analysis, [object[]]$Segments, $Vehicle) {
    $typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
    for ($index = 0; $index -lt $Segments.Count; $index++) {
        $typedSegments.SetValue($Segments[$index], $index)
    }

    $preparedPath = [Activator]::CreateInstance($preparedPathType, [object[]](, $typedSegments))
    $arguments = [object[]]@(
        $Analysis.Path, $Analysis.Segments, $preparedPath, (New-EmptyGeometryMap), $Vehicle, [double]0.05,
        $null, [double]0.0, $null)
    $accepted = $validateMethod.Invoke($validator, $arguments)
    return [pscustomobject]@{ Accepted = $accepted; Reason = $arguments[8] }
}

function New-CircularDirectionSegment(
    $Direction,
    [double]$Curvature,
    [int]$Intervals,
    [double]$ChordLength) {
    $radius = 1.0 / $Curvature
    $headingStep = 2.0 * [Math]::Asin($Curvature * $ChordLength / 2.0)
    $points = New-Object System.Collections.Generic.List[object]
    for ($index = 0; $index -le $Intervals; $index++) {
        $theta = $headingStep * $index
        $heading = if ($Direction.ToString() -eq 'Forward') { $theta } else { $theta + [Math]::PI }
        [void]$points.Add((New-GeometryPoint `
            (1.0 + $radius * [Math]::Sin($theta)) `
            (1.0 + $radius * (1.0 - [Math]::Cos($theta))) `
            ($index * $ChordLength) $heading $heading))
    }
    return New-DirectionSegment 0 $Direction $points.ToArray()
}

function Get-OldPolylineCurvatureMaximum($Analysis) {
    $maximum = 0.0
    $path = $Analysis.Path
    for ($index = 0; $index -lt $path.Count; $index++) {
        if ($path.Count -eq 1) {
            $curvature = 0.0
        }
        elseif ($index -eq 0) {
            $curvature = ($path[1].UnwrappedHeading - $path[0].UnwrappedHeading) /
                ($path[1].ArcLength - $path[0].ArcLength)
        }
        elseif ($index -eq $path.Count - 1) {
            $curvature = ($path[$index].UnwrappedHeading - $path[$index - 1].UnwrappedHeading) /
                ($path[$index].ArcLength - $path[$index - 1].ArcLength)
        }
        else {
            $curvature = ($path[$index + 1].UnwrappedHeading - $path[$index - 1].UnwrappedHeading) /
                ($path[$index + 1].ArcLength - $path[$index - 1].ArcLength)
        }
        $maximum = [Math]::Max($maximum, [Math]::Abs($curvature))
    }
    return $maximum
}

function Get-QuinticAnalyticCurvatureMaximum(
    [double]$C2,
    [double]$C3,
    [double]$C4,
    [double]$C5,
    [int]$ReferenceSamples = 20000) {
    $maximum = 0.0
    for ($index = 0; $index -lt $ReferenceSamples; $index++) {
        $t = $index / [double]($ReferenceSamples - 1)
        $firstDerivative = 2.0 * $C2 * $t + 3.0 * $C3 * $t * $t +
            4.0 * $C4 * $t * $t * $t + 5.0 * $C5 * $t * $t * $t * $t
        $secondDerivative = 2.0 * $C2 + 6.0 * $C3 * $t +
            12.0 * $C4 * $t * $t + 20.0 * $C5 * $t * $t * $t
        $curvature = [Math]::Abs($secondDerivative / [Math]::Pow(1.0 + $firstDerivative * $firstDerivative, 1.5))
        $maximum = [Math]::Max($maximum, $curvature)
    }
    return $maximum
}

function New-QuinticDirectionSegment(
    [double]$C2,
    [double]$C3,
    [double]$C4,
    [double]$C5,
    [double]$DistributionPower,
    [int]$Samples = 400) {
    $points = New-Object System.Collections.Generic.List[object]
    $arcLength = 0.0
    $previousX = 0.0
    $previousY = 0.0
    for ($index = 0; $index -lt $Samples; $index++) {
        $t = [Math]::Pow($index / [double]($Samples - 1), $DistributionPower)
        $x = 1.0 + $t
        $y = $C2 * $t * $t + $C3 * $t * $t * $t + $C4 * $t * $t * $t * $t + $C5 * $t * $t * $t * $t * $t
        if ($index -gt 0) {
            $arcLength += [Math]::Sqrt(($x - $previousX) * ($x - $previousX) + ($y - $previousY) * ($y - $previousY))
        }
        $heading = [Math]::Atan2(
            2.0 * $C2 * $t + 3.0 * $C3 * $t * $t + 4.0 * $C4 * $t * $t * $t + 5.0 * $C5 * $t * $t * $t * $t,
            1.0)
        [void]$points.Add((New-GeometryPoint $x $y $arcLength $heading $heading))
        $previousX = $x
        $previousY = $y
    }
    return New-DirectionSegment 0 $forward $points.ToArray()
}

$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
$processing = $root + 'Processing.'
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'

$analyzerType = Get-RequiredType ($processing + 'PathGeometryAnalyzer')
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
$analysisType = Get-RequiredType ($processing + 'PathGeometryAnalysis')
$preprocessorType = Get-RequiredType ($processing + 'PathSmoothingPreprocessor')
$resamplerType = Get-RequiredType ($processing + 'ArcLengthResampler')
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
$coarseSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
$coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
$smoothingRequestType = Get-RequiredType ($root + 'PathSmoothingRequest')
$smoothingConfigurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
$validatorType = Get-RequiredType ($root + 'Validation.SmoothedPathValidator')

Assert-True ($null -ne $preparedPathType) 'PreparedPath must be discoverable for smoothing algorithms.'
Assert-True ($null -ne $preprocessorType) 'PathSmoothingPreprocessor must be discoverable for request preparation.'
Assert-True ($null -ne $resamplerType) 'ArcLengthResampler must be discoverable for deterministic resampling.'
Assert-True ($null -ne $analysisType.GetProperty('MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter')) `
    'PathGeometryAnalysis must expose peak d-kappa/d-s.'

$analyzer = [Activator]::CreateInstance($analyzerType)
$analyzeMethod = $analyzerType.GetMethod('TryAnalyze')
Assert-True ($null -ne $analyzeMethod) 'PathGeometryAnalyzer must expose TryAnalyze.'
Assert-Equal 4 $analyzeMethod.GetParameters().Length 'TryAnalyze must accept segments, spacing, analysis, and reason.'
$validator = [Activator]::CreateInstance($validatorType)
$validateMethod = $validatorType.GetMethod('TryValidate')
Assert-True ($null -ne $validateMethod) 'SmoothedPathValidator must expose TryValidate.'
Assert-Equal 9 $validateMethod.GetParameters().Length 'SmoothedPathValidator.TryValidate must retain its public contract.'

$forward = [Enum]::Parse($directionType, 'Forward')
$reverse = [Enum]::Parse($directionType, 'Reverse')
$anchor = [Enum]::Parse($sourceType, 'Anchor')
$coarseAnchor = [Enum]::Parse($coarseSourceType, 'Start')

# The chord-corrected estimator must retain an exact circular curvature limit for both travel directions.
$maximumAllowedCurvature = 5.0 / 6.0
$circleVehicle = [Activator]::CreateInstance($vehicleType)
$circleVehicle.LengthMeters = 0.20
$circleVehicle.WidthMeters = 0.20
$circleVehicle.SafetyMarginMeters = 0.0
$circleVehicle.MaximumCurvaturePerMeter = $maximumAllowedCurvature
foreach ($direction in @($forward, $reverse)) {
    $exactLimitCircle = New-CircularDirectionSegment $direction $maximumAllowedCurvature 20 0.05
    $exactLimitAnalysis = Invoke-Analysis @($exactLimitCircle)
    foreach ($point in $exactLimitAnalysis.Path) {
        Assert-True ([Math]::Abs($point.VehicleCurvature) -le $maximumAllowedCurvature + 1.0e-9) `
            ('An exact-limit ' + $direction + ' circle must not exceed the curvature limit.')
    }
    $validation = Invoke-GeometryValidation $exactLimitAnalysis @($exactLimitCircle) $circleVehicle
    Assert-True $validation.Accepted ('The validator must accept an analyzed exact-limit ' + $direction + ' circle. Reason=' + $validation.Reason)
}

# An analyzed over-limit circle must remain detectable by the unchanged validator threshold.
$overLimitCircle = New-CircularDirectionSegment $forward ($maximumAllowedCurvature + 0.01) 20 0.05
$overLimitAnalysis = Invoke-Analysis @($overLimitCircle)
Assert-True ($overLimitAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter -gt $maximumAllowedCurvature + 1.0e-6) `
    'An over-limit circle must exceed the vehicle curvature limit by more than the validator tolerance.'
$overLimitValidation = Invoke-GeometryValidation $overLimitAnalysis @($overLimitCircle) $circleVehicle
Assert-False $overLimitValidation.Accepted 'The validator must reject an analyzed over-limit circle.'

# The chord-corrected estimator must not under-estimate these smooth references more than the former polyline estimator.
foreach ($quinticCase in @(
        [pscustomobject]@{ Name = 'SBend'; C2 = 0.0; C3 = 0.30; C4 = -0.45; C5 = 0.18; Power = 1.0 },
        [pscustomobject]@{ Name = 'EndpointPeak'; C2 = 0.18; C3 = -0.12; C4 = 0.0; C5 = 0.0; Power = 1.0 },
        [pscustomobject]@{ Name = 'NonUniformFinalInterval'; C2 = -0.12; C3 = 0.36; C4 = -0.30; C5 = 0.08; Power = 1.7 })) {
    $quintic = New-QuinticDirectionSegment $quinticCase.C2 $quinticCase.C3 $quinticCase.C4 $quinticCase.C5 $quinticCase.Power
    $quinticAnalysis = Invoke-Analysis @($quintic)
    $analyticMaximum = Get-QuinticAnalyticCurvatureMaximum $quinticCase.C2 $quinticCase.C3 $quinticCase.C4 $quinticCase.C5
    $newDeficit = [Math]::Max(0.0, $analyticMaximum - $quinticAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter)
    $oldDeficit = [Math]::Max(0.0, $analyticMaximum - (Get-OldPolylineCurvatureMaximum $quinticAnalysis))
    Assert-True ($newDeficit -le $oldDeficit + 1.0e-6) `
        ($quinticCase.Name + ' must not have greater one-sided curvature under-estimation than the old polyline estimator.')
}

# A half-turn over a single chord has no unambiguous geometric curvature estimate.
$ambiguousTurn = New-DirectionSegment 0 $forward @(
    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
    (New-GeometryPoint 1.0 0.0 1.0 ([Math]::PI) ([Math]::PI)))
Assert-AnalysisRejected @($ambiguousTurn) 'A two-point heading turn of π must be rejected as ambiguous.'

# Forward straight: resampling is exactly 0.05 m, preserves the exact endpoint, and has zero curvature.
$straight = New-DirectionSegment 0 $forward @(
    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
$straightAnalysis = Invoke-Analysis @($straight)
Assert-Equal 21 $straightAnalysis.Path.Count 'A one-metre straight must produce twenty 0.05 m intervals plus the initial point.'
Assert-Near 0.0 $straightAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter 0.000000001 `
    'A straight must have zero peak d-kappa/d-s.'
foreach ($point in $straightAnalysis.Path) {
    Assert-Near 0.0 $point.VehicleCurvatureDerivative 0.000000001 `
        'A straight point must carry zero d-kappa/d-s.'
}
for ($index = 1; $index -lt $straightAnalysis.Path.Count; $index++) {
    $left = $straightAnalysis.Path[$index - 1]
    $right = $straightAnalysis.Path[$index]
    $distance = [Math]::Sqrt(($right.X - $left.X) * ($right.X - $left.X) + ($right.Y - $left.Y) * ($right.Y - $left.Y))
    Assert-Near 0.05 $distance 0.000000001 'Straight resampling intervals must be exactly 0.05 m.'
    Assert-Near 0.0 $right.GeometricCurvature 0.000000001 'A forward straight must have zero geometric curvature.'
    Assert-Near 0.0 $right.VehicleCurvature 0.000000001 'A forward straight must have zero vehicle curvature.'
}
$straightEnd = $straightAnalysis.Path[$straightAnalysis.Path.Count - 1]
Assert-Near 1.0 $straightEnd.X 0.0 'Resampling must retain the exact final X coordinate.'
Assert-Near 0.0 $straightEnd.Y 0.0 'Resampling must retain the exact final Y coordinate.'

# Raw coarse anchors carry the vehicle pose, which may differ slightly from the chord tangent of a finite integration step.
$poseAnchoredCurve = New-DirectionSegment 0 $forward @(
    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
    (New-GeometryPoint 1.0 0.2 1.0 0.4 0.4))
$poseAnchoredAnalysis = Invoke-Analysis @($poseAnchoredCurve)
Assert-Near 0.0 $poseAnchoredAnalysis.Path[0].Heading 0.000000000001 'Geometry analysis must retain the first coarse-anchor heading rather than replace it with a chord tangent.'
$poseAnchoredEnd = $poseAnchoredAnalysis.Path[$poseAnchoredAnalysis.Path.Count - 1]
Assert-Near 0.4 $poseAnchoredEnd.Heading 0.000000000001 'Geometry analysis must retain the final coarse-anchor heading rather than replace it with a chord tangent.'

# A forward R=2 quarter circle has positive +0.5 1/m vehicle curvature.
$forwardArcPoints = New-Object System.Collections.Generic.List[object]
for ($index = 0; $index -le 32; $index++) {
    $theta = ([Math]::PI / 2.0) * $index / 32.0
    $x = 2.0 * [Math]::Sin($theta)
    $y = 2.0 * (1.0 - [Math]::Cos($theta))
    $arcLength = 2.0 * $theta
    [void]$forwardArcPoints.Add((New-GeometryPoint $x $y $arcLength $theta $theta))
}
$forwardArc = New-DirectionSegment 0 $forward $forwardArcPoints.ToArray()
$forwardArcAnalysis = Invoke-Analysis @($forwardArc)
$forwardArcMidpoint = $forwardArcAnalysis.Path[[int]($forwardArcAnalysis.Path.Count / 2)]
Assert-Near 0.5 $forwardArcMidpoint.GeometricCurvature 0.01 'An R=2 quarter circle must have geometric curvature +0.5 1/m.'
Assert-Near 0.5 $forwardArcMidpoint.VehicleCurvature 0.01 'A forward R=2 quarter circle must have vehicle curvature +0.5 1/m.'

# The same spatial R=2 circle in reverse retains geometric curvature but negates vehicle curvature.
$reverseArc = New-DirectionSegment 0 $reverse $forwardArcPoints.ToArray()
$reverseArcAnalysis = Invoke-Analysis @($reverseArc)
$reverseArcMidpoint = $reverseArcAnalysis.Path[[int]($reverseArcAnalysis.Path.Count / 2)]
Assert-Near 0.5 $reverseArcMidpoint.GeometricCurvature 0.01 'Reverse travel must not change geometric curvature.'
Assert-Near -0.5 $reverseArcMidpoint.VehicleCurvature 0.01 'A reverse R=2 quarter circle must have vehicle curvature -0.5 1/m.'

# Gear-switch poses are intentionally duplicated: they keep equal arc length and never enter a derivative denominator.
$forwardBeforeSwitch = New-DirectionSegment 0 $forward @(
    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0 $true)) $false $true
$reverseAfterSwitch = New-DirectionSegment 1 $reverse @(
    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0 $true),
    (New-GeometryPoint 0.0 0.0 2.0 0.0 0.0)) $true $false
$switchAnalysis = Invoke-Analysis @($forwardBeforeSwitch, $reverseAfterSwitch)
$firstSegment = $switchAnalysis.Segments[0]
$secondSegment = $switchAnalysis.Segments[1]
$switchLeft = $switchAnalysis.Path[$firstSegment.EndIndex]
$switchRight = $switchAnalysis.Path[$secondSegment.StartIndex]
Assert-Near $switchLeft.X $switchRight.X 0.0 'Gear-switch endpoints must retain duplicate X coordinates.'
Assert-Near $switchLeft.Y $switchRight.Y 0.0 'Gear-switch endpoints must retain duplicate Y coordinates.'
Assert-Near $switchLeft.ArcLength $switchRight.ArcLength 0.0 'Gear-switch endpoints must retain duplicate arc length.'
Assert-Equal 'Forward' $switchLeft.Direction.ToString() 'The first gear-switch pose must retain its forward segment direction.'
Assert-Equal 'Reverse' $switchRight.Direction.ToString() 'The second gear-switch pose must retain its reverse segment direction.'
Assert-True (-not [double]::IsNaN($switchLeft.GeometricCurvature)) 'No derivative may cross the gear-switch duplicate point.'
Assert-True (-not [double]::IsNaN($switchRight.GeometricCurvature)) 'No reverse derivative may cross the gear-switch duplicate point.'
Assert-True (-not [double]::IsNaN($switchLeft.VehicleCurvatureDerivative)) `
    'The forward side of a gear switch must have a finite one-sided derivative.'
Assert-True (-not [double]::IsNaN($switchRight.VehicleCurvatureDerivative)) `
    'The reverse side of a gear switch must have a finite one-sided derivative.'

# Curvature at a curved segment's end and the following straight reverse segment's start must remain independently differentiated.
$forwardArcToSwitch = New-DirectionSegment 0 $forward $forwardArcPoints.ToArray() $false $true
$reverseStraightAfterArc = New-DirectionSegment 1 $reverse @(
    (New-GeometryPoint 2.0 2.0 0.0 ([Math]::PI / 2.0) ([Math]::PI / 2.0) $true),
    (New-GeometryPoint 2.0 1.0 1.0 ([Math]::PI / 2.0) ([Math]::PI / 2.0))) $true $false
$curveSwitchAnalysis = Invoke-Analysis @($forwardArcToSwitch, $reverseStraightAfterArc)
$reverseStraightStart = $curveSwitchAnalysis.Path[$curveSwitchAnalysis.Segments[1].StartIndex]
Assert-Near 0.0 $reverseStraightStart.GeometricCurvature 0.000000001 'A gear-switch must not use the preceding curve to differentiate a reverse straight segment.'
Assert-Near 0.0 $reverseStraightStart.VehicleCurvatureDerivative 0.000000001 `
    'No curvature derivative may cross from the preceding forward arc into a reverse straight.'

$physicalStart = New-DirectionSegmentWithStartCurvature 0 $forward 0.20 @(
    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
$physicalStartAnalysis = Invoke-Analysis @($physicalStart)
Assert-Near 0.20 $physicalStartAnalysis.Path[0].VehicleCurvature 0.000000001 `
    'The unified analyzer must retain a real start steering-curvature boundary state.'
Assert-True ($physicalStartAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter -gt 0.0) `
    'A real start-curvature mismatch must remain visible to the quality analyzer.'

# A boundary that claims a gear switch must be a duplicated pose with opposite direction; discontinuities are rejected.
$invalidSwitch = New-DirectionSegment 1 $reverse @(
    (New-GeometryPoint 1.2 0.0 1.0 0.0 0.0 $true),
    (New-GeometryPoint 0.2 0.0 2.0 0.0 0.0)) $true $false
Assert-AnalysisRejected @($forwardBeforeSwitch, $invalidSwitch) 'A discontinuous gear-switch boundary must be rejected.'
$trailingGearSwitch = New-DirectionSegment 0 $forward @(
    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0)) $false $true
Assert-AnalysisRejected @($trailingGearSwitch) 'The final direction segment must not advertise a non-existent trailing gear switch.'

# The public preprocessor receives a raw coarse path and resets every prepared direction segment to local arc length zero.
$coarsePoints = [Array]::CreateInstance($coarsePointType, 4)
$coarsePoints.SetValue((New-CoarsePoint 0.0 0.0 0.0 $forward), 0)
$coarsePoints.SetValue((New-CoarsePoint 1.0 0.0 1.0 $forward), 1)
$coarsePoints.SetValue((New-CoarsePoint 1.0 0.0 1.0 $reverse $true), 2)
$coarsePoints.SetValue((New-CoarsePoint 0.0 0.0 2.0 $reverse), 3)
$coarseSegments = [Array]::CreateInstance($coarseSegmentType, 2)
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 1, $false, $true)), 0)
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(1, $reverse, 2, 3, $true, $false)), 1)
$vehicle = [Activator]::CreateInstance($vehicleType)
$vehicle.LengthMeters = 1.0
$vehicle.WidthMeters = 0.5
$vehicle.SafetyMarginMeters = 0.0
$vehicle.MaximumCurvaturePerMeter = 1.0
$configuration = [Activator]::CreateInstance($configurationType)
$uninitializedMap = [System.Runtime.Serialization.FormatterServices]::GetUninitializedObject($mapType)
$request = [Activator]::CreateInstance($requestType, @($coarsePoints, $coarseSegments, $uninitializedMap, $vehicle, $configuration))
$preprocessor = [Activator]::CreateInstance($preprocessorType)
$prepareMethod = $preprocessorType.GetMethod('TryPrepare')
$prepareArguments = [object[]]@($request, $null, $null)
$prepared = $prepareMethod.Invoke($preprocessor, $prepareArguments)
Assert-True $prepared ('Preprocessor must accept a legal forward/reverse raw coarse path. Reason=' + $prepareArguments[2])
Assert-Near 0.0 $prepareArguments[1].Segments[1].Points[0].ArcLength 0.0 'Every prepared direction segment must begin at local arc length zero.'
Assert-True $prepareArguments[1].Segments[1].Points[0].IsGearSwitchPoint 'The reverse prepared segment must retain its gear-switch point.'

# Finite coordinates can still overflow distance arithmetic; public resampling must reject them instead of emitting NaN/Infinity.
$overflowSegment = New-DirectionSegment 0 $forward @(
    (New-GeometryPoint -1.0e308 0.0 0.0 0.0 0.0),
    (New-GeometryPoint 1.0e308 0.0 1.0 0.0 0.0))
$resampler = [Activator]::CreateInstance($resamplerType)
$segmentResampleMethod = @($resamplerType.GetMethods() | Where-Object {
    $_.Name -eq 'TryResample' -and $_.GetParameters()[0].ParameterType -eq $segmentType
})[0]
$resampleArguments = [object[]]@($overflowSegment, [double]0.05, $null, $null)
$resampled = $segmentResampleMethod.Invoke($resampler, $resampleArguments)
Assert-True (-not $resampled) ('Resampling must reject a distance overflow. Reason=' + $resampleArguments[3])

# A prepared path must reject null direction segments rather than silently dropping them during flattening.
$nullSegmentArray = [Array]::CreateInstance($segmentType, 1)
$nullSegmentRejected = $false
try { [void][Activator]::CreateInstance($preparedPathType, @($nullSegmentArray)) } catch { $nullSegmentRejected = $true }
Assert-True $nullSegmentRejected 'PreparedPath must reject a null direction segment.'

# Unwrapped heading must not jump by 2π when tangents cross the -π/π branch cut.
$crossing = New-DirectionSegment 0 $forward @(
    (New-GeometryPoint 0.0 0.0 0.0 (170.0 * [Math]::PI / 180.0) (170.0 * [Math]::PI / 180.0)),
    (New-GeometryPoint -1.0 ([Math]::Tan(10.0 * [Math]::PI / 180.0)) 1.015 (170.0 * [Math]::PI / 180.0) (170.0 * [Math]::PI / 180.0)),
    (New-GeometryPoint -2.0 0.0 2.03 (-170.0 * [Math]::PI / 180.0) (-170.0 * [Math]::PI / 180.0)))
$crossingAnalysis = Invoke-Analysis @($crossing)
for ($index = 1; $index -lt $crossingAnalysis.Path.Count; $index++) {
    $difference = [Math]::Abs($crossingAnalysis.Path[$index].UnwrappedHeading - $crossingAnalysis.Path[$index - 1].UnwrappedHeading)
    Assert-True ($difference -lt [Math]::PI) 'Unwrapped headings must remain continuous across the ±π branch cut.'
}

# The request preprocessor must reset arc length independently for every direction segment,
# while retaining the duplicated pose that represents a legal forward-to-reverse gear switch.
$preprocessor = [Activator]::CreateInstance($preprocessorType)
$prepareMethod = $preprocessorType.GetMethod('TryPrepare')
Assert-True ($null -ne $prepareMethod) 'PathSmoothingPreprocessor must expose TryPrepare.'
$coarsePath = [Array]::CreateInstance($coarsePointType, 5)
$coarsePath.SetValue((New-CoarsePathPoint 0.0 0.0 0.0 $forward $false 'Start'), 0)
$coarsePath.SetValue((New-CoarsePathPoint 1.0 0.0 1.0 $forward), 1)
$coarsePath.SetValue((New-CoarsePathPoint 2.0 0.0 2.0 $forward), 2)
$coarsePath.SetValue((New-CoarsePathPoint 2.0 0.0 2.0 $reverse $true), 3)
$coarsePath.SetValue((New-CoarsePathPoint 1.0 0.0 3.0 $reverse), 4)
$coarseSegments = [Array]::CreateInstance($coarseSegmentType, 2)
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 2, $false, $true)), 0)
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(1, $reverse, 3, 4, $true, $false)), 1)
$vehicle = [Activator]::CreateInstance($vehicleType)
$vehicle.LengthMeters = [double]0.80
$vehicle.WidthMeters = [double]0.60
$vehicle.SafetyMarginMeters = [double]0.05
$vehicle.MaximumCurvaturePerMeter = [double]0.80
$configuration = [Activator]::CreateInstance($smoothingConfigurationType)
$smoothingRequest = [Activator]::CreateInstance($smoothingRequestType, @(
    $coarsePath, $coarseSegments, (New-EmptyGeometryMap), $vehicle, $configuration))
$prepareArguments = [object[]]@($smoothingRequest, $null, $null)
Assert-True $prepareMethod.Invoke($preprocessor, $prepareArguments) ('Preprocessor must accept legal forward/reverse topology. Reason=' + $prepareArguments[2])
$preparedPath = $prepareArguments[1]
Assert-Equal 2 $preparedPath.Segments.Count 'Preprocessor must preserve both direction segments.'
Assert-Near 0.0 $preparedPath.Segments[1].Points[0].ArcLength 0.0 'The reverse segment must restart local arc length at zero.'
Assert-True $preparedPath.Segments[1].Points[0].IsGearSwitchPoint 'The duplicate reverse gear-switch point must be retained.'

# Segments may meet only at a paired, coincident forward/reverse gear switch.
$illegalGearJump = New-DirectionSegment 1 $reverse @(
    (New-GeometryPoint 1.25 0.0 1.0 0.0 0.0 $true),
    (New-GeometryPoint 0.25 0.0 2.0 0.0 0.0)) $true $false
Invoke-RejectedAnalysis @($forwardBeforeSwitch, $illegalGearJump) 'A gear-switch boundary whose poses differ must be rejected.'
$illegalNormalBoundary = New-DirectionSegment 1 $forward @(
    (New-GeometryPoint 2.0 0.0 0.0 0.0 0.0),
    (New-GeometryPoint 3.0 0.0 1.0 0.0 0.0)) $false $false
Invoke-RejectedAnalysis @($straight, $illegalNormalBoundary) 'A non-gear segment boundary must be rejected.'

# Finite endpoint coordinates can still overflow while computing their separation; reject before interpolation.
$resampler = [Activator]::CreateInstance($resamplerType)
$resamplePointsMethod = $resamplerType.GetMethods() | Where-Object {
    $_.Name -eq 'TryResample' -and $_.GetParameters().Length -eq 4 -and
    $_.GetParameters()[0].ParameterType -eq [System.Collections.Generic.IReadOnlyList``1].MakeGenericType($pointType)
} | Select-Object -First 1
Assert-True ($null -ne $resamplePointsMethod) 'ArcLengthResampler must expose point-list TryResample.'
$hugePoints = [Array]::CreateInstance($pointType, 2)
$hugeCoordinate = [double]::MaxValue / 2.0
$hugePoints.SetValue((New-GeometryPoint (-$hugeCoordinate) 0.0 0.0 0.0 0.0), 0)
$hugePoints.SetValue((New-GeometryPoint $hugeCoordinate 0.0 1.0 0.0 0.0), 1)
$resampleArguments = [object[]]@($hugePoints, [double]0.05, $null, $null)
Assert-False $resamplePointsMethod.Invoke($resampler, $resampleArguments) 'Resampling must reject an infinite geometric distance caused by finite coordinates.'

# PreparedPath is an all-or-nothing immutable topology snapshot: null direction segments are invalid.
$nullPreparedSegments = [Array]::CreateInstance($segmentType, 1)
Assert-Throws { [Activator]::CreateInstance($preparedPathType, @($nullPreparedSegments)) } 'PreparedPath must reject null direction segments.'

# Segment indices are deliberately dense and equal to their position in the candidate array.
$sparseSegment = New-DirectionSegment 2 $forward @(
    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
Invoke-RejectedAnalysis @($sparseSegment) 'Prepared direction-segment indices must match their dense array position.'

Write-Output 'Path smoothing geometry checks passed.'