# WWW-POSIM route starter · IOES-Lab · MIT
# 과제: underwater_gate · BlueROV2
#
# 목표: 잠항 후 게이트를 순서대로 통과하고 마지막 수심·방향 유지 조건을 수행합니다.
#
# 제어: 게이트 평면 통과·재접근과 6개 추진기의 수심·방향 제어를 사용합니다.
#
# 실행: 설치형 앱 → 연습 경기장 → 이 과제·조건 → 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, 13.3338/100점입니다.
#
# 학습용 제어기입니다. 다섯 기록을 살핀 뒤 변수를 하나씩 조정합니다. 게인·게이트 가정·선체 여유는 기본 모델에 맞춘 값입니다. 주석본은 채점 원본과 같은 실행 로직이며
# Python AST 일치를 검사합니다.
#
# 채점 원본:
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/underwater_gate_controller.py
# 코드 설명: https://www-posim.vercel.app/course/examples.ko.html
# Ctrl+C 또는 입력 정지로 종료합니다. Session 정리가 제어권을 해제합니다. 예상하지 못한 SDK 오류는 숨기지 않습니다. 이 파일에서 서버 채점 규칙을 설정하지
# 않습니다.

"""A deliberately simple waypoint follower for surface and underwater challenges.

Choose a challenge in ROS 2 direct-control mode, start its evaluation, then run
this file. Positions, velocities and course targets come from the normal SDK.
The native evaluator owns the score and ends control when the run finishes.
"""
import math
import heapq
from www_posim_ros import Session


# 출력을 양·음 방향으로 제한하며 단위와 제한값은 호출부에서 정합니다.
def clamp(value, limit):
    return max(-limit, min(limit, value))


# 방향 오차를 [-pi, pi) 라디안으로 접어 짧은 회전 방향을 선택합니다.
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# 선체 여유만큼 부표 원을 키우고 작은 가시성 그래프에 Dijkstra 탐색을 적용합니다.
def slalom_detours(home, targets, obstacles, hull_radius=3.5, obstacle_margin=1.3):
    """Walk around the published buoy circles with a small visibility graph."""
    # 게이트 기둥에는 통로 여유를 유지하고 다른 부표에는 선택한 장애물 여유를 적용합니다.
    def margin(obstacle):
        gate_post=any(t.get('gate') and abs(t['x']-obstacle['x'])<1e-6 for t in targets)
        return 1.3 if gate_post else obstacle_margin
    circles=[(o['x'],o['y'],hull_radius+o['radius_m']+margin(o)) for o in obstacles]
    result=[];previous=home
    # 확장한 장애물 원과 겹치는 직선 그래프 간선을 제외합니다.
    def clear(a,b):
        dx,dy=b['x']-a['x'],b['y']-a['y'];length=dx*dx+dy*dy
        for x,y,r in circles:
            t=max(0,min(1,((x-a['x'])*dx+(y-a['y'])*dy)/length)) if length else 0
            if math.hypot(a['x']+t*dx-x,a['y']+t*dy-y)<r:return False
        return True
    for goal in targets:
        nodes=[previous,goal]
        for x,y,r in circles:
            for i in range(8):
                theta=i*math.pi/4
                nodes.append(dict(x=x+(r+1.2)*math.cos(theta),y=y+(r+1.2)*math.sin(theta),z=goal['z']))
        distance={0:0};parent={};queue=[(0,0)]
        while queue:
            cost,i=heapq.heappop(queue)
            if cost!=distance[i]:continue
            if i==1:break
            for j,node in enumerate(nodes):
                if i==j or not clear(nodes[i],node):continue
                candidate=cost+math.hypot(node['x']-nodes[i]['x'],node['y']-nodes[i]['y'])
                if candidate<distance.get(j,float('inf')):
                    distance[j]=candidate;parent[j]=i;heapq.heappush(queue,(candidate,j))
        if 1 not in distance:raise ValueError('No clear route through published buoy geometry')
        route=[];i=1
        while i:route.append(nodes[i]);i=parent[i]
        result.extend(reversed(route));previous=goal
    return result


# 연속 측정 자세를 X 방향 게이트 평면에서 보간해 통과 폭과 수심을 확인합니다.
def gate_crossed(previous_pose, position, goal, previous_target, depth_tolerance):
    """Confirm passage through the published gate plane using observed poses."""
    if previous_pose is None:return False
    direction=1 if goal['x']>=previous_target['x'] else -1
    before=(previous_pose['x']-goal['x'])*direction
    after=(position['x']-goal['x'])*direction
    if not before<0<=after:return False
    fraction=(goal['x']-previous_pose['x'])/(position['x']-previous_pose['x'])
    y=previous_pose['y']+fraction*(position['y']-previous_pose['y'])
    z=previous_pose['z']+fraction*(position['z']-previous_pose['z'])
    return abs(y-goal['y'])<goal['gate']['usable_half_width_m'] and abs(z-goal['z'])<depth_tolerance


# 통과를 놓치면 입구 쪽으로 돌아가 가로 위치·속도를 맞춘 뒤 재접근합니다.
def gate_aim(position, goal, previous_target, recovering, passed, lateral_speed):
    """Stay on the entry side while cross-current alignment is unsettled."""
    direction=1 if goal['x']>=previous_target['x'] else -1
    side=(position['x']-goal['x'])*direction
    lateral=goal['y']-position['y']
    width=goal['gate']['usable_half_width_m']
    safe_lateral=width-min(.75,width/2)
    if passed:recovering=False
    elif side>=0 or abs(lateral)>safe_lateral:recovering=True
    if recovering and side<=-2 and abs(lateral)<min(.5,safe_lateral/2) and abs(lateral_speed)<.15:
        recovering=False
    aim_x=goal['x']+direction*(-2.5 if recovering else 1.5)
    return aim_x-position['x'],lateral,recovering


# 시뮬레이션 dt로 제한된 기체 속도 오차를 월드 축에서 적분하고 힘 추정값을 제한합니다.
def velocity_integral(ix, iy, vx, vy, linear, yaw, dt, gain=1.):
    """Estimate the needed world-frame force without winding up in transit."""
    ex,ey=vx-linear['x'],vy-linear['y']
    world_x=ex*math.cos(yaw)-ey*math.sin(yaw)
    world_y=ex*math.sin(yaw)+ey*math.cos(yaw)
    return (clamp(ix+gain*clamp(world_x,.6)*dt,15),
            clamp(iy+gain*clamp(world_y,.6)*dt,15))


def run():
    # 설치형 앱·샌드박스 세션을 찾고 제어권을 요청하며 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 a route challenge; station keeping has its own starter.')
        underwater = mission['robot'] == 'bluerov2'
        # 차량별 토픽 이름을 고정하지 않고 ROS 메시지 종류로 오도메트리를 선택합니다.
        topic = next(name for name, kind in sim.topics.items()
                     if kind == 'nav_msgs/msg/Odometry')
        # 제어기의 진행 상태를 서버의 독립적인 채점 상태와 별도로 관리합니다.
        targets = mission['targets']
        # 수상 복귀 시 장애물을 가로지르는 직선 대신 중간 목표를 거꾸로 따라갑니다.
        if mission['task'] in ('surface_gates','surface_slalom'):
            # 지나온 통로로 복귀합니다. 출발점까지 직진하면 게이트 기둥이나 장애물 부표를 가로지를 수 있습니다.
            retrace = [{key:point[key] for key in ('x','y','z')}
                       for point in reversed(targets[:-1])]
            targets = targets[:-1]+retrace+targets[-1:]
        if mission['task']=='surface_slalom':
            # 이 과제의 공개 장애물에 맞춰 여유 거리를 둔 경유점으로 경로를 바꿉니다.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'])
        if mission['task']=='underwater_return':
            # 이 과제의 공개 장애물에 맞춰 여유 거리를 둔 경유점으로 경로를 바꿉니다.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'],
                                  hull_radius=.5,obstacle_margin=3.5)
        # 갱신 사이에 목표 번호, 연속 유지 타이머와 게이트 재접근 상태를 보관합니다.
        index, holding_since = 0, None
        integral_x=integral_y=0.;previous_time=None;previous_pose=None;recovering_gate=False
        while True:
            # 최근 실제 시간 0.5초 안에 수신한 표본을 요구하며 오래된 계측은 SDK 오류로 처리합니다.
            message = sim.latest(topic, max_age=0.5)
            pose, twist = message['pose']['pose'], message['twist']['twist']
            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))
            roll = math.atan2(2*(q['w']*q['x']+q['y']*q['z']),
                              1-2*(q['x']**2+q['y']**2))
            # 명령의 실제 시간 길이와 별도로 ROS 시뮬레이션 시각을 적분·유지 시간에 사용합니다.
            stamp = message['header']['stamp']
            now = stamp['sec']+stamp['nanosec']*1e-9
            dt=0 if previous_time is None else max(0,min(0.5,now-previous_time))
            previous_time=now
            # 현재 목표를 고르며 측정 위치와 목표 좌표는 같은 map/world 좌표계를 사용합니다.
            goal = targets[min(index, len(targets)-1)]
            previous = mission['home'] if index == 0 else targets[index-1]
            dx, dy = goal['x']-p['x'], goal['y']-p['y']
            distance = math.hypot(dx, dy)
            passage = False
            if underwater and goal.get('gate'):
                passage=gate_crossed(previous_pose,p,goal,previous,mission['depth_tolerance_m'])
                velocity=twist['linear']
                lateral_speed=velocity['x']*math.sin(yaw)+velocity['y']*math.cos(yaw)
                dx,dy,recovering_gate=gate_aim(p,goal,previous,recovering_gate,passage,lateral_speed)
            segment = math.atan2(goal['y']-previous['y'], goal['x']-previous['x'])
            passed = (p['x']-goal['x'])*math.cos(segment)+(p['y']-goal['y'])*math.sin(segment)
            near = distance < (0.5 if underwater else 0.8)
            depth_ok = not underwater or abs(p['z']-goal['z']) < 0.2
            desired = goal.get('yaw', segment) if near and goal.get('hold') else math.atan2(dy, dx)
            if goal.get('gate') and not underwater:
                # 게이트 평면 너머를 겨냥합니다. 평면 전에 회전하면 통과를 놓칠 수 있습니다.
                lead=max(0.3,min(2,distance*0.1))
                desired = math.atan2(dy+lead*math.sin(segment), dx+lead*math.cos(segment))
            # 공개된 변동 해류의 위상에 따라 유지할 목표 방향을 조정합니다.
            if goal.get('yaw_current_follow') and near:
                elapsed = now-mission['current_epoch_sim_time']
                desired += math.radians(mission['current_swing'])*math.sin(
                    2*math.pi*elapsed/mission['current_period']+mission['current_phase'])
            yaw_error = angle(desired-yaw)
            # 이 로컬 도착 판정은 제어기의 목표만 넘기며 실제 점수는 서버가 판정합니다.
            reached = (passage if underwater and goal.get('gate') else
                       (passed > 0 and distance < 2 if goal.get('gate') else near) and depth_ok)
            # 위치·수심·방향 조건을 벗어나면 유지 타이머를 초기화하며 유지는 연속이어야 합니다.
            if goal.get('hold'):
                reached = reached and abs(yaw_error) < mission['heading_tolerance_rad']
                holding_since = (now if holding_since is None else holding_since) if reached else None
                reached = holding_since is not None and now-holding_since >= goal['hold']
            if reached and index < len(targets)-1:
                index += 1
                holding_since = None
                recovering_gate = False
            previous_pose = dict(p)
            try:
                # WAM-V는 기체 전진 속도와 yaw 회전 속도의 상위 명령을 사용합니다.
                if not underwater:
                    if near and goal.get('hold'):
                        forward=clamp(0.3*(dx*math.cos(yaw)+dy*math.sin(yaw)),0.25)
                    else:
                        # 공개된 평가 조건에서 정상 해류를 읽습니다.
                        # 좁은 게이트 전에 상류를 겨냥하도록 해류를 뺍니다. 이 예제는 돌풍이나 파도 움직임을 추정하지 않습니다.
                        speed=min(0.6,0.3*distance)
                        flow=math.radians(mission['current_angle'])
                        gx,gy=speed*math.cos(desired),speed*math.sin(desired)
                        desired=math.atan2(gy-mission['current_speed']*math.sin(flow),
                                           gx-mission['current_speed']*math.cos(flow))
                        yaw_error=angle(desired-yaw)
                        turn_limit=0.35 if mission['task']=='surface_slalom' else 0.7
                        forward=max(0,gx*math.cos(yaw)+gy*math.sin(yaw)) if abs(yaw_error)<turn_limit else 0.
                    sim.set_velocity(forward=forward, yaw_rate=clamp(0.6*yaw_error, 0.5), duration=0.25)
                else:
                    # 기본 6추진기 모델의 조인트 순서로 힘을 배분합니다.
                    # 표류 보상 추정값과 6개 추진기에 보내는 명령을 제한합니다.
                    # 검사 코스에는 긴 이동 구간이 3개 있습니다.
                    # 이동 속도를 조금 높여 복귀할 시간을 남깁니다.
                    # 공유 수중 제어 루프는 과제별 이동 게인과 속도 제한을 선택합니다.
                    inspection = mission['task']=='underwater_path'
                    returning = mission['task']=='underwater_return'
                    gain = 0.24 if inspection else 0.17
                    vx = clamp(gain*(dx*math.cos(yaw)+dy*math.sin(yaw)), 0.3 if returning else 0.6 if inspection else 0.45)
                    vy = clamp(gain*(-dx*math.sin(yaw)+dy*math.cos(yaw)), 0.25 if returning else 0.5 if inspection else 0.35)
                    linear, angular = twist['linear'], twist['angular']
                    if mission['task']=='underwater_return':
                        # 긴 회전 중 위치 오차만 적분하면 계속 가속해 표지를 지나 인근 기둥에 부딪힐 수 있습니다.
                        # 정상 해류에는 완만하게 적응하고 변동 해류에는 변하는 힘에 더 빠르게 반응합니다.
                        integral_x,integral_y=velocity_integral(
                            integral_x,integral_y,vx,vy,linear,yaw,dt,
                            gain=4. if mission.get('current_gust',0)>0 else 1.)
                    else:
                        integral_x=clamp(integral_x+0.25*clamp(dx,1)*dt,15)
                        integral_y=clamp(integral_y+0.25*clamp(dy,1)*dt,15)
                    # 기체 속도 피드백과 월드 축 표류 추정값을 기체 축으로 돌린 값을 합칩니다.
                    fx = 10*(vx-linear['x'])+integral_x*math.cos(yaw)+integral_y*math.sin(yaw)
                    fy = 10*(vy-linear['y'])-integral_x*math.sin(yaw)+integral_y*math.cos(yaw)
                    # yaw·roll 피드백으로 자세를 안정화하며 수직 PD와 고정 트림으로 음의 z 수심 목표를 조절합니다.
                    mz = 0.7*yaw_error-0.4*angular['z']
                    fz = 12*(goal['z']-p['z'])-8*linear['z']-2
                    mx = -2*roll-0.4*angular['x']
                    # 기본 모델 형상인 45도 수평 추진기와 미터 단위 레버 암을 사용해 힘·토크를 배분합니다.
                    a, b, c = fx/(4*math.sqrt(0.5)), fy/(4*math.sqrt(0.5)), mz/(4*0.167)
                    forces = [-a-b-c, -a+b+c, a-b+c, a+b-c,
                              -fz/2+mx/(2*0.109), -fz/2-mx/(2*0.109)]
                    # 기본 조인트 순서로 6개 힘을 보내며 이 예제는 추진기별 35N으로 제한합니다.
                    sim.set_thrusters([clamp(force, 35) for force in forces], duration=0.25)
            # 정상 제어권 종료만 루프를 끝내고 예상하지 못한 오류는 진단할 수 있게 남깁니다.
            except RuntimeError as error:
                if str(error) != 'control_lease_ended':
                    raise
                break


if __name__ == '__main__':
    run()
