"""Fixed-trajectory performance tests.

Per (sim, num_robots, iter, trajectory_type): ready → takeoff → execute trajectory → land.

The drone takes off to TARGET_ALTITUDE_M, executes one fixed-pattern trajectory
(Circle, Figure8, Racetrack, or Line), then lands. Odometry is captured throughout
the trajectory phase and compared against an ideal reference path (generated in Python
from the same equations as fixed_trajectory_task.cpp) to measure cross-track error.

Each trajectory type is an independent full-cycle test so failures in one type do not
prevent the remaining types from running — the drone always returns to the ground at
the end of each cycle via the landing phase.
"""

import math
import statistics
import subprocess
import time
from concurrent.futures import ThreadPoolExecutor
from io import StringIO
from pathlib import Path

import pandas as pd
import pytest

from conftest import (
    ROS_DISTRO_SETUP,
    current_test_id,
    get_metrics,
    get_robot_containers,
    logger,
    ros2_exec,
)

# ── constants ─────────────────────────────────────────────────────────────

TARGET_ALTITUDE_M = 10.0
PX4_READY_TIMEOUT_S = 300.0
PX4_POLL_INTERVAL_S = 2.0
TAKEOFF_MOTION_THRESHOLD_M = 0.3   # z rise above starting z to count as "moving"
SETTLING_WINDOW_S = 1.0            # trailing window for steady-state altitude check
MAX_GT_MATCH_AGE_S = 0.1

# Cross-track tolerance is intentionally loose: we know the circle trajectory
# currently fails, so the assertion documents the failure without blocking landing.
CROSS_TRACK_TOLERANCE_M = 5.0

# Generous timeout covers the full trajectory execution at low velocity in slow sims.
TRAJ_EXEC_TIMEOUT_S = 180.0

# Odom CSV schema (ros2 topic echo --csv flattens all primitives in declaration order).
ODOM_SCHEMA = (
    ["header.stamp.sec", "header.stamp.nanosec",
     "header.frame_id", "child_frame_id",
     "pose.pose.position.x", "pose.pose.position.y", "pose.pose.position.z",
     "pose.pose.orientation.x", "pose.pose.orientation.y",
     "pose.pose.orientation.z", "pose.pose.orientation.w"]
    + [f"pose.covariance[{i}]" for i in range(36)]
    + ["twist.twist.linear.x", "twist.twist.linear.y", "twist.twist.linear.z",
       "twist.twist.angular.x", "twist.twist.angular.y", "twist.twist.angular.z"]
    + [f"twist.covariance[{i}]" for i in range(36)]
)

METRIC_UNITS = {
    "ready_duration_sys_s": "s",
    "trajectory_execution_time_sim_s": "s",
    "takeoff_duration_sim_s": "s",
    "land_duration_sim_s": "s",
    "trajectory_success": "",
    # Everything else defaults to "m".
}

# ── default trajectory parameters ────────────────────────────────────────

# These mirror the attributes consumed by fixed_trajectory_task.cpp.
# frame_id is omitted — the action server defaults to "base_link".
TRAJECTORY_CONFIGS: dict[str, dict[str, str]] = {
    "Circle": {
        "radius": "10.0",
        "velocity": "2.0",
    },
    "Figure8": {
        "length": "15.0",
        "width": "8.0",
        "height": "0.0",
        "velocity": "2.0",
        "max_acceleration": "1.0",
    },
    "Racetrack": {
        "length": "30.0",
        "width": "10.0",
        "height": "0.0",
        "velocity": "3.0",
        "turn_velocity": "1.5",
        "max_acceleration": "1.0",
    },
    "Line": {
        "length": "20.0",
        "height": "0.0",
        "velocity": "2.0",
        "max_acceleration": "1.0",
    },
}


# ── pytest hooks ──────────────────────────────────────────────────────────

def pytest_generate_tests(metafunc):
    """Parametrize tests that request `trajectory_type` from --trajectory-types."""
    if "trajectory_type" in metafunc.fixturenames:
        raw = metafunc.config.getoption("--trajectory-types")
        types = [t.strip() for t in raw.split(",") if t.strip()]
        metafunc.parametrize("trajectory_type", types, ids=types)


# ── ideal-path generators (Python mirrors of fixed_trajectory_task.cpp) ──

def _ideal_circle(radius: float) -> list[tuple[float, float, float]]:
    """Circle waypoints in base_link frame matching generate_circle() in C++."""
    pts: list[tuple[float, float, float]] = []
    pts.append((0.0, 0.0, 0.0))
    pts.append((radius, 0.0, 0.0))
    angle = 0.0
    step = 10.0 * math.pi / 180.0
    while angle < 2.0 * math.pi:
        pts.append((radius * math.cos(angle), radius * math.sin(angle), 0.0))
        angle += step
    pts.append((radius, 0.0, 0.0))
    pts.append((0.0, 0.0, 0.0))
    return pts


def _ideal_figure8(length: float, width: float, height: float) -> list[tuple[float, float, float]]:
    """Figure-8 waypoints in base_link frame matching generate_figure8() in C++."""
    n = 600
    pts: list[tuple[float, float, float]] = []
    for i in range(n - 1):
        t = 2.0 * math.pi * i / n
        x = math.cos(t) * length - length
        y = math.cos(t) * math.sin(t) * 2.0 * width
        pts.append((x, y, height))
    return pts


def _ideal_racetrack(length: float, width: float, height: float) -> list[tuple[float, float, float]]:
    """Racetrack waypoints in base_link frame matching generate_racetrack() in C++."""
    sl = length - width
    pts: list[tuple[float, float, float]] = []

    for i in range(80):
        x = sl * i / 79.0
        pts.append((x, 0.0, height))

    turn_n = 48
    for i in range(1, turn_n + 1):
        t = -math.pi / 2.0 + math.pi * i / (turn_n + 1)
        x = width / 2.0 * math.cos(t) + sl
        y = width / 2.0 * math.sin(t) + width / 2.0
        pts.append((x, y, height))

    for i in range(80):
        x = sl * (1.0 - i / 79.0)
        pts.append((x, width, height))

    for i in range(1, turn_n + 1):
        t = math.pi / 2.0 + math.pi * i / (turn_n + 1)
        x = width / 2.0 * math.cos(t)
        y = width / 2.0 * math.sin(t) + width / 2.0
        pts.append((x, y, height))

    return pts


def _ideal_line(length: float, height: float) -> list[tuple[float, float, float]]:
    """Line waypoints in base_link frame matching generate_line() in C++.

    C++ iterates `y` from 0 down to -length in steps of 0.5 and sets x = -y,
    so the drone moves along +x from 0 to length.
    """
    pts: list[tuple[float, float, float]] = []
    y = 0.0
    while y > -length:
        pts.append((-y, 0.0, height))
        y -= 0.5
    return pts


def _generate_ideal_path(traj_type: str, config: dict[str, str]) -> list[tuple[float, float, float]]:
    """Dispatch to the correct ideal-path generator."""
    if traj_type == "Circle":
        return _ideal_circle(float(config["radius"]))
    if traj_type == "Figure8":
        return _ideal_figure8(float(config["length"]), float(config["width"]),
                               float(config.get("height", "0")))
    if traj_type == "Racetrack":
        return _ideal_racetrack(float(config["length"]), float(config["width"]),
                                 float(config.get("height", "0")))
    if traj_type == "Line":
        return _ideal_line(float(config["length"]), float(config.get("height", "0")))
    return []


# ── geometry helpers ───────────────────────────────────────────────────────

def _quat_to_yaw(qx: float, qy: float, qz: float, qw: float) -> float:
    """Extract yaw (heading) from a unit quaternion."""
    return math.atan2(2.0 * (qw * qz + qx * qy),
                      1.0 - 2.0 * (qy * qy + qz * qz))


def _transform_to_world(
    base_link_pts: list[tuple[float, float, float]],
    x0: float, y0: float, z0: float, yaw0: float,
) -> list[tuple[float, float, float]]:
    """Rotate+translate base_link-frame points into the world frame.

    The trajectory controller publishes the trajectory in base_link at the
    moment of dispatch, so the reference frame origin is (x0, y0, z0) with
    heading yaw0.
    """
    cos_y = math.cos(yaw0)
    sin_y = math.sin(yaw0)
    world_pts: list[tuple[float, float, float]] = []
    for lx, ly, lz in base_link_pts:
        wx = x0 + lx * cos_y - ly * sin_y
        wy = y0 + lx * sin_y + ly * cos_y
        wz = z0 + lz
        world_pts.append((wx, wy, wz))
    return world_pts


# ── metric computations ───────────────────────────────────────────────────

def _cross_track_metrics(
    odom_rows: list[dict],
    ideal_world_pts: list[tuple[float, float, float]],
) -> dict:
    """Cross-track error statistics: mean, max, and RMSE against ideal path.

    Error is measured in the XY plane (these trajectories are flat; altitude
    hold is evaluated separately by the takeoff/hover tests).
    """
    if not odom_rows or not ideal_world_pts:
        return {}

    ideal_xy = [(px, py) for px, py, _ in ideal_world_pts]
    sq_dists: list[float] = []
    for row in odom_rows:
        ox = row["pose.pose.position.x"]
        oy = row["pose.pose.position.y"]
        sq_dists.append(min((ox - px) ** 2 + (oy - py) ** 2 for px, py in ideal_xy))

    dists = [math.sqrt(d) for d in sq_dists]
    return {
        "cross_track_error_mean_m": round(statistics.mean(dists), 3),
        "cross_track_error_max_m": round(max(dists), 3),
        "path_rmse_m": round(math.sqrt(statistics.mean(sq_dists)), 3),
    }


def _takeoff_metrics(odom: list[dict], target: float, velocity: float) -> dict:
    """Altitude error and duration from takeoff odom samples."""
    zs = [r["pose.pose.position.z"] for r in odom]
    ts = [_stamp(r) for r in odom]
    peak = max(zs)
    cutoff = ts[-1] - SETTLING_WINDOW_S
    settled = [z for z, t in zip(zs, ts) if t >= cutoff]
    out: dict = {
        "altitude_error_m": round(statistics.mean(settled) - target, 3),
        "overshoot_m": round(max(0.0, peak - target), 3),
    }
    z0 = zs[0]
    first_motion = next((i for i, z in enumerate(zs) if z > z0 + TAKEOFF_MOTION_THRESHOLD_M), None)
    first_at_target = next((i for i, z in enumerate(zs) if z >= target * 0.95), None)
    if first_motion is not None and first_at_target is not None and first_at_target > first_motion:
        out["takeoff_duration_sim_s"] = round(ts[first_at_target] - ts[first_motion], 3)
    return out


def _landing_metrics(odom: list[dict]) -> dict:
    """Final altitude and landing duration from landing odom samples."""
    zs = [r["pose.pose.position.z"] for r in odom]
    ts = [_stamp(r) for r in odom]
    out: dict = {"final_altitude_m": round(zs[-1], 3)}
    peak = max(zs)
    first_descent = next((i for i, z in enumerate(zs) if z < peak * 0.8), None)
    first_at_ground = next((i for i, z in enumerate(zs) if z < 0.5), None)
    if first_descent is not None and first_at_ground is not None and first_at_ground > first_descent:
        out["land_duration_sim_s"] = round(ts[first_at_ground] - ts[first_descent], 3)
    return out


def _record(robot_n: int, metrics_dict: dict) -> None:
    """Record per-robot scalar metrics; unit inferred from the METRIC_UNITS table."""
    m = get_metrics()
    tid = current_test_id()
    for key, value in metrics_dict.items():
        if value is None:
            continue
        unit = METRIC_UNITS.get(key, "m")
        direction = "higher_is_better" if key == "trajectory_success" else "lower_is_better"
        m.record(tid, f"robot_{robot_n}.{key}", value, unit=unit, direction=direction)


# ── CSV / subprocess helpers ───────────────────────────────────────────────

def _stamp(row: dict) -> float:
    """Sim-time seconds from a parsed odometry CSV row."""
    return row["header.stamp.sec"] + row["header.stamp.nanosec"] * 1e-9


def _start_csv_stream(
    container: str, topic: str, domain: int, setup_bash: str,
    duration_s: float, out_path: str,
) -> tuple:
    """Background `ros2 topic echo --csv` stream to out_path.

    Returns (popen, file_handle, err_file_handle). Caller must close both
    handles after the process terminates (see _finish_captures).
    """
    cmd = (
        f"source {ROS_DISTRO_SETUP} && source {setup_bash} && "
        f"export ROS_DOMAIN_ID={domain} && "
        f"timeout {int(duration_s)} ros2 topic echo --csv {topic}"
    )
    f = open(out_path, "w")
    ef = open(out_path + ".err", "w")
    try:
        proc = subprocess.Popen(
            ["docker", "exec", container, "bash", "-c", cmd],
            stdout=f, stderr=ef,
        )
    except BaseException:
        f.close()
        ef.close()
        raise
    return proc, f, ef


def _parse_csv(path: str, schema: list[str]) -> list[dict]:
    """Read ros2 `--csv` output, filtering non-CSV lines ros2 emits to stdout."""
    with open(path) as fh:
        good = [line for line in fh if line.count(",") >= len(schema) - 1]
    if not good:
        return []
    df = pd.read_csv(StringIO("".join(good)), header=None, names=schema)
    return df.to_dict("records")


def _start_captures(
    robot_container: str, setup_bash: str, domain: int, duration_s: float, tag: str,
) -> dict:
    """Start odom CSV stream for one robot. Returns a handle for _finish_captures."""
    odom_path = f"/tmp/traj_r{domain}_{tag}_odom.csv"
    odom_proc, odom_fh, odom_ef = _start_csv_stream(
        robot_container,
        f"/robot_{domain}/interface/mavros/local_position/odom",
        domain, setup_bash, duration_s, odom_path,
    )
    return {"duration_s": duration_s, "odom": (odom_proc, odom_fh, odom_ef, odom_path)}


def _finish_captures(streams: dict) -> list[dict]:
    """Terminate capture and return parsed odom rows."""
    odom_proc, odom_fh, odom_ef, odom_path = streams["odom"]
    try:
        odom_proc.terminate()
        try:
            odom_proc.wait(timeout=5)
        except subprocess.TimeoutExpired:
            odom_proc.kill()
            odom_proc.wait(timeout=5)
    finally:
        odom_fh.close()
        odom_ef.close()
    odom = _parse_csv(odom_path, ODOM_SCHEMA)
    if not odom:
        logger.warning(
            "odom capture empty. stdout=%r stderr=%r",
            Path(odom_path).read_text()[:500],
            Path(odom_path + ".err").read_text()[:500],
        )
    return odom


def _action_ok(stdout: str) -> bool:
    return "success: true" in stdout


def _action_message(stdout: str) -> str:
    for line in stdout.splitlines():
        s = line.strip()
        if s.startswith("message:"):
            return s[len("message:"):].strip().strip("'\"")
    return "\n".join(stdout.strip().splitlines()[-5:])


def _run_parallel(num_robots: int, fn) -> None:
    """Run fn(n) for n=1..num_robots concurrently."""
    if num_robots == 1:
        fn(1)
        return
    with ThreadPoolExecutor(max_workers=num_robots) as ex:
        list(ex.map(fn, range(1, num_robots + 1)))


def _build_traj_goal(traj_type: str, config: dict[str, str]) -> str:
    """Build the YAML goal string for a FixedTrajectoryTask action send_goal call."""
    attrs = ", ".join(f"{{key: {k}, value: '{v}'}}" for k, v in config.items())
    return f"{{trajectory_spec: {{type: {traj_type}, attributes: [{attrs}]}}, loop: false}}"


# ── per-robot workers ──────────────────────────────────────────────────────

def _takeoff_one_robot(n: int, robot_container: str, cfg: dict, target: float) -> None:
    velocity = 1.0  # fixed takeoff velocity for trajectory tests
    timeout = max(30.0, target / velocity + 15.0)
    streams = _start_captures(robot_container, cfg["robot_setup_bash"],
                               n, timeout + 5, "traj_takeoff")
    goal = f"{{target_altitude_m: {target}, velocity_m_s: {velocity}}}"
    result = ros2_exec(
        robot_container,
        f'ros2 action send_goal --feedback /robot_{n}/tasks/takeoff '
        f'task_msgs/action/TakeoffTask "{goal}"',
        domain_id=n, setup_bash=cfg["robot_setup_bash"],
        timeout=int(timeout + 10),
    )
    odom = _finish_captures(streams)
    if not _action_ok(result.stdout):
        pytest.fail(f"robot_{n} takeoff failed: {_action_message(result.stdout)}")
    if not odom:
        pytest.fail(f"robot_{n} takeoff: no odom samples captured")
    metrics = _takeoff_metrics(odom, target, velocity)
    _record(n, metrics)
    err = metrics.get("altitude_error_m", 0.0)
    assert abs(err) <= target * 0.1, (
        f"robot_{n} settled altitude {target + err:.2f}m differs from "
        f"target {target:.1f}m by more than 10%"
    )


def _trajectory_one_robot(
    n: int, robot_container: str, cfg: dict, traj_type: str,
) -> None:
    config = TRAJECTORY_CONFIGS[traj_type]
    streams = _start_captures(robot_container, cfg["robot_setup_bash"],
                               n, TRAJ_EXEC_TIMEOUT_S + 10, f"traj_{traj_type.lower()}")
    goal = _build_traj_goal(traj_type, config)

    # Snapshot the robot's world-frame pose immediately before dispatch so we can
    # transform the base_link ideal path to world frame for metric computation.
    odom_snap = ros2_exec(
        robot_container,
        f"timeout 5 ros2 topic echo --once --csv "
        f"/robot_{n}/interface/mavros/local_position/odom",
        domain_id=n, setup_bash=cfg["robot_setup_bash"], timeout=10,
    )
    x0, y0, z0, yaw0 = 0.0, 0.0, TARGET_ALTITUDE_M, 0.0
    for line in odom_snap.stdout.splitlines():
        parts = line.strip().split(",")
        if len(parts) >= len(ODOM_SCHEMA):
            try:
                row = dict(zip(ODOM_SCHEMA, parts))
                x0 = float(row["pose.pose.position.x"])
                y0 = float(row["pose.pose.position.y"])
                z0 = float(row["pose.pose.position.z"])
                yaw0 = _quat_to_yaw(
                    float(row["pose.pose.orientation.x"]),
                    float(row["pose.pose.orientation.y"]),
                    float(row["pose.pose.orientation.z"]),
                    float(row["pose.pose.orientation.w"]),
                )
                break
            except (ValueError, KeyError):
                pass

    t_start = time.monotonic()
    result = ros2_exec(
        robot_container,
        f'ros2 action send_goal --feedback /robot_{n}/tasks/fixed_trajectory '
        f'task_msgs/action/FixedTrajectoryTask "{goal}"',
        domain_id=n, setup_bash=cfg["robot_setup_bash"],
        timeout=int(TRAJ_EXEC_TIMEOUT_S + 15),
    )
    exec_time_s = round(time.monotonic() - t_start, 3)

    odom = _finish_captures(streams)

    success = _action_ok(result.stdout)
    _record(n, {"trajectory_success": 1.0 if success else 0.0})

    if not success:
        logger.warning("robot_%d %s trajectory did not succeed: %s",
                       n, traj_type, _action_message(result.stdout))

    if odom:
        ts = [_stamp(r) for r in odom]
        exec_sim_s = round(ts[-1] - ts[0], 3) if len(ts) > 1 else exec_time_s
        _record(n, {"trajectory_execution_time_sim_s": exec_sim_s})

        ideal_base = _generate_ideal_path(traj_type, config)
        if ideal_base:
            ideal_world = _transform_to_world(ideal_base, x0, y0, z0, yaw0)
            ct_metrics = _cross_track_metrics(odom, ideal_world)
            _record(n, ct_metrics)

            mean_err = ct_metrics.get("cross_track_error_mean_m")
            if mean_err is not None:
                assert mean_err < CROSS_TRACK_TOLERANCE_M, (
                    f"robot_{n} {traj_type}: mean cross-track error {mean_err:.2f}m "
                    f"exceeds tolerance {CROSS_TRACK_TOLERANCE_M:.1f}m"
                )
    else:
        logger.warning("robot_%d %s: no odom samples captured", n, traj_type)


def _landing_one_robot(n: int, robot_container: str, cfg: dict) -> None:
    velocity = 1.0
    timeout = max(30.0, TARGET_ALTITUDE_M / velocity + 15.0)
    streams = _start_captures(robot_container, cfg["robot_setup_bash"],
                               n, timeout + 5, "traj_land")
    goal = f"{{velocity_m_s: {velocity}}}"
    result = ros2_exec(
        robot_container,
        f'ros2 action send_goal --feedback /robot_{n}/tasks/land '
        f'task_msgs/action/LandTask "{goal}"',
        domain_id=n, setup_bash=cfg["robot_setup_bash"],
        timeout=int(timeout + 10),
    )
    odom = _finish_captures(streams)
    if not _action_ok(result.stdout):
        pytest.fail(f"robot_{n} landing failed: {_action_message(result.stdout)}")
    if not odom:
        pytest.fail(f"robot_{n} landing: no odom samples captured")
    metrics = _landing_metrics(odom)
    _record(n, metrics)
    final = metrics.get("final_altitude_m", 1.0)
    assert final < 0.5, f"robot_{n} final altitude {final:.2f}m > 0.5m"


# ── test class ─────────────────────────────────────────────────────────────

@pytest.mark.autonomy
@pytest.mark.timeout(2400)
class TestFixedTrajectory:
    """Full takeoff → fixed trajectory → land evaluation suite.

    Parametrized over trajectory_type (from --trajectory-types).
    Each trajectory type runs as an independent flight cycle so a failure on
    one type does not prevent other types from being evaluated.
    """

    @pytest.fixture(scope="session")
    def _failed_envs(self):
        return set()

    @pytest.fixture(scope="session")
    def _ready_envs(self):
        return set()

    @pytest.fixture(autouse=True)
    def _chain_guard(self, request, airstack_env, _failed_envs):
        """Skip tests whose env was poisoned by an earlier failure.

        Trajectory execution failures do NOT poison the env — landing always
        runs after a successful takeoff, keeping the drone from being stranded.
        Takeoff or landing failures do poison subsequent tests in the same env.
        """
        env_id = (airstack_env["sim"], airstack_env["num_robots"],
                  airstack_env["iteration"])
        if env_id in _failed_envs:
            pytest.skip(f"earlier fixed-trajectory test failed in {env_id}")
        yield
        rep = getattr(request.node, "_rep_call", None)
        if rep is not None and rep.failed:
            if "test_fixed_trajectory" not in request.node.name:
                _failed_envs.add(env_id)

    @pytest.mark.dependency(name="ftraj_ready")
    def test_px4_ready(self, airstack_env, trajectory_type, _ready_envs):
        """Wait until MAVROS is connected and local_position/odom is publishing.

        Runs only once per (sim, num_robots, iteration) env regardless of how
        many trajectory types are being tested.
        """
        env_id = (airstack_env["sim"], airstack_env["num_robots"],
                  airstack_env["iteration"])
        if env_id in _ready_envs:
            logger.info("px4_ready already confirmed for %s; skipping", env_id)
            return

        cfg = airstack_env["cfg"]
        robot_container = get_robot_containers(airstack_env["robot_pattern"])[0]
        num_robots = airstack_env["num_robots"]

        started = time.time()
        connected: set[int] = set()
        pending = list(range(1, num_robots + 1))
        ready_at: dict[int, float] = {}
        deadline = started + PX4_READY_TIMEOUT_S

        while pending and time.time() < deadline:
            for n in list(pending):
                if n not in connected:
                    r = ros2_exec(
                        robot_container,
                        f"timeout 5 ros2 topic echo --once --csv "
                        f"--field connected /robot_{n}/interface/mavros/state",
                        domain_id=n, setup_bash=cfg["robot_setup_bash"], timeout=10,
                    )
                    if any(line.strip() == "True" for line in r.stdout.splitlines()):
                        connected.add(n)
                    else:
                        continue

                r = ros2_exec(
                    robot_container,
                    f"timeout 5 ros2 topic echo --once "
                    f"/robot_{n}/interface/mavros/local_position/odom",
                    domain_id=n, setup_bash=cfg["robot_setup_bash"], timeout=10,
                )
                if r.returncode == 0 and "pose:" in r.stdout:
                    ready_at[n] = round(time.time() - started, 2)
                    pending.remove(n)

            if pending:
                logger.info("px4_ready: connected=%s pending=%s elapsed=%.0fs",
                            sorted(connected), pending, time.time() - started)
                time.sleep(PX4_POLL_INTERVAL_S)

        if pending:
            not_connected = [n for n in pending if n not in connected]
            if not_connected:
                pytest.fail(
                    f"robots {sorted(not_connected)} never reported MAVROS connected=True "
                    f"within {PX4_READY_TIMEOUT_S:.0f}s"
                )
            pytest.fail(
                f"robots {sorted(pending)} connected but never published "
                f"local_position/odom within {PX4_READY_TIMEOUT_S:.0f}s"
            )

        for n, dur in ready_at.items():
            _record(n, {"ready_duration_sys_s": dur})
        _ready_envs.add(env_id)

    @pytest.mark.dependency(name="ftraj_takeoff", depends=["ftraj_ready"])
    def test_takeoff(self, airstack_env, trajectory_type):
        """Take off to TARGET_ALTITUDE_M at a fixed velocity of 1 m/s."""
        cfg = airstack_env["cfg"]
        robot_container = get_robot_containers(airstack_env["robot_pattern"])[0]
        num_robots = airstack_env["num_robots"]
        _run_parallel(
            num_robots,
            lambda n: _takeoff_one_robot(n, robot_container, cfg, TARGET_ALTITUDE_M),
        )

    @pytest.mark.dependency(name="ftraj_execute", depends=["ftraj_takeoff"])
    def test_fixed_trajectory(self, airstack_env, trajectory_type):
        """Send FixedTrajectoryTask, capture odom, compute and record path deviation."""
        cfg = airstack_env["cfg"]
        robot_container = get_robot_containers(airstack_env["robot_pattern"])[0]
        num_robots = airstack_env["num_robots"]
        _run_parallel(
            num_robots,
            lambda n: _trajectory_one_robot(n, robot_container, cfg, trajectory_type),
        )

    @pytest.mark.dependency(name="ftraj_land", depends=["ftraj_takeoff"])
    def test_landing(self, airstack_env, trajectory_type):
        """Land the drone; runs even when test_fixed_trajectory fails."""
        cfg = airstack_env["cfg"]
        robot_container = get_robot_containers(airstack_env["robot_pattern"])[0]
        num_robots = airstack_env["num_robots"]
        _run_parallel(
            num_robots,
            lambda n: _landing_one_robot(n, robot_container, cfg),
        )