using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Runtime.Common;
using Microsoft.Extensions.Logging;

namespace Flyshot.ControllerClientCompat;

/// <summary>
/// 在宿主进程内实现 HTTP-only ControllerClient 兼容语义，并把控制器状态委托给运行时。
/// </summary>
public sealed class ControllerClientCompatService : IControllerClientCompatService
{
    private readonly object _stateLock = new();
    private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _uploadedTrajectories = new(StringComparer.Ordinal);
    private readonly ControllerClientCompatOptions _options;
    private readonly ControllerClientCompatRobotCatalog _robotCatalog;
    private readonly IControllerRuntime _runtime;
    private readonly ControllerClientTrajectoryOrchestrator _trajectoryOrchestrator;
    private readonly RobotConfigLoader _configLoader;
    private readonly FlyshotTrajectoryArtifactWriter _artifactWriter;
    private readonly JsonFlyshotTrajectoryStore _trajectoryStore;
    private readonly ILogger<ControllerClientCompatService>? _logger;
    private RobotProfile? _activeRobotProfile;
    private string? _configuredRobotName;
    private CompatibilityRobotSettings? _robotSettings;
    private string? _connectedServerIp;
    private int _connectedServerPort;
    private bool _showTcp = true;
    private double _showTcpAxisLength = 0.1;
    private int _showTcpAxisSize = 2;

    /// <summary>
    /// 初始化一份 HTTP-only 的 ControllerClient 兼容服务。
    /// </summary>
    /// <param name="options">兼容层基础配置。</param>
    /// <param name="robotCatalog">机器人模型目录。</param>
    /// <param name="runtime">控制器运行时。</param>
    /// <param name="trajectoryOrchestrator">轨迹规划与触发编排器。</param>
    /// <param name="configLoader">旧版 RobotConfig.json 加载器。</param>
    /// <param name="artifactWriter">saveTrajectory 规划结果点位导出器。</param>
    /// <param name="trajectoryStore">统一 RobotConfig.json 持久化存储；为空时按配置根目录创建默认实例。</param>
    /// <param name="logger">日志记录器；允许测试直接构造时传入 null。</param>
    public ControllerClientCompatService(
        ControllerClientCompatOptions options,
        ControllerClientCompatRobotCatalog robotCatalog,
        IControllerRuntime runtime,
        ControllerClientTrajectoryOrchestrator trajectoryOrchestrator,
        RobotConfigLoader configLoader,
        FlyshotTrajectoryArtifactWriter? artifactWriter = null,
        JsonFlyshotTrajectoryStore? trajectoryStore = null,
        ILogger<ControllerClientCompatService>? logger = null)
    {
        _options = options ?? throw new ArgumentNullException(nameof(options));
        _robotCatalog = robotCatalog ?? throw new ArgumentNullException(nameof(robotCatalog));
        _runtime = runtime ?? throw new ArgumentNullException(nameof(runtime));
        _trajectoryOrchestrator = trajectoryOrchestrator ?? throw new ArgumentNullException(nameof(trajectoryOrchestrator));
        _configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader));
        _artifactWriter = artifactWriter ?? new FlyshotTrajectoryArtifactWriter(_options, new RobotModelLoader());
        _trajectoryStore = trajectoryStore ?? new JsonFlyshotTrajectoryStore(_options, _configLoader);
        _logger = logger;
    }

    /// <inheritdoc />
    public string ServerVersion => _options.ServerVersion;

    /// <inheritdoc />
    public bool IsSetUp
    {
        get
        {
            lock (_stateLock)
            {
                return _activeRobotProfile is not null;
            }
        }
    }

    /// <summary>
    /// 获取当前运行时是否处于运动态。
    /// </summary>
    public bool IsInMotion => _runtime.GetSnapshot().IsInMotion;

    /// <inheritdoc />
    public void ConnectServer(string serverIp, int port)
    {
        if (string.IsNullOrWhiteSpace(serverIp))
        {
            throw new ArgumentException("服务端 IP 不能为空。", nameof(serverIp));
        }

        if (port <= 0)
        {
            throw new ArgumentOutOfRangeException(nameof(port), "端口必须大于 0。");
        }

        lock (_stateLock)
        {
            // HTTP-only 阶段仍记录旧客户端期望的 50001 地址，便于后续 TCP 入口恢复时复用状态。
            _connectedServerIp = serverIp;
            _connectedServerPort = port;
        }

        _logger?.LogInformation("ConnectServer 完成: {ServerIp}:{Port}", serverIp, port);
    }

    /// <inheritdoc />
    public string GetServerVersion()
    {
        return ServerVersion;
    }

    /// <inheritdoc />
    public string GetClientVersion()
    {
        return "flyshot-replacement-controller-client-compat/0.1.0";
    }

    /// <inheritdoc />
    public void SetUpRobot(string robotName)
    {
        _logger?.LogInformation("SetUpRobot 开始: robotName={RobotName}", robotName);

        var robotSettings = TryLoadRobotSettings() ?? CreateDefaultRobotSettings();
        var robotProfile = _robotCatalog.LoadProfile(
            robotName,
            robotSettings.AccLimitScale,
            robotSettings.JerkLimitScale);

        lock (_stateLock)
        {
            // 机器人重新初始化时同步重置运行时和上传轨迹目录，保持旧服务初始化语义。
            _configuredRobotName = robotName;
            _activeRobotProfile = robotProfile;
            _uploadedTrajectories.Clear();
            _runtime.ResetRobot(robotProfile, robotName);
            _robotSettings = robotSettings;

            // 从持久化存储恢复该机器人名下之前已上传的轨迹。
            var savedTrajectories = _trajectoryStore.LoadAll(robotName, out _);
            foreach (var saved in savedTrajectories)
            {
                _uploadedTrajectories[saved.Key] = saved.Value;
            }
        }

        _logger?.LogInformation(
            "SetUpRobot 完成: robotName={RobotName}, dof={Dof}, accLimit={AccLimit}, jerkLimit={JerkLimit}, 恢复轨迹数={TrajCount}",
            robotName,
            robotProfile.DegreesOfFreedom,
            robotSettings.AccLimitScale,
            robotSettings.JerkLimitScale,
            _uploadedTrajectories.Count);
    }

    /// <inheritdoc />
    public void SetUpRobotFromEnv(string envFile)
    {
        if (string.IsNullOrWhiteSpace(envFile))
        {
            throw new ArgumentException("环境文件路径不能为空。", nameof(envFile));
        }

        throw new NotSupportedException("SetUpRobotFromEnv 尚未接入环境文件解析。");
    }

    /// <inheritdoc />
    public void SetShowTcp(bool isShow, double axisLength, int axisSize)
    {
        if (axisLength <= 0.0)
        {
            throw new ArgumentOutOfRangeException(nameof(axisLength), "TCP 坐标轴长度必须大于 0。");
        }

        if (axisSize <= 0)
        {
            throw new ArgumentOutOfRangeException(nameof(axisSize), "TCP 坐标轴线宽必须大于 0。");
        }

        lock (_stateLock)
        {
            EnsureRobotSetup();

            // 当前无 GUI 渲染层，先保存显示参数，保证旧 SDK 参数不会在 HTTP 边界丢失。
            _showTcp = isShow;
            _showTcpAxisLength = axisLength;
            _showTcpAxisSize = axisSize;
        }
    }

    /// <inheritdoc />
    public void SetActiveController(bool sim)
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            _runtime.SetActiveController(sim);
        }
    }

    /// <inheritdoc />
    public void Connect(string robotIp)
    {
        if (string.IsNullOrWhiteSpace(robotIp))
        {
            throw new ArgumentException("控制器 IP 不能为空。", nameof(robotIp));
        }

        _logger?.LogInformation("Connect 开始: robotIp={RobotIp}", robotIp);

        lock (_stateLock)
        {
            EnsureRobotSetup();
            _runtime.Connect(robotIp);
        }

        _logger?.LogInformation("Connect 完成: robotIp={RobotIp}", robotIp);
    }

    /// <inheritdoc />
    public void Disconnect()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            _runtime.Disconnect();
        }
    }

    /// <inheritdoc />
    public void EnableRobot(int bufferSize)
    {
        _logger?.LogInformation("EnableRobot 开始: bufferSize={BufferSize}", bufferSize);
        lock (_stateLock)
        {
            EnsureRobotSetup();
            _runtime.EnableRobot(bufferSize);
        }
        _logger?.LogInformation("EnableRobot 完成");
    }

    /// <inheritdoc />
    public void DisableRobot()
    {
        _logger?.LogInformation("DisableRobot 开始");
        lock (_stateLock)
        {
            EnsureRobotSetup();
            _runtime.DisableRobot();
        }
        _logger?.LogInformation("DisableRobot 完成");
    }

    /// <inheritdoc />
    public void StopMove()
    {
        _logger?.LogInformation("StopMove 开始");
        lock (_stateLock)
        {
            EnsureRobotSetup();
            _runtime.StopMove();
        }
        _logger?.LogInformation("StopMove 完成");
    }

    /// <inheritdoc />
    public ControllerStateSnapshot GetControllerSnapshot()
    {
        return _runtime.GetSnapshot();
    }

    /// <inheritdoc />
    public double GetSpeedRatio()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            return _runtime.GetSpeedRatio();
        }
    }

    /// <inheritdoc />
    public void SetSpeedRatio(double ratio)
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            _runtime.SetSpeedRatio(ratio);
        }
    }

    /// <inheritdoc />
    public void SetIo(int port, bool value, string ioType)
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            _runtime.SetIo(port, value, ioType);
        }
    }

    /// <inheritdoc />
    public bool GetIo(int port, string ioType)
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            return _runtime.GetIo(port, ioType);
        }
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetNearestIk(IReadOnlyList<double> pose, IReadOnlyList<double> seed)
    {
        ArgumentNullException.ThrowIfNull(pose);
        ArgumentNullException.ThrowIfNull(seed);

        lock (_stateLock)
        {
            EnsureRobotSetup();
            if (pose.Count != 7)
            {
                throw new ArgumentException("位姿必须是 [x,y,z,qx,qy,qz,qw] 七元数组。", nameof(pose));
            }

            if (seed.Count != GetDegreesOfFreedom())
            {
                throw new ArgumentException("seed 关节数量必须与机器人自由度一致。", nameof(seed));
            }

            throw new NotSupportedException("GetNearestIK 尚未接入逆解求解器。");
        }
    }

    /// <inheritdoc />
    public void SetTcp(double x, double y, double z)
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            _runtime.SetTcp(x, y, z);
        }
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetTcp()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            return _runtime.GetTcp();
        }
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetJointPositions()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            return _runtime.GetJointPositions();
        }
    }

    /// <inheritdoc />
    public void MoveJoint(IReadOnlyList<double> jointPositions)
    {
        ArgumentNullException.ThrowIfNull(jointPositions);

        _logger?.LogInformation("MoveJoint 开始: 目标关节数={JointCount}", jointPositions.Count);
        _logger?.LogDebug("MoveJoint 目标关节: {Joints}", string.Join(", ", jointPositions.Select(j => j.ToString("F4"))));

        lock (_stateLock)
        {
            var robot = RequireActiveRobot();
            EnsureRuntimeEnabled();
            ExecuteMoveJointAndWaitLocked(robot, jointPositions, "MoveJoint");
        }

        _logger?.LogInformation("MoveJoint 完成");
    }

    /// <inheritdoc />
    public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null)
    {
        ArgumentNullException.ThrowIfNull(waypoints);
        options ??= new TrajectoryExecutionOptions();
        if (waypoints.Count == 0)
        {
            throw new ArgumentException("轨迹路点不能为空。", nameof(waypoints));
        }

        _logger?.LogInformation("ExecuteTrajectory 开始: 路点数={WaypointCount}, method={Method}, saveTraj={SaveTraj}",
            waypoints.Count, options.Method, options.SaveTrajectory);
        _logger?.LogDebug("ExecuteTrajectory 路点详情: {Waypoints}",
            string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Select(j => j.ToString("F4")))}]")));

        lock (_stateLock)
        {
            var robot = RequireActiveRobot();
            EnsureRuntimeEnabled();

            // 普通轨迹必须按调用方指定 method 规划，再把规划结果交给运行时执行。
            var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale;
            var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale);
            _logger?.LogInformation(
                "ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}",
                bundle.Result.Method,
                bundle.Result.Duration.TotalSeconds,
                bundle.Result.IsValid,
                bundle.Result.DenseJointTrajectory?.Count ?? 0,
                planningSpeedScale);
            var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
            _runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
        }

        _logger?.LogInformation("ExecuteTrajectory 完成");
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetPose()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            return _runtime.GetPose();
        }
    }

    /// <inheritdoc />
    public void UploadTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
    {
        ArgumentNullException.ThrowIfNull(trajectory);

        _logger?.LogInformation(
            "UploadTrajectory 开始: name={Name}, waypoints={WaypointCount}, shotFlags={ShotCount}",
            trajectory.Name,
            trajectory.Waypoints.Count,
            trajectory.ShotFlags.Count(static f => f));

        lock (_stateLock)
        {
            EnsureRuntimeEnabled();
            _uploadedTrajectories[trajectory.Name] = trajectory;

            var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
            var settings = _robotSettings ?? CreateDefaultRobotSettings();
            _trajectoryStore.Save(robotName, settings, trajectory);
        }

        _logger?.LogInformation("UploadTrajectory 完成: name={Name}", trajectory.Name);
    }

    /// <inheritdoc />
    public IReadOnlyList<string> ListTrajectoryNames()
    {
        lock (_stateLock)
        {
            return _uploadedTrajectories.Keys.ToArray();
        }
    }

    /// <inheritdoc />
    public void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null)
    {
        options ??= new FlyshotExecutionOptions();
        if (string.IsNullOrWhiteSpace(name))
        {
            throw new ArgumentException("轨迹名称不能为空。", nameof(name));
        }

        _logger?.LogInformation(
            "ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}, wait={Wait}",
            name, options.Method, options.MoveToStart, options.UseCache, options.Wait);

        lock (_stateLock)
        {
            var robot = RequireActiveRobot();
            EnsureRuntimeEnabled();

            if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
            {
                _logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹不存在 name={Name}", name);
                throw new InvalidOperationException("FlyShot trajectory does not exist.");
            }

            if (trajectory.Waypoints.Count == 0)
            {
                _logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹无路点 name={Name}", name);
                throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
            }

            // 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
            var settings = RequireRobotSettings();
            var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale);
            ExportFlyshotArtifactsIfRequested(name, options.SaveTrajectory, robot, bundle);
            _logger?.LogInformation(
                "ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}",
                name,
                bundle.Result.Method,
                bundle.Result.Duration.TotalSeconds,
                bundle.Result.TriggerTimeline.Count,
                bundle.Result.UsedCache,
                settings.PlanningSpeedScale);

            if (options.MoveToStart)
            {
                _logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
                ExecuteMoveJointAndWaitLocked(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
            }

            var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
            _runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
            if (options.Wait)
            {
                WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", bundle.Result.Duration);
            }
        }

        _logger?.LogInformation("ExecuteTrajectoryByName 完成: name={Name}", name);
    }

    /// <summary>
    /// 从当前关节位置生成临时 PTP 稠密轨迹并阻塞等待运行时完成，避免后续 J519 目标发生突变。
    /// </summary>
    /// <param name="robot">当前机器人模型。</param>
    /// <param name="targetJointPositions">目标关节位置，单位为弧度。</param>
    /// <param name="operationName">用于日志和超时异常的操作名。</param>
    private void ExecuteMoveJointAndWaitLocked(RobotProfile robot, IReadOnlyList<double> targetJointPositions, string operationName)
    {
        var currentJointPositions = _runtime.GetJointPositions();
        EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
        EnsureJointVector(targetJointPositions, robot.DegreesOfFreedom, nameof(targetJointPositions));

        var speedRatio = _runtime.GetSnapshot().SpeedRatio;
        var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, targetJointPositions, speedRatio, _logger);
        _logger?.LogInformation(
            "{OperationName} PTP规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
            operationName,
            speedRatio,
            moveResult.Duration.TotalSeconds,
            moveResult.DenseJointTrajectory?.Count ?? 0);

        _runtime.ExecuteTrajectory(moveResult, targetJointPositions);
        WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
    }

    /// <summary>
    /// 等待运行时报告当前运动结束，用于把 move_to_start 与正式飞拍轨迹串行化。
    /// </summary>
    /// <param name="operationName">用于日志和超时异常的操作名。</param>
    /// <param name="plannedDuration">规划运动时长。</param>
    private void WaitForRuntimeMotionComplete(string operationName, TimeSpan plannedDuration)
    {
        var timeout = ResolveMotionCompletionTimeout(plannedDuration);
        var deadline = DateTimeOffset.UtcNow.Add(timeout);

        while (true)
        {
            if (!_runtime.GetSnapshot().IsInMotion)
            {
                _logger?.LogInformation("{OperationName} 运动完成", operationName);
                return;
            }

            if (DateTimeOffset.UtcNow >= deadline)
            {
                throw new TimeoutException($"{operationName} 等待运动完成超时，planned={plannedDuration.TotalSeconds:F3}s, timeout={timeout.TotalSeconds:F3}s。");
            }

            Thread.Sleep(TimeSpan.FromMilliseconds(10));
        }
    }

    /// <summary>
    /// 根据规划时长推导等待超时，给真机通信和状态更新留出余量。
    /// </summary>
    /// <param name="plannedDuration">规划运动时长。</param>
    /// <returns>等待运行时完成的最大时长。</returns>
    private static TimeSpan ResolveMotionCompletionTimeout(TimeSpan plannedDuration)
    {
        var timeoutSeconds = Math.Max(5.0, plannedDuration.TotalSeconds * 3.0 + 2.0);
        return TimeSpan.FromSeconds(timeoutSeconds);
    }

    /// <inheritdoc />
    public void SaveTrajectoryInfo(string name, string method = "icsp")
    {
        if (string.IsNullOrWhiteSpace(name))
        {
            throw new ArgumentException("轨迹名称不能为空。", nameof(name));
        }

        _logger?.LogInformation("SaveTrajectoryInfo 开始: name={Name}, method={Method}", name, method);

        lock (_stateLock)
        {
            var robot = RequireActiveRobot();
            if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
            {
                _logger?.LogWarning("SaveTrajectoryInfo 失败: 轨迹不存在 name={Name}", name);
                throw new InvalidOperationException("FlyShot trajectory does not exist.");
            }

            // 先通过规划校验避免静默接受非法参数，同时把轨迹信息强制刷写到本地 JSON。
            var planningSettings = RequireRobotSettings();
            var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
                robot,
                trajectory,
                new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method),
                planningSettings,
                planningSettings.PlanningSpeedScale);
            ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle);

            var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
            var settings = _robotSettings ?? CreateDefaultRobotSettings();
            _trajectoryStore.Save(robotName, settings, trajectory);
        }

        _logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name);
    }

    /// <inheritdoc />
    public bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false)
    {
        if (string.IsNullOrWhiteSpace(name))
        {
            throw new ArgumentException("轨迹名称不能为空。", nameof(name));
        }

        _logger?.LogInformation("IsFlyshotTrajectoryValid 开始: name={Name}, method={Method}", name, method);

        lock (_stateLock)
        {
            var robot = RequireActiveRobot();
            if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
            {
                _logger?.LogWarning("IsFlyshotTrajectoryValid 失败: 轨迹不存在 name={Name}", name);
                throw new InvalidOperationException("FlyShot trajectory does not exist.");
            }

            var planningSettings = RequireRobotSettings();
            var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
                robot,
                trajectory,
                new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
                planningSettings,
                planningSettings.PlanningSpeedScale);
            ExportFlyshotArtifactsIfRequested(name, saveTrajectory, robot, bundle);

            duration = bundle.Result.Duration;
            _logger?.LogInformation(
                "IsFlyshotTrajectoryValid 结果: name={Name}, valid={Valid}, duration={Duration}s",
                name, bundle.Result.IsValid, duration.TotalSeconds);
            return bundle.Result.IsValid;
        }
    }

    /// <inheritdoc />
    public void DeleteTrajectory(string name)
    {
        if (string.IsNullOrWhiteSpace(name))
        {
            throw new ArgumentException("轨迹名称不能为空。", nameof(name));
        }

        _logger?.LogInformation("DeleteTrajectory 开始: name={Name}", name);

        lock (_stateLock)
        {
            if (!_uploadedTrajectories.Remove(name))
            {
                _logger?.LogWarning("DeleteTrajectory 失败: 轨迹不存在 name={Name}", name);
                throw new InvalidOperationException("DeleteFlyShotTraj failed");
            }

            var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
            _trajectoryStore.Delete(robotName, name);
        }

        _logger?.LogInformation("DeleteTrajectory 完成: name={Name}", name);
    }

    /// <inheritdoc />
    public string GetRobotName()
    {
        lock (_stateLock)
        {
            return _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
        }
    }

    /// <inheritdoc />
    public int GetDegreesOfFreedom()
    {
        lock (_stateLock)
        {
            return _activeRobotProfile?.DegreesOfFreedom ?? throw new InvalidOperationException("Robot has not been setup.");
        }
    }

    /// <summary>
    /// 获取当前机器人配置，未初始化时抛出兼容错误。
    /// </summary>
    /// <returns>当前机器人配置。</returns>
    private RobotProfile RequireActiveRobot()
    {
        return _activeRobotProfile ?? throw new InvalidOperationException("Robot has not been setup.");
    }

    /// <summary>
    /// 获取当前机器人兼容配置；未加载旧配置时回退到现场默认值。
    /// </summary>
    /// <returns>当前机器人配置。</returns>
    private CompatibilityRobotSettings RequireRobotSettings()
    {
        return _robotSettings ?? CreateDefaultRobotSettings();
    }

    /// <summary>
    /// 校验机器人已经完成初始化。
    /// </summary>
    private void EnsureRobotSetup()
    {
        _ = RequireActiveRobot();
    }

    /// <summary>
    /// 校验运行时已经处于可执行状态。
    /// </summary>
    private void EnsureRuntimeEnabled()
    {
        EnsureRobotSetup();
        if (!_runtime.GetSnapshot().IsEnabled)
        {
            throw new InvalidOperationException("Robot has not been enabled.");
        }
    }

    /// <summary>
    /// 校验关节向量与当前机器人自由度一致，且所有值都是有限数值。
    /// </summary>
    /// <param name="joints">待校验关节向量，单位为弧度。</param>
    /// <param name="expectedCount">期望自由度。</param>
    /// <param name="paramName">调用方参数名。</param>
    private static void EnsureJointVector(IReadOnlyList<double> joints, int expectedCount, string paramName)
    {
        if (joints.Count != expectedCount)
        {
            throw new ArgumentException($"关节数量必须为 {expectedCount}。", paramName);
        }

        for (var index = 0; index < joints.Count; index++)
        {
            var value = joints[index];
            if (double.IsNaN(value) || double.IsInfinity(value))
            {
                throw new ArgumentOutOfRangeException(paramName, $"第 {index} 个关节值必须是有限数值。");
            }
        }
    }

    /// <summary>
    /// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
    /// </summary>
    /// <param name="name">飞拍轨迹名称。</param>
    /// <param name="saveTrajectory">是否导出规划结果点位。</param>
    /// <param name="robot">当前机器人模型。</param>
    /// <param name="bundle">规划结果包。</param>
    private void ExportFlyshotArtifactsIfRequested(
        string name,
        bool saveTrajectory,
        RobotProfile robot,
        PlannedExecutionBundle bundle)
    {
        if (!saveTrajectory)
        {
            return;
        }

        var speedRatio = _runtime.GetSnapshot().SpeedRatio;
        _artifactWriter.WriteUploadedFlyshot(name, robot, bundle, speedRatio);
    }

    /// <summary>
    /// 尝试从配置根目录加载 RobotConfig.json 获取机器人配置；失败时返回 null。
    /// </summary>
    /// <returns>加载到的机器人配置，或 null。</returns>
    private CompatibilityRobotSettings? TryLoadRobotSettings()
    {
        foreach (var root in EnumerateRobotConfigRoots())
        {
            try
            {
                // 运行配置根本身已经是 Config 目录，这里用绝对路径避免再次追加 Config。
                var configPath = Path.Combine(root, "RobotConfig.json");
                var loaded = _configLoader.Load(configPath, root);
                return loaded.Robot;
            }
            catch
            {
                // 单个候选根目录加载失败时继续尝试下一个兼容入口。
            }
        }

        return null;
    }

    /// <summary>
    /// 枚举 RobotConfig.json 的配置根目录，运行目录 Config 优先，旧父工作区仅在显式配置时参与。
    /// </summary>
    /// <returns>待尝试的配置根目录列表。</returns>
    private IEnumerable<string> EnumerateRobotConfigRoots()
    {
        yield return _options.ResolveConfigRoot();

        var legacyWorkspaceRoot = _options.ResolveLegacyWorkspaceRoot();
        if (legacyWorkspaceRoot is not null)
        {
            yield return legacyWorkspaceRoot;
        }
    }

    /// <summary>
    /// 构造与旧现场默认行为一致的机器人兼容配置。
    /// </summary>
    /// <returns>默认机器人配置。</returns>
    private static CompatibilityRobotSettings CreateDefaultRobotSettings()
    {
        return new CompatibilityRobotSettings(
            useDo: false,
            ioAddresses: Array.Empty<int>(),
            ioKeepCycles: 2,
            accLimitScale: 1.0,
            jerkLimitScale: 1.0,
            adaptIcspTryNum: 5);
    }

}