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


# Write your program here.
ev3.speaker.beep()
left_motor = Motor(Port.B)
right_motor = Motor(Port.C)
cargo_motor = Motor(Port.D)

time = StopWatch()
poistka = 0

leftColor = ColorSensor(Port.S1)
midColor = ColorSensor(Port.S3)
rightColor = ColorSensor(Port.S4)
infraSensor = InfraredSensor(Port.S2)
robotWheels = DriveBase(left_motor, right_motor, wheel_diameter=50, axle_track=100)

def state(x):
    if x == 0:
        mainReflected = (midColor.reflection() - 50)*1.6
        robotWheels.drive(150, mainReflected)

    elif x == 1:
        robotWheels.stop()
        while midColor.reflection() > 40:
            robotWheels.drive(150, 90)

    elif x == 2:
        robotWheels.stop()
        while midColor.reflection() > 40:
            robotWheels.drive(150, -90)
    elif x == 3:
        robotWheels.stop()
        robotWheels.turn(150)
        cargo_motor.run(100)
        wait(500)
        global poistka 
        poistka = 1
        
time.reset()
while True:
#    if touchSensor.pressed() == True:
#        break 
    if rightColor.reflection() < 40:
        state(2)
    elif leftColor.reflection() < 40:
        state(1)
    elif time.time() >= 10000:
        if poistka == 0: 
            if infraSensor.distance() <= 40:
                state(3)
            else:
                state(0)
        else:
            state(0)
    else:
        state(0)