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

from assi import A
from robot_class import LazyRobot, Robot


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

    msu_R: Motor = robot.extra_R
    msu_L: Motor = robot.extra_L

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

    db.straight(608)  # mission 1
    msu_L.run_angle(1000, -570, Stop.HOLD, wait=True)  # abbassamento dell'asscenzore
    db.settings(864, 1000, 100, 50)
    db.straight(40)  # primo abbassamento del paletto rosso
    db.straight(-80)  # secondo abbassamento
    db.straight(40)

    msu_L.run_angle(1000, 420, Stop.HOLD, wait=False)  # inalzamento ascensore
    # db.settings(864, 300, 100, 50)

    db.straight(20)  # missione 2
    wait(2000)
    msu_L.run_angle(1000, -300, Stop.HOLD, wait=False)  # abbassamento dell'asscenzore
    db.arc(250, angle=30, then=Stop.HOLD, wait=True)

    msu_L.run_angle(1000, 90, Stop.HOLD, wait=True)
    db.straight(10)

    # 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)