ParkingRobot/ClumsyPilot/tests/verify_path_smoothing_bspline.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 New-Point(
    [double]$X,
    [double]$Y,
    [double]$ArcLength,
    [double]$Heading,
    [double]$BodyClearance,
    [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-spline test must create an explicit empty planning map.'
    return $map
}

function New-AlgorithmInput(
    [object[]]$Segments,
    [double]$ReserveMeters,
    [double]$EndpointTangentScale = (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.CubicBSpline.EndpointTangentScale = $EndpointTangentScale
    $options = $optionsConstructor.Invoke(@($configuration))
    return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
}

function Invoke-Candidate(
    [object[]]$Segments,
    [double]$ReserveMeters,
    [double]$Strength = 1.0,
    [double]$EndpointTangentScale = (1.0 / 3.0)) {
    return $smoothMethod.Invoke($smoother, @(
        (New-AlgorithmInput $Segments $ReserveMeters $EndpointTangentScale), $Strength, [Threading.CancellationToken]::None))
}

function Invoke-Smoothing(
    [object[]]$Segments,
    [double]$ReserveMeters,
    [double]$Strength = 1.0,
    [double]$EndpointTangentScale = (1.0 / 3.0)) {
    $candidate = Invoke-Candidate $Segments $ReserveMeters $Strength $EndpointTangentScale
    Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline smoothing must produce a candidate for the deterministic fixture.'
    return @(Get-PropertyValue $candidate 'Segments')
}

function Get-PointDistance($Left, $Right) {
    $deltaX = $Left.X - $Right.X
    $deltaY = $Left.Y - $Right.Y
    return [Math]::Sqrt($deltaX * $deltaX + $deltaY * $deltaY)
}

function Get-DistanceToSegment($Point, $Left, $Right) {
    $deltaX = $Right.X - $Left.X
    $deltaY = $Right.Y - $Left.Y
    $lengthSquared = $deltaX * $deltaX + $deltaY * $deltaY
    if ($lengthSquared -le 0.0) { return Get-PointDistance $Point $Left }
    $projection = (($Point.X - $Left.X) * $deltaX + ($Point.Y - $Left.Y) * $deltaY) / $lengthSquared
    $projection = [Math]::Max(0.0, [Math]::Min(1.0, $projection))
    $closest = New-Object PSObject -Property @{
        X = $Left.X + $projection * $deltaX
        Y = $Left.Y + $projection * $deltaY
    }
    return Get-PointDistance $Point $closest
}

function Get-DistanceToPolyline($Point, [object[]]$SourcePoints) {
    $minimum = [double]::PositiveInfinity
    for ($index = 1; $index -lt $SourcePoints.Count; $index++) {
        $minimum = [Math]::Min($minimum, (Get-DistanceToSegment $Point $SourcePoints[$index - 1] $SourcePoints[$index]))
    }
    return $minimum
}

function Get-TravelAngle($Left, $Right) {
    return [Math]::Atan2($Right.Y - $Left.Y, $Right.X - $Left.X)
}

function Get-AngleDifference([double]$Left, [double]$Right) {
    $difference = $Left - $Right
    while ($difference -gt [Math]::PI) { $difference -= 2.0 * [Math]::PI }
    while ($difference -lt -[Math]::PI) { $difference += 2.0 * [Math]::PI }
    return [Math]::Abs($difference)
}

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

$smootherType = Get-RequiredType ($algorithms + 'CubicBSplineSmoother')
$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')
$interpolatorType = Get-RequiredType ($processing + 'PathReferenceInterpolator')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
$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) 'Algorithm input must carry immutable smoothing options and the minimum clearance reserve for per-anchor movement limits.'
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
Assert-True ($null -ne $optionsConstructor) 'B-spline tests must create an immutable options snapshot.'
$interpolateMethod = $interpolatorType.GetMethod('TryInterpolateByArcLength', [Reflection.BindingFlags]'Static,Public,NonPublic')
Assert-True ($null -ne $interpolateMethod) 'PathReferenceInterpolator must expose arc-length interpolation.'
$smoother = [Activator]::CreateInstance($smootherType, $true)
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
Assert-True ($null -ne $smoothMethod) 'CubicBSplineSmoother must implement the internal smoother contract.'
Assert-Equal 'CubicBSpline' $smoother.Method.ToString() 'B-spline smoother must identify its public smoothing method.'

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

# Arc-length interpolation must bracket by local arc rather than by sample index.
$nonUniformPoints = [Array]::CreateInstance($pointType, 4)
$nonUniformPoints.SetValue((New-Point 0.0 0.0 0.000 0.0 1.0), 0)
$nonUniformPoints.SetValue((New-Point 5.0 0.0 0.050 0.1 0.9), 1)
$nonUniformPoints.SetValue((New-Point 10.0 0.0 0.100 0.2 0.8), 2)
$nonUniformPoints.SetValue((New-Point 20.0 0.0 0.125 0.3 0.7), 3)
$interpolateArguments = [object[]]@($nonUniformPoints, [double]0.1125, $null, $null)
Assert-True $interpolateMethod.Invoke($null, $interpolateArguments) 'Arc-length interpolation must accept a target within the final non-uniform interval.'
$arcReference = $interpolateArguments[2]
Assert-Near 15.0 $arcReference.X 0.000000001 'Target arc length 0.1125 must lie halfway through the final 0.100-0.125 interval, independent of point count.'
Assert-Near 0.1125 $arcReference.ArcLength 0.000000001 'Arc-length interpolation must preserve the requested target arc length.'
Assert-Near 0.25 $arcReference.Heading 0.000000001 'Arc-length interpolation must linearly interpolate heading.'
Assert-Near 0.75 $arcReference.BodyClearance 0.000000001 'Arc-length interpolation must linearly interpolate clearance.'

# Straight samples are returned exactly, so a straight is never distorted or densified.
$straightSource = @(
    (New-Point 0.0 0.0 0.0 0.0 0.03),
    (New-Point 1.0 0.0 1.0 0.0 0.03),
    (New-Point 2.0 0.0 2.0 0.0 0.03),
    (New-Point 3.0 0.0 3.0 0.0 0.03))
$straightResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)) 0.02
Assert-Equal 1 $straightResult.Count 'A single direction segment must produce exactly one candidate segment.'
Assert-Equal $straightSource.Count $straightResult[0].Points.Count 'A straight must retain its original samples.'
for ($index = 0; $index -lt $straightSource.Count; $index++) {
    Assert-Near $straightSource[$index].X $straightResult[0].Points[$index].X 0.0 'Straight X coordinates must remain exact.'
    Assert-Near $straightSource[$index].Y $straightResult[0].Points[$index].Y 0.0 'Straight Y coordinates must remain exact.'
}

# A five-anchor corner uses the preprocessor's 0.05 m sampling scale. It must retain exact endpoint poses,
# follow endpoint travel tangents, turn continuously, and stay within the per-anchor clearance reserve radius.
$cornerSource = @(
    (New-Point 0.0 0.0 0.0 0.0 0.08),
    (New-Point 0.05 0.0 0.05 0.0 0.08),
    (New-Point 0.10 0.0 0.10 0.0 0.08),
    (New-Point 0.10 0.05 0.15 ([Math]::PI / 2.0) 0.08),
    (New-Point 0.10 0.10 0.20 ([Math]::PI / 2.0) 0.08))
$cornerResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02
$cornerPoints = @($cornerResult[0].Points)
Assert-True ($cornerPoints.Count -gt $cornerSource.Count) 'A non-straight B-spline candidate must provide sampled curve geometry.'
$cornerStart = $cornerPoints[0]
$cornerEnd = $cornerPoints[$cornerPoints.Count - 1]
Assert-Near $cornerSource[0].X $cornerStart.X 0.0 'B-spline start X must be exact.'
Assert-Near $cornerSource[0].Y $cornerStart.Y 0.0 'B-spline start Y must be exact.'
Assert-Near $cornerSource[$cornerSource.Count - 1].X $cornerEnd.X 0.0 'B-spline end X must be exact.'
Assert-Near $cornerSource[$cornerSource.Count - 1].Y $cornerEnd.Y 0.0 'B-spline end Y must be exact.'
Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[0] $cornerPoints[1]) 0.0) 0.02 'B-spline start travel tangent must follow the supplied forward heading.'
Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[$cornerPoints.Count - 2] $cornerPoints[$cornerPoints.Count - 1]) ([Math]::PI / 2.0)) 0.02 'B-spline end travel tangent must follow the supplied forward heading.'
for ($index = 2; $index -lt $cornerPoints.Count; $index++) {
    $previousAngle = Get-TravelAngle $cornerPoints[$index - 2] $cornerPoints[$index - 1]
    $currentAngle = Get-TravelAngle $cornerPoints[$index - 1] $cornerPoints[$index]
    Assert-True ((Get-AngleDifference $previousAngle $currentAngle) -lt 0.08) 'B-spline corner samples must turn without a tangent discontinuity.'
}

# Endpoint tangent scale is an immutable option and must influence the clamped B-spline endpoint handle.
$shortHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.20)[0].Points
$longHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.60)[0].Points
Assert-True (($longHandlePoints[2].X - $shortHandlePoints[2].X) -gt 0.0001) 'A custom endpoint tangent scale must change the B-spline start handle and early curve samples.'

# Every evaluated point may deviate only by BodyClearance - reserve, never by raw BodyClearance.
$allowedRadius = 0.06
foreach ($point in $cornerPoints) {
    Assert-True ((Get-DistanceToPolyline $point $cornerSource) -le ($allowedRadius + 0.000000001)) 'Every B-spline displacement must stay inside the per-anchor clearance reserve radius.'
}

# A 0.035 m reserve radius must reject this corner by its parameter-matched source reference,
# even though the candidate's nearest-polyline distance is smaller than 0.035 m.
$parameterMatchedReserveCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.045
Assert-True (-not (Get-PropertyValue $parameterMatchedReserveCandidate 'Succeeded')) 'A medium reserve must reject B-spline geometry that exceeds its parameter-matched movement radius.'

# A tight reserve may not publish an evaluated B-spline that leaves its 0.005 m movement radius.
$tightReserveCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.075

# A tight endpoint circle that cannot meet the heading=0.2 rad tangent ray must fail, not silently rotate the handle.
$misalignedHeadingSource = @(
    (New-Point 0.0 0.0 0.0 0.2 0.08),
    (New-Point 0.05 0.0 0.05 0.0 0.08),
    (New-Point 0.10 0.0 0.10 0.0 0.08),
    (New-Point 0.10 0.05 0.15 ([Math]::PI / 2.0) 0.08),
    (New-Point 0.10 0.10 0.20 ([Math]::PI / 2.0) 0.08))
$misalignedHeadingCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $misalignedHeadingSource)) 0.075
$requiredFailures = New-Object System.Collections.Generic.List[string]
if (Get-PropertyValue $tightReserveCandidate 'Succeeded') {
    [void]$requiredFailures.Add('A tight reserve published an evaluated candidate outside its permitted movement radius.')
}
if (Get-PropertyValue $misalignedHeadingCandidate 'Succeeded') {
    [void]$requiredFailures.Add('A tight reserve silently accepted an endpoint handle that cannot follow the supplied travel tangent.')
}
Assert-Equal 0 $requiredFailures.Count ([string]::Join(' ', $requiredFailures))

# Adjacent direction segments retain their duplicated switch pose and independent topology; no fit may cross the switch.
$reverseSource = @(
    (New-Point 0.10 0.10 0.0 ([Math]::PI / 2.0) 0.08 $true),
    (New-Point 0.10 0.05 0.05 ([Math]::PI / 2.0) 0.08),
    (New-Point 0.10 0.0 0.10 ([Math]::PI / 2.0) 0.08))
$switchResult = Invoke-Smoothing @(
    (New-DirectionSegment 0 $forward $cornerSource $false $true),
    (New-DirectionSegment 1 $reverse $reverseSource $true $false)) 0.02
Assert-Equal 2 $switchResult.Count 'B-spline smoothing must preserve each direction segment boundary.'
Assert-True $switchResult[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.'
Assert-True $switchResult[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.'
$switchLeft = $switchResult[0].Points[$switchResult[0].Points.Count - 1]
$switchRight = $switchResult[1].Points[0]
Assert-Near $switchLeft.X $switchRight.X 0.0 'B-spline smoothing must retain the duplicated gear-switch X pose.'
Assert-Near $switchLeft.Y $switchRight.Y 0.0 'B-spline smoothing must retain the duplicated gear-switch Y pose.'

Write-Output 'Path smoothing cubic B-spline checks passed.'