# ---------------------------------------------------------------------------
# Hlavný kontrolér v3 — Pi je zodpovedný len za vízie/obete.
# Arduino sa pohybuje autonómne (roomba štýl).
# ---------------------------------------------------------------------------

import sys
import victim_recognition_26 as vc
import robot_movement_v3 as rm
# from picamera2 import Picamera2
from collections import deque
import 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 — uprav tu
# ---------------------------------------------------------------------------

MAX_TOTAL_KITS      = 8   # informačne pre Pi; limit sa vynucuje na Arduine
MAX_BLUE_TILE_VISITS = 1  # informačne; vynucuje sa na Arduine

LIMIT_PHI   = 1
LIMIT_PSI   = 1
LIMIT_OMEGA = 1
LIMIT_SUM2  = 1
LIMIT_SUM1  = 1
LIMIT_SUM0  = 1

DEBUG = True

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

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

RGB_PIN_R = 17
RGB_PIN_G = 27
RGB_PIN_B = 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 (odkomentuj na Raspberry Pi)
# ---------------------------------------------------------------------------

# 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: left camera not found (index 0): {e}")

# 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: right camera not found (index 1): {e}")

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

robot_movement = rm.RobotMovement()

running = True

last_5_left_victims  = deque(maxlen=5)
last_5_right_victims = deque(maxlen=5)
was_left_victim  = False
was_right_victim = False

# Počítadlá pre každý typ obete — double-drop prevention
victim_counters = {
    'Phi':   0,
    'Psi':   0,
    'Omega': 0,
    '2':     0,
    '1':     0,
    '0':     0,
}

victim_limits = {
    'Phi':   LIMIT_PHI,
    'Psi':   LIMIT_PSI,
    'Omega': LIMIT_OMEGA,
    '2':     LIMIT_SUM2,
    '1':     LIMIT_SUM1,
    '0':     LIMIT_SUM0,
}

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

def should_drop(victim):
    """
    Vráti True ak je počítadlo pre daný typ obete ešte pod limitom.
    """
    if victim not in victim_limits:
        return False
    return victim_counters.get(victim, 0) < victim_limits[victim]


def increment_counter(victim):
    """
    Zvýši počítadlo pre daný typ obete (ak je typ platný).
    """
    if victim in victim_counters:
        victim_counters[victim] += 1


def blink_led_white(duration_s=5.0, interval_s=0.5):
    """
    Bliká Pi RGB LED bielo po dobu duration_s sekúnd (0.5s ON / 0.5s OFF).
    Blokujúca funkcia — volá sa pred odoslaním DROP príkazu.
    """
    end_time = time.monotonic() + duration_s
    state = True
    while time.monotonic() < end_time:
        val = GPIO.HIGH if state else GPIO.LOW
        GPIO.output(RGB_PIN_R, val)
        GPIO.output(RGB_PIN_G, val)
        GPIO.output(RGB_PIN_B, val)
        state = not state
        time.sleep(interval_s)
    GPIO.output(RGB_PIN_R, GPIO.LOW)
    GPIO.output(RGB_PIN_G, GPIO.LOW)
    GPIO.output(RGB_PIN_B, GPIO.LOW)


def all_victims_same(victim_list):
    if len(victim_list) < victim_list.maxlen:
        return False
    if victim_list[0] == "None":
        return False
    return victim_list.count(victim_list[0]) == len(victim_list)


def all_victims_none(victim_list):
    if len(victim_list) < victim_list.maxlen:
        return False
    return victim_list.count("None") == len(victim_list)


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


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

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

while running:
    # --- Snímanie kamier (odkomentuj na Raspberry Pi) ---
    # import cv2
    # left_frame  = cv2.cvtColor(left_camera.capture_array(), cv2.COLOR_RGB2BGR)
    # right_frame = cv2.cvtColor(right_camera.capture_array(), cv2.COLOR_RGB2BGR)
    # left_victim  = vc.get_victim_on_wall(left_frame,  flip_mode=2)
    # right_victim = vc.get_victim_on_wall(right_frame, flip_mode=2)
    # last_5_left_victims.append(left_victim)
    # last_5_right_victims.append(right_victim)

    # --- Spracovanie ľavej obete ---
    # if all_victims_same(last_5_left_victims) and not was_left_victim:
    #     victim = last_5_left_victims[0]
    #     if should_drop(victim):
    #         if DEBUG:
    #             print(f"[LAVA] Obet detegovaná: {victim}, blikám LED a vysadzujem {medkit_values.get(victim, 0)} kit(ov)")
    #         blink_led_white(duration_s=5.0)
    #         total = robot_movement.drop_medkits_left(medkit_values.get(victim, 0))
    #         if DEBUG:
    #             print(f"[LAVA] Celkovo vysadených kitov: {total}")
    #         increment_counter(victim)
    #     elif DEBUG:
    #         print(f"[LAVA] Obet {victim} ignorovaná — limit dosiahnutý ({victim_counters.get(victim, 0)}/{victim_limits.get(victim, 0)})")
    #     was_left_victim = True

    # --- Spracovanie pravej obete ---
    # if all_victims_same(last_5_right_victims) and not was_right_victim:
    #     victim = last_5_right_victims[0]
    #     if should_drop(victim):
    #         if DEBUG:
    #             print(f"[PRAVA] Obet detegovaná: {victim}, blikám LED a vysadzujem {medkit_values.get(victim, 0)} kit(ov)")
    #         blink_led_white(duration_s=5.0)
    #         total = robot_movement.drop_medkits_right(medkit_values.get(victim, 0))
    #         if DEBUG:
    #             print(f"[PRAVA] Celkovo vysadených kitov: {total}")
    #         increment_counter(victim)
    #     elif DEBUG:
    #         print(f"[PRAVA] Obet {victim} ignorovaná — limit dosiahnutý ({victim_counters.get(victim, 0)}/{victim_limits.get(victim, 0)})")
    #     was_right_victim = True

    # --- Reset príznakov ked obet zmizne z pohľadu ---
    # if all_victims_none(last_5_left_victims):
    #     was_left_victim = False
    # if all_victims_none(last_5_right_victims):
    #     was_right_victim = False

    pass

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

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