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>
/// <remarks>
/// 本构建器只负责“时间轴映射”和“事件打包”，不负责轨迹规划与运行时发送。
/// 其中：
/// - ShotEvent 用于诊断、导出和日志观察；
/// - TrajectoryDoEvent 用于运行时伺服链路注入 DO 脉冲。
/// </remarks>
public sealed class ShotTimelineBuilder
{
    /// <summary>
    /// 将原始示教点索引映射到规划后时间轴上的时间戳解析器。
    /// </summary>
    private readonly WaypointTimestampResolver _resolver;

    /// <summary>
    /// 可选日志记录器，用于输出时间轴构建统计信息。
    /// </summary>
    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>
    /// <exception cref="ArgumentNullException">当 <paramref name="trajectory"/> 为 null 时抛出。</exception>
    /// <exception cref="ArgumentOutOfRangeException">
    /// 当 <paramref name="holdCycles"/> 小于 0，或 <paramref name="samplePeriod"/> 小于等于 0 时抛出。
    /// </exception>
    /// <remarks>
    /// 时间计算规则：
    /// - 理论触发时间：timestamp[i] + offset_values[i] * trigger_period；
    /// - 离散采样索引：Round(trigger_time / sample_period)；
    /// - 离散采样时间：sample_index * sample_period。
    /// 该离散化与运行时 8ms 周期下发模型一致，可用于对齐导出与诊断。
    /// </remarks>
    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>();

        // 遍历原始示教点，仅对 shot_flag=true 的点生成触发事件。
        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,
                    referenceJointsDegrees: program.Waypoints[i].Positions.Select(static radians => radians * 180.0 / Math.PI)));
            }
        }

        _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);
    }
}
