from pickle import STOP
from threading import ThreadError

from pybricks.hubs import PrimeHub
from pybricks.parameters import Axis, Direction, Port, Stop
from pybricks.pupdevices import Motor
from pybricks.robotics import DriveBase

from assi import A
from robot_class import LazyRobot, Robot

msu_R = Motor(Port.E)
msu_L = Motor(Port.D)


def main(robot: LazyRobot):
    hub = robot.hub
    db = robot.db

    db.settings(864, 300, 100, 50)

    db.straight(600)  # mission 1
    msu_L.run_angle(1000, -720, Stop.HOLD, wait=True)  # abbassamento dell'asscenzore
    db.straight(50)  # primo abbassamento del paletto rosso
    db.straight(-120)  # secondo abbassamento
    db.straight(40)
    msu_L.run_angle(1000, -720, Stop.HOLD, wait=False)  # inalzamento ascensore

    db.straight(50)  # missione 2
    db.arc(500, 20, then=Stop.HOLD, wait=True)
    msu_L.run_angle(1000, 360, Stop.HOLD, wait=True)

    # db.straight(-150)  # mission 3      #Return Point start arc
    # msu_L.run_angle(1000, -1440, Stop.COAST_SMART, wait=True)
    # db.arc(275, 90, then=Stop.HOLD, wait=False)
    # db.straight(130)
    # msu_L.run_angle(1000, 1440, Stop.COAST_SMART, wait=True)  # inclinazione carrello


if __name__ == "__main__":
    import batteria
    from robot_local import robot

    main(robot)