vguz_v2/decoder.py

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

Current Implementation: Simple pass-through
TODO: Implement actual byte unpacking later

Author: Kynsight
Version: 1.0.0 (pass-through)
Date: 2025-11-09
"""

from typing import Dict, Any


def decode_uart_packet(packet_bytes: bytes) -> Dict[str, Any]:
    """
    Decode UART packet bytes into telemetry fields.
    
    Current Implementation: Pass-through (returns raw data)
    TODO: Implement actual decoding based on packet format
    
    Expected packet format: EF FE [14 bytes] EE (17 bytes total)
    
    Future implementation should extract:
    - motor_current (2 bytes)
    - encoder_value (2 bytes)
    - relative_encoder_value (2 bytes)
    - v24_pec_diff (2 bytes)
    - pwm (1 byte)
    
    Args:
        packet_bytes: Raw packet bytes (including start/end markers)
    
    Returns:
        Dictionary with decoded fields (currently just raw data)
    
    Example:
        packet_bytes = b'\\xEF\\xFE...[14 bytes]...\\xEE'
        decoded = decode_uart_packet(packet_bytes)
        # Currently returns: {'raw_hex': 'ef fe ... ee', 'raw_bytes': b'...'}
        # Future: {'motor_current': 123, 'encoder_value': 456, ...}
    """
    return {
        'raw_hex': packet_bytes.hex(' '),
        'raw_bytes': packet_bytes,
        'packet_length': len(packet_bytes)
    }


def decode_i2c_sample(i2c_bytes: bytes) -> Dict[str, Any]:
    """
    Decode I2C sample bytes into angle telemetry.
    
    Current Implementation: Pass-through (returns raw data)
    TODO: Implement actual decoding based on I2C format
    
    Expected I2C format: 2 bytes (14-bit angle value)
    
    Future implementation should extract:
    - i2c_raw14 (14-bit raw value)
    - i2c_angle_deg (converted to degrees)
    - i2c_zero_raw14 (zero position)
    - i2c_delta_raw14 (delta from zero)
    
    Args:
        i2c_bytes: Raw I2C bytes (typically 2 bytes for angle)
    
    Returns:
        Dictionary with decoded fields (currently just raw data)
    
    Example:
        i2c_bytes = b'\\x3F\\xFF'  # 14-bit angle
        decoded = decode_i2c_sample(i2c_bytes)
        # Currently returns: {'raw_hex': '3f ff', 'raw_bytes': b'...'}
        # Future: {'i2c_raw14': 16383, 'i2c_angle_deg': 359.98, ...}
    """
    return {
        'raw_hex': i2c_bytes.hex(' '),
        'raw_bytes': i2c_bytes,
        'sample_length': len(i2c_bytes)
    }


# =============================================================================
# Future Implementation Template
# =============================================================================

# def decode_uart_packet_full(packet_bytes: bytes) -> Dict[str, Any]:
#     """
#     Full UART packet decoder (to be implemented).
#     
#     Packet format: EF FE [14 bytes] EE
#     
#     Byte layout:
#     [0-1]: Start marker (EF FE)
#     [2-3]: motor_current (signed 16-bit, little-endian)
#     [4-5]: encoder_value (unsigned 16-bit, little-endian)
#     [6-7]: relative_encoder_value (signed 16-bit, little-endian)
#     [8-9]: v24_pec_diff (signed 16-bit, little-endian)
#     [10]: pwm (unsigned 8-bit)
#     [11-15]: Reserved
#     [16]: End marker (EE)
#     """
#     # Verify packet length
#     if len(packet_bytes) != 17:
#         raise ValueError(f"Invalid packet length: {len(packet_bytes)}, expected 17")
#     
#     # Verify markers
#     if packet_bytes[0:2] != b'\xEF\xFE':
#         raise ValueError("Invalid start marker")
#     if packet_bytes[16:17] != b'\xEE':
#         raise ValueError("Invalid end marker")
#     
#     # Extract data bytes (skip markers)
#     data = packet_bytes[2:16]
#     
#     return {
#         'motor_current': int.from_bytes(data[0:2], 'little', signed=True),
#         'encoder_value': int.from_bytes(data[2:4], 'little', signed=False),
#         'relative_encoder_value': int.from_bytes(data[4:6], 'little', signed=True),
#         'v24_pec_diff': int.from_bytes(data[6:8], 'little', signed=True),
#         'pwm': data[8]
#     }


# def decode_i2c_sample_full(i2c_bytes: bytes) -> Dict[str, Any]:
#     """
#     Full I2C sample decoder (to be implemented).
#     
#     I2C format: 2 bytes (14-bit angle value)
#     
#     Byte layout:
#     [0]: High byte (bits 13-6)
#     [1]: Low byte (bits 5-0 in upper 6 bits)
#     """
#     if len(i2c_bytes) != 2:
#         raise ValueError(f"Invalid I2C sample length: {len(i2c_bytes)}, expected 2")
#     
#     # Extract 14-bit value
#     raw14 = ((i2c_bytes[0] << 6) | (i2c_bytes[1] >> 2)) & 0x3FFF
#     
#     # Convert to degrees (14-bit = 0-360°)
#     angle_deg = (raw14 / 16384.0) * 360.0
#     
#     return {
#         'i2c_raw14': raw14,
#         'i2c_angle_deg': angle_deg
#     }


if __name__ == "__main__":
    # Simple test
    print("Decoder Module - Pass-Through Mode")
    print("=" * 60)
    
    # Test UART packet decoding
    test_uart = b'\xEF\xFE' + b'\x01' * 14 + b'\xEE'
    decoded_uart = decode_uart_packet(test_uart)
    print(f"UART packet decoded: {decoded_uart}")
    
    # Test I2C sample decoding
    test_i2c = b'\x3F\xFF'
    decoded_i2c = decode_i2c_sample(test_i2c)
    print(f"I2C sample decoded: {decoded_i2c}")
    
    print()
    print("✓ Decoder ready (pass-through mode)")
    print("TODO: Implement actual decoding later")