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

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

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>按硬方向边界生成并合并局部 G2 曲率事件的候选窗口。</summary>
internal sealed class LocalG2WindowPlanner
{
    private const double MergeToleranceMeters = 1e-9d;

    internal bool TryPlan(
        PreparedPath originalPath,
        IReadOnlyList<CurvatureTransition> transitions,
        LocalG2OptionsSnapshot options,
        out IReadOnlyList<LocalG2SmoothingRegion> regions,
        out string reason)
    {
        regions = Empty<LocalG2SmoothingRegion>();
        reason = string.Empty;
        if (originalPath == null || transitions == null || options == null)
        {
            reason = "局部 G2 窗口规划输入无效。";
            return false;
        }
        if (!TryGetSegmentLengths(originalPath, out Dictionary<int, double> segmentLengths, out reason)) return false;

        var ordered = new List<CurvatureTransition>(transitions.Count);
        for (int index = 0; index < transitions.Count; index++)
        {
            CurvatureTransition transition = transitions[index];
            if (transition == null || !segmentLengths.TryGetValue(transition.SegmentIndex, out double length) ||
                !NumericGuard.IsFinite(transition.LocalArcLengthMeters) || transition.LocalArcLengthMeters < 0d ||
                transition.LocalArcLengthMeters > length + MergeToleranceMeters)
            {
                reason = "局部 G2 曲率事件不属于有效方向分段。";
                return false;
            }
            ordered.Add(transition);
        }
        ordered.Sort(CompareTransitions);

        var planned = new List<LocalG2SmoothingRegion>();
        int cursor = 0;
        while (cursor < ordered.Count)
        {
            CurvatureTransition first = ordered[cursor];
            double segmentLength = segmentLengths[first.SegmentIndex];
            WindowRange merged = MaximumLegalRange(first.LocalArcLengthMeters, segmentLength, options.MaximumWindowLengthMeters);
            var group = new List<CurvatureTransition> { first };
            cursor++;

            while (cursor < ordered.Count && ordered[cursor].SegmentIndex == first.SegmentIndex)
            {
                CurvatureTransition next = ordered[cursor];
                WindowRange nextRange = MaximumLegalRange(next.LocalArcLengthMeters, segmentLength, options.MaximumWindowLengthMeters);
                if (nextRange.StartArcLengthMeters > merged.EndArcLengthMeters + MergeToleranceMeters) break;
                group.Add(next);
                merged = new WindowRange(
                    Math.Min(merged.StartArcLengthMeters, nextRange.StartArcLengthMeters),
                    Math.Max(merged.EndArcLengthMeters, nextRange.EndArcLengthMeters));
                cursor++;
            }

            planned.Add(new LocalG2SmoothingRegion(
                first.SegmentIndex,
                group,
                merged.StartArcLengthMeters,
                merged.EndArcLengthMeters,
                BuildVariants(group, segmentLength, options)));
        }
        regions = new ReadOnlyCollection<LocalG2SmoothingRegion>(planned);
        return true;
    }

    private static IReadOnlyList<LocalG2WindowVariant> BuildVariants(
        IReadOnlyList<CurvatureTransition> transitions,
        double segmentLength,
        LocalG2OptionsSnapshot options)
    {
        var variants = new List<LocalG2WindowVariant>();
        double firstEvent = transitions[0].LocalArcLengthMeters;
        double lastEvent = transitions[transitions.Count - 1].LocalArcLengthMeters;
        double anchor = (firstEvent + lastEvent) / 2d;
        foreach (double target in BuildTargets(options, segmentLength))
        {
            if (variants.Count >= options.MaximumCandidatesPerRegion) break;
            AddIfLegal(variants, target, 0.5d, anchor, firstEvent, lastEvent, segmentLength, true, options.MaximumCandidatesPerRegion);
            AddIfLegal(variants, target, 0.4d, anchor, firstEvent, lastEvent, segmentLength, false, options.MaximumCandidatesPerRegion);
            AddIfLegal(variants, target, 0.6d, anchor, firstEvent, lastEvent, segmentLength, false, options.MaximumCandidatesPerRegion);
        }
        return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
    }

    private static void AddIfLegal(List<LocalG2WindowVariant> variants, double target, double leftRatio,
        double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift, int maximumCount)
    {
        if (variants.Count >= maximumCount) return;
        double left = target * leftRatio;
        double right = target - left;
        double availableLeft = anchor;
        double availableRight = segmentLength - anchor;
        if (permitBoundaryShift)
        {
            left = Math.Min(left, availableLeft);
            right = Math.Min(right, availableRight);
            double missing = target - left - right;
            double addRight = Math.Min(missing, availableRight - right);
            right += addRight;
            left += Math.Min(missing - addRight, availableLeft - left);
        }
        else if (left > availableLeft + MergeToleranceMeters || right > availableRight + MergeToleranceMeters)
        {
            return;
        }

        double start = anchor - left;
        double end = anchor + right;
        if (start > firstEvent + MergeToleranceMeters || end + MergeToleranceMeters < lastEvent || end - start + MergeToleranceMeters < target)
            return;
        variants.Add(new LocalG2WindowVariant(variants.Count, start, end, left, right));
    }

    private static IReadOnlyList<double> BuildTargets(LocalG2OptionsSnapshot options, double segmentLength)
    {
        double[] requested =
        {
            options.PreferredWindowLengthMeters,
            0.75d * options.PreferredWindowLengthMeters,
            1.25d * options.PreferredWindowLengthMeters,
            options.MinimumWindowLengthMeters,
            options.MaximumWindowLengthMeters,
        };
        var targets = new List<double>(requested.Length);
        for (int index = 0; index < requested.Length; index++)
        {
            double target = Math.Min(segmentLength,
                Math.Max(options.MinimumWindowLengthMeters, Math.Min(options.MaximumWindowLengthMeters, requested[index])));
            bool duplicate = false;
            for (int prior = 0; prior < targets.Count; prior++)
            {
                if (Math.Abs(targets[prior] - target) <= MergeToleranceMeters)
                {
                    duplicate = true;
                    break;
                }
            }
            if (!duplicate) targets.Add(target);
        }
        return targets;
    }

    private static WindowRange MaximumLegalRange(double eventArcLength, double segmentLength, double maximumWindowLength)
    {
        double target = Math.Min(maximumWindowLength, segmentLength);
        return new WindowRange(
            Math.Max(0d, eventArcLength - target),
            Math.Min(segmentLength, eventArcLength + target));
    }

    private static bool TryGetSegmentLengths(PreparedPath originalPath, out Dictionary<int, double> lengths, out string reason)
    {
        lengths = new Dictionary<int, double>();
        reason = string.Empty;
        for (int position = 0; position < originalPath.Segments.Count; position++)
        {
            PreparedDirectionSegment segment = originalPath.Segments[position];
            if (segment == null || segment.SegmentIndex != position || segment.Points == null || segment.Points.Count == 0)
            {
                reason = "局部 G2 窗口规划的预处理方向分段无效。";
                return false;
            }
            double previousArc = -1d;
            for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
            {
                double arc = segment.Points[pointIndex].ArcLength;
                if (!NumericGuard.IsFinite(arc) || arc < 0d || arc < previousArc)
                {
                    reason = "局部 G2 窗口规划要求分段弧长有限且非递减。";
                    return false;
                }
                previousArc = arc;
            }
            lengths.Add(segment.SegmentIndex, previousArc);
        }
        return true;
    }

    private static int CompareTransitions(CurvatureTransition left, CurvatureTransition right)
    {
        int segment = left.SegmentIndex.CompareTo(right.SegmentIndex);
        if (segment != 0) return segment;
        int arc = left.LocalArcLengthMeters.CompareTo(right.LocalArcLengthMeters);
        return arc != 0 ? arc : left.LeftCoarsePathIndex.CompareTo(right.LeftCoarsePathIndex);
    }

    private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());

    private readonly struct WindowRange
    {
        internal WindowRange(double startArcLengthMeters, double endArcLengthMeters)
        {
            StartArcLengthMeters = startArcLengthMeters;
            EndArcLengthMeters = endArcLengthMeters;
        }

        internal double StartArcLengthMeters { get; }
        internal double EndArcLengthMeters { get; }
    }
}