# WWW-POSIM route starter · IOES-Lab · MIT
# 任务：surface_slalom · 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
# 刷新命令，返回前释放命令租约。
#
# 实测结果：五个共同条件下 10.6000/100；WWW-POSIM 评分规则第 5 版。
#
# 学习用控制器：检查五次记录，每次调整一个参数。增益、门的假设和船体余量针对附带模型。注释保留已评分逻辑，并检查 Python AST 一致性。
#
# 评分原始文件：https://www-posim.vercel.app/course/examples/maritime-class-5/measured/surface_slalom_controller.py
# 代码说明：https://www-posim.vercel.app/course/examples.zh.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():
    # 发现安装版或沙箱会话并获取控制权；退出上下文时关闭 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（rad）；路线控制还提取 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 使用机体前向速度和偏航角速度的高层命令。
                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.58*yaw_error, 0.5), duration=0.25)
                else:
                    # 按附带六推进器模型的关节顺序分配。
                    # 限制漂移补偿估计和发送给六个推进器的命令。
                    # 巡检路线有三个较长的移动区段。
                    # 适当提高移动速度，为返航留出时间。
                    # 共享水下控制循环选择各任务的移动增益和速度限制。
                    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)]
                    # 按原生关节顺序发送六个力，本示例中每个推进器限幅为 35 N。
                    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()
