namespace VRio.Core.Protocol;

/// <summary>
/// Builds outbound packets: <c>[command][payload…][checksum]</c>. Same framing
/// as RIOJoy's builder, but the convenience methods here cover the RIO → PC
/// messages, because vRIO <em>is</em> the device.
/// </summary>
public static class PacketBuilder
{
    /// <summary>
    /// Build a wire-ready packet for <paramref name="command"/>. The payload
    /// length must match the command's entry in the length table.
    /// </summary>
    public static byte[] Build(RioCommand command, ReadOnlySpan<byte> payload)
    {
        int expected = RioCommandTable.PayloadLength(command);
        if (payload.Length != expected)
            throw new ArgumentException(
                $"{command} expects a {expected}-byte payload, got {payload.Length}.",
                nameof(payload));

        var packet = new byte[1 + expected + 1];
        packet[0] = (byte)command;
        payload.CopyTo(packet.AsSpan(1));
        packet[packet.Length - 1] = RioChecksum.Compute(packet.AsSpan(0, 1 + expected));
        return packet;
    }

    /// <summary>Build a zero-payload command packet.</summary>
    public static byte[] Build(RioCommand command) => Build(command, ReadOnlySpan<byte>.Empty);

    // --- Convenience builders for the RIO → PC messages ----------------------

    public static byte[] CheckReply(RioStatusType type, byte number) =>
        Build(RioCommand.CheckReply, stackalloc byte[] { (byte)type, number });

    public static byte[] VersionReply(byte major, byte minor) =>
        Build(RioCommand.VersionReply, stackalloc byte[] { major, minor });

    /// <summary>
    /// AnalogReply: 5 axes × (low, high) in the order throttle, left pedal,
    /// right pedal, joystick Y, joystick X. Each axis is a 14-bit signed value
    /// split into two 7-bit bytes (see <see cref="AnalogCodec"/>).
    /// </summary>
    public static byte[] AnalogReply(short throttle, short leftPedal, short rightPedal, short joystickY, short joystickX)
    {
        Span<byte> payload = stackalloc byte[10];
        AnalogCodec.Split(throttle, out payload[0], out payload[1]);
        AnalogCodec.Split(leftPedal, out payload[2], out payload[3]);
        AnalogCodec.Split(rightPedal, out payload[4], out payload[5]);
        AnalogCodec.Split(joystickY, out payload[6], out payload[7]);
        AnalogCodec.Split(joystickX, out payload[8], out payload[9]);
        return Build(RioCommand.AnalogReply, payload);
    }

    public static byte[] ButtonPressed(byte index) =>
        Build(RioCommand.ButtonPressed, stackalloc byte[] { index });

    public static byte[] ButtonReleased(byte index) =>
        Build(RioCommand.ButtonReleased, stackalloc byte[] { index });

    public static byte[] KeyPressed(byte pad, byte index) =>
        Build(RioCommand.KeyPressed, stackalloc byte[] { pad, index });

    public static byte[] KeyReleased(byte pad, byte index) =>
        Build(RioCommand.KeyReleased, stackalloc byte[] { pad, index });

    public static byte[] TestModeChange(byte mode) =>
        Build(RioCommand.TestModeChange, stackalloc byte[] { mode });
}

/// <summary>
/// Packs/unpacks the 14-bit signed axis values carried by AnalogReply: two
/// 7-bit bytes, low then high; bit 13 is the sign (riojoy/docs/PROTOCOL.md §4).
/// </summary>
public static class AnalogCodec
{
    /// <summary>Smallest representable axis value (14-bit signed).</summary>
    public const short Min = -8192;

    /// <summary>Largest representable axis value (14-bit signed).</summary>
    public const short Max = 8191;

    /// <summary>Split an axis value into its (low, high) 7-bit wire bytes.</summary>
    public static void Split(short value, out byte low, out byte high)
    {
        if (value is < Min or > Max)
            throw new ArgumentOutOfRangeException(nameof(value), $"Axis value must be {Min}..{Max}.");

        int raw = value & 0x3FFF; // 14-bit two's complement
        low = (byte)(raw & 0x7F);
        high = (byte)((raw >> 7) & 0x7F);
    }

    /// <summary>Recombine (low, high) wire bytes into the signed axis value.</summary>
    public static short Combine(byte low, byte high)
    {
        int raw = (low & 0x7F) | (high << 7); // 14 bits
        if ((raw & 0x2000) != 0)              // bit 13 set → negative
            raw |= ~0x3FFF;                   // sign-extend into the high bits
        return (short)raw;
    }
}
