#!/usr/bin/env python3
"""
Compare A2 message encoding between Python and C++ reference.
Verifies bit-level compatibility with GrifoScope implementation.
"""

import sys
import os
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))

from pybusmonitor1553.lib1553.messages.msg_a2 import MsgA2
from pybusmonitor1553.lib1553.constants import (
    RadarMode, DesignationControl, IntBitStatus, StandbyStatus,
    FreezeStatus, RWSSubmode, RangeScale, BarScan, AzimuthScan
)

def print_word_bits(word, word_num):
    """Print a 16-bit word with bit positions."""
    print(f"\nWord {word_num}: 0x{word:04X} = 0b{word:016b}")
    print("Bit pos (MSB):  F E D C B A 9 8 7 6 5 4 3 2 1 0")
    print("               ", end="")
    for i in range(15, -1, -1):
        bit = (word >> i) & 1
        print(f"{bit} ", end="")
    print()
    

def analyze_a2_message():
    """Create and analyze an A2 message matching GrifoScope defaults."""
    msg = MsgA2()
    
    # Configuration matching GrifoScope startup (from g346_a1a2.cpp lines 680-682)
    # Phase 1: Protection mode
    msg.master_mode = RadarMode.RWS          # 0
    msg.designation_ctrl = DesignationControl.NOT_VALID  # 7
    msg.int_bit_req = IntBitStatus.NORMAL    # 0
    msg.standby_cmd = StandbyStatus.ON       # 1 (protection)
    msg.freeze_cmd = FreezeStatus.NORMAL     # 0
    msg.pwr_up_stop_reserved = 0
    msg.reserved_w1_b12 = 0
    msg.silence_reserved = 1                 # 1 (protection)
    
    # Word 2 params
    msg.rws_submode = RWSSubmode.NAM
    msg.range_scale = RangeScale.NM_80
    msg.bar_scan = BarScan.BAR_2
    msg.azimuth_scan = AzimuthScan.DEG_60
    
    # Pack the message
    msg.pack()
    
    print("="*70)
    print("A2 MESSAGE ANALYSIS - PHASE 1 PROTECTION MODE")
    print("="*70)
    print("\n📋 Configuration (matching GrifoScope g346_a1a2.cpp:680-682):")
    print(f"   master_mode       = {msg.master_mode.name:20s} ({msg.master_mode.value})")
    print(f"   designation_ctrl  = {msg.designation_ctrl.name:20s} ({msg.designation_ctrl.value})")
    print(f"   int_bit_req       = {msg.int_bit_req.name:20s} ({msg.int_bit_req.value})")
    print(f"   standby_cmd       = {msg.standby_cmd.name:20s} ({msg.standby_cmd.value}) ← PROTECTION")
    print(f"   silence_reserved  = {msg.silence_reserved:20d} ← PROTECTION")
    
    print("\n📊 Binary word breakdown:")
    for i in range(3):
        word = msg._data[i]
        print_word_bits(word, i)
        
        if i == 0:
            # Extract fields manually to verify
            master_mode = (word >> 12) & 0xF
            des_ctrl = (word >> 9) & 0x7
            ibit = (word >> 8) & 0x1
            stby = (word >> 7) & 0x1
            freeze = (word >> 6) & 0x1
            pwr_stop = (word >> 5) & 0x1
            res12 = (word >> 4) & 0x1
            silence = (word >> 3) & 0x1
            
            print(f"   Decoded fields:")
            print(f"     Bits 00-03: master_mode    = {master_mode} ({RadarMode(master_mode).name})")
            print(f"     Bits 04-06: des_ctrl       = {des_ctrl} ({DesignationControl(des_ctrl).name})")
            print(f"     Bit  07:    ibit           = {ibit}")
            print(f"     Bit  08:    stby           = {stby} ← Should be 1")
            print(f"     Bit  09:    freeze         = {freeze}")
            print(f"     Bit  10:    pwr_stop       = {pwr_stop}")
            print(f"     Bit  11:    reserved       = {res12}")
            print(f"     Bit  12:    silence        = {silence} ← Should be 1")
    
    # Now test PHASE 2: Operational mode
    print("\n" + "="*70)
    print("PHASE 2: OPERATIONAL MODE (after 120ms)")
    print("="*70)
    
    msg.standby_cmd = StandbyStatus.OFF
    msg.silence_reserved = 0
    msg.pack()
    
    print(f"\n📋 Updated configuration:")
    print(f"   standby_cmd       = {msg.standby_cmd.name:20s} ({msg.standby_cmd.value}) ← OPERATIONAL")
    print(f"   silence_reserved  = {msg.silence_reserved:20d} ← OPERATIONAL")
    
    print_word_bits(msg._data[0], 0)
    
    word = msg._data[0]
    stby = (word >> 7) & 0x1
    silence = (word >> 3) & 0x1
    print(f"   Bit  08:    stby           = {stby} ← Should be 0")
    print(f"   Bit  12:    silence        = {silence} ← Should be 0")
    
    # Expected C++ encoding (from g346_a1a2.cpp)
    print("\n" + "="*70)
    print("EXPECTED C++ REFERENCE VALUES")
    print("="*70)
    print("""
From g346_a1a2.cpp lines 680-682:
    dSilence.setFromUser(1);  // Bit 12 (MSB) = 1
    dStby.setFromUser(1);      // Bit 08 (MSB) = 1

Expected Word 0 (Phase 1):
    master_mode=0 (RWS), des_ctrl=7 (NOT_VALID), ibit=0, stby=1, 
    freeze=0, pwr_stop=0, res=0, silence=1
    
Bit pattern (MSB first, 0-indexed):
    Bits 00-03: master_mode = 0000 (0)
    Bits 04-06: des_ctrl    = 111  (7)
    Bit  07:    ibit        = 0
    Bit  08:    stby        = 1
    Bit  09:    freeze      = 0
    Bit  10:    pwr_stop    = 0
    Bit  11:    reserved    = 0
    Bit  12:    silence     = 1
    Bits 13-15: spare       = 00
    
    = 0b 0000 1110 1000 1000 = 0x0E88

Expected Word 0 (Phase 2):
    Same but stby=0, silence=0
    = 0b 0000 1110 0000 0000 = 0x0E00
""")
    
    # Verify actual encoding
    expected_phase1 = 0x0E88  # Corrected calculation
    expected_phase2 = 0x0E00
    
    # Re-create phase 1
    msg.standby_cmd = StandbyStatus.ON
    msg.silence_reserved = 1
    msg.pack()
    actual_phase1 = msg._data[0]
    
    print(f"\n✓ Verification:")
    print(f"  Phase 1: Expected=0x{expected_phase1:04X}, Actual=0x{actual_phase1:04X}", end="")
    if actual_phase1 == expected_phase1:
        print(" ✓ MATCH")
    else:
        print(f" ✗ MISMATCH (diff: 0x{actual_phase1 ^ expected_phase1:04X})")
    
    msg.standby_cmd = StandbyStatus.OFF
    msg.silence_reserved = 0
    msg.pack()
    actual_phase2 = msg._data[0]
    
    print(f"  Phase 2: Expected=0x{expected_phase2:04X}, Actual=0x{actual_phase2:04X}", end="")
    if actual_phase2 == expected_phase2:
        print(" ✓ MATCH")
    else:
        print(f" ✗ MISMATCH (diff: 0x{actual_phase2 ^ expected_phase2:04X})")


if __name__ == "__main__":
    analyze_a2_message()