import machine
from machine import I2C, Pin
import time
import math
from imu import MPU6050

# initialize I2C interface
i2c = I2C(0, sda=Pin(20), scl=Pin(21), freq=400000)

# initialize MPU6050
mpu = MPU6050(i2c)

# initialize LED pins
led1 = Pin(16, Pin.OUT)
led2 = Pin(17, Pin.OUT)
led3 = Pin(18, Pin.OUT)
led4 = Pin(19, Pin.OUT)

# get tilt angle
def get_tilt():
    x_angle = get_x_rotation(mpu.accel.x, mpu.accel.y, mpu.accel.z)
    y_angle = get_y_rotation(mpu.accel.x, mpu.accel.y, mpu.accel.z)
    return x_angle, y_angle

def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)

def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)

def dist(a,b):
    return math.sqrt((a*a)+(b*b))

# map angle to LED state
def map_tilt_to_leds(x_angle, y_angle):
    if x_angle > 15:
        led1.on()
        led2.off()
    elif x_angle < -15:
        led1.off()
        led2.on()
    else:
        led1.off()
        led2.off()

    if y_angle > 15:
        led3.on()
        led4.off()
    elif y_angle < -15:
        led3.off()
        led4.on()
    else:
        led3.off()
        led4.off()

while True:
    x_angle, y_angle = get_tilt()
    map_tilt_to_leds(x_angle, y_angle)
    time.sleep(0.1)