import json
import math

#Identifie l'émotion grace à une combinaison de muscles faciaux
def trouver_emotion(donnees):
    if donnees.get("mouth_smile", 0) > 0.15:
        return "Joie"
    
    elif (donnees.get("brow_outer_up", 0) > 0.08 and
          donnees.get("eye_wide", 0) > 0.08 and
          donnees.get("mouth_open", 0) > 0.08):
        return "Surprise"
    
    elif (donnees.get("brow_outer_up", 0) > 0.05 and
          donnees.get("eye_wide", 0) > 0.05 and
          donnees.get("mouth_open", 0) > 0.05):
        return "Peur"

    elif donnees.get("brow_inner_up", 0) > 0.15:
        return "Tristesse"

    elif (donnees.get("brow_furrow", 0) > 0.15 and
          donnees.get("mouth_press", 0) > 0.1):
        return "Colere"

    return "Neutre"

# Génère les trajectoires des antennes du robot pour exprimer une émotion.
def calculer_cibles_antennes(emotion, temps_ecoule):
    if emotion == "Joie":
        bouge = math.radians(30 * math.sin(2 * math.pi * 0.5 * temps_ecoule))
        return 0.0, bouge, 0.0, bouge
    elif emotion == "Colere":
        tremble = math.radians(6 * math.sin(2 * math.pi * 8 * temps_ecoule))
        return math.radians(20), math.radians(-70) + tremble, math.radians(20), math.radians(70) - tremble
    elif emotion == "Peur":
        tremble = math.radians(6 * math.sin(2 * math.pi * 10 * temps_ecoule))
        return 0.0, math.radians(-160) + tremble, 0.0, math.radians(160) - tremble
    elif emotion == "Surprise":
        x = math.radians(15 * math.sin(2 * math.pi * 3 * temps_ecoule))
        y = math.radians(10 * math.cos(2 * math.pi * 3 * temps_ecoule))
        return 0.0, x + y, 0.0, -(x - y)
    elif emotion == "Tristesse":
        ratio = min(temps_ecoule / 2.0, 1.0) 
        return 0.0, math.radians(-160 * ratio), 0.0, math.radians(160 * ratio)

    return 0.0, 0.0, 0.0, 0.0

# Fonction de danse d'attente
def calculer_danse_antennes(temps_ecoule):
    vague_x = math.radians(45 * math.sin(2 * math.pi * 2.5 * temps_ecoule))
    vague_y = math.radians(45 * math.cos(2 * math.pi * 2.5 * temps_ecoule))
    return vague_x, vague_y, vague_y, vague_x


def adoucir_mouvement(position_actuelle, position_cible, vitesse):
    return position_actuelle + vitesse * (position_cible - position_actuelle)

# Converti un JSON de signaux en JSON de mouvements.
def generer_mouvement(input_json, output_json):
    with open(input_json, 'r', encoding='utf-8') as f:
        donnees_video = json.load(f)["face_data"]

    frames_robot = []
    emotion_validee = "Neutre"
    emotion_en_cours = "Neutre"
    confirmations = 0
    chronometre_emotion = 0.0

    if not donnees_video:
        return

    image_depart = donnees_video[0]
    tete_p, tete_y, tete_r = image_depart.get("pitch", 0.0), image_depart.get("yaw", 0.0), image_depart.get("roll", 0.0)
    ant_g_p, ant_g_r = 0.0, 0.0
    ant_d_p, ant_d_r = 0.0, 0.0

    for frame in donnees_video:
        if not frame["face_detected"]:
            continue
            
        temps_actuel = frame["timestamp"]
        emotion_detectee = trouver_emotion(frame)
        
        if emotion_detectee == emotion_en_cours:
            confirmations += 1
        else:
            emotion_en_cours = emotion_detectee
            confirmations = 1

        # Validation du changement d'état émotionnel
        if confirmations >= 5 and emotion_en_cours != emotion_validee:
            emotion_validee = emotion_en_cours
            chronometre_emotion = temps_actuel 
            
        temps_animation = temps_actuel - chronometre_emotion

        cible_tete_p, cible_tete_y, cible_tete_r = frame["pitch"], frame["yaw"], frame["roll"]
        cible_ag_p, cible_ag_r, cible_ad_p, cible_ad_r = calculer_cibles_antennes(emotion_validee, temps_animation)

        tete_p = adoucir_mouvement(tete_p, cible_tete_p, vitesse=0.6)
        tete_y = adoucir_mouvement(tete_y, cible_tete_y, vitesse=0.6)
        tete_r = adoucir_mouvement(tete_r, cible_tete_r, vitesse=0.6)

        ant_g_p = adoucir_mouvement(ant_g_p, cible_ag_p, vitesse=0.5)
        ant_g_r = adoucir_mouvement(ant_g_r, cible_ag_r, vitesse=0.5)
        ant_d_p = adoucir_mouvement(ant_d_p, cible_ad_p, vitesse=0.5)
        ant_d_r = adoucir_mouvement(ant_d_r, cible_ad_r, vitesse=0.5)

        frames_robot.append({
            "timestamp": temps_actuel,
            "head": {"pitch": tete_p, "yaw": tete_y, "roll": tete_r},
            "body_yaw": 0.0, 
            "antennas": {
                "left": {"pitch": ant_g_p, "roll": ant_g_r},
                "right": {"pitch": ant_d_p, "roll": ant_d_r}
            }
        })

    with open(output_json, 'w', encoding='utf-8') as f:
        json.dump({"mouvements": frames_robot}, f, indent=4)

if __name__ == "__main__":
    generer_mouvement("../data/signaux_extraits.json", "../data/mouvement_reachy.json")