from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Button, Color, Direction, Port, Stop
from pybricks.tools import wait
from pybricks.iodevices import XboxController

#unstable test version

# Constants
PORT_DRIVE = Port.A
PORT_STEERING = Port.C
PORT_TILT = Port.B

LOW_BATTERY = 7.0  # V
LOOP_DELAY = 100  # ms

STEERING_SPEED = 170  
TILT_SPEED = 100  # % duty cyle

# Global variables
hub = TechnicHub()
remote = XboxController()

motor_drive = Motor(PORT_DRIVE, Direction.COUNTERCLOCKWISE)
motor_steering = Motor(PORT_STEERING, Direction.COUNTERCLOCKWISE)
motor_tilt = Motor(PORT_TILT)


# Utility functions
def halt():
    hub.light.blink(Color.RED, [100, 100, 100, 100, 100, 500])
    for _ in range(10):
        wait(LOOP_DELAY)


def calibrate(motor, speed):
    hub.light.blink(Color.GREEN, [500, 500])

    angle_left = motor.run_until_stalled(speed, duty_limit=50)
    wait(500)
    
    angle_right = motor.run_until_stalled(-speed, duty_limit=50)
    wait(500)

    half_range = (angle_right - angle_left) / 2.0
    motor.reset_angle(half_range)
    motor.run_target(speed, target_angle=0.0)
    wait(500)

    return half_range


# Calibration
try:
    max_steer_angle = calibrate(motor_steering, STEERING_SPEED)
except:
    halt()


# Main event loop
try:
    while True:
        if hub.battery.voltage() > LOW_BATTERY:
            hub.light.on(Color.GREEN)
        else:
            hub.light.blink(Color.YELLOW, [500, 500])

        steering = remote.joystick_right()[0]

        motor_steering.run_target(STEERING_SPEED, max_steer_angle * steering / 100.0)

        drive_fwd, drive_bwd = remote.triggers()
        motor_drive.dc(drive_fwd - drive_bwd)
       
        buttons = remote.buttons.pressed()
        if Button.A in buttons:
            motor_tilt.dc(TILT_SPEED)  # up
        elif Button.B in buttons:
            motor_tilt.dc(-TILT_SPEED)  # down
        else:
            motor_tilt.dc(0)

        if Button.GUIDE in buttons:
            hub.system.shutdown()
except:
    halt()