import socket
import struct

PACKET_VERSION = 1
FLOAT_COUNT = 30 * 6 * 3

PACKET_FMT = f"<H I B B {FLOAT_COUNT}f"
PACKET_SIZE = struct.calcsize(PACKET_FMT)

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

import config as cfg
import DataTypes as dt


class ESP32Communication(Thread):
    def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port, timeout=cfg.timeout):
        super().__init__()
        self.daemon = True
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.settimeout(timeout)

        print(f"Connecting to ESP32 at {host}:{port} ...")
        self.sock.connect((host, port))
        print("Connected to ESP32!")

        self.command_queue = Queue()
        self.response_queue = Queue()
        self.running = Event()
        self.running.set()

    def run(self):
        while self.running.is_set():
            # 1. Send queued commands
            if not self.command_queue.empty():
                command = self.command_queue.get()
                self._write(command)

            # 2. Read response
            self.sock.setblocking(False)
            try:
                data = self.sock.recv(1024)
                if data:
                    line = data.decode('utf-8', errors='ignore').strip()
                    print(f"[ESP32] {line}")
                    self.response_queue.put(line)
            except BlockingIOError:
                pass

            time.sleep(0.01)

    def write(self, target):
        if isinstance(target, dt.RadArray):
            target = dt.DegArray(np.degrees(target.data))
        leg_str = '\n'.join([','.join(map(str, row)) for row in target])
        formatted_data = f"<{leg_str}>"
        print(f"Gesendete Daten:\n{formatted_data}")
        self._write(formatted_data)

    def wait4esp32(self, expected_message):
        while True:
            try:
                data = self.sock.recv(1024).decode('utf-8').strip()
                print(f"Empfangene Daten: {data}")
                if data == expected_message:
                    print("Bewegung abgeschlossen!")
                    break
            except socket.timeout:
                time.sleep(0.05)

    def _write(self, message):
        self.sock.sendall(message.encode('utf-8'))
        
    def stop(self):
        self.running.clear()
        self.join()
        self.sock.close()