from machine import Pin, I2C
import ssd1306, gfx, time, math
from lucsmpulib import init_mpu, get_data


# ESP32 Pin assignment 
i2c = I2C(0, scl=Pin(22), sda=Pin(21))

oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

init_mpu(i2c)

graphics = gfx.GFX(128, 64, oled.pixel)

led_center = Pin(27, Pin.OUT)
led_notcenter = Pin(14, Pin.OUT)

'''
# oled.text('Hello, Wokwi!', 10, 10)      
oled.fill(0)
# base graphics (same for each drawing cycle)
oled.text('4.1 V', 0, 0, 1) # replace with ADS1115 derived battery voltage
graphics.circle(64, 32, 5, 1)
graphics.circle(64, 32, 15, 1)
graphics.circle(64, 32, 25, 1)
graphics.circle(64, 32, 35, 1)

# bubble ball is a filled circle
graphics.fill_circle(60, 18, 3, 1)
oled.show()
'''

while True:
    readx, ready, readz = get_data(i2c)

    x = math.atan2(ready, math.sqrt(readx * readx + readz * readz)) * 180 / math.pi
    y = math.atan2(-readx, math.sqrt(ready * ready + readz * readz)) * 180 / math.pi
    z = math.atan2(readz, math.sqrt(readx * readx + ready * ready)) * 180 / math.pi
    
    print("=======================================================")
    
    if (x <= 0):
        x = math.floor(x)
    else:
        x = math.ceil(x)
    if (y <= 0):
        y = math.floor(y)
    else:
        y = math.ceil(y)
    print("x = ", x)
    print("y = ", y)

    center_x = x + 64
    center_y = y + 32

    led_x = abs(x)
    led_y =abs(y)

    if led_x <= 3 and led_y <= 3:
        led_center.on()
        led_notcenter.off()
    else:
        led_center.off()
        led_notcenter.on()


    oled.fill(0)
# base graphics (same for each drawing cycle)
    oled.text('4.1 V', 0, 0, 1) # replace with ADS1115 derived battery voltage
    oled.text('X:', 0, 40)
    oled.text('Y:', 100, 40)
    oled.text(str(x), 0, 50)
    oled.text(str(y), 100, 50)
    graphics.circle(64, 32, 5, 1)
    graphics.circle(64, 32, 15, 1)
    graphics.circle(64, 32, 25, 1)
    graphics.circle(64, 32, 35, 1)

# bubble ball is a filled circle
    graphics.fill_circle(center_x, center_y, 4, 1)
    oled.show()

    time.sleep(2)