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

from assi import A
from leo import giro
from robot_class import LazyRobot, Robot
from robots import leorobot as robot


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

    def gira_fino_a_quando(gradi: int):
        posizione_iniziale = db.angle()
        gradi_mancanti = gradi - posizione_iniziale
        db.turn(gradi_mancanti)
        # dangolo = db.angle()
        # if angolo < gradi:
        # db.turn(gradi - angolo)

    def giro_preciso(gradi: int):
        destinazione = gradi
        aumento_v = 30
        posizione_iniziale = db.angle()
        gradi_attuali = round(db.angle(), 1)
        gradi_mancanti_iniziali = gradi - posizione_iniziale
        if gradi_mancanti_iniziali <= 180 and gradi_mancanti_iniziali >= -180:
            curva_normale = True
        else:
            curva_normale = False
            if gradi_mancanti_iniziali > 0:
                gradi_da_percorerre = gradi_mancanti_iniziali - 360
                destinazione = posizione_iniziale + gradi_da_percorerre
            else:
                gradi_da_percorerre = gradi_mancanti_iniziali + 360
                destinazione = posizione_iniziale + gradi_da_percorerre
        print(
            f"curva precisa a {gradi} per andare da {posizione_iniziale} a {destinazione}"
        )
        while (destinazione - gradi_attuali) > 0.2 or (
            destinazione - gradi_attuali
        ) < -0.2:
            gradi_mancanti = gradi_attuali - destinazione
            velocita = abs(gradi_mancanti)
            print("Gradi manc: ", gradi_mancanti)
            if curva_normale:
                if gradi_mancanti > 0:
                    robot.left.run(velocita + aumento_v)
                    robot.right.run(-velocita - aumento_v)
                    hub.display.char(">")
                elif gradi_mancanti < 0:
                    robot.right.run(velocita + aumento_v)
                    robot.left.run(-velocita - aumento_v)
                    hub.display.char("<")
                gradi_attuali = round(db.angle(), 1)
            # else:
            # if gradi > 0:
            #     if gradi_di_arrivo_calcolati == false:
            #         gradi_di_arrivo = 360 - gradi_mancanti
            #         gradi_mancanti = gradi_di_arrivo - gradi
            #     print("Gradi manc magg: ", gradi_mancanti)
            #     if gradi_mancanti < 0:
            #         robot.left.run(velocita + aumento_v)
            #         robot.right.run(-velocita - aumento_v)
            #         hub.display.char(">")
            #     elif gradi_mancanti > 0:
            #         robot.right.run(velocita + aumento_v)
            #         robot.left.run(-velocita - aumento_v)
            #         hub.display.char("<")
            #     gradi_attuali = round(db.angle(), 1)
            # elif gradi < 0:
            #     gradi_di_arrivo = 360 - gradi_mancanti * -1
            #     gradi_mancanti = gradi_di_arrivo - gradi
            #     print("Gradi manc magg: ", gradi_mancanti)
            #     if gradi_mancanti < 0:
            #         robot.left.run(velocita + aumento_v)
            #         robot.right.run(-velocita - aumento_v)
            #     elif gradi_mancanti > 0:
            #         robot.right.run(velocita + aumento_v)
            #         robot.left.run(-velocita - aumento_v)
            #         hub.display.char("<")
            #     gradi_attuali = round(db.angle(), 1)
            # if gradi_mancanti > 0:
            #     robot.right.run(velocita + aumento_v)
            #     robot.left.run(-velocita - aumento_v)
            #     hub.display.char(">")
            # elif gradi_mancanti < 0:
            #     robot.left.run(velocita + aumento_v)
            #     robot.right.run(-velocita - aumento_v)
            #     hub.display.char("<")
            # gradi_attuali = round(db.angle(), 1)

        print("Gradi att: ", gradi_attuali)

    db.use_gyro(True)  # abilitiamo il giroscopio
    # (mm/s, mm/s², deg/s, deg/s²)
    db.settings(100, 100, 90, 50)
    wait(300)
    db.reset(angle=0)

    # db.straight(100)
    # gira_precisa(90)
    # print(db.angle())
    # db.straight(100)
    # gira_precisa(180)
    # print(db.angle())
    # db.straight(100)
    # gira_precisa(270)
    # print(db.angle())
    # db.straight(100)
    # gira_precisa(360)
    # print(db.angle())

    giro_preciso(-120)
    print(db.angle())
    giro_preciso(120)
    print(db.angle())
    # gira_precisa(-60)
    # print(db.angle())

    # gira_fino_a_quando(120)
    # print(db.angle())
    # gira_fino_a_quando(60)
    # print(db.angle())
    # gira_fino_a_quando(-60)
    # print(db.angle())

    print(f"drived {db.distance()}")


if __name__ == "__main__":
    import batteria
    from robots import cbrobot as robot

    main(robot)