"""
Contrôle d'une base roulante avec ESP32-C3-DevkitM-1
- Pont en H L298N pour 2 moteurs DC
- Capteur à ultrasons pour détection d'obstacles
"""

from machine import Pin, PWM, time_pulse_us
import time
import math

# ============================================================================
# Configuration des pins GPIO
# ============================================================================

# Capteur à ultrasons
TRIG_PIN = 8
ECHO_PIN = 9

# Pont en H L298N
ENA_PIN = 4   # PWM pour moteur A
IN1_PIN = 5   # Direction moteur A
IN2_PIN = 6   # Direction moteur A
IN3_PIN = 2   # Direction moteur B
IN4_PIN = 1   # Direction moteur B
ENB_PIN = 0   # PWM pour moteur B

# ============================================================================
# Paramètres
# ============================================================================

PWM_FREQ = 1000      # Fréquence PWM en Hz
MAX_PWM = 65535      # Valeur PWM maximale pour ESP32
SPEED_MAX = 65535    # Vitesse maximale (100%)
SPEED_MIN = 32767    # Vitesse minimale (50%)

# Distance de sécurité pour le capteur à ultrasons (en cm)
SAFE_DISTANCE = 20


# ============================================================================
# Classe pour le contrôle des moteurs
# ============================================================================

class MoteurDC:
    """Classe pour contrôler un moteur DC via le pont en H"""
    
    def __init__(self, en_pin, in1_pin, in2_pin):
        """
        Initialise un moteur DC
        
        Args:
            en_pin: PIN PWM pour la vitesse
            in1_pin: PIN pour la direction (sens 1)
            in2_pin: PIN pour la direction (sens 2)
        """
        self.en = PWM(Pin(en_pin))
        self.en.freq(PWM_FREQ)
        self.in1 = Pin(in1_pin, Pin.OUT)
        self.in2 = Pin(in2_pin, Pin.OUT)
    
    def avant(self, vitesse=SPEED_MAX):
        """
        Fait tourner le moteur vers l'avant
        
        Args:
            vitesse: Vitesse (0-65535), par défaut vitesse maximale
        """
        vitesse = min(vitesse, SPEED_MAX)
        vitesse = max(vitesse, 0)
        self.in1.on()
        self.in2.off()
        self.en.duty_u16(vitesse)
    
    def arriere(self, vitesse=SPEED_MAX):
        """
        Fait tourner le moteur vers l'arrière
        
        Args:
            vitesse: Vitesse (0-65535), par défaut vitesse maximale
        """
        vitesse = min(vitesse, SPEED_MAX)
        vitesse = max(vitesse, 0)
        self.in1.off()
        self.in2.on()
        self.en.duty_u16(vitesse)
    
    def arreter(self):
        """Arrête le moteur"""
        self.in1.off()
        self.in2.off()
        self.en.duty_u16(0)


# ============================================================================
# Classe pour le capteur à ultrasons
# ============================================================================

class CapteurUltrasons:
    """Classe pour lire la distance avec un capteur à ultrasons HC-SR04"""
    
    def __init__(self, trig_pin, echo_pin):
        """
        Initialise le capteur à ultrasons
        
        Args:
            trig_pin: PIN de déclenchement (TRIG)
            echo_pin: PIN de retour (ECHO)
        """
        self.trig = Pin(trig_pin, Pin.OUT)
        self.echo = Pin(echo_pin, Pin.IN)
    
    def mesurer_distance(self):
        """
        Mesure la distance avec le capteur à ultrasons
        
        Returns:
            Distance en centimètres (float)
        """
        # Envoyer une impulsion de 10µs sur TRIG
        self.trig.off()
        time.sleep_us(2)
        self.trig.on()
        time.sleep_us(10)
        self.trig.off()
        
        # Mesurer le temps de l'impulsion ECHO
        try:
            pulse_time = time_pulse_us(self.echo, 1, 30000)  # Timeout: 30ms
        except OSError:
            # En cas de timeout, retourner une grande valeur
            return 400.0
        
        # Calculer la distance (vitesse du son ≈ 343 m/s à 20°C)
        # Distance = (temps / 2) * vitesse = (pulse_time / 2) * 343 * 10^-6 * 100
        distance_cm = (pulse_time / 2) * 0.01715
        
        return distance_cm
    
    def obstacle_detecte(self, distance_min=SAFE_DISTANCE):
        """
        Détecte si un obstacle est présent
        
        Args:
            distance_min: Distance minimale en cm (par défaut SAFE_DISTANCE)
        
        Returns:
            True si un obstacle est détecté, False sinon
        """
        distance = self.mesurer_distance()
        return distance < distance_min


# ============================================================================
# Classe pour la base roulante
# ============================================================================

class BaseRoulante:
    """Classe pour contrôler la base roulante complète"""
    
    def __init__(self):
        """Initialise la base roulante"""
        # Initialiser les moteurs
        self.moteur_a = MoteurDC(ENA_PIN, IN1_PIN, IN2_PIN)
        self.moteur_b = MoteurDC(ENB_PIN, IN3_PIN, IN4_PIN)
        
        # Initialiser le capteur à ultrasons
        self.capteur = CapteurUltrasons(TRIG_PIN, ECHO_PIN)
        
        print("Base roulante initialisée ✓")
    
    def avancer(self, vitesse=SPEED_MAX):
        """
        Fait avancer la base roulante
        
        Args:
            vitesse: Vitesse (0-65535), par défaut vitesse maximale
        """
        self.moteur_a.avant(vitesse)
        self.moteur_b.avant(vitesse)
    
    def reculer(self, vitesse=SPEED_MAX):
        """
        Fait reculer la base roulante
        
        Args:
            vitesse: Vitesse (0-65535), par défaut vitesse maximale
        """
        self.moteur_a.arriere(vitesse)
        self.moteur_b.arriere(vitesse)
    
    def tourner_droite(self, vitesse=SPEED_MAX):
        """
        Fait tourner la base roulante à droite
        
        Args:
            vitesse: Vitesse (0-65535), par défaut vitesse maximale
        """
        self.moteur_a.avant(vitesse)
        self.moteur_b.arriere(vitesse)
    
    def tourner_gauche(self, vitesse=SPEED_MAX):
        """
        Fait tourner la base roulante à gauche
        
        Args:
            vitesse: Vitesse (0-65535), par défaut vitesse maximale
        """
        self.moteur_a.arriere(vitesse)
        self.moteur_b.avant(vitesse)
    
    def arreter(self):
        """Arrête la base roulante"""
        self.moteur_a.arreter()
        self.moteur_b.arreter()
    
    def obtenir_distance(self):
        """
        Obtient la distance mesurée par le capteur à ultrasons
        
        Returns:
            Distance en centimètres (float)
        """
        return self.capteur.mesurer_distance()
    
    def avancer_securise(self, vitesse=SPEED_MAX, distance_min=SAFE_DISTANCE):
        """
        Fait avancer la base roulante avec détection d'obstacle
        
        Args:
            vitesse: Vitesse (0-65535)
            distance_min: Distance minimale de sécurité en cm
        """
        distance = self.obtenir_distance()
        
        if distance > distance_min:
            self.avancer(vitesse)
            return True
        else:
            self.arreter()
            print(f"Obstacle détecté à {distance:.1f} cm!")
            return False
    
    def comportement_autonome(self, duree=None):
        """
        Fait se déplacer la base roulante de manière autonome en évitant les obstacles
        
        Args:
            duree: Durée du comportement autonome en secondes (None = infini)
        """
        debut = time.time()
        
        print("Démarrage du mode autonome...")
        
        try:
            while duree is None or (time.time() - debut) < duree:
                # Vérifier les obstacles
                distance = self.obtenir_distance()
                
                if distance > SAFE_DISTANCE:
                    # Pas d'obstacle, avancer
                    self.avancer(SPEED_MAX)
                    print(f"Distance: {distance:.1f} cm - Avance")
                else:
                    # Obstacle détecté, arrêter et tourner
                    print(f"Obstacle à {distance:.1f} cm - Demi-tour!")
                    self.arreter()
                    time.sleep(0.5)
                    
                    # Faire un demi-tour (tourner à gauche)
                    self.tourner_gauche(SPEED_MAX)
                    time.sleep(1)
                
                time.sleep(0.1)  # Délai pour la lecture capteur
        
        except KeyboardInterrupt:
            print("Mode autonome interrompu")
        
        finally:
            self.arreter()
            print("Base arrêtée")


# ============================================================================
# Fonction de démonstration
# ============================================================================

def demonstration():
    """Fonction de démonstration des mouvements"""
    
    base = BaseRoulante()
    
    # Afficher la distance initiale
    distance = base.obtenir_distance()
    print(f"Distance initiale: {distance:.1f} cm\n")
    
    # Séquence de test
    print("=== Test des mouvements ===\n")
    
    # Avancer
    print("Avance pendant 2 secondes...")
    base.avancer(SPEED_MAX)
    time.sleep(2)
    base.arreter()
    print("Arrêt\n")
    
    # Reculer
    print("Recule pendant 2 secondes...")
    base.reculer(SPEED_MAX)
    time.sleep(2)
    base.arreter()
    print("Arrêt\n")
    
    # Tourner gauche
    print("Tourne à gauche pendant 1 seconde...")
    base.tourner_gauche(SPEED_MAX)
    time.sleep(1)
    base.arreter()
    print("Arrêt\n")
    
    # Tourner droite
    print("Tourne à droite pendant 1 seconde...")
    base.tourner_droite(SPEED_MAX)
    time.sleep(1)
    base.arreter()
    print("Arrêt\n")
    
    # Test avec vitesse réduite
    print("Avance lentement pendant 3 secondes (50% vitesse)...")
    base.avancer(SPEED_MIN)
    time.sleep(3)
    base.arreter()
    print("Arrêt\n")
    
    print("Démonstration terminée!")


# ============================================================================
# Point d'entrée principal
# ============================================================================

if __name__ == "__main__":
    try:
        # Créer l'instance de la base roulante
        base = BaseRoulante()
        
        # Décommenter la fonction à exécuter:
        
        # Démonstration simple
        # demonstration()
        
        # Mode autonome (infini)
        # base.comportement_autonome()
        
        # Mode autonome (60 secondes)
        # base.comportement_autonome(duree=60)
        
        # Pour tester manuellement, vous pouvez faire:
        # base.avancer()
        # time.sleep(2)
        # base.arreter()
        
        print("Base roulante prête! Utilisez les méthodes de la classe BaseRoulante")
        print("Exemples:")
        print("  base.avancer()")
        print("  base.reculer()")
        print("  base.tourner_gauche()")
        print("  base.tourner_droite()")
        print("  base.arreter()")
        print("  base.obtenir_distance()")
        print("  base.comportement_autonome(duree=60)")
    
    except Exception as e:
        print(f"Erreur: {e}")