# WWW-POSIM route starter · IOES-Lab · MIT
# Aufgabe: underwater_return · BlueROV2
#
# Ziel: Unterwasser-Hinderniskurs durchfahren, zurückkehren und abschließenden Aufstieg mit Halten
# erreichen.
#
# Steuerung: Umwege für kleinen Rumpf, Integration des Geschwindigkeitsfehlers und Verteilung auf
# sechs Thruster.
#
# Start: installierte App → Übungsarena → Aufgabe/Bedingung → ROS-2-Direktsteuerung → Welt starten
# → Bewertung starten. Im vorbereiteten Python-Terminal ausführen: python main.py
#
# Website: Datei als main.py hochladen; der Server richtet Python und SDK ein. SDK: www-posim-ros
# 0.4.2. Keine Zugangsdaten im Skript speichern.
#
# Einheiten: map/world x,y,z in Metern; positives z zeigt nach oben, Unterwasserziele sind
# negativ. Quaternion-basiertes yaw/roll in Radiant; current_angle/current_swing der Mission in
# Grad. Befehle: vorwärts m/s, yaw_rate rad/s, Thrusterkraft N in nativer Gelenkreihenfolge.
#
# Zeit: ROS-Zeitstempel, Haltetimer und dt in Simulationssekunden; max_age und duration in
# Echtzeitsekunden. Während duration=0.25 aktualisiert das SDK Befehle mit 10 Hz und gibt die
# Befehlsberechtigung vor der Rückkehr frei.
#
# Gemessenes Ergebnis: 9.3347/100 über fünf gemeinsame Bedingungen; WWW-POSIM-Bewertungsversion 5.
#
# Lernregler: alle fünf Läufe prüfen und jeweils einen Parameter abstimmen. Verstärkungen,
# Torannahmen und Rumpfabstände gelten für das mitgelieferte Modell. Kommentare bewahren die
# bewertete Logik; Python-AST-Gleichheit wird geprüft.
#
# Bewertetes Original:
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/underwater_return_controller.py
# Code-Erläuterung: https://www-posim.vercel.app/course/examples.de.html
# Mit Ctrl+C oder Eingabe stoppen beenden. Session-Bereinigung gibt die Steuerung frei.
# Unerwartete SDK-Fehler sichtbar lassen; diese Datei legt keine Server-Bewertungsregeln fest.

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


# Ausgabe symmetrisch begrenzen; der Aufrufer liefert den Grenzwert in der physikalischen Einheit.
def clamp(value, limit):
    return max(-limit, min(limit, value))


# Kursfehler auf [-pi, pi) Radiant abbilden, um die kürzere Drehung zu wählen.
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# Bojenkreise um den Rumpfabstand vergrößern, dann Dijkstra auf einem kleinen Sichtbarkeitsgraphen
# anwenden.
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."""
    # Torpfosten behalten den Durchfahrtsabstand; andere Bojen nutzen den gewählten
    # Hindernisabstand.
    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
    # Gerade Graphkanten verwerfen, die einen vergrößerten Hinderniskreis schneiden.
    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


# Aufeinanderfolgende gemessene Posen an der auf X ausgerichteten Torebene interpolieren; Breite
# und Tiefe prüfen.
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


# Nach verfehlter Durchfahrt zur Eingangsseite zurückkehren; Seitenversatz und Geschwindigkeit vor
# dem nächsten Versuch ausrichten.
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


# Begrenzten Körpergeschwindigkeitsfehler mit Simulations-dt in Weltachsen integrieren;
# Kraftschätzung begrenzen.
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():
    # Installierte oder Sandbox-Sitzung finden und Steuerung übernehmen; beim Verlassen des
    # Kontexts wird die SDK-Sitzung geschlossen.
    with Session.local(control=True) as sim:
        # Ziele und Bedingungen dieses Versuchs aus der Sitzung lesen; Kurskoordinaten nicht fest
        # im Code hinterlegen.
        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'
        # Odometrie anhand des ROS-Nachrichtentyps wählen; ein fahrzeugspezifischer Topic-Name ist
        # nicht nötig.
        topic = next(name for name, kind in sim.topics.items()
                     if kind == 'nav_msgs/msg/Odometry')
        # Reglerfortschritt getrennt von der unabhängigen Serverbewertung verwalten.
        targets = mission['targets']
        # Auf dem Rückweg Zwischenziele an der Oberfläche rückwärts abfahren, statt Hindernisse
        # direkt zu queren.
        if mission['task'] in ('surface_gates','surface_slalom'):
            # Durch den bereits befahrenen Korridor zurückkehren. Eine Gerade zum Start kann durch
            # einen Torpfosten oder eine Hindernisboje führen.
            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':
            # Route durch Wegpunkte mit Sicherheitsabstand zu den veröffentlichten
            # Aufgabenhindernissen ersetzen.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'])
        if mission['task']=='underwater_return':
            # Route durch Wegpunkte mit Sicherheitsabstand zu den veröffentlichten
            # Aufgabenhindernissen ersetzen.
            targets=slalom_detours(mission['home'],targets,mission['obstacles'],
                                  hull_radius=.5,obstacle_margin=3.5)
        # Zielindex, Timer für kontinuierliches Halten und Tor-Wiederanfahrzustand zwischen
        # Aktualisierungen erhalten.
        index, holding_since = 0, None
        integral_x=integral_y=0.;previous_time=None;previous_pose=None;recovering_gate=False
        while True:
            # Eine innerhalb von 0.5 Echtzeitsekunden empfangene Messung verlangen; veraltete
            # Telemetrie löst einen SDK-Fehler aus.
            message = sim.latest(topic, max_age=0.5)
            pose, twist = message['pose']['pose'], message['twist']['twist']
            p, q = pose['position'], pose['orientation']
            # Quaternion in Gierwinkel (rad) umrechnen; die Routensteuerung ermittelt auch Roll
            # zur aufrechten Stabilisierung.
            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))
            # ROS-Simulationszeit für Integration und Haltezeit verwenden, unabhängig von der
            # Echtzeitdauer der Befehle.
            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
            # Aktuelles Ziel wählen; Positionen und Ziele nutzen denselben map/world-Rahmen.
            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:
                # Hinter die Torebene zielen; Drehen davor kann die Durchfahrt verhindern.
                lead=max(0.3,min(2,distance*0.1))
                desired = math.atan2(dy+lead*math.sin(segment), dx+lead*math.cos(segment))
            # Gewünschten Haltekurs anhand der veröffentlichten Phase der schwankenden Strömung
            # anpassen.
            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)
            # Diese lokale Ankunftsheuristik schaltet nur den Regler weiter; die tatsächliche
            # Punktzahl vergibt der Server.
            reached = (passage if underwater and goal.get('gate') else
                       (passed > 0 and distance < 2 if goal.get('gate') else near) and depth_ok)
            # Haltetimer zurücksetzen, sobald Positions-, Tiefen- oder Kursbedingungen verloren
            # gehen; Halten muss durchgehend erfolgen.
            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 nutzt übergeordnete Befehle für Körper-Vorwärtsgeschwindigkeit und
                # Gierrate.
                if not underwater:
                    if near and goal.get('hold'):
                        forward=clamp(0.3*(dx*math.cos(yaw)+dy*math.sin(yaw)),0.25)
                    else:
                        # Die veröffentlichte Bedingung liefert die konstante Strömung.
                        # Sie abziehen, um vor einem engen Tor stromaufwärts zu zielen; dieses
                        # Beispiel schätzt weder Böen noch Wellenbewegung.
                        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:
                    # Verteilung in Gelenkreihenfolge des mitgelieferten Sechs-Thruster-Modells.
                    # Driftkompensationsschätzung und Befehle für jeden der sechs Thruster
                    # begrenzen.
                    # Der Inspektionskurs hat drei längere Transitabschnitte.
                    # Etwas schnellerer Transit lässt Zeit für den Rückweg.
                    # Die gemeinsame Unterwasserschleife wählt aufgabenspezifische
                    # Transitverstärkungen und Geschwindigkeitsgrenzen.
                    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':
                        # Bei langen Drehungen kann reine Positionsintegration über eine
                        # Markierung hinaus bis in einen nahen Pfosten beschleunigen.
                        # Konstante Strömung braucht sanfte Anpassung; schwankende Strömung eine
                        # schnellere Reaktion auf ihre wechselnde Kraft.
                        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)
                    # Körpergeschwindigkeitsrückführung mit der aus Weltachsen zurück in
                    # Körperachsen gedrehten Driftschätzung kombinieren.
                    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-Rückführung stabilisiert die Lage; vertikaler PD-Regler und feste
                    # Trimmung regeln das negative z-Tiefenziel.
                    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']
                    # Kraft und Moment anhand der mitgelieferten Geometrie verteilen: horizontale
                    # Thruster bei 45 Grad, Hebelarme in Metern.
                    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)]
                    # Sechs Kräfte in nativer Gelenkreihenfolge senden; hier auf 35 N je Thruster
                    # begrenzt.
                    sim.set_thrusters([clamp(force, 35) for force in forces], duration=0.25)
            # Nur normale Steuerungsfreigabe beendet die Schleife; unerwartete Fehler bleiben zur
            # Diagnose sichtbar.
            except RuntimeError as error:
                if str(error) != 'control_lease_ended':
                    raise
                break


if __name__ == '__main__':
    run()
