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

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;

/// <summary>
/// 一条经连续碰撞检查的恒曲率运动原语。
/// 位置和长度单位为 m，航向单位为 rad，曲率单位为 1/m；<see cref="Points"/> 不包含起点，只包含按积分顺序产生的后续点。
/// </summary>
public sealed class MotionPrimitive
{
    /// <summary>
    /// 创建不可变运动原语。
    /// 参数：start 为原语起点；direction 为行驶方向；curvaturePerMeter 为恒定曲率；actualLengthMeters 为已实际行驶长度；
    /// points 与 bodyClearancesMeters 按同一索引保存内部积分位姿和对应的车体净空；isGoalTruncation 表示末点是否首次命中目标容差。
    /// </summary>
    public MotionPrimitive(
        Pose2D start,
        TravelDirection direction,
        double curvaturePerMeter,
        double actualLengthMeters,
        IEnumerable<Pose2D> points,
        IEnumerable<double> bodyClearancesMeters,
        bool isGoalTruncation)
    {
        if (start == null) throw new ArgumentNullException(nameof(start));
        if (points == null) throw new ArgumentNullException(nameof(points));
        if (bodyClearancesMeters == null) throw new ArgumentNullException(nameof(bodyClearancesMeters));

        var copiedPoints = new List<Pose2D>(points);
        var copiedClearances = new List<double>(bodyClearancesMeters);
        if (copiedPoints.Count != copiedClearances.Count)
            throw new ArgumentException("The point and clearance counts must match.", nameof(bodyClearancesMeters));

        Start = start;
        Direction = direction;
        CurvaturePerMeter = curvaturePerMeter;
        ActualLengthMeters = actualLengthMeters;
        Points = new ReadOnlyCollection<Pose2D>(copiedPoints);
        BodyClearancesMeters = new ReadOnlyCollection<double>(copiedClearances);
        End = copiedPoints.Count == 0 ? start : copiedPoints[copiedPoints.Count - 1];
        IsGoalTruncation = isGoalTruncation;
    }

    /// <summary>原语起始车辆几何中心位姿。</summary>
    public Pose2D Start { get; }

    /// <summary>原语最后一个积分点；零长度终点候选时等于 <see cref="Start"/>。</summary>
    public Pose2D End { get; }

    /// <summary>原语对应的行驶方向。</summary>
    public TravelDirection Direction { get; }

    /// <summary>原语全程采用的恒定车辆曲率，单位 1/m。</summary>
    public double CurvaturePerMeter { get; }

    /// <summary>从 <see cref="Start"/> 到 <see cref="End"/> 的实际行驶弧长，单位 m。</summary>
    public double ActualLengthMeters { get; }

    /// <summary>不含起点的连续积分位姿，只读且按行驶顺序排列。</summary>
    public IReadOnlyList<Pose2D> Points { get; }

    /// <summary>与 <see cref="Points"/> 一一对应的扩大车体保守净空下界，单位 m。</summary>
    public IReadOnlyList<double> BodyClearancesMeters { get; }

    /// <summary>末点是否因首次满足目标位置、航向和方向约束而截断。</summary>
    public bool IsGoalTruncation { get; }
}