from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor, ColorDistanceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

#GBC 57
hub = TechnicHub()

#Initialize motors
colorsensor=ColorDistanceSensor(Port.A) #Blue
motorturn=Motor(Port.B, gears=[5,27]) #Red
motorheight=Motor(Port.D, gears=[12,20], profile=7) #White
motormain=Motor(Port.C, positive_direction=Direction.COUNTERCLOCKWISE) #Green

#Point A = position after calibration
#Point B = position before ball input
#Point C = position before output 1 (orange)
#Point D = position before output 2 (white)

#Coordinates points
#Adjust these numbers slightly to define the points if the GBC does not work properly
#A
turn_A=-74
height_A=91
#B
turn_B=0
height_B=0
#C
turn_C= -40
height_C= 47
#D
turn_D= -62
height_D= 59

#Rotation amount to drop a ball into robotarm tray (input)
input_turn= 30

speed = 250 #Speed robotarm
speed_main_motor = 250 #Speed main motor for other modules

#Use 2 code lines below to get the hsv code for custom colors. Delete the # to uncomment this code and run it.
#You can see the values in the terminal below.
#Note that in different rooms/lighting conditions, the hvs codes can change.
#If you have the hvs codes you can eneter them beow for each color.

#color = colorsensor.hsv()
#print(color)

#Define the colors the sensor will recognize
Color.WHITE= Color(h=250, s=17, v=48)
Color.ORANGE=Color(h=17, s=93, v=42)
Color.NONE= Color(h=105, s=60, v=5)

mycolors = (Color.WHITE, Color.ORANGE, Color.NONE)
colorsensor.detectable_colors(mycolors)

###Functions###
#Function to move the robotarm upwards until it reaches its top position (calibrate height)
def calibrate_height():
    motorheight.dc(30)
    wait(100)
    while motorheight.speed() > 0:
        wait(10)
    motorheight.hold()

#Function to rotate the robotarm until it reaches the starting position (calibrate rotation)
def calibrate_turn():
    motorturn.dc(-20)
    wait(300)
    while motorturn.speed() < -20:
        wait(10)
    motorturn.hold()

#Function from point A to point B
def A_B():
    motorturn.run_angle(speed*2, 0.4*(turn_B-turn_A))
    motorheight.run_angle(speed/2, (height_B-height_A), wait=False)
    motorturn.run_angle(speed, 0.6*(turn_B-turn_A), wait=False)
    while not motorheight.done() or not motorturn.done():
        wait(50)

#Function from point B to point C
def B_C():
    motorturn.run_angle(speed, 0.3*(turn_C-turn_B), wait=False)
    motorheight.run_angle(speed/2, (height_C-height_B), wait=False)
    while not motorturn.done() or not motorheight.done():
        wait(50)
    motorturn.run_angle(speed*3, 0.7*(turn_C-turn_B))

#Function from point B to point D
def B_D():
    motorturn.run_angle(speed, 0.2*(turn_D-turn_B), wait=False)
    motorheight.run_angle(speed/2, (height_D-height_B), wait=False)
    while not motorheight.done() or not motorturn.done():
        wait(50)
    motorturn.run_angle(speed*3, 0.8*(turn_D-turn_B))

#Functions for ball input and output
#Function to put a ball in the tray at point B
def input_B():
    motorturn.run_angle(speed, input_turn)
    motorturn.run_angle(speed, -input_turn)

#Function to drop ball at point C and calibrate height
def output_C():
    calibrate_height()
    motorturn.run_angle(speed, (turn_B-turn_C), wait=False)
    motorheight.run_angle(speed/2, (height_B-height_A), wait=False)
    while not motorturn.done() or not motorheight.done():
        wait(50)

#Function to drop ball at point D and calibrate height and rotation
def output_D():
    calibrate_height()
    calibrate_turn()
    motorturn.run_angle(speed*2, 0.3*(turn_B-turn_A))
    motorturn.run_angle(speed, 0.7*(turn_B-turn_A), wait=False)
    motorheight.run_angle(speed/2, (height_B-height_A), wait=False)
    while not motorheight.done() or not motorturn.done():
        wait(50)

#START main programm

#Calibrate robotarm
calibrate_height()
calibrate_turn()

#Move robotarm from point A to point B
A_B()

#Start motor
motormain.run(speed_main_motor)

#Start loop
while True:
    color=colorsensor.color()
    if color == Color.WHITE:
        print("White")
        input_B()
        B_D()
        output_D()
    if color == Color.ORANGE:
        print("Orange")
        input_B()
        B_C()
        output_C()
    if color == Color.NONE:
        wait(300)