#Motor drivers without accelerometer


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

# Get Adafruit IO username and key from settings.toml
aio_username = os.getenv("AIO_USERNAME")
aio_key = os.getenv("AIO_KEY")

# Setup feeds
throttle_feed = aio_username + "/feeds/throttle_feed"
take_off_feed = aio_username + "/feeds/take_off_feed"
emergency_off_feed = aio_username + "/feeds/emergency_off_feed"

# Setup PWM outputs for four motors (DRV8833 motor drivers)
# Motor driver 1
motor_pwm_a1 = pwmio.PWMOut(board.GP0, frequency=50)  # Motor A1 forward
motor_pwm_b1 = pwmio.PWMOut(board.GP1, frequency=50)  # Motor A1 reverse
motor_pwm_a2 = pwmio.PWMOut(board.GP2, frequency=50)  # Motor A2 forward
motor_pwm_b2 = pwmio.PWMOut(board.GP3, frequency=50)  # Motor A2 reverse

# Motor driver 2
motor_pwm_a3 = pwmio.PWMOut(board.GP4, frequency=50)  # Motor A3 forward
motor_pwm_b3 = pwmio.PWMOut(board.GP5, frequency=50)  # Motor A3 reverse
motor_pwm_a4 = pwmio.PWMOut(board.GP6, frequency=50)  # Motor A4 forward
motor_pwm_b4 = pwmio.PWMOut(board.GP7, frequency=50)  # Motor A4 reverse

# Create motor objects
motor_a1 = motor.DCMotor(motor_pwm_a1, motor_pwm_b1)
motor_a2 = motor.DCMotor(motor_pwm_a2, motor_pwm_b2)
motor_a3 = motor.DCMotor(motor_pwm_a3, motor_pwm_b3)
motor_a4 = motor.DCMotor(motor_pwm_a4, motor_pwm_b4)

# Set initial motor states
throttle = 0  # Initial throttle (0%)
for motor_obj in [motor_a1, motor_a2, motor_a3, motor_a4]:
    motor_obj.throttle = 0


# Helper to set motor throttles
def set_motor_throttles(throttle_value):
    """Set the same throttle for all motors."""
    motor_a1.throttle = throttle_value
    motor_a2.throttle = throttle_value
    motor_a3.throttle = throttle_value
    motor_a4.throttle = throttle_value


# MQTT Callbacks
def connected(client, userdata, flags, rc):
    print("Connected to Adafruit IO! Subscribing to feeds...")
    client.subscribe(throttle_feed)
    client.subscribe(take_off_feed)
    client.subscribe(emergency_off_feed)


def disconnected(client, userdata, rc):
    print("Disconnected from Adafruit IO!")


def message(client, topic, message):
    global throttle

    print(f"Received message on topic '{topic}': {message}")
    if topic == throttle_feed:
        try:
            throttle = float(message) / 100  # Convert 0–100% to 0.0–1.0
            set_motor_throttles(throttle)
            print(f"Throttle set to {throttle * 100}%")
        except ValueError:
            print("Invalid throttle value received.")
    elif topic == take_off_feed:
        if message.lower() == "on":
            print("Take off initiated!")
            throttle = 0.5  # Default lift-off throttle (50%)
            set_motor_throttles(throttle)
        elif message.lower() == "off":
            print("Landing initiated!")
            for i in reversed(range(50)): #if this is too low or high (will be too low likely bc not enough thrust) then make it a higher number for takeoff
                throttle = i/100
                time.sleep(0.1)
                set_motor_throttles(throttle)
            throttle = 0  # Stop all motors
            set_motor_throttles(throttle)
    elif topic == emergency_off_feed:
        if message.lower() == "stop":
            print("Emergency stop triggered!")
            throttle = 0  # Immediately cut power
            set_motor_throttles(throttle)


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

# Set up MQTT Client
pool = socketpool.SocketPool(wifi.radio)
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.on_connect = connected
mqtt_client.on_disconnect = disconnected
mqtt_client.on_message = message

# Connect to Adafruit IO
print("Connecting to Adafruit IO...")
mqtt_client.connect()


while True:
    mqtt_client.loop(2)