import serial
import time
import glob

DEBUG = True

class RobotMovement:
    # -----------------------------
    # Inicializácia a konfigurácia
    # -----------------------------
    def __init__(self, port=None, baudrate=115200, timeout=1, sample_amount=3):
        """
        Inicializuje komunikáciu s Arduinom cez sériový port.
        Parametre:
        - port: Názov sériového portu (napr. '/dev/ttyUSB0').
        - baudrate: Rýchlosť prenosu dát (predvolená 115200).
        - timeout: Časový limit pre čítanie (predvolený 1 s).
        - sample_amount: Počet vzoriek pre zahriatie senzorového čítania.
        """
        paths = glob.glob('/dev/serial/by-id/usb-Arduino*')
        if paths:
            port = paths[0]
            if DEBUG:
                print(f"Found Arduino on {port}")
        else:
            if port is None:
                raise ValueError("No Arduino found in /dev/serial/by-id/ and no port was provided.")
            if DEBUG:
                print(f"No Arduino found in /dev/serial/by-id/. Using provided port: {port}")
        time.sleep(4)
        self.serial_connection = serial.Serial(port, baudrate, timeout=timeout)
        self.serial_connection.reset_input_buffer()

        for _ in range(sample_amount):
            self.get_sensor_data()  # Zahriatie — nevymazávať

    # -----------------------------
    # Sériové funkcie (interné)
    # -----------------------------
    def send_data(self, data):
        """
        INTERNÁ FUNKCIA. Posiela dáta do Arduina (ukončené '\n').
        """
        if not data.endswith('\n'):
            data += '\n'
        self.serial_connection.write(data.encode('utf-8'))

    def receive_data(self):
        """
        INTERNÁ FUNKCIA. Prijíma riadok dát z Arduina.
        """
        try:
            return self.serial_connection.readline().decode('utf-8').strip()
        except Exception:
            return "None"

    def wait_until_done(self, timeout=10.0):
        """
        INTERNÁ FUNKCIA. Blokuje kým z Arduina nepríde 'DONE:N'.
        Vracia:
        - N z 'DONE:N' (napr. totalKitsDeployed), alebo -1 po timeoute.
        Poznámka: Zámerné ignorovanie plain 'DONE' (bez dvojbodky) — tie
        sú interné navigačné správy Arduina, nie potvrdenia príkazov Pi.
        """
        start_time = time.monotonic()
        while time.monotonic() - start_time < timeout:
            data = self.receive_data().strip()
            if data and DEBUG:
                print(f"Received from Arduino: {data}")
            if data.upper().startswith('DONE:'):
                parts = data.split(':')
                try:
                    return int(parts[1])
                except (IndexError, ValueError):
                    return 0
            time.sleep(0.01)
        return -1

    def close(self):
        """
        Zatvára sériové pripojenie k Arduinu.
        """
        self.serial_connection.close()

    # -----------------------------
    # Príkazy pre Arduino
    # -----------------------------
    def drop_medkits_left(self, amount):
        """
        Požiada Arduino o vysadenie medkitov pre ľavú obeť.
        Arduino vykoná drop iba ak ľavý senzor meria < 15 cm.
        Vracia totalKitsDeployed, alebo -1 po timeoute.
        """
        self.send_data('DROPL:' + str(amount))
        return self.wait_until_done(timeout=max(10.0, amount * 4.0))

    def drop_medkits_right(self, amount):
        """
        Požiada Arduino o vysadenie medkitov pre pravú obeť.
        Arduino vykoná drop iba ak pravý senzor meria < 15 cm.
        Vracia totalKitsDeployed, alebo -1 po timeoute.
        """
        self.send_data('DROPR:' + str(amount))
        return self.wait_until_done(timeout=max(10.0, amount * 4.0))

    def get_sensor_data(self, index=None):
        """
        Vracia hodnoty senzorov z Arduina (len pre ladenie, Pi neriadi pohyb).
        Vracia:
        - Zoznam: [front_cm, back_cm, left_cm, right_cm, R, G, B, yaw]
        - Ak je zadaný index, vráti iba tú hodnotu.
        """
        raw_sensor_data = ""
        for _ in range(4):
            self.send_data('SENSOR:')
            raw_sensor_data = self.receive_data()
            if raw_sensor_data.count(':') >= 7:
                break
            if DEBUG and raw_sensor_data:
                print(f"Ignoring non-sensor response: {raw_sensor_data}")
            time.sleep(0.05)

        sensor_data = raw_sensor_data.split(':')
        while len(sensor_data) <= 7:
            sensor_data.append("0.0")
        for i in range(len(sensor_data)):
            try:
                sensor_data[i] = float(sensor_data[i].strip())
            except Exception:
                sensor_data[i] = 0.0

        if index is None:
            return sensor_data
        try:
            return sensor_data[index]
        except IndexError:
            return 0.0
