ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/DirectionSegmentView.cs

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// 平滑参考路径中单一前进或倒车方向段的只读视图；EM 规划和投影不得跨越该段的精确换向边界。
/// </summary>
public sealed class DirectionSegmentView
{
    /// <summary>
    /// 创建一个已重基到局部 S=0 的方向段只读视图。
    /// 参数：points 的 ArcLength 为 m 且方向必须一致，边界与 sourceStartArcLength 为 m；首点与起边界必须位于局部 S=0，终边界须在 1e-12 m 内匹配末点。
    /// 返回：防御性复制的不可变段；索引、方向、边界或弧长不一致时引发异常。
    /// </summary>
    public DirectionSegmentView(
        int segmentIndex,
        TravelDirection direction,
        IReadOnlyList<SmoothedPathPoint> points,
        ReferenceBoundary startBoundary,
        ReferenceBoundary endBoundary,
        double sourceStartArcLength)
    {
        if (segmentIndex < 0)
            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
        if (points == null || points.Count == 0)
            throw new ArgumentException("A direction segment requires points.", nameof(points));
        if (startBoundary == null || endBoundary == null)
            throw new ArgumentNullException(startBoundary == null ? nameof(startBoundary) : nameof(endBoundary));
        if (startBoundary.SegmentIndex != segmentIndex || endBoundary.SegmentIndex != segmentIndex)
            throw new ArgumentException("Boundary segment identity must match the segment.");
        if (points[0] == null || Math.Abs(points[0].ArcLength) > 1e-12d)
            throw new ArgumentException("A direction segment must begin at local S zero.", nameof(points));

        var copy = new List<SmoothedPathPoint>(points.Count);
        double previousS = -1d;
        for (int index = 0; index < points.Count; index++)
        {
            SmoothedPathPoint point = points[index];
            if (point == null || point.Direction != direction || point.ArcLength < 0d || point.ArcLength < previousS)
                throw new ArgumentException("Direction segment points are invalid.", nameof(points));
            copy.Add(point);
            previousS = point.ArcLength;
        }
        if (Math.Abs(endBoundary.SegmentLocalS - previousS) > 1e-12d)
            throw new ArgumentException("End boundary must match the final local S.", nameof(endBoundary));

        SegmentIndex = segmentIndex;
        Direction = direction;
        Points = new ReadOnlyCollection<SmoothedPathPoint>(copy);
        StartBoundary = startBoundary;
        EndBoundary = endBoundary;
        SourceStartArcLength = sourceStartArcLength;
    }

    /// <summary>
    /// 在原始平滑路径方向段序列中的零基索引。
    /// </summary>
    public int SegmentIndex { get; }

    /// <summary>
    /// 全段唯一的行驶方向；投影和优化不得在本视图内切换方向。
    /// </summary>
    public TravelDirection Direction { get; }

    /// <summary>
    /// 局部弧长已重基、按 S（m）非递减排列的平滑参考点只读副本。
    /// </summary>
    public IReadOnlyList<SmoothedPathPoint> Points { get; }

    /// <summary>
    /// 局部 S=0 的起始边界，含换向离开语义（若有）。
    /// </summary>
    public ReferenceBoundary StartBoundary { get; }

    /// <summary>
    /// 局部段末端的边界，含目标或换向接近语义（若有）。
    /// </summary>
    public ReferenceBoundary EndBoundary { get; }

    /// <summary>
    /// 本段局部 S=0 在原始平滑路径中的累计弧长，单位 m。
    /// </summary>
    public double SourceStartArcLength { get; }

    /// <summary>
    /// 本段从局部 S=0 到终边界的长度，单位 m。
    /// </summary>
    public double LengthMeters { get { return EndBoundary.SegmentLocalS; } }
}