from pybricks.hubs import InventorHub
from pybricks.parameters import Axis, Port, Stop
from pybricks.pupdevices import Motor
from pybricks.tools import vector, wait

# from pybricks.hub import PrimHub

hub = InventorHub(top_side=vector(1, 1, 0), front_side=vector(-1, 1, 0))
mDestra = Motor(Port.F)
mSinistra = Motor(Port.B)


def vai_avanti(speed, gradi):
    print(f"vado avanti di {gradi} a {speed}")
    mSinistra.run_angle(speed, gradi, then=Stop.NONE, wait=False)
    mDestra.run_angle(-speed, gradi, then=Stop.NONE, wait=True)


def turn(speed_L, speed_R, gradi):
    print("girogiro")
    mSinistra.run_angle(speed_L, gradi, then=Stop.NONE, wait=False)
    mDestra.run_angle(speed_R, gradi, then=Stop.NONE, wait=True)


# reset_angle()

# vai_avanti(1000, 360)
turn(1000, 1000, 120)
# vai_avanti(1000, 360)
# turn(1000, 1000, 360)
# vai_avanti(1000, 360)
# turn(1000, 1000, 360)
# vai_avanti(1000, 360)
# turn(1000, 1000, 360)

giroX = hub.imu.rotation(Axis.X)
giroY = hub.imu.rotation(Axis.Y)
giroZ = hub.imu.rotation(Axis.Z)
print(giroX)
print(giroY)
print(giroZ)

while not (mSinistra.done() and mDestra.done()):
    wait(10)