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

namespace Flyshot.ControllerClientCompat;

/// <summary>
/// MovePose 直角坐标轨迹生成器。
/// 将起始 TCP 位姿到目标 TCP 位姿的单段运动，按保守直角速度、加速度和 jerk 限制生成 8ms 稠密点。
/// </summary>
internal static class MovePoseTrajectoryGenerator
{
    /// <summary>
    /// 直角位置轴最大速度，单位为 mm/s。
    /// </summary>
    private const double LinearVelocityLimit = 50.0;

    /// <summary>
    /// 直角位置轴最大加速度，单位为 mm/s^2。
    /// </summary>
    private const double LinearAccelerationLimit = 250.0;

    /// <summary>
    /// 直角位置轴最大 jerk，单位为 mm/s^3。
    /// </summary>
    private const double LinearJerkLimit = 1250.0;

    /// <summary>
    /// 姿态轴最大速度，单位为 deg/s。
    /// </summary>
    private const double AngularVelocityLimit = 10.0;

    /// <summary>
    /// 姿态轴最大加速度，单位为 deg/s^2。
    /// </summary>
    private const double AngularAccelerationLimit = 50.0;

    /// <summary>
    /// 姿态轴最大 jerk，单位为 deg/s^3。
    /// </summary>
    private const double AngularJerkLimit = 250.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>
    /// 单次 MovePose 最大采样点数上限，避免极低速度倍率生成过大的队列。
    /// </summary>
    private const int MaxMovePoseSampleCount = 1_000_000;

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

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

    /// <summary>
    /// 计算 MovePose 轨迹的完整结果。
    /// </summary>
    /// <param name="startPose">起始位姿 [x,y,z,w,p,r]，单位为 mm/deg。</param>
    /// <param name="targetPose">目标位姿 [x,y,z,w,p,r]，单位为 mm/deg。</param>
    /// <param name="servoPeriod">J519 伺服发送周期。</param>
    /// <param name="speedRatio">速度倍率，必须大于 0。</param>
    /// <param name="logger">可选诊断日志。</param>
    /// <returns>包含稠密直角轨迹的规划结果。</returns>
    public static TrajectoryResult CreateResult(
        IReadOnlyList<double> startPose,
        IReadOnlyList<double> targetPose,
        TimeSpan servoPeriod,
        double speedRatio,
        ILogger? logger = null)
    {
        ArgumentNullException.ThrowIfNull(startPose);
        ArgumentNullException.ThrowIfNull(targetPose);
        EnsurePoseVector(startPose, nameof(startPose));
        EnsurePoseVector(targetPose, nameof(targetPose));

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

        var samplePeriodSeconds = servoPeriod.TotalSeconds;
        if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
        {
            throw new InvalidOperationException("MovePose servo period must be a finite positive duration.");
        }

        var requestedDurationSeconds = ResolveDurationSeconds(startPose, targetPose, speedRatio);
        var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
        var denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);

        var stretchCount = 0;
        while (!SatisfiesDefaultCartesianLimits(denseCartesianTrajectory, speedRatio))
        {
            stretchCount++;
            if (stretchCount > MaxDiscreteLimitStretchIterations)
            {
                throw new InvalidOperationException("MovePose duration cannot be stretched enough to satisfy Cartesian limits.");
            }

            // 离散差分以真实下发点为准；不满足时逐周期拉长后重采样。
            durationSeconds = AlignDurationToServoStep(durationSeconds + samplePeriodSeconds, samplePeriodSeconds);
            denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);
        }

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

        return new TrajectoryResult(
            programName: "move-pose",
            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: denseCartesianTrajectory.Count,
            denseCartesianTrajectory: denseCartesianTrajectory);
    }

    /// <summary>
    /// 检查稠密直角轨迹是否满足默认六维速度、加速度和 jerk 限制。
    /// </summary>
    /// <param name="rows">稠密轨迹行，每行为 [time,x,y,z,w,p,r]。</param>
    /// <param name="speedRatio">生成该轨迹时使用的速度倍率。</param>
    /// <returns>满足限制时返回 true，否则返回 false。</returns>
    public static bool SatisfiesDefaultCartesianLimits(IReadOnlyList<IReadOnlyList<double>> rows, double speedRatio)
    {
        if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
        {
            throw new InvalidOperationException("Speed ratio must be greater than zero for MovePose limit validation.");
        }

        double? previousTime = null;
        double[]? previousPositions = null;
        double[]? previousVelocities = null;
        double[]? previousAccelerations = null;

        foreach (var row in rows)
        {
            if (row.Count != 7)
            {
                throw new InvalidOperationException("MovePose dense trajectory rows must contain time plus six pose values.");
            }

            var currentTime = row[0];
            var currentPositions = row.Skip(1).Take(6).ToArray();

            if (previousTime is not null && previousPositions is not null)
            {
                var dt = currentTime - previousTime.Value;
                if (dt <= 0.0)
                {
                    throw new InvalidOperationException("MovePose dense trajectory timestamps must be strictly increasing.");
                }

                var velocities = new double[6];
                var accelerations = new double[6];
                for (var index = 0; index < 6; index++)
                {
                    var limits = GetAxisLimits(index, speedRatio);
                    velocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
                    if (Math.Abs(velocities[index]) > limits.Velocity * DiscreteLimitTolerance)
                    {
                        return false;
                    }

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

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

                previousVelocities = velocities;
                previousAccelerations = accelerations;
            }

            previousTime = currentTime;
            previousPositions = currentPositions;
        }

        return true;
    }

    /// <summary>
    /// 根据 7 阶平滑点到点时间律和直角六维限制计算理论最短时长。
    /// </summary>
    private static double ResolveDurationSeconds(IReadOnlyList<double> startPose, IReadOnlyList<double> targetPose, double speedRatio)
    {
        var duration = 0.0;
        for (var index = 0; index < 6; index++)
        {
            var distance = Math.Abs(targetPose[index] - startPose[index]);
            if (distance <= 0.0)
            {
                continue;
            }

            var limits = GetAxisLimits(index, speedRatio);
            var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limits.Velocity;
            var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limits.Acceleration);
            var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limits.Jerk);
            duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
        }

        return duration;
    }

    /// <summary>
    /// 生成从起始位姿到目标位姿的稠密等时间隔直角轨迹点序列。
    /// </summary>
    private static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
        IReadOnlyList<double> startPose,
        IReadOnlyList<double> targetPose,
        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, startPose, targetPose));
        }

        return rows;
    }

    /// <summary>
    /// 将请求时长向上对齐到整数个伺服周期。
    /// </summary>
    private static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
    {
        return ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) * samplePeriodSeconds;
    }

    /// <summary>
    /// 计算时长对应的采样间隔数，轨迹至少包含起点和终点两帧。
    /// </summary>
    private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
    {
        var rawIntervals = durationSeconds / samplePeriodSeconds;
        if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
        {
            throw new InvalidOperationException("MovePose 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 > MaxMovePoseSampleCount)
        {
            throw new InvalidOperationException($"MovePose sample count must be between 2 and {MaxMovePoseSampleCount}.");
        }

        return intervals;
    }

    /// <summary>
    /// 构造单个轨迹行：[time_seconds,x,y,z,w,p,r]。
    /// </summary>
    private static IReadOnlyList<double> CreateRow(
        double timeSeconds,
        double durationSeconds,
        IReadOnlyList<double> startPose,
        IReadOnlyList<double> targetPose)
    {
        var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
        var scale = MoveJointTrajectoryGenerator.EvaluateSmoothPtpPositionScale(u);
        var row = new double[7];
        row[0] = Math.Round(timeSeconds, 9);

        for (var index = 0; index < 6; index++)
        {
            row[index + 1] = startPose[index] + ((targetPose[index] - startPose[index]) * scale);
        }

        return row;
    }

    /// <summary>
    /// 获取指定直角轴在当前速度倍率下的有效限制。
    /// </summary>
    private static CartesianAxisLimit GetAxisLimits(int index, double speedRatio)
    {
        var linearAxis = index < 3;
        var velocity = linearAxis ? LinearVelocityLimit : AngularVelocityLimit;
        var acceleration = linearAxis ? LinearAccelerationLimit : AngularAccelerationLimit;
        var jerk = linearAxis ? LinearJerkLimit : AngularJerkLimit;
        return new CartesianAxisLimit(
            velocity * speedRatio,
            acceleration * speedRatio * speedRatio,
            jerk * speedRatio * speedRatio * speedRatio);
    }

    /// <summary>
    /// 校验位姿向量必须为六维有限数。
    /// </summary>
    private static void EnsurePoseVector(IReadOnlyList<double> pose, string parameterName)
    {
        if (pose.Count != 6)
        {
            throw new ArgumentException("MovePose expects pose [x,y,z,w,p,r].", parameterName);
        }

        if (pose.Any(static value => double.IsNaN(value) || double.IsInfinity(value)))
        {
            throw new ArgumentException("MovePose pose values must be finite.", parameterName);
        }
    }

    /// <summary>
    /// 表示单个直角轴的有效速度、加速度和 jerk 限制。
    /// </summary>
    private readonly record struct CartesianAxisLimit(double Velocity, double Acceleration, double Jerk);
}