//! The `mavlink.*` nodes in a generated program, against the vehicle twin
//! (`flowrs_connect::mavlink::peer`: a copter built from the same XML, speaking MAVLink 2 on
//! a TCP port as SITL does).
//!
//! One graph holds every node of the family on one link: `"mavlink.telemetry"` reads
//! `GLOBAL_POSITION_INT` (latitude and longitude in degrees, altitude in millimetres),
//! `"mavlink.heartbeat"` reads the vehicle's mode, `"mavlink.command"` sets GUIDED under an
//! approval naming its hash, and `"mavlink.setpoint"` sends a position given in ENU (ROS's
//! frame) under an approved envelope. The program is generated (`Graph::to_rust`), built
//! against flowrs-connect's `mavlink` runtime and run; the test reads what it printed and
//! what the vehicle received:
//!
//! * the telemetry is the twin's position, each value in its unit, exactly (a latitude of
//!   -353632621 degE7 is -35.3632621 degrees);
//! * the command went once, under its approval, and the vehicle acknowledged it;
//! * the setpoint arrived as NED (north 5, east -2.5, down -10 from ENU east -2.5, north 5,
//!   up 10), and only because the envelope admitted it: the same graph with no approval, and
//!   with a position outside the envelope, sends nothing and says why;
//! * the heartbeat names the vehicle's mode (GUIDED, once the command has set it).
//!
//! A second graph carries a command from ROS to the vehicle: `"ros.subscribe"` reads a
//! `geometry_msgs/PointStamped` goal on `/drone/goal` (ENU, metres, REP 103) and wires its
//! point into `"mavlink.setpoint"`. The partitioner makes it one Linux program, the ROS 2 node
//! and the MAVLink link together; the test publishes the goal over DDS on loopback and
//! requires the vehicle to receive it as the NED target, under the approved envelope. Run
//! first with no time to open DDS (`FLOWROS_OPEN_TIMEOUT_MS=0`), the same program must exit
//! at once and say why.
//!
//! The programs build under `$TMPDIR/flowrs-mavlink-nodes*`, their target in
//! `$TMPDIR/flowrs-mavlink-nodes-target`, offline, against the workspace lockfile. Every
//! run has a wall-clock limit.

use std::io::Read;
use std::path::{Path, PathBuf};
use std::process::{Command, Stdio};
use std::time::{Duration, Instant};

use flowrs_connect::mavlink::gate::{ENVELOPE_KIND, KIND};
use flowrs_connect::mavlink::nodes;
use flowrs_connect::mavlink::peer::{Peer, PeerConfig};
use flowrs_core::graph::{Graph, NodeId, WireMode};
use flowrs_instruments::grammar::Match;
use flowrs_instruments::proposal::{Phase, PlannedCommand, Proposal, ProposalTable, Target};

fn add(g: &mut Graph, ty: &str, props: &[(&str, &str)]) -> NodeId {
    let n = g.add_node(ty, 0.0, 0.0);
    for (k, v) in props {
        g.nodes
            .get_mut(&n)
            .unwrap()
            .properties
            .insert((*k).into(), (*v).into());
    }
    n
}

fn print(g: &mut Graph, from: NodeId, port: &str) {
    let p = add(g, "PrintLn", &[]);
    g.add_wire(from, port, p, "input", WireMode::Move).unwrap();
}

/// A proposal of `kind` with `lines`, approved by a person and claimed by the envelope, as
/// flowrs-server hands it to the program that executes it.
fn claimed(kind: &str, endpoint: &str, lines: &[String]) -> Proposal {
    let p = Proposal::new(
        kind,
        "mavlink nodes test".into(),
        "agent",
        "now",
        vec![Target {
            model: "mavlink".into(),
            endpoint: endpoint.into(),
            mock: false,
        }],
        Vec::new(),
        lines
            .iter()
            .map(|l| PlannedCommand {
                target: 0,
                line: l.clone(),
                tier: flowrs_instruments::Tier::Energize,
                matched: Match::Raw,
                phase: Phase::Main,
                query: false,
            })
            .collect(),
        Vec::new(),
    );
    let mut t = ProposalTable::new();
    let id = t.insert(p).unwrap().id.clone();
    let hash = t.get(&id).unwrap().hash.clone();
    t.decide(&id, &hash, true, "human", "now", None).unwrap();
    t.claim(&id, &hash).unwrap();
    t.get(&id).unwrap().clone()
}

/// A node of `ty` on `endpoint` (the link every mavlink node here shares), with `props`.
fn mav(g: &mut Graph, ty: &str, endpoint: &str, props: &[(&str, &str)]) -> NodeId {
    let n = add(g, ty, props);
    let p = &mut g.nodes.get_mut(&n).unwrap().properties;
    p.insert("link".into(), endpoint.into());
    p.insert("timeout_ms".into(), "8000".into());
    n
}

/// The graph: every mavlink node on `endpoint`, the setpoint at ENU `position`. Ordered as
/// the program runs them: the command (GUIDED) first, then the setpoint, which the vehicle
/// takes only in GUIDED.
fn graph(endpoint: &str, position: &str, cmd_approval: &str, env_approval: &str) -> Graph {
    let mut g = Graph::new();
    let trigger = add(&mut g, "BoolLiteral", &[("value", "true")]);
    let cmd = mav(
        &mut g,
        "mavlink.command",
        endpoint,
        &[
            ("system", "1"),
            ("command", "MAV_CMD_DO_SET_MODE"),
            ("param1", "1"),
            ("param2", "4"),
            ("approval", cmd_approval),
        ],
    );
    g.add_wire(trigger, "value", cmd, "trigger", WireMode::Copy)
        .unwrap();
    print(&mut g, cmd, "status");
    let pos = add(
        &mut g,
        "Control",
        &[("widget", "array"), ("value", position)],
    );
    let sp = mav(
        &mut g,
        "mavlink.setpoint",
        endpoint,
        &[
            ("system", "1"),
            ("frame", "enu"),
            ("approval", env_approval),
        ],
    );
    g.add_wire(pos, "value", sp, "position", WireMode::Move)
        .unwrap();
    print(&mut g, sp, "status");
    let tel = mav(&mut g, "mavlink.telemetry", endpoint, &[("field3", "alt")]);
    for p in ["value", "value2", "value3", "time"] {
        print(&mut g, tel, p);
    }
    let hb = mav(&mut g, "mavlink.heartbeat", endpoint, &[]);
    print(&mut g, hb, "mode");
    g
}

/// The workspace checkout.
fn checkout() -> PathBuf {
    PathBuf::from(env!("CARGO_MANIFEST_DIR"))
        .join("../..")
        .canonicalize()
        .unwrap()
}

/// Build the crate `cargo_toml` + `src/main.rs` = `src` under `$TMPDIR/<dir>`, offline
/// against the workspace lockfile; its binary `bin`.
fn build_crate(dir: &str, cargo_toml: &str, src: &str, bin: &str) -> PathBuf {
    let dir = std::env::temp_dir().join(dir);
    std::fs::create_dir_all(dir.join("src")).unwrap();
    std::fs::write(dir.join("src/main.rs"), src).unwrap();
    std::fs::write(dir.join("Cargo.toml"), cargo_toml).unwrap();
    std::fs::copy(checkout().join("Cargo.lock"), dir.join("Cargo.lock")).unwrap();
    let target = std::env::temp_dir().join("flowrs-mavlink-nodes-target");
    let out = Command::new(std::env::var("CARGO").unwrap_or_else(|_| "cargo".into()))
        .args(["build", "--quiet", "--offline", "--manifest-path"])
        .arg(dir.join("Cargo.toml"))
        .env("CARGO_TARGET_DIR", &target)
        .output()
        .unwrap();
    assert!(
        out.status.success(),
        "the generated program did not build:\n{}\n--- source ---\n{src}",
        String::from_utf8_lossy(&out.stderr)
    );
    target.join("debug").join(bin)
}

/// Build the program `src` against flowrs-connect's `mavlink` runtime.
fn build(src: &str) -> PathBuf {
    let co = checkout();
    let toml = format!(
        "[package]\nname = \"flowrs-mavlink-nodes\"\nversion = \"0.0.0\"\nedition = \"2024\"\n\
         publish = false\n\n[workspace]\n\n[dependencies]\n\
         flowrs-connect = {{ path = {:?}, features = [\"mavlink\"] }}\n",
        co.join("crates/flowrs-connect").display().to_string(),
    );
    build_crate("flowrs-mavlink-nodes", &toml, src, "flowrs-mavlink-nodes")
}

/// Run `bin` for at most `limit`, returning its exit success, stdout and stderr.
fn run(bin: &Path, limit: Duration) -> (bool, String, String) {
    run_with(bin, limit, &[], &mut || {})
}

/// [`run`] with `env`, calling `meanwhile` every 20 ms while it runs.
fn run_with(
    bin: &Path,
    limit: Duration,
    env: &[(&str, String)],
    meanwhile: &mut dyn FnMut(),
) -> (bool, String, String) {
    let mut child = Command::new(bin)
        .envs(env.iter().map(|(k, v)| (*k, v.as_str())))
        .env("FLOWRS_MAVLINK_WINDOW_MS", "8000")
        .stdout(Stdio::piped())
        .stderr(Stdio::piped())
        .spawn()
        .unwrap();
    let start = Instant::now();
    let ok = loop {
        if let Some(s) = child.try_wait().unwrap() {
            break s.success();
        }
        if start.elapsed() > limit {
            let _ = child.kill();
            let _ = child.wait();
            panic!("the generated program ran past {limit:?}");
        }
        meanwhile();
        std::thread::sleep(Duration::from_millis(20));
    };
    let (mut o, mut e) = (String::new(), String::new());
    child.stdout.take().unwrap().read_to_string(&mut o).unwrap();
    child.stderr.take().unwrap().read_to_string(&mut e).unwrap();
    (ok, o, e)
}

/// Build the generated program `src` and run it, for at most 90 s.
fn run_generated(src: &str) -> (bool, String, String) {
    run(&build(src), Duration::from_secs(90))
}

#[test]
fn every_mavlink_node_runs_against_the_vehicle_twin() {
    let d = nodes::dialect("ardupilotmega").unwrap();
    let scratch = std::env::temp_dir().join(format!("flowrs-mavlink-nodes-{}", std::process::id()));
    std::fs::create_dir_all(&scratch).unwrap();
    let peer = Peer::spawn(PeerConfig::copter(d.clone())).unwrap();
    let ep = peer.endpoint();
    let count = |name: &str, command: Option<u64>| {
        peer.log()
            .received
            .iter()
            .filter(|g| {
                g.name == name
                    && command.is_none_or(|c| {
                        g.values.get("command").and_then(serde_json::Value::as_u64) == Some(c)
                    })
            })
            .count()
    };

    // Approvals, made on the twin's endpoint: GUIDED by its hash; the default envelope.
    let guided = nodes::command_long(
        &d,
        (1, 1),
        "MAV_CMD_DO_SET_MODE",
        [1.0, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    )
    .unwrap();
    let cmd_file = scratch.join("cmd.json");
    let line = format!("MAV_CMD_DO_SET_MODE GUIDED {}", guided.hash(&ep, "").tag());
    std::fs::write(
        &cmd_file,
        serde_json::to_string(&claimed(KIND, &ep, &[line])).unwrap(),
    )
    .unwrap();
    let env = nodes::envelope(&ep, (1, 1), 20.0, 30.0, 20.0, 300.0, "").unwrap();
    let env_file = scratch.join("env.json");
    std::fs::write(
        &env_file,
        serde_json::to_string(&claimed(ENVELOPE_KIND, &ep, &[env.describe()])).unwrap(),
    )
    .unwrap();
    let (cmd_path, env_path) = (
        cmd_file.display().to_string(),
        env_file.display().to_string(),
    );

    // 1. Approved, inside the envelope: GUIDED, then the setpoint, then what the vehicle says.
    let src = graph(&ep, "-2.5,5,10", &cmd_path, &env_path)
        .to_rust()
        .unwrap();
    assert!(
        src.contains("flowrs_connect::mavlink::graph::command("),
        "{src}"
    );
    assert!(
        src.contains("flowrs_connect::mavlink::graph::setpoint("),
        "{src}"
    );
    assert!(
        src.contains("flowrs_connect::mavlink::graph::telemetry("),
        "{src}"
    );
    assert!(
        src.contains("flowrs_connect::mavlink::graph::heartbeat("),
        "{src}"
    );
    let (ok, out, err) = run_generated(&src);
    assert!(ok, "the program failed:\n{out}\n{err}");
    let lines: Vec<&str> = out.lines().collect();
    assert_eq!(lines.len(), 7, "{out}\n{err}");
    assert!(
        lines[0].starts_with("MAV_CMD_DO_SET_MODE sent seq ") && lines[0].ends_with(": ACCEPTED"),
        "{out}"
    );
    assert!(
        lines[1].starts_with("sent seq ") && lines[1].ends_with(" north 5 east -2.5 down -10 m"),
        "{out}"
    );
    // The twin's position, each value in its unit: degE7 to degrees, mm as mm.
    assert_eq!(lines[2], "-35.3632621", "{out}");
    assert_eq!(lines[3], "149.1652374", "{out}");
    assert_eq!(lines[4], "584090", "{out}");
    assert!(
        lines[5].parse::<f64>().is_ok_and(|t| t >= 0.0),
        "time_boot_ms: {out}"
    );
    // The newest heartbeat may predate the mode change; it is named either way.
    assert!(matches!(lines[6], "STABILIZE" | "GUIDED"), "{out}");
    let log = peer.log();
    assert!(log.guided);
    assert_eq!(log.target, Some([5.0, -2.5, -10.0]), "{log:?}");
    assert_eq!(count("COMMAND_LONG", Some(176)), 1, "the command went once");
    let setpoints = count("SET_POSITION_TARGET_LOCAL_NED", None);
    assert_eq!(setpoints, 1, "{log:?}");

    // 2. The same graph with no approval: the command and the setpoint send nothing and say
    //    why. The vehicle is still in GUIDED, and its heartbeat says so.
    let src = graph(&ep, "-2.5,5,10", "", "").to_rust().unwrap();
    let (ok, out, err) = run_generated(&src);
    assert!(ok, "{out}\n{err}");
    let lines: Vec<&str> = out.lines().collect();
    assert_eq!(lines.len(), 7, "{out}\n{err}");
    assert!(lines[0].starts_with("refused: no approval"), "{out}");
    assert!(lines[1].starts_with("refused: no approval"), "{out}");
    assert_eq!(lines[6], "GUIDED", "{out}");
    assert_eq!(
        count("COMMAND_LONG", Some(176)),
        1,
        "nothing unapproved went"
    );
    assert_eq!(count("SET_POSITION_TARGET_LOCAL_NED", None), setpoints);

    // 3. Approved, but outside the envelope (up 31 m of 30): refused, unsent.
    let src = graph(&ep, "0,0,31", "", &env_path).to_rust().unwrap();
    let (ok, out, err) = run_generated(&src);
    assert!(ok, "{out}\n{err}");
    let lines: Vec<&str> = out.lines().collect();
    assert!(
        lines[1].starts_with("refused: down -31 m is outside the approved -30..0 m"),
        "{out}"
    );
    assert_eq!(count("SET_POSITION_TARGET_LOCAL_NED", None), setpoints);
    assert_eq!(peer.log().target, Some([5.0, -2.5, -10.0]));
    let _ = std::fs::remove_dir_all(&scratch);
}

#[test]
fn a_ros_goal_reaches_the_vehicle_as_an_approved_setpoint() {
    use flowrs_connect::ros2::EdgeClass;
    use flowrs_connect::ros2::node::{Distro, EndpointSpec, NodeSpec};
    use flowrs_connect::ros2::rt::RosNode;
    use flowrs_core::codegen::partition::{Options, partition};

    let d = nodes::dialect("ardupilotmega").unwrap();
    let peer = Peer::spawn(PeerConfig::copter(d)).unwrap();
    let ep = peer.endpoint();
    let scratch = std::env::temp_dir().join(format!("flowrs-mavlink-goal-{}", std::process::id()));
    std::fs::create_dir_all(&scratch).unwrap();
    let env = nodes::envelope(&ep, (1, 1), 20.0, 30.0, 20.0, 300.0, "").unwrap();
    let env_file = scratch.join("env.json");
    std::fs::write(
        &env_file,
        serde_json::to_string(&claimed(ENVELOPE_KIND, &ep, &[env.describe()])).unwrap(),
    )
    .unwrap();

    // ros.subscribe (a PointStamped goal, ENU) -> mavlink.setpoint (frame enu) -> its status.
    let mut g = Graph::new();
    let goal = add(
        &mut g,
        "ros.subscribe",
        &[
            ("topic", "goal"),
            ("type", "geometry_msgs/msg/PointStamped"),
            ("field", "point"),
            ("domain", "ros(/drone)"),
        ],
    );
    let env_path = env_file.display().to_string();
    let sp = mav(
        &mut g,
        "mavlink.setpoint",
        &ep,
        &[("system", "1"), ("frame", "enu"), ("approval", &env_path)],
    );
    g.add_wire(goal, "value", sp, "position", WireMode::Move)
        .unwrap();
    print(&mut g, sp, "status");
    let co = checkout().display().to_string();
    let p = partition(
        &g,
        &Options {
            name: "flowrs_goal".into(),
            host_passes: 1,
            timeout_ms: 30_000,
            checkout: co,
            distro: Distro::Jazzy,
        },
    )
    .unwrap_or_else(|e| panic!("{e:?}"));
    let linux = p.linux().unwrap();
    assert!(
        linux.cargo_toml.contains("\"ros2\", \"mavlink\""),
        "{}",
        linux.cargo_toml
    );
    let calls = flowrs_connect::ros2::xcdr_gen::dynamic_codec_calls(&linux.source);
    assert!(calls.is_empty(), "{calls:?}");
    let bin = build_crate(
        "flowrs-mavlink-goal",
        &linux.cargo_toml,
        &linux.source,
        &linux.crate_name.replace('_', "-"),
    );

    let domain = 150 + (std::process::id() % 50) as u16;
    // A program whose ROS 2 node cannot open DDS in time exits at once with the reason, not
    // waiting for ever with nothing said (a stuck airportd did that on the build host,
    // 2026-10-30). No participant opens in no time at all.
    let start = Instant::now();
    let (ok, out, err) = run_with(
        &bin,
        Duration::from_secs(60),
        &[
            ("ROS_DOMAIN_ID", domain.to_string()),
            ("ROS_AUTOMATIC_DISCOVERY_RANGE", "LOCALHOST".into()),
            ("FLOWROS_OPEN_TIMEOUT_MS", "0".into()),
        ],
        &mut || {},
    );
    assert!(
        !ok,
        "the program ran with no time to open DDS:\n{out}\n{err}"
    );
    // main returns the error, and Rust prints it: `Error: "DDS: opening the node /drone/…`.
    assert!(
        err.contains("Error: \"DDS: opening the node /drone/"),
        "{out}\n{err}"
    );
    assert!(
        err.contains(&format!(
            "on ROS_DOMAIN_ID {domain} did not finish within 0 ms; the thread still waiting on \
             it cannot be stopped, so the program must exit"
        )),
        "{out}\n{err}"
    );
    assert!(
        start.elapsed() < Duration::from_secs(30),
        "{:?}",
        start.elapsed()
    );

    // The goal, published until the program has read one and exited.
    let mut publisher = RosNode::open_with(
        &NodeSpec {
            name: "goal_source".into(),
            namespace: "/drone".into(),
            distro: Distro::Jazzy,
            endpoints: vec![EndpointSpec {
                name: "GOAL".into(),
                node: 0,
                topic: "/drone/goal".into(),
                type_name: "geometry_msgs/msg/PointStamped".into(),
                rihs01: flowrs_iface::ros::catalog::rihs01("geometry_msgs/msg/PointStamped")
                    .unwrap(),
                publish: true,
                class: EdgeClass::auto("geometry_msgs/msg/PointStamped"),
                qos: EdgeClass::auto("geometry_msgs/msg/PointStamped").qos(),
                paths: vec!["point".into()],
                constants: String::new(),
                frame_id: "map".into(),
                field: String::new(),
                device_clock: String::new(),
            }],
        },
        domain,
        true,
    )
    .unwrap();
    let mut last = Instant::now() - Duration::from_secs(1);
    let mut sent = 0u32;
    let (ok, out, err) = run_with(
        &bin,
        Duration::from_secs(90),
        &[
            ("ROS_DOMAIN_ID", domain.to_string()),
            ("ROS_AUTOMATIC_DISCOVERY_RANGE", "LOCALHOST".into()),
            ("FLOWROS_WAIT_MATCHED_MS", "30000".into()),
        ],
        &mut || {
            if last.elapsed() >= Duration::from_millis(100) {
                let m = publisher
                    .compose(0, &[serde_json::json!([-2.5, 5.0, 10.0])])
                    .unwrap();
                publisher.publish(0, m, None).unwrap();
                sent += 1;
                last = Instant::now();
            }
        },
    );
    assert!(ok, "the program failed after {sent} goals:\n{out}\n{err}");
    assert!(
        out.lines()
            .any(|l| l.starts_with("sent seq ") && l.ends_with(" north 5 east -2.5 down -10 m")),
        "{out}\n{err}"
    );
    let got: Vec<_> = peer
        .log()
        .received
        .into_iter()
        .filter(|g| g.name == "SET_POSITION_TARGET_LOCAL_NED")
        .collect();
    assert_eq!(got.len(), 1, "{got:?}");
    let v = |k: &str| got[0].values[k].as_f64().unwrap();
    assert_eq!((v("x"), v("y"), v("z")), (5.0, -2.5, -10.0));
    let _ = std::fs::remove_dir_all(&scratch);
}