#!/usr/bin/python3

import MR25       # Bibliothèque de contrôle du robot MR25
import time       # Pour les pauses dans la boucle principale

# ============================================================
# PERCEPTRON — Neurone artificiel à seuil
# ============================================================
# Un perceptron calcule une somme pondérée de ses entrées,
# puis applique une fonction d'activation pour décider : 0 ou 1.

def activation(x):
    """Fonction d'activation en échelon (step function).
    Retourne 1 si x >= 0 (voie libre), 0 sinon (obstacle)."""
    return 1 if x >= 0 else 0

# 5 poids — un par capteur de proximité (c1 à c5)
w = [0.0, 0.0, 0.0, 0.0, 0.0]
b = 0.0    # Biais : ajuste le seuil de décision
eta = 0.1  # Taux d'apprentissage (learning rate) : amplitude des corrections

# ============================================================
# DONNÉES D'ENTRAÎNEMENT
# ============================================================
# Chaque exemple est un couple ([c1,c2,c3,c4,c5], étiquette)
# Les valeurs des capteurs sont normalisées entre 0.0 et 1.0 :
#   - Proche de 0.0 → obstacle détecté (capteur saturé)
#   - Proche de 1.0 → voie dégagée (capteur à portée max)
# Étiquette : 0 = obstacle → s'arrêter/tourner, 1 = avancer

training_data = [
    # --- Cas OBSTACLE : capteurs faibles (obstacles proches) ---
    ([0.1, 0.1, 0.1, 0.1, 0.1], 0),  # Obstacles partout
    ([0.2, 0.2, 0.2, 0.2, 0.2], 0),  # Obstacles proches
    ([0.3, 0.2, 0.1, 0.2, 0.3], 0),  # Obstacle central fort
    ([0.2, 0.2, 0.2, 0.8, 0.8], 0),  # Obstacles proches
    ([0.8, 0.8, 0.2, 0.2, 0.2], 0),  # Obstacles proches

    # --- Cas AVANCER : capteurs forts (voie libre) ---
    ([0.8, 0.8, 0.8, 0.8, 0.8], 1),  # Voie totalement libre
    ([0.9, 0.8, 0.9, 0.8, 0.9], 1),  # Voie très dégagée
    ([0.7, 0.9, 0.8, 0.9, 0.7], 1),  # Voie dégagée (légère variation)
]

# ============================================================
# PHASE D'APPRENTISSAGE — Règle de Hebb / Perceptron
# ============================================================
# Pour chaque exemple, on calcule la sortie et on corrige les
# poids proportionnellement à l'erreur : Δw = η × erreur × entrée

for epoch in range(100):  # Maximum 100 passes sur les données

    erreur_totale = 0

    for inputs, target in training_data:

        # Calcul de la somme pondérée : s = Σ(wi × xi) + b
        s = sum(wi * xi for wi, xi in zip(w, inputs)) + b

        # Application de la fonction d'activation
        y = activation(s)

        # Erreur = différence entre valeur attendue et valeur calculée
        erreur = target - y  # 0, +1 ou -1

        # Mise à jour des poids : correction proportionnelle à l'erreur
        for i in range(5):
            w[i] += eta * erreur * inputs[i]

        # Mise à jour du biais (entrée fictive toujours égale à 1)
        b += eta * erreur

        erreur_totale += abs(erreur)

    # Arrêt anticipé si le perceptron classe tout correctement
    if erreur_totale == 0:
        break

# Affichage des paramètres appris
print("Poids :", w)
print("Biais :", b)

# ============================================================
# PILOTAGE EN TEMPS RÉEL — Boucle principale
# ============================================================

SEUIL_MAX = 250.0  # Distance maximale de détection (en cm ou unité capteur)
                  # Au-delà, on considère la voie comme libre

try:
  while True:
  
      # --- Lecture et normalisation des 5 capteurs de proximité ---
      # proxSensor(n) retourne une valeur brute ; on la plafonne à SEUIL_MAX,
      # puis on divise pour obtenir un ratio entre 0.0 et 1.0
      p1 = MR25.proxSensor(1)
      p2 = MR25.proxSensor(2)
      p3 = MR25.proxSensor(3)
      p4 = MR25.proxSensor(4)
      p5 = MR25.proxSensor(5)

      capteurs = [p1, p2, p3, p4, p5]
      print(capteurs)

      c1 = min(p1, SEUIL_MAX) / SEUIL_MAX  # Capteur avant-gauche extrême
      c2 = min(p2, SEUIL_MAX) / SEUIL_MAX  # Capteur avant-gauche
      c3 = min(p3, SEUIL_MAX) / SEUIL_MAX  # Capteur avant-centre
      c4 = min(p4, SEUIL_MAX) / SEUIL_MAX  # Capteur avant-droite
      c5 = min(p5, SEUIL_MAX) / SEUIL_MAX  # Capteur avant-droite extrême
  
      entree = [c1, c2, c3, c4, c5]
      print(entree)
  
      # --- Inférence : le perceptron décide d'avancer ou non ---
      s = sum(wi * xi for wi, xi in zip(w, entree)) + b
      print("s = ", s)
      avance = activation(s)  # 1 = avancer, 0 = éviter l'obstacle
  
      print("avance = ", avance)

      if avance:
          # Voie libre → avancer à vitesse modérée
          MR25.forward(25)
          print("AVANCE")
  
      else:
          # Obstacle détecté → choisir le côté le plus dégagé pour tourner
  
          gauche = c1 + c2   # Score d'espace à gauche (somme des 2 capteurs gauches)
          droite = c4 + c5   # Score d'espace à droite (somme des 2 capteurs droits)
  
          # Le côté avec le score le plus élevé est le plus dégagé
          if gauche > droite:
              MR25.turnLeft(40)   # Plus d'espace à gauche → tourner à gauche
              print("TOURNE GAUCHE")
          else:
              MR25.turnRight(40)  # Plus d'espace à droite (ou égalité) → tourner à droite
              print("TOURNE DROITE")
  
      time.sleep(0.2)  # Pause 

except KeyboardInterrupt:
    # Arrêt propre du robot lors d'une interruption clavier (Ctrl+C)
    MR25.stop()
# en of file