#!/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
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile

#For Serial communication: 
from pybricks.parameters import Color, Port
from pybricks.iodevices import AnalogSensor, UARTDevice

# Write your program here
ev3 = EV3Brick()
ev3.speaker.beep()

#Variables for Speed: 
speed = 500
open_speed = 50

#Angle Variables: 
move_angle = 270
dump_angle = 100
open_angle = -20
close_angle = 25
setting_angle = 0 

direction = -1

#Motor and Sensors: 
platform = Motor(Port.D)
funnel = Motor(Port.C)
dump_m = Motor(Port.B)
setting_m = Motor(Port.A)


# setup uart port
uart = UARTDevice(Port.S2, 9600, timeout=2000)

#To connect to Serial using UART
def get_pi_value(): 
    try:
        if (uart.waiting()) !=0:
            msg = uart.read(uart.waiting()).decode('utf-8')
            print(msg[-1]) 
            return msg[-1]
    except Exception as e:
        print(e)
    

def color_setter():
    color = setting_m.angle() 
    print(color)
    uart.write(str(color).encode())
    
#To Open and place the lego on the platform
def open_door():
    funnel.run_angle(open_speed,open_angle,stop_type = Stop.COAST, wait=True)
    wait(500)
    funnel.run_angle(speed,close_angle,stop_type = Stop.COAST, wait=True)

#To sort into the the two boxes 
def judgement(pi_value):
    spin = 0
    if pi_value == '1': #1 correlates to red 
        spin = 1
        ev3.speaker.play_file('angel.wav') 
    elif pi_value == '0': #2 correlates to other colors
        spin = -1
    for i in range(2):
        dump_m.run_angle(speed,dump_angle*spin,stop_type = Stop.COAST, wait=True)
        spin = spin*-1
        wait(500)

#Main Code:
def main(): 
    direction = -1  
    while True:
        color_setter() 
        if direction == -1: 
            open_door()     

        platform.run_angle(speed, direction*move_angle, stop_type=Stop.COAST, wait=True)
        direction  = direction*-1
        
        if direction == 1:
            wait(2000)
            pi_value = get_pi_value()
            print(pi_value)
            judgement(pi_value)

        
main()