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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// 将已发布的平滑参考路径拆分为不会跨越换向点的局部方向段视图。
/// 每个输出段以局部 S=0 重基（m），同时保留原始累计弧长（m）及目标/换向边界语义；索引覆盖或弧长不连续会被拒绝。
/// </summary>
public static class ReferencePathSegmenter
{
    /// <summary>
    /// 从已验证的平滑路径及其方向分段创建局部方向段视图。
    /// 参数：referencePath 必须含完整 Path 和 Segments，路径弧长为有限 m 制量；返回：按源段索引排列的只读视图，任何漏覆盖、方向或严格递增弧长错误均引发异常。
    /// </summary>
    public static IReadOnlyList<DirectionSegmentView> Create(PathSmoothingResult referencePath)
    {
        if (referencePath == null || referencePath.Path == null || referencePath.Segments == null ||
            referencePath.Path.Count == 0 || referencePath.Segments.Count == 0)
            throw new ArgumentException("A published reference path with direction segments is required.", nameof(referencePath));

        var result = new List<DirectionSegmentView>(referencePath.Segments.Count);
        int expectedStart = 0;
        for (int segmentListIndex = 0; segmentListIndex < referencePath.Segments.Count; segmentListIndex++)
        {
            SmoothedPathSegment sourceSegment = referencePath.Segments[segmentListIndex];
            if (sourceSegment == null || sourceSegment.SegmentIndex != segmentListIndex ||
                sourceSegment.StartIndex != expectedStart || sourceSegment.StartIndex < 0 ||
                sourceSegment.EndIndex < sourceSegment.StartIndex || sourceSegment.EndIndex >= referencePath.Path.Count)
                throw new ArgumentException("Reference path segment indexing is invalid.", nameof(referencePath));

            SmoothedPathPoint firstSourcePoint = referencePath.Path[sourceSegment.StartIndex];
            if (firstSourcePoint == null || firstSourcePoint.Direction != sourceSegment.Direction)
                throw new ArgumentException("Reference path segment start is invalid.", nameof(referencePath));

            double sourceStartS = firstSourcePoint.ArcLength;
            var rebasedPoints = new List<SmoothedPathPoint>(sourceSegment.EndIndex - sourceSegment.StartIndex + 1);
            double previousLocalS = -1d;
            for (int pathIndex = sourceSegment.StartIndex; pathIndex <= sourceSegment.EndIndex; pathIndex++)
            {
                SmoothedPathPoint sourcePoint = referencePath.Path[pathIndex];
                if (sourcePoint == null || sourcePoint.Direction != sourceSegment.Direction ||
                    double.IsNaN(sourcePoint.ArcLength) || double.IsInfinity(sourcePoint.ArcLength))
                    throw new ArgumentException("Reference path point is invalid.", nameof(referencePath));

                double localS = sourcePoint.ArcLength - sourceStartS;
                if (localS < 0d || (pathIndex > sourceSegment.StartIndex && localS <= previousLocalS))
                    throw new ArgumentException("Reference path segment arc length must strictly increase.", nameof(referencePath));
                rebasedPoints.Add(CloneAtLocalS(sourcePoint, localS));
                previousLocalS = localS;
            }

            EmBoundaryType startType = sourceSegment.StartsAtGearSwitch
                ? EmBoundaryType.GearSwitchDeparture
                : EmBoundaryType.None;
            EmBoundaryType endType = sourceSegment.EndsAtGearSwitch
                ? EmBoundaryType.GearSwitchApproach
                : segmentListIndex == referencePath.Segments.Count - 1
                    ? EmBoundaryType.Goal
                    : EmBoundaryType.None;
            var startBoundary = new ReferenceBoundary(sourceSegment.SegmentIndex, 0d, startType, sourceStartS);
            var endBoundary = new ReferenceBoundary(sourceSegment.SegmentIndex, previousLocalS, endType,
                referencePath.Path[sourceSegment.EndIndex].ArcLength);
            result.Add(new DirectionSegmentView(sourceSegment.SegmentIndex, sourceSegment.Direction, rebasedPoints,
                startBoundary, endBoundary, sourceStartS));
            expectedStart = sourceSegment.EndIndex + 1;
        }

        if (expectedStart != referencePath.Path.Count)
            throw new ArgumentException("Reference path segments do not cover the full path.", nameof(referencePath));
        return new ReadOnlyCollection<DirectionSegmentView>(result);
    }

    /// <summary>
    /// 复制一个源参考点并将其弧长重基到当前方向段局部坐标。
    /// 参数：source 保存世界 X/Y（m）、航向（rad）和曲率量，localS 为非负段局部 m 制弧长；返回：除 ArcLength 外保持源点几何和来源不变的副本。
    /// </summary>
    internal static SmoothedPathPoint CloneAtLocalS(SmoothedPathPoint source, double localS)
    {
        return new SmoothedPathPoint(
            source.X,
            source.Y,
            source.Heading,
            source.UnwrappedHeading,
            localS,
            source.Direction,
            source.GeometricCurvature,
            source.VehicleCurvature,
            source.VehicleCurvatureDerivative,
            source.BodyClearance,
            source.IsGearSwitchPoint,
            source.Source);
    }
}