# WWW-POSIM route starter · IOES-Lab · MIT
# Task: underwater_gate · BlueROV2
#
# Goal: Dive, cross each gate in order and meet the final depth/heading hold.
#
# Control: Gate-plane crossing/recovery and six-thruster depth/heading control.
#
# Start: installed app → Practice arena → this task/condition → ROS 2 direct control → Start world
# → Start scoring. In the prepared Python terminal, run: python main.py
#
# Website: upload this file as main.py; the server prepares Python and the SDK. SDK: www-posim-ros
# 0.4.2. Keep credentials out of the script.
#
# Units: map/world x,y,z in metres; positive z is up, so underwater targets are negative.
# Quaternion-derived yaw/roll are radians. Mission current_angle/current_swing are degrees.
# Commands use forward m/s, yaw_rate rad/s and thruster force N in native joint order.
#
# Timing: ROS stamps, hold timers and dt use simulation seconds. max_age and duration use
# wall-clock seconds. During duration=0.25 the SDK refreshes commands at 10 Hz, then releases the
# command lease before returning.
#
# Measured result: 13.3338/100 over five shared conditions; WWW-POSIM scoring revision 5.
#
# Learning controller: inspect all five runs and tune one parameter at a time. Gains, gate
# assumptions and hull margins belong to the bundled model. Comments preserve the scored logic;
# Python AST equality is checked.
#
# Scored original:
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/underwater_gate_controller.py
# Code walkthrough: https://www-posim.vercel.app/course/examples.html
# Stop with Ctrl+C or Stop input. Session cleanup releases control. Keep unexpected SDK errors
# visible; this file does not set the server scoring rules.

"""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


# Limit an output symmetrically; the caller supplies its physical-unit limit.
def clamp(value, limit):
    return max(-limit, min(limit, value))


# Wrap a heading error into [-pi, pi) radians to choose the shorter turn.
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# Inflate buoy circles by hull clearance, then use Dijkstra on a small visibility graph.
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."""
    # Gate posts retain their corridor clearance; other buoys use the selected obstacle margin.
    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
    # Reject straight graph edges that intersect an inflated obstacle circle.
    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


# Interpolate consecutive measured poses at the X-aligned gate plane; check width and depth.
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


# Recover to the entry side after a missed crossing; align lateral offset and speed before
# retrying.
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


# Integrate bounded body-speed error in world axes using simulation dt; clamp the force estimate.
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():
    # Discover the installed/sandbox session and acquire control; context exit closes the SDK
    # session.
    with Session.local(control=True) as sim:
        # Read this trial's targets and conditions from the session; do not hard-code course
        # coordinates.
        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'
        # Select odometry by ROS message type so a vehicle-specific topic name is unnecessary.
        topic = next(name for name, kind in sim.topics.items()
                     if kind == 'nav_msgs/msg/Odometry')
        # Maintain controller progress separately from the independent server scorer.
        targets = mission['targets']
        # Retrace intermediate surface targets on return instead of cutting directly across
        # obstacles.
        if mission['task'] in ('surface_gates','surface_slalom'):
            # Return through the corridor already traversed. A straight line home can cut through
            # a gate post or an obstacle buoy.
            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':
            # Replace the route with clearance-aware waypoints for this task's published
            # obstacles.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'])
        if mission['task']=='underwater_return':
            # Replace the route with clearance-aware waypoints for this task's published
            # obstacles.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'],
                                  hull_radius=.5,obstacle_margin=3.5)
        # Keep the target index, continuous-hold timer and gate-recovery state between updates.
        index, holding_since = 0, None
        integral_x=integral_y=0.;previous_time=None;previous_pose=None;recovering_gate=False
        while True:
            # Require a sample received within 0.5 wall-clock seconds; stale telemetry raises an
            # SDK error.
            message = sim.latest(topic, max_age=0.5)
            pose, twist = message['pose']['pose'], message['twist']['twist']
            p, q = pose['position'], pose['orientation']
            # Convert the quaternion to yaw (rad); route control also extracts roll for upright
            # stabilization.
            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))
            # Use the ROS simulation timestamp for integration and hold timing, independently of
            # wall-clock command duration.
            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
            # Choose the current target; positions and target coordinates share the map/world
            # frame.
            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:
                # Aim beyond the gate plane; turning before it can miss passage.
                lead=max(0.3,min(2,distance*0.1))
                desired = math.atan2(dy+lead*math.sin(segment), dx+lead*math.cos(segment))
            # Adjust the requested hold heading with the published oscillating-current phase.
            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)
            # This local arrival heuristic advances the controller only; the server awards the
            # actual score.
            reached = (passage if underwater and goal.get('gate') else
                       (passed > 0 and distance < 2 if goal.get('gate') else near) and depth_ok)
            # Reset the hold timer whenever position/depth/heading conditions are lost; hold must
            # be continuous.
            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 uses high-level body forward velocity and yaw-rate commands.
                if not underwater:
                    if near and goal.get('hold'):
                        forward=clamp(0.3*(dx*math.cos(yaw)+dy*math.sin(yaw)),0.25)
                    else:
                        # The published condition supplies the steady current.
                        # Subtract it to aim upstream before a narrow gate; this starter does not
                        # estimate gusts or wave motion.
                        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:
                    # Joint-order allocation from the bundled six-thruster model.
                    # Bound the drift-compensation estimate and the commands sent to each of the
                    # six thrusters.
                    # The inspection course has three longer transit legs.
                    # A modestly faster transit leaves time for the return leg.
                    # The shared underwater loop selects task-specific transit gains and speed
                    # limits.
                    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':
                        # During long turns, position-only integration can keep accelerating past
                        # a marker and into a nearby post.
                        # Steady flow needs gentle adaptation; oscillating flow needs a faster
                        # response to its changing force.
                        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)
                    # Combine body-speed feedback with the world-frame drift estimate rotated back
                    # into the body frame.
                    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 feedback stabilizes attitude; vertical PD and a fixed trim regulate
                    # the negative-z depth target.
                    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']
                    # Allocate force/torque through the bundled geometry: 45-degree horizontal
                    # thrusters and lever arms in metres.
                    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)]
                    # Send six forces in native joint order, capped at 35 N per thruster in this
                    # example.
                    sim.set_thrusters([clamp(force, 35) for force in forces], duration=0.25)
            # Only normal control release ends the loop; unexpected errors remain visible for
            # diagnosis.
            except RuntimeError as error:
                if str(error) != 'control_lease_ended':
                    raise
                break


if __name__ == '__main__':
    run()
