fix: deduplicate Local G2 evaluator windows

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs

@@ -16,6 +16,7 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
 internal sealed class LocalG2CandidateEvaluator
 {
     private const double CurvatureRangeTolerance = 1e-6d;
+    private const double WindowPointToleranceMeters = 1e-10d;
     private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
         new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
     private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
@@ -192,13 +193,28 @@ internal sealed class LocalG2CandidateEvaluator
         for (int index = 0; index < segment.Points.Count; index++)
         {
             SmoothingPoint2D point = segment.Points[index];
-            if (point.ArcLength > startArcLength && point.ArcLength < endArcLength) points.Add(point);
+            if (point.ArcLength > startArcLength && point.ArcLength < endArcLength &&
+                !SamePosition(points[points.Count - 1], point))
+            {
+                points.Add(point);
+            }
         }
-        points.Add(end);
+        if (SamePosition(points[points.Count - 1], end))
+            points[points.Count - 1] = end;
+        else
+            points.Add(end);
         window = new ReadOnlyCollection<SmoothingPoint2D>(points);
         return true;
     }
 
+    private static bool SamePosition(SmoothingPoint2D left, SmoothingPoint2D right)
+    {
+        if (left == null || right == null) return false;
+        double x = right.X - left.X;
+        double y = right.Y - left.Y;
+        return x * x + y * y <= WindowPointToleranceMeters * WindowPointToleranceMeters;
+    }
+
     private bool TryAnalyzeWindow(PreparedDirectionSegment source, IReadOnlyList<SmoothingPoint2D> points, double spacing,
         out PathGeometryAnalysis analysis, out string reason)
     {

ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1

@@ -1,6 +1,8 @@
 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) {
@@ -21,6 +23,26 @@ 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.'
@@ -135,4 +157,56 @@ $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.'