using Flyshot.Core.Domain;

namespace Flyshot.Core.Planning.Sampling;

/// <summary>
/// 对最终生成的关节轨迹点做速度、加速度和 Jerk 离散复核。
/// </summary>
public static class TrajectoryLimitValidator
{
    /// <summary>
    /// 离散差分校验允许的默认浮点容差倍率。
    /// </summary>
    public const double DefaultLimitTolerance = 1.000001;

    /// <summary>
    /// 校验弧度制稠密关节轨迹是否满足机器人关节限制。
    /// </summary>
    /// <param name="robot">机器人约束配置。</param>
    /// <param name="rows">稠密轨迹行，格式为 time + 关节弧度。</param>
    /// <param name="toleranceMultiplier">限值容差倍率，用于过滤浮点舍入误差。</param>
    /// <param name="trajectoryName">诊断用轨迹名称。</param>
    /// <param name="validateJerk">是否校验离散 jerk；飞拍链路可临时关闭，仅保留速度/加速度约束。</param>
    public static void ValidateDenseJointTrajectory(
        RobotProfile robot,
        IReadOnlyList<IReadOnlyList<double>> rows,
        double toleranceMultiplier = DefaultLimitTolerance,
        string? trajectoryName = null,
        bool validateJerk = true)
    {
        ArgumentNullException.ThrowIfNull(robot);
        ArgumentNullException.ThrowIfNull(rows);
        ValidateTolerance(toleranceMultiplier);
        ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "dense-joint-trajectory", validateJerk);
    }

    /// <summary>
    /// 校验 J519 实际发送采样点是否满足机器人关节限制。
    /// </summary>
    /// <param name="robot">机器人约束配置。</param>
    /// <param name="samples">J519 发送采样点，关节单位为角度。</param>
    /// <param name="toleranceMultiplier">限值容差倍率，用于过滤浮点舍入误差。</param>
    /// <param name="trajectoryName">诊断用轨迹名称。</param>
    /// <param name="validateJerk">是否校验离散 jerk；飞拍链路可临时关闭，仅保留速度/加速度约束。</param>
    /// <param name="strictJerkTolerance">是否按原始 jerk 限值做硬约束；false 时保留历史宽容口径。</param>
    public static void ValidateJ519SendSamples(
        RobotProfile robot,
        IReadOnlyList<J519SendSample> samples,
        double toleranceMultiplier = DefaultLimitTolerance,
        string? trajectoryName = null,
        bool validateJerk = false,
        bool strictJerkTolerance = true)
    {
        ArgumentNullException.ThrowIfNull(robot);
        ArgumentNullException.ThrowIfNull(samples);
        ValidateTolerance(toleranceMultiplier);

        var rows = new List<IReadOnlyList<double>>(samples.Count);
        foreach (var sample in samples)
        {
            var row = new double[robot.DegreesOfFreedom + 1];
            row[0] = sample.SendTime;
            for (var index = 0; index < robot.DegreesOfFreedom; index++)
            {
                row[index + 1] = DegreesToRadians(sample.JointsDegrees[index]);
            }

            rows.Add(row);
        }

        ValidateRows(
            robot,
            rows,
            toleranceMultiplier,
            trajectoryName ?? "j519-send-trajectory",
            validateJerk,
            strictJerkTolerance);
    }

    /// <summary>
    /// 校验容差倍率必须为有限正数。
    /// </summary>
    private static void ValidateTolerance(double toleranceMultiplier)
    {
        if (toleranceMultiplier <= 0.0 || double.IsNaN(toleranceMultiplier) || double.IsInfinity(toleranceMultiplier))
        {
            throw new ArgumentOutOfRangeException(nameof(toleranceMultiplier), "限值容差倍率必须是有限正数。");
        }
    }

    /// <summary>
    /// 对弧度制轨迹行执行统一的离散差分限幅校验。
    /// </summary>
    private static void ValidateRows(
        RobotProfile robot,
        IReadOnlyList<IReadOnlyList<double>> rows,
        double toleranceMultiplier,
        string trajectoryName,
        bool validateJerk,
        bool strictJerkTolerance = false)
    {
        double? previousTime = null;
        double[]? previousPositions = null;
        double[]? previousVelocities = null;
        double[]? previousAccelerations = null;

        for (var rowIndex = 0; rowIndex < rows.Count; rowIndex++)
        {
            var row = rows[rowIndex];
            if (row.Count < robot.DegreesOfFreedom + 1)
            {
                throw new InvalidOperationException(
                    $"轨迹 {trajectoryName} 第 {rowIndex + 1} 行关节列数量不足，期望至少 {robot.DegreesOfFreedom + 1} 列，实际 {row.Count} 列。");
            }

            var currentTime = row[0];
            var currentPositions = new double[robot.DegreesOfFreedom];
            for (var index = 0; index < robot.DegreesOfFreedom; index++)
            {
                currentPositions[index] = row[index + 1];
            }

            if (previousTime is not null && previousPositions is not null)
            {
                var dt = currentTime - previousTime.Value;
                if (dt <= 0.0)
                {
                    throw new InvalidOperationException(
                        $"轨迹 {trajectoryName} 时间戳必须严格递增，第 {rowIndex + 1} 行 dt={dt:F9}s。");
                }

                var currentVelocities = new double[robot.DegreesOfFreedom];
                var currentAccelerations = new double[robot.DegreesOfFreedom];
                for (var index = 0; index < robot.DegreesOfFreedom; index++)
                {
                    var jointLimit = robot.JointLimits[index];
                    currentVelocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
                    ThrowIfExceeded(
                        trajectoryName,
                        rowIndex,
                        previousTime.Value,
                        currentTime,
                        jointLimit.JointName,
                        "速度",
                        currentVelocities[index],
                        jointLimit.VelocityLimit,
                        toleranceMultiplier);

                    currentAccelerations[index] = previousVelocities is null
                        ? 0.0
                        : (currentVelocities[index] - previousVelocities[index]) / dt;
                    ThrowIfExceeded(
                        trajectoryName,
                        rowIndex,
                        previousTime.Value,
                        currentTime,
                        jointLimit.JointName,
                        "加速度",
                        currentAccelerations[index],
                        jointLimit.AccelerationLimit,
                        toleranceMultiplier);

                    if (validateJerk && previousAccelerations is not null)
                    {
                        var jerk = (currentAccelerations[index] - previousAccelerations[index]) / dt;
                        ThrowIfExceeded(
                            trajectoryName,
                            rowIndex,
                            previousTime.Value,
                            currentTime,
                            jointLimit.JointName,
                            "Jerk",
                            jerk,
                            jointLimit.JerkLimit,
                            strictJerkTolerance ? toleranceMultiplier : toleranceMultiplier * 4.0);
                    }
                }

                previousVelocities = currentVelocities;
                previousAccelerations = currentAccelerations;
            }

            previousTime = currentTime;
            previousPositions = currentPositions;
        }
    }

    /// <summary>
    /// 当某个差分指标超过限制时抛出包含关节和时间窗的诊断异常。
    /// </summary>
    private static void ThrowIfExceeded(
        string trajectoryName,
        int rowIndex,
        double previousTime,
        double currentTime,
        string jointName,
        string metricName,
        double actual,
        double limit,
        double toleranceMultiplier)
    {
        var absActual = Math.Abs(actual);
        var effectiveLimit = limit * toleranceMultiplier;
        if (absActual <= effectiveLimit)
        {
            return;
        }

        throw new InvalidOperationException(
            $"轨迹 {trajectoryName} 第 {rowIndex + 1} 行 {jointName} {metricName}超限: " +
            $"time={previousTime:F6}->{currentTime:F6}s, actual={actual:F6}, limit={limit:F6}, ratio={absActual / limit:F4}。");
    }

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