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

my_left_kidney_ran_away_from_home_and_i_am_really_sad = Motor(Port.C)
right_motor = Motor(Port.B)
sacrifice = Motor(Port.A)
devil = DriveBase(my_left_kidney_ran_away_from_home_and_i_am_really_sad, right_motor, wheel_diameter=57.15, axle_track=88.9)
sensor = ColorSensor(Port.S1)
# Color sensor is defined. It is in Sensor Port 1 (frontend) so we are putting S1.

# Defining values below so I don't have to do it later.
one = 30
# first sensor value
two = 35
# second sensor value
w = 3
# wait
t = 10
# turn

# This loop is for debugging. You can view the current reflection and adjust values based off it.
# You can hit center to exit debugger. Alternatively, you can disable this section.
while True:
    ev3.screen.print(sensor.reflection)
    if Button.CENTER in ev3.buttons.pressed():
        ev3.screen.clear()
        break
    else:
        ev3.screen.clear()
        wait(5)

# W is wait time, T is turn radius to decide how much sensitivity is placed in the turn.
# In theory, it should follow values based off one % two (with 5% in the middle as of current)
# It will go forward if sensitivity is to the correct value, and if it is not, it will search for the line.
while True:
    if sensor.reflection > one%:
        check = False
        devil.forward(10)
    elif sensor.reflection < two%:
        check = True
        devil.turn(t)
        wait(w)
        devil.turn(-t)
        wait(w)
        devil.turn(-t)
        wait(w)
        devil.turn(t)
    elif check = False:
        if devil.speed() < 8:
            devil.hold()

# Below here is experimental code that may or may not work.
# The point of this code is to automatically exit the loop once go = false (meaning robot is considered to be at a dead end.)
# If robot is found to not be at a dead end (anymore) it will return to the line-searching loop.

# while True:
#     while go = True:
#         if sensor.reflection > one%:
#             check = False
#             devil.forward(10)
#         elif sensor.reflection < two%:
#             check = True
#             devil.turn(t)
#             wait(w)
#             devil.turn(-t)
#             wait(w)
#             devil.turn(-t)
#             wait(w)
#             devil.turn(t)
#         elif check = False
#             if devil.speed < 8:
#                 go = False
#     while go = False:
#         if sensor.reflection > one%:
#             devil.forward(10)
#             if devil.speed < 8:
#                 devil.hold()
#         else:
#             wait(5)