using Flyshot.Core.Planning.Sampling;

namespace Flyshot.Core.Tests;

/// <summary>
/// 验证 J519 实发重采样器在离线导出和运行时下发之间保持一致的时间轴语义。
/// </summary>
public sealed class J519SendTrajectorySamplerTests
{
    /// <summary>
    /// 验证 speed_ratio 只缩放轨迹时间，物理发送时间仍按固定伺服周期推进。
    /// </summary>
    [Fact]
    public void SampleDenseJointTrajectory_MapsSendTimeToScaledTrajectoryTimeAndDegrees()
    {
        var denseTrajectory = new[]
        {
            new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
        };

        var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
            denseTrajectory,
            durationSeconds: 0.016,
            servoPeriodSeconds: 0.008,
            speedRatio: 0.5);

        Assert.Equal(5, samples.Count);
        Assert.Equal(0, samples[0].SampleIndex);
        Assert.Equal(0.0, samples[0].SendTime, precision: 6);
        Assert.Equal(0.0, samples[0].TrajectoryTime, precision: 6);
        Assert.Equal(0.0, samples[0].JointsDegrees[0], precision: 6);

        Assert.Equal(1, samples[1].SampleIndex);
        Assert.Equal(0.008, samples[1].SendTime, precision: 6);
        Assert.Equal(0.004, samples[1].TrajectoryTime, precision: 6);
        Assert.Equal(45.0, samples[1].JointsDegrees[0], precision: 6);

        Assert.Equal(4, samples[^1].SampleIndex);
        Assert.Equal(0.032, samples[^1].SendTime, precision: 6);
        Assert.Equal(0.016, samples[^1].TrajectoryTime, precision: 6);
        Assert.Equal(180.0, samples[^1].JointsDegrees[0], precision: 6);
    }

    /// <summary>
    /// 验证空稠密轨迹会直接暴露为调用错误，避免生成无意义下发点。
    /// </summary>
    [Fact]
    public void SampleDenseJointTrajectory_RejectsEmptyDenseTrajectory()
    {
        var exception = Assert.Throws<InvalidOperationException>(() =>
            J519SendTrajectorySampler.SampleDenseJointTrajectory(
                Array.Empty<IReadOnlyList<double>>(),
                durationSeconds: 0.016,
                servoPeriodSeconds: 0.008,
                speedRatio: 1.0));

        Assert.Contains("稠密关节轨迹为空", exception.Message);
    }

    /// <summary>
    /// 验证非法 speed_ratio 会在公共入口统一拦截。
    /// </summary>
    [Theory]
    [InlineData(0.0)]
    [InlineData(double.NaN)]
    [InlineData(double.PositiveInfinity)]
    public void SampleDenseJointTrajectory_RejectsInvalidSpeedRatio(double speedRatio)
    {
        var denseTrajectory = new[]
        {
            new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
        };

        Assert.Throws<ArgumentOutOfRangeException>(() =>
            J519SendTrajectorySampler.SampleDenseJointTrajectory(
                denseTrajectory,
                durationSeconds: 0.0,
                servoPeriodSeconds: 0.008,
                speedRatio: speedRatio));
    }

    /// <summary>
    /// 验证公共诊断行格式与既有 ActualSendTiming 文件保持一致。
    /// </summary>
    [Fact]
    public void BuildTimingRow_UsesLegacyActualSendColumnOrder()
    {
        var row = J519SendTrajectorySampler.BuildTimingRow(new J519SendSample(
            sampleIndex: 2,
            sendTime: 0.016,
            trajectoryTime: 0.008,
            speedRatio: 0.5,
            jointsDegrees: [90.0, 0.0, 0.0, 0.0, 0.0, 0.0]));

        Assert.Equal([2.0, 0.016, 0.008, 0.5], row);
    }
}