# ---------------------------------------------------------------------------
# Hlavný kontrolér v2 — pohyb po dlaždiciach s mapovaním bludiska
# ---------------------------------------------------------------------------

import cv2
import sys
import victim_recognition_26 as vc
import mapping
import robot_movement_v2 as rm
from picamera2 import Picamera2
import random, threading, time
try:
    import RPi.GPIO as GPIO
    GPIO.setmode(GPIO.BCM)
except ImportError:
    class GPIO:
        BCM = HIGH = LOW = OUT = None
        @staticmethod
        def setup(*a): pass
        @staticmethod
        def output(*a): pass
        @staticmethod
        def cleanup(*a): pass

# ---------------------------------------------------------------------------
# Konštanty
# ---------------------------------------------------------------------------

ROBOT_LENGHT = 20
SAFE_DISTANCE = 10
TILE_SIZE = 30
PROBE_DISTANCE = 10
VICTIM_WALL_DISTANCE = 15
DEBUG = True
ROUND_DURATION_S = 8 * 60
RETURN_THRESHOLD_S = ROUND_DURATION_S - 30

medkit_values = {
    'Omega': 0, 'Phi': 2, 'Psi': 1,
    '2': 2, '1': 1, '0': 0
}

running = True

# ---------------------------------------------------------------------------
# GPIO
# ---------------------------------------------------------------------------

RGB_PIN_B = 17
RGB_PIN_G = 27
RGB_PIN_R = 22

GPIO.setup(RGB_PIN_R, GPIO.OUT)
GPIO.setup(RGB_PIN_G, GPIO.OUT)
GPIO.setup(RGB_PIN_B, GPIO.OUT)
GPIO.output(RGB_PIN_R, GPIO.LOW)
GPIO.output(RGB_PIN_G, GPIO.LOW)
GPIO.output(RGB_PIN_B, GPIO.LOW)

# ---------------------------------------------------------------------------
# Kamery
# ---------------------------------------------------------------------------

try:
    left_camera = Picamera2(0)
    left_camera.configure(left_camera.create_preview_configuration(main={"format": "RGB888"}))
    left_camera.start()
except Exception as e:
    print(f"Error: ľavá kamera nenájdená (index 0): {e}")
    running = False

try:
    right_camera = Picamera2(1)
    right_camera.configure(right_camera.create_preview_configuration(main={"format": "RGB888"}))
    right_camera.start()
except Exception as e:
    print(f"Error: pravá kamera nenájdená (index 1): {e}")
    running = False

# ---------------------------------------------------------------------------
# Inicializácia
# ---------------------------------------------------------------------------

robot_movement = rm.RobotMovement()

identified_victims = set()
last_left_victim_tile = None   # Suradnice dlazdice kde bola naposledy detekovana obeta vlavo
last_right_victim_tile = None  # Suradnice dlazdice kde bola naposledy detekovana obeta vpravo

DIRECTION_OFFSETS = {
    "north": (0, 1),
    "east": (1, 0),
    "south": (0, -1),
    "west": (-1, 0)
}

LEFT_OF = {"north": "west", "west": "south", "south": "east", "east": "north"}
RIGHT_OF = {"north": "east", "east": "south", "south": "west", "west": "north"}
OPPOSITE_OF = {"north": "south", "south": "north", "east": "west", "west": "east"}

x, y = 0, 0
direction = "north"

# ---------------------------------------------------------------------------
# Pomocné funkcie
# ---------------------------------------------------------------------------

def get_direction_from_gyro(gyro_value):
    angle = gyro_value % 360
    if angle < 0:
        angle += 360
    if 315 <= angle or angle < 45:
        return "north"
    if 45 <= angle < 135:
        return "west"
    if 135 <= angle < 225:
        return "south"
    return "east"

def get_tile_color(sensor_values):
    r = sensor_values[4]
    g = sensor_values[5]
    b = sensor_values[6]
    if r < 30 and g < 30 and b < 30:
        return "black"
    if r < 30 and g < 30 and b > 200:
        return "blue"
    return "white"

def get_next_position(pos_x, pos_y, move_direction):
    dx, dy = DIRECTION_OFFSETS[move_direction]
    return pos_x + dx, pos_y + dy

def is_tile_blocked(nx, ny):
    """Skontroluje ci je dlazdica v mape cierna alebo modra (swamp)."""
    tile_info = mapping.get_tile(nx, ny)
    if tile_info is None:
        return False
    if tile_info["color"] == "black":
        return True
    if tile_info["color"] == "blue":
        return True
    return False

def blink_led(pin, duration=2.0, interval=0.2):
    """
    Zabliká LED na danom pine po zadanu dobu.
    ### Parametre:
        - pin: GPIO pin LED
        - duration: celková doba blikania (sekundy)
        - interval: doba jedného stavu (sekundy)
    """
    end_time = time.monotonic() + duration
    state = True
    while time.monotonic() < end_time:
        GPIO.output(pin, GPIO.HIGH if state else GPIO.LOW)
        state = not state
        time.sleep(interval)
    GPIO.output(pin, GPIO.LOW)

# ---------------------------------------------------------------------------
# Funkcie pre návrat na štart
# ---------------------------------------------------------------------------

def get_sensor_for_direction(facing, target_dir):
    """
    Vrati index senzora pre dany  smer vzhladom na aktualny smer robota.
    ### Parametre:
        - facing: aktualny smer robota
        - target_dir:  smer ktory chceme skontrolovat
    ### Vracia:
        - index senzora (0=predu, 1=vzadu, 2=vlavo, 3=vpravo)
    """
    if target_dir == facing:
        return 0
    if target_dir == LEFT_OF[facing]:
        return 2
    if target_dir == RIGHT_OF[facing]:
        return 3
    return 1

def turn_to_direction(target_dir):
    """
    Otoci robota zo sucasneho smeru na cielovy smer.
    ### Parametre:
        - target_dir: cielovy smer ("north", "east", "south", "west")
    """
    global direction
    if direction == target_dir:
        return
    if target_dir == LEFT_OF[direction]:
        robot_movement.turn_left(90)
    elif target_dir == RIGHT_OF[direction]:
        robot_movement.turn_right(90)
    else:
        robot_movement.turn_left(180)
    direction = target_dir

def get_return_directions(current_x, current_y):
    """
    Vrati zoznam smerov zoradeny podla priority pre navrat na (0,0).
    Optimisticky pristup — preferuje os s vacsou vzdialenostou od startu.
    ### Parametre:
        - current_x: aktualna x suradnica
        - current_y: aktualna y suradnica
    ### Vracia:
        - zoznam smerov zoradeny od najlepsieho po najhorsi
    """
    moves = []
    if current_x > 0:
        moves.append(("west", abs(current_x)))
    elif current_x < 0:
        moves.append(("east", abs(current_x)))
    if current_y > 0:
        moves.append(("south", abs(current_y)))
    elif current_y < 0:
        moves.append(("north", abs(current_y)))

    moves.sort(key=lambda m: m[1], reverse=True)
    preferred = [m[0] for m in moves]

    for d in ["north", "east", "south", "west"]:
        if d not in preferred:
            preferred.append(d)

    return preferred

def navigate_to_start():
    """
    Maze-mouse navigacia spat na (0,0) s optimistickym pristupom.
    Robot sa pokusi ist najkratsou cestou, adaptuje sa na steny.
    Neprekroci modru ani nevstúpi na ciernu.
    """
    global x, y, direction, running

    if DEBUG:
        print("[NAVRAT] Zacinam navrat na start (0,0)")

    while running and (x != 0 or y != 0):
        elapsed = time.monotonic() - start_time
        if elapsed >= ROUND_DURATION_S:
            if DEBUG:
                print("[NAVRAT] Cas vyprsal, koncim")
            break

        sensor_data = robot_movement.get_sensor_data()
        try:
            direction = get_direction_from_gyro(sensor_data[7])
        except Exception:
            pass

        if DEBUG:
            print(f"[NAVRAT] Pozicia: ({x}, {y}), Smer: {direction}")

        preferred_dirs = get_return_directions(x, y)

        moved = False
        for target_dir in preferred_dirs:
            sensor_idx = get_sensor_for_direction(direction, target_dir)
            if sensor_data[sensor_idx] <= SAFE_DISTANCE:
                continue

            nx, ny = get_next_position(x, y, target_dir)
            if is_tile_blocked(nx, ny):
                continue

            turn_to_direction(target_dir)
            moved = probe_and_move(target_dir)
            if moved:
                break

            # Sonda zlyhala, precitaj senzory znova pre dalsi pokus
            sensor_data = robot_movement.get_sensor_data()
            try:
                direction = get_direction_from_gyro(sensor_data[7])
            except Exception:
                pass

        if not moved:
            if DEBUG:
                print("[NAVRAT] Ziadny volny smer, cuvam")
            sensor_data = robot_movement.get_sensor_data()
            if sensor_data[1] > SAFE_DISTANCE:
                robot_movement.move_reverse(TILE_SIZE)
                dx, dy = DIRECTION_OFFSETS[direction]
                x -= dx
                y -= dy
            elif DEBUG:
                print("[NAVRAT] Za mnou je stena, nemozem cuvat")

    if DEBUG:
        if x == 0 and y == 0:
            print("[NAVRAT] Uspesne som sa vratil na start!")
        else:
            print(f"[NAVRAT] Nepodarilo sa vratit, pozicia: ({x}, {y})")

def probe_and_move(move_direction):
    """
    Sonduje nasledujuce policko a ak nie je cierne, dokonci pohyb.
    Na modrej sa vycentruje. Vracia True ak sa pohyb podaril.
    Pocas sondy skenuje obete — robot je blizsie k stene ako v strede dlazdice.
    """
    global x, y, last_left_victim_tile, last_right_victim_tile

    # Sonda — pol dlazdice vpred, precita farbu
    if DEBUG:
        print(f"[DEBUG] probe_and_move: move_direction={move_direction}")
    probe_actual = robot_movement.move_forward(PROBE_DISTANCE)
    if DEBUG:
        print(f"[DEBUG] probe_and_move: probe_actual={probe_actual}")

    data = robot_movement.get_sensor_data()
    if DEBUG:
        print(f"[DEBUG] probe_and_move: sensor_data={data}")
    color = get_tile_color(data)
    if DEBUG:
        print(f"[DEBUG] probe_and_move: detected color={color}")
    nx, ny = get_next_position(x, y, move_direction)
    if DEBUG:
        print(f"[DEBUG] probe_and_move: next pos=({nx}, {ny})")

    # Cierna — vrat sa, do policka nevstupuj
    if color == "black":
        if DEBUG:
            print(f"[DEBUG] Sonda: cierna na ({nx}, {ny}), vraciam sa")
        robot_movement.move_reverse(probe_actual)
        mapping.update_tile(nx, ny, color="black", direction=move_direction)
        return False

    # Skenuj obete pocas sondy — robot je v sonde blizsie k bočnej stene ako v strede
    if data[2] < VICTIM_WALL_DISTANCE and last_left_victim_tile != (nx, ny):
        left_readings = []
        for _ in range(3):
            left_frame = cv2.cvtColor(left_camera.capture_array(), cv2.COLOR_RGB2BGR)
            left_readings.append(vc.get_victim_on_wall(left_frame, flip_mode=2))
        left_victim = max(set(left_readings), key=left_readings.count)
        if left_victim != "None":
            if ((nx, ny), left_victim) not in identified_victims:
                identified_victims.add(((nx, ny), left_victim))
                mapping.update_tile(nx, ny, victim=left_victim)
                blink_led(RGB_PIN_R, duration=2.0, interval=0.2)
                robot_movement.drop_medkits_left(medkit_values.get(left_victim, 0))
            last_left_victim_tile = (nx, ny)

    if data[3] < VICTIM_WALL_DISTANCE and last_right_victim_tile != (nx, ny):
        right_readings = []
        for _ in range(3):
            right_frame = cv2.cvtColor(right_camera.capture_array(), cv2.COLOR_RGB2BGR)
            right_readings.append(vc.get_victim_on_wall(right_frame, flip_mode=2))
        right_victim = max(set(right_readings), key=right_readings.count)
        if right_victim != "None":
            if ((nx, ny), right_victim) not in identified_victims:
                identified_victims.add(((nx, ny), right_victim))
                mapping.update_tile(nx, ny, victim=right_victim)
                blink_led(RGB_PIN_R, duration=2.0, interval=0.2)
                robot_movement.drop_medkits_right(medkit_values.get(right_victim, 0))
            last_right_victim_tile = (nx, ny)

    # Biela alebo modra (prvy vstup) — dokonci pohyb
    remaining = TILE_SIZE - probe_actual
    if DEBUG:
        print(f"[DEBUG] probe_and_move: remaining={remaining}")
    if remaining > 0:
        robot_movement.move_forward(remaining)

    if color == "blue" and DEBUG:
        print(f"[DEBUG] Sonda: modra (swamp) na ({nx}, {ny}), vstupujem prvy krat")

    x, y = nx, ny
    if DEBUG:
        print(f"[DEBUG] probe_and_move: updated pos=({x}, {y})")
    mapping.update_tile(x, y, color=color, direction=move_direction)
    return True

def terminal_listener():
    global running
    while True:
        command = sys.stdin.readline().strip()
        if command:
            running = False
            robot_movement.close()
            break

# ---------------------------------------------------------------------------
# Hlavný program
# ---------------------------------------------------------------------------

if DEBUG:
    threading.Thread(target=terminal_listener, daemon=True).start()

start_time = time.monotonic()

sensor_data = robot_movement.get_sensor_data()
direction = get_direction_from_gyro(sensor_data[7])
color = get_tile_color(sensor_data)
mapping.update_tile(x, y, color=color, direction=direction)

while running:
    # Pauza 1 sekunda po kazdom nastupi na dlazdicu
    time.sleep(1)

    # Kontrola casoveho limitu — navrat na start
    elapsed = time.monotonic() - start_time
    if elapsed >= RETURN_THRESHOLD_S:
        if DEBUG:
            print(f"[DEBUG] Zostava {ROUND_DURATION_S - elapsed:.0f}s, zacinam navrat na start")
        navigate_to_start()
        break

    sensor_data = robot_movement.get_sensor_data()
    if DEBUG:
        print(f"[DEBUG] main loop: sensor_data={sensor_data}")
    try:
        direction = get_direction_from_gyro(sensor_data[7])
    except Exception as e:
        if DEBUG:
            print(f"[ERROR] get_direction_from_gyro failed: {e}")
        direction = "north"

    if DEBUG:
        print(f"[DEBUG] main loop: Pozicia: ({x}, {y}), Smer: {direction}")

    # Detekcia obeti (kamery) — skenuje iba keď je stena bližšie ako VICTIM_WALL_DISTANCE
    # Zachytí 3 snímky a použije väčšinové hlasovanie.
    if sensor_data[2] < VICTIM_WALL_DISTANCE:  # ľavá stena
        if last_left_victim_tile != (x, y):
            left_readings = []
            for _ in range(3):
                left_frame = cv2.cvtColor(left_camera.capture_array(), cv2.COLOR_RGB2BGR)
                left_readings.append(vc.get_victim_on_wall(left_frame, flip_mode=2))
            left_victim = max(set(left_readings), key=left_readings.count)
            if left_victim != "None":
                if ((x, y), left_victim) not in identified_victims:
                    identified_victims.add(((x, y), left_victim))
                    mapping.update_tile(x, y, victim=left_victim)
                    blink_led(RGB_PIN_R, duration=3.0, interval=0.5)
                    robot_movement.drop_medkits_left(medkit_values.get(left_victim, 0))
                last_left_victim_tile = (x, y)

    if sensor_data[3] < VICTIM_WALL_DISTANCE:  # pravá stena
        if last_right_victim_tile != (x, y):
            right_readings = []
            for _ in range(3):
                right_frame = cv2.cvtColor(right_camera.capture_array(), cv2.COLOR_RGB2BGR)
                right_readings.append(vc.get_victim_on_wall(right_frame, flip_mode=2))
            right_victim = max(set(right_readings), key=right_readings.count)
            if right_victim != "None":
                if ((x, y), right_victim) not in identified_victims:
                    identified_victims.add(((x, y), right_victim))
                    mapping.update_tile(x, y, victim=right_victim)
                    blink_led(RGB_PIN_R, duration=3.0, interval=0.5)
                    robot_movement.drop_medkits_right(medkit_values.get(right_victim, 0))
                last_right_victim_tile = (x, y)

    # Vyber smeru
    forward_ok = sensor_data[0] > SAFE_DISTANCE
    left_ok = sensor_data[2] > SAFE_DISTANCE
    right_ok = sensor_data[3] > SAFE_DISTANCE

    options = []
    if forward_ok:
        options.append("forward")
    if left_ok:
        options.append("left")
    if right_ok:
        options.append("right")
    random.shuffle(options)

    moved = False
    for option in options:
        if option == "forward":
            nx, ny = get_next_position(x, y, direction)
            if is_tile_blocked(nx, ny):
                continue
            moved = probe_and_move(direction)
            if moved:
                break

        elif option == "left":
            left_dir = LEFT_OF[direction]
            nx, ny = get_next_position(x, y, left_dir)
            if is_tile_blocked(nx, ny):
                continue
            robot_movement.turn_left(90)
            direction = left_dir
            moved = probe_and_move(left_dir)
            if moved:
                break
            # Sonda zlyhala, otoc sa naspat
            robot_movement.turn_right(90)
            direction = RIGHT_OF[left_dir]

        elif option == "right":
            right_dir = RIGHT_OF[direction]
            nx, ny = get_next_position(x, y, right_dir)
            if is_tile_blocked(nx, ny):
                continue
            robot_movement.turn_right(90)
            direction = right_dir
            moved = probe_and_move(right_dir)
            if moved:
                break
            # Sonda zlyhala, otoc sa naspat
            robot_movement.turn_left(90)
            direction = LEFT_OF[right_dir]

    # Deadlock — ziadny volny smer, cuvaj
    if not moved:
        if DEBUG:
            print("Ziadny volny smer, cuvam")
        sensor_data = robot_movement.get_sensor_data()
        if sensor_data[1] > SAFE_DISTANCE:
            robot_movement.move_reverse(TILE_SIZE)
            dx, dy = DIRECTION_OFFSETS[direction]
            x -= dx
            y -= dy
        elif DEBUG:
            print("Za mnou je stena, nemozem cuvat")
    
    if DEBUG:
        print("---------------------------------------------------------------------------")

# ---------------------------------------------------------------------------
# Ukončenie
# ---------------------------------------------------------------------------

robot_movement.close()
GPIO.output(RGB_PIN_R, GPIO.LOW)
GPIO.output(RGB_PIN_G, GPIO.LOW)
GPIO.output(RGB_PIN_B, GPIO.LOW)
GPIO.cleanup()
