#!/usr/bin/env python3
"""
Decoder Module - vzug-e-hinge
==============================
Decodes raw UART and I2C data into telemetry fields.

UART Packet Format (14 bytes, Little Endian / LSB first):
  Byte 0-1:   Start marker EF FE
  Byte 2-3:   MotorCurrent (signed 16-bit, LSB first)
  Byte 4-5:   EncoderValue (signed 16-bit, LSB first)
  Byte 6-7:   RelativeEncoderValue (signed 16-bit, LSB first)
  Byte 8-9:   V24PecDiff (signed 16-bit, LSB first)
  Byte 10-11: Reserved0 (signed 16-bit, LSB first)
  Byte 12:    PWM (unsigned 8-bit, 0-100)
  Byte 13:    End marker EE

I2C Format (2 bytes, 14-bit angle from AS5600/AS5048A):
  Byte 0: High byte (bits 13-6)
  Byte 1: Low byte (bits 5-0)

Reference: eHinge Encoder.pdf, Datalogger files

Author: Kynsight
Version: 2.0.0 (full decoding)
Date: 2025-11-22
"""

from typing import Dict, Any, Optional

# UART Packet Constants
FRAME_LEN = 14
START0 = 0xEF
START1 = 0xFE
END = 0xEE


def _le_s16(b0: int, b1: int) -> int:
    """Decode Little Endian signed 16-bit value (LSB first)."""
    u = (b1 << 8) | b0  # b1 is high byte, b0 is low byte
    return u - 0x10000 if (u & 0x8000) else u


def decode_uart_packet(packet_bytes: bytes) -> Dict[str, Any]:
    """
    Decode UART packet bytes into telemetry fields.

    Packet format: EF FE [data] EE (14 bytes total, Little Endian / LSB first)

    Args:
        packet_bytes: Raw packet bytes (14 bytes including start/end markers)

    Returns:
        Dictionary with decoded fields or None values if invalid

    Example:
        packet_bytes = b'\\xEF\\xFE\\x04\\x00\\xED\\xFF\\x00\\x00\\x20\\x08\\x00\\x00\\x00\\xEE'
        decoded = decode_uart_packet(packet_bytes)
        # Returns: {'motor_current': 4, 'encoder_value': -19, 'v24_pec_diff': 2080, ...}
    """
    # Validate packet
    if packet_bytes is None or len(packet_bytes) != FRAME_LEN:
        return {
            'motor_current': None,
            'encoder_value': None,
            'relative_encoder_value': None,
            'v24_pec_diff': None,
            'pwm': None,
            'reserved0': None,
            'error': f'Invalid packet length: {len(packet_bytes) if packet_bytes else 0}, expected {FRAME_LEN}'
        }

    # Verify markers
    if not (packet_bytes[0] == START0 and packet_bytes[1] == START1 and packet_bytes[13] == END):
        return {
            'motor_current': None,
            'encoder_value': None,
            'relative_encoder_value': None,
            'v24_pec_diff': None,
            'pwm': None,
            'reserved0': None,
            'error': f'Invalid markers: start={packet_bytes[0]:02X} {packet_bytes[1]:02X}, end={packet_bytes[13]:02X}'
        }

    # Decode fields (all Little Endian signed 16-bit except PWM)
    motor_current = _le_s16(packet_bytes[2], packet_bytes[3])
    encoder_value = _le_s16(packet_bytes[4], packet_bytes[5])
    relative_encoder_value = _le_s16(packet_bytes[6], packet_bytes[7])
    v24_pec_diff = _le_s16(packet_bytes[8], packet_bytes[9])
    reserved0 = _le_s16(packet_bytes[10], packet_bytes[11])
    pwm = int(packet_bytes[12])  # Unsigned 8-bit (0-100)

    # Optional sanity check (adjust bounds as needed)
    if not (-40000 <= motor_current <= 40000 and 0 <= pwm <= 100):
        return {
            'motor_current': motor_current,
            'encoder_value': encoder_value,
            'relative_encoder_value': relative_encoder_value,
            'v24_pec_diff': v24_pec_diff,
            'pwm': pwm,
            'reserved0': reserved0,
            'error': f'Out of range: motor_current={motor_current}, pwm={pwm}'
        }

    return {
        'motor_current': motor_current,
        'encoder_value': encoder_value,
        'relative_encoder_value': relative_encoder_value,
        'v24_pec_diff': v24_pec_diff,
        'pwm': pwm,
        'reserved0': reserved0,
        'error': None
    }


def _wrap14_delta(current: int, zero: int) -> int:
    """
    Calculate signed wrap-around delta in 14-bit space.

    Maps (current - zero) to range [-8192..+8191] handling wrap-around.

    Args:
        current: Current 14-bit value (0-16383)
        zero: Zero reference 14-bit value (0-16383)

    Returns:
        Signed delta in range [-8192, +8191]

    Example:
        _wrap14_delta(100, 16300) = 184  # Wrapped forward
        _wrap14_delta(16300, 100) = -184  # Wrapped backward
    """
    return ((current - zero + 8192) % 16384) - 8192


def decode_i2c_sample(i2c_bytes: Optional[bytes], zero_ref: int = 0) -> Dict[str, Any]:
    """
    Decode I2C sample bytes into angle telemetry with zero reference.

    I2C format: 2 bytes (14-bit angle from AS5600/AS5048A sensor)
      Byte 0: High byte (bits 13-6 of angle)
      Byte 1: Low byte (bits 5-0 of angle)

    Args:
        i2c_bytes: Raw I2C bytes (2 bytes)
        zero_ref: Zero reference value (0-16383), default 0

    Returns:
        Dictionary with decoded I2C angle fields

    Example:
        i2c_bytes = b'\\xEC\\x36'  # Sample from sensor
        decoded = decode_i2c_sample(i2c_bytes, zero_ref=11318)
        # Returns: {
        #   'i2c_raw14': 11318,
        #   'i2c_zero_raw14': 11318,
        #   'i2c_delta_raw14': 0,
        #   'i2c_angle_deg': 0.0,
        #   'i2c_zero_angle_deg': 248.69
        # }
    """
    # Validate input
    if i2c_bytes is None or len(i2c_bytes) != 2:
        return {
            'i2c_raw14': None,
            'i2c_zero_raw14': zero_ref if zero_ref != 0 else None,
            'i2c_delta_raw14': None,
            'i2c_angle_deg': None,
            'i2c_zero_angle_deg': (zero_ref * 360.0 / 16384.0) if zero_ref != 0 else None,
            'error': f'Invalid I2C length: {len(i2c_bytes) if i2c_bytes else 0}, expected 2'
        }

    # Decode 14-bit value
    # Format: MSB contains bits [13:6], LSB contains bits [5:0]
    msb, lsb = i2c_bytes[0], i2c_bytes[1]
    raw14 = ((msb << 6) | (lsb & 0x3F)) & 0x3FFF

    # Calculate delta from zero with wrap-around
    delta14 = _wrap14_delta(raw14, zero_ref) if zero_ref != 0 else None

    # Convert to degrees
    # NOTE: Negative sign to match coordinate system (clockwise positive)
    angle_deg = -(delta14 * 360.0 / 16384.0) if delta14 is not None else None
    zero_angle_deg = (zero_ref * 360.0 / 16384.0) if zero_ref != 0 else None

    return {
        'i2c_raw14': raw14,
        'i2c_zero_raw14': zero_ref if zero_ref != 0 else None,
        'i2c_delta_raw14': delta14,
        'i2c_angle_deg': angle_deg,
        'i2c_zero_angle_deg': zero_angle_deg,
        'error': None
    }


# =============================================================================
# Test Code
# =============================================================================

if __name__ == "__main__":
    print("Decoder Module - Full Implementation")
    print("=" * 70)
    print()

    # Test UART packet decoding (from PDF example)
    # EF FE | 04 00 | ED FF | 00 00 | 20 08 | 00 00 | 00 | EE
    # Expected: MotorCurrent=4, EncoderValue=-19, Relative=0, V24=2080, PWM=0
    test_uart = bytes([
        0xEF, 0xFE,  # Start
        0x04, 0x00,  # MotorCurrent = 4 (Little Endian: LSB first)
        0xED, 0xFF,  # EncoderValue = -19 (Little Endian: LSB first)
        0x00, 0x00,  # RelativeEncoderValue = 0
        0x20, 0x08,  # V24PecDiff = 2080 (Little Endian: LSB first)
        0x00, 0x00,  # Reserved0 = 0
        0x00,        # PWM = 0
        0xEE         # End
    ])

    decoded_uart = decode_uart_packet(test_uart)
    print("UART Packet Test:")
    print(f"  Hex: {test_uart.hex(' ').upper()}")
    print(f"  Decoded:")
    for key, val in decoded_uart.items():
        print(f"    {key:25s} = {val}")
    print()

    # Test I2C angle decoding (from your log)
    # EC 36 = 11318 raw14 = 248.69°
    test_i2c = bytes([0xEC, 0x36])
    decoded_i2c = decode_i2c_sample(test_i2c, zero_ref=0)
    print("I2C Angle Test (no zero):")
    print(f"  Hex: {test_i2c.hex(' ').upper()}")
    print(f"  Decoded:")
    for key, val in decoded_i2c.items():
        if val is not None:
            print(f"    {key:25s} = {val}")
    print()

    # Test I2C with zero reference
    decoded_i2c_zero = decode_i2c_sample(test_i2c, zero_ref=11318)
    print("I2C Angle Test (with zero=11318):")
    print(f"  Hex: {test_i2c.hex(' ').upper()}")
    print(f"  Decoded:")
    for key, val in decoded_i2c_zero.items():
        if val is not None:
            print(f"    {key:25s} = {val}")
    print()

    print("✓ Decoder ready (full implementation)")
    print("✓ UART: Little Endian (LSB first), signed values")
    print("✓ I2C: 14-bit angle with zero reference and wrap-around")
