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

# from pybricks.hub import PrimHub

hub = InventorHub(top_side=vector(1, 1, 0), front_side=vector(-1, 1, 0))

mDestra = Motor(Port.A)
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, g):
#     print("girogiro")
#     mSinistra.run_angle(speed_L, g, then=Stop.NONE, wait=False)
#     mDestra.run_angle(speed_R, g, then=Stop.NONE, wait=True)


def precision_turn(g):
    while True:
        gradi = hub.imu.rotation(Axis.Z)
        print(f"gradi: {gradi}")
        speed = g - gradi
        if g < 0:
            mSinistra.run_angle(speed, g, then=Stop.NONE, wait=False)
            mDestra.run_angle(speed, g, then=Stop.NONE, wait=True)
        elif g > 0:
            mSinistra.run_angle(speed, g, then=Stop.NONE, wait=False)
            mDestra.run_angle(speed, g, then=Stop.NONE, wait=True)

        if gradi == g:
            break


# reset_yaw()
print("G: " + str(hub.imu.rotation(Axis.Z)))
precision_turn(90)