#!/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.

ev3 = EV3Brick()
villain = ColorSensor(Port.S1)
my_left_kidney_ran_away_from_home_and_i_am_really_sad = Motor(Port.C)
if_tweek_and_craig_dont_kiss_soon_this_robot_turns_evil_again = Motor(Port.B)
# sacrifice = Motor(Port.A)
devil = DriveBase(my_left_kidney_ran_away_from_home_and_i_am_really_sad, if_tweek_and_craig_dont_kiss_soon_this_robot_turns_evil_again, wheel_diameter=57.15, axle_track=88.9)
# Default defines ^ Sacrifice is commented because it doesn't play a role.
one = 2
two = 3
t = 10
w = 50
# W is wait time, T is turn radius to decide how much sensitivity is placed in the turn.
# One decides the first loop's threshold (meaning it will trigger if reflectively is less or equal to value)
# Two decides the second loop's threshold (it will only trigger if reflectively is equal to or higher than 3)
ev3.speaker.beep()
print('Program start')
# This loop is for debugging. You can view the current reflection and adjust values based off it.
# You can hit center to exit debugger. In Visual Code, highlighting it and pressing Ctrl+/ will uncomment it.
# There is no loss to the code if you set it as comment. It just skips the portion.
while True:
    print('Debug')
    ev3.screen.print(villain.reflection())
    print(villain.reflection())
    if Button.CENTER in ev3.buttons.pressed():
        ev3.screen.clear()
        ev3.speaker.beep()
        break
    else:
        wait(100)
        ev3.screen.clear()

def valupd():
    t = t + 5
    w = w + 10
    print('Turn is' t', and wait is' w'.')
    # Print is entirely just for debug purposes. Does not show on robot.
def valclr():
    t = 10
    w = 50
    print('Turn is' t', and wait is' w'.')
    # Print is entirely just for debug purposes. Does not show on robot.
# These two functions are designated for clearing and updating values.
# They aren't necessary, but it clears up clutter (ignoring the fact I add clutter with comments)

while True:
    print ('#1' villain.reflection())
    if villain.reflection() <= one:
        valclr()
        # Sets variables to default as the qualifier for the loop means a wide swing isn't necessary
        devil.straight(50)
        # devil.run(50)
        # Code to be tested to see if it will run smoothly
    else:
        while villain.reflection() >= two:
            print ('#2' villain.reflection())
            # This one is #2 to view which loop it is actively in at the moment.
            devil.turn(t)
            if villain.reflection() <= one:
                break
            wait(w)
            devil.turn(-t)
            if villain.reflection() <= one:
                break
            wait(w)
            devil.turn(-t)
            if villain.reflection() <= one:
                break
            wait(w)
            devil.turn(t)
            if villain.reflection() <= one:
                break
            valupd()
            # Updates values by 5 and 10 respectively. The wait gets a higher value to prevent overswing going by a value.
            # It would likely miss the update in reflectively if it went too fast.
            # This code likely isn't as efficient/compact as it can be, but it's still relatively quick to run on a EV3
            # valupd is just updating both variables. It's just meant to keep it more compact.