import socket
import struct
import time
import numpy as np
from threading import Thread, Event
from queue import Queue

import config as cfg
import DataTypes as dt

# ============================================================
# Protocol definitions (MATCHES THE ULTIMATE .INO)
# ============================================================
PACKET_VERSION = 1
PKT_COMMAND = 1
PKT_CONFIG  = 2
PKT_DISPLAY = 3

FT_ARRAY_F32 = 1
FT_STRING    = 2
FT_UINT8     = 3
FT_CONFIG_VAL = 4

HEADER_FMT  = "<B B H I" # ID, Ver, Len, Timestamp
TLV_FMT = "<B H"

class ESP32Communication(Thread):
    def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port):
        super().__init__(daemon=True)
        self.addr = (host, port)
        
        # UDP Socket - Zero Lag
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        
        self.tx_queue = Queue()
        self.running = Event()
        self.running.set()
        self.last_motion_data = None
        print(f"Communication Initialized: Target {host}:{port}")

    def run(self):
        # --- THE FIX: Send Configs immediately upon starting the thread ---
        self.sync_robot_configs()
        
        while self.running.is_set():
            if not self.tx_queue.empty():
                data = self.tx_queue.get()
                try:
                    self.sock.sendto(data, self.addr)
                except Exception as e:
                    print(f"UDP Send Error: {e}")
            
            # Tiny sleep to prevent 100% CPU usage
            time.sleep(0.001)

    def sync_robot_configs(self):
        """
        Sends the parameters from your config.py to the ESP32.
        This tells the ESP32 how to map degrees to pulses and 
        how fast the Python loop is running for smoothing.
        """
        print("Sending Config Sync to Robot...")
        
        # Calculate interval in ms based on your FPS (tickpersec)
        interval_ms = int(1000 / cfg.standard_tickpersec)
        
        # Config IDs: 1=Min, 2=Max, 3=Interval
        configs = [
            (1, 125), # servoMin (Default for 50Hz)
            (2, 625), # servoMax (Default for 50Hz)
            (3, interval_ms)
        ]
        
        fields = []
        for c_id, val in configs:
            # Payload: 1 byte for ID, 2 bytes for Value (Little Endian)
            payload = struct.pack("<B h", c_id, val)
            fields.append(struct.pack(TLV_FMT, FT_CONFIG_VAL, len(payload)) + payload)
            
        packet = self._build_packet(PKT_CONFIG, fields)
        self.tx_queue.put(packet)

    def send_motion(self, radial_array: dt.RadArray):
        """Sends motion only if the data has changed."""
        # 1. Convert to degrees/uint8 as you did before
        new_data = np.degrees(radial_array.data).astype(np.uint8)
        
        # 2. Check if it's different from the last sent packet
        if self.last_motion_data is not None:
            if np.array_equal(new_data, self.last_motion_data):
                return  # Skip sending if data is identical
        
        # 3. Update the tracker and send
        self.last_motion_data = new_data.copy()
        
        fields = [self._tlv(FT_ARRAY_F32, new_data.tobytes())]
        packet = self._build_packet(PKT_COMMAND, fields)
        self.tx_queue.put(packet)

    def send_text(self, text=""):
        """Sends a dedicated display packet independent of motion."""
        # Convert string to bytes
        text_bytes = text.encode("utf-8")
    
        # Wrap in TLV format (Type 2 = FT_STRING)
        field = self._tlv(FT_STRING, text_bytes)
    
        # Build the packet using the PKT_DISPLAY (ID 3)
        packet = self._build_packet(PKT_DISPLAY, [field])
    
        self.tx_queue.put(packet)

    def _tlv(self, ftype, data):
        return struct.pack(TLV_FMT, ftype, len(data)) + data

    def _build_packet(self, packet_id, fields):
        payload = b"".join(fields)
        header = struct.pack(
            HEADER_FMT,
            packet_id,
            PACKET_VERSION,
            len(payload),
            int(time.time() * 1000) & 0xFFFFFFFF
        )
        return header + payload

    def stop(self):
        self.running.clear()
        self.sock.close()