using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;

/// <summary>已校验并按方向拆分的粗路径输入快照。</summary>
public sealed class PreparedPath
{
    /// <summary>创建不可变预处理路径。</summary>
    public PreparedPath(IReadOnlyList<PreparedDirectionSegment> segments)
    {
        if (segments == null || segments.Count == 0)
            throw new ArgumentException("A prepared path requires direction segments.", nameof(segments));
        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
        {
            if (segments[segmentIndex] == null)
                throw new ArgumentException("A prepared path cannot contain null direction segments.", nameof(segments));
        }

        Segments = CopyReadOnly(segments);
        Points = Flatten(Segments);
    }

    /// <summary>按原始前进/倒车拓扑排列的方向段。</summary>
    public IReadOnlyList<PreparedDirectionSegment> Segments { get; }

    /// <summary>将所有方向段顺序拼接后的点；换向重复点保留两次。</summary>
    public IReadOnlyList<SmoothingPoint2D> Points { get; }

    private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
    {
        var copy = new List<T>(source.Count);
        for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
        return new ReadOnlyCollection<T>(copy);
    }

    private static IReadOnlyList<SmoothingPoint2D> Flatten(IReadOnlyList<PreparedDirectionSegment> segments)
    {
        var points = new List<SmoothingPoint2D>();
        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
        {
            PreparedDirectionSegment segment = segments[segmentIndex];
            for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
                points.Add(segment.Points[pointIndex]);
        }
        return new ReadOnlyCollection<SmoothingPoint2D>(points);
    }
}
