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

import config as cfg
import DataTypes as dt

class ArduinoCommunication(Thread):
    def __init__(self, port=cfg.port, baudrate=cfg.baudrate, timeout=cfg.timeout):
        super().__init__()
        self.daemon = True  # Thread schließt sich beim Programmende
        self.serial_conn  = serial.Serial(port, baudrate, timeout=timeout)
        time.sleep(2)  # Warten bis Arduino ready

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

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

            # 2. Antwort lesen (falls vorhanden)
            if self.serial_conn.in_waiting > 0:
                line = self.serial_conn.readline().decode('utf-8', errors='ignore').strip()
                if line:
                    print(f"[Arduino] {line}")
                    self.response_queue.put(line)

            time.sleep(0.01)

    # Werte auf Arduino schreiben
    def write(self, target):
        if isinstance(target, dt.RadArray):
            target = dt.DegArray(np.degrees(target.data))
        if self.serial_conn and self.serial_conn.is_open:
            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.serial_conn.write(bytes(formatted_data, 'utf-8'))
    
    # Werte von Arduino ablesen und Synchronisierunga
    def wait4arduino(self, expected_message):
        while True:
            # Lese eine Zeile von der seriellen Verbindung
            data = self.serial_conn .readline()
        
            # Konvertiere die Zeile in einen lesbaren String
            data = str(data, "utf-8").strip("\r\n")
        
            # Debugging-Ausgabe (optional)
            print(f"Empfangene Daten: {data}")
        
            # Ueberpruefe, ob die empfangene Nachricht der erwarteten Nachricht entspricht
            if data == expected_message:
                print("Bewegung abgeschlossen!")
                break
            else:
                time.sleep(0.05) 

    def _write(self, message):
        if self.serial_conn and self.serial_conn.is_open:
            self.serial_conn.write(message.encode('utf-8'))

    def stop(self):
        self.running.clear()
        self.join()
        self.serial_conn.close()