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

import time
# 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()

motorL = Motor(Port.B)
motorR = Motor(Port.C)
motors = DriveBase(motorL, motorR, 100, 110)

lightL = ColorSensor(Port.S1)
lightR = ColorSensor(Port.S4)

leftSensor = False
rightSensor = False
ultra = False
# Write your program here.
pravy_vidi_smer = ""
lavy_vidi_smer = ""
pravy_vidi = ""
lavy_vidi = ""

kombinacie = []

def smer(x):
    global speed, direction
    if x[0] == "l":
        direction = 90
    elif x[0] == "r":
        direction = -90
    elif x[0] == "d":
        speed = 0
        direction = 0

ev3.speaker.beep()

combo = ""
lastcombo = ""

while True:
    if Button.CENTER in ev3.buttons.pressed():
        break
    elif Button.LEFT in ev3.buttons.pressed():
        combo= "l"
        if lightR.reflection() >= 50:
            combo += "0"
        else:
            combo += "1"
        
        if lightL.reflection() >= 50:
            combo += "0"
        else:
            combo += "1"
        kombinacie.append(combo)
        time.sleep(0.5)

    elif Button.RIGHT in ev3.buttons.pressed():
        combo= "r"
        if lightR.reflection() >= 50:
            combo += "0"
        else:
            combo += "1"
        
        if lightL.reflection() >= 50:
            combo += "0"
        else:
            combo += "1"
        kombinacie.append(combo)
        time.sleep(0.5)

    elif Button.DOWN in ev3.buttons.pressed():
        combo= "d"
        if lightR.reflection() >= 50:
            combo += "0"
        else:
            combo += "1"
        
        if lightL.reflection() >= 50:
            combo += "0"
        else:
            combo += "1"
        kombinacie.append(combo)
        time.sleep(0.5)

    if combo != lastcombo:
        print(combo)
    lastcombo = combo

ev3.speaker.beep()
time.sleep(0.5)

while True:
    speed = -150
    direction = 0

    lavy = lightL.reflection() >= 50
    pravy = lightR.reflection() >= 50

    for i in range(0, len(kombinacie)):
        y = kombinacie[i]
        pr = int(y[1]) == 0
        lv = int(y[2]) == 0

        if pr == pravy:
            if lv == lavy:
                smer(y)

    motors.drive(speed, direction)