# WWW-POSIM route starter · IOES-Lab · MIT
# Tarea: surface_gates · WAM-V
#
# Objetivo: Pase las puertas en orden, vuelva por el corredor y manténgase en el inicio.
#
# Control: Objetivos de ruta, puntos de retorno en sentido inverso y compensación de corriente
# constante.
#
# Inicio: app instalada → Campo de práctica → esta tarea/condición → Control directo ROS 2 →
# Iniciar mundo → Iniciar puntuación. En el terminal Python preparado: python main.py
#
# Web: suba este archivo como main.py; el servidor prepara Python y el SDK. SDK: www-posim-ros
# 0.4.2. No incluya credenciales en el script.
#
# Unidades: map/world x,y,z en metros; z positivo apunta arriba y los objetivos submarinos son
# negativos. Yaw/roll del cuaternión están en radianes; current_angle/current_swing de la misión,
# en grados. Comandos: avance m/s, yaw_rate rad/s y fuerza N en orden nativo de articulaciones.
#
# Tiempo: marcas ROS, permanencia y dt usan segundos simulados; max_age y duration, segundos
# reales. Durante duration=0.25 el SDK actualiza comandos a 10 Hz y libera el permiso de comando
# antes de retornar.
#
# Resultado medido: 18.5392/100 en cinco condiciones comunes; reglas WWW-POSIM, revisión 5.
#
# Controlador didáctico: revise las cinco pruebas y ajuste un parámetro a la vez. Ganancias,
# supuestos de puertas y márgenes son propios del modelo incluido. Los comentarios conservan la
# lógica puntuada; se verifica la igualdad del AST de Python.
#
# Original puntuado:
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/surface_gates_controller.py
# Guía del código: https://www-posim.vercel.app/course/examples.es.html
# Detenga con Ctrl+C o Detener entrada. La limpieza de Session libera el control. Mantenga
# visibles los errores inesperados del SDK; este archivo no define las reglas de puntuación del
# servidor.

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


# Limite la salida simétricamente; quien llama indica el límite en sus unidades físicas.
def clamp(value, limit):
    return max(-limit, min(limit, value))


# Normalice el error de rumbo a [-pi, pi) radianes para elegir el giro más corto.
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# Amplíe los círculos de las boyas según la holgura del casco y aplique Dijkstra a un pequeño
# grafo de visibilidad.
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."""
    # Los postes conservan la holgura del paso; las demás boyas usan el margen de obstáculo
    # elegido.
    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
    # Descarte las aristas rectas que intersecten un círculo de obstáculo ampliado.
    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


# Interpole poses medidas consecutivas en el plano de puerta alineado con X; compruebe anchura y
# profundidad.
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


# Tras un cruce fallido, vuelva al lado de entrada y ajuste el desplazamiento lateral y la
# velocidad antes de reintentar.
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


# Integre el error limitado de velocidad del cuerpo en ejes globales con dt de simulación; limite
# la fuerza estimada.
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():
    # Busque la sesión instalada o aislada y obtenga el control; salir del contexto cierra la
    # sesión SDK.
    with Session.local(control=True) as sim:
        # Lea objetivos y condiciones de esta prueba desde la sesión; no fije las coordenadas del
        # recorrido en el código.
        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'
        # Seleccione odometría por tipo de mensaje ROS, sin depender del nombre del tópico de cada
        # vehículo.
        topic = next(name for name, kind in sim.topics.items()
                     if kind == 'nav_msgs/msg/Odometry')
        # Mantenga el progreso del controlador separado del evaluador independiente del servidor.
        targets = mission['targets']
        # Al volver, recorra los objetivos intermedios de superficie en sentido inverso para no
        # atravesar obstáculos.
        if mission['task'] in ('surface_gates','surface_slalom'):
            # Vuelva por el corredor ya recorrido. La línea recta al inicio puede atravesar un
            # poste o una boya obstáculo.
            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':
            # Sustituya la ruta por puntos con margen de seguridad para los obstáculos publicados
            # de esta tarea.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'])
        if mission['task']=='underwater_return':
            # Sustituya la ruta por puntos con margen de seguridad para los obstáculos publicados
            # de esta tarea.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'],
                                  hull_radius=.5,obstacle_margin=3.5)
        # Conserve el índice de objetivo, el temporizador de permanencia y el estado de
        # recuperación entre actualizaciones.
        index, holding_since = 0, None
        integral_x=integral_y=0.;previous_time=None;previous_pose=None;recovering_gate=False
        while True:
            # Exija una muestra recibida en los últimos 0.5 segundos reales; la telemetría
            # obsoleta provoca un error SDK.
            message = sim.latest(topic, max_age=0.5)
            pose, twist = message['pose']['pose'], message['twist']['twist']
            p, q = pose['position'], pose['orientation']
            # Convierta el cuaternión a yaw (rad); el control de ruta también extrae roll para
            # estabilizar la inclinación.
            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 la marca temporal ROS de simulación para integrar y medir permanencia,
            # independientemente de la duración real del comando.
            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
            # Elija el objetivo actual; posiciones y objetivos comparten el marco 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:
                # Apunte más allá del plano de puerta; girar antes puede impedir el cruce.
                lead=max(0.3,min(2,distance*0.1))
                desired = math.atan2(dy+lead*math.sin(segment), dx+lead*math.cos(segment))
            # Ajuste el rumbo de mantenimiento con la fase publicada de la corriente oscilante.
            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)
            # Esta heurística local de llegada solo avanza el controlador; el servidor asigna la
            # puntuación real.
            reached = (passage if underwater and goal.get('gate') else
                       (passed > 0 and distance < 2 if goal.get('gate') else near) and depth_ok)
            # Reinicie el temporizador si se pierde la condición de posición, profundidad o rumbo;
            # la permanencia debe ser continua.
            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 usa comandos de alto nivel de velocidad longitudinal del cuerpo y
                # velocidad de guiñada.
                if not underwater:
                    if near and goal.get('hold'):
                        forward=clamp(0.3*(dx*math.cos(yaw)+dy*math.sin(yaw)),0.25)
                    else:
                        # La condición publicada proporciona la corriente constante.
                        # Réstela para apuntar aguas arriba antes de una puerta estrecha; este
                        # ejemplo no estima ráfagas ni movimiento de olas.
                        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:
                    # Reparto en orden de articulaciones del modelo incluido de seis propulsores.
                    # Limite la estimación de compensación de deriva y los comandos a cada uno de
                    # los seis propulsores.
                    # El recorrido de inspección tiene tres tramos de tránsito más largos.
                    # Un tránsito algo más rápido deja tiempo para la vuelta.
                    # El bucle submarino compartido selecciona ganancias y límites de velocidad
                    # específicos de cada tarea.
                    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':
                        # En giros largos, integrar solo posición puede seguir acelerando más allá
                        # de una marca y hacia un poste cercano.
                        # La corriente constante requiere adaptación suave; la oscilante exige
                        # responder más rápido a su fuerza cambiante.
                        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 la realimentación de velocidad del cuerpo con la estimación de
                    # deriva global rotada al marco del cuerpo.
                    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)
                    # La realimentación yaw/roll estabiliza la actitud; un PD vertical y un trim
                    # fijo regulan la profundidad z negativa.
                    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']
                    # Distribuya fuerzas y momentos según la geometría incluida: propulsores
                    # horizontales a 45 grados y brazos en metros.
                    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)]
                    # Envíe seis fuerzas en el orden nativo de articulaciones; este ejemplo limita
                    # cada propulsor a 35 N.
                    sim.set_thrusters([clamp(force, 35) for force in forces], duration=0.25)
            # Solo la liberación normal del control termina el bucle; los errores inesperados
            # quedan visibles para diagnosticarlos.
            except RuntimeError as error:
                if str(error) != 'control_lease_ended':
                    raise
                break


if __name__ == '__main__':
    run()
