# WWW-POSIM route starter · IOES-Lab · MIT
# Tâche : surface_gates · WAM-V
#
# Objectif : Passer les portes dans l'ordre, revenir par le couloir et tenir au départ.
#
# Contrôle : Cibles de route, points de retour retracés et compensation du courant constant.
#
# Démarrage : application installée → Terrain d'entraînement → tâche/condition → Contrôle direct
# ROS 2 → Démarrer le monde → Démarrer l'évaluation. Dans le terminal Python préparé : python
# main.py
#
# Site web : envoyer ce fichier sous main.py ; le serveur prépare Python et le SDK. SDK :
# www-posim-ros 0.4.2. Ne pas inclure d'identifiants dans le script.
#
# Unités : map/world x,y,z en mètres ; z positif vers le haut, cibles immergées négatives.
# Yaw/roll issus du quaternion en radians ; current_angle/current_swing de mission en degrés.
# Commandes : avance m/s, yaw_rate rad/s et force N dans l'ordre natif des articulations.
#
# Temps : horodatages ROS, maintien et dt en secondes simulées ; max_age et duration en secondes
# réelles. Pendant duration=0.25, le SDK actualise à 10 Hz puis libère le bail de commande avant
# le retour.
#
# Résultat mesuré : 18.5392/100 sur cinq conditions communes ; règles WWW-POSIM, révision 5.
#
# Contrôleur pédagogique : examiner les cinq essais et régler un paramètre à la fois. Gains,
# hypothèses de portes et marges sont propres au modèle fourni. Les commentaires conservent la
# logique évaluée ; l'égalité des AST Python est vérifiée.
#
# Original évalué :
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/surface_gates_controller.py
# Explication du code : https://www-posim.vercel.app/course/examples.fr.html
# Arrêter avec Ctrl+C ou Arrêter l'entrée. Le nettoyage de Session libère le contrôle. Garder
# visibles les erreurs SDK inattendues ; ce fichier ne définit pas les règles de score du serveur.

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


# Limiter la sortie symétriquement ; l'appelant fournit la limite dans son unité physique.
def clamp(value, limit):
    return max(-limit, min(limit, value))


# Ramener l'erreur de cap dans [-pi, pi) radians pour choisir la rotation la plus courte.
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# Élargir les cercles des bouées selon la marge de coque, puis appliquer Dijkstra à un petit
# graphe de visibilité.
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."""
    # Les poteaux conservent la marge du couloir ; les autres bouées utilisent la marge d'obstacle
    # choisie.
    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
    # Écarter les arêtes droites qui coupent un cercle d'obstacle élargi.
    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


# Interpoler les poses mesurées successives au plan de porte aligné sur X ; vérifier largeur et
# profondeur.
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


# Après un passage manqué, revenir du côté entrée ; aligner décalage latéral et vitesse avant de
# réessayer.
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


# Intégrer l'erreur bornée de vitesse du corps dans les axes monde avec dt simulé ; borner la
# force estimée.
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():
    # Découvrir la session installée ou isolée et obtenir le contrôle ; la sortie du contexte
    # ferme la session SDK.
    with Session.local(control=True) as sim:
        # Lire les cibles et conditions de cet essai dans la session ; ne pas coder les
        # coordonnées du parcours en dur.
        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'
        # Choisir l'odométrie par type de message ROS, sans dépendre d'un nom de topic propre au
        # véhicule.
        topic = next(name for name, kind in sim.topics.items()
                     if kind == 'nav_msgs/msg/Odometry')
        # Conserver la progression du contrôleur séparément de l'évaluateur indépendant du
        # serveur.
        targets = mission['targets']
        # Au retour, reprendre les cibles intermédiaires de surface en sens inverse pour éviter de
        # couper à travers les obstacles.
        if mission['task'] in ('surface_gates','surface_slalom'):
            # Revenir par le couloir déjà parcouru. Une ligne droite vers le départ peut traverser
            # un poteau ou une bouée obstacle.
            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':
            # Remplacer la route par des points de passage avec marge pour les obstacles publiés
            # de cette tâche.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'])
        if mission['task']=='underwater_return':
            # Remplacer la route par des points de passage avec marge pour les obstacles publiés
            # de cette tâche.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'],
                                  hull_radius=.5,obstacle_margin=3.5)
        # Conserver l'indice de cible, le chronomètre de maintien continu et l'état de reprise du
        # passage entre mises à jour.
        index, holding_since = 0, None
        integral_x=integral_y=0.;previous_time=None;previous_pose=None;recovering_gate=False
        while True:
            # Exiger un échantillon reçu depuis moins de 0.5 seconde réelle ; une télémétrie
            # périmée déclenche une erreur SDK.
            message = sim.latest(topic, max_age=0.5)
            pose, twist = message['pose']['pose'], message['twist']['twist']
            p, q = pose['position'], pose['orientation']
            # Convertir le quaternion en yaw (rad) ; le contrôle de route extrait aussi roll pour
            # stabiliser l'assiette.
            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))
            # Utiliser l'horodatage ROS simulé pour l'intégration et le maintien, indépendamment
            # de la durée réelle des commandes.
            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
            # Choisir la cible courante ; positions et cibles partagent le repère 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:
                # Viser au-delà du plan de porte ; tourner avant peut faire manquer le passage.
                lead=max(0.3,min(2,distance*0.1))
                desired = math.atan2(dy+lead*math.sin(segment), dx+lead*math.cos(segment))
            # Ajuster le cap à maintenir selon la phase publiée du courant oscillant.
            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)
            # Cette heuristique locale d'arrivée fait seulement progresser le contrôleur ; le
            # serveur attribue le score réel.
            reached = (passage if underwater and goal.get('gate') else
                       (passed > 0 and distance < 2 if goal.get('gate') else near) and depth_ok)
            # Réinitialiser le chronomètre dès qu'une condition de position, profondeur ou cap est
            # perdue ; le maintien doit être continu.
            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 utilise des commandes de haut niveau de vitesse longitudinale du corps et
                # de vitesse de lacet.
                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 condition publiée fournit le courant constant.
                        # La soustraire pour viser l'amont avant une porte étroite ; cet exemple
                        # n'estime ni rafales ni mouvement des vagues.
                        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:
                    # Répartition selon l'ordre des articulations du modèle fourni à six
                    # propulseurs.
                    # Borner l'estimation de compensation de dérive et les commandes de chacun des
                    # six propulseurs.
                    # Le parcours d'inspection comporte trois trajets de transit plus longs.
                    # Un transit un peu plus rapide laisse du temps pour le retour.
                    # La boucle sous-marine commune choisit les gains de transit et limites de
                    # vitesse propres à la tâche.
                    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':
                        # Pendant les longs virages, intégrer seulement la position peut prolonger
                        # l'accélération au-delà d'un repère vers un poteau voisin.
                        # Le courant constant demande une adaptation douce ; le courant oscillant
                        # exige une réponse plus rapide à sa force variable.
                        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)
                    # Combiner le retour de vitesse du corps avec la dérive estimée dans le monde,
                    # ramenée dans le repère corps.
                    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)
                    # Le retour yaw/roll stabilise l'attitude ; un PD vertical et une compensation
                    # fixe régulent la cible de profondeur z négative.
                    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']
                    # Répartir forces et couples selon la géométrie fournie : propulseurs
                    # horizontaux à 45 degrés et bras en mètres.
                    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)]
                    # Envoyer six forces dans l'ordre natif des articulations, limitées ici à 35 N
                    # par propulseur.
                    sim.set_thrusters([clamp(force, 35) for force in forces], duration=0.25)
            # Seule la libération normale du contrôle termine la boucle ; les erreurs inattendues
            # restent visibles pour le diagnostic.
            except RuntimeError as error:
                if str(error) != 'control_lease_ended':
                    raise
                break


if __name__ == '__main__':
    run()
