FlyShotHost/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs

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

namespace Flyshot.Core.Triggering;

/// <summary>
/// 根据规划轨迹和飞拍配置生成触发时间轴，把示教点上的 shot_flags / offset_values / addr
/// 映射成带理论时间和离散化时间的 ShotEvent，以及可直接注入伺服流的 TrajectoryDoEvent。
/// </summary>
public sealed class ShotTimelineBuilder
{
    private readonly WaypointTimestampResolver _resolver;
    private readonly ILogger<ShotTimelineBuilder>? _logger;

    /// <summary>
    /// 初始化 ShotTimelineBuilder，依赖一个时间戳解析器来对齐补中点后的轨迹与原始示教点。
    /// </summary>
    /// <param name="resolver">时间戳解析器。</param>
    /// <param name="logger">日志记录器；允许 null。</param>
    public ShotTimelineBuilder(WaypointTimestampResolver resolver, ILogger<ShotTimelineBuilder>? logger = null)
    {
        _resolver = resolver ?? throw new ArgumentNullException(nameof(resolver));
        _logger = logger;
    }

    /// <summary>
    /// 为给定轨迹构建完整的触发时间轴。
    /// </summary>
    /// <param name="trajectory">规划后的轨迹（含补中点信息和机器人配置）。</param>
    /// <param name="holdCycles">IO 保持周期数（对应原系统的 io_keep_cycles）。</param>
    /// <param name="samplePeriod">稠密采样周期，用于离散化 sample_index 和 sample_time。</param>
    /// <param name="useDo">是否生成可注入伺服流的 DO 事件。</param>
    /// <returns>包含 ShotEvent 和 TrajectoryDoEvent 的触发时间轴。</returns>
    public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod, bool useDo = true)
    {
        ArgumentNullException.ThrowIfNull(trajectory);

        if (holdCycles < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(holdCycles), "IO 保持周期数不能为负数。");
        }

        if (samplePeriod <= TimeSpan.Zero)
        {
            throw new ArgumentOutOfRangeException(nameof(samplePeriod), "采样周期必须大于零。");
        }

        var timestamps = _resolver.Resolve(trajectory);
        var program = trajectory.OriginalProgram;
        var robot = trajectory.Robot;
        double triggerPeriodSeconds = robot.TriggerPeriod.TotalSeconds;
        double samplePeriodSeconds = samplePeriod.TotalSeconds;

        var shotEvents = new List<ShotEvent>();
        var triggerTimeline = new List<TrajectoryDoEvent>();

        for (int i = 0; i < program.Waypoints.Count; i++)
        {
            if (!program.ShotFlags[i])
            {
                continue;
            }

            double triggerTime = timestamps[i] + program.OffsetValues[i] * triggerPeriodSeconds;
            int sampleIndex = (int)Math.Round(triggerTime / samplePeriodSeconds);
            double sampleTime = sampleIndex * samplePeriodSeconds;

            var addressGroup = program.AddressGroups[i];

            shotEvents.Add(new ShotEvent(
                waypointIndex: i,
                triggerTime: triggerTime,
                sampleIndex: sampleIndex,
                sampleTime: sampleTime,
                addressGroup: addressGroup));

            if (useDo)
            {
                // use_do=false 时保留 ShotEvent 诊断信息，但不向运行时下发 IO 脉冲。
                triggerTimeline.Add(new TrajectoryDoEvent(
                    waypointIndex: i,
                    triggerTime: triggerTime,
                    offsetCycles: program.OffsetValues[i],
                    holdCycles: holdCycles,
                    addressGroup: addressGroup));
            }
        }

        _logger?.LogInformation(
            "ShotTimeline 构建完成: shotFlags总数={ShotFlagCount}, 触发事件数={TriggerCount}, useDo={UseDo}, holdCycles={HoldCycles}",
            program.ShotFlags.Count(static f => f),
            triggerTimeline.Count,
            useDo,
            holdCycles);

        return new ShotTimeline(shotEvents, triggerTimeline);
    }
}