from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor
from pybricks.parameters import Port
from pybricks.tools import wait, StopWatch

# ================= HUB & DEVICES =================
hub = PrimeHub()

left_motor = Motor(Port.A)
right_motor = Motor(Port.E)

front = Motor(Port.F)
back = Motor(Port.B)

floor = ColorSensor(Port.C)

timer = StopWatch()

# ================= BASIC SETTINGS =================
kP = 0.6
kD = 8
last_error = 0

# ================= BASIC FUNCTIONS =================
def run(l, r):
    left_motor.dc(l)
    right_motor.dc(r)

def stop():
    left_motor.brake()
    right_motor.brake()

def reset():
    left_motor.reset_angle(0)
    right_motor.reset_angle(0)

def PD(target, current):
    global last_error
    error = target - current
    output = (error * kP) + ((error - last_error) * kD)
    last_error = error
    return output

# ================= MOVEMENTS =================
def move(speed, distance):
    reset()
    while (abs(left_motor.angle()) + abs(right_motor.angle())) / 2 < distance:
        run(speed, speed)
    stop()

def turn(speed, angle):
    reset()
    while True:
        current = (left_motor.angle() - right_motor.angle()) / 2
        error = angle - current

        if abs(error) < 2:
            break

        correction = PD(angle, current)
        run(-speed + correction, speed - correction)

    stop()

def follow(speed, distance):
    reset()
    target = 50

    while (abs(left_motor.angle()) + abs(right_motor.angle())) / 2 < distance:
        light = floor.reflection()
        correction = PD(target, light)
        run(speed - correction, speed + correction)

    stop()

# ================= GLOBAL VARIABLES =================
currentPosition = 0
taking = 0
currentDirection = 1

# Example placeholders (you can adjust in competition)
blue = 0
white = 1
green = 2
red = 3
black = 4
yellow = 5

# ================= PROGRAMS =================

def TakeBlueWhite():
    global currentPosition, taking, currentDirection

    currentPosition = 6
    taking = 0
    currentDirection = -1
    target = [blue, white, green]

    turn(60, -90)
    move(-60, 100)
    turn(60, -90)

    while taking < 2:

        move(60, 120)
        currentPosition += currentDirection

        while currentPosition != target[taking]:
            move(60, 120)
            currentPosition += currentDirection

        move(60, 108)
        turn(60, 90 * currentDirection)

        front.dc(100)
        wait(100)

        move(60, 92)
        front.dc(-100)

        move(-60, 110)

        taking += 1

        if taking < 2:
            if target[taking] > currentPosition:
                currentDirection = 1
            else:
                currentDirection = -1

        turn(60, -90 * currentDirection)


def TakeGreen():
    move(40, 120)
    turn(60, -90)

    back.dc(65)
    move(-60, 170)
    back.dc(-65)
    move(-40, 45)


def Put1():
    move(80, 260)
    turn(60, -115)

    front.dc(100)
    wait(100)

    move(-100, 50)
    move(-70, 230)

    back.dc(70)
    move(70, 180)

    turn(60, 90)

    move(-90, 220)

    front.dc(100)
    wait(200)


def TakeRed():
    move(-100, 80)
    move(-80, 100)

    back.dc(50)
    move(-55, 150)

    back.dc(65)

    turn(60, 25)
    move(60, 50)


def TakeYellowBlack():
    global currentPosition, taking, currentDirection

    currentPosition = red
    taking = 0
    target = [black, yellow]
    currentDirection = -1

    turn(60, 90)

    while taking < 2:

        move(60, 120)
        currentPosition += currentDirection

        while currentPosition != target[taking]:
            move(60, 120)
            currentPosition += currentDirection

        move(60, 115)
        turn(60, 90)

        front.dc(100)
        wait(100)

        move(60, 85)
        front.dc(-100)

        if taking == 0:
            move(-80, 120)
            turn(60, -90)

        taking += 1


def GoToJunction():
    move(-100, 190)
    turn(60, -90)
    follow(60, 1650)


def Put2():
    turn(60, 39)

    move(-80, 710)

    back.dc(70)
    move(50, 300)

    back.dc(-70)

    turn(60, -39)

    move(70, 100)

    front.dc(100)
    wait(100)


def Cable1():
    move(-100, 350)
    move(100, 110)
    turn(60, 90)
    follow(60, 300)


def Cable2():
    move(100, 120)
    turn(60, 15)
    move(100, 480)


def Microphone():
    turn(60, -92)
    move(100, 250)


def Instruments():
    move(100, 300)
    turn(60, 90)
    move(100, 2300)


# ================= MAIN =================
def main():

    TakeBlueWhite()
    TakeGreen()
    Put1()

    TakeRed()
    TakeYellowBlack()

    GoToJunction()
    Put2()

    Cable1()
    Cable2()

    Microphone()
    Instruments()

    stop()


main()