# WWW-POSIM station starter · IOES-Lab · MIT
# 任务：surface_station · WAM-V
#
# 目标：到达目标并连续保持其位置和指定航向。
#
# 控制：使用接近/保持滞回以及前进和偏航比例命令。
#
# 启动：安装版应用 → 练习场 → 本任务/条件 → ROS 2 直接控制 → 启动世界 → 开始评分。在准备好的 Python 终端运行：python main.py
#
# 网站：将此文件作为 main.py 上传；服务器准备 Python 和 SDK。SDK：www-posim-ros 0.4.2。不要在脚本中包含凭据。
#
# 单位：map/world x,y,z 为米；z 正方向向上，水下目标为负。四元数得到的 yaw/roll 为弧度；任务 current_angle/current_swing 为度。命令：前进
# m/s、yaw_rate rad/s，推进力按原生关节顺序以 N 表示。
#
# 时间：ROS 时间戳、保持计时器和 dt 使用仿真秒；max_age 和 duration 使用实际秒。在 duration=0.25 内 SDK 以 10 Hz
# 刷新命令，返回前释放命令租约。
#
# 实测结果：五个共同条件下 15.2108/100；WWW-POSIM 评分规则第 5 版。
#
# 学习用控制器：检查五次记录，每次调整一个参数。增益、门的假设和船体余量针对附带模型。注释保留已评分逻辑，并检查 Python AST 一致性。
#
# 评分原始文件：https://www-posim.vercel.app/course/examples/maritime-class-5/measured/station_controller.py
# 代码说明：https://www-posim.vercel.app/course/examples.zh.html
# 使用 Ctrl+C 或停止输入结束。Session 清理会释放控制权。保留意外 SDK 错误；此文件不设置服务器评分规则。

import math
from www_posim_ros import Session


# 将航向误差归一化到 [-pi, pi) 弧度，选择较短的转向。
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# 发现安装版或沙箱会话并获取控制权；退出上下文时关闭 SDK 会话。
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]
    # 按 ROS 消息类型选择里程计，无需指定车辆专用话题名。
    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:
        # 要求使用最近 0.5 秒实际时间内收到的样本；过期遥测会触发 SDK 错误。
        message = sim.latest(topic, max_age=0.5)
        pose = message["pose"]["pose"]
        p, q = pose["position"], pose["orientation"]
        # 将四元数转换为 yaw（rad）；路线控制还提取 roll 以保持直立姿态。
        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 滞回，避免频繁切换接近与航向保持模式。
        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)
        # 分别调整距离到速度增益、转向增益和饱和值；速度单位为 m/s，偏航角速度为 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)
        # 只有正常释放控制权才结束循环；意外错误保留显示以便诊断。
        except RuntimeError as error:
            if str(error) != "control_lease_ended":
                raise
            print("Control ended; the scorer or user stopped this run.", flush=True)
            break
