"""
Simple Packet Builder - Uses the legacy format from qg1553overudp.cpp

This format is simpler than the full UDP1553 protocol:
- uint16_t number_of_messages
- For each message:
  - uint16_t ctrl_word (Command Word)
  - uint16_t data[32] (Payload)
"""
import struct

class SimplePacketBuilder:
    """
    Builds packets in the simple format used by qg1553overudp.cpp
    
    Format:
    - number_of_messages (uint16)
    - For each message:
      - ctrl_word (uint16): RT(5) | TR(1) | SA(5) | WC(5)
      - data[32] (32 x uint16): Payload words
    """
    
    RT_ADDRESS = 20  # Radar RT Address
    
    MAX_MESSAGES = 20  # Fixed size like C++ b1553_datagram_t
    
    def build_frame(self, messages):
        """
        Build a frame containing multiple messages.
        
        The frame has a FIXED size of 1322 bytes to match the C++ struct:
        - 2 bytes: number_of_messages (uint16)  
        - 20 x 66 bytes: message slots (ctrl_word + 32 data words each)
        
        Args:
            messages: List of message objects with SUBADDRESS, IS_TRANSMIT, data
            
        Returns:
            bytes: The complete frame (always 1322 bytes)
        """
        num_msgs = len(messages)
        if num_msgs > self.MAX_MESSAGES:
            num_msgs = self.MAX_MESSAGES
            messages = messages[:self.MAX_MESSAGES]
        
        # Start with number of messages (uint16, little endian)
        frame = struct.pack('<H', num_msgs)
        
        # Process actual messages
        for msg in messages:
            frame += self._build_message(msg)
        
        # Pad with empty messages to fill 20 slots
        empty_msg_bytes = b'\x00' * 66  # ctrl_word(2) + data[32](64)
        for _ in range(self.MAX_MESSAGES - num_msgs):
            frame += empty_msg_bytes
        
        return frame
    
    def _build_message(self, msg):
        """Build a single message (66 bytes: ctrl_word + 32 data words)."""
        rt = self.RT_ADDRESS
        tr = 1 if msg.IS_TRANSMIT else 0
        sa = msg.SUBADDRESS
        
        # Determine word count
        if hasattr(msg, 'WORD_COUNT'):
            wc = msg.WORD_COUNT
        else:
            wc = 32
        
        # In 1553, WC=0 means 32 words
        wc_field = wc if wc < 32 else 0
        
        # Pack as C++ does: struct with bitfields (little-endian bitfield order)
        # C++ struct layout:
        #   uint16_t rt: 5;  // bits 0-4
        #   uint16_t tr: 1;  // bit 5
        #   uint16_t sa: 5;  // bits 6-10  
        #   uint16_t wc: 5;  // bits 11-15
        cw = (rt & 0x1F) | ((tr & 0x1) << 5) | ((sa & 0x1F) << 6) | ((wc_field & 0x1F) << 11)
        
        # Command word
        result = struct.pack('<H', cw)
        
        # Add 32 data words (always 32, padded with zeros)
        data_words = list(msg.data[:32])
        while len(data_words) < 32:
            data_words.append(0)
        
        # Pack data as little endian (matching C++ on Windows)
        result += struct.pack('<32H', *data_words)
        
        return result
    
    def build_single_message(self, msg):
        """Build a frame with a single message."""
        return self.build_frame([msg])


# Test
if __name__ == "__main__":
    # Quick test
    class TestMsg:
        SUBADDRESS = 1
        IS_TRANSMIT = False
        WORD_COUNT = 10
        data = [0x1234, 0x5678] + [0] * 30
    
    builder = SimplePacketBuilder()
    pkt = builder.build_single_message(TestMsg())
    print(f"Packet length: {len(pkt)} bytes")
    print(f"Hex: {pkt.hex()}")
