# WWW-POSIM station starter · IOES-Lab · MIT
# Tâche : surface_station · WAM-V
#
# Objectif : Atteindre la cible et maintenir en continu sa position et le cap demandé.
#
# Contrôle : Hystérésis approche/maintien et commandes proportionnelles d'avance et de lacet.
#
# Démarrage : application installée → Terrain d'entraînement → tâche/condition → Contrôle direct
# ROS 2 → Démarrer le monde → Démarrer l'évaluation. Dans le terminal Python préparé : python
# main.py
#
# Site web : envoyer ce fichier sous main.py ; le serveur prépare Python et le SDK. SDK :
# www-posim-ros 0.4.2. Ne pas inclure d'identifiants dans le script.
#
# Unités : map/world x,y,z en mètres ; z positif vers le haut, cibles immergées négatives.
# Yaw/roll issus du quaternion en radians ; current_angle/current_swing de mission en degrés.
# Commandes : avance m/s, yaw_rate rad/s et force N dans l'ordre natif des articulations.
#
# Temps : horodatages ROS, maintien et dt en secondes simulées ; max_age et duration en secondes
# réelles. Pendant duration=0.25, le SDK actualise à 10 Hz puis libère le bail de commande avant
# le retour.
#
# Résultat mesuré : 15.2108/100 sur cinq conditions communes ; règles WWW-POSIM, révision 5.
#
# Contrôleur pédagogique : examiner les cinq essais et régler un paramètre à la fois. Gains,
# hypothèses de portes et marges sont propres au modèle fourni. Les commentaires conservent la
# logique évaluée ; l'égalité des AST Python est vérifiée.
#
# Original évalué :
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/station_controller.py
# Explication du code : https://www-posim.vercel.app/course/examples.fr.html
# Arrêter avec Ctrl+C ou Arrêter l'entrée. Le nettoyage de Session libère le contrôle. Garder
# visibles les erreurs SDK inattendues ; ce fichier ne définit pas les règles de score du serveur.

import math
from www_posim_ros import Session


# Ramener l'erreur de cap dans [-pi, pi) radians pour choisir la rotation la plus courte.
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# Découvrir la session installée ou isolée et obtenir le contrôle ; la sortie du contexte ferme la
# session SDK.
with Session.local(control=True) as sim:
    # Lire les cibles et conditions de cet essai dans la session ; ne pas coder les coordonnées du
    # parcours en dur.
    mission = sim.profile.get("mission")
    if not mission or mission["task"] != "surface_station":
        raise ValueError("Choose the surface_station competition task")
    goal = mission["targets"][0]
    # Choisir l'odométrie par type de message ROS, sans dépendre d'un nom de topic propre au
    # véhicule.
    topic = next(name for name, kind in sim.topics.items()
                 if kind == "nav_msgs/msg/Odometry")
    print("Moving toward the station target:", goal, flush=True)

    holding = False
    while True:
        # Exiger un échantillon reçu depuis moins de 0.5 seconde réelle ; une télémétrie périmée
        # déclenche une erreur SDK.
        message = sim.latest(topic, max_age=0.5)
        pose = message["pose"]["pose"]
        p, q = pose["position"], pose["orientation"]
        # Convertir le quaternion en yaw (rad) ; le contrôle de route extrait aussi roll pour
        # stabiliser l'assiette.
        yaw = math.atan2(2 * (q["w"] * q["z"] + q["x"] * q["y"]),
                         1 - 2 * (q["y"] ** 2 + q["z"] ** 2))
        dx, dy = goal["x"] - p["x"], goal["y"] - p["y"]
        distance = math.hypot(dx, dy)
        # Utiliser une hystérésis de 0.25/0.6 m pour éviter les basculements rapides entre
        # approche et maintien du cap.
        if distance < 0.25:
            holding = True
        elif distance > 0.6:
            holding = False
        wanted_yaw = goal["yaw"]
        # Ajuster le cap à maintenir selon la phase publiée du courant oscillant.
        if goal.get("yaw_current_follow"):
            stamp = message["header"]["stamp"]
            elapsed = stamp["sec"] + stamp["nanosec"] * 1e-9 - mission["current_epoch_sim_time"]
            wanted_yaw += math.radians(mission["current_swing"]) * math.sin(
                2 * math.pi * elapsed / mission["current_period"] + mission["current_phase"])
        desired = wanted_yaw if holding else math.atan2(dy, dx)
        error = angle(desired - yaw)
        # Régler séparément le gain distance-vitesse, le gain de rotation et la saturation ;
        # vitesse en m/s et lacet en rad/s.
        forward = min(0.4, 0.14 * distance) if not holding and abs(error) < 0.6 else 0.0
        turn = max(-0.5, min(0.5, 0.6 * error))
        try:
            sim.set_velocity(forward=forward, yaw_rate=turn, duration=0.25)
        # Seule la libération normale du contrôle termine la boucle ; les erreurs inattendues
        # restent visibles pour le diagnostic.
        except RuntimeError as error:
            if str(error) != "control_lease_ended":
                raise
            print("Control ended; the scorer or user stopped this run.", flush=True)
            break
