import socket
import struct
import time
import sys
import os

# Add parent directory to path to import lib1553
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))

from lib1553.headers import UDP1553Header, UDP1553MessageHeader, CommandWordUnion
from lib1553.messages.msg_a1 import MsgA1
from lib1553.messages.msg_b5 import MsgB5
from lib1553.constants import TargetHistory, AltitudeBlock

TARGET_IP = "127.0.0.1"
TARGET_PORT = 61553 # Must match __main__.py RX_PORT

def create_packet(msg_obj):
    """Wraps a 1553 message object into the proprietary UDP packet structure."""
    
    # 1. Create UDP Header
    udp_header = UDP1553Header()
    # (Other fields default to 0/correct constants)
    
    # 2. Create 1553 Message Header (Command Word)
    # Note: We simulate the Command Word based on the Message properties
    # RT Address is arbitrary here (e.g. 1)
    # Word Count: In 1553, 32 words is represented as 0. 
    wc_val = 32 if len(msg_obj.data) == 32 else len(msg_obj.data)
    if wc_val == 32: wc_val = 0
    
    cw = CommandWordUnion(
        rt_addr=1,
        sub_addr=msg_obj.SUBADDRESS,
        word_count=wc_val,
        transmit=msg_obj.IS_TRANSMIT
    )
    
    msg_header = UDP1553MessageHeader(command_word_union=cw)
    
    # 3. Serialize Data
    payload = msg_obj.pack()
    
    # 4. Combine
    return bytes(udp_header) + bytes(msg_header) + payload

def main():
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    
    print(f"Simulating Traffic -> {TARGET_IP}:{TARGET_PORT}")
    
    try:
        while True:
            # --- Send Message A1 (Settings) ---
            a1 = MsgA1()
            # Populate some fields to verify decoding
            a1.target_history = TargetHistory.LEVEL_4
            a1.symbol_intensity = 99
            a1.beacon_delay = 12.5 # microseconds
            a1.altitude_block = AltitudeBlock.TPBK
            
            data_a1 = create_packet(a1)
            sock.sendto(data_a1, (TARGET_IP, TARGET_PORT))
            print("Sent Msg A1")
            
            time.sleep(0.5)
            
            # --- Send Message B5 (Tracked Target) ---
            b5 = MsgB5()
            b5.target_range = 15000 # ft
            b5.target_cas = 350.5   # knots
            b5.norm_pos_x = 0.8     # Normalized
            b5.vel_x = 450.0        # ft/s
            b5.ant_elevation = 0.12 # Semicircles
            
            data_b5 = create_packet(b5)
            sock.sendto(data_b5, (TARGET_IP, TARGET_PORT))
            print("Sent Msg B5")
            
            time.sleep(1.5)

    except KeyboardInterrupt:
        print("Simulation stopped.")
    finally:
        sock.close()

if __name__ == "__main__":
    main()