from manobot import ManoBot
from time import sleep_ms

robot = ManoBot()

# ============================================================
# MANOCITY GLOBAL CITY SHUTTLE
#
# Fixed competition route:
#
#   START
#     -> Faisal Mosque passenger
#     -> LEFT at South T
#     -> STRAIGHT at South junction
#     -> LEFT at Centre junction
#     -> Parliament House passenger
#     -> RIGHT at West junction
#     -> sharp 90-degree corner
#     -> STRAIGHT at North junction
#     -> Cathedral of Brasília passenger
#     -> sharp 90-degree corner
#     -> LEFT at East junction
#     -> GLOBAL MOBILITY BRIDGE
#
# The robot ALWAYS follows the black line with its five IR
# sensors. Coordinates are used only to decide when to STOP
# for a passenger or at the Global Mobility Bridge.
# ============================================================


# ============================================================
# PID SETTINGS
# ============================================================

KP = 6.0
KI = 0.0
KD = 2.0

BASE_SPEED = 24

last_error = 0
integral = 0
error = 0


# ============================================================
# FIXED MISSION (stable legacy IDs preserve saved programmes)
# ============================================================

PICKUP_DISTANCE = 0.58
DESTINATION_DISTANCE = 0.68

PICKUPS = {
    "LOC_BUCKINGHAM_PALACE": (-5.08, -6.15),
    "LOC_HYDE_PARK": (-5.08, 0.0),
    "LOC_BIG_BEN": (3.15, 4.3),
}

GLOBAL_MOBILITY_BRIDGE = (12.8, 0.0)


# ============================================================
# FIXED JUNCTION PLAN
#
# Corners are NOT junctions. The normal PID follows the
# 90-degree corner line.
# ============================================================

JUNCTION_DECISIONS = [
    "LEFT",      # South T: east -> north
    "STRAIGHT",  # South junction: continue north
    "LEFT",      # Centre: north -> west
    "RIGHT",     # West: west -> north
    "STRAIGHT",  # North: continue east
    "LEFT",      # East: south -> east towards Global Mobility Bridge
]

junction_index = 0
junction_mode = ""
junction_action = ""
junction_ticks = 0
junction_clear_ticks = 0
junction_cooldown = 0

ENTER_JUNCTION_TICKS = 32
MIN_TURN_TICKS = 34
MAX_TURN_TICKS = 90
EXIT_CLEAR_TICKS = 8

JUNCTION_SPEED = 18
TURN_SPEED = 20
EXIT_SPEED = 18


# ============================================================
# HELPERS
# ============================================================

def distance(x1, z1, x2, z2):
    dx = x2 - x1
    dz = z2 - z1

    return (
        dx * dx +
        dz * dz
    ) ** 0.5


def line_visible(sensors):
    return (
        sensors["far_left"]
        or sensors["left"]
        or sensors["center"]
        or sensors["right"]
        or sensors["far_right"]
    )


def first_waiting_passenger(passengers):
    for passenger in passengers:
        if passenger["status"] == "WAITING":
            return passenger

    return None


# ============================================================
# NORMAL BLACK-LINE PID
# ============================================================

def follow_line(sensors):
    global last_error
    global integral
    global error

    # Side sensors have priority over CENTER.
    #
    # This is important at a square 90-degree corner where a
    # sensor pattern can briefly become:
    #
    #   0 0 1 1 1
    #
    # FAR_RIGHT must win before CENTER.

    if sensors["far_left"]:
        error = 4

    elif sensors["far_right"]:
        error = -4

    elif sensors["left"]:
        error = 2

    elif sensors["right"]:
        error = -2

    elif sensors["center"]:
        error = 0

    else:
        # Lost-line recovery.
        #
        # Search toward the side where the line was last seen.

        if last_error < 0:
            robot.set_motors(8, 26)

        elif last_error > 0:
            robot.set_motors(26, 8)

        else:
            robot.stop()

        return

    integral += error

    if integral > 30:
        integral = 30

    elif integral < -30:
        integral = -30

    derivative = error - last_error

    correction = (
        KP * error
        + KI * integral
        + KD * derivative
    )

    left_speed = BASE_SPEED + correction
    right_speed = BASE_SPEED - correction

    robot.set_motors(
        left_speed,
        right_speed
    )

    last_error = error


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

while True:

    sensors = robot.read_sensors()
    passengers = robot.get_passengers()
    position = robot.get_position()
    mission = robot.get_mission()

    # --------------------------------------------------------
    # Mission complete / failed
    # --------------------------------------------------------

    if mission is not None:

        if mission["status"] == "COMPLETE":
            robot.stop()
            sleep_ms(10)
            continue

        if mission["status"] == "FAILED":
            robot.stop()
            sleep_ms(10)
            continue

    if position is None:
        robot.stop()
        sleep_ms(10)
        continue


    # ========================================================
    # PASSENGER PICKUP
    # ========================================================

    waiting = first_waiting_passenger(
        passengers
    )

    if waiting is not None:

        pickup = PICKUPS.get(
            waiting["pickupLocationId"]
        )

        if pickup is not None:

            passenger_distance = distance(
                position["x"],
                position["z"],
                pickup[0],
                pickup[1]
            )

            # Stop on the black line.
            # The mission engine changes WAITING -> ONBOARD.

            if passenger_distance <= PICKUP_DISTANCE:

                robot.stop()

                sleep_ms(10)

                continue


    # ========================================================
    # ALL PASSENGERS ONBOARD -> GLOBAL MOBILITY BRIDGE
    # ========================================================

    else:

        bridge_distance = distance(
            position["x"],
            position["z"],
            GLOBAL_MOBILITY_BRIDGE[0],
            GLOBAL_MOBILITY_BRIDGE[1]
        )

        if bridge_distance <= DESTINATION_DISTANCE:

            # All passengers are delivered together here.
            # Mission status becomes COMPLETE and the sustainability
            # results are displayed by ManoCity.

            robot.stop()

            sleep_ms(10)

            continue


    # ========================================================
    # JUNCTION MANOEUVRE IN PROGRESS
    # ========================================================

    if junction_mode == "ENTER":

        robot.set_motors(
            JUNCTION_SPEED,
            JUNCTION_SPEED
        )

        junction_ticks += 1

        # Move the robot body into the centre of the junction
        # before starting the chosen manoeuvre.

        if junction_ticks >= ENTER_JUNCTION_TICKS:

            junction_ticks = 0

            if junction_action == "STRAIGHT":

                junction_mode = "EXIT"

            else:

                junction_mode = "TURN"

        sleep_ms(10)

        continue


    if junction_mode == "TURN":

        junction_ticks += 1

        if junction_action == "LEFT":

            # Rotate LEFT in place.
            robot.set_motors(
                TURN_SPEED,
                -TURN_SPEED
            )

        else:

            # Rotate RIGHT in place.
            robot.set_motors(
                -TURN_SPEED,
                TURN_SPEED
            )

        # Do not accept the old junction line immediately.
        # After enough rotation, wait for the CENTER sensor to
        # find the outgoing black line.

        if (
            junction_ticks >= MIN_TURN_TICKS
            and sensors["center"]
            and not robot.is_junction()
        ):

            junction_mode = "EXIT"
            junction_ticks = 0
            junction_clear_ticks = 0

        # Safety fallback: after a full turn window, leave TURN
        # mode and let normal line recovery reacquire the road.

        elif junction_ticks >= MAX_TURN_TICKS:

            junction_mode = "EXIT"
            junction_ticks = 0
            junction_clear_ticks = 0

        sleep_ms(10)

        continue


    if junction_mode == "EXIT":

        robot.set_motors(
            EXIT_SPEED,
            EXIT_SPEED
        )

        if (
            not robot.is_junction()
            and line_visible(sensors)
        ):

            junction_clear_ticks += 1

        else:

            junction_clear_ticks = 0

        if junction_clear_ticks >= EXIT_CLEAR_TICKS:

            junction_mode = ""
            junction_action = ""
            junction_clear_ticks = 0

            junction_index += 1

            # Prevent immediate re-triggering on the same junction.
            junction_cooldown = 18

        sleep_ms(10)

        continue


    # ========================================================
    # START A NEW FIXED JUNCTION DECISION
    # ========================================================

    if junction_cooldown > 0:

        junction_cooldown -= 1

    elif (
        robot.is_junction()
        and junction_index < len(JUNCTION_DECISIONS)
    ):

        junction_action = (
            JUNCTION_DECISIONS[
                junction_index
            ]
        )

        junction_mode = "ENTER"
        junction_ticks = 0
        junction_clear_ticks = 0

        robot.set_motors(
            JUNCTION_SPEED,
            JUNCTION_SPEED
        )

        sleep_ms(10)

        continue


    # ========================================================
    # SHARP 90-DEGREE CORNER RECOVERY
    #
    # Both sharp corners on this fixed route turn RIGHT:
    #
    #   NW corner: north -> east
    #   NE corner: east -> south
    #
    # The PID should normally see the corner side sensors.
    # If the square elbow creates a brief blank sensor frame,
    # rotate slowly RIGHT until the black line reappears.
    # ========================================================

    if not line_visible(sensors):

        collected = 0

        if mission is not None:
            collected = mission["collectedCount"]

        if collected >= 2:

            robot.set_motors(
                8,
                26
            )

            last_error = -4

            sleep_ms(10)

            continue


    # ========================================================
    # NORMAL LINE FOLLOWING
    # ========================================================

    follow_line(
        sensors
    )

    sleep_ms(10)