# 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는 위쪽이므로 수중 목표 z는 음수입니다. 쿼터니언에서 구한 yaw/roll은 라디안, 임무
# current_angle/current_swing은 도 단위입니다. 명령은 전진 m/s, yaw_rate rad/s, 기본 조인트 순서의 추진기 힘 N입니다.
#
# 시간: ROS 시각·유지 타이머·dt는 시뮬레이션 초, max_age·duration은 실제 초입니다. duration=0.25 동안 SDK가 10Hz로 명령을 갱신하고
# 반환 전에 명령 임대를 해제합니다.
#
# 측정 결과: 공통 조건 5회, WWW-POSIM 채점 규칙 버전 5, 15.2108/100점입니다.
#
# 학습용 제어기입니다. 다섯 기록을 살핀 뒤 변수를 하나씩 조정합니다. 게인·게이트 가정·선체 여유는 기본 모델에 맞춘 값입니다. 주석본은 채점 원본과 같은 실행 로직이며
# Python AST 일치를 검사합니다.
#
# 채점 원본:
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/station_controller.py
# 코드 설명: https://www-posim.vercel.app/course/examples.ko.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


# 설치형 앱·샌드박스 세션을 찾고 제어권을 요청하며 with 종료 시 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(라디안)로 변환하며 경로 제어에서는 수평 자세 유지를 위해 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.6m 히스테리시스로 접근·방향 유지 상태의 잦은 전환을 줄입니다.
        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
