FlyShotHost/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs

using Flyshot.Core.Domain;

namespace Flyshot.Core.Planning.Sampling;

/// <summary>
/// 负责在飞拍进入运行时前构建最终 8ms 发送队列，并执行发送口径的离散限幅校验。
/// </summary>
public static class FlyshotExecutionSendSequenceBuilder
{
    /// <summary>
    /// 根据连续规划轨迹构建最终发送队列；请求 speedRatio 只作为诊断信息保留。
    /// </summary>
    /// <param name="robot">机器人关节限值配置。</param>
    /// <param name="plannedTrajectory">规划阶段产出的连续轨迹。</param>
    /// <param name="result">规划结果。</param>
    /// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
    /// <param name="requestedSpeedRatio">请求的执行倍率，必须已经在规划阶段折算进轨迹。</param>
    /// <param name="samplingSpeedRatio">最终队列采样倍率；为空时使用 1.0，表示直接采样已调速后的规划轨迹。</param>
    /// <returns>通过离散校验后的飞拍最终发送结果。</returns>
    public static FlyshotPreparedExecution Build(
        RobotProfile robot,
        PlannedTrajectory plannedTrajectory,
        TrajectoryResult result,
        double servoPeriodSeconds,
        double requestedSpeedRatio,
        double? samplingSpeedRatio = null)
    {
        ArgumentNullException.ThrowIfNull(robot);
        ArgumentNullException.ThrowIfNull(plannedTrajectory);
        ArgumentNullException.ThrowIfNull(result);

        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 finalSpeedRatio = samplingSpeedRatio ?? requestedSpeedRatio;
        if (finalSpeedRatio <= 0.0 || double.IsNaN(finalSpeedRatio) || double.IsInfinity(finalSpeedRatio))
        {
            throw new ArgumentOutOfRangeException(nameof(samplingSpeedRatio), "最终队列采样倍率必须是有限正数。");
        }

        var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
            plannedTrajectory,
            servoPeriodSeconds,
            finalSpeedRatio);

        try
        {
            TrajectoryLimitValidator.ValidateJ519SendSamples(
                robot,
                samples,
                trajectoryName: result.ProgramName,
                validateJerk: true,
                strictJerkTolerance: true);
        }
        catch (InvalidOperationException exception)
        {
            throw new InvalidOperationException(
                $"飞拍最终发送队列在请求 speed_ratio={requestedSpeedRatio:F6} 下离散限幅校验失败。" +
                "请降低 planning_speed_scale 或 speed_ratio 后重新规划；执行侧不会在发送阶段临场重采样。",
                exception);
        }

        return BuildPreparedExecution(
            result,
            samples,
            requestedSpeedRatio,
            finalSpeedRatio,
            stretchIterationCount: 0);
    }

    /// <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(sample => J519SendTrajectorySampler.BuildTimingRow(sample, requestedSpeedRatio, stretchIterationCount))
            .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;
    }
}