using Flyshot.ControllerClientCompat;
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc;
using Flyshot.Runtime.Fanuc.Protocol;
using System.Globalization;
using System.Reflection;

namespace Flyshot.Core.Tests;

/// <summary>
/// 验证最小运行时编排链路会把规划结果交给控制器运行时，而不是停留在兼容层内存状态。
/// </summary>
public sealed class RuntimeOrchestrationTests
{
    /// <summary>
    /// 验证 FANUC 最小运行时执行轨迹后会更新状态快照与最终关节位置。
    /// </summary>
    [Fact]
    public void FanucControllerRuntime_ExecuteTrajectory_UpdatesSnapshotAndFinalJointPositions()
    {
        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 result = new TrajectoryResult(
            programName: "demo",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(1.2),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 4,
            plannedWaypointCount: 4);

        runtime.ExecuteTrajectory(result, [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);

        var snapshot = runtime.GetSnapshot();
        Assert.Equal("Connected", snapshot.ConnectionState);
        Assert.False(snapshot.IsInMotion);
        Assert.Equal([1.0, 2.0, 3.0, 4.0, 5.0, 6.0], snapshot.JointPositions);
    }

    /// <summary>
    /// 验证真机运行时会把 TCP 10010 状态通道健康度映射为可诊断连接状态。
    /// </summary>
    [Theory]
    [InlineData(FanucStateConnectionState.Connected, false, "Connected")]
    [InlineData(FanucStateConnectionState.Connected, true, "StateTimeout")]
    [InlineData(FanucStateConnectionState.TimedOut, true, "StateTimeout")]
    [InlineData(FanucStateConnectionState.Reconnecting, true, "Reconnecting")]
    [InlineData(FanucStateConnectionState.Disconnected, false, "Disconnected")]
    public void FanucControllerRuntime_ResolveRealConnectionState_ReflectsStateChannelHealth(
        FanucStateConnectionState state,
        bool isFrameStale,
        string expected)
    {
        var status = new FanucStateClientStatus(
            state,
            isFrameStale,
            lastFrameAt: null,
            reconnectAttemptCount: 0,
            lastErrorMessage: null);

        var actual = FanucControllerRuntime.ResolveRealConnectionState(status);

        Assert.Equal(expected, actual);
    }

    /// <summary>
    /// 验证只有已连接且未陈旧的 TCP 10010 帧会被 runtime 当作当前机器人状态使用。
    /// </summary>
    [Theory]
    [InlineData(FanucStateConnectionState.Connected, false, true)]
    [InlineData(FanucStateConnectionState.Connected, true, false)]
    [InlineData(FanucStateConnectionState.Reconnecting, false, false)]
    [InlineData(FanucStateConnectionState.TimedOut, false, false)]
    [InlineData(FanucStateConnectionState.Disconnected, false, false)]
    public void FanucControllerRuntime_ShouldUseStateFrame_RequiresConnectedFreshState(
        FanucStateConnectionState state,
        bool isFrameStale,
        bool expected)
    {
        var status = new FanucStateClientStatus(
            state,
            isFrameStale,
            lastFrameAt: null,
            reconnectAttemptCount: 0,
            lastErrorMessage: null);

        var actual = FanucControllerRuntime.ShouldUseStateFrame(status);

        Assert.Equal(expected, actual);
    }

    /// <summary>
    /// 验证普通轨迹会先进入 ICSP 规划，并沿用 ICSP 对示教点数量的约束。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_RejectsThreeTeachPoints()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();

        void Act() =>
            orchestrator.PlanOrdinaryTrajectory(
                robot,
                [
                    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
                ]);

        Assert.Throws<ArgumentException>(Act);
    }

    /// <summary>
    /// 验证已上传飞拍轨迹会经过 self-adapt-icsp 并生成拍照触发时间轴。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_BuildsShotTimeline()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();

        var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);

        Assert.True(bundle.Result.IsValid);
        Assert.Single(bundle.Result.ShotEvents);
        Assert.Single(bundle.Result.TriggerTimeline);
    }

    /// <summary>
    /// 验证飞拍规划会把规划限速倍率纳入速度/加速度/Jerk 限制，而不是复用运行时下发倍率。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_AppliesPlanningSpeedScaleToLimits()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();

        var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 1.0);
        var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 0.5);

        Assert.True(
            halfSpeed.Result.Duration.TotalSeconds > fullSpeed.Result.Duration.TotalSeconds * 1.9,
            $"半速规划时长应接近全速的 2 倍，实际 full={fullSpeed.Result.Duration.TotalSeconds}, half={halfSpeed.Result.Duration.TotalSeconds}");
    }

    /// <summary>
    /// 验证飞拍缓存键包含规划限速倍率，避免降速验证时误用 100% 速度下的规划结果。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_CacheKeyIncludesPlanningSpeedScale()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
        var options = new FlyshotExecutionOptions(useCache: true);

        var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 1.0);
        var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 0.5);

        Assert.False(halfSpeed.Result.UsedCache);
        Assert.True(halfSpeed.Result.Duration > fullSpeed.Result.Duration);
    }

    /// <summary>
    /// 验证飞拍编排会使用 RobotConfig.json 中的 IO 保持周期。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesRobotSettingsForHoldCycles()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
        var settings = new CompatibilityRobotSettings(
            useDo: true,
            ioAddresses: [7, 8],
            ioKeepCycles: 4,
            accLimitScale: 1.0,
            jerkLimitScale: 1.0,
            adaptIcspTryNum: 5);

        var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings);

        var doEvent = Assert.Single(bundle.Result.TriggerTimeline);
        Assert.Equal(4, doEvent.HoldCycles);
    }

    /// <summary>
    /// 验证 RobotConfig.json 关闭 use_do 时仍保留 ShotEvent 诊断信息，但不生成伺服 DO 事件。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_SuppressesDoTimeline_WhenUseDoIsFalse()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
        var settings = new CompatibilityRobotSettings(
            useDo: false,
            ioAddresses: [7, 8],
            ioKeepCycles: 4,
            accLimitScale: 1.0,
            jerkLimitScale: 1.0,
            adaptIcspTryNum: 5);

        var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings);

        Assert.Single(bundle.Result.ShotEvents);
        Assert.Empty(bundle.Result.TriggerTimeline);
    }

    /// <summary>
    /// 验证普通轨迹规划后会生成稠密关节采样序列。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_ReturnsDenseJointTrajectory()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();

        var bundle = orchestrator.PlanOrdinaryTrajectory(
            robot,
            [
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
            ]);

        Assert.NotNull(bundle.Result.DenseJointTrajectory);
        Assert.NotEmpty(bundle.Result.DenseJointTrajectory);

        // 验证时间单调递增。
        var times = bundle.Result.DenseJointTrajectory.Select(static row => row[0]).ToArray();
        for (var i = 1; i < times.Length; i++)
        {
            Assert.True(times[i] > times[i - 1], $"采样时间点应在索引 {i} 处单调递增。");
        }

        // 验证每行包含时间 + 6 个关节值。
        Assert.All(bundle.Result.DenseJointTrajectory, row => Assert.Equal(7, row.Count));
    }

    /// <summary>
    /// 验证飞拍轨迹规划后的稠密采样时间轴与伺服周期一致。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_DenseTrajectoryUsesServoPeriod()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();

        var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);

        Assert.NotNull(bundle.Result.DenseJointTrajectory);
        Assert.True(bundle.Result.DenseJointTrajectory.Count > 1);

        // 采样周期应为 8ms（伺服周期）。
        var firstDt = bundle.Result.DenseJointTrajectory[1][0] - bundle.Result.DenseJointTrajectory[0][0];
        Assert.Equal(0.008, firstDt, precision: 3);
    }

    /// <summary>
    /// 验证可直接使用宿主输出目录 Config/RobotConfig.json 中的真实 UTTC_MS11 示教点，
    /// 在 1x 规划速度下生成一条完整飞拍轨迹。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_GeneratesCompleteTrajectoryAtOneX()
    {
        var fixture = LoadUttcMs11RuntimeFixture();
        var orchestrator = new ControllerClientTrajectoryOrchestrator();

        var bundle = orchestrator.PlanUploadedFlyshot(
            fixture.Robot,
            fixture.Uploaded,
            settings: EnableSmoothStartStopTiming(fixture.Settings),
            planningSpeedScale: 1.0);

        Assert.Equal("UTTC_MS11", bundle.Result.ProgramName);
        Assert.NotNull(bundle.Result.DenseJointTrajectory);
        Assert.True(bundle.Result.DenseJointTrajectory.Count > fixture.Uploaded.Waypoints.Count);
        Assert.True(bundle.Result.Duration.TotalSeconds > 0.0);
        Assert.Equal(fixture.Uploaded.ShotFlags.Count(static flag => flag), bundle.Result.ShotEvents.Count);
        Assert.Equal(fixture.Uploaded.ShotFlags.Count(static flag => flag), bundle.Result.TriggerTimeline.Count);

        var firstDt = bundle.Result.DenseJointTrajectory[1][0] - bundle.Result.DenseJointTrajectory[0][0];
        Assert.Equal(fixture.Robot.ServoPeriod.TotalSeconds, firstDt, precision: 6);

        AssertJointRadiansEqual(fixture.Uploaded.Waypoints[0], bundle.Result.DenseJointTrajectory[0].Skip(1).ToArray());
        AssertJointRadiansEqual(fixture.Uploaded.Waypoints[^1], bundle.Result.DenseJointTrajectory[^1].Skip(1).ToArray());
    }

    /// <summary>
    /// 验证现场 UTTC_MS11 配置使用通用规划速度倍率拉长时间轴，并关闭二次平滑时间重映射。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromRuntimeLegacyFitConfig_UsesPlanningScaleWithoutSecondRemap()
    {
        var fixture = LoadUttcMs11RuntimeFixture();
        var orchestrator = new ControllerClientTrajectoryOrchestrator();

        var bundle = orchestrator.PlanUploadedFlyshot(
            fixture.Robot,
            fixture.Uploaded,
            settings: fixture.Settings);
        var baselineBundle = orchestrator.PlanUploadedFlyshot(
            fixture.Robot,
            fixture.Uploaded,
            settings: EnableSmoothStartStopTiming(fixture.Settings),
            planningSpeedScale: 1.0);
        var rawDense = TrajectorySampler.SampleJointTrajectory(
            bundle.PlannedTrajectory,
            samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);

        Assert.Equal(0.7422771653721995, fixture.Settings.PlanningSpeedScale, precision: 12);
        Assert.False(fixture.Settings.SmoothStartStopTiming);
        Assert.Equal(fixture.Uploaded.Waypoints.Count, bundle.PlannedTrajectory.WaypointTimes.Count);
        Assert.Equal(
            baselineBundle.PlannedTrajectory.WaypointTimes[^1] / fixture.Settings.PlanningSpeedScale,
            bundle.PlannedTrajectory.WaypointTimes[^1],
            precision: 6);
        Assert.Equal(7.403046, bundle.PlannedTrajectory.WaypointTimes[^1], precision: 3);

        // 关闭二次时间重映射时，运行时稠密点应直接使用规划样条采样，避免再次改变通用规划时间轴。
        Assert.Equal(rawDense.Count, bundle.Result.DenseJointTrajectory!.Count);
        Assert.Equal(rawDense[1][1], bundle.Result.DenseJointTrajectory[1][1], precision: 12);
        Assert.True(
            Math.Abs(bundle.PlannedTrajectory.WaypointTimes[^1] - bundle.Result.Duration.TotalSeconds) < 1e-6,
            $"执行时长应保留规划终点时间，planned={bundle.PlannedTrajectory.WaypointTimes[^1]}, result={bundle.Result.Duration.TotalSeconds}");
    }

    /// <summary>
    /// 使用运行时 RobotConfig.json 中的真实 UTTC_MS11 示教点，导出首尾 10 点整形前后的逐点对比数据，
    /// 便于和现场报警时间段逐项核对关节、速度、加速度与跃度。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ExportsEdgeComparisonAtOneX()
    {
        const int edgePointCount = 10;
        var fixture = LoadUttcMs11RuntimeFixture();
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var bundle = orchestrator.PlanUploadedFlyshot(
            fixture.Robot,
            fixture.Uploaded,
            settings: fixture.Settings,
            planningSpeedScale: 1.0);
        var rawDense = TrajectorySampler.SampleJointTrajectory(
            bundle.PlannedTrajectory,
            samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
        var shapedDense = bundle.Result.DenseJointTrajectory!;

        var outputDir = Path.Combine(
            fixture.ConfigRoot,
            "Data",
            "UTTC_MS11_EdgeShapeDebug_1x",
            DateTime.Now.ToString("yyyyMMdd_HHmmss_fff", CultureInfo.InvariantCulture));
        Directory.CreateDirectory(outputDir);

        var leadingPath = Path.Combine(outputDir, "Leading10Comparison.csv");
        var trailingPath = Path.Combine(outputDir, "Trailing10Comparison.csv");
        var summaryPath = Path.Combine(outputDir, "Summary.txt");

        WriteEdgeComparisonCsv(leadingPath, rawDense, shapedDense, 0, Math.Min(edgePointCount, shapedDense.Count));
        WriteEdgeComparisonCsv(
            trailingPath,
            rawDense,
            shapedDense,
            Math.Max(0, shapedDense.Count - edgePointCount),
            Math.Min(edgePointCount, shapedDense.Count));

        var summaryLines = new List<string>
        {
            $"program=UTTC_MS11",
            $"planning_speed_scale=1.000000",
            $"smooth_start_stop_timing=true",
            $"servo_period_seconds={fixture.Robot.ServoPeriod.TotalSeconds.ToString("F6", CultureInfo.InvariantCulture)}",
            $"dense_point_count={shapedDense.Count}",
            $"leading_first_step_raw_deg={ComputeStepDegrees(rawDense, 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
            $"leading_first_step_shaped_deg={ComputeStepDegrees(shapedDense, 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
            $"trailing_last_step_raw_deg={ComputeStepDegrees(rawDense, rawDense.Count - 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
            $"trailing_last_step_shaped_deg={ComputeStepDegrees(shapedDense, shapedDense.Count - 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
            $"leading_window_max_abs_raw_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(rawDense, 0, Math.Min(edgePointCount, rawDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
            $"leading_window_max_abs_shaped_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(shapedDense, 0, Math.Min(edgePointCount, shapedDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
            $"trailing_window_max_abs_raw_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(rawDense, Math.Max(0, rawDense.Count - edgePointCount), Math.Min(edgePointCount, rawDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
            $"trailing_window_max_abs_shaped_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(shapedDense, Math.Max(0, shapedDense.Count - edgePointCount), Math.Min(edgePointCount, shapedDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
            $"leading_csv={leadingPath}",
            $"trailing_csv={trailingPath}"
        };

        File.WriteAllLines(summaryPath, summaryLines);

        Assert.True(File.Exists(leadingPath));
        Assert.True(File.Exists(trailingPath));
        Assert.True(File.Exists(summaryPath));
    }

    /// <summary>
    /// 验证运行时 UTTC_MS11 在 1x 下的平滑起停会降低首尾窗口最大跃度，而不是只把第一步压小。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ReducesEdgeWindowJerkAtOneX()
    {
        const int edgePointCount = 10;
        var fixture = LoadUttcMs11RuntimeFixture();
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var bundle = orchestrator.PlanUploadedFlyshot(
            fixture.Robot,
            fixture.Uploaded,
            settings: EnableSmoothStartStopTiming(fixture.Settings),
            planningSpeedScale: 1.0);
        var rawDense = TrajectorySampler.SampleJointTrajectory(
            bundle.PlannedTrajectory,
            samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
        var shapedDense = bundle.Result.DenseJointTrajectory!;

        var rawLeadingMaxJerk = ComputeWindowMaxAbsJerkDegrees(rawDense, 0, Math.Min(edgePointCount, rawDense.Count));
        var shapedLeadingMaxJerk = ComputeWindowMaxAbsJerkDegrees(shapedDense, 0, Math.Min(edgePointCount, shapedDense.Count));
        var rawTrailingMaxJerk = ComputeWindowMaxAbsJerkDegrees(rawDense, Math.Max(0, rawDense.Count - edgePointCount), Math.Min(edgePointCount, rawDense.Count));
        var shapedTrailingMaxJerk = ComputeWindowMaxAbsJerkDegrees(shapedDense, Math.Max(0, shapedDense.Count - edgePointCount), Math.Min(edgePointCount, shapedDense.Count));

        Assert.True(
            shapedLeadingMaxJerk < rawLeadingMaxJerk,
            $"首段最大跃度应下降，raw={rawLeadingMaxJerk:F6}, shaped={shapedLeadingMaxJerk:F6}");
        Assert.True(
            shapedTrailingMaxJerk < rawTrailingMaxJerk,
            $"尾段最大跃度应下降，raw={rawTrailingMaxJerk:F6}, shaped={shapedTrailingMaxJerk:F6}");
    }

    /// <summary>
    /// 验证运行时 UTTC_MS11 在 1x 下的平滑起停会缩小首尾第一步，
    /// 但不能把首尾直接压成静止平台，否则现场执行会变成先停住再起步。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ReducesEdgeStepWithoutFlatteningToZeroAtOneX()
    {
        var fixture = LoadUttcMs11RuntimeFixture();
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var bundle = orchestrator.PlanUploadedFlyshot(
            fixture.Robot,
            fixture.Uploaded,
            settings: EnableSmoothStartStopTiming(fixture.Settings),
            planningSpeedScale: 1.0);
        var rawDense = TrajectorySampler.SampleJointTrajectory(
            bundle.PlannedTrajectory,
            samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
        var shapedDense = bundle.Result.DenseJointTrajectory!;

        var rawLeadingFirstStep = Math.Abs(ComputeStepDegrees(rawDense, 1, 0));
        var shapedLeadingFirstStep = Math.Abs(ComputeStepDegrees(shapedDense, 1, 0));
        var rawTrailingLastStep = Math.Abs(ComputeStepDegrees(rawDense, rawDense.Count - 1, 0));
        var shapedTrailingLastStep = Math.Abs(ComputeStepDegrees(shapedDense, shapedDense.Count - 1, 0));

        Assert.True(
            shapedLeadingFirstStep < rawLeadingFirstStep,
            $"首段第一步应变小，raw={rawLeadingFirstStep:F6}, shaped={shapedLeadingFirstStep:F6}");
        Assert.True(
            shapedTrailingLastStep < rawTrailingLastStep,
            $"尾段最后一步应变小，raw={rawTrailingLastStep:F6}, shaped={shapedTrailingLastStep:F6}");
        Assert.True(
            shapedLeadingFirstStep > 1e-6,
            $"首段第一步不应被压成 0，actual={shapedLeadingFirstStep:F9}");
        Assert.True(
            shapedTrailingLastStep > 1e-6,
            $"尾段最后一步不应被压成 0，actual={shapedTrailingLastStep:F9}");
    }

    /// <summary>
    /// 对比真实可运行轨迹前 0.04s 反推出的局部三次边界特征与当前首段样条系数，
    /// 用于判断当前 clamped-zero 边界是否已经和现场可运行轨迹明显偏离。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ExportsLeadingBoundaryComparison()
    {
        var fixture = LoadUttcMs11RuntimeFixture();
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var bundle = orchestrator.PlanUploadedFlyshot(
            fixture.Robot,
            fixture.Uploaded,
            settings: fixture.Settings,
            planningSpeedScale: 1.0);
        var applyMethod = typeof(ControllerClientTrajectoryOrchestrator).GetMethod(
            "ApplySmoothStartStopTiming",
            BindingFlags.Static | BindingFlags.NonPublic);
        Assert.NotNull(applyMethod);
        var executionTrajectory = Assert.IsType<PlannedTrajectory>(applyMethod!.Invoke(null, [bundle.PlannedTrajectory]));
        var spline = new CubicSplineInterpolator(
            executionTrajectory.WaypointTimes.ToArray(),
            executionTrajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray());

        var comparisonPath = Path.Combine(
            fixture.ConfigRoot,
            "Data",
            "UTTC_MS11_EdgeShapeDebug_1x",
            DateTime.Now.ToString("yyyyMMdd_HHmmss_fff", CultureInfo.InvariantCulture),
            "LeadingBoundaryComparison.txt");
        Directory.CreateDirectory(Path.GetDirectoryName(comparisonPath)!);

        var realPath = Path.Combine(
            TestRobotFactory.GetReplacementRoot(),
            "..",
            "Rvbust",
            "前两个点正常 飞拍失败的运行",
            "真实可运行的飞拍轨迹点位JointDetialTraj.txt");
        var realRows = File.ReadLines(realPath)
            .TakeWhile(static line => !string.IsNullOrWhiteSpace(line))
            .Select(ParseSpaceSeparatedDoubles)
            .Take(4)
            .ToArray();
        Assert.Equal(4, realRows.Length);

        var fieldH = typeof(CubicSplineInterpolator).GetField("_h", BindingFlags.Instance | BindingFlags.NonPublic);
        var fieldA = typeof(CubicSplineInterpolator).GetField("_a", BindingFlags.Instance | BindingFlags.NonPublic);
        var fieldB = typeof(CubicSplineInterpolator).GetField("_b", BindingFlags.Instance | BindingFlags.NonPublic);
        var fieldC = typeof(CubicSplineInterpolator).GetField("_c", BindingFlags.Instance | BindingFlags.NonPublic);
        Assert.NotNull(fieldH);
        Assert.NotNull(fieldA);
        Assert.NotNull(fieldB);
        Assert.NotNull(fieldC);

        var segmentDurations = Assert.IsType<double[]>(fieldH!.GetValue(spline));
        var coeffA = Assert.IsType<double[][]>(fieldA!.GetValue(spline));
        var coeffB = Assert.IsType<double[][]>(fieldB!.GetValue(spline));
        var coeffC = Assert.IsType<double[][]>(fieldC!.GetValue(spline));

        var lines = new List<string>
        {
            $"real_path={Path.GetFullPath(realPath)}",
            $"current_segment0_h_seconds={segmentDurations[0].ToString("F9", CultureInfo.InvariantCulture)}",
            $"real_window_t0={realRows[0][0].ToString("F9", CultureInfo.InvariantCulture)}",
            $"real_window_t1={realRows[1][0].ToString("F9", CultureInfo.InvariantCulture)}",
            $"real_window_t2={realRows[2][0].ToString("F9", CultureInfo.InvariantCulture)}",
            $"real_window_t3={realRows[3][0].ToString("F9", CultureInfo.InvariantCulture)}"
        };

        for (var jointIndex = 0; jointIndex < 6; jointIndex++)
        {
            var currentStartVelocityDeg = RadiansToDegrees(coeffC[0][jointIndex]);
            var currentStartAccelerationDeg = RadiansToDegrees(2.0 * coeffB[0][jointIndex]);
            var currentJerkDeg = RadiansToDegrees(6.0 * coeffA[0][jointIndex]);
            var currentEndVelocityDeg = RadiansToDegrees(
                (3.0 * coeffA[0][jointIndex] * segmentDurations[0] * segmentDurations[0])
                + (2.0 * coeffB[0][jointIndex] * segmentDurations[0])
                + coeffC[0][jointIndex]);

            var localRealFit = FitCubicFromFirstFourRows(realRows, jointIndex + 1);
            var realStartVelocityDeg = localRealFit.Coefficient1;
            var realStartAccelerationDeg = 2.0 * localRealFit.Coefficient2;
            var realJerkDeg = 6.0 * localRealFit.Coefficient3;
            var realEndVelocityDeg = (3.0 * localRealFit.Coefficient3 * localRealFit.LastLocalTime * localRealFit.LastLocalTime)
                + (2.0 * localRealFit.Coefficient2 * localRealFit.LastLocalTime)
                + localRealFit.Coefficient1;

            lines.Add(
                $"J{jointIndex + 1}: current_start_v_deg_s={currentStartVelocityDeg:F9}, real_start_v_deg_s={realStartVelocityDeg:F9}, current_start_a_deg_s2={currentStartAccelerationDeg:F9}, real_start_a_deg_s2={realStartAccelerationDeg:F9}, current_jerk_deg_s3={currentJerkDeg:F9}, real_jerk_deg_s3={realJerkDeg:F9}, current_end_v_deg_s={currentEndVelocityDeg:F9}, real_end_v_deg_s={realEndVelocityDeg:F9}");
        }

        File.WriteAllLines(comparisonPath, lines);
        Assert.True(File.Exists(comparisonPath));
    }

    /// <summary>
    /// 验证飞拍规划结果会应用平滑起停时间轴，而不是直接复用原始线性时间轴采样。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_ShapesDenseTrajectoryEdges()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();

        var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);
        var rawDense = TrajectorySampler.SampleJointTrajectory(
            bundle.PlannedTrajectory,
            samplePeriod: robot.ServoPeriod.TotalSeconds);

        Assert.NotNull(bundle.Result.DenseJointTrajectory);
        Assert.True(bundle.Result.DenseJointTrajectory.Count > 4);

        var dense = bundle.Result.DenseJointTrajectory;
        Assert.NotEqual(rawDense[1][1], dense[1][1]);
        Assert.NotEqual(rawDense[^2][1], dense[^2][1]);
        Assert.Equal(rawDense[0][1], dense[0][1], precision: 6);
        Assert.Equal(rawDense[^1][1], dense[^1][1], precision: 6);
        Assert.Equal(rawDense.Count, dense.Count);
    }

    /// <summary>
    /// 验证旧的首尾整形工具在独立调用时仍会改动首尾窗口，供对比和回归保留。
    /// </summary>
    [Fact]
    public void FlyshotTrajectoryEdgeShaper_ShapesLeadingAndTrailingStepsWithoutFlatteningMiddleSection()
    {
        var denseTrajectory = new IReadOnlyList<double>[]
        {
            [0.000, DegreesToRadians(60.546226), 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.008, DegreesToRadians(60.541482), 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.016, DegreesToRadians(60.536738), 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.024, DegreesToRadians(60.531994), 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.032, DegreesToRadians(60.527250), 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.040, DegreesToRadians(60.522506), 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.048, DegreesToRadians(60.517762), 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.056, DegreesToRadians(60.513018), 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.064, DegreesToRadians(60.508274), 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.072, DegreesToRadians(60.503530), 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.080, DegreesToRadians(60.498786), 0.0, 0.0, 0.0, 0.0, 0.0]
        };

        var shaped = FlyshotTrajectoryEdgeShaper.ShapeDenseJointTrajectory(denseTrajectory);

        Assert.Equal(denseTrajectory.Length, shaped.Count);
        Assert.Equal(denseTrajectory[0][0], shaped[0][0], precision: 6);
        Assert.Equal(denseTrajectory[^1][0], shaped[^1][0], precision: 6);
        Assert.Equal(denseTrajectory[0][1], shaped[0][1], precision: 12);
        Assert.Equal(denseTrajectory[^1][1], shaped[^1][1], precision: 12);

        var firstStepDegrees = Math.Abs(RadiansToDegrees(shaped[1][1] - shaped[0][1]));
        var lastStepDegrees = Math.Abs(RadiansToDegrees(shaped[^1][1] - shaped[^2][1]));
        var originalFirstStepDegrees = Math.Abs(RadiansToDegrees(denseTrajectory[1][1] - denseTrajectory[0][1]));
        var originalLastStepDegrees = Math.Abs(RadiansToDegrees(denseTrajectory[^1][1] - denseTrajectory[^2][1]));
        Assert.NotEqual(originalFirstStepDegrees, firstStepDegrees);
        Assert.NotEqual(originalLastStepDegrees, lastStepDegrees);

        var middleStepDegrees = Math.Abs(RadiansToDegrees(shaped[5][1] - shaped[4][1]));
        Assert.True(middleStepDegrees > 0.002, $"中段单步变化不应被整体压平，actual={middleStepDegrees:F6}deg");
    }

    /// <summary>
    /// 验证飞拍首尾整形只覆盖首尾各 10 个点，锚点之外的中段采样保持不变。
    /// </summary>
    [Fact]
    public void FlyshotTrajectoryEdgeShaper_ShapesOnlyLeadingAndTrailingTenPoints()
    {
        var denseTrajectory = Enumerable.Range(0, 31)
            .Select(index => (IReadOnlyList<double>)
            [
                index * 0.008,
                DegreesToRadians(60.0 - index),
                DegreesToRadians(index * 0.1),
                DegreesToRadians(-index * 0.5),
                DegreesToRadians(index * 0.02),
                DegreesToRadians(index * 0.2),
                DegreesToRadians(index * 0.05)
            ])
            .ToArray();

        var shaped = FlyshotTrajectoryEdgeShaper.ShapeDenseJointTrajectory(denseTrajectory);

        Assert.Equal(denseTrajectory.Length, shaped.Count);

        Assert.NotEqual(denseTrajectory[1][1], shaped[1][1]);
        Assert.NotEqual(denseTrajectory[9][1], shaped[9][1]);
        Assert.Equal(denseTrajectory[10][1], shaped[10][1], precision: 12);
        Assert.Equal(denseTrajectory[11][1], shaped[11][1], precision: 12);
        Assert.Equal(denseTrajectory[19][1], shaped[19][1], precision: 12);
        Assert.Equal(denseTrajectory[20][1], shaped[20][1], precision: 12);
        Assert.NotEqual(denseTrajectory[21][1], shaped[21][1]);
        Assert.NotEqual(denseTrajectory[29][1], shaped[29][1]);
        Assert.Equal(denseTrajectory[30][1], shaped[30][1], precision: 12);
    }

    /// <summary>
    /// 验证兼容服务执行普通轨迹时会进入规划链路，而不是直接把最后一个路点写入状态。
    /// </summary>
    [Fact]
    public void ControllerClientCompatService_ExecuteTrajectory_RejectsThreeTeachPointsAfterPlanningIsIntroduced()
    {
        var service = TestRobotFactory.CreateCompatService();
        service.SetUpRobot("FANUC_LR_Mate_200iD");
        service.SetActiveController(sim: true);
        service.Connect("192.168.10.101");
        service.EnableRobot(2);

        void Act() =>
            service.ExecuteTrajectory(
                [
                    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
                ]);

        Assert.Throws<ArgumentException>(Act);
    }

    /// <summary>
    /// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点，并等待该段运动完成后再启动飞拍轨迹。
    /// </summary>
    [Fact]
    public void ControllerClientCompatService_ExecuteTrajectoryByName_MoveToStartWaitsBeforeFlyshot()
    {
        var configRoot = CreateTempConfigRoot();
        try
        {
            var options = new ControllerClientCompatOptions
            {
                ConfigRoot = configRoot
            };
            var runtime = new DelayedCompletionControllerRuntime(
                initialJointPositions: [0.4, 0.0, 0.0, 0.0, 0.0, 0.0],
                firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80));
            var service = new ControllerClientCompatService(
                options,
                new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
                runtime,
                new ControllerClientTrajectoryOrchestrator(),
                new RobotConfigLoader());

            service.SetUpRobot("FANUC_LR_Mate_200iD");
            service.SetActiveController(sim: false);
            service.Connect("192.168.10.101");
            service.EnableRobot(2);
            service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());

            service.ExecuteTrajectoryByName(
                "demo-flyshot",
                new FlyshotExecutionOptions(moveToStart: true, method: "icsp", saveTrajectory: false, useCache: false));

            Assert.True(runtime.ExecuteCalls.Count >= 2);
            Assert.NotNull(runtime.ExecuteCalls[0].Result.DenseJointTrajectory);
            Assert.True(runtime.ExecuteCalls[0].Result.DenseJointTrajectory!.Count > 1);
            Assert.False(runtime.SecondTrajectoryStartedBeforeFirstMotionCompleted);
        }
        finally
        {
            Directory.Delete(configRoot, recursive: true);
        }
    }

    /// <summary>
    /// 验证正式飞拍前若真实反馈仍未接近起点，则兼容层会拒绝继续执行。
    /// </summary>
    [Fact]
    public void ControllerClientCompatService_ExecuteTrajectoryByName_RejectsFlyshotWhenFeedbackIsNotNearStartAfterMoveToStart()
    {
        var configRoot = CreateTempConfigRoot();
        try
        {
            var options = new ControllerClientCompatOptions
            {
                ConfigRoot = configRoot
            };
            var runtime = new StickyFeedbackControllerRuntime(
                initialJointPositions: [0.4, 0.0, 0.0, 0.0, 0.0, 0.0],
                firstMotionCompletionDelay: TimeSpan.FromMilliseconds(20));
            var service = new ControllerClientCompatService(
                options,
                new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
                runtime,
                new ControllerClientTrajectoryOrchestrator(),
                new RobotConfigLoader());

            service.SetUpRobot("FANUC_LR_Mate_200iD");
            service.SetActiveController(sim: false);
            service.Connect("192.168.10.101");
            service.EnableRobot(2);
            service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());

            var exception = Assert.Throws<InvalidOperationException>(() =>
                service.ExecuteTrajectoryByName(
                    "demo-flyshot",
                    new FlyshotExecutionOptions(moveToStart: true, method: "icsp", saveTrajectory: false, useCache: false)));

            Assert.Contains("not near flyshot start", exception.Message, StringComparison.OrdinalIgnoreCase);
            Assert.Single(runtime.ExecuteCalls);
        }
        finally
        {
            Directory.Delete(configRoot, recursive: true);
        }
    }

    /// <summary>
    /// 验证 ExecuteFlyShotTraj(wait=true) 会等待正式飞拍轨迹完成后再返回。
    /// </summary>
    [Fact]
    public void ControllerClientCompatService_ExecuteTrajectoryByName_WaitTrueWaitsForFlyshotCompletion()
    {
        var configRoot = CreateTempConfigRoot();
        try
        {
            var options = new ControllerClientCompatOptions
            {
                ConfigRoot = configRoot
            };
            var runtime = new DelayedCompletionControllerRuntime(
                initialJointPositions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80));
            var service = new ControllerClientCompatService(
                options,
                new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
                runtime,
                new ControllerClientTrajectoryOrchestrator(),
                new RobotConfigLoader());

            service.SetUpRobot("FANUC_LR_Mate_200iD");
            service.SetActiveController(sim: false);
            service.Connect("192.168.10.101");
            service.EnableRobot(2);
            service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());

            service.ExecuteTrajectoryByName(
                "demo-flyshot",
                new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true));

            Assert.Single(runtime.ExecuteCalls);
            Assert.False(runtime.GetSnapshot().IsInMotion);
        }
        finally
        {
            Directory.Delete(configRoot, recursive: true);
        }
    }

    /// <summary>
    /// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。
    /// </summary>
    [Fact]
    public void ControllerClientCompatService_SetUpRobot_AppliesRobotConfigLimitScales()
    {
        var configRoot = CreateTempConfigRoot();
        try
        {
            File.WriteAllText(
                Path.Combine(configRoot, "RobotConfig.json"),
                """
                {
                  "robot": {
                    "use_do": true,
                    "io_addr": [7, 8],
                    "io_keep_cycles": 4,
                    "acc_limit": 0.5,
                    "jerk_limit": 0.25,
                    "adapt_icsp_try_num": 3
                  },
                  "flying_shots": {}
                }
                """);

            var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
            var runtime = new RecordingControllerRuntime();
            var service = new ControllerClientCompatService(
                options,
                new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
                runtime,
                new ControllerClientTrajectoryOrchestrator(),
                new RobotConfigLoader());

            service.SetUpRobot("FANUC_LR_Mate_200iD");

            var profile = Assert.IsType<RobotProfile>(runtime.LastRobotProfile);
            Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3);
            Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
        }
        finally
        {
            Directory.Delete(configRoot, recursive: true);
        }
    }

    /// <summary>
    /// 验证 IsFlyshotTrajectoryValid(saveTrajectory=true) 会把规划后的结果点位导出到 Config/Data/name。
    /// </summary>
    [Fact]
    public void ControllerClientCompatService_IsFlyshotTrajectoryValid_SaveTrajectoryExportsPlannedData()
    {
        var configRoot = CreateTempConfigRoot();
        try
        {
            WriteRobotConfigWithDemoTrajectory(configRoot);
            var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
            var service = new ControllerClientCompatService(
                options,
                new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
                new RecordingControllerRuntime(),
                new ControllerClientTrajectoryOrchestrator(),
                new RobotConfigLoader());

            service.SetUpRobot("FANUC_LR_Mate_200iD");

            var valid = service.IsFlyshotTrajectoryValid(
                out var duration,
                "demo-flyshot",
                method: "icsp",
                saveTrajectory: true);

            var outputDir = Path.Combine(configRoot, "Data", "demo-flyshot");
            Assert.True(valid);
            Assert.True(duration > TimeSpan.Zero);
            Assert.True(File.Exists(Path.Combine(outputDir, "JointTraj.txt")));
            Assert.True(File.Exists(Path.Combine(outputDir, "JointDetialTraj.txt")));
            Assert.True(File.Exists(Path.Combine(outputDir, "CartTraj.txt")));
            Assert.True(File.Exists(Path.Combine(outputDir, "CartDetialTraj.txt")));
            Assert.True(File.Exists(Path.Combine(outputDir, "ShotEvents.json")));
            Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt")));
            Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "CartDetialTraj.txt")));
        }
        finally
        {
            Directory.Delete(configRoot, recursive: true);
        }
    }

    /// <summary>
    /// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位，并应用当前 speed_ratio。
    /// </summary>
    [Fact]
    public void ControllerClientCompatService_SaveTrajectoryInfo_ExportsActualSendRowsWithSpeedRatio()
    {
        var configRoot = CreateTempConfigRoot();
        try
        {
            WriteRobotConfigWithDemoTrajectory(configRoot);
            var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
            var runtime = new RecordingControllerRuntime();
            var service = new ControllerClientCompatService(
                options,
                new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
                runtime,
                new ControllerClientTrajectoryOrchestrator(),
                new RobotConfigLoader());

            service.SetUpRobot("FANUC_LR_Mate_200iD");
            runtime.SetSpeedRatio(0.5);

            service.SaveTrajectoryInfo("demo-flyshot");

            var outputDir = Path.Combine(configRoot, "Data", "demo-flyshot");
            var pointsPath = Path.Combine(outputDir, "ActualSendJointTraj.txt");
            var timingPath = Path.Combine(outputDir, "ActualSendTiming.txt");
            Assert.True(File.Exists(pointsPath));
            Assert.True(File.Exists(timingPath));

            var pointRows = File.ReadAllLines(pointsPath).Select(ParseSpaceSeparatedDoubles).ToArray();
            var timingRows = File.ReadAllLines(timingPath).Select(ParseSpaceSeparatedDoubles).ToArray();
            var duration = double.Parse(File.ReadLines(Path.Combine(outputDir, "JointTraj.txt")).Last().Split(' ')[0], CultureInfo.InvariantCulture);
            var expectedRows = (int)Math.Ceiling(Math.Max(0.0, (duration / (0.008 * 0.5)) - 1e-9)) + 1;

            Assert.Equal(expectedRows, pointRows.Length);
            Assert.Equal(expectedRows, timingRows.Length);
            Assert.Equal(0.0, pointRows[0][0], precision: 6);
            Assert.Equal(0.008, pointRows[1][0], precision: 6);
            Assert.Equal(0.004, timingRows[1][2], precision: 6);
            Assert.Equal(0.5, timingRows[1][3], precision: 6);
        }
        finally
        {
            Directory.Delete(configRoot, recursive: true);
        }
    }

    /// <summary>
    /// 创建只包含当前支持机器人 JSON 模型和 RobotConfig.json 的临时运行配置根。
    /// </summary>
    private static string CreateTempConfigRoot()
    {
        var configRoot = Path.Combine(Path.GetTempPath(), "flyshot-runtime-tests", Guid.NewGuid().ToString("N"), "Config");
        var modelDir = Path.Combine(configRoot, "Models");
        Directory.CreateDirectory(modelDir);

        var sourceModel = Path.Combine(
            TestRobotFactory.GetReplacementRoot(),
            "Config",
            "Models",
            "LR_Mate_200iD_7L.json");
        File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.json"));

        return configRoot;
    }

    /// <summary>
    /// 写入包含一条飞拍轨迹的最小 RobotConfig.json，供兼容服务从统一配置恢复轨迹。
    /// </summary>
    /// <param name="configRoot">测试运行配置根。</param>
    private static void WriteRobotConfigWithDemoTrajectory(string configRoot)
    {
        File.WriteAllText(
            Path.Combine(configRoot, "RobotConfig.json"),
            """
            {
              "robot": {
                "use_do": true,
                "io_addr": [7, 8],
                "io_keep_cycles": 2,
                "acc_limit": 1.0,
                "jerk_limit": 1.0,
                "adapt_icsp_try_num": 5
              },
              "flying_shots": {
                "demo-flyshot": {
                  "traj_waypoints": [
                    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
                  ],
                  "shot_flags": [false, true, false, false],
                  "offset_values": [0, 1, 0, 0],
                  "addr": [[], [7, 8], [], []]
                }
              }
            }
            """);
    }

    /// <summary>
    /// 角度转弧度，供轨迹整形测试构造输入。
    /// </summary>
    private static double DegreesToRadians(double degrees)
    {
        return degrees * Math.PI / 180.0;
    }

    /// <summary>
    /// 弧度转角度，供轨迹整形测试验证单步变化量。
    /// </summary>
    private static double RadiansToDegrees(double radians)
    {
        return radians * 180.0 / Math.PI;
    }

    /// <summary>
    /// 验证稠密轨迹端点仍与原始示教点一致。
    /// </summary>
    private static void AssertJointRadiansEqual(IReadOnlyList<double> expectedRadians, IReadOnlyList<double> actualRadians)
    {
        Assert.Equal(expectedRadians.Count, actualRadians.Count);
        for (var index = 0; index < expectedRadians.Count; index++)
        {
            Assert.Equal(expectedRadians[index], actualRadians[index], precision: 6);
        }
    }

    /// <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>
    /// 为首尾平滑对比测试显式打开二次时间重映射，避免受现场 legacy-fit 配置影响。
    /// </summary>
    private static CompatibilityRobotSettings EnableSmoothStartStopTiming(CompatibilityRobotSettings settings)
    {
        return new CompatibilityRobotSettings(
            useDo: settings.UseDo,
            ioAddresses: settings.IoAddresses,
            ioKeepCycles: settings.IoKeepCycles,
            accLimitScale: settings.AccLimitScale,
            jerkLimitScale: settings.JerkLimitScale,
            adaptIcspTryNum: settings.AdaptIcspTryNum,
            planningSpeedScale: settings.PlanningSpeedScale,
            smoothStartStopTiming: true);
    }

    /// <summary>
    /// 把指定窗口内的整形前后逐点数据导出为 CSV，包含关节、步长、速度、加速度与跃度。
    /// </summary>
    private static void WriteEdgeComparisonCsv(
        string path,
        IReadOnlyList<IReadOnlyList<double>> rawDense,
        IReadOnlyList<IReadOnlyList<double>> shapedDense,
        int startIndex,
        int count)
    {
        var lines = new List<string>
        {
            BuildEdgeComparisonHeader()
        };

        for (var index = startIndex; index < startIndex + count && index < rawDense.Count && index < shapedDense.Count; index++)
        {
            var columns = new List<string>
            {
                index.ToString(CultureInfo.InvariantCulture),
                rawDense[index][0].ToString("F6", CultureInfo.InvariantCulture)
            };

            AppendJointColumns(columns, "raw_j", index, rawDense, static (rows, rowIndex, jointIndex) => RadiansToDegrees(rows[rowIndex][jointIndex + 1]));
            AppendJointColumns(columns, "shaped_j", index, shapedDense, static (rows, rowIndex, jointIndex) => RadiansToDegrees(rows[rowIndex][jointIndex + 1]));
            AppendJointColumns(columns, "raw_step", index, rawDense, ComputeStepDegrees);
            AppendJointColumns(columns, "shaped_step", index, shapedDense, ComputeStepDegrees);
            AppendJointColumns(columns, "raw_velocity", index, rawDense, ComputeVelocityDegrees);
            AppendJointColumns(columns, "shaped_velocity", index, shapedDense, ComputeVelocityDegrees);
            AppendJointColumns(columns, "raw_acceleration", index, rawDense, ComputeAccelerationDegrees);
            AppendJointColumns(columns, "shaped_acceleration", index, shapedDense, ComputeAccelerationDegrees);
            AppendJointColumns(columns, "raw_jerk", index, rawDense, ComputeJerkDegrees);
            AppendJointColumns(columns, "shaped_jerk", index, shapedDense, ComputeJerkDegrees);
            lines.Add(string.Join(",", columns));
        }

        File.WriteAllLines(path, lines);
    }

    /// <summary>
    /// 构造首尾整形逐点对比 CSV 表头。
    /// </summary>
    private static string BuildEdgeComparisonHeader()
    {
        var columns = new List<string>
        {
            "sample_index",
            "time_seconds"
        };

        AppendJointHeader(columns, "raw_j");
        AppendJointHeader(columns, "shaped_j");
        AppendJointHeader(columns, "raw_step");
        AppendJointHeader(columns, "shaped_step");
        AppendJointHeader(columns, "raw_velocity");
        AppendJointHeader(columns, "shaped_velocity");
        AppendJointHeader(columns, "raw_acceleration");
        AppendJointHeader(columns, "shaped_acceleration");
        AppendJointHeader(columns, "raw_jerk");
        AppendJointHeader(columns, "shaped_jerk");

        return string.Join(",", columns);
    }

    /// <summary>
    /// 追加 J1..J6 表头列名。
    /// </summary>
    private static void AppendJointHeader(List<string> columns, string prefix)
    {
        for (var jointIndex = 1; jointIndex <= 6; jointIndex++)
        {
            columns.Add($"{prefix}_j{jointIndex}");
        }
    }

    /// <summary>
    /// 追加某一行对应的六轴数值列。
    /// </summary>
    private static void AppendJointColumns(
        List<string> columns,
        string prefix,
        int rowIndex,
        IReadOnlyList<IReadOnlyList<double>> rows,
        Func<IReadOnlyList<IReadOnlyList<double>>, int, int, double> selector)
    {
        for (var jointIndex = 0; jointIndex < 6; jointIndex++)
        {
            columns.Add(selector(rows, rowIndex, jointIndex).ToString("F6", CultureInfo.InvariantCulture));
        }
    }

    /// <summary>
    /// 计算某一行相对上一行的单步角度变化量。
    /// </summary>
    private static double ComputeStepDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
    {
        if (rowIndex <= 0)
        {
            return 0.0;
        }

        return RadiansToDegrees(rows[rowIndex][jointIndex + 1] - rows[rowIndex - 1][jointIndex + 1]);
    }

    /// <summary>
    /// 计算某一行的角速度，单位 deg/s。
    /// </summary>
    private static double ComputeVelocityDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
    {
        if (rowIndex <= 0)
        {
            return 0.0;
        }

        var dt = rows[rowIndex][0] - rows[rowIndex - 1][0];
        if (dt <= 0.0)
        {
            return 0.0;
        }

        return ComputeStepDegrees(rows, rowIndex, jointIndex) / dt;
    }

    /// <summary>
    /// 计算某一行的角加速度，单位 deg/s^2。
    /// </summary>
    private static double ComputeAccelerationDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
    {
        if (rowIndex <= 1)
        {
            return 0.0;
        }

        var dt = rows[rowIndex][0] - rows[rowIndex - 1][0];
        if (dt <= 0.0)
        {
            return 0.0;
        }

        return (ComputeVelocityDegrees(rows, rowIndex, jointIndex) - ComputeVelocityDegrees(rows, rowIndex - 1, jointIndex)) / dt;
    }

    /// <summary>
    /// 计算某一行的角跃度，单位 deg/s^3。
    /// </summary>
    private static double ComputeJerkDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
    {
        if (rowIndex <= 2)
        {
            return 0.0;
        }

        var dt = rows[rowIndex][0] - rows[rowIndex - 1][0];
        if (dt <= 0.0)
        {
            return 0.0;
        }

        return (ComputeAccelerationDegrees(rows, rowIndex, jointIndex) - ComputeAccelerationDegrees(rows, rowIndex - 1, jointIndex)) / dt;
    }

    /// <summary>
    /// 统计给定窗口内所有关节的最大绝对值跃度，便于快速看首尾尖峰是否被压低。
    /// </summary>
    private static double ComputeWindowMaxAbsJerkDegrees(
        IReadOnlyList<IReadOnlyList<double>> rows,
        int startIndex,
        int count)
    {
        var maxAbsJerk = 0.0;
        for (var rowIndex = startIndex; rowIndex < startIndex + count && rowIndex < rows.Count; rowIndex++)
        {
            for (var jointIndex = 0; jointIndex < 6; jointIndex++)
            {
                maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(ComputeJerkDegrees(rows, rowIndex, jointIndex)));
            }
        }

        return maxAbsJerk;
    }

    /// <summary>
    /// 解析空格分隔的关节轨迹行。
    /// </summary>
    private static double[] ParseSpaceSeparatedDoubles(string line)
    {
        return line
            .Split(' ', StringSplitOptions.RemoveEmptyEntries)
            .Select(static value => double.Parse(value, CultureInfo.InvariantCulture))
            .ToArray();
    }

    /// <summary>
    /// 用真实可运行轨迹前 4 个采样点拟合局部三次曲线，返回相对首点时间的系数。
    /// 曲线形式为 p(t)=c3*t^3+c2*t^2+c1*t+c0，单位保持输入文件的角度制。
    /// </summary>
    private static LocalCubicFit FitCubicFromFirstFourRows(IReadOnlyList<double[]> rows, int valueColumn)
    {
        var t0 = rows[0][0];
        var matrix = new double[4][];
        var rhs = new double[4];
        for (var index = 0; index < 4; index++)
        {
            var localTime = rows[index][0] - t0;
            matrix[index] =
            [
                localTime * localTime * localTime,
                localTime * localTime,
                localTime,
                1.0
            ];
            rhs[index] = rows[index][valueColumn];
        }

        var solution = SolveLinearSystem4x4(matrix, rhs);
        return new LocalCubicFit(
            Coefficient3: solution[0],
            Coefficient2: solution[1],
            Coefficient1: solution[2],
            Coefficient0: solution[3],
            LastLocalTime: rows[3][0] - t0);
    }

    /// <summary>
    /// 用高斯消元求解 4x4 线性方程组。
    /// </summary>
    private static double[] SolveLinearSystem4x4(double[][] matrix, double[] rhs)
    {
        var a = matrix.Select(static row => row.ToArray()).ToArray();
        var b = rhs.ToArray();

        for (var pivot = 0; pivot < 4; pivot++)
        {
            var bestRow = pivot;
            for (var row = pivot + 1; row < 4; row++)
            {
                if (Math.Abs(a[row][pivot]) > Math.Abs(a[bestRow][pivot]))
                {
                    bestRow = row;
                }
            }

            if (Math.Abs(a[bestRow][pivot]) <= 1e-12)
            {
                throw new InvalidOperationException("真实轨迹前 4 点无法稳定拟合局部三次曲线。");
            }

            if (bestRow != pivot)
            {
                (a[pivot], a[bestRow]) = (a[bestRow], a[pivot]);
                (b[pivot], b[bestRow]) = (b[bestRow], b[pivot]);
            }

            var divisor = a[pivot][pivot];
            for (var column = pivot; column < 4; column++)
            {
                a[pivot][column] /= divisor;
            }

            b[pivot] /= divisor;

            for (var row = 0; row < 4; row++)
            {
                if (row == pivot)
                {
                    continue;
                }

                var factor = a[row][pivot];
                for (var column = pivot; column < 4; column++)
                {
                    a[row][column] -= factor * a[pivot][column];
                }

                b[row] -= factor * b[pivot];
            }
        }

        return b;
    }
}

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

/// <summary>
/// 记录基于真实轨迹前 4 点拟合得到的局部三次曲线系数。
/// </summary>
internal sealed record LocalCubicFit(
    double Coefficient3,
    double Coefficient2,
    double Coefficient1,
    double Coefficient0,
    double LastLocalTime);

/// <summary>
/// 为运行时编排测试构造稳定的最小领域对象。
/// </summary>
internal static class TestRobotFactory
{
    /// <summary>
    /// 构造六轴测试机器人配置，避免运行时测试依赖真实 .robot 文件。
    /// </summary>
    /// <returns>可用于规划和运行时状态校验的机器人配置。</returns>
    public static RobotProfile CreateRobotProfile()
    {
        return new RobotProfile(
            name: "TestRobot",
            modelPath: "Models/Test.robot",
            degreesOfFreedom: 6,
            jointLimits: Enumerable.Range(1, 6)
                .Select(static index => new JointLimit($"J{index}", 10.0, 20.0, 100.0))
                .ToArray(),
            jointCouplings: Array.Empty<JointCoupling>(),
            servoPeriod: TimeSpan.FromMilliseconds(8),
            triggerPeriod: TimeSpan.FromMilliseconds(8));
    }

    /// <summary>
    /// 构造一条含单个拍照点的上传飞拍轨迹。
    /// </summary>
    /// <returns>可用于触发时间轴测试的上传轨迹。</returns>
    public static ControllerClientCompatUploadedTrajectory CreateUploadedTrajectoryWithSingleShot()
    {
        return new ControllerClientCompatUploadedTrajectory(
            name: "demo-flyshot",
            waypoints:
            [
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
            ],
            shotFlags: [false, true, false, false],
            offsetValues: [0, 1, 0, 0],
            addressGroups:
            [
                Array.Empty<int>(),
                [7, 8],
                Array.Empty<int>(),
                Array.Empty<int>()
            ]);
    }

    /// <summary>
    /// 构造一份真实依赖注入等价的兼容服务，覆盖运行时和编排器协作。
    /// </summary>
    /// <returns>可执行 ControllerClient 兼容语义的服务实例。</returns>
    public static ControllerClientCompatService CreateCompatService()
    {
        var options = new ControllerClientCompatOptions
        {
            ConfigRoot = GetConfigRoot()
        };

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

    /// <summary>
    /// 定位 replacement 仓库内的运行配置根目录。
    /// </summary>
    /// <returns>当前仓库 Config 目录。</returns>
    public static string GetConfigRoot()
    {
        return Path.Combine(GetReplacementRoot(), "Config");
    }

    /// <summary>
    /// 定位 replacement 仓库根目录，供测试读取仓库内固化配置。
    /// </summary>
    /// <returns>replacement 仓库根目录。</returns>
    public static string GetReplacementRoot()
    {
        var current = new DirectoryInfo(AppContext.BaseDirectory);
        while (current is not null)
        {
            if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
            {
                return current.FullName;
            }

            current = current.Parent;
        }

        throw new DirectoryNotFoundException("Unable to locate the flyshot replacement root.");
    }

    /// <summary>
    /// 定位父工作区根目录，供兼容服务加载真实机器人模型。
    /// </summary>
    /// <returns>父工作区根目录。</returns>
    public static string GetWorkspaceRoot()
    {
        var current = new DirectoryInfo(AppContext.BaseDirectory);
        while (current is not null)
        {
            if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
            {
                return Path.GetFullPath(Path.Combine(current.FullName, ".."));
            }

            current = current.Parent;
        }

        throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
    }
}

/// <summary>
/// 记录 ResetRobot 入参的测试运行时，用于验证兼容服务传递的机器人配置。
/// </summary>
internal sealed class RecordingControllerRuntime : IControllerRuntime
{
    private double _speedRatio = 1.0;

    /// <summary>
    /// 获取最近一次 ResetRobot 收到的机器人配置。
    /// </summary>
    public RobotProfile? LastRobotProfile { get; private set; }

    /// <inheritdoc />
    public void ResetRobot(RobotProfile robot, string robotName)
    {
        LastRobotProfile = robot;
    }

    /// <inheritdoc />
    public void SetActiveController(bool sim)
    {
    }

    /// <inheritdoc />
    public void Connect(string robotIp)
    {
    }

    /// <inheritdoc />
    public void Disconnect()
    {
    }

    /// <inheritdoc />
    public void EnableRobot(int bufferSize)
    {
    }

    /// <inheritdoc />
    public void DisableRobot()
    {
    }

    /// <inheritdoc />
    public void StopMove()
    {
    }

    /// <inheritdoc />
    public double GetSpeedRatio() => _speedRatio;

    /// <inheritdoc />
    public void SetSpeedRatio(double ratio)
    {
        _speedRatio = ratio;
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];

    /// <inheritdoc />
    public void SetTcp(double x, double y, double z)
    {
    }

    /// <inheritdoc />
    public bool GetIo(int port, string ioType) => false;

    /// <inheritdoc />
    public void SetIo(int port, bool value, string ioType)
    {
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetJointPositions() => Array.Empty<double>();

    /// <inheritdoc />
    public IReadOnlyList<double> GetPose() => Array.Empty<double>();

    /// <inheritdoc />
    public ControllerStateSnapshot GetSnapshot()
    {
        return new ControllerStateSnapshot(
            capturedAt: DateTimeOffset.UtcNow,
            connectionState: "Connected",
            isEnabled: true,
            isInMotion: false,
            speedRatio: _speedRatio,
            jointPositions: Array.Empty<double>(),
            cartesianPose: Array.Empty<double>(),
            activeAlarms: Array.Empty<RuntimeAlarm>());
    }

    /// <inheritdoc />
    public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
    {
    }
}

/// <summary>
/// 模拟第一段运动异步完成的测试运行时，用于验证兼容层是否等待 move_to_start 完成。
/// </summary>
internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
{
    private readonly object _lock = new();
    private readonly TimeSpan _firstMotionCompletionDelay;
    private double[] _jointPositions;
    private bool _isEnabled;
    private bool _isInMotion;
    private bool _firstMotionCompleted;

    /// <summary>
    /// 初始化可延迟完成第一段运动的测试运行时。
    /// </summary>
    /// <param name="initialJointPositions">运行时报告的初始关节位置。</param>
    /// <param name="firstMotionCompletionDelay">第一段运动完成前保持忙碌的时间。</param>
    public DelayedCompletionControllerRuntime(
        IReadOnlyList<double> initialJointPositions,
        TimeSpan firstMotionCompletionDelay)
    {
        _jointPositions = initialJointPositions.ToArray();
        _firstMotionCompletionDelay = firstMotionCompletionDelay;
    }

    /// <summary>
    /// 获取所有 ExecuteTrajectory 调用记录。
    /// </summary>
    public List<(TrajectoryResult Result, IReadOnlyList<double> FinalJointPositions)> ExecuteCalls { get; } = [];

    /// <summary>
    /// 获取第二条轨迹是否在第一段 move_to_start 完成前启动。
    /// </summary>
    public bool SecondTrajectoryStartedBeforeFirstMotionCompleted { get; private set; }

    /// <inheritdoc />
    public void ResetRobot(RobotProfile robot, string robotName)
    {
    }

    /// <inheritdoc />
    public void SetActiveController(bool sim)
    {
    }

    /// <inheritdoc />
    public void Connect(string robotIp)
    {
    }

    /// <inheritdoc />
    public void Disconnect()
    {
    }

    /// <inheritdoc />
    public void EnableRobot(int bufferSize)
    {
        _isEnabled = true;
    }

    /// <inheritdoc />
    public void DisableRobot()
    {
        _isEnabled = false;
    }

    /// <inheritdoc />
    public void StopMove()
    {
        lock (_lock)
        {
            _isInMotion = false;
        }
    }

    /// <inheritdoc />
    public double GetSpeedRatio() => 1.0;

    /// <inheritdoc />
    public void SetSpeedRatio(double ratio)
    {
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];

    /// <inheritdoc />
    public void SetTcp(double x, double y, double z)
    {
    }

    /// <inheritdoc />
    public bool GetIo(int port, string ioType) => false;

    /// <inheritdoc />
    public void SetIo(int port, bool value, string ioType)
    {
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetJointPositions()
    {
        lock (_lock)
        {
            return _jointPositions.ToArray();
        }
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetPose() => Array.Empty<double>();

    /// <inheritdoc />
    public ControllerStateSnapshot GetSnapshot()
    {
        lock (_lock)
        {
            return new ControllerStateSnapshot(
                capturedAt: DateTimeOffset.UtcNow,
                connectionState: "Connected",
                isEnabled: _isEnabled,
                isInMotion: _isInMotion,
                speedRatio: 1.0,
                jointPositions: _jointPositions.ToArray(),
                cartesianPose: Array.Empty<double>(),
                activeAlarms: Array.Empty<RuntimeAlarm>());
        }
    }

    /// <inheritdoc />
    public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
    {
        lock (_lock)
        {
            ExecuteCalls.Add((result, finalJointPositions.ToArray()));
            if (ExecuteCalls.Count == 1)
            {
                _isInMotion = true;
                _ = Task.Run(async () =>
                {
                    await Task.Delay(_firstMotionCompletionDelay).ConfigureAwait(false);
                    lock (_lock)
                    {
                        _jointPositions = finalJointPositions.ToArray();
                        _isInMotion = false;
                        _firstMotionCompleted = true;
                    }
                });
                return;
            }

            if (!_firstMotionCompleted)
            {
                SecondTrajectoryStartedBeforeFirstMotionCompleted = true;
            }

            _jointPositions = finalJointPositions.ToArray();
        }
    }
}

/// <summary>
/// 模拟 move_to_start 逻辑完成后，真实反馈仍停留在旧位置的测试运行时。
/// </summary>
internal sealed class StickyFeedbackControllerRuntime : IControllerRuntime
{
    private readonly object _lock = new();
    private readonly TimeSpan _firstMotionCompletionDelay;
    private double[] _jointPositions;
    private bool _isEnabled;
    private bool _isInMotion;
    private bool _firstMotionCompleted;

    public StickyFeedbackControllerRuntime(
        IReadOnlyList<double> initialJointPositions,
        TimeSpan firstMotionCompletionDelay)
    {
        _jointPositions = initialJointPositions.ToArray();
        _firstMotionCompletionDelay = firstMotionCompletionDelay;
    }

    public List<(TrajectoryResult Result, IReadOnlyList<double> FinalJointPositions)> ExecuteCalls { get; } = [];

    public void ResetRobot(RobotProfile robot, string robotName)
    {
    }

    public void SetActiveController(bool sim)
    {
    }

    public void Connect(string robotIp)
    {
    }

    public void Disconnect()
    {
    }

    public void EnableRobot(int bufferSize)
    {
        _isEnabled = true;
    }

    public void DisableRobot()
    {
        _isEnabled = false;
    }

    public void StopMove()
    {
        lock (_lock)
        {
            _isInMotion = false;
        }
    }

    public double GetSpeedRatio() => 1.0;

    public void SetSpeedRatio(double ratio)
    {
    }

    public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];

    public void SetTcp(double x, double y, double z)
    {
    }

    public bool GetIo(int port, string ioType) => false;

    public void SetIo(int port, bool value, string ioType)
    {
    }

    public IReadOnlyList<double> GetJointPositions()
    {
        lock (_lock)
        {
            return _jointPositions.ToArray();
        }
    }

    public IReadOnlyList<double> GetPose() => Array.Empty<double>();

    public ControllerStateSnapshot GetSnapshot()
    {
        lock (_lock)
        {
            return new ControllerStateSnapshot(
                capturedAt: DateTimeOffset.UtcNow,
                connectionState: "Connected",
                isEnabled: _isEnabled,
                isInMotion: _isInMotion,
                speedRatio: 1.0,
                jointPositions: _jointPositions.ToArray(),
                cartesianPose: Array.Empty<double>(),
                activeAlarms: Array.Empty<RuntimeAlarm>());
        }
    }

    public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
    {
        lock (_lock)
        {
            ExecuteCalls.Add((result, finalJointPositions.ToArray()));
            if (!_firstMotionCompleted)
            {
                _isInMotion = true;
                _ = Task.Run(async () =>
                {
                    await Task.Delay(_firstMotionCompletionDelay).ConfigureAwait(false);
                    lock (_lock)
                    {
                        // 模拟控制链逻辑上结束，但真实反馈仍卡在旧位置。
                        _isInMotion = false;
                        _firstMotionCompleted = true;
                    }
                });
                return;
            }

            _jointPositions = finalJointPositions.ToArray();
        }
    }
}