using System.Diagnostics;
using Flyshot.Core.Domain;
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc.Protocol;
using Microsoft.Extensions.Logging;

namespace Flyshot.Runtime.Fanuc;

/// <summary>
/// FANUC 控制器运行时，将上层兼容层指令转换为三条真实 Socket 通道的交互。
/// 仿真模式下仍保持内存桩行为，便于离线测试与回退。
/// </summary>
public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
{
    private readonly object _stateLock = new();
    private readonly Dictionary<(string IoType, int Port), bool> _ioValues = new();
    private readonly FanucCommandClient _commandClient;
    private readonly FanucStateClient _stateClient;
    private readonly FanucJ519Client _j519Client;
    private readonly ILogger<FanucControllerRuntime>? _logger;

    private RobotProfile? _robot;
    private string? _robotName;
    private bool? _activeControllerIsSimulation;
    private string? _connectedRobotIp;
    private bool _isEnabled;
    private bool _isInMotion;
    private int _bufferSize;
    private double _speedRatio = 1.0;
    private double[] _tcp = [0.0, 0.0, 0.0];
    private double[] _jointPositions = Array.Empty<double>();
    private double[] _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
    private bool _disposed;
    private CancellationTokenSource? _sendCts;
    private Task? _sendTask;

    /// <summary>
    /// 初始化 FANUC 控制器运行时。
    /// </summary>
    /// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
    public FanucControllerRuntime(ILogger<FanucControllerRuntime>? logger = null)
    {
        _commandClient = new FanucCommandClient();
        _stateClient = new FanucStateClient();
        _j519Client = new FanucJ519Client();
        _logger = logger;
    }

    /// <summary>
    /// 供测试注入 mock 客户端的内部构造函数。
    /// </summary>
    internal FanucControllerRuntime(FanucCommandClient commandClient, FanucStateClient stateClient, FanucJ519Client j519Client, ILogger<FanucControllerRuntime>? logger = null)
    {
        _commandClient = commandClient;
        _stateClient = stateClient;
        _j519Client = j519Client;
        _logger = logger;
    }

    /// <inheritdoc />
    public void ResetRobot(RobotProfile robot, string robotName)
    {
        ArgumentNullException.ThrowIfNull(robot);
        if (string.IsNullOrWhiteSpace(robotName))
        {
            throw new ArgumentException("机器人名称不能为空。", nameof(robotName));
        }

        _logger?.LogInformation("ResetRobot: robotName={RobotName}, dof={Dof}", robotName, robot.DegreesOfFreedom);

        lock (_stateLock)
        {
            DisconnectClients();
            _robot = robot;
            _robotName = robotName;
            _activeControllerIsSimulation = null;
            _connectedRobotIp = null;
            _isEnabled = false;
            _isInMotion = false;
            _bufferSize = 0;
            _speedRatio = 1.0;
            _tcp = [0.0, 0.0, 0.0];
            _jointPositions = new double[robot.DegreesOfFreedom];
            _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
            _ioValues.Clear();
        }
    }

    /// <inheritdoc />
    public void SetActiveController(bool sim)
    {
        _logger?.LogInformation("SetActiveController: sim={Sim}", sim);
        lock (_stateLock)
        {
            EnsureRobotSetup();
            DisconnectClients();
            _activeControllerIsSimulation = sim;
            _connectedRobotIp = null;
            _isEnabled = false;
            _isInMotion = false;
        }
    }

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

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

        lock (_stateLock)
        {
            EnsureActiveControllerSelected();
            if (_activeControllerIsSimulation!.Value)
            {
                _connectedRobotIp = robotIp;
                _isEnabled = false;
                _isInMotion = false;
                _logger?.LogInformation("Connect 完成(仿真): robotIp={RobotIp}", robotIp);
                return;
            }

            // 真机模式：顺序建立三条通道 — 状态 → 命令 → 运动。
            _stateClient.ConnectAsync(robotIp, 10010).GetAwaiter().GetResult();
            _commandClient.ConnectAsync(robotIp, 10012).GetAwaiter().GetResult();
            _j519Client.ConnectAsync(robotIp, 60015).GetAwaiter().GetResult();

            _connectedRobotIp = robotIp;
            _isEnabled = false;
            _isInMotion = false;
        }

        _logger?.LogInformation("Connect 完成(真机): robotIp={RobotIp}, 三条通道已建立", robotIp);
    }

    /// <inheritdoc />
    public void Disconnect()
    {
        _logger?.LogInformation("Disconnect 开始");
        lock (_stateLock)
        {
            EnsureRobotSetup();
            CancelSendTaskLocked();
            DisconnectClients();
            _connectedRobotIp = null;
            _isEnabled = false;
            _isInMotion = false;
        }
        _logger?.LogInformation("Disconnect 完成");
    }

    /// <inheritdoc />
    public void EnableRobot(int bufferSize)
    {
        if (bufferSize <= 0)
        {
            throw new ArgumentOutOfRangeException(nameof(bufferSize), "buffer_size 必须大于 0。");
        }

        _logger?.LogInformation("EnableRobot 开始: bufferSize={BufferSize}, 仿真={IsSim}", bufferSize, _activeControllerIsSimulation);

        lock (_stateLock)
        {
            EnsureConnected();
            _bufferSize = bufferSize;

            if (IsSimulationMode)
            {
                _isEnabled = true;
                _logger?.LogInformation("EnableRobot 完成(仿真)");
                return;
            }

            // 真机模式：走完整 RVBUSTSM 启动序列（与抓包一致）。
            _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
            _commandClient.ResetRobotAsync().GetAwaiter().GetResult();
            _commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult();
            _commandClient.StartProgramAsync("RVBUSTSM").GetAwaiter().GetResult();

            _j519Client.StartMotion();
            _isEnabled = true;
        }

        _logger?.LogInformation("EnableRobot 完成(真机): RVBUSTSM 已启动, J519 运动循环已开启");
    }

    /// <inheritdoc />
    public void DisableRobot()
    {
        _logger?.LogInformation("DisableRobot 开始");
        lock (_stateLock)
        {
            EnsureRobotSetup();
            CancelSendTaskLocked();
            if (!IsSimulationMode)
            {
                _j519Client.StopMotionAsync().GetAwaiter().GetResult();
                _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
            }

            _isEnabled = false;
            _isInMotion = false;
        }
        _logger?.LogInformation("DisableRobot 完成");
    }

    /// <inheritdoc />
    public void StopMove()
    {
        _logger?.LogInformation("StopMove 开始");
        lock (_stateLock)
        {
            EnsureRobotSetup();
            CancelSendTaskLocked();
            if (!IsSimulationMode)
            {
                _j519Client.StopMotionAsync().GetAwaiter().GetResult();
            }

            _isInMotion = false;
        }
        _logger?.LogInformation("StopMove 完成");
    }

    /// <inheritdoc />
    public double GetSpeedRatio()
    {
        lock (_stateLock)
        {
            EnsureConnected();
            if (!IsSimulationMode)
            {
                var response = _commandClient.GetSpeedRatioAsync().GetAwaiter().GetResult();
                _speedRatio = response.Ratio;
            }

            return _speedRatio;
        }
    }

    /// <inheritdoc />
    public void SetSpeedRatio(double ratio)
    {
        if (double.IsNaN(ratio) || double.IsInfinity(ratio))
        {
            throw new ArgumentOutOfRangeException(nameof(ratio), "ratio 必须是有限数值。");
        }

        _logger?.LogInformation("SetSpeedRatio: ratio={Ratio}", ratio);

        lock (_stateLock)
        {
            EnsureConnected();
            var clampedRatio = Math.Clamp(ratio, 0.0, 1.0);
            if (!IsSimulationMode)
            {
                _commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult();
            }

            _speedRatio = clampedRatio;
        }

        _logger?.LogInformation("SetSpeedRatio 完成: clampedRatio={ClampedRatio}", _speedRatio);
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetTcp()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            if (_activeControllerIsSimulation is false && !string.IsNullOrWhiteSpace(_connectedRobotIp))
            {
                var response = _commandClient.GetTcpAsync(1).GetAwaiter().GetResult();
                _tcp = response.Pose.Take(3).ToArray();
            }

            return _tcp.ToArray();
        }
    }

    /// <inheritdoc />
    public void SetTcp(double x, double y, double z)
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            if (_activeControllerIsSimulation is false)
            {
                EnsureConnected();
                _commandClient.SetTcpAsync(1, CreateTcpPose(x, y, z)).GetAwaiter().GetResult();
            }

            _tcp = [x, y, z];
        }
    }

    /// <inheritdoc />
    public bool GetIo(int port, string ioType)
    {
        if (port < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(port), "IO 端口不能为负数。");
        }

        var normalizedIoType = NormalizeIoType(ioType);

        lock (_stateLock)
        {
            EnsureConnected();
            if (!IsSimulationMode)
            {
                var response = _commandClient.GetIoAsync(port, normalizedIoType).GetAwaiter().GetResult();
                _ioValues[(normalizedIoType, port)] = response.Value;
                return response.Value;
            }

            return _ioValues.TryGetValue((normalizedIoType, port), out var value) && value;
        }
    }

    /// <inheritdoc />
    public void SetIo(int port, bool value, string ioType)
    {
        if (port < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(port), "IO 端口不能为负数。");
        }

        var normalizedIoType = NormalizeIoType(ioType);

        lock (_stateLock)
        {
            EnsureEnabled();
            if (!IsSimulationMode)
            {
                _commandClient.SetIoAsync(port, value, normalizedIoType).GetAwaiter().GetResult();
            }

            _ioValues[(normalizedIoType, port)] = value;
        }
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetJointPositions()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            if (!IsSimulationMode)
            {
                var frame = GetFreshStateFrame();
                if (frame?.JointDegrees.Count >= _jointPositions.Length)
                {
                    return frame.JointDegrees.Take(_jointPositions.Length).Select(v => (double)v).ToArray();
                }
            }

            return _jointPositions.ToArray();
        }
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetPose()
    {
        lock (_stateLock)
        {
            EnsureRobotSetup();
            if (!IsSimulationMode)
            {
                var frame = GetFreshStateFrame();
                if (frame?.CartesianPose.Count >= 6)
                {
                    return frame.CartesianPose.Take(6).Select(v => (double)v).ToArray();
                }
            }

            return _pose.ToArray();
        }
    }

    /// <inheritdoc />
    public ControllerStateSnapshot GetSnapshot()
    {
        lock (_stateLock)
        {
            var jointPositions = _jointPositions;
            var cartesianPose = _pose;
            var isInMotion = _isInMotion;
            IReadOnlyList<uint> stateTailWords = Array.Empty<uint>();
            byte? j519Status = null;
            uint? j519Sequence = null;
            bool? j519AcceptsCommand = null;
            bool? j519ReceivedCommand = null;
            bool? j519SystemReady = null;
            bool? j519RobotInMotion = null;

            if (!IsSimulationMode)
            {
                var frame = GetFreshStateFrame();
                if (frame is not null)
                {
                    if (frame.JointDegrees.Count >= jointPositions.Length)
                    {
                        jointPositions = frame.JointDegrees.Take(jointPositions.Length).Select(v => (double)v).ToArray();
                    }

                    if (frame.CartesianPose.Count >= 6)
                    {
                        cartesianPose = frame.CartesianPose.Take(6).Select(v => (double)v).ToArray();
                    }

                    stateTailWords = frame.RawTailWords.ToArray();
                }

                var j519Response = _j519Client.GetLatestResponse();
                if (j519Response is not null)
                {
                    isInMotion = j519Response.RobotInMotion;
                    j519Status = j519Response.Status;
                    j519Sequence = j519Response.Sequence;
                    j519AcceptsCommand = j519Response.AcceptsCommand;
                    j519ReceivedCommand = j519Response.ReceivedCommand;
                    j519SystemReady = j519Response.SystemReady;
                    j519RobotInMotion = j519Response.RobotInMotion;
                }
            }

            return new ControllerStateSnapshot(
                capturedAt: DateTimeOffset.UtcNow,
                connectionState: ResolveConnectionState(),
                isEnabled: _isEnabled,
                isInMotion: isInMotion,
                speedRatio: _speedRatio,
                jointPositions: jointPositions,
                cartesianPose: cartesianPose,
                activeAlarms: Array.Empty<RuntimeAlarm>(),
                stateTailWords: stateTailWords,
                j519Status: j519Status,
                j519Sequence: j519Sequence,
                j519AcceptsCommand: j519AcceptsCommand,
                j519ReceivedCommand: j519ReceivedCommand,
                j519SystemReady: j519SystemReady,
                j519RobotInMotion: j519RobotInMotion);
        }
    }

    /// <inheritdoc />
    public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
    {
        ArgumentNullException.ThrowIfNull(result);
        ArgumentNullException.ThrowIfNull(finalJointPositions);

        _logger?.LogInformation(
            "ExecuteTrajectory 开始: program={ProgramName}, method={Method}, 时长={Duration}s, 稠密采样={HasDense}, 触发事件数={TriggerCount}, speedRatio={SpeedRatio}",
            result.ProgramName, result.Method, result.Duration.TotalSeconds,
            result.DenseJointTrajectory is not null, result.TriggerTimeline.Count, _speedRatio);

        lock (_stateLock)
        {
            EnsureEnabled();
            EnsureValidTrajectory(result);
            EnsureJointCount(finalJointPositions.Count);
            CancelSendTaskLocked();

            if (!IsSimulationMode && result.DenseJointTrajectory is not null)
            {
                if (_speedRatio <= 0.0)
                {
                    throw new InvalidOperationException("Speed ratio must be greater than zero for dense J519 execution.");
                }

                EnsureJ519ReadyForDenseExecution();

                // 真机模式且存在稠密路点：启动后台高精度发送任务。
                _isInMotion = true;
                _sendCts = new CancellationTokenSource();
                var ct = _sendCts.Token;
                _sendTask = Task.Run(() => SendDenseTrajectory(result, finalJointPositions, ct), ct);
                _logger?.LogInformation("ExecuteTrajectory 已启动后台稠密发送任务");
                return;
            }

            if (!IsSimulationMode)
            {
                // 真机模式无稠密路点：回退到单点收敛。
                var command = new FanucJ519Command(
                    sequence: 0,
                    targetJoints: finalJointPositions.Select(j => (double)j).ToArray());
                _j519Client.UpdateCommand(command);
            }

            _isInMotion = true;
            _jointPositions = finalJointPositions.ToArray();
            _isInMotion = false;
            _logger?.LogInformation("ExecuteTrajectory 完成(单点模式)");
        }
    }

    /// <summary>
    /// 释放运行时持有的所有 Socket 客户端。
    /// </summary>
    public void Dispose()
    {
        if (_disposed)
        {
            return;
        }

        _disposed = true;
        CancelSendTaskLocked();
        DisconnectClients();
        _commandClient.Dispose();
        _stateClient.Dispose();
        _j519Client.Dispose();
    }

    /// <summary>
    /// 后台高精度发送任务：按 J519 周期发送命令，并按 speed_ratio 推进原始轨迹时间。
    /// </summary>
    private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
    {
        var denseTrajectory = result.DenseJointTrajectory!;
        var triggers = result.TriggerTimeline;
        var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds;
        var speedRatio = _speedRatio;
        var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
        var triggerToleranceSeconds = trajectoryStepSeconds / 2.0;
        var durationSeconds = result.Duration.TotalSeconds;
        var sampleCount = CalculateDenseSendSampleCount(durationSeconds, trajectoryStepSeconds);
        var periodTicks = (long)(servoPeriodSeconds * Stopwatch.Frequency);

        _logger?.LogInformation(
            "SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, speedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
            result.ProgramName, sampleCount, durationSeconds, speedRatio, servoPeriodSeconds * 1000, triggers.Count);

        var stopwatch = Stopwatch.StartNew();
        long nextTick = stopwatch.ElapsedTicks;
        ushort ioValue = 0;
        ushort ioMask = 0;
        int holdRemaining = -1;
        int segmentIndex = 0;
        long logInterval = Math.Max(1, sampleCount / 10);
        int triggerFiredCount = 0;

        try
        {
            for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
            {
                cancellationToken.ThrowIfCancellationRequested();
                nextTick += periodTicks;

                var trajectoryTime = Math.Min(sampleIndex * trajectoryStepSeconds, durationSeconds);
                var joints = SampleDenseJointTrajectoryDegrees(denseTrajectory, trajectoryTime, ref segmentIndex);

                // 递减 IO 保持计数器；若已到期则清零。
                var clearMaskAfterSend = false;
                if (holdRemaining > 0)
                {
                    holdRemaining--;
                }
                else if (holdRemaining == 0)
                {
                    ioValue = 0;
                    holdRemaining = -1;
                    clearMaskAfterSend = true;
                }

                // 检查当前周期是否有新的触发事件。
                if (holdRemaining < 0 && !clearMaskAfterSend)
                {
                    foreach (var trigger in triggers)
                    {
                        if (Math.Abs(trajectoryTime - trigger.TriggerTime) <= triggerToleranceSeconds)
                        {
                            ioMask = ComputeIoValue(trigger.AddressGroup);
                            ioValue = ioMask;
                            holdRemaining = Math.Max(trigger.HoldCycles - 1, 0);
                            triggerFiredCount++;
                            _logger?.LogInformation(
                                "J519 IO触发: time={Time:F4}s, addr=[{Addr}], holdCycles={HoldCycles}",
                                trajectoryTime, string.Join(",", trigger.AddressGroup.Addresses), trigger.HoldCycles);
                            break;
                        }
                    }
                }

                var command = new FanucJ519Command(
                    sequence: 0,
                    targetJoints: joints,
                    writeIoType: 2,
                    writeIoIndex: 1,
                    writeIoMask: ioMask,
                    writeIoValue: ioValue);

                _j519Client.UpdateCommand(command);

                if (clearMaskAfterSend)
                {
                    ioMask = 0;
                }

                // 周期性记录进度（Debug 级别，避免高频 Info 日志）。
                if (sampleIndex > 0 && sampleIndex % logInterval == 0)
                {
                    _logger?.LogDebug(
                        "SendDenseTrajectory 进度: {Percent}% ({Current}/{Total}), time={Time:F4}s",
                        (int)((double)sampleIndex / sampleCount * 100), sampleIndex, sampleCount, trajectoryTime);
                }

                // 高精度忙等待直到下一伺服周期。
                while (stopwatch.ElapsedTicks < nextTick)
                {
                    Thread.SpinWait(1);
                }
            }

            _logger?.LogInformation(
                "SendDenseTrajectory 正常完成: 采样数={SampleCount}, 触发次数={TriggerFiredCount}, 实际耗时={ElapsedMs}ms",
                sampleCount, triggerFiredCount, stopwatch.ElapsedMilliseconds);
        }
        catch (OperationCanceledException)
        {
            _logger?.LogWarning(
                "SendDenseTrajectory 被取消: 已完成 {Percent}% ({Current}/{Total}), 触发次数={TriggerFiredCount}",
                (int)((double)(sampleCount > 0 ? 0 : 0) / sampleCount * 100), 0, sampleCount, triggerFiredCount);
            // 正常取消，轨迹被中断。
        }
        finally
        {
            lock (_stateLock)
            {
                _isInMotion = false;
                _jointPositions = finalJointPositions.ToArray();
            }
        }
    }

    /// <summary>
    /// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发包数。
    /// </summary>
    private static long CalculateDenseSendSampleCount(double durationSeconds, double trajectoryStepSeconds)
    {
        if (durationSeconds < 0.0)
        {
            throw new ArgumentOutOfRangeException(nameof(durationSeconds), "Trajectory duration must be non-negative.");
        }

        if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
        {
            throw new InvalidOperationException("Speed ratio must be greater than zero for dense J519 execution.");
        }

        return (long)Math.Floor((durationSeconds / trajectoryStepSeconds) + 1e-9) + 1;
    }

    /// <summary>
    /// 在稠密关节轨迹上按时间线性插值，并转换成 J519 要求的角度制关节目标。
    /// </summary>
    private static double[] SampleDenseJointTrajectoryDegrees(
        IReadOnlyList<IReadOnlyList<double>> denseTrajectory,
        double trajectoryTime,
        ref int segmentIndex)
    {
        if (denseTrajectory.Count == 0)
        {
            throw new InvalidOperationException("Dense joint trajectory is empty.");
        }

        if (denseTrajectory.Count == 1 || trajectoryTime <= denseTrajectory[0][0])
        {
            return denseTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
        }

        var lastIndex = denseTrajectory.Count - 1;
        if (trajectoryTime >= denseTrajectory[lastIndex][0])
        {
            return denseTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
        }

        while (segmentIndex < lastIndex - 1 && denseTrajectory[segmentIndex + 1][0] < trajectoryTime)
        {
            segmentIndex++;
        }

        var start = denseTrajectory[segmentIndex];
        var end = denseTrajectory[segmentIndex + 1];
        var startTime = start[0];
        var endTime = end[0];
        var segmentDuration = endTime - startTime;
        var alpha = segmentDuration <= 0.0 ? 0.0 : (trajectoryTime - startTime) / segmentDuration;

        var jointCount = start.Count - 1;
        var joints = new double[jointCount];
        for (var index = 0; index < jointCount; index++)
        {
            var radians = start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha);
            joints[index] = RadiansToDegrees(radians);
        }

        return joints;
    }

    /// <summary>
    /// 若已有 J519 响应，则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。
    /// </summary>
    private void EnsureJ519ReadyForDenseExecution()
    {
        var response = _j519Client.GetLatestResponse();
        if (response is null)
        {
            return;
        }

        if (response.AcceptsCommand && response.SystemReady)
        {
            return;
        }

        throw new InvalidOperationException(
            "J519 status is not ready for dense execution: "
            + $"accept_cmd={response.AcceptsCommand}, "
            + $"received_cmd={response.ReceivedCommand}, "
            + $"sysrdy={response.SystemReady}, "
            + $"rbt_inmotion={response.RobotInMotion}, "
            + $"seq={response.Sequence}, "
            + $"status=0x{response.Status:X2}.");
    }

    private static double RadiansToDegrees(double radians)
    {
        return radians * 180.0 / Math.PI;
    }

    /// <summary>
    /// 取消并等待当前后台发送任务，避免旧任务与新轨迹并发。
    /// </summary>
    private void CancelSendTaskLocked()
    {
        _sendCts?.Cancel();

        if (_sendTask is not null)
        {
            try
            {
                _sendTask.Wait(TimeSpan.FromSeconds(2));
            }
            catch (AggregateException)
            {
                // 忽略取消异常。
            }

            _sendTask = null;
        }

        _sendCts?.Dispose();
        _sendCts = null;
    }

    /// <summary>
    /// 把 IO 地址组中的地址号映射为 writeIoValue 的位掩码。
    /// </summary>
    internal static ushort ComputeIoValue(IoAddressGroup group)
    {
        ushort value = 0;
        foreach (var addr in group.Addresses)
        {
            if (addr is >= 0 and < 16)
            {
                value |= (ushort)(1 << addr);
            }
        }

        return value;
    }

    /// <summary>
    /// 判断当前是否处于仿真模式；若尚未选择控制器则抛出异常。
    /// </summary>
    private bool IsSimulationMode
    {
        get
        {
            if (_activeControllerIsSimulation is null)
            {
                return false;
            }

            return _activeControllerIsSimulation.Value;
        }
    }

    /// <summary>
    /// 断开所有真实 Socket 通道，不影响内存状态。
    /// </summary>
    private void DisconnectClients()
    {
        _j519Client.Disconnect();
        _commandClient.Disconnect();
        _stateClient.Disconnect();
    }

    /// <summary>
    /// 归一化 IO 类型字符串，避免调用方大小写差异影响缓存键。
    /// </summary>
    private static string NormalizeIoType(string ioType)
    {
        if (string.IsNullOrWhiteSpace(ioType))
        {
            throw new ArgumentException("IO 类型不能为空。", nameof(ioType));
        }

        return ioType.Trim().ToUpperInvariant();
    }

    /// <summary>
    /// 将 HTTP 层三维 TCP 请求扩展为 FANUC 命令通道需要的 7 维 Pose。
    /// </summary>
    private static double[] CreateTcpPose(double x, double y, double z)
    {
        return [x, y, z, 0.0, 0.0, 0.0, 1.0];
    }

    /// <summary>
    /// 校验轨迹规划结果可执行。
    /// </summary>
    private static void EnsureValidTrajectory(TrajectoryResult result)
    {
        if (!result.IsValid)
        {
            throw new InvalidOperationException(result.FailureReason ?? "Trajectory result is invalid.");
        }
    }

    /// <summary>
    /// 根据当前内部状态生成连接状态标签。
    /// </summary>
    private string ResolveConnectionState()
    {
        if (_robot is null)
        {
            return "NotConfigured";
        }

        if (string.IsNullOrWhiteSpace(_connectedRobotIp))
        {
            return "Disconnected";
        }

        return _activeControllerIsSimulation is false
            ? ResolveRealConnectionState(_stateClient.GetStatus())
            : "Connected";
    }

    /// <summary>
    /// 把真实 10010 状态通道健康度映射为上层快照连接状态。
    /// </summary>
    internal static string ResolveRealConnectionState(FanucStateClientStatus status)
    {
        ArgumentNullException.ThrowIfNull(status);

        return status.State switch
        {
            FanucStateConnectionState.Connected when status.IsFrameStale => "StateTimeout",
            FanucStateConnectionState.Connected => "Connected",
            FanucStateConnectionState.TimedOut => "StateTimeout",
            FanucStateConnectionState.Reconnecting => "Reconnecting",
            FanucStateConnectionState.Connecting => "Connecting",
            _ => "Disconnected",
        };
    }

    /// <summary>
    /// 判断 runtime 是否可以把某个状态通道帧作为当前机器人状态使用。
    /// </summary>
    internal static bool ShouldUseStateFrame(FanucStateClientStatus status)
    {
        ArgumentNullException.ThrowIfNull(status);
        return status.State == FanucStateConnectionState.Connected && !status.IsFrameStale;
    }

    /// <summary>
    /// 获取未超时的状态帧；超时或重连期间不把旧状态作为当前机器人状态使用。
    /// </summary>
    private FanucStateFrame? GetFreshStateFrame()
    {
        var status = _stateClient.GetStatus();
        return ShouldUseStateFrame(status) ? _stateClient.GetLatestFrame() : null;
    }

    /// <summary>
    /// 校验给定关节数组长度与当前机器人自由度一致。
    /// </summary>
    private void EnsureJointCount(int jointCount)
    {
        var expectedJointCount = _robot?.DegreesOfFreedom ?? throw new InvalidOperationException("Robot has not been setup.");
        if (jointCount != expectedJointCount)
        {
            throw new InvalidOperationException($"Expected {expectedJointCount} joints but received {jointCount}.");
        }
    }

    /// <summary>
    /// 校验机器人已经完成初始化。
    /// </summary>
    private void EnsureRobotSetup()
    {
        if (_robot is null || string.IsNullOrWhiteSpace(_robotName))
        {
            throw new InvalidOperationException("Robot has not been setup.");
        }
    }

    /// <summary>
    /// 校验活动控制器已经被选择。
    /// </summary>
    private void EnsureActiveControllerSelected()
    {
        EnsureRobotSetup();
        if (_activeControllerIsSimulation is null)
        {
            throw new InvalidOperationException("Active controller has not been selected.");
        }
    }

    /// <summary>
    /// 校验控制器已经建立连接。
    /// </summary>
    private void EnsureConnected()
    {
        EnsureActiveControllerSelected();
        if (string.IsNullOrWhiteSpace(_connectedRobotIp))
        {
            throw new InvalidOperationException("Controller has not been connected.");
        }
    }

    /// <summary>
    /// 校验机器人已经处于使能态。
    /// </summary>
    private void EnsureEnabled()
    {
        EnsureConnected();
        if (!_isEnabled || _bufferSize <= 0)
        {
            throw new InvalidOperationException("Robot has not been enabled.");
        }
    }
}
