# WWW-POSIM route starter · IOES-Lab · MIT
"""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))


def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


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


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


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 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'
        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'):
            # 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':
            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:
            message = sim.latest(topic, max_age=0.5)
            pose, twist = message['pose']['pose'], message['twist']['twist']
            p, q = pose['position'], pose['orientation']
            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))
            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
            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))
            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:
                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.58*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.
                    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)
                    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)
                    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']
                    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)]
                    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()
