#!/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.
ev3.speaker.beep()
wait(300)

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

# 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
    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 varible so it knows how far its gone
        degrees_traveled = degrees_traveled + 1


# while fish_collected <= amount_per_pond:
#     color_one = color_sensor_one()
#     color_two = color_sensor_two()
#     if color_two == Color.GREEN:# The if statements are just checking to see if we are looking ate a Color.GREEN fish
#         green_fish()              # if the sensor is looking at a Color.GREEN fish it will run the drop code function
#     while color_sensor_one() == Color.BLUE:
#         left_motor.run(65)
#         right_motor.run(75)
#         if color_two == Color.GREEN:
#             green_fish()
#         color_one = color-_sensor_one()
#         color_two = color_sensor_two()
#     while color_sensor_one() != Color.BLUE:
#         left_motor.run(80)
#         right_motor.run(60)
#         if color_two == Color.GREEN:
#             green_fish()
#         color_one = color_sensor_one()
#         color_two = color_sensor_two()

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