using Flyshot.Core.Domain;
using Flyshot.ControllerClientCompat;
using Flyshot.Core.Config;
using Flyshot.Runtime.Fanuc;
using Flyshot.Runtime.Fanuc.Protocol;
using System.Reflection;

namespace Flyshot.Core.Tests;

/// <summary>
/// 验证 FANUC 控制器运行在稠密轨迹流式执行与 IO 触发上的行为。
/// </summary>
public sealed class FanucControllerRuntimeDenseTests
{
    private const double CapturedMvpointVelocityShapeCoefficient = 2.0759961613199973;
    private const double CapturedMvpointAccelerationShapeCoefficient = 7.986313199999984;
    private const double CapturedMvpointJerkShapeCoefficient = 36.12609273600853;

    /// <summary>
    /// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
    {
        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, 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 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, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
        WaitUntilIdle(runtime);

        var commands = j519Client.GetCommandHistoryForTests();
        Assert.Equal(5, commands.Count);
        Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
        Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
    }

    /// <summary>
    /// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
    /// </summary>
    [Theory]
    [InlineData(1.0)]
    [InlineData(0.7)]
    [InlineData(0.5)]
    public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryAndResamplesBySpeedRatio(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_LowerSpeedUsesMoreSamplesWithoutFixedCountContract()
    {
        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);
        Assert.InRange(fullSpeed.Duration.TotalSeconds, 0.318, 0.322);
        Assert.True(speed07.Duration.TotalSeconds >= 0.320);
        Assert.InRange(speed05.Duration.TotalSeconds, 0.318, 0.322);
    }

    [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>
    /// 验证 speed_ratio=0 时不会启动无法推进轨迹时间的后台发送任务。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsZeroSpeedRatio()
    {
        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: 0.0);

        var denseTrajectory = new[]
        {
            new[] { 0.0, 0.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 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);

        var exception = Assert.Throws<InvalidOperationException>(
            () => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]));
        Assert.Contains("Speed ratio", exception.Message, StringComparison.OrdinalIgnoreCase);
    }

    /// <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)10, (ushort)10, (ushort)10], commands.Select(static command => command.WriteIoMask));
        Assert.Equal([(ushort)0, (ushort)10, (ushort)10, (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>
    /// 验证 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 地址组中的地址号被正确映射为 writeIoValue 的位掩码。
    /// </summary>
    [Theory]
    [InlineData(new[] { 0 }, (ushort)1)]
    [InlineData(new[] { 7 }, (ushort)128)]
    [InlineData(new[] { 7, 8 }, (ushort)384)] // 128 + 256
    [InlineData(new[] { 15 }, (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>
    /// 验证超过 15 的地址号会被安全忽略，不会溢出位掩码。
    /// </summary>
    [Fact]
    public void ComputeIoValue_IgnoresOutOfRangeAddresses()
    {
        var group = new IoAddressGroup([0, 16, 7]);
        var actual = FanucControllerRuntime.ComputeIoValue(group);
        Assert.Equal((ushort)(1 | 128), actual);
    }

    [Fact]
    public void MoveJointTrajectoryGenerator_MatchesCapturedMvpointAlphaLawAtSpeedOne()
    {
        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.Equal(41, rows.Count);
        Assert.InRange(result.Duration.TotalSeconds, 0.318, 0.322);

        var expectedAlpha = new[]
        {
            0.000000000000,
            0.000012196163,
            0.000106156906,
            0.000764380061,
            0.002550804028,
            0.006029689194,
            0.011765134027,
            0.020321400844,
            0.032262426551,
            0.048152469303,
            0.068555498563,
            0.093895155669,
            0.124210027377,
            0.159174512929,
            0.198230386318,
            0.240813559900,
            0.286359937276,
            0.334305411725,
            0.384085546646,
            0.435136609163,
            0.486894129077,
            0.538794033110,
            0.590272360135,
            0.640764719629,
            0.689707151220,
            0.736535405849,
            0.780685354316,
            0.821592775628,
            0.858693734065,
            0.891423926949,
            0.919286047395,
            0.942156722091,
            0.960255163676,
            0.974119666692,
            0.984314536393,
            0.991403790959,
            0.995951593494,
            0.998522142663,
            0.999679443354,
            0.999987892657,
            1.000000000000
        };

        for (var index = 0; index < rows.Count; index++)
        {
            var actualDegrees = rows[index].Skip(1).Select(RadiansToDegrees).ToArray();
            var alpha = ComputeLineAlpha(actualDegrees, startJoints, targetJoints);
            Assert.Equal(expectedAlpha[index], alpha, precision: 6);
        }
    }

    [Fact]
    public void MoveJointTrajectoryGenerator_DoesNotShortenBaseDurationWhenSpeedRatioDoesNotDivideWindow()
    {
        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 >= 0.320, $"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
        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 * CapturedMvpointVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
        var expectedAccelerationDuration = Math.Sqrt(Math.PI * CapturedMvpointAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
        var expectedJerkDuration = Math.Cbrt(Math.PI * CapturedMvpointJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
        var expectedMinimumDuration = new[]
        {
            0.320,
            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(),
            new InMemoryFlyshotTrajectoryStore());
    }

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

    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.");
    }

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