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. Na Linuxe sa pokusi najst port automaticky. Na Windows je nutné zadať port.
        Parametre:
        - port: Názov sériového portu (napr. 'COM3' pre Windows alebo '/dev/ttyUSB0' pre Linux).
        - baudrate: Rýchlosť prenosu dát (predvolená hodnota 115200).
        - timeout: Časový limit pre čítanie dát zo sériového portu (predvolená hodnota 0.1 sekundy).
        - sample_amount: Počet vzoriek pre meranie senzorov (predvolená hodnota 3).
        """
        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()  # Je tu, aby dalsie merania senzorov boli presne. Dovod neznami. NEVYMAZAVAT.
       

    # -----------------------------
    # Sériové funkcie (interné)
    # -----------------------------
    def _wait_for_ready(self, timeout=20.0):
        """
        INTERNÁ FUNKCIA. NEPOUŽÍVAŤ JU PRIAMO.
        Caka na spravu 'DONE' z Arduina po dokonceni setup(). Blokuje az do timeoutu.
        ### Parametre:
            - timeout: Maximálny čas čakania v sekundách.
        """
        if DEBUG:
            print("Cakam na DONE z Arduina (setup)...")
        start = time.monotonic()
        while time.monotonic() - start < timeout:
            self.send_data("START:0")  # Spustí init_all na Arduine
            line = self.receive_data()
            if DEBUG and line:
                print(f"Setup: {line}")
            if line.upper().startswith('DONE'):
                if DEBUG:
                    print("Arduino setup dokonceny.")
                return
            time.sleep(0.05)
        if DEBUG:
            print("Timeout: Arduino neodpovedalo DONE po setup.")

    def send_data(self, data):
        """
        INTERNÁ FUNKCIA. NEPOUŽÍVAŤ JU PRIAMO.
        Posiela dáta do Arduina (ukončené znakom '\n').
        """
        if not data.endswith('\n'):
            data += '\n'
        self.serial_connection.write(data.encode('utf-8'))

    def receive_data(self):
        """
        INTERNÁ FUNKCIA. NEPOUŽÍVAŤ JU PRIAMO.
        Prijíma riadok dát z Arduina a vracia ho ako reťazec.
        """
        try:
            return self.serial_connection.readline().decode('utf-8').strip()
        except:
            return "None"

    def wait_until_done(self, timeout=2.0):
        """
        INTERNÁ FUNKCIA. NEPOUŽÍVAŤ JU PRIAMO.
        Blokuje vykonávanie, kým z Arduina nepríde správa 'DONE' alebo 'DONE:N'.
        Parametre:
        - timeout: Maximálny čas čakania v sekundách.
        Vracia:
        - Hodnota N z 'DONE:N' (napr. skutocna prekonana vzdialenost), 0 pre plain 'DONE', -1 po timeoute.
        """
        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(':')
                if len(parts) >= 2:
                    try:
                        return int(parts[1])
                    except ValueError:
                        pass
                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 turn_right(self, angle):
        """
        Otočí robotom doprava o zadaný uhol.
        Parametre:
        - angle: Uhol otočenia v stupňoch. MAX: 180°
        """
        self.send_data('RIGHT:' + str(angle))
        self.wait_until_done(timeout=max(3.0, angle / 30.0))

    def turn_left(self, angle):
        """
        Otočí robotom doľava o zadaný uhol.
        Parametre:
        - angle: Uhol otočenia v stupňoch. MAX: 180°
        """
        self.send_data('LEFT:' + str(angle))
        self.wait_until_done(timeout=max(3.0, angle / 30.0))

    def set_speed(self, amount):
        """
        Nastavi rychlost robota. V rozmedzí od -100 do 100.
        Zaporne hodnoty = pohyb dozadu, kladne = pohyb dopredu, 0 = stop.
        ### Parametre:
            - amount: Rychlost pohybu (-100 az 100).
        """
        self.send_data('SPEED:' + str(amount))

    def drop_medkits(self, amount):
        """
        Spustí mechanizmus na vysadenie medkitov.
        Parametre:
        - amount: Počet medkitov na vysadenie.
        """
        self.send_data('DROP:' + str(amount))
        self.wait_until_done()

    def get_sensor_data(self, index=None):
        """
        Vracia hodnoty senzorov z Arduina vo forme zoznamu čísel.
        Parametre:
        - index (voliteľné): Index konkrétneho senzora (od 0).
        Vracia:
        - Ak nie je index zadaný, vráti všetky hodnoty senzorov ako zoznam čísel.
        - Ak je index zadaný, vráti hodnotu konkrétneho senzora ako číslo.
            - Indexy:
                 0. Predna vzdialenost (cm)
                 1. Zadna vzdialenost (cm)
                 2. Lava vzdialenost (cm)
                 3. Prava vzdialenost (cm)
                 4. R - farebny senzor
                 5. G - farebny senzor
                 6. B - farebny senzor
                 7. Relativne otocenie (yaw)
        """
        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:
                sensor_data[i] = 0.0

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


