FlyShotHost/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs

using Flyshot.Core.Domain;
using Flyshot.Runtime.Common;

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 RobotProfile? _activeRobotProfile;
    private string? _configuredRobotName;
    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>
    public ControllerClientCompatService(
        ControllerClientCompatOptions options,
        ControllerClientCompatRobotCatalog robotCatalog,
        IControllerRuntime runtime,
        ControllerClientTrajectoryOrchestrator trajectoryOrchestrator)
    {
        _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));
    }

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

    /// <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)
    {
        var robotProfile = _robotCatalog.LoadProfile(robotName);

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

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

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

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

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

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

    /// <inheritdoc />
    public void StopMove()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            _runtime.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);

        lock (_stateLock)
        {
            EnsureRobotSetup();
            _runtime.ExecuteTrajectory(CreateImmediateMoveResult(), jointPositions);
        }
    }

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

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

            // 普通轨迹必须按调用方指定 method 规划，再把规划结果交给运行时执行。
            var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options);
            var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
            _runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
        }
    }

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

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

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

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

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

            if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
            {
                throw new InvalidOperationException("FlyShot trajectory does not exist.");
            }

            if (trajectory.Waypoints.Count == 0)
            {
                throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
            }

            // 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
            var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options);
            if (options.MoveToStart)
            {
                _runtime.ExecuteTrajectory(CreateImmediateMoveResult(), bundle.PlannedTrajectory.PlannedWaypoints[0].Positions);
            }

            var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
            _runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
        }
    }

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

        lock (_stateLock)
        {
            var robot = RequireActiveRobot();
            if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
            {
                throw new InvalidOperationException("FlyShot trajectory does not exist.");
            }

            // 当前阶段没有落地文件导出，先通过 saveTrajectory=true 走规划校验，避免静默接受非法参数。
            _ = _trajectoryOrchestrator.PlanUploadedFlyshot(
                robot,
                trajectory,
                new FlyshotExecutionOptions(saveTrajectory: true, method: method));
        }
    }

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

        lock (_stateLock)
        {
            var robot = RequireActiveRobot();
            if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
            {
                throw new InvalidOperationException("FlyShot trajectory does not exist.");
            }

            var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
                robot,
                trajectory,
                new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory));

            duration = bundle.Result.Duration;
            return bundle.Result.IsValid;
        }
    }

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

        lock (_stateLock)
        {
            if (!_uploadedTrajectories.Remove(name))
            {
                throw new InvalidOperationException("DeleteFlyShotTraj failed");
            }
        }
    }

    /// <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>
    private void EnsureRobotSetup()
    {
        _ = RequireActiveRobot();
    }

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

    /// <summary>
    /// 构造 MoveJoint 直达运行时所需的最小合法轨迹结果。
    /// </summary>
    /// <returns>可立即执行的轨迹结果。</returns>
    private static TrajectoryResult CreateImmediateMoveResult()
    {
        return new TrajectoryResult(
            programName: "move-joint",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.Zero,
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 1,
            plannedWaypointCount: 1);
    }
}