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

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;

/// <summary>已校验、已按单一行驶方向分割并重采样的路径段。</summary>
public sealed class PreparedDirectionSegment
{
    /// <summary>创建不可变方向段。</summary>
    /// <param name="segmentIndex">在 <see cref="PreparedPath.Segments"/> 中从零开始的段索引。</param>
    /// <param name="direction">本段唯一的车辆行驶方向。</param>
    /// <param name="points">本段的非空采样点集合；构造后会复制为只读快照。</param>
    /// <param name="startsAtGearSwitch">若首点紧随换向，则为 <see langword="true"/>。</param>
    /// <param name="endsAtGearSwitch">若末点紧邻换向，则为 <see langword="true"/>。</param>
    public PreparedDirectionSegment(
        int segmentIndex,
        TravelDirection direction,
        IReadOnlyList<SmoothingPoint2D> points,
        bool startsAtGearSwitch,
        bool endsAtGearSwitch)
        : this(segmentIndex, direction, points, startsAtGearSwitch, endsAtGearSwitch, null)
    {
    }

    /// <summary>创建带有真实起始车辆曲率边界状态的不可变方向段。</summary>
    /// <param name="segmentIndex">在 <see cref="PreparedPath.Segments"/> 中从零开始的段索引。</param>
    /// <param name="direction">本段唯一的车辆行驶方向。</param>
    /// <param name="points">本段的非空采样点集合；构造后会复制为只读快照。</param>
    /// <param name="startsAtGearSwitch">若首点紧随换向，则为 <see langword="true"/>。</param>
    /// <param name="endsAtGearSwitch">若末点紧邻换向，则为 <see langword="true"/>。</param>
    /// <param name="startVehicleCurvaturePerMeter">物理起点车辆曲率边界，单位 1/m；未知时为 <see langword="null"/>。</param>
    public PreparedDirectionSegment(
        int segmentIndex,
        TravelDirection direction,
        IReadOnlyList<SmoothingPoint2D> points,
        bool startsAtGearSwitch,
        bool endsAtGearSwitch,
        double? startVehicleCurvaturePerMeter)
    {
        if (segmentIndex < 0) throw new ArgumentOutOfRangeException(nameof(segmentIndex));
        if (points == null || points.Count == 0) throw new ArgumentException("A prepared segment requires points.", nameof(points));
        if (startVehicleCurvaturePerMeter.HasValue && !IsFinite(startVehicleCurvaturePerMeter.Value))
            throw new ArgumentOutOfRangeException(nameof(startVehicleCurvaturePerMeter));

        SegmentIndex = segmentIndex;
        Direction = direction;
        Points = CopyReadOnly(points);
        StartsAtGearSwitch = startsAtGearSwitch;
        EndsAtGearSwitch = endsAtGearSwitch;
        StartVehicleCurvaturePerMeter = startVehicleCurvaturePerMeter;
    }

    /// <summary>从零开始的分段序号；在 <see cref="PreparedPath.Segments"/> 中必须与其位置一致。</summary>
    public int SegmentIndex { get; }

    /// <summary>该段的唯一行驶方向。</summary>
    public TravelDirection Direction { get; }

    /// <summary>不包含相邻段点的本段不可变采样点。</summary>
    public IReadOnlyList<SmoothingPoint2D> Points { get; }

    /// <summary>本段首点是否为换向后保留的新方向点。</summary>
    public bool StartsAtGearSwitch { get; }

    /// <summary>本段末点之后是否紧邻换向点。</summary>
    public bool EndsAtGearSwitch { get; }

    /// <summary>原始车辆在本段物理起点的曲率边界状态，单位 1/m。</summary>
    public double? StartVehicleCurvaturePerMeter { get; }

    private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);

    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);
    }
}