from machine import Pin, PWM, time_pulse_us
from time import sleep_us, sleep_ms, sleep

# -------------------------
# Configuration des broches
# PINS d'exemple, à adapter selon le câblage
# -------------------------

# Moteur gauche (L298N - côté A)
PIN_ENA = 25   # PWM moteur gauche
PIN_IN1 = 26
PIN_IN2 = 27

# Moteur droit (L298N - côté B)
PIN_ENB = 14   # PWM moteur droit
PIN_IN3 = 12
PIN_IN4 = 13

# Capteur HC-SR04
PIN_TRIG = 5
PIN_ECHO = 18

# -------------------------
# Initialisation des broches
# -------------------------

# Moteur gauche
in1 = Pin(PIN_IN1, Pin.OUT)
in2 = Pin(PIN_IN2, Pin.OUT)
ena_pwm = PWM(Pin(PIN_ENA))
ena_pwm.freq(1000)  # fréquence PWM

# Moteur droit
in3 = Pin(PIN_IN3, Pin.OUT)
in4 = Pin(PIN_IN4, Pin.OUT)
enb_pwm = PWM(Pin(PIN_ENB))
enb_pwm.freq(1000)

# HC-SR04
trig = Pin(PIN_TRIG, Pin.OUT)
echo = Pin(PIN_ECHO, Pin.IN)

# Vitesse par défaut (0 à 1023)
VITESSE_BASE = 600


# -------------------------
# Fonctions moteur
# -------------------------

def moteur_gauche_stop():
    """Arrête le moteur gauche."""
    in1.value(0)
    in2.value(0)
    ena_pwm.duty(0)


def moteur_droit_stop():
    """Arrête le moteur droit."""
    in3.value(0)
    in4.value(0)
    enb_pwm.duty(0)


def moteurs_stop():
    """Arrête les deux moteurs."""
    moteur_gauche_stop()
    moteur_droit_stop()


def moteur_gauche_avant(vitesse):
    """Fait tourner le moteur gauche en avant."""
    in1.value(1)
    in2.value(0)
    ena_pwm.duty(vitesse)


def moteur_gauche_arriere(vitesse):
    """Fait tourner le moteur gauche en arrière."""
    in1.value(0)
    in2.value(1)
    ena_pwm.duty(vitesse)


def moteur_droit_avant(vitesse):
    """Fait tourner le moteur droit en avant."""
    in3.value(1)
    in4.value(0)
    enb_pwm.duty(vitesse)


def moteur_droit_arriere(vitesse):
    """Fait tourner le moteur droit en arrière."""
    in3.value(0)
    in4.value(1)
    enb_pwm.duty(vitesse)


def avancer(vitesse):
    """Le robot avance tout droit."""
    moteur_gauche_avant(vitesse)
    moteur_droit_avant(vitesse)


def reculer(vitesse):
    """Le robot recule tout droit."""
    moteur_gauche_arriere(vitesse)
    moteur_droit_arriere(vitesse)


def tourner_gauche(vitesse):
    """
    Le robot tourne sur place vers la gauche :
    - moteur droit en avant
    - moteur gauche en arrière
    """
    moteur_gauche_arriere(vitesse)
    moteur_droit_avant(vitesse)


def tourner_droite(vitesse):
    """Le robot tourne sur place vers la droite."""
    moteur_gauche_avant(vitesse)
    moteur_droit_arriere(vitesse)


# -------------------------
# Fonction mesure HC-SR04
# -------------------------

def mesure_distance_cm():
    """
    Mesure la distance en cm avec le HC-SR04.
    Retourne None si la mesure échoue.
    """
    # S'assurer que TRIG est bas
    trig.value(0)
    sleep_ms(2)

    # Envoyer une impulsion de 10 us sur TRIG
    trig.value(1)
    sleep_us(10)
    trig.value(0)

    # Mesurer la durée de l'impulsion sur ECHO
    # timeout_us permet d'éviter de bloquer si pas de réponse
    duree = time_pulse_us(echo, 1, 30000)  # 30 ms max

    if duree < 0:
        # -1, -2, -3 = codes d'erreur
        return None

    # Convertir en distance :
    # vitesse du son ≈ 343 m/s = 0,0343 cm/us
    # distance aller-retour -> on divise par 2
    distance = (duree * 0.0343) / 2

    return distance