using Flyshot.Core.Domain;
using Microsoft.Extensions.Logging;

namespace Flyshot.ControllerClientCompat;

/// <summary>
/// MoveJoint 轨迹生成器。
/// 将起始关节角到目标关节角的单段运动，按速度、加速度、jerk 约束生成稠密点到点轨迹，
/// 供 FANUC J519 伺服流逐周期下发。
///
/// 核心思路：路径只取关节空间直线 q=q0+(q1-q0)*s(t)，时间律使用 7 阶平滑函数；
/// 生成后再按离散采样点反算速度、加速度和 jerk，确保真实下发点也满足限制。
/// </summary>
internal static class MoveJointTrajectoryGenerator
{
    /// <summary>
    /// 最小段基础时长（秒）。零位移时仍由采样对齐逻辑生成起点和终点两帧。
    /// </summary>
    private const double MinimumMoveJointDurationSeconds = 0.0;

    /// <summary>
    /// 7 阶平滑点到点时间律的一阶导数最大值。
    /// </summary>
    private const double SmoothPtpVelocityShapeCoefficient = 2.1875;

    /// <summary>
    /// 7 阶平滑点到点时间律的二阶导数最大值。
    /// </summary>
    private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;

    /// <summary>
    /// 7 阶平滑点到点时间律的三阶导数最大值。
    /// </summary>
    private const double SmoothPtpJerkShapeCoefficient = 52.5;

    /// <summary>
    /// 单次 MoveJoint 最大采样点数上限，防止极端配置下生成过大的轨迹数组。
    /// </summary>
    private const int MaxMoveJointSampleCount = 1_000_000;

    /// <summary>
    /// 离散限位校验允许的浮点容差。
    /// </summary>
    private const double DiscreteLimitTolerance = 1.000001;

    /// <summary>
    /// 离散限位校验失败时最多拉长的采样周期次数。
    /// </summary>
    private const int MaxDiscreteLimitStretchIterations = 10_000;

    /// <summary>
    /// 计算 MoveJoint 轨迹的完整结果。
    ///
    /// 处理流程：
    /// 1. 根据关节限位计算连续时间律理论最短时长
    /// 2. 按 speedRatio 缩放规划限位，并将时长对齐到固定伺服采样间隔
    /// 3. 用 7 阶平滑点到点时间律生成稠密轨迹点
    /// 4. 按离散点反查速度、加速度和 jerk，必要时拉长时长重算
    /// 5. 封装为 TrajectoryResult 返回
    /// </summary>
    /// <param name="robot">机器人配置，含自由度数和关节限位。</param>
    /// <param name="startJoints">起始关节角（弧度）。</param>
    /// <param name="targetJoints">目标关节角（弧度）。</param>
    /// <param name="speedRatio">速度倍率，必须大于 0；在生成阶段折算进有效关节限位。</param>
    /// <param name="logger">可选的诊断日志。</param>
    public static TrajectoryResult CreateResult(
        RobotProfile robot,
        IReadOnlyList<double> startJoints,
        IReadOnlyList<double> targetJoints,
        double speedRatio,
        ILogger? logger = null)
    {
        ArgumentNullException.ThrowIfNull(robot);
        ArgumentNullException.ThrowIfNull(startJoints);
        ArgumentNullException.ThrowIfNull(targetJoints);

        if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
        {
            throw new InvalidOperationException("Speed ratio must be greater than zero for MoveJoint execution.");
        }

        if (startJoints.Count != robot.DegreesOfFreedom || targetJoints.Count != robot.DegreesOfFreedom)
        {
            throw new InvalidOperationException($"MoveJoint expects {robot.DegreesOfFreedom} joints.");
        }

        var planningRobot = ApplySpeedRatioToJointLimits(robot, speedRatio);
        var requestedDurationSeconds = ResolveDurationSeconds(planningRobot, startJoints, targetJoints);
        var samplePeriodSeconds = robot.ServoPeriod.TotalSeconds;
        var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
        var denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);

        var stretchCount = 0;
        while (!SatisfiesDiscreteJointLimits(planningRobot, denseJointTrajectory))
        {
            stretchCount++;
            if (stretchCount > MaxDiscreteLimitStretchIterations)
            {
                throw new InvalidOperationException("MoveJoint duration cannot be stretched enough to satisfy joint limits.");
            }

            // 连续时间律满足限位后，仍以实际离散点为准；不满足时逐周期拉长后重采样。
            durationSeconds = AlignDurationToServoStep(durationSeconds + samplePeriodSeconds, samplePeriodSeconds);
            denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);
        }

        logger?.LogDebug(
            "MoveJointTrajectoryGenerator: requestedDuration={RequestedDuration:F4}s, duration={Duration:F4}s, speedRatio={SpeedRatio}, samplePeriod={SamplePeriod:F6}s, sampleCount={SampleCount}, stretchCount={StretchCount}",
            requestedDurationSeconds,
            durationSeconds,
            speedRatio,
            samplePeriodSeconds,
            denseJointTrajectory.Count,
            stretchCount);

        return new TrajectoryResult(
            programName: "move-joint",
            method: PlanningMethod.Doubles,
            isValid: true,
            duration: TimeSpan.FromSeconds(durationSeconds),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 2,
            plannedWaypointCount: denseJointTrajectory.Count,
            denseJointTrajectory: denseJointTrajectory);
    }

    /// <summary>
    /// 将执行速度倍率折算为 MoveJoint 规划限位，保持最终稠密轨迹仍按物理 8ms 周期生成。
    /// </summary>
    private static RobotProfile ApplySpeedRatioToJointLimits(RobotProfile robot, double speedRatio)
    {
        if (Math.Abs(speedRatio - 1.0) < 1e-12)
        {
            return robot;
        }

        var scaledLimits = robot.JointLimits
            .Select(limit => new JointLimit(
                limit.JointName,
                limit.VelocityLimit * speedRatio,
                limit.AccelerationLimit * speedRatio * speedRatio,
                limit.JerkLimit * speedRatio * speedRatio * speedRatio))
            .ToArray();

        return new RobotProfile(
            name: robot.Name,
            modelPath: robot.ModelPath,
            degreesOfFreedom: robot.DegreesOfFreedom,
            jointLimits: scaledLimits,
            jointCouplings: robot.JointCouplings,
            servoPeriod: robot.ServoPeriod,
            triggerPeriod: robot.TriggerPeriod);
    }

    /// <summary>
    /// 根据 7 阶平滑点到点时间律和每轴限位，计算 MoveJoint 理论最短时长。
    ///
    /// 时间律为 s(u)=35u^4-84u^5+70u^6-20u^7，其中 u=t/T。
    /// 各轴位移 d_i 共用同一个 s(t)，所以每轴分别按 d_i 放大速度、加速度和 jerk，再取全局最大时长。
    /// </summary>
    internal static double ResolveDurationSeconds(
        RobotProfile robot,
        IReadOnlyList<double> startJoints,
        IReadOnlyList<double> targetJoints)
    {
        var duration = MinimumMoveJointDurationSeconds;

        for (var index = 0; index < robot.DegreesOfFreedom; index++)
        {
            var distance = Math.Abs(targetJoints[index] - startJoints[index]);
            if (distance <= 0.0)
            {
                continue;
            }

            var limit = robot.JointLimits[index];
            var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limit.VelocityLimit;
            var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limit.AccelerationLimit);
            var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limit.JerkLimit);

            duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
        }

        return duration;
    }

    /// <summary>
    /// 将请求时长向上对齐到整数个采样周期，确保轨迹末帧正好落在 duration 处。
    /// </summary>
    /// <param name="durationSeconds">请求的理论最短时长（秒）。</param>
    /// <param name="samplePeriodSeconds">采样周期（秒）。</param>
    /// <returns>对齐后的时长，为 samplePeriodSeconds 的整数倍。</returns>
    internal static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
    {
        if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
        {
            throw new InvalidOperationException("Speed ratio must be greater than zero for MoveJoint execution.");
        }

        var intervals = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds);

        return intervals * samplePeriodSeconds;
    }

    /// <summary>
    /// 生成从起始关节角到目标关节角的稠密等时间隔轨迹点序列。
    ///
    /// 每行格式：[time_seconds, joint_0, joint_1, ..., joint_n-1]。
    /// </summary>
    internal static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
        IReadOnlyList<double> startJoints,
        IReadOnlyList<double> targetJoints,
        double durationSeconds,
        double samplePeriodSeconds)
    {
        var sampleCount = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) + 1;
        var rows = new List<IReadOnlyList<double>>(checked((int)sampleCount));

        for (var index = 0L; index < sampleCount; index++)
        {
            var time = Math.Min(index * samplePeriodSeconds, durationSeconds);
            rows.Add(CreateRow(time, durationSeconds, startJoints, targetJoints));
        }

        return rows;
    }

    /// <summary>
    /// 计算 7 阶平滑点到点时间律的位置归一化值。
    /// </summary>
    /// <param name="normalizedTime">归一化时间 u，取值会被限制在 [0, 1]。</param>
    /// <returns>归一化位置 s(u)，范围 [0, 1]。</returns>
    internal static double EvaluateSmoothPtpPositionScale(double normalizedTime)
    {
        var clamped = Math.Clamp(normalizedTime, 0.0, 1.0);
        var u2 = clamped * clamped;
        var u4 = u2 * u2;

        return u4 * (35.0 + (clamped * (-84.0 + (clamped * (70.0 - (20.0 * clamped))))));
    }

    /// <summary>
    /// 计算时长对应的采样间隔数（向上取整）。
    /// 采样间隔数 + 1 = 采样点数，因为轨迹包含起点和终点。
    /// </summary>
    private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
    {
        var rawIntervals = durationSeconds / samplePeriodSeconds;
        if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
        {
            throw new InvalidOperationException("MoveJoint sample count is not representable.");
        }

        var intervals = (long)Math.Ceiling(Math.Max(0.0, rawIntervals) - 1e-9);
        intervals = Math.Max(1, intervals);
        if (intervals + 1 > MaxMoveJointSampleCount)
        {
            throw new InvalidOperationException($"MoveJoint sample count must be between 2 and {MaxMoveJointSampleCount}.");
        }

        return intervals;
    }

    /// <summary>
    /// 构造单个轨迹行：[time_seconds, joint_0, ..., joint_N-1]。
    /// </summary>
    private static IReadOnlyList<double> CreateRow(
        double timeSeconds,
        double durationSeconds,
        IReadOnlyList<double> startJoints,
        IReadOnlyList<double> targetJoints)
    {
        var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
        var scale = EvaluateSmoothPtpPositionScale(u);
        var row = new double[startJoints.Count + 1];
        row[0] = Math.Round(timeSeconds, 9);

        for (var index = 0; index < startJoints.Count; index++)
        {
            row[index + 1] = startJoints[index] + ((targetJoints[index] - startJoints[index]) * scale);
        }

        return row;
    }

    /// <summary>
    /// 用生成后的离散采样点复核每轴速度、加速度和 jerk，避免连续时间律在采样后仍出现差分越限。
    /// </summary>
    private static bool SatisfiesDiscreteJointLimits(RobotProfile robot, IReadOnlyList<IReadOnlyList<double>> rows)
    {
        double? previousTime = null;
        double[]? previousPositions = null;
        double[]? previousVelocities = null;
        double[]? previousAccelerations = null;

        foreach (var row in rows)
        {
            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("MoveJoint dense trajectory timestamps must be strictly increasing.");
                }

                var velocities = new double[robot.DegreesOfFreedom];
                var accelerations = new double[robot.DegreesOfFreedom];
                for (var index = 0; index < robot.DegreesOfFreedom; index++)
                {
                    var limit = robot.JointLimits[index];
                    velocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
                    if (Math.Abs(velocities[index]) > limit.VelocityLimit * DiscreteLimitTolerance)
                    {
                        return false;
                    }

                    accelerations[index] = previousVelocities is null
                        ? 0.0
                        : (velocities[index] - previousVelocities[index]) / dt;
                    if (Math.Abs(accelerations[index]) > limit.AccelerationLimit * DiscreteLimitTolerance)
                    {
                        return false;
                    }

                    if (previousAccelerations is not null)
                    {
                        var jerk = (accelerations[index] - previousAccelerations[index]) / dt;
                        if (Math.Abs(jerk) > limit.JerkLimit * DiscreteLimitTolerance)
                        {
                            return false;
                        }
                    }
                }

                previousVelocities = velocities;
                previousAccelerations = accelerations;
            }

            previousTime = currentTime;
            previousPositions = currentPositions;
        }

        return true;
    }
}
