using System.Buffers.Binary;
using System.Text;

namespace Flyshot.Runtime.Fanuc.Protocol;

/// <summary>
/// 定义 FANUC TCP 10012 命令通道已经由抓包和逆向资料确认的消息号。
/// </summary>
public static class FanucCommandMessageIds
{
    /// <summary>
    /// 获取控制器程序状态的消息号。
    /// </summary>
    public const uint GetProgramStatus = 0x2003;

    /// <summary>
    /// 复位控制器的消息号。
    /// </summary>
    public const uint ResetRobot = 0x2100;

    /// <summary>
    /// 启动控制器程序的消息号。
    /// </summary>
    public const uint StartProgram = 0x2102;

    /// <summary>
    /// 停止控制器程序的消息号。
    /// </summary>
    public const uint StopProgram = 0x2103;

    /// <summary>
    /// 读取控制器 TCP 的消息号。
    /// </summary>
    public const uint GetTcp = 0x2200;

    /// <summary>
    /// 设置控制器 TCP 的消息号。
    /// </summary>
    public const uint SetTcp = 0x2201;

    /// <summary>
    /// 读取控制器速度倍率的消息号。
    /// </summary>
    public const uint GetSpeedRatio = 0x2206;

    /// <summary>
    /// 设置控制器速度倍率的消息号。
    /// </summary>
    public const uint SetSpeedRatio = 0x2207;

    /// <summary>
    /// 读取控制器 IO 的消息号。
    /// </summary>
    public const uint GetIo = 0x2208;

    /// <summary>
    /// 设置控制器 IO 的消息号。
    /// </summary>
    public const uint SetIo = 0x2209;
}

/// <summary>
/// 定义旧 ControllerClient 公开的 FANUC IO 类型枚举值。
/// </summary>
public static class FanucIoTypes
{
    /// <summary>
    /// FANUC 数字输入 DI。
    /// </summary>
    public const uint DigitalInput = 1;

    /// <summary>
    /// FANUC 数字输出 DO。
    /// </summary>
    public const uint DigitalOutput = 2;

    /// <summary>
    /// FANUC 机器人输入 RI。
    /// </summary>
    public const uint RobotInput = 8;

    /// <summary>
    /// FANUC 机器人输出 RO。
    /// </summary>
    public const uint RobotOutput = 9;

    /// <summary>
    /// 将 HTTP/兼容层传入的 IO 类型字符串转换为 FANUC 命令通道枚举值。
    /// </summary>
    /// <param name="ioType">IO 类型字符串，例如 DI、DO、RI、RO。</param>
    /// <returns>命令通道使用的 IO 类型数值。</returns>
    public static uint FromName(string ioType)
    {
        if (string.IsNullOrWhiteSpace(ioType))
        {
            throw new ArgumentException("IO 类型不能为空。", nameof(ioType));
        }

        return ioType.Trim().ToUpperInvariant() switch
        {
            "DI" or "KIOTYPEDI" => DigitalInput,
            "DO" or "KIOTYPEDO" => DigitalOutput,
            "RI" or "KIOTYPERI" => RobotInput,
            "RO" or "KIOTYPERO" => RobotOutput,
            _ => throw new ArgumentOutOfRangeException(nameof(ioType), ioType, "未知 IO 类型。")
        };
    }
}

/// <summary>
/// 表示 FANUC TCP 10012 命令通道中只携带结果码的响应。
/// </summary>
public sealed class FanucCommandResultResponse
{
    /// <summary>
    /// 初始化命令结果响应。
    /// </summary>
    /// <param name="messageId">响应对应的消息号。</param>
    /// <param name="resultCode">控制器返回的结果码。</param>
    public FanucCommandResultResponse(uint messageId, uint resultCode)
    {
        MessageId = messageId;
        ResultCode = resultCode;
    }

    /// <summary>
    /// 获取响应对应的消息号。
    /// </summary>
    public uint MessageId { get; }

    /// <summary>
    /// 获取控制器返回的结果码。
    /// </summary>
    public uint ResultCode { get; }

    /// <summary>
    /// 获取当前响应是否表示成功。
    /// </summary>
    public bool IsSuccess => ResultCode == 0;
}

/// <summary>
/// 表示 FANUC TCP 10012 速度倍率响应。
/// </summary>
public sealed class FanucSpeedRatioResponse
{
    /// <summary>
    /// 初始化速度倍率响应。
    /// </summary>
    /// <param name="messageId">响应对应的消息号。</param>
    /// <param name="ratioInt">控制器返回的整数百分比。</param>
    /// <param name="resultCode">控制器返回的结果码。</param>
    public FanucSpeedRatioResponse(uint messageId, uint ratioInt, uint resultCode)
    {
        MessageId = messageId;
        RatioInt = ratioInt;
        ResultCode = resultCode;
    }

    /// <summary>
    /// 获取响应对应的消息号。
    /// </summary>
    public uint MessageId { get; }

    /// <summary>
    /// 获取控制器返回的整数百分比。
    /// </summary>
    public uint RatioInt { get; }

    /// <summary>
    /// 获取控制器返回的结果码。
    /// </summary>
    public uint ResultCode { get; }

    /// <summary>
    /// 获取转换后的 0.0 到 1.0 速度倍率。
    /// </summary>
    public double Ratio => RatioInt / 100.0;

    /// <summary>
    /// 获取当前响应是否表示成功。
    /// </summary>
    public bool IsSuccess => ResultCode == 0;
}

/// <summary>
/// 表示 FANUC TCP 10012 TCP 位姿响应。
/// </summary>
public sealed class FanucTcpResponse
{
    /// <summary>
    /// 初始化 TCP 位姿响应。
    /// </summary>
    /// <param name="messageId">响应对应的消息号。</param>
    /// <param name="resultCode">控制器返回的结果码。</param>
    /// <param name="tcpId">控制器返回的 TCP ID。</param>
    /// <param name="pose">7 维 TCP 位姿。</param>
    public FanucTcpResponse(uint messageId, uint resultCode, uint tcpId, IReadOnlyList<double> pose)
    {
        MessageId = messageId;
        ResultCode = resultCode;
        TcpId = tcpId;
        Pose = pose.ToArray();
    }

    /// <summary>
    /// 获取响应对应的消息号。
    /// </summary>
    public uint MessageId { get; }

    /// <summary>
    /// 获取控制器返回的结果码。
    /// </summary>
    public uint ResultCode { get; }

    /// <summary>
    /// 获取控制器返回的 TCP ID。
    /// </summary>
    public uint TcpId { get; }

    /// <summary>
    /// 获取 7 维 TCP 位姿。
    /// </summary>
    public IReadOnlyList<double> Pose { get; }

    /// <summary>
    /// 获取当前响应是否表示成功。
    /// </summary>
    public bool IsSuccess => ResultCode == 0;
}

/// <summary>
/// 表示 FANUC TCP 10012 IO 读取响应。
/// </summary>
public sealed class FanucIoResponse
{
    /// <summary>
    /// 初始化 IO 读取响应。
    /// </summary>
    /// <param name="messageId">响应对应的消息号。</param>
    /// <param name="resultCode">控制器返回的结果码。</param>
    /// <param name="numericValue">控制器返回的 float IO 数值。</param>
    public FanucIoResponse(uint messageId, uint resultCode, double numericValue)
    {
        MessageId = messageId;
        ResultCode = resultCode;
        NumericValue = numericValue;
    }

    /// <summary>
    /// 获取响应对应的消息号。
    /// </summary>
    public uint MessageId { get; }

    /// <summary>
    /// 获取控制器返回的结果码。
    /// </summary>
    public uint ResultCode { get; }

    /// <summary>
    /// 获取控制器返回的原始数值。
    /// </summary>
    public double NumericValue { get; }

    /// <summary>
    /// 获取按布尔 IO 解释后的值。
    /// </summary>
    public bool Value => Math.Abs(NumericValue) > double.Epsilon;

    /// <summary>
    /// 获取当前响应是否表示成功。
    /// </summary>
    public bool IsSuccess => ResultCode == 0;
}

/// <summary>
/// 表示 FANUC TCP 10012 程序状态响应。
/// </summary>
public sealed class FanucProgramStatusResponse
{
    /// <summary>
    /// 初始化程序状态响应。
    /// </summary>
    /// <param name="messageId">响应对应的消息号。</param>
    /// <param name="resultCode">控制器返回的结果码。</param>
    /// <param name="programStatus">控制器程序状态。</param>
    public FanucProgramStatusResponse(uint messageId, uint resultCode, uint programStatus)
    {
        MessageId = messageId;
        ResultCode = resultCode;
        ProgramStatus = programStatus;
    }

    /// <summary>
    /// 获取响应对应的消息号。
    /// </summary>
    public uint MessageId { get; }

    /// <summary>
    /// 获取控制器返回的结果码。
    /// </summary>
    public uint ResultCode { get; }

    /// <summary>
    /// 获取控制器程序状态值。
    /// </summary>
    public uint ProgramStatus { get; }

    /// <summary>
    /// 获取当前响应是否表示成功。
    /// </summary>
    public bool IsSuccess => ResultCode == 0;
}

/// <summary>
/// 提供 FANUC TCP 10012 命令通道的基础封包与响应解析能力。
/// </summary>
public static class FanucCommandProtocol
{
    /// <summary>
    /// 将无业务体命令封装为 TCP 10012 二进制帧。
    /// </summary>
    /// <param name="messageId">命令消息号。</param>
    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
    public static byte[] PackEmptyCommand(uint messageId)
    {
        return PackFrame(messageId, ReadOnlySpan<byte>.Empty);
    }

    /// <summary>
    /// 将程序名命令封装为 TCP 10012 二进制帧。
    /// </summary>
    /// <param name="messageId">命令消息号。</param>
    /// <param name="programName">控制器程序名。</param>
    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
    public static byte[] PackProgramCommand(uint messageId, string programName)
    {
        if (string.IsNullOrWhiteSpace(programName))
        {
            throw new ArgumentException("程序名不能为空。", nameof(programName));
        }

        var programNameBytes = Encoding.ASCII.GetBytes(programName);
        var body = new byte[sizeof(uint) + programNameBytes.Length];
        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), (uint)programNameBytes.Length);
        programNameBytes.CopyTo(body.AsSpan(sizeof(uint)));

        return PackFrame(messageId, body);
    }

    /// <summary>
    /// 封装读取速度倍率命令。
    /// </summary>
    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
    public static byte[] PackGetSpeedRatioCommand()
    {
        return PackEmptyCommand(FanucCommandMessageIds.GetSpeedRatio);
    }

    /// <summary>
    /// 封装设置速度倍率命令，按旧系统逻辑转换为 0..100 的整数百分比。
    /// </summary>
    /// <param name="ratio">目标速度倍率。</param>
    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
    public static byte[] PackSetSpeedRatioCommand(double ratio)
    {
        if (double.IsNaN(ratio) || double.IsInfinity(ratio))
        {
            throw new ArgumentOutOfRangeException(nameof(ratio), "ratio 必须是有限数值。");
        }

        var ratioInt = (uint)Math.Clamp((int)(ratio * 100.0), 0, 100);
        var body = new byte[sizeof(uint)];
        BinaryPrimitives.WriteUInt32BigEndian(body, ratioInt);
        return PackFrame(FanucCommandMessageIds.SetSpeedRatio, body);
    }

    /// <summary>
    /// 封装读取 TCP 位姿命令。
    /// </summary>
    /// <param name="tcpId">目标 TCP ID。</param>
    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
    public static byte[] PackGetTcpCommand(uint tcpId)
    {
        var body = new byte[sizeof(uint)];
        BinaryPrimitives.WriteUInt32BigEndian(body, tcpId);
        return PackFrame(FanucCommandMessageIds.GetTcp, body);
    }

    /// <summary>
    /// 封装设置 TCP 位姿命令。
    /// </summary>
    /// <param name="tcpId">目标 TCP ID。</param>
    /// <param name="pose">7 维 TCP 位姿。</param>
    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
    public static byte[] PackSetTcpCommand(uint tcpId, IReadOnlyList<double> pose)
    {
        ArgumentNullException.ThrowIfNull(pose);
        if (pose.Count != 7)
        {
            throw new ArgumentException("TCP 位姿必须包含 7 个数值。", nameof(pose));
        }

        var body = new byte[sizeof(uint) + sizeof(float) * 7];
        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), tcpId);
        for (int i = 0; i < 7; i++)
        {
            BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) + i * sizeof(float), sizeof(float)), (float)pose[i]);
        }

        return PackFrame(FanucCommandMessageIds.SetTcp, body);
    }

    /// <summary>
    /// 封装读取 IO 命令，字段顺序为 io_type 后接 io_index。
    /// </summary>
    /// <param name="ioType">IO 类型数值。</param>
    /// <param name="ioIndex">IO 索引。</param>
    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
    public static byte[] PackGetIoCommand(uint ioType, int ioIndex)
    {
        if (ioIndex < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
        }

        var body = new byte[sizeof(uint) * 2];
        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
        return PackFrame(FanucCommandMessageIds.GetIo, body);
    }

    /// <summary>
    /// 封装设置 IO 命令，字段顺序为 io_type、io_index、float io_value。
    /// </summary>
    /// <param name="ioType">IO 类型数值。</param>
    /// <param name="ioIndex">IO 索引。</param>
    /// <param name="value">目标 IO 布尔值。</param>
    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
    public static byte[] PackSetIoCommand(uint ioType, int ioIndex, bool value)
    {
        if (ioIndex < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
        }

        var body = new byte[sizeof(uint) * 2 + sizeof(float)];
        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
        BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) * 2, sizeof(float)), value ? 1.0f : 0.0f);
        return PackFrame(FanucCommandMessageIds.SetIo, body);
    }

    /// <summary>
    /// 解析只携带结果码的 TCP 10012 响应帧。
    /// </summary>
    /// <param name="frame">完整响应帧。</param>
    /// <returns>命令结果响应。</returns>
    public static FanucCommandResultResponse ParseResultResponse(ReadOnlySpan<byte> frame)
    {
        var messageId = ValidateAndReadMessageId(frame);
        var body = GetBody(frame);
        if (body.Length < sizeof(uint))
        {
            throw new InvalidDataException("FANUC 命令响应体长度不足。");
        }

        return new FanucCommandResultResponse(
            messageId,
            BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]));
    }

    /// <summary>
    /// 解析 GetSpeedRatio 的 TCP 10012 响应帧。
    /// </summary>
    /// <param name="frame">完整响应帧。</param>
    /// <returns>速度倍率响应。</returns>
    public static FanucSpeedRatioResponse ParseSpeedRatioResponse(ReadOnlySpan<byte> frame)
    {
        var messageId = ValidateAndReadMessageId(frame);
        var body = GetBody(frame);
        if (body.Length < sizeof(uint) * 2)
        {
            throw new InvalidDataException("FANUC 速度倍率响应体长度不足。");
        }

        // GetSpeedRatio 的字段顺序特殊：ratio_int 在前，result_code 在后。
        var ratioInt = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
        var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
        return new FanucSpeedRatioResponse(messageId, ratioInt, resultCode);
    }

    /// <summary>
    /// 解析 GetTCP 的 TCP 10012 响应帧。
    /// </summary>
    /// <param name="frame">完整响应帧。</param>
    /// <returns>TCP 位姿响应。</returns>
    public static FanucTcpResponse ParseTcpResponse(ReadOnlySpan<byte> frame)
    {
        var messageId = ValidateAndReadMessageId(frame);
        var body = GetBody(frame);
        if (body.Length < sizeof(uint) * 2 + sizeof(float) * 7)
        {
            throw new InvalidDataException("FANUC TCP 响应体长度不足。");
        }

        var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
        var tcpId = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
        var pose = new double[7];
        for (int i = 0; i < pose.Length; i++)
        {
            pose[i] = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint) * 2 + i * sizeof(float), sizeof(float)));
        }

        return new FanucTcpResponse(messageId, resultCode, tcpId, pose);
    }

    /// <summary>
    /// 解析 GetIO 的 TCP 10012 响应帧。
    /// </summary>
    /// <param name="frame">完整响应帧。</param>
    /// <returns>IO 读取响应。</returns>
    public static FanucIoResponse ParseIoResponse(ReadOnlySpan<byte> frame)
    {
        var messageId = ValidateAndReadMessageId(frame);
        var body = GetBody(frame);
        if (body.Length < sizeof(uint) + sizeof(float))
        {
            throw new InvalidDataException("FANUC IO 响应体长度不足。");
        }

        var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
        var ioValue = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint), sizeof(float)));
        return new FanucIoResponse(messageId, resultCode, ioValue);
    }

    /// <summary>
    /// 解析 GetProgStatus 的 TCP 10012 响应帧。
    /// </summary>
    /// <param name="frame">完整响应帧。</param>
    /// <returns>程序状态响应。</returns>
    public static FanucProgramStatusResponse ParseProgramStatusResponse(ReadOnlySpan<byte> frame)
    {
        var messageId = ValidateAndReadMessageId(frame);
        var body = GetBody(frame);
        if (body.Length < sizeof(uint) * 2)
        {
            throw new InvalidDataException("FANUC 程序状态响应体长度不足。");
        }

        // 抓包样本中的字段顺序为 result_code 后接 prog_status。
        var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
        var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
        return new FanucProgramStatusResponse(messageId, resultCode, programStatus);
    }

    /// <summary>
    /// 按 FANUC 命令通道 framing 规则封装完整帧。
    /// </summary>
    /// <param name="messageId">命令消息号。</param>
    /// <param name="body">业务体。</param>
    /// <returns>完整命令帧。</returns>
    internal static byte[] PackFrame(uint messageId, ReadOnlySpan<byte> body)
    {
        var frameLength = 3 + sizeof(uint) + sizeof(uint) + body.Length + 3;
        var frame = new byte[frameLength];

        frame[0] = (byte)'d';
        frame[1] = (byte)'o';
        frame[2] = (byte)'z';
        BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(3, sizeof(uint)), (uint)frameLength);
        BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(7, sizeof(uint)), messageId);
        body.CopyTo(frame.AsSpan(11));
        frame[^3] = (byte)'z';
        frame[^2] = (byte)'o';
        frame[^1] = (byte)'d';

        return frame;
    }

    /// <summary>
    /// 校验完整帧并读取消息号。
    /// </summary>
    /// <param name="frame">完整响应帧。</param>
    /// <returns>响应消息号。</returns>
    private static uint ValidateAndReadMessageId(ReadOnlySpan<byte> frame)
    {
        if (frame.Length < 14)
        {
            throw new InvalidDataException("FANUC 命令帧长度不足。");
        }

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

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

        var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
        if (declaredLength != frame.Length)
        {
            throw new InvalidDataException("FANUC 命令帧长度字段与实际长度不一致。");
        }

        return BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint)));
    }

    /// <summary>
    /// 获取完整帧中的业务体切片。
    /// </summary>
    /// <param name="frame">完整响应帧。</param>
    /// <returns>业务体切片。</returns>
    private static ReadOnlySpan<byte> GetBody(ReadOnlySpan<byte> frame)
    {
        return frame.Slice(11, frame.Length - 14);
    }
}