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

hub = InventorHub(top_side=vector(0, 0, 1), front_side=vector(0, 1, 0))
hub = InventorHub(top_side=Axis.Z, front_side=Axis.Y)

mDestra = Motor(Port.A)
mSinistra = Motor(Port.B)


def vai_avanti(speed: int, gradi: int):
    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.heading()
        print(f"gradi: {gradi}")
        speed = g - gradi
        print(speed)
        if g < 0:
            mSinistra.run_angle(speed, g, then=Stop.NONE, wait=False)
            mDestra.run_angle(speed, g, then=Stop.NONE, wait=False)
        elif g > 0:
            mSinistra.run_angle(speed, g, then=Stop.NONE, wait=False)
            mDestra.run_angle(speed, g, then=Stop.NONE, wait=False)

        if speed < 1 and speed > -1:
            break


hub.imu.reset_heading(0)
a = hub.imu.heading()
# print("G: " + str(hub.imu.rotation(Axis.Z)))
print(a)
precision_turn(90)