from ..message_base import MessageBase
from ..constants import Subaddress
from ..fields import BitField, ScaledField

class MsgA5(MessageBase):
    """
    Message A5: INU High Speed Vector
    
    ID: A5
    Direction: BC -> RT (Receive)
    Subaddress: 05
    Rate: 50 Hz
    Document Ref: 7.1.5
    """
    SUBADDRESS = Subaddress.RX_INU_HS
    IS_TRANSMIT = False
    RATE_HZ = 50

    # --- Word 01: INU Mode Word (Ref 7.1.5.1) ---
    # Note: ICD says "disregarded by the Radar", mapped as raw.
    inu_mode_word = BitField(word_index=0, start_bit=0, width=16)

    # --- Word 02: INU Control Word 2 (Time Tag) (Ref 7.1.5.2) ---
    # LSB = 64 us
    time_tag = ScaledField(word_index=1, start_bit=0, width=16, lsb_value=64.0)

    # --- Helper for 32-bit 2's complement Scaled Fields ---
    def _get_32bit_scaled(self, word_idx_msw, word_idx_lsw, lsb_value):
        """
        Combines MSW and LSW to form a 32-bit 2's complement value.
        """
        msw = self._data[word_idx_msw]
        lsw = self._data[word_idx_lsw]
        
        raw_32 = (msw << 16) | lsw
        
        # Handle 2's complement for 32 bits
        if raw_32 & (1 << 31): 
            raw_32 -= (1 << 32)
            
        return float(raw_32 * lsb_value)

    # --- Words 03-04: X Velocity (Ref 7.1.5.3) ---
    # 32 bit, 2's comp. LSB = 3.81470e-6 ft/sec
    @property
    def velocity_x(self):
        return self._get_32bit_scaled(2, 3, 3.81470e-6)

    # --- Words 05-06: Y Velocity (Ref 7.1.5.4) ---
    @property
    def velocity_y(self):
        return self._get_32bit_scaled(4, 5, 3.81470e-6)

    # --- Words 07-08: Z Velocity (Ref 7.1.5.5) ---
    @property
    def velocity_z(self):
        return self._get_32bit_scaled(6, 7, 3.81470e-6)

    # --- Word 09: Platform Azimuth (Ref 7.1.5.6) ---
    # Semicircles, 2's comp
    platform_azimuth = ScaledField(word_index=8, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)

    # --- Word 10: Roll (Ref 7.1.5.7) ---
    roll = ScaledField(word_index=9, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)

    # --- Word 11: Pitch (Ref 7.1.5.8) ---
    pitch = ScaledField(word_index=10, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)

    # --- Word 12: Roll Rate (Ref 7.1.5.9) ---
    # Units: Semicircles/sec. LSB = 1.22070e-4
    roll_rate = ScaledField(word_index=11, start_bit=0, width=16, lsb_value=1.22070e-4, signed=True)

    # --- Word 13: Pitch Rate (Ref 7.1.5.10) ---
    pitch_rate = ScaledField(word_index=12, start_bit=0, width=16, lsb_value=1.22070e-4, signed=True)

    # --- Word 14: Yaw Rate (Ref 7.1.5.11) ---
    yaw_rate = ScaledField(word_index=13, start_bit=0, width=16, lsb_value=1.22070e-4, signed=True)

    # --- Word 15: Longitudinal Acceleration (nx) (Ref 7.1.5.12) ---
    # Units: ft/sec^2. LSB = 0.03125
    accel_longitudinal = ScaledField(word_index=14, start_bit=0, width=16, lsb_value=0.03125, signed=True)

    # --- Word 16: Lateral Acceleration (ny) (Ref 7.1.5.13) ---
    accel_lateral = ScaledField(word_index=15, start_bit=0, width=16, lsb_value=0.03125, signed=True)

    # --- Word 17: Normal Acceleration (nz) (Ref 7.1.5.14) ---
    accel_normal = ScaledField(word_index=16, start_bit=0, width=16, lsb_value=0.03125, signed=True)

    # --- Word 18: Platform Azimuth Time Tag (Ref 7.1.5.15) ---
    # LSB = 64 us
    time_tag_azimuth = ScaledField(word_index=17, start_bit=0, width=16, lsb_value=64.0)

    # --- Word 19: Roll Time Tag (Ref 7.1.5.16) ---
    time_tag_roll = ScaledField(word_index=18, start_bit=0, width=16, lsb_value=64.0)

    # --- Word 20: Pitch Time Tag (Ref 7.1.5.17) ---
    time_tag_pitch = ScaledField(word_index=19, start_bit=0, width=16, lsb_value=64.0)

    # --- Word 21: Roll Axis Angular Acceleration (Ref 7.1.5.18) ---
    # Units: Semicircles/sec^2. LSB = 2.44141e-4
    ang_accel_roll = ScaledField(word_index=20, start_bit=0, width=16, lsb_value=2.44141e-4, signed=True)

    # --- Word 22: Pitch Axis Angular Acceleration (Ref 7.1.5.19) ---
    ang_accel_pitch = ScaledField(word_index=21, start_bit=0, width=16, lsb_value=2.44141e-4, signed=True)

    # --- Word 23: Yaw Axis Angular Acceleration (Ref 7.1.5.20) ---
    ang_accel_yaw = ScaledField(word_index=22, start_bit=0, width=16, lsb_value=2.44141e-4, signed=True)