# WWW-POSIM station starter · IOES-Lab · MIT
# Prova: surface_station · WAM-V
#
# Obiettivo: Raggiungere l'obiettivo e mantenerne continuamente posizione e rotta richiesta.
#
# Controllo: Isteresi avvicinamento/mantenimento e comandi proporzionali di avanzamento e
# imbardata.
#
# Avvio: app installata → Arena di pratica → prova/condizione → Controllo diretto ROS 2 → Avvia
# mondo → Avvia valutazione. Nel terminale Python preparato: python main.py
#
# Sito web: caricare il file come main.py; il server prepara Python e SDK. SDK: www-posim-ros
# 0.4.2. Non inserire credenziali nello script.
#
# Unità: map/world x,y,z in metri; z positivo verso l'alto, obiettivi subacquei negativi. Yaw/roll
# dal quaternione in radianti; current_angle/current_swing della missione in gradi. Comandi:
# avanzamento m/s, yaw_rate rad/s e forza N nell'ordine nativo dei giunti.
#
# Tempi: timestamp ROS, timer di mantenimento e dt in secondi simulati; max_age e duration in
# secondi reali. Durante duration=0.25 l'SDK aggiorna a 10 Hz e rilascia il lease del comando
# prima di ritornare.
#
# Risultato misurato: 15.2108/100 su cinque condizioni comuni; regole WWW-POSIM, revisione 5.
#
# Controllore didattico: esaminare le cinque prove e regolare un parametro alla volta. Guadagni,
# ipotesi sui varchi e margini appartengono al modello incluso. I commenti preservano la logica
# valutata; si verifica l'uguaglianza degli AST Python.
#
# Originale valutato:
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/station_controller.py
# Spiegazione del codice: https://www-posim.vercel.app/course/examples.it.html
# Arrestare con Ctrl+C o Ferma input. La pulizia di Session rilascia il controllo. Lasciare
# visibili gli errori SDK inattesi; il file non imposta le regole di punteggio del server.

import math
from www_posim_ros import Session


# Riportare l'errore di rotta in [-pi, pi) radianti per scegliere la rotazione più breve.
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# Individuare la sessione installata o isolata e acquisire il controllo; l'uscita dal contesto
# chiude la sessione SDK.
with Session.local(control=True) as sim:
    # Leggere obiettivi e condizioni della prova dalla sessione; non fissare le coordinate del
    # percorso nel codice.
    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]
    # Selezionare l'odometria per tipo di messaggio ROS, senza dipendere dal nome del topic del
    # veicolo.
    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:
        # Richiedere un campione ricevuto negli ultimi 0.5 secondi reali; la telemetria obsoleta
        # genera un errore SDK.
        message = sim.latest(topic, max_age=0.5)
        pose = message["pose"]["pose"]
        p, q = pose["position"], pose["orientation"]
        # Convertire il quaternione in yaw (rad); il controllo del percorso estrae anche roll per
        # stabilizzare l'assetto.
        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)
        # Usare un'isteresi di 0.25/0.6 m per evitare rapidi cambi tra avvicinamento e
        # mantenimento della rotta.
        if distance < 0.25:
            holding = True
        elif distance > 0.6:
            holding = False
        wanted_yaw = goal["yaw"]
        # Regolare la rotta di mantenimento con la fase pubblicata della corrente oscillante.
        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)
        # Regolare separatamente guadagno distanza-velocità, guadagno di rotazione e saturazione;
        # velocità in m/s e imbardata in 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)
        # Solo il rilascio normale del controllo termina il ciclo; gli errori inattesi restano
        # visibili per la diagnosi.
        except RuntimeError as error:
            if str(error) != "control_lease_ended":
                raise
            print("Control ended; the scorer or user stopped this run.", flush=True)
            break
