#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile


# This program requires LEGO EV3 MicroPython v2.0 or higher.
# Click "Open user guide" on the EV3 extension tab for more information.


# Create your objects here.
ev3 = EV3Brick()
right_motor = Motor(Port.B)
left_motor = Motor(Port.C)
lift_motor = Motor(Port.A)
claw = Motor(Port.D)
#light_sensor = LightSensor(Port.S)
#color_sensor_one = ColorSensor(Port.S2)
color_sensor_two = ColorSensor(Port.S1)
wheel_diameter = 43.3
axle_track = 130

# Configure drivebase and set speed/acceleration options
driver = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
driver.settings(800, 200, 30, 1200)

# Write your program here.

# Beep the speaker so you can tell when the code is running
ev3.speaker.beep()
wait(300)

# Define code to run when a green fish is detected
def green_fish():
    # Drive back so axis of rotation lines up with fish, then rotate 90 degrees and drive forward a bit
    driver.straight(-40)
    driver.turn(90)
    driver.straight(20)

    # Reset angles
    claw.reset_angle(0)
    lift_motor.reset_angle(0)

    # Open claw
    claw.track_target(95)
    wait(200)
    claw.hold()
    wait(1000)

    # Lower tray
    lift_motor.run_angle(100, -180, then=Stop.HOLD, wait=True)
    wait(1000)

    # Close claw
    claw.track_target(0)
    wait(200)
    claw.hold()
    wait(1000)

    # Lift tray
    lift_motor.run_angle(100, 180, then=Stop.HOLD, wait=True)

    # Drive back and rotate to original heading
    driver.straight(-20)
    driver.turn(-90)

# Define code to drive around a circle of a given diameter
def circle_pond(pond_radius):

    # This variable is used to keep track of how far along the circle the robot has traveled, which obviously starts at 0
    degrees_traveled = 0

    # This code will keep looping until the robot has gone 360 degrees around the circle
    while degrees_traveled <= 360:

        # driver.curve() won't work until the Mindstorm is updated, but it should drive around the circle
        # one degree at a time.
        driver.curve(pond_radius, 1, then=Stop.COAST, wait=True)

        # Read the color detected by the sensor, and if it's green, run the green_fish() function to collect it
        color_two = color_sensor_two()
        wait(100)
        if color_two == Color.GREEN:
            green_fish()

        # Add one degree to the variable so it knows how far its gone
        degrees_traveled = degrees_traveled + 1


# Run the function to circle each pond. The argument is the radius of the pond in mm. The first two are disabled for testing.
#circle_pond(203.2) # medium pond
#circle_pond(152.4) # small pond
circle_pond(254) # large pond