using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning.Sampling;
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 object _motionLock = 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);
            // RobotConfig.json 的 speed_ratio 是启动默认值；在线 set_speedRatio 仍可覆盖后续执行。
            _runtime.SetSpeedRatio(robotSettings.SpeedRatio);
            _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}, speedRatio={SpeedRatio}, 恢复轨迹数={TrajCount}",
            robotName,
            robotProfile.DegreesOfFreedom,
            robotSettings.AccLimitScale,
            robotSettings.JerkLimitScale,
            robotSettings.SpeedRatio,
            _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();
        }

        // DisableRobot 是中断/控制命令，不能被长时间运动串行锁挡住。
        _runtime.DisableRobot();
        _logger?.LogInformation("DisableRobot 完成");
    }

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

        // StopMove 必须能在运动执行期间直接进入 runtime 取消发送任务。
        _runtime.StopMove();
        _logger?.LogInformation("StopMove 完成");
    }

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

    /// <inheritdoc />
    public ControllerClientStatusSnapshotMetadata GetStatusSnapshotMetadata()
    {
        lock (_stateLock)
        {
            var isSetup = _activeRobotProfile is not null;
            return new ControllerClientStatusSnapshotMetadata(
                isSetup,
                isSetup ? _configuredRobotName : null,
                isSetup ? _activeRobotProfile!.DegreesOfFreedom : 0,
                isSetup ? _uploadedTrajectories.Keys.ToArray() : Array.Empty<string>(),
                GetServerVersion(),
                GetClientVersion());
        }
    }

    /// <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"))));

        RobotProfile robot;
        lock (_stateLock)
        {
            robot = RequireActiveRobot();
            EnsureRuntimeEnabled();
        }

        lock (_motionLock)
        {
            ExecuteMoveJointAndWait(robot, jointPositions, "MoveJoint");
        }

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

    /// <inheritdoc />
    public void MovePose(IReadOnlyList<double> pose)
    {
        ArgumentNullException.ThrowIfNull(pose);

        _logger?.LogInformation("MovePose 开始: 目标位姿维度={PoseCount}", pose.Count);
        _logger?.LogDebug("MovePose 目标位姿: {Pose}", string.Join(", ", pose.Select(v => v.ToString("F4"))));

        RobotProfile robot;
        lock (_stateLock)
        {
            robot = RequireActiveRobot();
            EnsureRuntimeEnabled();
        }

        lock (_motionLock)
        {
            ExecuteMovePoseAndWait(robot, pose, "MovePose");
        }

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

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

        RobotProfile robot;
        CompatibilityRobotSettings settings;
        lock (_stateLock)
        {
            robot = RequireActiveRobot();
            EnsureRuntimeEnabled();
            settings = RequireRobotSettings();
        }

        lock (_motionLock)
        {
            // 普通轨迹必须按调用方指定 method 规划，再把规划结果交给运行时执行。
            var planningSpeedScale = settings.PlanningSpeedScale;
            var speedRatio = _runtime.GetSnapshot().SpeedRatio;
            var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale, speedRatio);
            _logger?.LogInformation(
                "ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}",
                bundle.Result.Method,
                bundle.Result.Duration.TotalSeconds,
                bundle.Result.IsValid,
                bundle.Result.DenseJointTrajectory?.Count ?? 0,
                planningSpeedScale,
                speedRatio);
            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));

        string robotName;
        CompatibilityRobotSettings settings;
        lock (_stateLock)
        {
            EnsureRuntimeEnabled();
            _uploadedTrajectories[trajectory.Name] = trajectory;

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

        // RobotConfig.json 持久化是文件 I/O，放在状态锁外，避免状态页轮询被磁盘写入拖住。
        _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);

        RobotProfile robot;
        CompatibilityRobotSettings settings;
        ControllerClientCompatUploadedTrajectory trajectory;
        lock (_stateLock)
        {
            robot = RequireActiveRobot();
            EnsureRuntimeEnabled();

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

            // 飞拍执行只拿上传轨迹的瞬时副本，后续规划/导出都不再依赖字典锁。
            trajectory = CloneUploadedTrajectory(uploadedTrajectory);
            if (trajectory.Waypoints.Count == 0)
            {
                _logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹无路点 name={Name}", name);
                throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
            }

            settings = RequireRobotSettings();
        }

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

            if (options.MoveToStart)
            {
                _logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
                ExecuteMoveJointAndWait(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
                EnsureFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name);
            }
            else
            {
                // 正式飞拍前必须确认机器人反馈已经在轨迹起点附近，避免 J519 目标突变。
                if (!IsFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name))
                {
                    return;
                }
            }

            var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
            _runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
            if (options.Wait)
            {
                var executionDuration = bundle.PreparedExecution is null
                    ? bundle.Result.Duration
                    : TimeSpan.FromSeconds(bundle.PreparedExecution.FinalDurationSeconds);
                WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", executionDuration);
            }
        }

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

    /// <summary>
    /// 从当前关节位置生成临时 PTP 稠密轨迹并阻塞等待运行时完成，避免后续 J519 目标发生突变。
    /// </summary>
    /// <param name="robot">当前机器人模型。</param>
    /// <param name="targetJointPositions">目标关节位置，单位为弧度。</param>
    /// <param name="operationName">用于日志和超时异常的操作名。</param>
    private void ExecuteMoveJointAndWait(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>
    /// 从当前 TCP 位姿生成临时直角 PTP 稠密轨迹并阻塞等待运行时完成。
    /// </summary>
    /// <param name="robot">当前机器人模型，用于取得 J519 伺服周期。</param>
    /// <param name="targetPose">目标位姿 [x,y,z,w,p,r]，单位为 mm/deg。</param>
    /// <param name="operationName">用于日志和超时异常的操作名。</param>
    private void ExecuteMovePoseAndWait(RobotProfile robot, IReadOnlyList<double> targetPose, string operationName)
    {
        var currentPose = NormalizeRuntimePose(_runtime.GetPose());
        EnsurePoseVector(targetPose, nameof(targetPose));

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

        _runtime.ExecuteCartesianTrajectory(moveResult, targetPose);
        WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
    }

    /// <summary>
    /// 校验当前反馈是否接近飞拍起点；不接近时直接抛出兼容错误。
    /// </summary>
    private void EnsureFeedbackNearFlyshotStart(IReadOnlyList<double> targetJointPositions, string name)
    {
        if (!IsFeedbackNearFlyshotStart(targetJointPositions, name))
        {
            throw new InvalidOperationException("Robot feedback is not near flyshot start.");
        }
    }

    /// <summary>
    /// 检查当前机械臂关节反馈与计划轨迹第一个点之间的差异。
    /// </summary>
    private bool IsFeedbackNearFlyshotStart(IReadOnlyList<double> targetJointPositions, string name)
    {
        var currentJointPositions = _runtime.GetJointPositions();
        var diff = currentJointPositions.Zip(targetJointPositions, (current, target) => Math.Abs(current - target)).Sum();
        if (diff <= 0.01)
        {
            return true;
        }

        _logger?.LogWarning("ExecuteTrajectoryByName 当前关节坐标与计划轨迹的第一个点之间的差异过大 name={Name}, diff={Diff}", name, diff);
        return false;
    }

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

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

            // 先通过规划校验避免静默接受非法参数，同时把轨迹信息强制刷写到本地 JSON。
            // 保存轨迹信息会执行规划和文件导出，先复制上传数据再释放状态锁。
            trajectory = CloneUploadedTrajectory(uploadedTrajectory);
            planningSettings = RequireRobotSettings();
        }

        var speedRatio = _runtime.GetSnapshot().SpeedRatio;
        var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
            robot,
            trajectory,
            new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
            planningSettings,
            planningSettings.PlanningSpeedScale,
            speedRatio);
        bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
        _logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
        ExportFlyshotArtifactsIfRequested(
            name,
            saveTrajectory: true,
            robot,
            trajectory,
            new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
            planningSettings,
            bundle,
            planningSettings.PlanningSpeedScale,
            speedRatio);

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

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

            // 有效性检查只消费当前快照，不要求和后续上传/删除形成长事务。
            trajectory = CloneUploadedTrajectory(uploadedTrajectory);
            planningSettings = RequireRobotSettings();
        }

        var speedRatio = _runtime.GetSnapshot().SpeedRatio;
        var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
            robot,
            trajectory,
            new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
            planningSettings,
            planningSettings.PlanningSpeedScale,
            speedRatio);
        bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
        ExportFlyshotArtifactsIfRequested(
            name,
            saveTrajectory,
            robot,
            trajectory,
            new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
            planningSettings,
            bundle,
            planningSettings.PlanningSpeedScale,
            speedRatio);

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

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

            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>
    /// <param name="trajectory">待复制的上传轨迹。</param>
    /// <returns>上传轨迹副本。</returns>
    private static ControllerClientCompatUploadedTrajectory CloneUploadedTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
    {
        return new ControllerClientCompatUploadedTrajectory(
            trajectory.Name,
            trajectory.Waypoints,
            trajectory.ShotFlags,
            trajectory.OffsetValues,
            trajectory.AddressGroups);
    }

    /// <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>
    /// 校验直角位姿为 [x,y,z,w,p,r] 六维有限数。
    /// </summary>
    /// <param name="pose">待校验位姿，单位为 mm/deg。</param>
    /// <param name="paramName">调用方参数名。</param>
    private static void EnsurePoseVector(IReadOnlyList<double> pose, string paramName)
    {
        if (pose.Count != 6)
        {
            throw new ArgumentException("位姿必须为 [x,y,z,w,p,r] 六维数组。", paramName);
        }

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

    /// <summary>
    /// 将运行时位姿快照归一化为 J519 直角命令需要的 [x,y,z,w,p,r] 六维。
    /// </summary>
    /// <param name="pose">运行时返回的位姿数组。</param>
    /// <returns>六维直角位姿。</returns>
    private static IReadOnlyList<double> NormalizeRuntimePose(IReadOnlyList<double> pose)
    {
        if (pose.Count < 6)
        {
            throw new InvalidOperationException("Runtime pose must contain at least [x,y,z,w,p,r].");
        }

        var normalized = pose.Take(6).ToArray();
        EnsurePoseVector(normalized, nameof(pose));
        return normalized;
    }

    /// <summary>
    /// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
    /// </summary>
    /// <param name="name">飞拍轨迹名称。</param>
    /// <param name="saveTrajectory">是否导出规划结果点位。</param>
    /// <param name="robot">当前机器人模型。</param>
    /// <param name="uploaded">已上传的飞拍轨迹。</param>
    /// <param name="options">本次规划选项。</param>
    /// <param name="settings">当前机器人兼容设置。</param>
    /// <param name="executionBundle">运行时和 ActualSend 诊断使用的规划结果包。</param>
    /// <param name="planningSpeedScale">规划速度倍率。</param>
    /// <param name="speedRatio">本次运行时速度倍率。</param>
    private void ExportFlyshotArtifactsIfRequested(
        string name,
        bool saveTrajectory,
        RobotProfile robot,
        ControllerClientCompatUploadedTrajectory uploaded,
        FlyshotExecutionOptions options,
        CompatibilityRobotSettings settings,
        PlannedExecutionBundle executionBundle,
        double planningSpeedScale,
        double speedRatio)
    {
        if (!saveTrajectory)
        {
            return;
        }

        // 旧格式 Joint/Cart 导出必须只反映 planning_speed_scale；ActualSend* 才反映当前运行时 speedRatio。
        var exportPlanningBundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
            robot,
            uploaded,
            new FlyshotExecutionOptions(
                moveToStart: options.MoveToStart,
                useCache: false,
                saveTrajectory: options.SaveTrajectory,
                method: options.Method,
                wait: options.Wait),
            settings,
            planningSpeedScale,
            speedRatio: 1.0);
        _artifactWriter.WriteUploadedFlyshot(name, robot, exportPlanningBundle, executionBundle, speedRatio);
    }

    /// <summary>
    /// 为飞拍链路预先构建最终发送队列，确保运行时只消费已经离散校验通过的 8ms 点列。
    /// </summary>
    private static PlannedExecutionBundle PrepareFlyshotExecutionBundle(
        RobotProfile robot,
        PlannedExecutionBundle bundle,
        double speedRatio)
    {
        var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build(
            robot,
            bundle.ExecutionTrajectory,
            bundle.Result,
            robot.ServoPeriod.TotalSeconds,
            requestedSpeedRatio: speedRatio,
            samplingSpeedRatio: 1.0);
        var preparedResult = new TrajectoryResult(
            programName: bundle.Result.ProgramName,
            method: bundle.Result.Method,
            isValid: bundle.Result.IsValid,
            duration: bundle.Result.Duration,
            shotEvents: bundle.Result.ShotEvents,
            triggerTimeline: bundle.Result.TriggerTimeline,
            artifacts: bundle.Result.Artifacts,
            failureReason: bundle.Result.FailureReason,
            usedCache: bundle.Result.UsedCache,
            originalWaypointCount: bundle.Result.OriginalWaypointCount,
            plannedWaypointCount: bundle.Result.PlannedWaypointCount,
            triggerSampleIndexOffsetCycles: bundle.Result.TriggerSampleIndexOffsetCycles,
            denseJointTrajectory: bundle.Result.DenseJointTrajectory,
            preparedFlyshotExecution: preparedExecution);
        return new PlannedExecutionBundle(
            bundle.PlannedTrajectory,
            bundle.ShotTimeline,
            preparedResult,
            preparedExecution,
            bundle.ExecutionTrajectory);
    }

    /// <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,
            triggerSampleIndexOffsetCycles: 7,
            accLimitScale: 1.0,
            jerkLimitScale: 1.0,
            adaptIcspTryNum: 5);
    }

}