namespace RioJoy.Core.Protocol;

/// <summary>
/// The five raw analog axes decoded from an <see cref="RioCommand.AnalogReply"/>
/// payload. Values are the 14-bit signed raw counts straight off the RIO, before
/// any calibration/deadzone mapping (that math is Phase 4). Port of
/// <c>AnalogEvent</c> / <c>CombinePair</c> (riovjoy2.cpp#L1116); see
/// docs/PROTOCOL.md §4.
/// </summary>
public readonly struct AnalogReport
{
    public short Throttle { get; }
    public short LeftPedal { get; }
    public short RightPedal { get; }
    public short JoystickY { get; }
    public short JoystickX { get; }

    public AnalogReport(short throttle, short leftPedal, short rightPedal, short joystickY, short joystickX)
    {
        Throttle = throttle;
        LeftPedal = leftPedal;
        RightPedal = rightPedal;
        JoystickY = joystickY;
        JoystickX = joystickX;
    }

    /// <summary>
    /// Decode a 14-bit signed axis value from a (low, high) byte pair, each
    /// carrying 7 data bits. Port of <c>CombinePair</c> (riovjoy2.cpp#L1116):
    /// combine the 7-bit halves then sign-extend bit 13.
    /// </summary>
    public static short CombinePair(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;
    }

    /// <summary>
    /// Try to decode an <see cref="RioCommand.AnalogReply"/> payload (10 bytes:
    /// 5 axes × low,high in the order throttle, left pedal, right pedal,
    /// joystick Y, joystick X). Returns <see langword="false"/> if any byte equals
    /// <c>0xFE</c> (<see cref="RioControl.Restart"/>), which the RIO uses as an
    /// "invalid sample" sentinel — the legacy code ignores such replies.
    /// </summary>
    public static bool TryParse(byte[] payload, out AnalogReport report)
    {
        if (payload is null) throw new ArgumentNullException(nameof(payload));
        if (payload.Length != RioCommandTable.PayloadLength(RioCommand.AnalogReply))
            throw new ArgumentException(
                $"AnalogReply payload must be {RioCommandTable.PayloadLength(RioCommand.AnalogReply)} bytes.",
                nameof(payload));

        foreach (byte b in payload)
        {
            if (b == (byte)RioControl.Restart)
            {
                report = default;
                return false;
            }
        }

        report = new AnalogReport(
            throttle: CombinePair(payload[0], payload[1]),
            leftPedal: CombinePair(payload[2], payload[3]),
            rightPedal: CombinePair(payload[4], payload[5]),
            joystickY: CombinePair(payload[6], payload[7]),
            joystickX: CombinePair(payload[8], payload[9]));
        return true;
    }

    public override string ToString() =>
        $"T:{Throttle} L:{LeftPedal} R:{RightPedal} Y:{JoystickY} X:{JoystickX}";
}