import epuck2
import time

# Tunable constants
BASE_SPEED = 120
MAX_SPEED = 260
ROTATE_SPEED = 220
STEER_GAIN = 0.22
FRONT_BLOCK_THRESHOLD = 1200
ROTATE_IN_PLACE_THRESHOLD = 2200
NEAR_THRESHOLD = 500
VERY_NEAR_THRESHOLD = 1200
BEEP_FREQ = 2000
BEEP_DURATION = 0.08
LOOP_PERIOD = 0.05


def clamp(value, minimum, maximum):
    if value < minimum:
        return minimum
    if value > maximum:
        return maximum
    return value


def stop_all():
    epuck2.set_motors_speed(0, 0)
    epuck2.set_leds(0)
    epuck2.set_all_rgb(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
    epuck2.play_freq(0)


def update_rgb_for_quadrants(prox):
    quadrant_values = [
        max(prox[0], prox[1]),
        max(prox[2], prox[3]),
        max(prox[4], prox[5]),
        max(prox[6], prox[7]),
    ]

    for led_index, value in enumerate(quadrant_values):
        if value >= VERY_NEAR_THRESHOLD:
            color = (100, 0, 0)
        elif value >= NEAR_THRESHOLD:
            color = (100, 80, 0)
        else:
            color = (0, 100, 0)
        epuck2.set_rgb(led_index, color[0], color[1], color[2])


try:
    running = False
    button_was_pressed = False
    rotate_dir = 0
    previous_rotate_dir = 0

    while True:
        button_pressed = epuck2.button_pressed()

        if button_pressed and not button_was_pressed:
            running = not running
            if running:
                epuck2.play_freq(BEEP_FREQ)
                time.sleep(BEEP_DURATION)
                epuck2.play_freq(0)
            else:
                break

        button_was_pressed = button_pressed

        if not running:
            stop_all()
            time.sleep(0.02)
            continue

        prox = epuck2.get_proximity()
        front_left = prox[6] + prox[7]
        front_right = prox[0] + prox[1]
        front_total = front_left + front_right

        # Proportional steering using the two front sensors.
        # Positive steering means the robot turns left; negative means it turns right.
        steering = (front_left - front_right) * STEER_GAIN

        if front_total >= ROTATE_IN_PLACE_THRESHOLD:
            # If the left front sensors see more obstacle than the right front sensors,
            # rotate right to move away from the obstacle on the left side.
            if front_left > front_right:
                rotate_dir = 1  # right turn
                side_led = 8    # LED7 on the left side
            else:
                rotate_dir = -1 # left turn
                side_led = 2    # LED3 on the right side

            if rotate_dir != previous_rotate_dir:
                epuck2.play_freq(BEEP_FREQ)
                time.sleep(BEEP_DURATION)
                epuck2.play_freq(0)

            epuck2.set_leds(16 | side_led)
            left_speed = -rotate_dir * ROTATE_SPEED
            right_speed = rotate_dir * ROTATE_SPEED
            previous_rotate_dir = rotate_dir
        elif front_total > FRONT_BLOCK_THRESHOLD:
            rotate_dir = 0
            left_speed = clamp(BASE_SPEED + steering, -MAX_SPEED, MAX_SPEED)
            right_speed = clamp(BASE_SPEED - steering, -MAX_SPEED, MAX_SPEED)
            epuck2.set_leds(0)
            previous_rotate_dir = 0
        else:
            rotate_dir = 0
            left_speed = clamp(BASE_SPEED + steering, -MAX_SPEED, MAX_SPEED)
            right_speed = clamp(BASE_SPEED - steering, -MAX_SPEED, MAX_SPEED)
            epuck2.set_leds(0)
            previous_rotate_dir = 0

        update_rgb_for_quadrants(prox)
        epuck2.set_motors_speed(int(left_speed), int(right_speed))
        time.sleep(LOOP_PERIOD)
finally:
    stop_all()