ParkingRobot/ClumsyPilot/tests/verify_path_smoothing_bezier.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 Get-RequiredType([string]$Name) {
    return $assembly.GetType($Name, $true)
}

function Get-PropertyValue($Instance, [string]$Name) {
    $property = $Instance.GetType().GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
    Assert-True ($null -ne $property) ("Missing property: " + $Name)
    return $property.GetValue($Instance)
}

function Assert-PointBitwiseEqual($Expected, $Actual, [string]$Message) {
    foreach ($name in @('X', 'Y', 'ArcLength', 'Heading', 'UnwrappedHeading', 'BodyClearance')) {
        $expectedBits = [BitConverter]::DoubleToInt64Bits([double]$Expected.$name)
        $actualBits = [BitConverter]::DoubleToInt64Bits([double]$Actual.$name)
        Assert-Equal $expectedBits $actualBits ($Message + ' ' + $name)
    }
    Assert-Equal $Expected.IsGearSwitchPoint $Actual.IsGearSwitchPoint ($Message + ' IsGearSwitchPoint')
    Assert-Equal $Expected.Source.ToString() $Actual.Source.ToString() ($Message + ' Source')
}

function New-Point(
    [double]$X,
    [double]$Y,
    [double]$ArcLength,
    [double]$Heading,
    [double]$BodyClearance = 0.10,
    [bool]$IsGearSwitch = $false) {
    return [Activator]::CreateInstance($pointType, @(
        $X, $Y, $ArcLength, $Heading, $Heading, $BodyClearance, $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-EmptyMap {
    $request = [Activator]::CreateInstance($mapRequestType)
    $request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
    $request.ResolutionMm = [single]50
    $request.AllowExplicitEmptyMap = $true
    $map = [Activator]::CreateInstance($mapFactoryType).Create($request).Map
    Assert-True ($null -ne $map) 'Bézier test must create an explicit empty planning map.'
    return $map
}

function New-AlgorithmInput(
    [object[]]$Segments,
    [double]$ReserveMeters = 0.02,
    [double]$CornerThresholdRadians = ([Math]::PI / 18.0),
    [double]$MaximumWindowLengthMeters = 0.60,
    [double]$HandleLengthRatio = (1.0 / 3.0)) {
    $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))
    $vehicle = [Activator]::CreateInstance($vehicleType)
    $vehicle.LengthMeters = [double]0.20
    $vehicle.WidthMeters = [double]0.20
    $vehicle.SafetyMarginMeters = [double]0.0
    $vehicle.MaximumCurvaturePerMeter = [double]100.0
    $vehicle.MinimumTurningRadiusMeters = [double]0.01
    $configuration = [Activator]::CreateInstance($configurationType)
    $configuration.LocalCubicBezier.CornerHeadingThresholdRadians = $CornerThresholdRadians
    $configuration.LocalCubicBezier.MaximumWindowLengthMeters = $MaximumWindowLengthMeters
    $configuration.LocalCubicBezier.HandleLengthRatio = $HandleLengthRatio
    $options = $optionsConstructor.Invoke(@($configuration))
    return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
}

function Invoke-Candidate(
    [object[]]$Segments,
    [double]$ReserveMeters = 0.02,
    [double]$CornerThresholdRadians = ([Math]::PI / 18.0),
    [double]$MaximumWindowLengthMeters = 0.60,
    [double]$HandleLengthRatio = (1.0 / 3.0),
    [double]$Strength = 1.0) {
    return $smoothMethod.Invoke($smoother, @(
        (New-AlgorithmInput $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio),
        $Strength,
        [Threading.CancellationToken]::None))
}

function Invoke-Smoothing(
    [object[]]$Segments,
    [double]$ReserveMeters = 0.02,
    [double]$CornerThresholdRadians = ([Math]::PI / 18.0),
    [double]$MaximumWindowLengthMeters = 0.60,
    [double]$HandleLengthRatio = (1.0 / 3.0),
    [double]$Strength = 1.0) {
    $candidate = Invoke-Candidate $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio $Strength
    Assert-True (Get-PropertyValue $candidate 'Succeeded') 'Bézier smoothing must produce a candidate for the deterministic fixture.'
    return @(Get-PropertyValue $candidate 'Segments')
}

function Get-InterpolatedRunCount($Points) {
    $runCount = 0
    $inRun = $false
    foreach ($point in $Points) {
        $interpolated = $point.Source.ToString() -eq 'Interpolated'
        if ($interpolated -and -not $inRun) { $runCount++ }
        $inRun = $interpolated
    }
    return $runCount
}

function Get-PointAtArcLength($Points, [double]$ArcLength) {
    foreach ($point in $Points) {
        if ([BitConverter]::DoubleToInt64Bits([double]$point.ArcLength) -eq
            [BitConverter]::DoubleToInt64Bits($ArcLength)) {
            return $point
        }
    }
    throw "No output point found at local arc length $ArcLength."
}

function Get-FirstInterpolatedPoint($Points) {
    foreach ($point in $Points) {
        if ($point.Source.ToString() -eq 'Interpolated') { return $point }
    }
    throw 'Expected an interpolated Bézier point.'
}

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

$smootherType = Get-RequiredType ($algorithms + 'LocalCubicBezierSmoother')
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
$optionsType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
$candidateStatusType = Get-RequiredType ($algorithms + 'SmoothingCandidateStatus')
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'

$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
    @($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null)
Assert-True ($null -ne $inputConstructor) 'Bézier tests must construct algorithm input with immutable option values.'
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
Assert-True ($null -ne $optionsConstructor) 'Bézier tests must create immutable option snapshots.'
$smoother = [Activator]::CreateInstance($smootherType, $true)
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
Assert-True ($null -ne $smoothMethod) 'LocalCubicBezierSmoother must implement the internal smoother contract.'
Assert-Equal 'LocalCubicBezier' $smoother.Method.ToString() 'Bézier smoother must identify its public smoothing method.'

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

# A straight path must not create a local Bézier window or alter any sample.
$straightSource = @(
    (New-Point 0.0 0.0 0.0 0.0),
    (New-Point 0.1 0.0 0.1 0.0),
    (New-Point 0.2 0.0 0.2 0.0),
    (New-Point 0.3 0.0 0.3 0.0))
$straightOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)))[0].Points
Assert-Equal 0 (Get-InterpolatedRunCount $straightOutput) 'A straight line must create no Bézier replacement window.'
Assert-Equal $straightSource.Count $straightOutput.Count 'A straight line must retain its original sample count.'
for ($index = 0; $index -lt $straightSource.Count; $index++) {
    Assert-PointBitwiseEqual $straightSource[$index] $straightOutput[$index] 'Straight samples must remain bitwise unchanged.'
}

# One corner is one local replacement: only the corner sample is evaluated while the window endpoints stay fixed.
$cornerSource = @(
    (New-Point 0.0 0.0 0.0 0.0),
    (New-Point 0.1 0.0 0.1 0.0),
    (New-Point 0.2 0.0 0.2 0.0),
    (New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)),
    (New-Point 0.2 0.2 0.4 ([Math]::PI / 2.0)))
$cornerOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)))[0].Points
Assert-Equal 1 (Get-InterpolatedRunCount $cornerOutput) 'One corner must produce exactly one contiguous Bézier replacement.'
Assert-Equal $cornerSource.Count $cornerOutput.Count 'One local replacement must preserve the segment sampling topology.'
Assert-True (($cornerOutput[2].X -ne $cornerSource[2].X) -or ($cornerOutput[2].Y -ne $cornerSource[2].Y)) 'The corner sample must be replaced by cubic Bézier geometry.'
Assert-PointBitwiseEqual $cornerSource[1] $cornerOutput[1] 'Bézier entry anchor must remain fixed.'
Assert-PointBitwiseEqual $cornerSource[3] $cornerOutput[3] 'Bézier exit anchor must remain fixed.'
$cornerChordLength = [Math]::Sqrt(
    [Math]::Pow($cornerSource[3].X - $cornerSource[1].X, 2.0) +
    [Math]::Pow($cornerSource[3].Y - $cornerSource[1].Y, 2.0))
$cornerHandleLength = $cornerChordLength / 3.0
$expectedCornerX =
    0.125 * $cornerSource[1].X +
    0.375 * ($cornerSource[1].X + $cornerHandleLength) +
    0.375 * $cornerSource[3].X +
    0.125 * $cornerSource[3].X
$expectedCornerY =
    0.125 * $cornerSource[1].Y +
    0.375 * $cornerSource[1].Y +
    0.375 * ($cornerSource[3].Y - $cornerHandleLength) +
    0.125 * $cornerSource[3].Y
$cornerInterpolated = Get-FirstInterpolatedPoint $cornerOutput
Assert-Near $expectedCornerX $cornerInterpolated.X 0.000000000001 'Bézier control handles must use the local endpoint chord length for X geometry.'
Assert-Near $expectedCornerY $cornerInterpolated.Y 0.000000000001 'Bézier control handles must use the local endpoint chord length for Y geometry.'

# Adjacent corner windows touch/overlap and must become one merged cubic replacement, not two sequential fits.
$overlappingSource = @(
    (New-Point 0.0 0.0 0.0 0.0),
    (New-Point 0.1 0.0 0.1 0.0),
    (New-Point 0.2 0.0 0.2 0.0),
    (New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)),
    (New-Point 0.3 0.1 0.4 0.0),
    (New-Point 0.4 0.1 0.5 0.0))
$overlappingOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $overlappingSource)))[0].Points
Assert-Equal 1 (Get-InterpolatedRunCount $overlappingOutput) 'Touching local corner windows must merge into exactly one Bézier replacement.'
Assert-PointBitwiseEqual $overlappingSource[1] $overlappingOutput[1] 'Merged Bézier entry anchor must remain fixed.'
Assert-PointBitwiseEqual $overlappingSource[4] $overlappingOutput[4] 'Merged Bézier exit anchor must remain fixed.'
Assert-True (($overlappingOutput[2].X -ne $overlappingSource[2].X) -or ($overlappingOutput[2].Y -ne $overlappingSource[2].Y)) 'Merged window must replace the first interior corner sample.'
Assert-True (($overlappingOutput[3].X -ne $overlappingSource[3].X) -or ($overlappingOutput[3].Y -ne $overlappingSource[3].Y)) 'Merged window must replace the second interior corner sample.'

# Safe bounded policy: decline an entire connected set when its merged interval exceeds the cap.
# The two candidate windows below are each 0.20 m, but their merged 0.30 m interval must not
# produce one over-length curve or be split into new unrequested joins.
$overCapMergedOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $overlappingSource)) 0.02 ([Math]::PI / 18.0) 0.20)[0].Points
Assert-Equal 0 (Get-InterpolatedRunCount $overCapMergedOutput) 'An oversized connected Bézier window set must be declined instead of emitting an over-cap replacement.'
for ($index = 0; $index -lt $overlappingSource.Count; $index++) {
    Assert-PointBitwiseEqual $overlappingSource[$index] $overCapMergedOutput[$index] 'Declining an oversized connected set must preserve its anchors.'
}

# Samples outside a local window must remain bitwise unchanged rather than be globally re-fit.
$isolatedSource = @(
    (New-Point 0.0 0.0 0.0 0.0),
    (New-Point 0.1 0.0 0.1 0.0),
    (New-Point 0.2 0.0 0.2 0.0),
    (New-Point 0.3 0.0 0.3 0.0),
    (New-Point 0.3 0.1 0.4 ([Math]::PI / 2.0)),
    (New-Point 0.3 0.2 0.5 ([Math]::PI / 2.0)),
    (New-Point 0.3 0.3 0.6 ([Math]::PI / 2.0)))
$isolatedOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $isolatedSource)))[0].Points
foreach ($index in @(0, 1, 2, 5, 6)) {
    Assert-PointBitwiseEqual $isolatedSource[$index] (Get-PointAtArcLength $isolatedOutput $isolatedSource[$index].ArcLength) 'Samples outside a Bézier window must remain bitwise unchanged.'
}

# Direction segments stay independent; segment endpoints and the gear-switch anchor are fixed.
$reverseSource = @(
    (New-Point 0.2 0.2 0.0 ([Math]::PI / 2.0) 0.10 $true),
    (New-Point 0.2 0.1 0.1 ([Math]::PI / 2.0)),
    (New-Point 0.2 0.0 0.2 ([Math]::PI / 2.0)))
$switchOutput = @(Invoke-Smoothing @(
    (New-DirectionSegment 0 $forward $cornerSource $false $true),
    (New-DirectionSegment 1 $reverse $reverseSource $true $false)))
Assert-Equal 2 $switchOutput.Count 'Bézier smoothing must retain separate forward and reverse direction segments.'
Assert-True $switchOutput[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.'
Assert-True $switchOutput[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.'
Assert-PointBitwiseEqual $cornerSource[0] $switchOutput[0].Points[0] 'Segment start endpoint must remain fixed.'
Assert-PointBitwiseEqual $cornerSource[$cornerSource.Count - 1] $switchOutput[0].Points[$switchOutput[0].Points.Count - 1] 'Segment end endpoint must remain fixed.'
Assert-PointBitwiseEqual $reverseSource[0] $switchOutput[1].Points[0] 'Gear-switch point must remain fixed.'

# The immutable options each change only their own local behavior.
$thresholdOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.70)[0].Points
Assert-Equal 0 (Get-InterpolatedRunCount $thresholdOutput) 'A non-default heading threshold above the corner angle must suppress only corner detection.'
$windowOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.15)[0].Points
Assert-Equal 0 (Get-InterpolatedRunCount $windowOutput) 'A non-default maximum window shorter than the local connection must suppress only that window.'
$shortHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.10)[0].Points
$longHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.60)[0].Points
Assert-Equal 1 (Get-InterpolatedRunCount $shortHandleOutput) 'Changing handle ratio must not change detected window topology.'
Assert-Equal 1 (Get-InterpolatedRunCount $longHandleOutput) 'Changing handle ratio must not change detected window topology.'
$shortHandlePoint = Get-FirstInterpolatedPoint $shortHandleOutput
$longHandlePoint = Get-FirstInterpolatedPoint $longHandleOutput
Assert-True (($shortHandlePoint.X -ne $longHandlePoint.X) -or ($shortHandlePoint.Y -ne $longHandlePoint.Y)) 'A non-default handle ratio must change only the local Bézier geometry.'

# Parameter-matched local arc-length reference comparison must reject excess displacement as retryable and publish no geometry.
$infeasible = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.095
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $infeasible 'Status').ToString() 'Exceeded local arc-length displacement must be retryable, not terminal.'
Assert-True (-not (Get-PropertyValue $infeasible 'Succeeded')) 'An infeasible Bézier curve must not be executable.'
Assert-Equal 0 (Get-PropertyValue $infeasible 'Segments').Count 'A retryable Bézier infeasibility must publish no executable geometry.'

# This nonuniform, offset window evaluates its only interior point at t=0.25 and local s=6.
# A wrong global/index mapping would instead compare to s=5 and accept the 0.50 m clearance;
# the required local-arc reference at s=6 must reject the roughly 0.65 m displacement.
$nonuniformOffsetSource = @(
    (New-Point 0.0 0.0 0.0 0.0 0.50),
    (New-Point 1.0 0.0 4.0 0.0 0.50),
    (New-Point 2.0 0.0 5.0 0.0 0.50),
    (New-Point 3.0 0.0 6.0 0.0 0.50),
    (New-Point 3.0 1.0 9.0 ([Math]::PI / 2.0) 0.50),
    (New-Point 3.0 2.0 20.0 ([Math]::PI / 2.0) 0.50))
$nonuniformOffsetInfeasible = Invoke-Candidate @((New-DirectionSegment 0 $forward $nonuniformOffsetSource)) 0.0 ([Math]::PI / 18.0) 5.0
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $nonuniformOffsetInfeasible 'Status').ToString() 'A nonuniform offset window must use local arc-length mapping for retryable clearance rejection.'
Assert-True (-not (Get-PropertyValue $nonuniformOffsetInfeasible 'Succeeded')) 'The nonuniform local-arc infeasibility must not be executable.'
Assert-Equal 0 (Get-PropertyValue $nonuniformOffsetInfeasible 'Segments').Count 'The nonuniform local-arc infeasibility must publish no geometry.'

Write-Output 'Path smoothing local cubic Bézier checks passed.'