from machine import Pin, PWM, I2C
import time
from ssd1306 import SSD1306_I2C  # Bibliothèque pour OLED
from math import sqrt, atan2
from mpu6050 import MPU6050  # Assurez-vous d'avoir la classe MPU6050 correcte

# Initialisation des broches et des PWM pour les LEDs
led_left = PWM(Pin(16), freq=500)
led_right = PWM(Pin(17), freq=500)

# Initialisation de l'I2C pour l'écran OLED
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=100000)  # Assurez-vous que les bonnes broches sont utilisées
oled = SSD1306_I2C(128, 64, i2c)  # Résolution de l'écran : 128x64

# Initialisation du MPU6050 (Assurez-vous que le capteur est bien connecté)
mpu = MPU6050(i2c)

# Fonction pour ajuster l'intensité des LEDs en fonction de l'inclinaison
def update_leds_angle(angle_x):
    # Convertir l'angle en une plage de valeurs de 0 à 1023 pour le PWM
    intensity = abs(int(angle_x * 100))  # Angle multiplié par un facteur plus grand
    intensity = max(0, min(1023, intensity))  # Limiter entre 0 et 1023

    # Appliquer l'intensité aux LEDs
    led_left.duty(intensity)
    led_right.duty(1023 - intensity)  # L'intensité de la LED droite est inverse à celle de la gauche

# Affichage des valeurs sur l'écran OLED
def display_on_oled(angle_x, angle_y, intensity_left, intensity_right):
    oled.fill(0)  # Effacer l'écran
    oled.text('Angle X: {:.2f}'.format(angle_x), 0, 0)
    oled.text('Angle Y: {:.2f}'.format(angle_y), 0, 10)
    oled.text('LED Left: {}'.format(intensity_left), 0, 20)
    oled.text('LED Right: {}'.format(intensity_right), 0, 30)
    oled.show()  # Mettre à jour l'écran

# Boucle principale
while True:
    # Lire les données d'inclinaison
    accel_data = mpu.read_accel_data()  # Lire les données d'accéléromètre
    angle_x = atan2(accel_data['y'], accel_data['z'])  # Calculer l'angle X
    angle_y = atan2(-accel_data['x'], accel_data['z'])  # Calculer l'angle Y

    # Afficher les valeurs des angles pour le débogage
    print("Angle X: {:.2f}, Angle Y: {:.2f}".format(angle_x, angle_y))

    # Calculer l'intensité des LEDs
    update_leds_angle(angle_x)

    # Afficher les informations sur l'écran OLED
    display_on_oled(angle_x, angle_y, led_left.duty(), led_right.duty())

    time.sleep(0.1)  # Attendre un court instant avant de réafficher