import struct
import binascii
from ..lib1553.headers import UDP1553Header, UDP1553MessageHeader

# Debug flag
DEBUG_BUILD = False

class PacketBuilder:
    """
    Helper class to construct the proprietary UDP/1553 packets. 
    Matches the format from avddriverudp.cpp.
    
    Frame structure:
    - UDP Header (64 bytes)
    - For each message:
      - Marker Begin (0x3C3C)
      - Command Word (CW)
      - Status Word (SW) 
      - Error Code
      - Data payload (if BC->RT, i.e. TR=0)
      - ~CW (inverted command word)
      - Marker End (0x3E3E)
    - End Marker (0x5315)
    """
    
    RT_ADDRESS = 20  # Radar RT Address
    
    def __init__(self):
        self._mcounter = 0
        self._fcounter = 0

    def _build_command_word(self, rt, tr, sa, wc):
        """
        Build command word using C++ bitfield layout (little-endian):
        struct { wc:5, sa:5, tr:1, rt:5 }
        
        This means:
        - bits 0-4: wc (word count)
        - bits 5-9: sa (subaddress)
        - bit 10: tr (transmit/receive)
        - bits 11-15: rt (remote terminal)
        """
        wc_field = wc if wc < 32 else 0  # WC=0 means 32 words
        cw = (wc_field & 0x1F) | ((sa & 0x1F) << 5) | ((tr & 0x1) << 10) | ((rt & 0x1F) << 11)
        return cw

    def build_frame(self, messages):
        """
        Creates a complete UDP frame containing multiple 1553 messages.
        
        Args:
            messages: List of message objects with SUBADDRESS, IS_TRANSMIT, data
            
        Returns:
            bytes: Complete UDP1553 frame
        """
        # --- 1. Build UDP Header (64 bytes) ---
        udp_hdr = struct.pack('<H', UDP1553Header.MARKER_1553)  # 0x1553
        udp_hdr += struct.pack('<BB', 0, 0)  # vmajor, vminor
        udp_hdr += struct.pack('<H', UDP1553Header.OTYPE_BC)  # 0x4342 "BC"
        udp_hdr += struct.pack('<BB', 0, 0)  # ta=0 (BC), flags=0
        udp_hdr += struct.pack('<I', self._fcounter)  # fcounter
        udp_hdr += struct.pack('<I', self._mcounter)  # mcounter
        udp_hdr += struct.pack('<I', 0)  # scounter
        udp_hdr += struct.pack('<I', 0)  # mmiss
        udp_hdr += struct.pack('<I', 0)  # smiss
        udp_hdr += struct.pack('<I', 0)  # ltt (local timetag)
        udp_hdr += struct.pack('<I', 0)  # errors
        udp_hdr += struct.pack('<II', 0, 0)  # bc_reserved[2]
        udp_hdr += struct.pack('<II', 0, 0)  # rt_reserved[2]
        udp_hdr += struct.pack('<III', 0, 0, 0)  # spare[3]
        
        assert len(udp_hdr) == 64, f"UDP header should be 64 bytes, got {len(udp_hdr)}"
        
        # --- 2. Build message payloads ---
        msg_data = b""
        
        for msg in messages:
            rt = self.RT_ADDRESS
            tr = 1 if msg.IS_TRANSMIT else 0
            sa = msg.SUBADDRESS
            wc = getattr(msg, 'WORD_COUNT', 32)
            
            cw = self._build_command_word(rt, tr, sa, wc)
            
            # Message header: marker_begin, cw, sw, errcode
            msg_data += struct.pack('<H', UDP1553MessageHeader.MARKER_BEGIN)  # 0x3C3C
            msg_data += struct.pack('<H', cw)
            msg_data += struct.pack('<H', 0)  # SW = 0 for BC->RT
            msg_data += struct.pack('<H', 0)  # errcode = 0
            
            # Payload data (only for BC->RT messages, i.e. TR=0)
            if tr == 0:
                # Get data words
                data_words = list(msg.data[:wc])
                while len(data_words) < wc:
                    data_words.append(0)
                
                # Pack as little-endian (matching C++ on Windows)
                for w in data_words:
                    msg_data += struct.pack('<H', w)
            
            # Trailer: inverted CW + marker end
            inverted_cw = (~cw) & 0xFFFF
            msg_data += struct.pack('<H', inverted_cw)
            msg_data += struct.pack('<H', UDP1553MessageHeader.MARKER_END)  # 0x3E3E
        
        # --- 3. End marker ---
        end_marker = struct.pack('<H', UDP1553Header.MARKER_END_1553)  # 0x5315
        
        # --- 4. Assemble frame ---
        frame = udp_hdr + msg_data + end_marker
        
        # Update counters
        self._mcounter += 1
        
        if DEBUG_BUILD:
            print(f"  [PacketBuilder] Frame: {len(messages)} msgs, {len(frame)} bytes")
            print(f"  [PacketBuilder] Header hex: {udp_hdr[:16].hex()}")
        
        return frame

    def build_packet(self, msg_obj, is_request=False):
        """
        Creates a full UDP packet for a single 1553 Message Object.
        For backward compatibility - wraps build_frame for single message.
        """
        return self.build_frame([msg_obj])