using Flyshot.Core.Planning;

namespace Flyshot.Core.Planning.Sampling;

/// <summary>
/// 负责把规划层关节轨迹重采样为 J519 物理发送周期上的角度制目标。
/// <para>
/// 算法约定：
/// 输入的稠密关节轨迹行格式固定为 [time, j1..jN]，time 为规划轨迹时间，关节单位为弧度；
/// 输出的 J519 采样点按物理伺服周期排列，关节单位转换为角度制，供 UDP 60015 实时下发和离线 ActualSend 文件共用。
/// </para>
/// <para>
/// 采样点数先按轨迹时间步长 trajectoryStep = servoPeriod * speedRatio 计算：
/// sampleCount = ceil(max(0, duration / trajectoryStep - 1e-9)) + 1。
/// 末尾额外保留一个终点钳制周期，确保轨迹时长不是周期整数倍时仍会输出最终点。
/// </para>
/// <para>
/// 飞拍最终发送队列优先从 <see cref="PlannedTrajectory"/> 重建连续三次样条并直接取点；
/// 旧稠密点入口仅保留给运行时 fallback 和兼容导出。
/// 第 k 个采样点的物理发送时间为 sendTime = k * servoPeriod；
/// speedRatio 不改变物理发送周期，只用于把发送时间映射回规划轨迹时间：
/// trajectoryTime = min(sendTime * speedRatio, duration)。
/// 之后在连续样条或旧稠密轨迹上求取关节，并把每个关节从 rad 转为 deg。
/// </para>
/// <para>
/// 诊断行也在这里统一生成：Timing 行格式为 sample_index + send_time + trajectory_time + speed_ratio；
/// Jerk 行使用相邻发送点上的角度制关节目标做后向差分，依次近似速度、加速度和跃度，格式为
/// start_time + end_time + dt + max_abs_jerk + jerk[j1..jN]。
/// </para>
/// </summary>
public static class J519SendTrajectorySampler
{
    /// <summary>
    /// 根据连续规划样条和 J519 伺服周期生成完整实发采样序列。
    /// </summary>
    /// <param name="trajectory">规划阶段产出的连续轨迹信息。</param>
    /// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
    /// <param name="speedRatio">速度倍率；只缩放轨迹采样时间，不改变物理发送周期。</param>
    /// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
    public static IReadOnlyList<J519SendSample> SamplePlannedTrajectory(
        PlannedTrajectory trajectory,
        double servoPeriodSeconds,
        double speedRatio)
    {
        ArgumentNullException.ThrowIfNull(trajectory);
        ValidatePlannedTrajectoryInputs(trajectory, servoPeriodSeconds, speedRatio);

        var durationSeconds = trajectory.WaypointTimes[^1];
        var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
        var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
        var spline = RebuildSpline(trajectory);
        var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));

        for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
        {
            // 直接在连续样条上按 trajectoryTime 取点，避免先离散再线性回采样引入局部跃度尖峰。
            var sendTime = sampleIndex * servoPeriodSeconds;
            var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
            var joints = spline.Evaluate(trajectoryTime).Select(RadiansToDegrees).ToArray();
            samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
        }

        return samples;
    }

    /// <summary>
    /// 根据 J519 伺服周期和 speed_ratio 生成完整实发采样序列。
    /// </summary>
    /// <param name="denseJointTrajectory">规划层稠密关节轨迹，每行格式为 [time, j1..jN]，关节单位为弧度。</param>
    /// <param name="durationSeconds">规划轨迹总时长，单位为秒。</param>
    /// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
    /// <param name="speedRatio">速度倍率；只缩放轨迹采样时间，不改变物理发送周期。</param>
    /// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
    public static IReadOnlyList<J519SendSample> SampleDenseJointTrajectory(
        IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
        double durationSeconds,
        double servoPeriodSeconds,
        double speedRatio)
    {
        ArgumentNullException.ThrowIfNull(denseJointTrajectory);
        ValidateInputs(denseJointTrajectory, durationSeconds, servoPeriodSeconds, speedRatio);

        var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
        var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
        var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));
        var segmentIndex = 0;

        for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
        {
            // J519 物理周期固定，speed_ratio 只用于把发送时间映射回原始轨迹时间。
            var sendTime = sampleIndex * servoPeriodSeconds;
            var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
            var joints = SampleDenseJointTrajectoryDegrees(denseJointTrajectory, trajectoryTime, ref segmentIndex);
            samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
        }

        return samples;
    }

    /// <summary>
    /// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发采样数。
    /// </summary>
    /// <param name="durationSeconds">规划轨迹总时长，单位为秒。</param>
    /// <param name="trajectoryStepSeconds">每个物理发送周期对应的轨迹时间步长，单位为秒。</param>
    /// <returns>包含终点钳制周期的采样点数量。</returns>
    public static long CalculateSampleCount(double durationSeconds, double trajectoryStepSeconds)
    {
        if (durationSeconds < 0.0)
        {
            throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
        }

        if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
        {
            throw new ArgumentOutOfRangeException(nameof(trajectoryStepSeconds), "轨迹采样步长必须是有限正数。");
        }

        // 非周期整数倍时多保留一个终点钳制周期，和真实 J519 下发序列保持一致。
        return (long)Math.Ceiling(Math.Max(0.0, (durationSeconds / trajectoryStepSeconds) - 1e-9)) + 1;
    }

    /// <summary>
    /// 构造实发时间映射文本行，格式为 sample_index + send_time + trajectory_time + final_speed_ratio。
    /// </summary>
    /// <param name="sample">待写出的 J519 实发采样点。</param>
    /// <returns>与 ActualSendTiming.txt 兼容的数值行。</returns>
    public static IReadOnlyList<double> BuildTimingRow(J519SendSample sample)
    {
        ArgumentNullException.ThrowIfNull(sample);
        return
        [
            sample.SampleIndex,
            Math.Round(sample.SendTime, 6),
            Math.Round(sample.TrajectoryTime, 6),
            Math.Round(sample.SpeedRatio, 6)
        ];
    }

    /// <summary>
    /// 构造带倍率诊断信息的实发时间映射文本行。
    /// </summary>
    /// <param name="sample">待写出的 J519 实发采样点。</param>
    /// <param name="requestedSpeedRatio">用户请求的执行倍率。</param>
    /// <param name="stretchIterationCount">历史诊断字段；当前执行侧不自动改写 speed_ratio，因此正常为 0。</param>
    /// <returns>前 4 列兼容 ActualSendTiming.txt，后续列记录请求倍率和倍率改写次数。</returns>
    public static IReadOnlyList<double> BuildTimingRow(
        J519SendSample sample,
        double requestedSpeedRatio,
        int stretchIterationCount)
    {
        if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio))
        {
            throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "请求 speed_ratio 必须是有限正数。");
        }

        if (stretchIterationCount < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "倍率改写次数不能为负数。");
        }

        var legacyRow = BuildTimingRow(sample);
        return
        [
            legacyRow[0],
            legacyRow[1],
            legacyRow[2],
            legacyRow[3],
            Math.Round(requestedSpeedRatio, 6),
            stretchIterationCount
        ];
    }

    /// <summary>
    /// 构造相邻发送点之间的角度制跃度统计行。
    /// </summary>
    /// <param name="previousTime">上一帧发送时间，单位为秒。</param>
    /// <param name="currentTime">当前帧发送时间，单位为秒。</param>
    /// <param name="previousJoints">上一帧角度制关节目标。</param>
    /// <param name="currentJoints">当前帧角度制关节目标。</param>
    /// <param name="previousVelocity">上一帧关节速度，调用后更新为当前帧速度。</param>
    /// <param name="previousAcceleration">上一帧关节加速度，调用后更新为当前帧加速度。</param>
    /// <returns>与 ActualSendJerkStats.txt 兼容的数值行。</returns>
    public static IReadOnlyList<double> BuildJerkRow(
        double previousTime,
        double currentTime,
        IReadOnlyList<double> previousJoints,
        IReadOnlyList<double> currentJoints,
        ref double[]? previousVelocity,
        ref double[]? previousAcceleration)
    {
        ArgumentNullException.ThrowIfNull(previousJoints);
        ArgumentNullException.ThrowIfNull(currentJoints);

        var dt = currentTime - previousTime;
        if (dt <= 0.0)
        {
            dt = 1e-9;
        }

        var jointCount = currentJoints.Count;
        var currentVelocity = new double[jointCount];
        var currentAcceleration = new double[jointCount];
        var currentJerk = new double[jointCount];
        var maxAbsJerk = 0.0;

        for (var index = 0; index < jointCount; index++)
        {
            currentVelocity[index] = (currentJoints[index] - previousJoints[index]) / dt;
            if (previousVelocity is not null)
            {
                currentAcceleration[index] = (currentVelocity[index] - previousVelocity[index]) / dt;
            }

            if (previousAcceleration is not null)
            {
                currentJerk[index] = (currentAcceleration[index] - previousAcceleration[index]) / dt;
                maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(currentJerk[index]));
            }
        }

        previousVelocity = currentVelocity;
        previousAcceleration = currentAcceleration;

        var row = new double[jointCount + 4];
        row[0] = Math.Round(previousTime, 6);
        row[1] = Math.Round(currentTime, 6);
        row[2] = Math.Round(dt, 6);
        row[3] = Math.Round(maxAbsJerk, 6);
        for (var index = 0; index < jointCount; index++)
        {
            row[index + 4] = Math.Round(currentJerk[index], 6);
        }

        return row;
    }

    /// <summary>
    /// 在稠密关节轨迹上按时间线性插值，并转换成 J519 下发使用的角度制目标。
    /// </summary>
    private static double[] SampleDenseJointTrajectoryDegrees(
        IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
        double trajectoryTime,
        ref int segmentIndex)
    {
        if (denseJointTrajectory.Count == 1 || trajectoryTime <= denseJointTrajectory[0][0])
        {
            return denseJointTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
        }

        var lastIndex = denseJointTrajectory.Count - 1;
        if (trajectoryTime >= denseJointTrajectory[lastIndex][0])
        {
            return denseJointTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
        }

        while (segmentIndex < lastIndex - 1 && denseJointTrajectory[segmentIndex + 1][0] < trajectoryTime)
        {
            segmentIndex++;
        }

        var start = denseJointTrajectory[segmentIndex];
        var end = denseJointTrajectory[segmentIndex + 1];
        var startTime = start[0];
        var endTime = end[0];
        var segmentDuration = endTime - startTime;
        var alpha = segmentDuration <= 0.0 ? 0.0 : (trajectoryTime - startTime) / segmentDuration;
        var joints = new double[start.Count - 1];
        for (var index = 0; index < joints.Length; index++)
        {
            joints[index] = RadiansToDegrees(start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha));
        }

        return joints;
    }

    /// <summary>
    /// 校验 J519 实发采样的基础输入，避免错误时间轴进入运行时链路。
    /// </summary>
    private static void ValidateInputs(
        IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
        double durationSeconds,
        double servoPeriodSeconds,
        double speedRatio)
    {
        if (denseJointTrajectory.Count == 0)
        {
            throw new InvalidOperationException("稠密关节轨迹为空。");
        }

        if (durationSeconds < 0.0)
        {
            throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
        }

        if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
        {
            throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "J519 伺服周期必须是有限正数。");
        }

        if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
        {
            throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
        }
    }

    /// <summary>
    /// 校验连续规划轨迹采样入口的基础输入，避免异常时间轴进入最终发送队列。
    /// </summary>
    private static void ValidatePlannedTrajectoryInputs(
        PlannedTrajectory trajectory,
        double servoPeriodSeconds,
        double speedRatio)
    {
        if (trajectory.WaypointTimes.Count < 2)
        {
            throw new InvalidOperationException("规划轨迹至少需要两个路点才能重建连续样条。");
        }

        if (trajectory.WaypointTimes[^1] < 0.0)
        {
            throw new ArgumentOutOfRangeException(nameof(trajectory), "规划轨迹总时长不能为负数。");
        }

        if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
        {
            throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "J519 伺服周期必须是有限正数。");
        }

        if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
        {
            throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
        }
    }

    /// <summary>
    /// 从规划轨迹重建连续三次样条，供飞拍最终发送队列直接采样。
    /// </summary>
    private static CubicSplineInterpolator RebuildSpline(PlannedTrajectory trajectory)
    {
        var times = trajectory.WaypointTimes.ToArray();
        var values = trajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray();
        return new CubicSplineInterpolator(times, values);
    }

    /// <summary>
    /// 角度单位转换：rad -> deg。
    /// </summary>
    private static double RadiansToDegrees(double radians)
    {
        return radians * 180.0 / Math.PI;
    }
}