from machine import Pin, I2C
import time
import math
import network
from umqtt.simple import MQTTClient

# Adafruit IO credentials
ADAFRUIT_AIO_USERNAME = "Aswathvikram"
ADAFRUIT_AIO_KEY = "aio_awwx79uAeeHK8jkZRkM3cN3ON5j5"
FEEDS = {
    "up_down": "Aswathvikram/feeds/up-down",
    "left_right": "Aswathvikram/feeds/left-right",
    "side_ways": "Aswathvikram/feeds/side-ways"
}

# Wi-Fi credentials
WIFI_SSID = "Wokwi-GUEST"
WIFI_PASSWORD = ""

# MPU6050 class
class MPU6050:
    def __init__(self, i2c: I2C, address: int = 0x68):
        self.address = address
        self.i2c = i2c
        
    def wake(self) -> None:
        self.i2c.writeto_mem(self.address, 0x6B, bytes([0x01]))

    def read_accel_data(self) -> tuple:
        data = self.i2c.readfrom_mem(self.address, 0x3B, 6)
        x = self._translate_pair(data[0], data[1])
        y = self._translate_pair(data[2], data[3])
        z = self._translate_pair(data[4], data[5])
        modifier = 16384.0
        return (x / modifier, y / modifier, z / modifier)
    
    def _translate_pair(self, high: int, low: int) -> int:
        value = (high << 8) + low
        if value >= 0x8000:
            value = -((65535 - value) + 1)
        return value

# Initialize I2C and MPU6050
i2c = I2C(0, scl=Pin(17), sda=Pin(16), freq=400000)
mpu = MPU6050(i2c)
mpu.wake()

# Connect to Wi-Fi
def connect_wifi():
    wlan = network.WLAN(network.STA_IF)
    wlan.active(True)
    wlan.connect(WIFI_SSID, WIFI_PASSWORD)
    while not wlan.isconnected():
        time.sleep(0.5)
    print("Connected to Wi-Fi:", wlan.ifconfig())

# Send data to Adafruit IO
def send_to_adafruit(client, feed, value):
    topic = FEEDS[feed]
    client.publish(topic, str(value))

# Main program
def main():
    connect_wifi()
    client = MQTTClient("mpu6050_client", "io.adafruit.com", user=ADAFRUIT_AIO_USERNAME, password=ADAFRUIT_AIO_KEY)
    client.connect()
    
    while True:
        x, y, z = mpu.read_accel_data()
        send_to_adafruit(client, "up_down", x)
        send_to_adafruit(client, "left_right", y)
        send_to_adafruit(client, "side_ways", z)
        print("Uploaded: X={:.2f}, Y={:.2f}, Z={:.2f}".format(x, y, z))
        time.sleep(2)

# Run the program
try:
    main()
except KeyboardInterrupt:
    print("Program stopped.")