# WWW-POSIM station starter · IOES-Lab · MIT
# 課題: surface_station · WAM-V
#
# 目標: 目標に到達し、指定位置と方位を連続して保持します。
#
# 制御: 接近・保持のヒステリシスと、前進・yaw の比例制御を使います。
#
# 実行: インストール版 → 練習アリーナ → この課題・条件 → 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 はラジアン、mission の
# current_angle/current_swing は度です。命令は前進 m/s、yaw_rate rad/s、基本ジョイント順の推進力 N です。
#
# 時間: ROS 時刻・保持タイマー・dt はシミュレーション秒、max_age・duration は実時間秒です。duration=0.25 の間、SDK は 10 Hz
# で命令を更新し、戻る前に命令リースを解放します。
#
# 測定結果: 共通条件 5 回で 15.2108/100。WWW-POSIM 採点規則バージョン 5。
#
# 学習用制御器です。5 回の記録を確認し、一度に 1 変数を調整します。ゲイン・ゲートの仮定・船体余裕は付属モデル用です。実行ロジックを変えず、Python AST の一致を検査します。
#
# 採点原本:
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/station_controller.py
# コード解説: https://www-posim.vercel.app/course/examples.ja.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、yaw 角速度は 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
