using Flyshot.Core.Domain;

namespace Flyshot.Core.Planning.Sampling;

/// <summary>
/// 负责在飞拍进入运行时前构建最终 8ms 发送队列，并在必要时自动拉长执行时长直到通过离散限幅校验。
/// </summary>
public static class FlyshotExecutionSendSequenceBuilder
{
    /// <summary>
    /// 自动拉长时每轮采用的保守倍率缩减系数。
    /// </summary>
    private const double StretchFactor = 0.95;

    /// <summary>
    /// 自动拉长尝试的最大迭代次数。
    /// </summary>
    private const int MaxStretchIterations = 16;

    /// <summary>
    /// 根据规划层稠密轨迹和执行层 speedRatio 构建最终发送队列。
    /// </summary>
    /// <param name="robot">机器人关节限值配置。</param>
    /// <param name="result">规划结果。</param>
    /// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
    /// <param name="requestedSpeedRatio">请求的执行倍率。</param>
    /// <returns>通过离散校验后的飞拍最终发送结果。</returns>
    public static FlyshotPreparedExecution Build(
        RobotProfile robot,
        TrajectoryResult result,
        double servoPeriodSeconds,
        double requestedSpeedRatio)
    {
        ArgumentNullException.ThrowIfNull(robot);
        ArgumentNullException.ThrowIfNull(result);

        if (result.DenseJointTrajectory is null)
        {
            throw new InvalidOperationException("飞拍执行准备前必须先生成规划层稠密关节轨迹。");
        }

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

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

        var effectiveSpeedRatio = requestedSpeedRatio;
        InvalidOperationException? firstFailure = null;

        for (var iteration = 0; iteration <= MaxStretchIterations; iteration++)
        {
            var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
                result.DenseJointTrajectory,
                result.Duration.TotalSeconds,
                servoPeriodSeconds,
                effectiveSpeedRatio);

            try
            {
                TrajectoryLimitValidator.ValidateJ519SendSamples(
                    robot,
                    samples,
                    trajectoryName: result.ProgramName,
                    validateJerk: false);

                return BuildPreparedExecution(
                    result,
                    samples,
                    requestedSpeedRatio,
                    effectiveSpeedRatio,
                    iteration);
            }
            catch (InvalidOperationException exception)
            {
                firstFailure ??= exception;
                if (iteration >= MaxStretchIterations)
                {
                    throw new InvalidOperationException(
                        $"飞拍最终发送队列离散限幅校验失败，已达到最大自动拉长次数 {MaxStretchIterations}。",
                        firstFailure);
                }

                // 只在执行侧进一步保守化 trajectoryTime(sendTime) 映射，不回写规划层轨迹。
                effectiveSpeedRatio *= StretchFactor;
            }
        }

        throw new InvalidOperationException("飞拍最终发送队列构建失败。", firstFailure);
    }

    /// <summary>
    /// 将通过校验的 J519 采样点和触发绑定结果封装为稳定的飞拍执行结果。
    /// </summary>
    private static FlyshotPreparedExecution BuildPreparedExecution(
        TrajectoryResult result,
        IReadOnlyList<J519SendSample> samples,
        double requestedSpeedRatio,
        double finalSpeedRatio,
        int stretchIterationCount)
    {
        var preparedSamples = samples.Select(static sample =>
            new FlyshotPreparedSample(
                sample.SampleIndex,
                sample.SendTime,
                sample.TrajectoryTime,
                sample.SpeedRatio,
                sample.JointsDegrees)).ToArray();
        var sampleLookup = preparedSamples.ToDictionary(static sample => sample.SampleIndex);
        var triggerBindings = TriggerSampleBinder.Bind(
            result.TriggerTimeline,
            samples,
            result.TriggerSampleIndexOffsetCycles)
            .Select(binding =>
                new FlyshotPreparedTriggerBinding(
                    binding.Trigger,
                    sampleLookup[binding.Sample.SampleIndex],
                    binding.SampleIndex,
                    binding.FoundInWindow))
            .ToArray();
        var timingRows = samples.Select(J519SendTrajectorySampler.BuildTimingRow).ToArray();
        var jerkRows = BuildJerkRows(samples);
        var finalDurationSeconds = preparedSamples.Length == 0 ? 0.0 : preparedSamples[^1].SendTime;

        return new FlyshotPreparedExecution(
            preparedSamples,
            triggerBindings,
            timingRows,
            jerkRows,
            requestedSpeedRatio,
            finalSpeedRatio,
            finalDurationSeconds,
            stretchIterationCount);
    }

    /// <summary>
    /// 为最终发送点列构建跃度诊断行，保证导出工件和运行时发送复用同一份数据。
    /// </summary>
    private static IReadOnlyList<IReadOnlyList<double>> BuildJerkRows(IReadOnlyList<J519SendSample> samples)
    {
        var jerkRows = new List<IReadOnlyList<double>>(Math.Max(0, samples.Count - 1));
        double[]? previousVelocity = null;
        double[]? previousAcceleration = null;

        for (var index = 1; index < samples.Count; index++)
        {
            jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
                samples[index - 1].SendTime,
                samples[index].SendTime,
                samples[index - 1].JointsDegrees,
                samples[index].JointsDegrees,
                ref previousVelocity,
                ref previousAcceleration));
        }

        return jerkRows;
    }
}
