from machine import Pin
import time

# =========================================================
# INPUTS — Simulated Camera / AI Detection
# =========================================================

CLEAR_INPUT = Pin(10, Pin.IN, Pin.PULL_UP)
STATIC_INPUT = Pin(11, Pin.IN, Pin.PULL_UP)
PERSON_PET_INPUT = Pin(12, Pin.IN, Pin.PULL_UP)

# =========================================================
# OUTPUTS
# =========================================================

RUN_LED = Pin(16, Pin.OUT)
REROUTE_LED = Pin(17, Pin.OUT)
STOP_LED = Pin(18, Pin.OUT)

# =========================================================
# ORIGINAL BOUSTROPHEDON PATH
# =========================================================

original_path = [
    (0, 0), (1, 0), (2, 0), (3, 0), (4, 0),
    (4, 1), (3, 1), (2, 1), (1, 1), (0, 1),
    (0, 2), (1, 2), (2, 2), (3, 2), (4, 2),
    (4, 3), (3, 3), (2, 3), (1, 3), (0, 3),
    (0, 4), (1, 4), (2, 4), (3, 4), (4, 4)
]

position = [0, 0]
path_index = 0

# =========================================================
# AI CLASSIFICATION DATABASE
# =========================================================

static_objects = [
    "ROCK",
    "TREE",
    "CHAIR"
]

living_objects = [
    "PERSON",
    "PET"
]

static_index = 0
living_index = 0

# =========================================================
# LED FUNCTIONS
# =========================================================

def all_leds_off():
    RUN_LED.value(0)
    REROUTE_LED.value(0)
    STOP_LED.value(0)


def show_run():
    all_leds_off()
    RUN_LED.value(1)


def show_reroute():
    all_leds_off()
    REROUTE_LED.value(1)


def show_stop():
    all_leds_off()
    STOP_LED.value(1)


# =========================================================
# AI CLASSIFIER
# =========================================================

def classify_static_obstacle():

    global static_index

    detected_object = static_objects[static_index]

    static_index += 1

    if static_index >= len(static_objects):
        static_index = 0

    return detected_object


def classify_living_object():

    global living_index

    detected_object = living_objects[living_index]

    living_index += 1

    if living_index >= len(living_objects):
        living_index = 0

    return detected_object


# =========================================================
# ROBOT MOVEMENT
# =========================================================

def move_to(point):

    global position

    position[0] = point[0]
    position[1] = point[1]

    print("ROBOT POSITION :", tuple(position))
    print("PATH STATUS     : CLEAR")
    print("ACTION          : MOWING")

    time.sleep_ms(500)


# =========================================================
# EMERGENCY STOP
# =========================================================

def emergency_stop(object_name):

    show_stop()

    print("\n")
    print("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
    print("        EMERGENCY STOP")
    print("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
    print("AI OBJECT : " + object_name)
    print("CLASS     : LIVING_HAZARD")
    print("ACTION    : STOP ROBOT")
    print("BLADE     : DISABLED")
    print("PATH      : PAUSED")
    print("WAITING   : CLEAR BUTTON")
    print("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")


# =========================================================
# WAIT FOR CLEAR
# =========================================================

def wait_for_clear():

    while True:

        # Keep robot stopped
        if PERSON_PET_INPUT.value() == 0:
            show_stop()

        # Clear button releases emergency stop
        elif CLEAR_INPUT.value() == 0:

            print("\n================================")
            print("CLEAR SIGNAL RECEIVED")
            print("LIVING HAZARD CLEARED")
            print("RESUMING ORIGINAL PATH")
            print("================================")

            show_run()

            while CLEAR_INPUT.value() == 0:
                time.sleep_ms(50)

            return

        time.sleep_ms(100)


# =========================================================
# STATIC OBSTACLE REROUTE
# =========================================================

def perform_reroute(object_name):

    show_reroute()

    x = position[0]
    y = position[1]

    print("\n================================")
    print("STATIC OBSTACLE DETECTED")
    print("AI OBJECT       :", object_name)
    print("CLASS           : STATIC_OBSTACLE")
    print("CURRENT POSITION:", tuple(position))
    print("================================")

    # Choose local detour
    if x < 4:

        detour = [
            (x + 1, y),
            (x + 1, y + 1),
            (x, y + 1)
        ]

        print("REROUTE DIRECTION : RIGHT")

    else:

        detour = [
            (x - 1, y),
            (x - 1, y + 1),
            (x, y + 1)
        ]

        print("REROUTE DIRECTION : LEFT")

    print("LOCAL DETOUR PATH:")

    for point in detour:

        # Living hazard always overrides reroute
        if PERSON_PET_INPUT.value() == 0:

            living_object = classify_living_object()

            emergency_stop(living_object)
            wait_for_clear()

        move_to(point)

    print("\nOBSTACLE PASSED")
    print("OBJECT          :", object_name)
    print("REJOINING ORIGINAL PATH")

    show_run()

    # Wait for yellow button release
    while STATIC_INPUT.value() == 0:
        time.sleep_ms(50)


# =========================================================
# START
# =========================================================

print("\n========================================")
print("      AI ROBOTIC LAWN MOWER")
print("       RASPBERRY PI CONTROLLER")
print("========================================")

print("AI CLASSIFICATION ENABLED")
print("STATIC : ROCK -> TREE -> CHAIR")
print("LIVING : PERSON -> PET")

show_run()

# =========================================================
# MAIN NAVIGATION LOOP
# =========================================================

while path_index < len(original_path):

    # -----------------------------------------------------
    # PERSON / PET — HIGHEST PRIORITY
    # -----------------------------------------------------

    if PERSON_PET_INPUT.value() == 0:

        living_object = classify_living_object()

        emergency_stop(living_object)

        wait_for_clear()

        continue

    # -----------------------------------------------------
    # STATIC OBSTACLE
    # -----------------------------------------------------

    if STATIC_INPUT.value() == 0:

        static_object = classify_static_obstacle()

        perform_reroute(static_object)

        continue

    # -----------------------------------------------------
    # CLEAR
    # -----------------------------------------------------

    if CLEAR_INPUT.value() == 0:

        show_run()

        print("\nAI DETECTION : NO OBJECT")
        print("CLASS        : CLEAR")
        print("DECISION     : RUN")

        while CLEAR_INPUT.value() == 0:
            time.sleep_ms(50)

    # -----------------------------------------------------
    # NORMAL MOWING
    # -----------------------------------------------------

    show_run()

    move_to(original_path[path_index])

    path_index += 1

    time.sleep_ms(200)


# =========================================================
# COMPLETE
# =========================================================

all_leds_off()

print("\n========================================")
print("       LAWN MOWING COMPLETE")
print("========================================")
print("FINAL POSITION :", tuple(position))
print("TOTAL POINTS   :", len(original_path))