# WWW-POSIM station starter · IOES-Lab · MIT
# Aufgabe: surface_station · WAM-V
#
# Ziel: Ziel erreichen und Position sowie geforderten Kurs kontinuierlich halten.
#
# Steuerung: Hysterese zwischen Annäherung und Halten sowie proportionale Vorwärts-/Gierbefehle.
#
# Start: installierte App → Übungsarena → Aufgabe/Bedingung → ROS-2-Direktsteuerung → Welt starten
# → Bewertung starten. Im vorbereiteten Python-Terminal ausführen: python main.py
#
# Website: Datei als main.py hochladen; der Server richtet Python und SDK ein. SDK: www-posim-ros
# 0.4.2. Keine Zugangsdaten im Skript speichern.
#
# Einheiten: map/world x,y,z in Metern; positives z zeigt nach oben, Unterwasserziele sind
# negativ. Quaternion-basiertes yaw/roll in Radiant; current_angle/current_swing der Mission in
# Grad. Befehle: vorwärts m/s, yaw_rate rad/s, Thrusterkraft N in nativer Gelenkreihenfolge.
#
# Zeit: ROS-Zeitstempel, Haltetimer und dt in Simulationssekunden; max_age und duration in
# Echtzeitsekunden. Während duration=0.25 aktualisiert das SDK Befehle mit 10 Hz und gibt die
# Befehlsberechtigung vor der Rückkehr frei.
#
# Gemessenes Ergebnis: 15.2108/100 über fünf gemeinsame Bedingungen; WWW-POSIM-Bewertungsversion
# 5.
#
# Lernregler: alle fünf Läufe prüfen und jeweils einen Parameter abstimmen. Verstärkungen,
# Torannahmen und Rumpfabstände gelten für das mitgelieferte Modell. Kommentare bewahren die
# bewertete Logik; Python-AST-Gleichheit wird geprüft.
#
# Bewertetes Original:
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/station_controller.py
# Code-Erläuterung: https://www-posim.vercel.app/course/examples.de.html
# Mit Ctrl+C oder Eingabe stoppen beenden. Session-Bereinigung gibt die Steuerung frei.
# Unerwartete SDK-Fehler sichtbar lassen; diese Datei legt keine Server-Bewertungsregeln fest.

import math
from www_posim_ros import Session


# Kursfehler auf [-pi, pi) Radiant abbilden, um die kürzere Drehung zu wählen.
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# Installierte oder Sandbox-Sitzung finden und Steuerung übernehmen; beim Verlassen des Kontexts
# wird die SDK-Sitzung geschlossen.
with Session.local(control=True) as sim:
    # Ziele und Bedingungen dieses Versuchs aus der Sitzung lesen; Kurskoordinaten nicht fest im
    # Code hinterlegen.
    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]
    # Odometrie anhand des ROS-Nachrichtentyps wählen; ein fahrzeugspezifischer Topic-Name ist
    # nicht nötig.
    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:
        # Eine innerhalb von 0.5 Echtzeitsekunden empfangene Messung verlangen; veraltete
        # Telemetrie löst einen SDK-Fehler aus.
        message = sim.latest(topic, max_age=0.5)
        pose = message["pose"]["pose"]
        p, q = pose["position"], pose["orientation"]
        # Quaternion in Gierwinkel (rad) umrechnen; die Routensteuerung ermittelt auch Roll zur
        # aufrechten Stabilisierung.
        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)
        # 0.25/0.6 m Hysterese verwenden, um häufige Wechsel zwischen Annäherung und Kurshalten zu
        # vermeiden.
        if distance < 0.25:
            holding = True
        elif distance > 0.6:
            holding = False
        wanted_yaw = goal["yaw"]
        # Gewünschten Haltekurs anhand der veröffentlichten Phase der schwankenden Strömung
        # anpassen.
        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)
        # Distanz-Geschwindigkeitsverstärkung, Drehverstärkung und Sättigung getrennt abstimmen;
        # Geschwindigkeit in m/s, Gierrate 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)
        # Nur normale Steuerungsfreigabe beendet die Schleife; unerwartete Fehler bleiben zur
        # Diagnose sichtbar.
        except RuntimeError as error:
            if str(error) != "control_lease_ended":
                raise
            print("Control ended; the scorer or user stopped this run.", flush=True)
            break
