cu_logmon/

lib.rs

#![cfg_attr(not(feature = "std"), no_std)]

//! Lightweight Copper monitor that periodically dumps compact runtime stats over the
//! standard `debug!` / `info!` logging macros.
//!
//! This monitor is `no_std` friendly and keeps allocations to a minimum while still
//! reporting a per-second summary of the Copperlist cadence and latencies.

extern crate alloc;

use alloc::format;
use alloc::vec;
use alloc::vec::Vec;
use cu29::prelude::*;
#[cfg(all(feature = "std", debug_assertions))]
use cu29_log_runtime::{
    format_message_only, register_live_log_listener, unregister_live_log_listener,
};
use spin::Mutex;
#[cfg(all(feature = "std", debug_assertions))]
use std::collections::HashMap;

const REPORT_INTERVAL_SECS: u64 = 1;
const MAX_LATENCY_SECS: u64 = 5;

#[cfg(all(feature = "std", debug_assertions))]
fn format_timestamp(time: CuTime) -> String {
    // Render CuTime (nanoseconds from an epoch) as HH:mm:ss.xxxx where xxxx is 1e-4 s.
    let nanos = time.as_nanos();
    let total_seconds = nanos / 1_000_000_000;
    let hours = total_seconds / 3600;
    let minutes = (total_seconds / 60) % 60;
    let seconds = total_seconds % 60;
    let fractional_1e4 = (nanos % 1_000_000_000) / 100_000; // 4 fractional digits.
    format!("{hours:02}:{minutes:02}:{seconds:02}.{fractional_1e4:04}")
}

struct WindowState {
    total_copperlists: u64,
    window_copperlists: u32,
    last_report_at: Option<CuTime>,
    last_log_duration: CuDuration,
    end_to_end: CuDurationStatistics,
    per_task: Vec<CuDurationStatistics>,
}

impl WindowState {
    fn new(task_count: usize) -> Self {
        let max_sample = CuDuration::from_secs(MAX_LATENCY_SECS);
        #[cfg(target_os = "none")]
        info!("WindowState::new: init end_to_end");
        let end_to_end = CuDurationStatistics::new(max_sample);
        #[cfg(target_os = "none")]
        info!("WindowState::new: init per_task");
        #[cfg(target_os = "none")]
        info!(
            "WindowState::new: stats_size={} per_task_bytes={}",
            core::mem::size_of::<CuDurationStatistics>(),
            core::mem::size_of::<CuDurationStatistics>() * task_count
        );
        let per_task = vec![CuDurationStatistics::new(max_sample); task_count];
        #[cfg(target_os = "none")]
        info!("WindowState::new: init done");
        Self {
            total_copperlists: 0,
            window_copperlists: 0,
            last_report_at: None,
            last_log_duration: CuDuration::MIN,
            end_to_end,
            per_task,
        }
    }

    fn reset_window(&mut self, now: CuTime) {
        self.window_copperlists = 0;
        self.last_report_at = Some(now);
        self.end_to_end.reset();
        for stat in &mut self.per_task {
            stat.reset();
        }
    }
}

struct Snapshot<'a> {
    copperlist_index: u64,
    rate_whole: u64,
    rate_tenths: u64,
    e2e_p50_us: u64,
    e2e_p90_us: u64,
    e2e_p99_us: u64,
    e2e_max_us: u64,
    slowest_task: &'a str,
    slowest_task_p99_us: u64,
    log_overhead_us: u64,
}

pub struct CuLogMon {
    taskids: &'static [&'static str],
    clock: Mutex<Option<RobotClock>>,
    window: Mutex<WindowState>,
}

impl CuLogMon {
    fn compute_snapshot<'a>(&'a self, state: &WindowState, now: CuTime) -> Option<Snapshot<'a>> {
        let last_report = state.last_report_at?;

        let elapsed = now - last_report;
        let elapsed_ns = elapsed.as_nanos();

        if elapsed_ns < CuDuration::from_secs(REPORT_INTERVAL_SECS).as_nanos() {
            return None;
        }

        let rate_x10 = if elapsed_ns > 0 {
            (state.window_copperlists as u64 * 10 * 1_000_000_000u64) / elapsed_ns
        } else {
            0
        };

        let slowest = find_slowest_task(&state.per_task);
        let (slowest_task, slowest_task_p99_us) = slowest
            .map(|(idx, dur)| {
                let name = self.taskids.get(idx).copied().unwrap_or("<?>");
                (name, dur.as_micros())
            })
            .unwrap_or(("none", 0));

        let e2e_p50 = state.end_to_end.percentile(0.5).as_micros();
        let e2e_p90 = state.end_to_end.percentile(0.9).as_micros();
        let e2e_p99 = state.end_to_end.percentile(0.99).as_micros();
        // Max can skew low if a bucket underflows; keep it at least as high as p99.
        let e2e_max = state.end_to_end.max().as_micros().max(e2e_p99);

        Some(Snapshot {
            copperlist_index: state.total_copperlists,
            rate_whole: rate_x10 / 10,
            rate_tenths: rate_x10 % 10,
            e2e_p50_us: e2e_p50,
            e2e_p90_us: e2e_p90,
            e2e_p99_us: e2e_p99,
            e2e_max_us: e2e_max,
            slowest_task,
            slowest_task_p99_us,
            log_overhead_us: state.last_log_duration.as_micros(),
        })
    }
}

fn task_duration(meta: &CuMsgMetadata) -> Option<CuDuration> {
    let start = Option::<CuTime>::from(meta.process_time.start)?;
    let end = Option::<CuTime>::from(meta.process_time.end)?;
    (end >= start).then_some(end - start)
}

fn end_to_end_latency(msgs: &[&CuMsgMetadata]) -> Option<CuDuration> {
    let start = msgs
        .first()
        .and_then(|m| Option::<CuTime>::from(m.process_time.start));
    let end = msgs
        .last()
        .and_then(|m| Option::<CuTime>::from(m.process_time.end));
    match (start, end) {
        (Some(s), Some(e)) if e >= s => Some(e - s),
        _ => None,
    }
}

fn find_slowest_task(per_task: &[CuDurationStatistics]) -> Option<(usize, CuDuration)> {
    per_task
        .iter()
        .enumerate()
        .filter_map(|(idx, stats)| {
            if stats.is_empty() {
                None
            } else {
                Some((idx, stats.percentile(0.99)))
            }
        })
        .max_by_key(|(_, dur)| dur.as_nanos())
}

fn task_state_label(state: &CuTaskState) -> &'static str {
    match state {
        CuTaskState::Start => "start",
        CuTaskState::Preprocess => "pre",
        CuTaskState::Process => "process",
        CuTaskState::Postprocess => "post",
        CuTaskState::Stop => "stop",
    }
}

impl CuMonitor for CuLogMon {
    fn new(_config: &CuConfig, taskids: &'static [&'static str]) -> CuResult<Self> {
        #[cfg(target_os = "none")]
        info!("CuLogMon::new: task_count={}", taskids.len());
        let window = WindowState::new(taskids.len());
        #[cfg(target_os = "none")]
        info!("CuLogMon::new: window ready");
        Ok(Self {
            taskids,
            clock: Mutex::new(None),
            window: Mutex::new(window),
        })
    }

    fn start(&mut self, clock: &RobotClock) -> CuResult<()> {
        *self.clock.lock() = Some(clock.clone());
        let mut window = self.window.lock();
        window.last_report_at = Some(clock.recent());
        info!("cu_logmon started ({} tasks)", self.taskids.len());

        // Also listen to structured logs and print them with color.
        #[cfg(all(feature = "std", debug_assertions))]
        register_live_log_listener(|entry, format_str, param_names| {
            const PARAM_COLOR: &str = "\x1b[36m"; // cyan
            const RESET: &str = "\x1b[0m";

            let params: Vec<String> = entry.params.iter().map(|v| v.to_string()).collect();
            let colored_params: Vec<String> = params
                .iter()
                .map(|v| format!("{PARAM_COLOR}{v}{RESET}"))
                .collect();
            let colored_named: HashMap<String, String> = param_names
                .iter()
                .zip(params.iter())
                .map(|(k, v)| (k.to_string(), format!("{PARAM_COLOR}{v}{RESET}")))
                .collect();

            if let Ok(msg) =
                format_message_only(format_str, colored_params.as_slice(), &colored_named)
            {
                let level_color = match entry.level {
                    CuLogLevel::Debug => "\x1b[32m",   // green
                    CuLogLevel::Info => "\x1b[90m",    // gray
                    CuLogLevel::Warning => "\x1b[93m", // yellow
                    CuLogLevel::Error => "\x1b[91m",   // red
                    CuLogLevel::Critical => "\x1b[91m",
                };
                let ts_color = "\x1b[34m";
                let ts = format_timestamp(entry.time);
                println!(
                    "{ts_color}{ts}{reset} {level_color}[{:?}]{reset} {msg}",
                    entry.level,
                    ts = ts,
                    ts_color = ts_color,
                    level_color = level_color,
                    reset = RESET,
                    msg = msg
                );
            }
        });
        Ok(())
    }

    fn process_copperlist(&self, msgs: &[&CuMsgMetadata]) -> CuResult<()> {
        let Some(clock) = self.clock.lock().clone() else {
            return Ok(());
        };

        let call_start = clock.recent();

        let snapshot = {
            let mut window = self.window.lock();
            window.last_report_at.get_or_insert(call_start);

            window.total_copperlists = window.total_copperlists.saturating_add(1);
            window.window_copperlists = window.window_copperlists.saturating_add(1);

            if let Some(latency) = end_to_end_latency(msgs) {
                window.end_to_end.record(latency);
            }

            for (idx, meta) in msgs.iter().enumerate() {
                if let Some(task_stat) = window.per_task.get_mut(idx)
                    && let Some(duration) = task_duration(meta)
                {
                    task_stat.record(duration);
                }
            }

            let snapshot = self.compute_snapshot(&window, call_start);
            if snapshot.is_some() {
                window.reset_window(call_start);
            }
            snapshot
        };

        if let Some(snapshot) = snapshot {
            let log_start = clock.recent();
            let use_color = cfg!(feature = "color_log");
            let base = format!(
                "[CL {}] rate {}.{} Hz | slowest {} {}us | e2e p50 {}us p90 {}us p99 {}us max {}us | log_overhead {}us",
                snapshot.copperlist_index,
                snapshot.rate_whole,
                snapshot.rate_tenths,
                snapshot.slowest_task,
                snapshot.slowest_task_p99_us,
                snapshot.e2e_p50_us,
                snapshot.e2e_p90_us,
                snapshot.e2e_p99_us,
                snapshot.e2e_max_us,
                snapshot.log_overhead_us,
            );
            if use_color {
                // Colored labels for readability (values stay uncolored).
                const CL_COLOR: &str = "\x1b[94m"; // blue
                const LABEL_COLOR: &str = "\x1b[92m"; // green for main labels
                const SUBLABEL_COLOR: &str = "\x1b[93m"; // yellow for sublabels
                const TASK_NAME_COLOR: &str = "\x1b[38;5;208m"; // orange for task name
                const RESET: &str = "\x1b[0m";
                let colored = format!(
                    "[{cl_color}CL {cl}{reset}] {label}rate{reset} {rate_whole}.{rate_tenths} Hz | {label}slowest{reset} {task_color}{slow_task}{reset} {slow_p99}us | {label}e2e{reset} {sublabel}p50{reset} {p50}us {sublabel}p90{reset} {p90}us {sublabel}p99{reset} {p99}us {sublabel}max{reset} {max}us | {label}log_overhead{reset} {log_overhead}us",
                    cl_color = CL_COLOR,
                    label = LABEL_COLOR,
                    sublabel = SUBLABEL_COLOR,
                    task_color = TASK_NAME_COLOR,
                    reset = RESET,
                    cl = snapshot.copperlist_index,
                    rate_whole = snapshot.rate_whole,
                    rate_tenths = snapshot.rate_tenths,
                    slow_task = snapshot.slowest_task,
                    slow_p99 = snapshot.slowest_task_p99_us,
                    p50 = snapshot.e2e_p50_us,
                    p90 = snapshot.e2e_p90_us,
                    p99 = snapshot.e2e_p99_us,
                    max = snapshot.e2e_max_us,
                    log_overhead = snapshot.log_overhead_us,
                );
                info!("{}", &colored);
            } else {
                info!("{}", &base);
            }
            let log_end = clock.recent();
            self.window.lock().last_log_duration = log_end - log_start;
        }

        Ok(())
    }

    fn process_error(&self, taskid: usize, step: CuTaskState, error: &CuError) -> Decision {
        let task_name = self.taskids.get(taskid).copied().unwrap_or("<??>");
        error!(
            "Task {} @ {}: Error: {}.",
            task_name,
            task_state_label(&step),
            error,
        );
        Decision::Ignore
    }

    fn stop(&mut self, _clock: &RobotClock) -> CuResult<()> {
        #[cfg(all(feature = "std", debug_assertions))]
        unregister_live_log_listener();
        Ok(())
    }
}