# WWW-POSIM station starter · IOES-Lab · MIT
import math
from www_posim_ros import Session


def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


with Session.local(control=True) as sim:
    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]
    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:
        message = sim.latest(topic, max_age=0.5)
        pose = message["pose"]["pose"]
        p, q = pose["position"], pose["orientation"]
        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)
        if distance < 0.25:
            holding = True
        elif distance > 0.6:
            holding = False
        wanted_yaw = goal["yaw"]
        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)
        forward = min(0.4, 0.15 * distance) if not holding and abs(error) < 0.6 else 0.0
        turn = max(-0.5, min(0.5, 0.6 * error))
        sim.set_velocity(forward=forward, yaw_rate=turn, duration=0.25)