ParkingRobot/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs

using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>只替换一个方向段内局部窗口，并保持其余方向拓扑不变。</summary>
internal sealed class LocalG2PathSplicer
{
    internal bool TryReplace(
        PreparedPath currentPath,
        LocalG2CandidateGeometry candidate,
        out PreparedPath replacedPath,
        out string reason)
    {
        replacedPath = null;
        reason = string.Empty;
        if (currentPath == null || candidate == null || candidate.SegmentIndex < 0 ||
            candidate.SegmentIndex >= currentPath.Segments.Count || candidate.RegionPoints == null ||
            candidate.RegionPoints.Count < 2)
        {
            reason = "局部 G2 拼接输入无效。";
            return false;
        }

        PreparedDirectionSegment source = currentPath.Segments[candidate.SegmentIndex];
        if (source == null || source.SegmentIndex != candidate.SegmentIndex ||
            !PathReferenceInterpolator.TryInterpolateByArcLength(source.Points, candidate.StartArcLengthMeters, out SmoothingPoint2D start, out reason) ||
            !PathReferenceInterpolator.TryInterpolateByArcLength(source.Points, candidate.EndArcLengthMeters, out SmoothingPoint2D end, out reason) ||
            !SamePosition(candidate.RegionPoints[0], start) || !SamePosition(candidate.RegionPoints[candidate.RegionPoints.Count - 1], end))
        {
            if (string.IsNullOrEmpty(reason)) reason = "局部 G2 候选端点不匹配原始窗口。";
            return false;
        }

        var combined = new List<SmoothingPoint2D>();
        for (int index = 0; index < source.Points.Count; index++)
        {
            SmoothingPoint2D point = source.Points[index];
            if (point.ArcLength < candidate.StartArcLengthMeters) AddWithoutNonGearDuplicates(combined, point);
        }
        // Candidate endpoints are only validated within a numerical tolerance.  The replacement
        // itself must use the exact source-window endpoints, including a possible gear marker.
        AddWithoutNonGearDuplicates(combined, start);
        for (int index = 1; index < candidate.RegionPoints.Count - 1; index++)
            AddWithoutNonGearDuplicates(combined, candidate.RegionPoints[index]);
        AddWithoutNonGearDuplicates(combined, end);
        for (int index = 0; index < source.Points.Count; index++)
        {
            SmoothingPoint2D point = source.Points[index];
            if (point.ArcLength > candidate.EndArcLengthMeters) AddWithoutNonGearDuplicates(combined, point);
        }
        if (!TryRecalculateArcLengths(combined, out IReadOnlyList<SmoothingPoint2D> localPoints, out reason)) return false;

        var segments = new List<PreparedDirectionSegment>(currentPath.Segments.Count);
        for (int index = 0; index < currentPath.Segments.Count; index++)
        {
            PreparedDirectionSegment segment = currentPath.Segments[index];
            if (index != candidate.SegmentIndex)
            {
                segments.Add(segment);
                continue;
            }
            segments.Add(new PreparedDirectionSegment(segment.SegmentIndex, segment.Direction, localPoints,
                segment.StartsAtGearSwitch, segment.EndsAtGearSwitch, segment.StartVehicleCurvaturePerMeter));
        }
        replacedPath = new PreparedPath(segments);
        return true;
    }

    private static bool TryRecalculateArcLengths(
        IReadOnlyList<SmoothingPoint2D> points,
        out IReadOnlyList<SmoothingPoint2D> recalculated,
        out string reason)
    {
        recalculated = null;
        reason = string.Empty;
        if (points == null || points.Count < 2)
        {
            reason = "局部 G2 拼接后方向段没有足够点。";
            return false;
        }
        var output = new List<SmoothingPoint2D>(points.Count);
        double arcLength = 0d;
        for (int index = 0; index < points.Count; index++)
        {
            SmoothingPoint2D point = points[index];
            if (!IsValid(point))
            {
                reason = "局部 G2 拼接点包含非法数值。";
                return false;
            }
            if (index > 0)
            {
                double distance = Distance(points[index - 1], point);
                if (!NumericGuard.IsPositiveFinite(distance))
                {
                    reason = "局部 G2 拼接后存在重复非换向点。";
                    return false;
                }
                arcLength += distance;
            }
            output.Add(new SmoothingPoint2D(point.X, point.Y, arcLength, point.Heading, point.UnwrappedHeading,
                point.BodyClearance, point.IsGearSwitchPoint, point.Source));
        }
        recalculated = output;
        return true;
    }

    private static void AddWithoutNonGearDuplicates(List<SmoothingPoint2D> output, SmoothingPoint2D point)
    {
        if (output.Count == 0)
        {
            output.Add(point);
            return;
        }
        SmoothingPoint2D previous = output[output.Count - 1];
        if (!previous.IsGearSwitchPoint && !point.IsGearSwitchPoint && SamePosition(previous, point)) return;
        output.Add(point);
    }

    private static bool IsValid(SmoothingPoint2D point) => point != null && NumericGuard.IsFinite(point.X) &&
        NumericGuard.IsFinite(point.Y) && NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
        NumericGuard.IsFinite(point.BodyClearance) && point.BodyClearance >= 0d;
    private static bool SamePosition(SmoothingPoint2D left, SmoothingPoint2D right) =>
        left != null && right != null && Distance(left, right) <= 1e-9d;
    private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
    {
        double dx = right.X - left.X;
        double dy = right.Y - left.Y;
        return Math.Sqrt(dx * dx + dy * dy);
    }
}