import ev3_dc as ev3
import time
import keyboard
from thread_task import (
    Task,
    Repeated,
    Periodic,
    STATE_STARTED,
    STATE_FINISHED,
    STATE_STOPPED,
)

MAC = '00:16:53:50:8A:86'  # Cambia esto por la dirección MAC de tu ladrillo EV3

vt = 0
vr = 0
vd = 0
vi = 0
speed = 0
turn = 0
exit = False

def get_turn(vmax,vmin,sign=1):
    if vmax == 0:
        return 0
    ratio = 100 * (1 - vmin / vmax)
    return int(sign * ratio)

def update_speed_turn(vt_change, vr_change):
    global vt, vr, speed, turn, vd, vi
    vt = max(min(vt + vt_change, 50), -50)
    vr = max(min(vr + vr_change, 50), -50)
    vd = vt + vr
    vi = vt - vr
    if abs(vd) > abs(vi):
        speed = max(min(vd, 100), -100)
        turn = get_turn(vd, vi, 1)
    else:
        speed = max(min(vi, 100), -100)
        turn = get_turn(vi, vd, -1)

keyboard.add_hotkey('up', lambda: update_speed_turn(5, 0))
keyboard.add_hotkey('down', lambda: update_speed_turn(-5, 0))
keyboard.add_hotkey('left', lambda: update_speed_turn(0, 5))
keyboard.add_hotkey('right', lambda: update_speed_turn(0, -5))
keyboard.add_hotkey('q', lambda: globals().update(exit=True))

with ev3.TwoWheelVehicle(
    0.0210,  # radius_wheel
    0.1224,  # tread
    protocol=ev3.BLUETOOTH,
    host=MAC,
    speed=40
) as vehicle:

    print("Use arrow keys to change speed and turn:")
    print("↑/↓ arrows: change speed variable")
    print("←/→ arrows: change turn variable")
    print("Press 'q' to quit")

    def drive():
        global speed, turn
        if speed != 0:
            vehicle.move(speed, turn)
        else:
            vehicle.stop(brake=True)
        # print(vehicle.position)

    drive_task = Periodic(0.1, drive)
    drive_task.start()

    while not exit:
        if keyboard.is_pressed("up") or keyboard.is_pressed("down") or \
           keyboard.is_pressed("left") or keyboard.is_pressed("right"):
            print(f"vt: {vt}, vr: {vr}, vd: {vd}, vi: {vi}, speed: {speed}, turn: {turn}")
            time.sleep(0.1)  # Prevent rapid changes
        time.sleep(0.05)  # Small delay to reduce CPU usage

    drive_task.stop()
    vehicle.stop(brake=True)
    time.sleep(1)  # Wait for the vehicle to stop
    print("Programa terminado")