#!/usr/bin/env pybricks-micropython

from pybricks import ev3brick as brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import (Port, Stop, Direction, Button, Color,
                                 SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase

# Write your program here

color_sensor = ColorSensor(Port.S4)
distance_sensor = UltrasonicSensor(Port.S1)

# Initialize two motors with default settings on Port B and Port C.
# These will be the left and right motors of the drive base.
left_motor = Motor(Port.B)
right_motor = Motor(Port.C)

# The wheel diameter of the Robot Educator is 56 millimeters.
wheel_diameter = 56

# The axle track is the distance between the centers of each of the wheels.
# For the Robot Educator this is 114 millimeters.
axle_track = 114

# The DriveBase is composed of two motors, with a wheel on each motor.
# The wheel_diameter and axle_track values are used to make the motors
# move at the correct speed when you give a motor command.
robot = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)

brick.display.clear()

def calibrate():
    brick.display.text("WHITE?")
    brick.sound.beep()
    while not any(brick.buttons()):
        wait(10)
    white = color_sensor.reflection()

    while any(brick.buttons()):
        wait(10)

    brick.display.text("BLACK?")
    brick.sound.beep()
    while not any(brick.buttons()):
        wait(10)
    black = color_sensor.reflection()
    return black, white    

black, white = calibrate()
reference = (black + white)/2
Kp = 1

brick.display.text("Reference at: "+str(reference))

while True:

    measure = color_sensor.reflection()
    error   = reference - measure 

    robot.drive(100, Kp*error)

    wait(50)