using Flyshot.Core.Domain;

namespace Flyshot.Core.Planning;

/// <summary>
/// 表示一次规划完成后的轨迹结果，包含原始示教点、补中点后的完整路点、时间轴和收敛信息。
/// </summary>
public sealed class PlannedTrajectory
{
    /// <summary>
    /// 初始化一份已验证的规划轨迹。
    /// </summary>
    public PlannedTrajectory(
        RobotProfile robot,
        FlyshotProgram originalProgram,
        IReadOnlyList<JointWaypoint> plannedWaypoints,
        IReadOnlyList<double> waypointTimes,
        IReadOnlyList<double> segmentDurations,
        IReadOnlyList<double> segmentScales,
        PlanningMethod method,
        int iterations,
        double threshold)
    {
        ArgumentNullException.ThrowIfNull(robot);
        ArgumentNullException.ThrowIfNull(originalProgram);
        ArgumentNullException.ThrowIfNull(plannedWaypoints);
        ArgumentNullException.ThrowIfNull(waypointTimes);
        ArgumentNullException.ThrowIfNull(segmentDurations);
        ArgumentNullException.ThrowIfNull(segmentScales);

        if (waypointTimes.Count != plannedWaypoints.Count)
        {
            throw new ArgumentException("路点时间轴长度必须与路点数量一致。", nameof(waypointTimes));
        }

        if (segmentDurations.Count != plannedWaypoints.Count - 1)
        {
            throw new ArgumentException("段时长数量必须等于路点数量减一。", nameof(segmentDurations));
        }

        if (segmentScales.Count != segmentDurations.Count)
        {
            throw new ArgumentException("段缩放因子数量必须与段时长数量一致。", nameof(segmentScales));
        }

        if (iterations < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(iterations), "迭代次数不能为负数。");
        }

        if (threshold < 0.0)
        {
            throw new ArgumentOutOfRangeException(nameof(threshold), "收敛阈值不能为负数。");
        }

        Robot = robot;
        OriginalProgram = originalProgram;
        PlannedWaypoints = plannedWaypoints;
        WaypointTimes = waypointTimes;
        SegmentDurations = segmentDurations;
        SegmentScales = segmentScales;
        Method = method;
        Iterations = iterations;
        Threshold = threshold;

        OriginalWaypointCount = originalProgram.Waypoints.Count;
        InsertedWaypointCount = plannedWaypoints.Count - OriginalWaypointCount;
        PlannedWaypointCount = plannedWaypoints.Count;
    }

    /// <summary>
    /// 获取规划时使用的机器人配置文件。
    /// </summary>
    public RobotProfile Robot { get; }

    /// <summary>
    /// 获取原始上传的飞拍程序（含示教点、shot_flags 等）。
    /// </summary>
    public FlyshotProgram OriginalProgram { get; }

    /// <summary>
    /// 获取规划后完整的路点列表（可能已补中点）。
    /// </summary>
    public IReadOnlyList<JointWaypoint> PlannedWaypoints { get; }

    /// <summary>
    /// 获取每个路点对应的绝对时间（单位：秒）。
    /// </summary>
    public IReadOnlyList<double> WaypointTimes { get; }

    /// <summary>
    /// 获取每段的规划时长（单位：秒）。
    /// </summary>
    public IReadOnlyList<double> SegmentDurations { get; }

    /// <summary>
    /// 获取每段最终的速度/加速度/jerk 缩放因子。
    /// </summary>
    public IReadOnlyList<double> SegmentScales { get; }

    /// <summary>
    /// 获取本次规划使用的方法。
    /// </summary>
    public PlanningMethod Method { get; }

    /// <summary>
    /// 获取 ICSP 实际执行的迭代轮数。
    /// </summary>
    public int Iterations { get; }

    /// <summary>
    /// 获取最终收敛指标（sum(|scale_i - 1|)）。
    /// </summary>
    public double Threshold { get; }

    /// <summary>
    /// 获取原始示教点数量（不含补中点）。
    /// </summary>
    public int OriginalWaypointCount { get; }

    /// <summary>
    /// 获取补中点插入的数量。
    /// </summary>
    public int InsertedWaypointCount { get; }

    /// <summary>
    /// 获取规划后的总路点数量（含补中点）。
    /// </summary>
    public int PlannedWaypointCount { get; }
}
