#!/usr/bin/env pybricks-micropython

from pybricks import ev3brick as brick
from pybricks.ev3devices import (Motor, ColorSensor)
from pybricks.parameters import (Port, Stop, Direction, Color,
                                 SoundFile)
from pybricks.tools import print, wait
from pybricks.robotics import DriveBase

baseWidth = 150
wheelDiametr = 82

rightMotor = Motor(Port.B, Direction.COUNTERCLOCKWISE)
leftMotor = Motor(Port.C, Direction.COUNTERCLOCKWISE)

robot = DriveBase(leftMotor, rightMotor, wheelDiametr, baseWidth)

# Write your program here
brick.sound.beep()

lineSensor = ColorSensor(Port.S2)
colorSensor = ColorSensor(Port.S1)
crossSensor = ColorSensor(Port.S3)

class PD:
    def __init__(self, p, d):
        self.deltaOld = None
        self.p = p
        self.d = d

    def error(self, delta):
        if self.deltaOld == None:
            self.deltaOld = delta

        error = self.p*delta + (delta - self.deltaOld) * self.d
        self.deltaOld = delta
        return error

gray = 14
kp = 2
kd = 2.25
speed = 300
pd = PD(kp, kd)

def p_line():

    error = pd.error(lineSensor.reflection() - gray)
    leftMotor.run(speed+error)
    rightMotor.run(speed-error)


while True:

    p_line()
    print(lineSensor.reflection())
    if colorSensor.color() == Color.RED:
        brick.sound.file(SoundFile.RED, 100)
    elif colorSensor.color() == Color.BLUE:
        brick.sound.file(SoundFile.BLUE, 100)
    elif colorSensor.color() == Color.GREEN:
        brick.sound.file(SoundFile.GREEN, 100)
    elif colorSensor.color() == Color.YELLOW:
        brick.sound.file(SoundFile.YELLOW, 100)
    elif colorSensor.color() == Color.WHITE:
        brick.sound.file(SoundFile.WHITE, 100)