import board, pwmio, digitalio, time
from adafruit_motor import servo
from adafruit_ble import BLERadio
from adafruit_ble.advertising.standard import ProvideServicesAdvertisement
from adafruit_ble.services.nordic import UARTService


ble = BLERadio()
uart = UARTService()
advertisement = ProvideServicesAdvertisement(uart)
advertisement.complete_name = "minigolf"
ble.name = advertisement.complete_name

pwm = pwmio.PWMOut(board.A1, frequency=50)
pwm2 = pwmio.PWMOut(board.A2, frequency=50)
# Change min_pulse / max_pulse values to calibrate servo arm in either direction.
# min ~ 500, max ~ 2500. Defaults are below & can be eliminated.
servo_1 = servo.Servo(pwm, min_pulse = 750, max_pulse = 2250)
cont_servo = servo.ContinuousServo(pwm2)

def move_position(command):
    if command == "left":
        cont_servo.throttle = -0.08
    elif command == "right":
        cont_servo.throttle = 0.03

while True:
    ble.start_advertising(advertisement)  # Start advertising.
    print(f"Advertising as: {advertisement.complete_name}") # Name prints once each time the board isn't connected
    was_connected = False

    while not was_connected or ble.connected:
        if ble.connected:  # If BLE is connected...
            was_connected = True
            if uart.in_waiting:
                try:
                    data = uart.readline().decode().strip()
                    club_angle, hole_command = data.split(",")
                    club_angle = int(club_angle)
                    servo_1.angle = club_angle
                    if hole_command:
                        move_position(hole_command)
                    else:
                        cont_servo.throttle = 0
                except ValueError:
                    print("invalid data")