clankers 0.1.5

//! MCAP recording of node I/O, driven by `[logging] record_mcap` in
//! `clankeRS.toml`.
//!
//! When enabled, every topic declared under `[topics.input.*]` and
//! `[topics.output.*]` is taped to an MCAP file while the node runs, so the
//! same log can be fed back through `clankers replay` and
//! [`ReplayContext`](clankers_testing::ReplayContext).
//!
//! Nodes written with [`#[clankers::node]`](crate::node) get this for free:
//! the generated `main` starts a recorder when the config (or the
//! `CLANKERS_RECORD_MCAP=1` environment variable, as set by `clankers record`)
//! asks for one, and finishes the file on node exit or Ctrl-C.
//!
//! ```no_run
//! # #[tokio::main]
//! # async fn main() -> clankers_core::RobotResult<()> {
//! use clankers::recording::McapRecorder;
//! use clankers::RobotContext;
//!
//! let ctx = RobotContext::from_work_dir(".")?;
//! if let Some(recorder) = McapRecorder::start(&ctx).await? {
//!     // ... publish and subscribe as usual ...
//!     let summary = recorder.finish().await?;
//!     println!("{} messages -> {}", summary.messages, summary.path.display());
//! }
//! # Ok(())
//! # }
//! ```

use std::path::{Path, PathBuf};

use tokio::sync::broadcast::error::{RecvError, TryRecvError};
use tokio::sync::{mpsc, watch};
use tokio::task::JoinHandle;

use clankers_core::{RobotContext, RobotError, RobotResult, Timestamp};
use clankers_data::McapWriter;
use clankers_ros2::sim::SimBus;

/// Payload encoding used on the sim bus and therefore in recorded files.
const ENCODING: &str = "json";

/// Schema name used for topics declared without a `type` in `clankeRS.toml`.
const DEFAULT_SCHEMA: &str = "clankeRS/Json";

/// Resolved recording decision: where the file goes and which topics are taped.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct RecordingPlan {
    pub path: PathBuf,
    /// `(topic name, schema name)` for every recorded topic.
    pub topics: Vec<(String, String)>,
}

impl RecordingPlan {
    /// Decide whether to record for this context. `env_record` /
    /// `env_output` are the values of `CLANKERS_RECORD_MCAP` /
    /// `CLANKERS_RECORD_OUTPUT` (passed explicitly so the decision is
    /// testable). The environment wins over the config in both directions:
    /// `1`/`true` forces recording on, `0`/`false` forces it off.
    pub fn resolve(
        ctx: &RobotContext,
        env_record: Option<&str>,
        env_output: Option<&str>,
    ) -> Option<Self> {
        let enabled = match env_record.map(|s| s.trim().to_ascii_lowercase()).as_deref() {
            Some("1") | Some("true") => true,
            Some("0") | Some("false") => false,
            _ => ctx.config.logging.record_mcap,
        };
        if !enabled {
            return None;
        }

        let path = match env_output.map(str::trim) {
            Some(p) if !p.is_empty() => PathBuf::from(p),
            _ => ctx
                .resolve_path(&ctx.config.logging.output_dir)
                .join(format!(
                    "{}_{}.mcap",
                    ctx.node_name(),
                    Timestamp::now().as_nanos()
                )),
        };

        let mut topics: Vec<(String, String)> = ctx
            .config
            .topics
            .input
            .values()
            .chain(ctx.config.topics.output.values())
            .map(|t| {
                (
                    t.name.clone(),
                    t.r#type.clone().unwrap_or_else(|| DEFAULT_SCHEMA.into()),
                )
            })
            .collect();
        topics.sort();
        topics.dedup_by(|a, b| a.0 == b.0);

        Some(Self { path, topics })
    }
}

/// Summary returned by [`McapRecorder::finish`].
#[derive(Debug, Clone)]
pub struct RecordingSummary {
    pub path: PathBuf,
    pub messages: u64,
}

/// Tapes configured topics off the sim bus into an MCAP file.
///
/// Must be started before the node begins publishing (subscriptions only see
/// messages sent after they exist); [`finish`](Self::finish) drains anything
/// still queued, writes the MCAP footer, and returns the file path.
pub struct McapRecorder {
    stop_tx: watch::Sender<bool>,
    forwarders: Vec<JoinHandle<()>>,
    writer_task: JoinHandle<RobotResult<u64>>,
    path: PathBuf,
}

impl McapRecorder {
    /// Start recording if the context's config or the environment asks for it;
    /// `Ok(None)` means recording is disabled.
    pub async fn start(ctx: &RobotContext) -> RobotResult<Option<Self>> {
        let plan = RecordingPlan::resolve(
            ctx,
            std::env::var("CLANKERS_RECORD_MCAP").ok().as_deref(),
            std::env::var("CLANKERS_RECORD_OUTPUT").ok().as_deref(),
        );
        match plan {
            Some(plan) => Ok(Some(Self::start_with_plan(plan).await?)),
            None => Ok(None),
        }
    }

    /// Start recording with an explicit plan, regardless of config.
    pub async fn start_with_plan(plan: RecordingPlan) -> RobotResult<Self> {
        if let Some(parent) = plan.path.parent() {
            std::fs::create_dir_all(parent)
                .map_err(|e| RobotError::Data(format!("create {}: {e}", parent.display())))?;
        }
        let mut writer = McapWriter::create(&plan.path)?;
        if plan.topics.is_empty() {
            tracing::warn!(
                "record_mcap is enabled but no [topics.*] are configured; \
                 the recording will be empty"
            );
        }

        let (msg_tx, mut msg_rx) = mpsc::channel::<(usize, u64, Vec<u8>)>(1024);
        let (stop_tx, stop_rx) = watch::channel(false);

        let bus = SimBus::global();
        let mut forwarders = Vec::with_capacity(plan.topics.len());
        for (idx, (topic, _)) in plan.topics.iter().enumerate() {
            let mut rx = bus.subscribe(topic).await;
            let tx = msg_tx.clone();
            let mut stop = stop_rx.clone();
            let topic = topic.clone();
            forwarders.push(tokio::spawn(async move {
                loop {
                    tokio::select! {
                        biased;
                        res = rx.recv() => match res {
                            Ok((stamp, data)) => {
                                if tx.send((idx, stamp, data)).await.is_err() {
                                    break;
                                }
                            }
                            Err(RecvError::Lagged(n)) => {
                                tracing::warn!(dropped = n, topic = %topic, "recorder lagged");
                            }
                            Err(RecvError::Closed) => break,
                        },
                        _ = stop.changed() => {
                            // Drain whatever the bus already queued, then exit.
                            loop {
                                match rx.try_recv() {
                                    Ok((stamp, data)) => {
                                        if tx.send((idx, stamp, data)).await.is_err() {
                                            break;
                                        }
                                    }
                                    Err(TryRecvError::Lagged(n)) => {
                                        tracing::warn!(dropped = n, topic = %topic, "recorder lagged");
                                    }
                                    Err(_) => break,
                                }
                            }
                            break;
                        }
                    }
                }
            }));
        }
        drop(msg_tx);

        let topics = plan.topics.clone();
        let writer_task = tokio::spawn(async move {
            let mut count = 0u64;
            while let Some((idx, stamp, data)) = msg_rx.recv().await {
                let (topic, schema) = &topics[idx];
                writer.write_message(
                    topic,
                    schema,
                    ENCODING,
                    &data,
                    Timestamp::from_nanos(stamp),
                )?;
                count += 1;
            }
            writer.finish()?;
            Ok(count)
        });

        Ok(Self {
            stop_tx,
            forwarders,
            writer_task,
            path: plan.path,
        })
    }

    /// Where the recording is being written.
    pub fn path(&self) -> &Path {
        &self.path
    }

    /// Stop taping, drain queued messages, and finalize the MCAP file.
    pub async fn finish(self) -> RobotResult<RecordingSummary> {
        let _ = self.stop_tx.send(true);
        for forwarder in self.forwarders {
            let _ = forwarder.await;
        }
        let messages = self
            .writer_task
            .await
            .map_err(|e| RobotError::Data(format!("recorder task failed: {e}")))??;
        tracing::info!(path = %self.path.display(), messages, "MCAP recording finished");
        Ok(RecordingSummary {
            path: self.path,
            messages,
        })
    }
}

/// Drive a node future with recording and Ctrl-C shutdown.
///
/// This is what [`#[clankers::node]`](crate::node) expands to: start a
/// recorder when [`RecordingPlan::resolve`] says so, run the node until it
/// returns or Ctrl-C arrives, then finish the recording so the file is a
/// valid, replayable MCAP.
pub async fn run_with_recording<F>(ctx: RobotContext, node: F) -> RobotResult<()>
where
    F: std::future::Future<Output = RobotResult<()>>,
{
    let recorder = McapRecorder::start(&ctx).await?;
    if let Some(rec) = &recorder {
        tracing::info!(path = %rec.path().display(), "recording node I/O to MCAP");
    }

    let result = tokio::select! {
        r = node => r,
        _ = tokio::signal::ctrl_c() => {
            tracing::info!("Ctrl-C received; shutting down");
            Ok(())
        }
    };

    match recorder {
        // Keep the node's error if both fail; a lost recording shouldn't mask it.
        Some(rec) => result.and(rec.finish().await.map(|_| ())),
        None => result,
    }
}