# Macé Robotics
#
#!/usr/bin/python3
import MR25
import time

# -----------------------------
# Fonctions du perceptron
# -----------------------------

def activation(x):
    """
    Fonction d'activation de type seuil (step function).
    Retourne 1 si x >= 0 (avancer), sinon 0 (stopper).
    """
    if x >= 0:
        return 1
    return 0

# -----------------------------
# Initialisation des poids et biais
# -----------------------------
# w1 : poids associé au capteur 2 (x1)
# w2 : poids associé au capteur 4 (x2)
# b  : biais du perceptron (décalage du seuil de décision)
w1 = 0.0
w2 = 0.0
b = 0.0

# Taux d'apprentissage : contrôle la vitesse de correction des poids
# Une valeur trop grande peut rendre l'apprentissage instable
eta = 0.1

# -----------------------------
# Jeu d'apprentissage (données d'entraînement)
# -----------------------------
#
# x1 = capteur de proximité 2 (distance normalisée entre 0 et 1)
# x2 = capteur de proximité 4 (distance normalisée entre 0 et 1)
#
# Normalisation : 0 = obstacle très proche, 1 = espace libre
#
# Sortie attendue :
#   0 = STOP  (obstacle détecté, distances faibles)
#   1 = AVANCE (voie libre, distances élevées)
training_data = [
    ([0.1, 0.1], 0),  # Obstacle très proche sur les deux capteurs → STOP
    ([0.2, 0.3], 0),  # Obstacle proche → STOP
    ([0.3, 0.2], 0),  # Obstacle proche → STOP
    ([0.8, 0.8], 1),  # Espace libre sur les deux capteurs → AVANCE
    ([0.7, 0.9], 1),  # Espace majoritairement libre → AVANCE
    ([0.9, 0.7], 1),  # Espace majoritairement libre → AVANCE
]

# -----------------------------
# Phase d'apprentissage (entraînement du perceptron)
# -----------------------------
# On répète jusqu'à 100 époques (passages complets sur le jeu d'entraînement)
# L'apprentissage s'arrête prématurément si tous les exemples sont bien classés
for epoch in range(100):
    erreur_totale = 0  # Compteur d'erreurs pour cette époque

    for inputs, target in training_data:
        x1, x2 = inputs

        # Calcul de la sortie du perceptron : combinaison linéaire + activation
        y = activation(w1*x1 + w2*x2 + b)
        print("Y = ", y)

        # Calcul de l'erreur : différence entre la sortie attendue et la sortie calculée
        erreur = target - y

        # Mise à jour des poids selon la règle d'apprentissage du perceptron
        # Si erreur = 0 : pas de modification
        # Si erreur = +1 : poids augmentés (le perceptron devait dire 1)
        # Si erreur = -1 : poids diminués (le perceptron devait dire 0)
        w1 += eta * erreur * x1
        w2 += eta * erreur * x2
        b  += eta * erreur  # Le biais est mis à jour sans facteur d'entrée
        print("w1, w2 = ", w1, w2)

        # Accumulation de l'erreur absolue sur cette époque
        erreur_totale += abs(erreur)
        print("Erreur totale = ", erreur_totale)

    # Convergence atteinte : tous les exemples sont correctement classés
    if erreur_totale == 0:
        break

# Affichage des poids finaux après l'apprentissage
print("Apprentissage terminé")
print("w1 =", w1)
print("w2 =", w2)
print("b =", b)

# Pause avant de démarrer le pilotage (laisse le temps de lire les résultats)
time.sleep(5)

# -----------------------------
# Pilotage du robot MR25 en temps réel
# -----------------------------

# Distance maximale prise en compte (en mm)
# Au-delà de SEUIL_MAX, la distance est considérée comme maximale (voie libre)
SEUIL_MAX = 80.0  # mm

try:
    while True:
        # --- Lecture des capteurs de proximité ---
        # proxSensor retourne une distance en mm
        x1 = MR25.proxSensor(2)  # Capteur 2
        x2 = MR25.proxSensor(4)  # Capteur 4

        # --- Normalisation des distances dans l'intervalle [0, 1] ---
        # min(x, SEUIL_MAX) écrête les valeurs supérieures au seuil
        # La division ramène la valeur entre 0 et 1
        x1 = min(x1, SEUIL_MAX) / SEUIL_MAX
        x2 = min(x2, SEUIL_MAX) / SEUIL_MAX

        # --- Inférence : décision du perceptron ---
        # Utilise les poids appris pour décider d'avancer ou de s'arrêter
        sortie = activation(w1*x1 + w2*x2 + b)

        # --- Exécution de la commande moteur ---
        if sortie == 1:
            MR25.forward(40)  # Avance à 40% de vitesse
            print("AVANCE")
        else:
            MR25.stop()       # Arrêt immédiat
            print("STOP")

        # Fréquence de rafraîchissement : 10 Hz (une décision toutes les 100 ms)
        time.sleep(0.1)

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

# end of file

vitesse = int(sortie * 50)
MR25.forward(vitesse)