# WWW-POSIM station starter · IOES-Lab · MIT
# Task: surface_station · WAM-V
#
# Goal: Reach the target and continuously hold its position and required heading.
#
# Control: Approach/hold hysteresis and proportional forward/yaw commands.
#
# Start: installed app → Practice arena → this task/condition → ROS 2 direct control → Start world
# → Start scoring. In the prepared Python terminal, run: python main.py
#
# Website: upload this file as main.py; the server prepares Python and the SDK. SDK: www-posim-ros
# 0.4.2. Keep credentials out of the script.
#
# Units: map/world x,y,z in metres; positive z is up, so underwater targets are negative.
# Quaternion-derived yaw/roll are radians. Mission current_angle/current_swing are degrees.
# Commands use forward m/s, yaw_rate rad/s and thruster force N in native joint order.
#
# Timing: ROS stamps, hold timers and dt use simulation seconds. max_age and duration use
# wall-clock seconds. During duration=0.25 the SDK refreshes commands at 10 Hz, then releases the
# command lease before returning.
#
# Measured result: 15.2108/100 over five shared conditions; WWW-POSIM scoring revision 5.
#
# Learning controller: inspect all five runs and tune one parameter at a time. Gains, gate
# assumptions and hull margins belong to the bundled model. Comments preserve the scored logic;
# Python AST equality is checked.
#
# Scored original:
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/station_controller.py
# Code walkthrough: https://www-posim.vercel.app/course/examples.html
# Stop with Ctrl+C or Stop input. Session cleanup releases control. Keep unexpected SDK errors
# visible; this file does not set the server scoring rules.

import math
from www_posim_ros import Session


# Wrap a heading error into [-pi, pi) radians to choose the shorter turn.
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# Discover the installed/sandbox session and acquire control; context exit closes the SDK session.
with Session.local(control=True) as sim:
    # Read this trial's targets and conditions from the session; do not hard-code course
    # coordinates.
    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]
    # Select odometry by ROS message type so a vehicle-specific topic name is unnecessary.
    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:
        # Require a sample received within 0.5 wall-clock seconds; stale telemetry raises an SDK
        # error.
        message = sim.latest(topic, max_age=0.5)
        pose = message["pose"]["pose"]
        p, q = pose["position"], pose["orientation"]
        # Convert the quaternion to yaw (rad); route control also extracts roll for upright
        # stabilization.
        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)
        # Use 0.25/0.6 m hysteresis to avoid rapidly switching approach and heading-hold modes.
        if distance < 0.25:
            holding = True
        elif distance > 0.6:
            holding = False
        wanted_yaw = goal["yaw"]
        # Adjust the requested hold heading with the published oscillating-current phase.
        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)
        # Tune the distance-to-speed gain, turn gain and saturation separately; velocity is m/s
        # and yaw rate is 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)
        # Only normal control release ends the loop; unexpected errors remain visible for
        # diagnosis.
        except RuntimeError as error:
            if str(error) != "control_lease_ended":
                raise
            print("Control ended; the scorer or user stopped this run.", flush=True)
            break
