import board, time, pwmio
import os, ssl, socketpool, wifi
import adafruit_minimqtt.adafruit_minimqtt as MQTT
from adafruit_motor import servo

#set up servos
pwm1 = pwmio.PWMOut(board.GP12, frequency=50)
servo_1 = servo.Servo(pwm1, min_pulse = 750, max_pulse = 2250)
pwm2 = pwmio.PWMOut(board.GP11, frequency=50)
servo_2 = servo.Servo(pwm2, min_pulse = 750, max_pulse = 2250)

def claw_servo():
    for i in range(0, 61, 1):
        servo_1.angle = i
        time.sleep(0.05)
    for i in range(60, -1, -1):
        servo_1.angle = i
        time.sleep(0.05)

def down_servo():
    for i in range (180, -1, -1):
        servo_2.angle = i
        time.sleep(0.05)

def up_servo():
    for i in range(0, 180, 1):
        servo_2.angle = i
        time.sleep(0.05)

# Setup continuous servo on GP15 left GP14 right
pwm_left = pwmio.PWMOut(board.GP15, frequency=50)
pwm_right = pwmio.PWMOut(board.GP14, frequency=50)
servo_left = servo.ContinuousServo(pwm_left)
servo_right = servo.ContinuousServo(pwm_right)

def move_servo(left_throttle, right_throttle):
    left_adjustment = -1.0
    right_adjustmemnt = 1.0
    servo_left.throttle = left_throttle * left_adjustment
    servo_right.throttle = right_throttle * right_adjustmemnt

# Get adafruit io username and key from settings.toml
aio_username = os.getenv('AIO_USERNAME')
aio_key = os.getenv('AIO_KEY')

# Setup a feed: This may have a different name than your Dashboard
move_feed = aio_username + "/feeds/move_feed"
claw_feed = aio_username + "/feeds/claw_feed"
down_feed = aio_username + "/feeds/pulley_down_feed"
up_feed = aio_username + "/feeds/pulley_up_feed"
# Setup functions to respond to MQTT events

def connected(client, userdata, flags, rc):
    # Connected to broker at adafruit io
    print("Connected to Adafruit IO! Listening for topic changes in feeds I've subscribed to")
    # Subscribe to all changes on the feed.
    client.subscribe(move_feed)
    client.subscribe(claw_feed)
    client.subscribe(down_feed)
    client.subscribe(up_feed)

def disconnected(client, userdata, rc):
    # Disconnected from the broker at adafruit io
    print("Disconnected from Adafruit IO!")

def message(client, topic, message):
    # The bulk of your code to respond to MQTT will be here, NOT in while True:
    print(f"topic: {topic}, message: {message}")
    if topic == move_feed: # robot directions
        if message == "stop":
            move_servo(0.0, 0.0)
        elif message == "forward":
            move_servo(-0.5, -0.5)
        elif message == "backward":
            move_servo(0.5, 0.5)
        elif message == "left":
            move_servo(0.5, -0.5)
        elif message == "right":
            move_servo(-0.5, 0.5)
        elif message == "0":
            move_servo(0.0, 0.0)
    if topic == claw_feed:
        if message == "1":
            servo_1.angle = claw_servo()
    elif topic == down_feed:
        if message == "1":
            servo_2.angle = down_servo()
    elif topic == up_feed:
        if message == "1":
            servo_2.angle = up_servo()

# Connect to WiFi
print(f"Connecting to WiFi")
wifi.radio.connect(os.getenv("WIFI_SSID"), os.getenv("WIFI_PASSWORD"))
print("Connected!")

# Create a socket pool
pool = socketpool.SocketPool(wifi.radio)

# Set up a MiniMQTT Client - this is our current program that subscribes or "listens")
mqtt_client = MQTT.MQTT(
    broker=os.getenv("BROKER"),
    port=os.getenv("PORT"),
    username=aio_username,
    password=aio_key,
    socket_pool=pool,
    ssl_context=ssl.create_default_context(),
)

mqtt_client.connect()

# Setup the "callback" mqtt methods above
mqtt_client.on_connect = connected
mqtt_client.on_disconnect = disconnected
mqtt_client.on_message = message

# Connect to the MQTT broker (adafruit io for us)
print("Connecting to Adafruit IO...")
mqtt_client.connect()

mqtt_client.publish(move_feed + "/get", "")
mqtt_client.publish(claw_feed + "/get", "")
mqtt_client.publish(up_feed + "/get", "")
mqtt_client.publish(down_feed + "/get", "")
print("Robot Working!")

servo_2.angle = 180
servo_1.angle = 0

while True:
    mqtt_client.loop(2)