import cv2
import victim_recognition_26 as vc
import robot_movement_v1 as rm
# from picamera2 import Picamera2 # pyright: ignore[reportMissingImports, reportMissingModuleSource]
try:
    import RPi.GPIO as GPIO
    GPIO.setmode(GPIO.BCM)
except ImportError:
    class GPIO:  # no-op stub ked nie sme na Raspberry Pi
        BCM = HIGH = LOW = OUT = None
        @staticmethod
        def setup(*a): pass
        @staticmethod
        def output(*a): pass
        @staticmethod
        def cleanup(*a): pass
from collections import deque
import random, threading, time

ROBOT_LENGHT = 20
SAFE_DISTANCE = 15
DEBUG = True
SPEED = 35

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

running = True

# try:
#     left_camera = Picamera2(0)
#     left_camera.configure(left_camera.create_preview_configuration(main={"format": "RGB888", "size": (1280, 720)}))
#     left_camera.start()
# except Exception as e:
#     print(f"Error: left camera not found (index 0): {e}")
#     running = False

# try:
#     right_camera = Picamera2(1)
#     right_camera.configure(right_camera.create_preview_configuration(main={"format": "RGB888", "size": (1280, 720)}))
#     right_camera.start()
# except Exception as e:
#     print(f"Error: right camera not found (index 1): {e}")
#     running = False

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)

robot_movement = rm.RobotMovement(sample_amount=10)

sensor_data = [0]*8

last_5_left_victims = deque(maxlen=5)
last_5_right_victims = deque(maxlen=5)

blue_tile = False
was_in_center_window = False
was_left_victim = False
was_right_victim = False
allow_turning = True
special_tile_handled = False


def is_black_tile(sensor_values):
    return sensor_values[4] < 20 and sensor_values[5] < 20 and sensor_values[6] < 20


def is_blue_tile(sensor_values):
    return sensor_values[4] < 60 and sensor_values[5] < 80 and 150 < sensor_values[6]

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

def all_victims_same(victim_list):
    """
    Skontroluje, ci su vsetky prvky v zozname rovnake a nie su "None".
    ### Parametre:
        - victim_list: zoznam s poslednymi detekciami obetí
    ### Vracia:
        - True ak su vsetky prvky rovnake a nie su "None", inak False
    """
    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):
    """
    Skontroluje, ci su vsetky prvky v zozname "None".
    ### Parametre:
        - victim_list: zoznam s poslednymi detekciami obetí
    ### Vracia:
        - True ak su vsetky prvky "None", inak False
    """
    if len(victim_list) < victim_list.maxlen:
        return False
    return victim_list.count("None") == len(victim_list)

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

robot_movement.set_speed(SPEED)

while running:
    sensor_data = robot_movement.get_sensor_data()
    if DEBUG:
        print("Sensor data:", sensor_data)

    # 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)

    # if all_victims_same(last_5_left_victims) and not was_left_victim:
    #     GPIO.output(RGB_PIN_R, GPIO.HIGH)
    #     robot_movement.drop_medkits(medkit_values.get(left_victim, 0))
    #     GPIO.output(RGB_PIN_R, GPIO.LOW)
    #     was_left_victim = True

    # if all_victims_same(last_5_right_victims) and not was_right_victim:
    #     GPIO.output(RGB_PIN_R, GPIO.HIGH)
    #     robot_movement.drop_medkits(medkit_values.get(right_victim, 0))
    #     GPIO.output(RGB_PIN_R, GPIO.LOW)
    #     was_right_victim = True

    # if all_victims_none(last_5_left_victims):
    #     was_left_victim = False
    # if all_victims_none(last_5_right_victims):
    #     was_right_victim = False

    black_tile_detected = is_black_tile(sensor_data)
    blue_tile_detected = is_blue_tile(sensor_data)

    if not black_tile_detected and not blue_tile_detected:
        special_tile_handled = False

    if black_tile_detected and not special_tile_handled:
        if DEBUG:
            print("Black tile detected, turning 180")
        special_tile_handled = True
        robot_movement.set_speed(0)
        robot_movement.set_speed(-SPEED)
        time.sleep(1.5)
        continue

    if blue_tile_detected and not special_tile_handled:
        if DEBUG:
            print("Blue tile detected, waiting 5s")
        special_tile_handled = True
        robot_movement.set_speed(0)
        time.sleep(5.5)
        robot_movement.set_speed(SPEED)
        continue

    in_center_window = 12 <= (sensor_data[0]+ROBOT_LENGHT/2) % 30 <= 18

    if sensor_data[0] < 10:
        if DEBUG:
            time.sleep(1)
            GPIO.output(RGB_PIN_B, GPIO.HIGH)
            print("Too close, sensor value:", sensor_data[0])
            time.sleep(1)
            GPIO.output(RGB_PIN_B, GPIO.LOW)
        robot_movement.set_speed(0)
        random_turn = random.randint(0,2)

        if sensor_data[2] > SAFE_DISTANCE and random_turn == 0:
            if DEBUG:
                print("Too close, turning left")
            robot_movement.turn_left(90)
        elif sensor_data[3] > SAFE_DISTANCE and random_turn == 1:
            if DEBUG:
                print("Too close, turning right")
            robot_movement.turn_right(90)
        else:
            if sensor_data[2] > SAFE_DISTANCE:
                if DEBUG:
                    print("Too close, turning left")
                robot_movement.turn_left(90)
            elif sensor_data[3] > SAFE_DISTANCE:
                if DEBUG:
                    print("Too close, turning right")
                robot_movement.turn_right(90)
            else:
                if DEBUG:
                    print("Too close, turning 180")
                robot_movement.turn_left(180)
            

        robot_movement.set_speed(SPEED)

        continue

    elif in_center_window and not was_in_center_window:
        if DEBUG:
            time.sleep(1)
            GPIO.output(RGB_PIN_G, GPIO.HIGH)
            print("In center window, sensor value:", sensor_data[0])
            time.sleep(1)
            GPIO.output(RGB_PIN_G, GPIO.LOW)
        random_turn = random.randint(0,2)
        robot_movement.set_speed(0)
        if DEBUG:
            print("In center window, random turn:", random_turn)
        if sensor_data[2] > SAFE_DISTANCE and random_turn == 0:
            if DEBUG:
                print("Turning left")
            robot_movement.turn_left(90)
            robot_movement.set_speed(SPEED)
            pass
        elif sensor_data[3] > SAFE_DISTANCE and random_turn == 1:
            if DEBUG:
                print("Turning right")
            robot_movement.turn_right(90)
            robot_movement.set_speed(SPEED)
            pass
        elif sensor_data[0] > SAFE_DISTANCE and random_turn == 2:
            if DEBUG:
                print("Moving forward")
            robot_movement.set_speed(SPEED)
            pass
        else:
            if sensor_data[2] > SAFE_DISTANCE:
                if DEBUG:
                    print("Turning left")
                robot_movement.turn_left(90)
                robot_movement.set_speed(SPEED)
            elif sensor_data[3] > SAFE_DISTANCE:
                if DEBUG:
                    print("Turning right")
                robot_movement.turn_right(90)
                robot_movement.set_speed(SPEED)
            elif sensor_data[0] > SAFE_DISTANCE:
                if DEBUG:
                    print("Moving forward")
                robot_movement.set_speed(SPEED)
            else:
                if DEBUG:
                    print("No safe direction, turning 180")
                robot_movement.turn_left(180)
                robot_movement.set_speed(SPEED)

    was_in_center_window = in_center_window

robot_movement.set_speed(0)
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()
# left_camera.stop()
# right_camera.stop()



