# WWW-POSIM route starter · IOES-Lab · MIT
# Prova: underwater_return · BlueROV2
#
# Obiettivo: Percorrere gli ostacoli subacquei, tornare e completare risalita e mantenimento
# finali.
#
# Controllo: Deviazioni per scafo piccolo, integrazione dell'errore di velocità e ripartizione su
# sei propulsori.
#
# Avvio: app installata → Arena di pratica → prova/condizione → Controllo diretto ROS 2 → Avvia
# mondo → Avvia valutazione. Nel terminale Python preparato: python main.py
#
# Sito web: caricare il file come main.py; il server prepara Python e SDK. SDK: www-posim-ros
# 0.4.2. Non inserire credenziali nello script.
#
# Unità: map/world x,y,z in metri; z positivo verso l'alto, obiettivi subacquei negativi. Yaw/roll
# dal quaternione in radianti; current_angle/current_swing della missione in gradi. Comandi:
# avanzamento m/s, yaw_rate rad/s e forza N nell'ordine nativo dei giunti.
#
# Tempi: timestamp ROS, timer di mantenimento e dt in secondi simulati; max_age e duration in
# secondi reali. Durante duration=0.25 l'SDK aggiorna a 10 Hz e rilascia il lease del comando
# prima di ritornare.
#
# Risultato misurato: 9.3347/100 su cinque condizioni comuni; regole WWW-POSIM, revisione 5.
#
# Controllore didattico: esaminare le cinque prove e regolare un parametro alla volta. Guadagni,
# ipotesi sui varchi e margini appartengono al modello incluso. I commenti preservano la logica
# valutata; si verifica l'uguaglianza degli AST Python.
#
# Originale valutato:
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/underwater_return_controller.py
# Spiegazione del codice: https://www-posim.vercel.app/course/examples.it.html
# Arrestare con Ctrl+C o Ferma input. La pulizia di Session rilascia il controllo. Lasciare
# visibili gli errori SDK inattesi; il file non imposta le regole di punteggio del server.

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


# Limitare simmetricamente l'uscita; il chiamante fornisce il limite nella sua unità fisica.
def clamp(value, limit):
    return max(-limit, min(limit, value))


# Riportare l'errore di rotta in [-pi, pi) radianti per scegliere la rotazione più breve.
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# Ampliare i cerchi delle boe con il margine dello scafo, quindi applicare Dijkstra a un piccolo
# grafo di 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."""
    # I pali mantengono il margine del corridoio; le altre boe usano il margine ostacoli scelto.
    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
    # Scartare gli archi rettilinei che intersecano un cerchio ostacolo ampliato.
    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


# Interpolare pose misurate consecutive sul piano del varco allineato a X; verificare larghezza e
# profondità.
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


# Dopo un passaggio mancato, tornare sul lato d'ingresso e allineare scostamento laterale e
# velocità prima di riprovare.
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


# Integrare l'errore limitato di velocità del corpo negli assi mondo con dt simulato; limitare la
# forza stimata.
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():
    # Individuare la sessione installata o isolata e acquisire il controllo; l'uscita dal contesto
    # chiude la sessione SDK.
    with Session.local(control=True) as sim:
        # Leggere obiettivi e condizioni della prova dalla sessione; non fissare le coordinate del
        # percorso nel codice.
        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'
        # Selezionare l'odometria per tipo di messaggio ROS, senza dipendere dal nome del topic
        # del veicolo.
        topic = next(name for name, kind in sim.topics.items()
                     if kind == 'nav_msgs/msg/Odometry')
        # Mantenere l'avanzamento del controllore separato dal valutatore indipendente del server.
        targets = mission['targets']
        # Al ritorno ripercorrere gli obiettivi intermedi di superficie, evitando tagli attraverso
        # gli ostacoli.
        if mission['task'] in ('surface_gates','surface_slalom'):
            # Tornare nel corridoio già percorso. La linea retta verso l'inizio può attraversare
            # un palo o una boa ostacolo.
            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':
            # Sostituire la rotta con punti dotati di margine rispetto agli ostacoli pubblicati
            # della prova.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'])
        if mission['task']=='underwater_return':
            # Sostituire la rotta con punti dotati di margine rispetto agli ostacoli pubblicati
            # della prova.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'],
                                  hull_radius=.5,obstacle_margin=3.5)
        # Conservare indice dell'obiettivo, timer di mantenimento continuo e stato di recupero del
        # varco tra aggiornamenti.
        index, holding_since = 0, None
        integral_x=integral_y=0.;previous_time=None;previous_pose=None;recovering_gate=False
        while True:
            # Richiedere un campione ricevuto negli ultimi 0.5 secondi reali; la telemetria
            # obsoleta genera un errore SDK.
            message = sim.latest(topic, max_age=0.5)
            pose, twist = message['pose']['pose'], message['twist']['twist']
            p, q = pose['position'], pose['orientation']
            # Convertire il quaternione in yaw (rad); il controllo del percorso estrae anche roll
            # per stabilizzare l'assetto.
            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))
            # Usare il timestamp ROS simulato per integrazione e mantenimento, separatamente dalla
            # durata reale dei comandi.
            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
            # Scegliere l'obiettivo corrente; posizioni e obiettivi condividono il riferimento
            # 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:
                # Mirare oltre il piano del varco; girare prima può far mancare il passaggio.
                lead=max(0.3,min(2,distance*0.1))
                desired = math.atan2(dy+lead*math.sin(segment), dx+lead*math.cos(segment))
            # Regolare la rotta di mantenimento con la fase pubblicata della corrente oscillante.
            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)
            # Questa euristica locale di arrivo fa avanzare solo il controllore; il server assegna
            # il punteggio effettivo.
            reached = (passage if underwater and goal.get('gate') else
                       (passed > 0 and distance < 2 if goal.get('gate') else near) and depth_ok)
            # Azzerare il timer se si perde una condizione di posizione, profondità o rotta; il
            # mantenimento deve essere continuo.
            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 comandi di alto livello per velocità longitudinale del corpo e
                # velocità di imbardata.
                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 condizione pubblicata fornisce la corrente costante.
                        # Sottrarla per puntare controcorrente prima di un varco stretto; questo
                        # esempio non stima raffiche né moto ondoso.
                        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:
                    # Ripartizione nell'ordine dei giunti del modello incluso a sei propulsori.
                    # Limitare la stima di compensazione della deriva e i comandi di ciascuno dei
                    # sei propulsori.
                    # Il percorso d'ispezione ha tre tratti di transito più lunghi.
                    # Un transito leggermente più rapido lascia tempo per il ritorno.
                    # Il ciclo subacqueo condiviso sceglie guadagni di transito e limiti di
                    # velocità specifici della prova.
                    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':
                        # Nelle curve lunghe, integrare solo la posizione può continuare ad
                        # accelerare oltre un segnale verso un palo vicino.
                        # La corrente costante richiede adattamento graduale; quella oscillante
                        # una risposta più rapida alla forza variabile.
                        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)
                    # Combinare la retroazione di velocità del corpo con la deriva stimata nel
                    # mondo, ruotata nel riferimento del corpo.
                    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 retroazione yaw/roll stabilizza l'assetto; un PD verticale e un trim
                    # fisso regolano la profondità 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']
                    # Ripartire forze e momenti secondo la geometria inclusa: propulsori
                    # orizzontali a 45 gradi e bracci in metri.
                    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)]
                    # Inviare sei forze nell'ordine nativo dei giunti, limitate qui a 35 N per
                    # propulsore.
                    sim.set_thrusters([clamp(force, 35) for force in forces], duration=0.25)
            # Solo il rilascio normale del controllo termina il ciclo; gli errori inattesi restano
            # visibili per la diagnosi.
            except RuntimeError as error:
                if str(error) != 'control_lease_ended':
                    raise
                break


if __name__ == '__main__':
    run()
