FlyShotHost/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs

using System.Net.Sockets;

namespace Flyshot.Runtime.Fanuc.Protocol;

/// <summary>
/// FANUC TCP 10012 命令通道客户端，提供 Req/Res 同步命令下发能力。
/// </summary>
public sealed class FanucCommandClient : IDisposable
{
    private readonly SemaphoreSlim _sendLock = new(1, 1);
    private TcpClient? _tcpClient;
    private NetworkStream? _stream;
    private bool _disposed;

    /// <summary>
    /// 获取当前是否已建立连接。
    /// </summary>
    public bool IsConnected => _tcpClient?.Connected ?? false;

    /// <summary>
    /// 建立到 FANUC 控制柜 TCP 10012 命令通道的连接。
    /// </summary>
    /// <param name="ip">控制柜 IP 地址。</param>
    /// <param name="port">命令通道端口，默认 10012。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    public async Task ConnectAsync(string ip, int port = 10012, CancellationToken cancellationToken = default)
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        if (string.IsNullOrWhiteSpace(ip))
        {
            throw new ArgumentException("IP 不能为空。", nameof(ip));
        }

        if (_tcpClient is not null)
        {
            throw new InvalidOperationException("命令通道已经连接，请先 Disconnect。");
        }

        _tcpClient = new TcpClient { NoDelay = true };
        await _tcpClient.ConnectAsync(ip, port, cancellationToken).ConfigureAwait(false);
        _stream = _tcpClient.GetStream();
    }

    /// <summary>
    /// 断开命令通道并释放资源。
    /// </summary>
    public void Disconnect()
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        _stream?.Dispose();
        _stream = null;
        _tcpClient?.Dispose();
        _tcpClient = null;
    }

    /// <summary>
    /// 发送通用命令并等待响应。
    /// </summary>
    /// <param name="messageId">命令消息号。</param>
    /// <param name="body">命令业务体。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>原始响应帧。</returns>
    public async Task<byte[]> SendCommandAsync(uint messageId, ReadOnlyMemory<byte> body, CancellationToken cancellationToken = default)
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        if (_stream is null)
        {
            throw new InvalidOperationException("命令通道未连接。");
        }

        await _sendLock.WaitAsync(cancellationToken).ConfigureAwait(false);
        try
        {
            var frame = FanucCommandProtocol.PackFrame(messageId, body.Span);
            await _stream.WriteAsync(frame, cancellationToken).ConfigureAwait(false);

            return await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false);
        }
        finally
        {
            _sendLock.Release();
        }
    }

    /// <summary>
    /// 发送携带程序名的命令并等待响应。
    /// </summary>
    /// <param name="messageId">命令消息号。</param>
    /// <param name="programName">程序名。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>结果响应。</returns>
    public async Task<FanucCommandResultResponse> SendProgramCommandAsync(uint messageId, string programName, CancellationToken cancellationToken = default)
    {
        var frame = FanucCommandProtocol.PackProgramCommand(messageId, programName);
        var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
        return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
    }

    /// <summary>
    /// 停止指定程序。
    /// </summary>
    /// <param name="programName">程序名。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>结果响应。</returns>
    public Task<FanucCommandResultResponse> StopProgramAsync(string programName, CancellationToken cancellationToken = default)
    {
        return SendProgramCommandAsync(FanucCommandMessageIds.StopProgram, programName, cancellationToken);
    }

    /// <summary>
    /// 复位控制器。
    /// </summary>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>结果响应。</returns>
    public async Task<FanucCommandResultResponse> ResetRobotAsync(CancellationToken cancellationToken = default)
    {
        var frame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot);
        var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
        return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
    }

    /// <summary>
    /// 查询指定程序状态。
    /// </summary>
    /// <param name="programName">程序名。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>程序状态响应。</returns>
    public async Task<FanucProgramStatusResponse> GetProgramStatusAsync(string programName, CancellationToken cancellationToken = default)
    {
        var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, programName);
        var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
        return EnsureSuccess(FanucCommandProtocol.ParseProgramStatusResponse(response));
    }

    /// <summary>
    /// 启动指定程序。
    /// </summary>
    /// <param name="programName">程序名。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>结果响应。</returns>
    public Task<FanucCommandResultResponse> StartProgramAsync(string programName, CancellationToken cancellationToken = default)
    {
        return SendProgramCommandAsync(FanucCommandMessageIds.StartProgram, programName, cancellationToken);
    }

    /// <summary>
    /// 读取控制器速度倍率。
    /// </summary>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>速度倍率响应。</returns>
    public async Task<FanucSpeedRatioResponse> GetSpeedRatioAsync(CancellationToken cancellationToken = default)
    {
        var frame = FanucCommandProtocol.PackGetSpeedRatioCommand();
        var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
        return EnsureSuccess(FanucCommandProtocol.ParseSpeedRatioResponse(response));
    }

    /// <summary>
    /// 设置控制器速度倍率。
    /// </summary>
    /// <param name="ratio">目标速度倍率。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>结果响应。</returns>
    public async Task<FanucCommandResultResponse> SetSpeedRatioAsync(double ratio, CancellationToken cancellationToken = default)
    {
        var frame = FanucCommandProtocol.PackSetSpeedRatioCommand(ratio);
        var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
        return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
    }

    /// <summary>
    /// 读取控制器 TCP 位姿。
    /// </summary>
    /// <param name="tcpId">TCP ID。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>TCP 位姿响应。</returns>
    public async Task<FanucTcpResponse> GetTcpAsync(uint tcpId = 1, CancellationToken cancellationToken = default)
    {
        var frame = FanucCommandProtocol.PackGetTcpCommand(tcpId);
        var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
        return EnsureSuccess(FanucCommandProtocol.ParseTcpResponse(response));
    }

    /// <summary>
    /// 设置控制器 TCP 位姿。
    /// </summary>
    /// <param name="tcpId">TCP ID。</param>
    /// <param name="pose">7 维 TCP 位姿。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>结果响应。</returns>
    public async Task<FanucCommandResultResponse> SetTcpAsync(uint tcpId, IReadOnlyList<double> pose, CancellationToken cancellationToken = default)
    {
        var frame = FanucCommandProtocol.PackSetTcpCommand(tcpId, pose);
        var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
        return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
    }

    /// <summary>
    /// 读取控制器 IO。
    /// </summary>
    /// <param name="port">IO 索引。</param>
    /// <param name="ioType">IO 类型字符串。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>IO 读取响应。</returns>
    public async Task<FanucIoResponse> GetIoAsync(int port, string ioType, CancellationToken cancellationToken = default)
    {
        var frame = FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.FromName(ioType), port);
        var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
        return EnsureSuccess(FanucCommandProtocol.ParseIoResponse(response));
    }

    /// <summary>
    /// 设置控制器 IO。
    /// </summary>
    /// <param name="port">IO 索引。</param>
    /// <param name="value">目标 IO 值。</param>
    /// <param name="ioType">IO 类型字符串。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>结果响应。</returns>
    public async Task<FanucCommandResultResponse> SetIoAsync(int port, bool value, string ioType, CancellationToken cancellationToken = default)
    {
        var frame = FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.FromName(ioType), port, value);
        var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
        return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
    }

    /// <summary>
    /// 释放客户端资源。
    /// </summary>
    public void Dispose()
    {
        if (_disposed)
        {
            return;
        }

        _disposed = true;
        _stream?.Dispose();
        _stream = null;
        _tcpClient?.Dispose();
        _tcpClient = null;
        _sendLock.Dispose();
    }

    /// <summary>
    /// 直接发送已封装的帧并读取响应。
    /// </summary>
    private async Task<byte[]> SendRawFrameAsync(byte[] frame, CancellationToken cancellationToken)
    {
        if (_stream is null)
        {
            throw new InvalidOperationException("命令通道未连接。");
        }

        await _sendLock.WaitAsync(cancellationToken).ConfigureAwait(false);
        try
        {
            await _stream.WriteAsync(frame, cancellationToken).ConfigureAwait(false);
            return await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false);
        }
        finally
        {
            _sendLock.Release();
        }
    }

    /// <summary>
    /// 校验普通命令响应结果码，失败时抛出包含消息号和结果码的诊断异常。
    /// </summary>
    private static FanucCommandResultResponse EnsureSuccess(FanucCommandResultResponse response)
    {
        if (!response.IsSuccess)
        {
            throw CreateCommandFailureException(response.MessageId, response.ResultCode);
        }

        return response;
    }

    /// <summary>
    /// 校验程序状态响应结果码，失败时抛出包含消息号和结果码的诊断异常。
    /// </summary>
    private static FanucProgramStatusResponse EnsureSuccess(FanucProgramStatusResponse response)
    {
        if (!response.IsSuccess)
        {
            throw CreateCommandFailureException(response.MessageId, response.ResultCode);
        }

        return response;
    }

    /// <summary>
    /// 校验速度倍率响应结果码，失败时抛出包含消息号和结果码的诊断异常。
    /// </summary>
    private static FanucSpeedRatioResponse EnsureSuccess(FanucSpeedRatioResponse response)
    {
        if (!response.IsSuccess)
        {
            throw CreateCommandFailureException(response.MessageId, response.ResultCode);
        }

        return response;
    }

    /// <summary>
    /// 校验 TCP 位姿响应结果码，失败时抛出包含消息号和结果码的诊断异常。
    /// </summary>
    private static FanucTcpResponse EnsureSuccess(FanucTcpResponse response)
    {
        if (!response.IsSuccess)
        {
            throw CreateCommandFailureException(response.MessageId, response.ResultCode);
        }

        return response;
    }

    /// <summary>
    /// 校验 IO 读取响应结果码，失败时抛出包含消息号和结果码的诊断异常。
    /// </summary>
    private static FanucIoResponse EnsureSuccess(FanucIoResponse response)
    {
        if (!response.IsSuccess)
        {
            throw CreateCommandFailureException(response.MessageId, response.ResultCode);
        }

        return response;
    }

    /// <summary>
    /// 构造包含 FANUC 命令上下文的失败异常。
    /// </summary>
    private static InvalidOperationException CreateCommandFailureException(uint messageId, uint resultCode)
    {
        return new InvalidOperationException(
            $"FANUC command 0x{messageId:X4} failed with result_code {resultCode}.");
    }

    /// <summary>
    /// 从流中读取一条完整的 doz/zod 响应帧。
    /// </summary>
    private async Task<byte[]> ReadResponseFrameAsync(CancellationToken cancellationToken)
    {
        if (_stream is null)
        {
            throw new InvalidOperationException("命令通道未连接。");
        }

        // 先读取 11 字节头：doz(3) + length(4) + msg_id(4)
        var header = new byte[11];
        await ReadExactAsync(header, cancellationToken).ConfigureAwait(false);

        if (header[0] != (byte)'d' || header[1] != (byte)'o' || header[2] != (byte)'z')
        {
            throw new InvalidDataException("响应帧头 magic 不正确。");
        }

        var declaredLength = System.Buffers.Binary.BinaryPrimitives.ReadUInt32BigEndian(header.AsSpan(3, 4));
        if (declaredLength < 14)
        {
            throw new InvalidDataException("响应帧声明长度过短。");
        }

        var remaining = (int)declaredLength - 11;
        var frame = new byte[declaredLength];
        header.CopyTo(frame, 0);
        await ReadExactAsync(frame.AsMemory(11, remaining), cancellationToken).ConfigureAwait(false);

        // 校验帧尾
        if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
        {
            throw new InvalidDataException("响应帧尾 magic 不正确。");
        }

        return frame;
    }

    /// <summary>
    /// 从流中精确读取指定长度的字节。
    /// </summary>
    private async Task ReadExactAsync(Memory<byte> buffer, CancellationToken cancellationToken)
    {
        if (_stream is null)
        {
            throw new InvalidOperationException("命令通道未连接。");
        }

        var totalRead = 0;
        while (totalRead < buffer.Length)
        {
            var read = await _stream.ReadAsync(buffer.Slice(totalRead), cancellationToken).ConfigureAwait(false);
            if (read == 0)
            {
                throw new IOException("命令通道已断开，读取到 EOF。");
            }

            totalRead += read;
        }
    }
}