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

using Flyshot.Core.Domain;

namespace Flyshot.Core.Planning.Sampling;

/// <summary>
/// 负责把理论触发事件绑定到实际 J519 发送 sample。
/// 绑定规则为：先在理论触发时间前后固定时间窗内筛候选点，再优先选择关节差最小的 sample。
/// </summary>
public static class TriggerSampleBinder
{
    /// <summary>
    /// 当前触发绑定使用的固定近时窗半宽，单位为秒。
    /// 即：以理论触发时间为中心，在前后各 100ms 的范围内寻找最优 sample。
    /// </summary>
    public const double TriggerBindingToleranceSeconds = 0.1;

    /// <summary>
    /// 把一组触发事件绑定到采样后的 J519 sample。
    /// </summary>
    /// <param name="triggers">待绑定的触发事件。</param>
    /// <param name="samples">J519 实发 sample。</param>
    /// <param name="sampleIndexOffsetCycles">在最佳绑定 sample 基础上继续向后偏移的命令周期数。</param>
    /// <returns>按最终 sampleIndex 排序的绑定结果。</returns>
    public static IReadOnlyList<TriggerSampleBinding> Bind(
        IReadOnlyList<TrajectoryDoEvent> triggers,
        IReadOnlyList<J519SendSample> samples,
        int sampleIndexOffsetCycles = 0)
    {
        ArgumentNullException.ThrowIfNull(triggers);
        ArgumentNullException.ThrowIfNull(samples);
        if (sampleIndexOffsetCycles < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(sampleIndexOffsetCycles), "触发 sample 偏移周期不能为负数。");
        }

        var bindings = new List<TriggerSampleBinding>(triggers.Count);
        if (triggers.Count == 0 || samples.Count == 0)
        {
            return bindings;
        }

        var lastSampleIndex = samples.Count - 1;
        foreach (var trigger in triggers)
        {
            var fallbackIndex = FindClosestTrajectoryTimeSampleIndex(samples, trigger.TriggerTime);
            var candidateStart = Math.Max(0, fallbackIndex - 1);
            var candidateEnd = Math.Min(lastSampleIndex, fallbackIndex + 1);
            var bestIndex = fallbackIndex;
            var bestScore = double.PositiveInfinity;
            var foundInWindow = false;

            for (var sampleIndex = candidateStart; sampleIndex <= candidateEnd; sampleIndex++)
            {
                var sample = samples[sampleIndex];
                if (Math.Abs(sample.TrajectoryTime - trigger.TriggerTime) > TriggerBindingToleranceSeconds)
                {
                    continue;
                }

                foundInWindow = true;
                var score = trigger.ReferenceJointsDegrees is { Count: > 0 }
                    ? ComputeJointDifferenceScore(trigger.ReferenceJointsDegrees, sample.JointsDegrees)
                    : Math.Abs(sample.TrajectoryTime - trigger.TriggerTime);
                if (score < bestScore)
                {
                    bestScore = score;
                    bestIndex = sampleIndex;
                }
            }

            var finalIndex = Math.Min(lastSampleIndex, bestIndex + sampleIndexOffsetCycles);
            bindings.Add(new TriggerSampleBinding(trigger, samples[finalIndex], finalIndex, foundInWindow));
        }

        return bindings
            .OrderBy(static binding => binding.SampleIndex)
            .ThenBy(static binding => binding.Trigger.WaypointIndex)
            .ToArray();
    }

    /// <summary>
    /// 按轨迹时间找到最接近理论触发时刻的 sample。
    /// </summary>
    public static int FindClosestTrajectoryTimeSampleIndex(IReadOnlyList<J519SendSample> samples, double triggerTime)
    {
        ArgumentNullException.ThrowIfNull(samples);

        var bestIndex = 0;
        var bestDelta = double.PositiveInfinity;
        for (var sampleIndex = 0; sampleIndex < samples.Count; sampleIndex++)
        {
            var delta = Math.Abs(samples[sampleIndex].TrajectoryTime - triggerTime);
            if (delta < bestDelta)
            {
                bestDelta = delta;
                bestIndex = sampleIndex;
            }
        }

        return bestIndex;
    }

    /// <summary>
    /// 计算参考示教点与候选 sample 的关节差评分，使用平方和即可稳定排序。
    /// </summary>
    public static double ComputeJointDifferenceScore(
        IReadOnlyList<double>? referenceJointsDegrees,
        IReadOnlyList<double> sampleJointsDegrees)
    {
        if (referenceJointsDegrees is null || referenceJointsDegrees.Count == 0)
        {
            return 0.0;
        }

        var jointCount = Math.Min(referenceJointsDegrees.Count, sampleJointsDegrees.Count);
        var score = 0.0;
        for (var index = 0; index < jointCount; index++)
        {
            var delta = sampleJointsDegrees[index] - referenceJointsDegrees[index];
            score += delta * delta;
        }

        return score;
    }
}

/// <summary>
/// 表示一个理论触发事件最终绑定到哪个 J519 sample 的结果。
/// </summary>
public sealed record TriggerSampleBinding(
    TrajectoryDoEvent Trigger,
    J519SendSample Sample,
    int SampleIndex,
    bool FoundInWindow);