using Flyshot.Core.Domain;
using Flyshot.ControllerClientCompat;
using Flyshot.Core.Config;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Runtime.Fanuc;
using Flyshot.Runtime.Fanuc.Protocol;
using System.Buffers.Binary;
using System.Net;
using System.Net.Sockets;
using System.Reflection;

namespace Flyshot.Core.Tests;

/// <summary>
/// 验证 FANUC 控制器运行在稠密轨迹流式执行与 IO 触发上的行为。
/// </summary>
public sealed class FanucControllerRuntimeDenseTests
{
    private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
    private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
    private const double SmoothPtpJerkShapeCoefficient = 52.5;

    /// <summary>
    /// 验证真机 J519 会直接消费已经按 8ms 生成好的稠密轨迹，不再按运行时 speedRatio 临场重采样。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_WithDenseWaypoints_RealMode_UsesPlannedDenseRowsAndConvertsRadiansToDegrees()
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var robot = TestRobotFactory.CreateRobotProfile();
        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
        j519Client.EnableCommandHistoryForTests();
        ForceRealModeEnabled(runtime, speedRatio: 0.5);

        var denseTrajectory = new[]
        {
            new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
        };

        var result = new TrajectoryResult(
            programName: "demo",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(0.016),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 4,
            plannedWaypointCount: 4,
            denseJointTrajectory: denseTrajectory);

        runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]);
        WaitUntilIdle(runtime);

        var commands = j519Client.GetCommandHistoryForTests();
        Assert.Equal(3, commands.Count);
        Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
        var expectedJ1Targets = new[] { 0.0, 0.08, 0.16 };
        var actualJ1Targets = commands.Select(static command => command.TargetJoints[0]).ToArray();
        Assert.Equal(expectedJ1Targets.Length, actualJ1Targets.Length);
        for (var index = 0; index < expectedJ1Targets.Length; index++)
        {
            Assert.Equal(expectedJ1Targets[index], actualJ1Targets[index], precision: 6);
        }
        Assert.False(j519Client.IsCommandQueueDrainedForTests());
    }

    /// <summary>
    /// 验证真机稠密轨迹接口会等待 J519 队列被状态包实际取完后再返回。
    /// </summary>
    [Fact]
    public async Task ExecuteTrajectory_WithDenseWaypoints_RealMode_WaitsForJ519QueueToDrainBeforeReturning()
    {
        using var server = new UdpClient(0);
        using var cts = new CancellationTokenSource(TimeSpan.FromSeconds(5));
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var robot = TestRobotFactory.CreateRobotProfile();
        var port = ((IPEndPoint)server.Client.LocalEndPoint!).Port;
        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
        j519Client.EnableCommandHistoryForTests();
        await j519Client.ConnectAsync("127.0.0.1", port, cts.Token);
        var initResult = await server.ReceiveAsync(cts.Token);
        j519Client.StartMotion();
        ForceRealModeEnabled(runtime, speedRatio: 1.0);

        var denseTrajectory = new[]
        {
            new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
        };
        var result = new TrajectoryResult(
            programName: "wait-drain",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(0.016),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 4,
            plannedWaypointCount: 4,
            denseJointTrajectory: denseTrajectory);

        var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token);
        await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 3, cts.Token);

        // 只有机器人状态包把队列全部取出后，ExecuteTrajectory 才能向上层返回。
        Assert.False(executeTask.IsCompleted);

        for (uint sequence = 100; sequence < 103; sequence++)
        {
            await SendStatusPacketAsync(server, initResult.RemoteEndPoint, sequence, cts.Token);
            _ = await server.ReceiveAsync(cts.Token);
        }

        await executeTask.WaitAsync(TimeSpan.FromSeconds(2), cts.Token);
        Assert.True(j519Client.IsCommandQueueDrainedForTests());
        Assert.False(runtime.GetSnapshot().IsInMotion);
    }

    /// <summary>
    /// 验证真机稠密发送会把实际下发给机器人的整条点位与点间跃度按时间目录落盘。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_WithDenseWaypoints_RealMode_WritesActualSentPointsAndJerkStats()
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var robot = TestRobotFactory.CreateRobotProfile();
        var programName = $"dense-send-{Guid.NewGuid():N}";
        var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", programName);

        try
        {
            runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
            j519Client.EnableCommandHistoryForTests();
            ForceRealModeEnabled(runtime, speedRatio: 0.5);

            var denseTrajectory = new[]
            {
                new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
                new[] { 0.008, DegreesToRadians(1.0), 0.0, 0.0, 0.0, 0.0, 0.0 },
                new[] { 0.016, DegreesToRadians(2.0), 0.0, 0.0, 0.0, 0.0, 0.0 }
            };

            var result = new TrajectoryResult(
                programName: programName,
                method: PlanningMethod.Icsp,
                isValid: true,
                duration: TimeSpan.FromSeconds(0.016),
                shotEvents: Array.Empty<ShotEvent>(),
                triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
                artifacts: Array.Empty<TrajectoryArtifact>(),
                failureReason: null,
                usedCache: false,
                originalWaypointCount: 4,
                plannedWaypointCount: 4,
                denseJointTrajectory: denseTrajectory);

            runtime.ExecuteTrajectory(result, [DegreesToRadians(2.0), 0.0, 0.0, 0.0, 0.0, 0.0]);
            WaitUntilIdle(runtime);

            var commands = j519Client.GetCommandHistoryForTests();
            var runDir = GetSingleDenseSendRunDirectory(outputRoot);
            var pointsPath = Path.Combine(runDir, "ActualSendJointTraj.txt");
            var timingPath = Path.Combine(runDir, "ActualSendTiming.txt");
            var jerkPath = Path.Combine(runDir, "ActualSendJerkStats.txt");
            var shotEventsPath = Path.Combine(runDir, "ShotEvents.json");

            Assert.True(File.Exists(pointsPath));
            Assert.True(File.Exists(timingPath));
            Assert.True(File.Exists(jerkPath));
            Assert.True(File.Exists(shotEventsPath));

            var pointLines = File.ReadAllLines(pointsPath);
            var timingLines = File.ReadAllLines(timingPath);
            var jerkLines = File.ReadAllLines(jerkPath);
            var shotEventsJson = File.ReadAllText(shotEventsPath);
            Assert.Equal(commands.Count, pointLines.Length);
            Assert.Equal(commands.Count, timingLines.Length);
            Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length);

            var firstColumns = ParseColumns(pointLines[0]);
            var secondColumns = ParseColumns(pointLines[1]);
            var lastColumns = ParseColumns(pointLines[^1]);
            Assert.Equal(9, firstColumns.Length);
            Assert.Equal(9, lastColumns.Length);
            Assert.Equal(0.0, firstColumns[0], precision: 6);
            Assert.Equal(0.008, secondColumns[0], precision: 6);
            Assert.Equal(2.0, lastColumns[1], precision: 6);

            var firstTimingColumns = ParseColumns(timingLines[0]);
            var secondTimingColumns = ParseColumns(timingLines[1]);
            var lastTimingColumns = ParseColumns(timingLines[^1]);
            Assert.Equal(6, firstTimingColumns.Length);
            Assert.Equal(0.0, firstTimingColumns[0], precision: 6);
            Assert.Equal(0.0, firstTimingColumns[1], precision: 6);
            Assert.Equal(0.0, firstTimingColumns[2], precision: 6);
            Assert.Equal(1.0, firstTimingColumns[3], precision: 6);
            Assert.Equal(0.5, firstTimingColumns[4], precision: 6);
            Assert.Equal(0.0, firstTimingColumns[5], precision: 6);
            Assert.Equal(1.0, secondTimingColumns[0], precision: 6);
            Assert.Equal(0.008, secondTimingColumns[1], precision: 6);
            Assert.Equal(0.008, secondTimingColumns[2], precision: 6);
            Assert.Equal(commands.Count - 1, lastTimingColumns[0], precision: 6);
            Assert.Equal(0.016, lastTimingColumns[2], precision: 6);

            var firstJerkColumns = ParseColumns(jerkLines[0]);
            Assert.Equal(10, firstJerkColumns.Length);
            Assert.Equal(0.0, firstJerkColumns[0], precision: 6);
            Assert.Equal(0.008, firstJerkColumns[2], precision: 6);

            Assert.Equal("[]", shotEventsJson.Trim());
        }
        finally
        {
            if (Directory.Exists(outputRoot))
            {
                Directory.Delete(outputRoot, recursive: true);
            }
        }
    }

    /// <summary>
    /// 使用运行时 RobotConfig.json 中的真实 UTTC_MS11 轨迹执行一次完整 1x 稠密下发，
    /// 并把 0.088s 报警窗口附近的实发时间、关节与跃度摘要落盘，便于继续对照现场报警帧。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_UttcMs11FromHostRuntimeConfig_RealMode_WritesDenseSendDebugWindowAtOneX()
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var fixture = LoadUttcMs11RuntimeFixture();
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var bundle = orchestrator.PlanUploadedFlyshot(
            fixture.Robot,
            fixture.Uploaded,
            settings: fixture.Settings);
        var result = WithUniqueProgramName(bundle.Result, $"UTTC_MS11_legacyfit_{Guid.NewGuid():N}");
        var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", result.ProgramName);
        var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
        var beforeRunDirectories = Directory.Exists(denseSendRoot)
            ? Directory.GetDirectories(denseSendRoot).ToHashSet(StringComparer.OrdinalIgnoreCase)
            : new HashSet<string>(StringComparer.OrdinalIgnoreCase);

        runtime.ResetRobot(fixture.Robot, fixture.Robot.Name);
        j519Client.EnableCommandHistoryForTests();
        ForceRealModeEnabled(runtime, speedRatio: 1.0);

        runtime.ExecuteTrajectory(result, result.DenseJointTrajectory![0].Skip(1).ToArray());
        WaitUntilIdle(runtime);

        var runDirectory = GetNewDenseSendRunDirectory(outputRoot, beforeRunDirectories);
        var pointsPath = Path.Combine(runDirectory, "ActualSendJointTraj.txt");
        var timingPath = Path.Combine(runDirectory, "ActualSendTiming.txt");
        var jerkPath = Path.Combine(runDirectory, "ActualSendJerkStats.txt");
        var summaryPath = Path.Combine(runDirectory, "AlarmWindow_0p088s_Summary.txt");

        Assert.True(File.Exists(pointsPath));
        Assert.True(File.Exists(timingPath));
        Assert.True(File.Exists(jerkPath));

        var pointsLines = File.ReadAllLines(pointsPath);
        var timingLines = File.ReadAllLines(timingPath);
        var jerkLines = File.ReadAllLines(jerkPath);

        Assert.NotEmpty(pointsLines);
        Assert.NotEmpty(timingLines);
        Assert.NotEmpty(jerkLines);
        Assert.Equal(result.DenseJointTrajectory!.Count, pointsLines.Length);
        Assert.Equal(result.DenseJointTrajectory.Count, timingLines.Length);

        var firstPoint = ParseColumns(pointsLines[0]);
        var secondPoint = ParseColumns(pointsLines[1]);
        Assert.Equal(0.0, firstPoint[0], precision: 6);
        Assert.Equal(0.008, secondPoint[0], precision: 6);

        var firstStepJ1 = Math.Abs(secondPoint[1] - firstPoint[1]);
        Assert.True(firstStepJ1 > 1e-6, $"UTTC_MS11 实发首步不应被压成 0，actual={firstStepJ1:F9}deg");

        const double targetSendTime = 0.088;
        const double windowHalfWidth = 0.024;
        var summaryLines = new List<string>
        {
            $"program={result.ProgramName}",
            $"send_time_target_seconds={targetSendTime:F6}",
            $"window_half_width_seconds={windowHalfWidth:F6}",
            $"points_path={pointsPath}",
            $"timing_path={timingPath}",
            $"jerk_path={jerkPath}",
            "timing_window:"
        };

        foreach (var line in timingLines.Select(ParseColumns))
        {
            var sendTime = line[1];
            if (Math.Abs(sendTime - targetSendTime) <= windowHalfWidth + 1e-9)
            {
                summaryLines.Add(
                    $"timing send_index={line[0]:F0} send_time={line[1]:F6} trajectory_time={line[2]:F6} speed_ratio={line[3]:F6}");
            }
        }

        summaryLines.Add("jerk_window:");
        foreach (var line in jerkLines.Select(ParseColumns))
        {
            var endTime = line[1];
            if (Math.Abs(endTime - targetSendTime) <= windowHalfWidth + 1e-9)
            {
                summaryLines.Add(
                    $"jerk end_time={line[1]:F6} dt={line[2]:F6} j1={line[3]:F6} j2={line[4]:F6} j3={line[5]:F6} j4={line[6]:F6} j5={line[7]:F6} j6={line[8]:F6} max_abs={line.Skip(3).Max(static value => Math.Abs(value)):F6}");
            }
        }

        File.WriteAllLines(summaryPath, summaryLines);
        Assert.True(File.Exists(summaryPath));
    }

    /// <summary>
    /// 验证真实 UTTC_MS11 轨迹在非 1 倍 speedRatio 下仍能生成并装载 J519 实发队列。
    /// </summary>
    [Theory]
    [InlineData(0.7)]
    [InlineData(0.5)]
    public void ExecuteTrajectory_UttcMs11FromHostRuntimeConfig_RealMode_AllowsNonOneSpeedRatio(double speedRatio)
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var fixture = LoadUttcMs11RuntimeFixture();
        var fullSpeedSettings = new CompatibilityRobotSettings(
            useDo: fixture.Settings.UseDo,
            ioAddresses: fixture.Settings.IoAddresses,
            ioKeepCycles: fixture.Settings.IoKeepCycles,
            triggerSampleIndexOffsetCycles: fixture.Settings.TriggerSampleIndexOffsetCycles,
            accLimitScale: fixture.Settings.AccLimitScale,
            jerkLimitScale: fixture.Settings.JerkLimitScale,
            adaptIcspTryNum: fixture.Settings.AdaptIcspTryNum,
            planningSpeedScale: 1.0,
            smoothStartStopTiming: true);
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var bundle = orchestrator.PlanUploadedFlyshot(
            fixture.Robot,
            fixture.Uploaded,
            settings: fullSpeedSettings,
            planningSpeedScale: fullSpeedSettings.PlanningSpeedScale);
        var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build(
            fixture.Robot,
            bundle.ExecutionTrajectory,
            bundle.Result,
            fixture.Robot.ServoPeriod.TotalSeconds,
            speedRatio);
        var result = WithUniqueProgramName(new TrajectoryResult(
            programName: bundle.Result.ProgramName,
            method: bundle.Result.Method,
            isValid: bundle.Result.IsValid,
            duration: bundle.Result.Duration,
            shotEvents: bundle.Result.ShotEvents,
            triggerTimeline: bundle.Result.TriggerTimeline,
            artifacts: bundle.Result.Artifacts,
            failureReason: bundle.Result.FailureReason,
            usedCache: bundle.Result.UsedCache,
            originalWaypointCount: bundle.Result.OriginalWaypointCount,
            plannedWaypointCount: bundle.Result.PlannedWaypointCount,
            triggerSampleIndexOffsetCycles: bundle.Result.TriggerSampleIndexOffsetCycles,
            denseJointTrajectory: bundle.Result.DenseJointTrajectory,
            preparedFlyshotExecution: preparedExecution), $"UTTC_MS11_speedratio_{speedRatio:F1}_{Guid.NewGuid():N}");
        var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", result.ProgramName);

        try
        {
            runtime.ResetRobot(fixture.Robot, fixture.Robot.Name);
            j519Client.EnableCommandHistoryForTests();
            ForceRealModeEnabled(runtime, speedRatio);

            runtime.ExecuteTrajectory(result, result.DenseJointTrajectory![^1].Skip(1).ToArray());
            WaitUntilIdle(runtime);

            var commands = j519Client.GetCommandHistoryForTests();
            Assert.NotEmpty(commands);
            Assert.Equal(preparedExecution.Samples.Count, commands.Count);
            Assert.Equal(speedRatio, preparedExecution.FinalSpeedRatio, precision: 6);
            Assert.Equal(0, preparedExecution.StretchIterationCount);
            Assert.Equal(preparedExecution.Samples[0].JointsDegrees, commands[0].TargetJoints);
        }
        finally
        {
            if (Directory.Exists(outputRoot))
            {
                Directory.Delete(outputRoot, recursive: true);
            }
        }
    }

    /// <summary>
    /// 验证运行时拿到飞拍预生成发送队列后，会直接消费该队列，而不是再按当前 speedRatio 临场重采样。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_WithPreparedFlyshotExecution_RealMode_UsesPreparedSamplesDirectly()
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var robot = TestRobotFactory.CreateRobotProfile();
        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
        j519Client.EnableCommandHistoryForTests();
        ForceRealModeEnabled(runtime, speedRatio: 0.5);

        var preparedSamples = new[]
        {
            new FlyshotPreparedSample(0, 0.0, 0.0, 1.0, [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
            new FlyshotPreparedSample(1, 0.008, 0.008, 1.0, [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
            new FlyshotPreparedSample(2, 0.016, 0.016, 1.0, [3.0, 0.0, 0.0, 0.0, 0.0, 0.0])
        };
        var preparedExecution = new FlyshotPreparedExecution(
            samples: preparedSamples,
            triggerBindings: Array.Empty<FlyshotPreparedTriggerBinding>(),
            timingRows: preparedSamples.Select(static sample => (IReadOnlyList<double>)
            [
                sample.SampleIndex,
                Math.Round(sample.SendTime, 6),
                Math.Round(sample.TrajectoryTime, 6),
                Math.Round(sample.SpeedRatio, 6)
            ]).ToArray(),
            jerkRows: Array.Empty<IReadOnlyList<double>>(),
            requestSpeedRatio: 1.0,
            finalSpeedRatio: 1.0,
            finalDurationSeconds: 0.016,
            stretchIterationCount: 0);
        var result = new TrajectoryResult(
            programName: "prepared-demo",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(0.016),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 4,
            plannedWaypointCount: 4,
            denseJointTrajectory:
            [
                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 }
            ],
            preparedFlyshotExecution: preparedExecution);

        runtime.ExecuteTrajectory(result, [DegreesToRadians(3.0), 0.0, 0.0, 0.0, 0.0, 0.0]);
        WaitUntilIdle(runtime);

        var commands = j519Client.GetCommandHistoryForTests();
        Assert.Equal(preparedSamples.Length, commands.Count);
        Assert.Equal([1.0, 2.0, 3.0], commands.Select(static command => command.TargetJoints[0]));
    }

    /// <summary>
    /// 验证 MoveJoint 会在生成阶段折算 speed_ratio，运行时只发送已经规划好的 8ms 稠密点列。
    /// </summary>
    [Theory]
    [InlineData(1.0)]
    [InlineData(0.7)]
    [InlineData(0.5)]
    public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryWithSpeedRatioAlreadyPlanned(double speedRatio)
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var service = CreateCompatService(runtime);
        var startJoints = new[]
        {
            1.056731,
            0.011664811,
            -0.017892333,
            -0.01516874,
            0.021492079,
            0.009567846
        };
        var targetJoints = new[]
        {
            0.8532358,
            0.03837953,
            -0.19235304,
            0.0071595116,
            0.109054826,
            0.040055145
        };

        service.SetUpRobot("FANUC_LR_Mate_200iD");
        j519Client.EnableCommandHistoryForTests();
        ForceRealModeEnabled(runtime, speedRatio);
        SetPrivateField(runtime, "_jointPositions", startJoints);

        var options = new ControllerClientCompatOptions
        {
            ConfigRoot = TestRobotFactory.GetConfigRoot()
        };
        var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
            .LoadProfile("FANUC_LR_Mate_200iD", accLimitScale: 1.0, jerkLimitScale: 1.0);
        var expectedResult = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio);

        service.MoveJoint(targetJoints);
        WaitUntilIdle(runtime);

        var commands = j519Client.GetCommandHistoryForTests();
        Assert.Equal(expectedResult.DenseJointTrajectory!.Count, commands.Count);
        AssertJointDegreesEqual(startJoints, commands[0].TargetJoints);
        AssertJointDegreesEqual(targetJoints, commands[^1].TargetJoints);

        var middleAlpha = ComputeLineAlpha(commands[commands.Count / 2].TargetJoints, startJoints, targetJoints);
        Assert.InRange(middleAlpha, 0.45, 0.55);

        var earlyAlpha = ComputeLineAlpha(commands[Math.Min(5, commands.Count - 1)].TargetJoints, startJoints, targetJoints);
        Assert.InRange(earlyAlpha, 0.0, 0.02);
    }

    [Fact]
    public void MoveJointTrajectoryGenerator_LowerSpeedUsesFixedServoPeriodAndLongerPlannedDuration()
    {
        var robot = CreateMoveJointReferenceRobotProfile();
        var startJoints = new[] { 1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846 };
        var targetJoints = new[] { 0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145 };

        var fullSpeed = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
        var speed07 = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
        var speed05 = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.5);

        Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
        Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
        AssertDenseRowsUseServoPeriod(fullSpeed.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
        AssertDenseRowsUseServoPeriod(speed07.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
        AssertDenseRowsUseServoPeriod(speed05.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
        Assert.True(fullSpeed.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 1.0));
        Assert.True(speed07.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.7));
        Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.5));
    }

    /// <summary>
    /// 验证 MovePose 低速倍率仍保持固定伺服周期，并通过拉长时长降低直角运动速度。
    /// </summary>
    [Fact]
    public void MovePoseTrajectoryGenerator_LowerSpeedUsesFixedServoPeriodAndLongerPlannedDuration()
    {
        var servoPeriod = TimeSpan.FromMilliseconds(8);
        var startPose = new[] { 100.0, 200.0, 300.0, 1.0, 2.0, 3.0 };
        var targetPose = new[] { 140.0, 260.0, 330.0, 8.0, 10.0, 12.0 };

        var fullSpeed = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 1.0);
        var speed07 = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 0.7);
        var speed05 = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 0.5);

        Assert.True(speed07.DenseCartesianTrajectory!.Count > fullSpeed.DenseCartesianTrajectory!.Count);
        Assert.True(speed05.DenseCartesianTrajectory!.Count > speed07.DenseCartesianTrajectory!.Count);
        AssertDenseRowsUseServoPeriod(fullSpeed.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
        AssertDenseRowsUseServoPeriod(speed07.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
        AssertDenseRowsUseServoPeriod(speed05.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
        AssertPoseEqual(startPose, fullSpeed.DenseCartesianTrajectory[0].Skip(1).ToArray());
        AssertPoseEqual(targetPose, fullSpeed.DenseCartesianTrajectory[^1].Skip(1).ToArray());
        Assert.True(MovePoseTrajectoryGenerator.SatisfiesDefaultCartesianLimits(speed05.DenseCartesianTrajectory, speedRatio: 0.5));
    }

    [Fact]
    public void MoveJoint_RealMode_LeavesFinalTargetForHoldStreaming()
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var service = CreateCompatService(runtime);
        var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
        var targetJoints = new[] { 0.2, -0.1, 0.05, 0.0, 0.0, 0.1 };

        service.SetUpRobot("FANUC_LR_Mate_200iD");
        ForceRealModeEnabled(runtime, speedRatio: 1.0);
        SetPrivateField(runtime, "_jointPositions", startJoints);

        service.MoveJoint(targetJoints);
        WaitUntilIdle(runtime);

        var currentCommand = j519Client.GetCurrentCommand();
        Assert.NotNull(currentCommand);
        AssertJointDegreesEqual(targetJoints, currentCommand.TargetJoints);
    }

    /// <summary>
    /// 验证 MovePose 会生成直角坐标 J519 队列，并使用 Data format=0 下发 X/Y/Z/W/P/R。
    /// </summary>
    [Fact]
    public void MovePose_RealMode_GeneratesCartesianJ519Queue()
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var service = CreateCompatService(runtime);
        var startPose = new[] { 100.0, 200.0, 300.0, 1.0, 2.0, 3.0 };
        var targetPose = new[] { 110.0, 220.0, 315.0, 4.0, 5.0, 6.0 };

        service.SetUpRobot("FANUC_LR_Mate_200iD");
        j519Client.EnableCommandHistoryForTests();
        ForceRealModeEnabled(runtime, speedRatio: 1.0);
        SetPrivateField(runtime, "_pose", startPose);

        service.MovePose(targetPose);
        WaitUntilIdle(runtime);

        var commands = j519Client.GetCommandHistoryForTests();
        Assert.NotEmpty(commands);
        Assert.All(commands, static command => Assert.Equal(0, command.DataStyle));
        AssertPoseEqual(startPose, commands[0].TargetValues.Take(6).ToArray());
        AssertPoseEqual(targetPose, commands[^1].TargetValues.Take(6).ToArray());
        Assert.All(commands, static command =>
        {
            Assert.Equal(0.0, command.TargetValues[6], precision: 6);
            Assert.Equal(0.0, command.TargetValues[7], precision: 6);
            Assert.Equal(0.0, command.TargetValues[8], precision: 6);
        });
    }

    /// <summary>
    /// 验证运行时稠密发送不再依赖当前 speed_ratio；倍率合法性应在上游规划/生成阶段处理。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_WithDenseWaypoints_RealMode_IgnoresRuntimeSpeedRatioForDenseFallback()
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var robot = TestRobotFactory.CreateRobotProfile();
        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
        j519Client.EnableCommandHistoryForTests();
        ForceRealModeEnabled(runtime, speedRatio: 0.0);

        var denseTrajectory = new[]
        {
            new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
        };

        var result = new TrajectoryResult(
            programName: "demo",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(0.016),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 4,
            plannedWaypointCount: 4,
            denseJointTrajectory: denseTrajectory);

        runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]);
        WaitUntilIdle(runtime);

        var commands = j519Client.GetCommandHistoryForTests();
        Assert.Equal(3, commands.Count);
        Assert.Equal(0.16, commands[^1].TargetJoints[0], precision: 6);
    }

    /// <summary>
    /// 验证真机模式下若 J519 响应明确显示伺服侧未就绪，则拒绝启动稠密轨迹发送。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsNotReadyJ519Status()
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var robot = TestRobotFactory.CreateRobotProfile();
        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
        ForceRealModeEnabled(runtime, speedRatio: 1.0);
        SetLatestJ519Response(j519Client, status: 0b0011);

        var result = CreateDenseResult(
            [
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0]
            ],
            durationSeconds: 0.008);

        var exception = Assert.Throws<InvalidOperationException>(
            () => runtime.ExecuteTrajectory(result, [Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));

        Assert.Contains("J519 status is not ready", exception.Message);
        Assert.Contains("sysrdy=False", exception.Message);
    }

    /// <summary>
    /// 验证控制器快照暴露最近一次 J519 响应中的四个状态位，便于状态页和诊断接口显示。
    /// </summary>
    [Fact]
    public void GetSnapshot_RealMode_IncludesLatestJ519StatusBits()
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var robot = TestRobotFactory.CreateRobotProfile();
        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
        ForceRealModeEnabled(runtime, speedRatio: 1.0);
        SetLatestJ519Response(j519Client, status: 0b0111);

        var snapshot = runtime.GetSnapshot();

        Assert.Equal((byte)0b0111, snapshot.J519Status);
        Assert.Equal(10u, snapshot.J519Sequence);
        Assert.True(snapshot.J519AcceptsCommand);
        Assert.True(snapshot.J519ReceivedCommand);
        Assert.True(snapshot.J519SystemReady);
        Assert.False(snapshot.J519RobotInMotion);
    }

    /// <summary>
    /// 验证飞拍 IO 脉冲按轨迹时间轴嵌入 J519 命令，并在保持周期后用同一 mask 清零。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_WithDenseWaypoints_RealMode_EmbedsIoPulseOnTrajectoryTimeline()
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var robot = TestRobotFactory.CreateRobotProfile();
        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
        j519Client.EnableCommandHistoryForTests();
        ForceRealModeEnabled(runtime, speedRatio: 1.0);

        var denseTrajectory = new[]
        {
            new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.024, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
        };

        var result = new TrajectoryResult(
            programName: "demo",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(0.024),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline:
            [
                new TrajectoryDoEvent(
                    waypointIndex: 1,
                    triggerTime: 0.008,
                    offsetCycles: 0,
                    holdCycles: 2,
                    addressGroup: new IoAddressGroup([1, 3]))
            ],
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 4,
            plannedWaypointCount: 4,
            denseJointTrajectory: denseTrajectory);

        runtime.ExecuteTrajectory(result, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
        WaitUntilIdle(runtime);

        var commands = j519Client.GetCommandHistoryForTests();
        Assert.Equal(4, commands.Count);
        Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)5], commands.Select(static command => command.WriteIoMask));
        Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)0], commands.Select(static command => command.WriteIoValue));
    }

    /// <summary>
    /// 验证同一时间窗口内存在多个候选 sample 时，会优先把 IO 挂到关节坐标最接近示教点的那一帧。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_WithDenseWaypoints_RealMode_PrefersClosestJointSampleWithinTriggerWindow()
    {
        using var commandClient = new FanucCommandClient();
        using var stateClient = new FanucStateClient();
        using var j519Client = new FanucJ519Client();
        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
        var robot = TestRobotFactory.CreateRobotProfile();
        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
        j519Client.EnableCommandHistoryForTests();
        ForceRealModeEnabled(runtime, speedRatio: 1.0);

        var denseTrajectory = new[]
        {
            new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.008, DegreesToRadians(1.0), 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.016, DegreesToRadians(2.0), 0.0, 0.0, 0.0, 0.0, 0.0 },
            new[] { 0.024, DegreesToRadians(3.0), 0.0, 0.0, 0.0, 0.0, 0.0 }
        };

        var result = new TrajectoryResult(
            programName: "demo",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(0.024),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline:
            [
                new TrajectoryDoEvent(
                    waypointIndex: 1,
                    triggerTime: 0.012,
                    offsetCycles: 0,
                    holdCycles: 1,
                    addressGroup: new IoAddressGroup([1]),
                    referenceJointsDegrees: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0])
            ],
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 4,
            plannedWaypointCount: 4,
            denseJointTrajectory: denseTrajectory);

        runtime.ExecuteTrajectory(result, [DegreesToRadians(3.0), 0.0, 0.0, 0.0, 0.0, 0.0]);
        WaitUntilIdle(runtime);

        var commands = j519Client.GetCommandHistoryForTests();
        Assert.Equal(4, commands.Count);
        Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)1], commands.Select(static command => command.WriteIoMask));
        Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)0], commands.Select(static command => command.WriteIoValue));
    }

    /// <summary>
    /// 验证仿真模式下即使传入稠密路点，也会回退到单点同步更新。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_WithDenseWaypoints_SimulationMode_FallsBackToSinglePoint()
    {
        var runtime = new FanucControllerRuntime();
        var robot = TestRobotFactory.CreateRobotProfile();
        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
        runtime.SetActiveController(sim: true);
        runtime.Connect("192.168.10.101");
        runtime.EnableRobot(bufferSize: 2);

        var denseTrajectory = new[]
        {
            new[] { 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 },
            new[] { 0.008, 0.11, 0.21, 0.31, 0.41, 0.51, 0.61 },
            new[] { 0.016, 0.12, 0.22, 0.32, 0.42, 0.52, 0.62 }
        };

        var result = new TrajectoryResult(
            programName: "demo",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(0.016),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 4,
            plannedWaypointCount: 4,
            denseJointTrajectory: denseTrajectory);

        runtime.ExecuteTrajectory(result, [0.12, 0.22, 0.32, 0.42, 0.52, 0.62]);

        var snapshot = runtime.GetSnapshot();
        Assert.False(snapshot.IsInMotion);
        Assert.Equal([0.12, 0.22, 0.32, 0.42, 0.52, 0.62], snapshot.JointPositions);
    }

    /// <summary>
    /// 验证稠密执行前若 J519 首次未就绪，会先尝试一次 EnableRobot，再在 500ms 后复查状态。
    /// </summary>
    [Fact]
    public void EnsureJ519ReadyForDenseExecutionCore_RetriesEnableRobotOnceBeforePassing()
    {
        var responses = new Queue<FanucJ519Response?>(
        [
            CreateJ519Response(status: 0b0001, sequence: 11),
            CreateJ519Response(status: 0b0101, sequence: 12)
        ]);
        var enableRobotRetryCount = 0;
        var waitCount = 0;

        using var runtime = new FanucControllerRuntime();

        var exception = Record.Exception(
            () => runtime.EnsureJ519ReadyForDenseExecutionCore(
                () => responses.Dequeue(),
                () => enableRobotRetryCount++,
                () => waitCount++));

        Assert.Null(exception);
        Assert.Equal(1, enableRobotRetryCount);
        Assert.Equal(1, waitCount);
    }

    /// <summary>
    /// 验证重试 EnableRobot 之后若 J519 仍未就绪，会继续抛出带状态位的异常。
    /// </summary>
    [Fact]
    public void EnsureJ519ReadyForDenseExecutionCore_ThrowsWhenStillNotReadyAfterRetry()
    {
        var responses = new Queue<FanucJ519Response?>(
        [
            CreateJ519Response(status: 0b0000, sequence: 21),
            CreateJ519Response(status: 0b0010, sequence: 22)
        ]);
        var enableRobotRetryCount = 0;
        var waitCount = 0;

        using var runtime = new FanucControllerRuntime();

        var exception = Assert.Throws<InvalidOperationException>(
            () => runtime.EnsureJ519ReadyForDenseExecutionCore(
                () => responses.Dequeue(),
                () => enableRobotRetryCount++,
                () => waitCount++));

        Assert.Equal(1, enableRobotRetryCount);
        Assert.Equal(1, waitCount);
        Assert.Contains("accept_cmd=False", exception.Message, StringComparison.Ordinal);
        Assert.Contains("sysrdy=False", exception.Message, StringComparison.Ordinal);
        Assert.Contains("seq=22", exception.Message, StringComparison.Ordinal);
    }

    /// <summary>
    /// 验证 StopMove 在没有任何后台发送任务运行时不会抛出异常。
    /// </summary>
    [Fact]
    public void StopMove_DoesNotThrowWhenNoSendTaskRunning()
    {
        var runtime = new FanucControllerRuntime();
        var robot = TestRobotFactory.CreateRobotProfile();
        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
        runtime.SetActiveController(sim: true);
        runtime.Connect("192.168.10.101");
        runtime.EnableRobot(bufferSize: 2);

        var exception = Record.Exception(() => runtime.StopMove());
        Assert.Null(exception);
        Assert.False(runtime.GetSnapshot().IsInMotion);
    }

    /// <summary>
    /// 验证 IO 地址组中的地址号按 writeIoIndex=1 的手册语义映射为 writeIoValue 位掩码。
    /// </summary>
    [Theory]
    [InlineData(new[] { 1 }, (ushort)1)]
    [InlineData(new[] { 2, 4 }, (ushort)10)]
    [InlineData(new[] { 3, 4 }, (ushort)12)]
    [InlineData(new[] { 2, 3, 4 }, (ushort)14)]
    [InlineData(new[] { 8 }, (ushort)128)]
    [InlineData(new[] { 8, 9 }, (ushort)384)] // 128 + 256
    [InlineData(new[] { 16 }, (ushort)32768)]
    [InlineData(new int[] { }, (ushort)0)]
    public void ComputeIoValue_MapsAddressesToBitMask(int[] addresses, ushort expected)
    {
        var group = new IoAddressGroup(addresses);
        var actual = FanucControllerRuntime.ComputeIoValue(group);
        Assert.Equal(expected, actual);
    }

    /// <summary>
    /// 验证超出 1..16 范围的地址号会被安全忽略，不会污染位掩码。
    /// </summary>
    [Fact]
    public void ComputeIoValue_IgnoresOutOfRangeAddresses()
    {
        var group = new IoAddressGroup([0, 1, 17, 8]);
        var actual = FanucControllerRuntime.ComputeIoValue(group);
        Assert.Equal((ushort)(1 | 128), actual);
    }

    [Fact]
    public void MoveJointTrajectoryGenerator_GeneratesSmoothJerkLimitedPtpTrajectoryAtSpeedOne()
    {
        var robot = CreateMoveJointReferenceRobotProfile();
        var startJoints = new[]
        {
            DegreesToRadians(60.546227),
            DegreesToRadians(0.668344),
            DegreesToRadians(-1.025155),
            DegreesToRadians(-0.869105),
            DegreesToRadians(1.231405),
            DegreesToRadians(0.548197)
        };
        var targetJoints = new[]
        {
            DegreesToRadians(48.886810),
            DegreesToRadians(2.198985),
            DegreesToRadians(-11.021017),
            DegreesToRadians(0.410210),
            DegreesToRadians(6.248381),
            DegreesToRadians(2.294991)
        };

        var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
        var rows = result.DenseJointTrajectory!;

        Assert.True(rows.Count > 41, $"平滑 MoveJoint 不应再固定输出 41 点，actual={rows.Count}。");
        Assert.True(result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
        AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
        AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
        AssertAllJointJerksWithinLimits(rows, robot);
    }

    /// <summary>
    /// 验证报警样本中的 MoveJoint 预移动作不会因为点到点时间律重采样而产生任一轴跃度尖峰。
    /// </summary>
    [Fact]
    public void MoveJointTrajectoryGenerator_ActualAlarmSampleKeepsJointOneJerkWithinLimit()
    {
        var robot = CreateMoveJointRuntimeLimitRobotProfile();
        var startJoints = new[]
        {
            DegreesToRadians(71.454618),
            DegreesToRadians(0.688433),
            DegreesToRadians(-1.074197),
            DegreesToRadians(-0.869001),
            DegreesToRadians(1.218057),
            DegreesToRadians(0.547036)
        };
        var targetJoints = new[]
        {
            DegreesToRadians(60.546227),
            DegreesToRadians(0.668344),
            DegreesToRadians(-1.025155),
            DegreesToRadians(-0.869105),
            DegreesToRadians(1.231405),
            DegreesToRadians(0.548197)
        };

        var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
        var rows = result.DenseJointTrajectory!;
        AssertAllJointJerksWithinLimits(rows, robot);
    }

    [Fact]
    public void MoveJointTrajectoryGenerator_DoesNotShortenLimitDurationWhenSpeedRatioDoesNotDivideWindow()
    {
        var robot = CreateMoveJointReferenceRobotProfile();
        var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
        var targetJoints = new[] { 0.05, 0.0, 0.0, 0.0, 0.0, 0.0 };

        var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
        var rows = result.DenseJointTrajectory!;

        Assert.True(
            result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.7),
            $"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
        AssertDenseRowsUseServoPeriod(rows, robot.ServoPeriod.TotalSeconds);
        AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
        AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
    }

    [Fact]
    public void MoveJointTrajectoryGenerator_RejectsUnrepresentableSampleCountForTinySpeedRatio()
    {
        var robot = CreateMoveJointReferenceRobotProfile();
        var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
        var targetJoints = new[] { 0.05, 0.0, 0.0, 0.0, 0.0, 0.0 };

        var exception = Assert.Throws<InvalidOperationException>(
            () => MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1e-12));

        Assert.Contains("sample count", exception.Message, StringComparison.OrdinalIgnoreCase);
    }

    [Fact]
    public void MoveJointTrajectoryGenerator_StretchesLongMoveFromJointLimitsInsteadOfKeepingFortyCycles()
    {
        var robot = CreateMoveJointReferenceRobotProfile();
        var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
        var targetJoints = new[] { Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 };

        var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
        var rows = result.DenseJointTrajectory!;
        var expectedVelocityDuration = Math.PI * SmoothPtpVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
        var expectedAccelerationDuration = Math.Sqrt(Math.PI * SmoothPtpAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
        var expectedJerkDuration = Math.Cbrt(Math.PI * SmoothPtpJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
        var expectedMinimumDuration = new[]
        {
            expectedVelocityDuration,
            expectedAccelerationDuration,
            expectedJerkDuration
        }.Max();
        var expectedCountFromDuration = (int)Math.Floor(result.Duration.TotalSeconds / robot.ServoPeriod.TotalSeconds + 1e-9) + 1;

        Assert.True(rows.Count > 41, $"Expected long MoveJoint to produce more than 41 points, got {rows.Count}.");
        Assert.True(
            result.Duration.TotalSeconds >= expectedMinimumDuration,
            $"Expected duration >= {expectedMinimumDuration:F6}s from joint limits, got {result.Duration.TotalSeconds:F6}s.");
        Assert.Equal(expectedCountFromDuration, rows.Count);
        AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
        AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
    }

    private static void ForceRealModeEnabled(FanucControllerRuntime runtime, double speedRatio)
    {
        SetPrivateField(runtime, "_activeControllerIsSimulation", false);
        SetPrivateField(runtime, "_connectedRobotIp", "127.0.0.1");
        SetPrivateField(runtime, "_isEnabled", true);
        SetPrivateField(runtime, "_bufferSize", 2);
        SetPrivateField(runtime, "_speedRatio", speedRatio);
    }

    private static ControllerClientCompatService CreateCompatService(FanucControllerRuntime runtime)
    {
        var options = new ControllerClientCompatOptions
        {
            ConfigRoot = TestRobotFactory.GetConfigRoot()
        };

        return new ControllerClientCompatService(
            options,
            new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
            runtime,
            new ControllerClientTrajectoryOrchestrator(),
            new RobotConfigLoader());
    }

    private static double ComputeLineAlpha(
        IReadOnlyList<double> actualDegrees,
        IReadOnlyList<double> startRadians,
        IReadOnlyList<double> targetRadians)
    {
        var numerator = 0.0;
        var denominator = 0.0;
        for (var index = 0; index < startRadians.Count; index++)
        {
            var startDegrees = RadiansToDegrees(startRadians[index]);
            var deltaDegrees = RadiansToDegrees(targetRadians[index]) - startDegrees;
            numerator += (actualDegrees[index] - startDegrees) * deltaDegrees;
            denominator += deltaDegrees * deltaDegrees;
        }

        return denominator <= 0.0 ? 0.0 : numerator / denominator;
    }

    private static void AssertJointDegreesEqual(IReadOnlyList<double> expectedRadians, IReadOnlyList<double> actualDegrees)
    {
        Assert.Equal(expectedRadians.Count, actualDegrees.Count);
        for (var index = 0; index < expectedRadians.Count; index++)
        {
            Assert.Equal(RadiansToDegrees(expectedRadians[index]), actualDegrees[index], precision: 4);
        }
    }

    private static void AssertPoseEqual(IReadOnlyList<double> expected, IReadOnlyList<double> actual)
    {
        Assert.Equal(expected.Count, actual.Count);
        for (var index = 0; index < expected.Count; index++)
        {
            Assert.Equal(expected[index], actual[index], precision: 6);
        }
    }

    /// <summary>
    /// 创建用于就绪状态测试的最小 J519 响应。
    /// </summary>
    /// <param name="status">待注入的 J519 状态位。</param>
    /// <param name="sequence">待注入的响应序号。</param>
    /// <returns>包含最小字段集合的测试响应。</returns>
    private static FanucJ519Response CreateJ519Response(byte status, uint sequence)
    {
        return new FanucJ519Response(
            messageType: 0,
            version: 1,
            sequence: sequence,
            status: status,
            readIoType: 0,
            readIoIndex: 0,
            readIoMask: 0,
            readIoValue: 0,
            timestamp: 0,
            pose: new double[6],
            externalAxes: new double[3],
            jointDegrees: new double[6],
            motorCurrents: new double[6]);
    }

    private static RobotProfile CreateMoveJointReferenceRobotProfile()
    {
        return new RobotProfile(
            name: "FANUC_LR_Mate_200iD",
            modelPath: "Models/FANUC_LR_Mate_200iD.robot",
            degreesOfFreedom: 6,
            jointLimits:
            [
                new JointLimit("J1", 7.85, 32.72, 272.7),
                new JointLimit("J2", 6.63, 27.63, 230.28),
                new JointLimit("J3", 9.07, 37.81, 315.12),
                new JointLimit("J4", 9.59, 39.99, 333.3),
                new JointLimit("J5", 9.51, 39.63, 330.27),
                new JointLimit("J6", 17.45, 72.72, 606.01)
            ],
            jointCouplings: Array.Empty<JointCoupling>(),
            servoPeriod: TimeSpan.FromMilliseconds(8),
            triggerPeriod: TimeSpan.FromMilliseconds(8));
    }

    /// <summary>
    /// 创建当前运行配置下的 MoveJoint 限值模型，覆盖报警样本使用的 acc_limit/jerk_limit=0.74。
    /// </summary>
    private static RobotProfile CreateMoveJointRuntimeLimitRobotProfile()
    {
        const double limitScale = 0.74;
        return new RobotProfile(
            name: "FANUC_LR_Mate_200iD",
            modelPath: "Models/FANUC_LR_Mate_200iD.robot",
            degreesOfFreedom: 6,
            jointLimits:
            [
                new JointLimit("J1", 6.45, 26.90 * limitScale, 224.22 * limitScale),
                new JointLimit("J2", 5.41, 22.54 * limitScale, 187.86 * limitScale),
                new JointLimit("J3", 7.15, 29.81 * limitScale, 248.46 * limitScale),
                new JointLimit("J4", 9.59, 39.99 * limitScale, 333.30 * limitScale),
                new JointLimit("J5", 9.51, 39.63 * limitScale, 330.27 * limitScale),
                new JointLimit("J6", 17.45, 72.72 * limitScale, 606.01 * limitScale)
            ],
            jointCouplings: Array.Empty<JointCoupling>(),
            servoPeriod: TimeSpan.FromMilliseconds(8),
            triggerPeriod: TimeSpan.FromMilliseconds(8));
    }

    /// <summary>
    /// 以发送周期上的二阶后向差分估算单轴最大绝对跃度，用来复现 ActualSendJerkStats 的判定口径。
    /// </summary>
    private static double MaxAbsJointJerk(IReadOnlyList<IReadOnlyList<double>> rows, int jointIndex)
    {
        double? previousTime = null;
        double? previousPosition = null;
        double? previousVelocity = null;
        double? previousAcceleration = null;
        var maxAbsJerk = 0.0;

        foreach (var row in rows)
        {
            var currentTime = row[0];
            var currentPosition = row[jointIndex + 1];
            if (previousTime is not null && previousPosition is not null)
            {
                var dt = currentTime - previousTime.Value;
                Assert.True(dt > 0.0, "MoveJoint 稠密轨迹时间戳必须严格递增。");

                var velocity = (currentPosition - previousPosition.Value) / dt;
                var acceleration = previousVelocity is null ? 0.0 : (velocity - previousVelocity.Value) / dt;
                if (previousAcceleration is not null)
                {
                    var jerk = (acceleration - previousAcceleration.Value) / dt;
                    maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(jerk));
                }

                previousVelocity = velocity;
                previousAcceleration = acceleration;
            }

            previousTime = currentTime;
            previousPosition = currentPosition;
        }

        return maxAbsJerk;
    }

    /// <summary>
    /// 验证稠密 MoveJoint 轨迹按 8ms 差分后不会超过机器人每轴 jerk 限值。
    /// </summary>
    private static void AssertAllJointJerksWithinLimits(IReadOnlyList<IReadOnlyList<double>> rows, RobotProfile robot)
    {
        for (var jointIndex = 0; jointIndex < robot.DegreesOfFreedom; jointIndex++)
        {
            var peakJerk = MaxAbsJointJerk(rows, jointIndex);
            var limit = robot.JointLimits[jointIndex].JerkLimit;
            Assert.True(
                peakJerk <= limit * 1.000001,
                $"J{jointIndex + 1} jerk {peakJerk:F6} rad/s^3 exceeds limit {limit:F6} rad/s^3.");
        }
    }

    /// <summary>
    /// 按 7 阶平滑点到点时间律的速度、加速度、jerk 形状系数计算理论最小时长。
    /// </summary>
    private static double ExpectedSmoothPtpDuration(
        RobotProfile robot,
        IReadOnlyList<double> startJoints,
        IReadOnlyList<double> targetJoints,
        double speedRatio = 1.0)
    {
        if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
        {
            throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
        }

        var duration = 0.0;
        for (var index = 0; index < robot.DegreesOfFreedom; index++)
        {
            var distance = Math.Abs(targetJoints[index] - startJoints[index]);
            if (distance <= 0.0)
            {
                continue;
            }

            var limit = robot.JointLimits[index];
            var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / (limit.VelocityLimit * speedRatio);
            var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / (limit.AccelerationLimit * speedRatio * speedRatio));
            var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / (limit.JerkLimit * speedRatio * speedRatio * speedRatio));
            duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
        }

        return duration;
    }

    /// <summary>
    /// 验证 MoveJoint 稠密轨迹已经是物理伺服周期，不把 speed_ratio 编码到采样间隔里。
    /// </summary>
    private static void AssertDenseRowsUseServoPeriod(IReadOnlyList<IReadOnlyList<double>> rows, double servoPeriodSeconds)
    {
        Assert.True(rows.Count > 1);
        for (var index = 1; index < rows.Count; index++)
        {
            Assert.Equal(servoPeriodSeconds, rows[index][0] - rows[index - 1][0], precision: 6);
        }
    }

    private static double DegreesToRadians(double degrees)
    {
        return degrees * Math.PI / 180.0;
    }

    private static double RadiansToDegrees(double radians)
    {
        return radians * 180.0 / Math.PI;
    }

    private static void WaitUntilIdle(FanucControllerRuntime runtime)
    {
        var deadline = DateTimeOffset.UtcNow.AddSeconds(1);
        while (DateTimeOffset.UtcNow < deadline)
        {
            if (!runtime.GetSnapshot().IsInMotion)
            {
                return;
            }

            Thread.Sleep(1);
        }

        throw new TimeoutException("Timed out waiting for dense trajectory send task to finish.");
    }

    /// <summary>
    /// 等待测试条件成立，用于观察后台执行路径是否已经到达指定状态。
    /// </summary>
    private static async Task WaitUntilAsync(Func<bool> condition, CancellationToken cancellationToken)
    {
        var deadline = DateTimeOffset.UtcNow.AddSeconds(1);
        while (DateTimeOffset.UtcNow < deadline)
        {
            cancellationToken.ThrowIfCancellationRequested();
            if (condition())
            {
                return;
            }

            await Task.Delay(1, cancellationToken);
        }

        throw new TimeoutException("Timed out waiting for test condition.");
    }

    /// <summary>
    /// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
    /// </summary>
    private static async Task SendStatusPacketAsync(
        UdpClient server,
        IPEndPoint clientEndpoint,
        uint sequence,
        CancellationToken cancellationToken,
        byte status = 0b0111)
    {
        var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence);
        responsePacket[0x0c] = status;
        await server.SendAsync(responsePacket, clientEndpoint, cancellationToken);
    }

    /// <summary>
    /// 获取一次测试执行生成的唯一稠密发送记录目录。
    /// </summary>
    private static string GetSingleDenseSendRunDirectory(string outputRoot)
    {
        var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
        Assert.True(Directory.Exists(denseSendRoot));

        var runDirectories = Directory.GetDirectories(denseSendRoot);
        Assert.Single(runDirectories);
        return runDirectories[0];
    }

    /// <summary>
    /// 获取本次测试刚生成的新稠密发送记录目录，避免误读历史运行产物。
    /// </summary>
    private static string GetNewDenseSendRunDirectory(string outputRoot, IReadOnlySet<string> beforeRunDirectories)
    {
        var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
        Assert.True(Directory.Exists(denseSendRoot));

        var newDirectories = Directory.GetDirectories(denseSendRoot)
            .Where(path => !beforeRunDirectories.Contains(path))
            .ToArray();
        Assert.Single(newDirectories);
        return newDirectories[0];
    }

    /// <summary>
    /// 加载运行时 Config/RobotConfig.json 中的 UTTC_MS11 轨迹和对应机器人配置。
    /// </summary>
    private static UttcMs11RuntimeFixture LoadUttcMs11RuntimeFixture()
    {
        var configRoot = TestRobotFactory.GetConfigRoot();
        var configPath = Path.Combine(configRoot, "RobotConfig.json");
        var loaded = new RobotConfigLoader().Load(configPath, configRoot);
        var program = loaded.Programs["UTTC_MS11"];
        var uploaded = new ControllerClientCompatUploadedTrajectory(
            name: program.Name,
            waypoints: program.Waypoints.Select(static waypoint => waypoint.Positions),
            shotFlags: program.ShotFlags,
            offsetValues: program.OffsetValues,
            addressGroups: program.AddressGroups.Select(static group => group.Addresses));
        var options = new ControllerClientCompatOptions
        {
            ConfigRoot = configRoot
        };
        var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
            .LoadProfile("FANUC_LR_Mate_200iD", loaded.Robot.AccLimitScale, loaded.Robot.JerkLimitScale);

        return new UttcMs11RuntimeFixture(configRoot, loaded.Robot, uploaded, robot);
    }

    /// <summary>
    /// 复制一份结果并替换程序名，让 DenseSend 调试文件写入唯一目录，避免测试之间复用旧目录权限。
    /// </summary>
    private static TrajectoryResult WithUniqueProgramName(TrajectoryResult result, string programName)
    {
        return new TrajectoryResult(
            programName: programName,
            method: result.Method,
            isValid: result.IsValid,
            duration: result.Duration,
            shotEvents: result.ShotEvents,
            triggerTimeline: result.TriggerTimeline,
            artifacts: result.Artifacts,
            failureReason: result.FailureReason,
            usedCache: result.UsedCache,
            originalWaypointCount: result.OriginalWaypointCount,
            plannedWaypointCount: result.PlannedWaypointCount,
            denseJointTrajectory: result.DenseJointTrajectory,
            preparedFlyshotExecution: result.PreparedFlyshotExecution);
    }

    /// <summary>
    /// 解析空格分隔的纯文本数值列。
    /// </summary>
    private static double[] ParseColumns(string line)
    {
        return line
            .Split(' ', StringSplitOptions.RemoveEmptyEntries)
            .Select(static value => double.Parse(value))
            .ToArray();
    }

    private static void SetPrivateField<T>(FanucControllerRuntime runtime, string name, T value)
    {
        var field = typeof(FanucControllerRuntime).GetField(name, BindingFlags.Instance | BindingFlags.NonPublic);
        Assert.NotNull(field);
        field.SetValue(runtime, value);
    }

    private static TrajectoryResult CreateDenseResult(IReadOnlyList<IReadOnlyList<double>> denseTrajectory, double durationSeconds)
    {
        return new TrajectoryResult(
            programName: "demo",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(durationSeconds),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 4,
            plannedWaypointCount: 4,
            denseJointTrajectory: denseTrajectory);
    }

    private static void SetLatestJ519Response(FanucJ519Client client, byte status)
    {
        var response = new FanucJ519Response(
            messageType: 0,
            version: 1,
            sequence: 10,
            status: status,
            readIoType: 2,
            readIoIndex: 1,
            readIoMask: 255,
            readIoValue: 0,
            timestamp: 1234,
            pose: new double[6],
            externalAxes: new double[3],
            jointDegrees: new double[9],
            motorCurrents: new double[9]);
        var field = typeof(FanucJ519Client).GetField("_latestResponse", BindingFlags.Instance | BindingFlags.NonPublic);
        Assert.NotNull(field);
        field.SetValue(client, response);
    }

    /// <summary>
    /// 封装运行时 UTTC_MS11 轨迹、配置和机器人模型，避免测试反复拼装。
    /// </summary>
    private sealed record UttcMs11RuntimeFixture(
        string ConfigRoot,
        CompatibilityRobotSettings Settings,
        ControllerClientCompatUploadedTrajectory Uploaded,
        RobotProfile Robot);
}
