#!/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
from pybricks.iodevices import UARTDevice

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


#Global Variables: 
fwdWheel_d = 56
fwdAxle_track = 114
speed = 50
angle = 50

#Variables:
light = ColorSensor(Port.S1) 
fwdDrive_left = Motor(Port.C)
fwdDrive_right = Motor(Port.A)
fwd = DriveBase(fwdDrive_left,fwdDrive_right,fwdWheel_d,fwdAxle_track)
sideDrive = Motor(Port.D)

# setup uart port
serial_port = UARTDevice(Port.S4, 9600, timeout = 3.0)


def fwdmove(y_pos):
    fwd.drive(speed,y_pos*angle) 

def sidemove(x_pos):
    sideDrive.run(x_pos*speed)

def detect_line():
    hit = False
    if (light.color() != Color.WHITE)
        fwdmove.stop()
        sidemove.stop()
        hit = True
    return hit

def get_stick_val()
    if serial_port.waiting > 1:
        serial_port.read(serial_port.waiting()-1)
        movement = serial_port.read()
    else:
        try:
            movement = serial_port.read()
        except error as e:
            print("Cannot read from buffer")
            print(e)
            movement = 0

def main():
    x_pos,y_pos = get_stick_val()
    sidemove(x_pos)
    fwdmove(y_pos)
    serial_port.write(detect_line())