svc-core/src/decoder/main.rs

//! decoder — the car's own broadcast frames to named signals on the bus.
//!
//! Every `source: z27ag` signal in contracts' `signals.json` is decoded from its
//! `can` block and published on `yukart/vehicle/<signal>`, retained, as an envelope.
//! The registry is the only place a layout is written (contracts guideline 6); this
//! service has no IDs of its own. It only reads the bus.
//!
//! Booleans are published on every change, so an indicator's flashes come through
//! edge for edge. Numbers are published on change, at most every 50 ms (20/s).
//! Every live value is also republished once a second, so a steady one (the handbrake
//! on) never looks stale to svc-gw, which judges staleness by arrival. A
//! signal whose frame stops for a second is republished once with `quality: stale`;
//! the next frame makes it ok again. The service's own state goes on
//! `yukart/system/decoder`, with a last will for when it vanishes.
//!
//! `--replay` reads a `candump -L` log on stdin and prints what would be published,
//! with the log's own times: the same code path, without a car.

mod layout;
mod socket;

use std::collections::HashMap;
use std::io::BufRead;
use std::process::ExitCode;
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::Arc;
use std::thread;
use std::time::{Duration, Instant};

use rumqttc::{Client, Event, LastWill, MqttOptions, Packet, QoS};
use serde_json::{json, Value};

const USAGE: &str = "\
usage: decoder [options]
  --broker HOST:PORT   MQTT broker                               (127.0.0.1:1883)
  --can IFACE          CAN interface to read                     (can_pcan)
  --replay             decode a candump -L log from stdin, print what would be
                       published as JSON lines, and exit (no broker, no bus)
  --list               print the decoded signals and their layouts, and exit";

const REGISTRY: &str = include_str!(concat!(env!("CARGO_MANIFEST_DIR"), "/../contracts/schema/signals.json"));
const SYSTEM_TOPIC: &str = "yukart/system/decoder";
/// Numbers are published at most this often (µs).
const NUMBER_EVERY_US: i64 = 50_000;
/// A frame silent this long makes its signals stale (µs).
const STALE_AFTER_US: i64 = 1_000_000;
/// A live value unchanged this long is republished (µs).
const REFRESH_US: i64 = 1_000_000;

pub fn now_us() -> i64 {
    let d = std::time::SystemTime::now().duration_since(std::time::UNIX_EPOCH).unwrap_or_default();
    d.as_micros() as i64
}

fn mono_ns() -> i64 {
    let mut ts: libc::timespec = unsafe { std::mem::zeroed() };
    unsafe { libc::clock_gettime(libc::CLOCK_MONOTONIC, &mut ts) };
    ts.tv_sec as i64 * 1_000_000_000 + ts.tv_nsec as i64
}

/// Units by signal name, from the registry: never authored here (contracts guideline 1).
fn units() -> HashMap<String, String> {
    let v: Value = serde_json::from_str(REGISTRY).expect("contracts signals.json is valid JSON");
    v["signals"]
        .as_object()
        .expect("signals.json has a signals object")
        .iter()
        .map(|(k, s)| (k.clone(), s["unit"].as_str().unwrap_or("").to_string()))
        .collect()
}

/// One value to publish: signal index, value, stale or not, time of the frame (µs).
struct Out {
    sig: usize,
    value: Value,
    stale: bool,
    ts_us: i64,
}

struct Sig {
    layout: layout::Layout,
    last: Option<Value>,
    last_pub_us: i64,
    pending: Option<(Value, i64)>,
    seen_us: i64,
    stale: bool,
}

/// The publishing policy, separate from where frames come from and where values go.
struct Decoder {
    sigs: Vec<Sig>,
    by_id: HashMap<u32, Vec<usize>>,
}

impl Decoder {
    fn new(layouts: Vec<layout::Layout>) -> Self {
        let mut by_id: HashMap<u32, Vec<usize>> = HashMap::new();
        for (i, l) in layouts.iter().enumerate() {
            by_id.entry(l.id).or_default().push(i);
        }
        let sigs = layouts
            .into_iter()
            .map(|layout| Sig { layout, last: None, last_pub_us: 0, pending: None, seen_us: 0, stale: false })
            .collect();
        Decoder { sigs, by_id }
    }

    fn ids(&self) -> Vec<u32> {
        let mut v: Vec<u32> = self.by_id.keys().copied().collect();
        v.sort();
        v
    }

    fn frame(&mut self, id: u32, data: &[u8], ts_us: i64, out: &mut Vec<Out>) {
        let Some(idx) = self.by_id.get(&id) else { return };
        for &i in idx {
            let s = &mut self.sigs[i];
            let Some(v) = s.layout.value(data) else { continue };
            s.seen_us = ts_us;
            let changed = s.last.as_ref() != Some(&v);
            let due = s.layout.kind == layout::Kind::Bool || ts_us - s.last_pub_us >= NUMBER_EVERY_US;
            if s.stale || (changed && due) {
                s.stale = false;
                s.pending = None;
                s.last = Some(v.clone());
                s.last_pub_us = ts_us;
                out.push(Out { sig: i, value: v, stale: false, ts_us });
            } else if changed {
                s.pending = Some((v, ts_us));
            } else {
                s.pending = None; // changed and changed back inside the window
            }
        }
    }

    /// Throttled values that are now due, and signals whose frames went silent.
    fn tick(&mut self, now_us: i64, out: &mut Vec<Out>) {
        for (i, s) in self.sigs.iter_mut().enumerate() {
            if let Some((v, ts)) = s.pending.take() {
                if now_us - s.last_pub_us >= NUMBER_EVERY_US {
                    s.last = Some(v.clone());
                    s.last_pub_us = now_us;
                    out.push(Out { sig: i, value: v, stale: false, ts_us: ts });
                } else {
                    s.pending = Some((v, ts));
                }
            }
            if !s.stale && s.seen_us > 0 && now_us - s.seen_us > STALE_AFTER_US {
                s.stale = true;
                s.pending = None;
                if let Some(v) = &s.last {
                    out.push(Out { sig: i, value: v.clone(), stale: true, ts_us: s.seen_us });
                }
            } else if !s.stale && s.pending.is_none() && now_us - s.last_pub_us >= REFRESH_US {
                if let Some(v) = &s.last {
                    s.last_pub_us = now_us;
                    out.push(Out { sig: i, value: v.clone(), stale: false, ts_us: s.seen_us });
                }
            }
        }
    }
}

fn layouts() -> Vec<layout::Layout> {
    match layout::load(REGISTRY) {
        Ok(l) => l,
        Err(e) => panic!("contracts signals.json: {e}"),
    }
}

fn replay() -> ExitCode {
    let mut d = Decoder::new(layouts());
    let mut out = Vec::new();
    let names: Vec<String> = d.sigs.iter().map(|s| s.layout.signal.clone()).collect();
    let mut last_tick = 0i64;
    for line in std::io::stdin().lock().lines() {
        let Ok(line) = line else { break };
        // (1791878450.945008) can_pcan 139#3333230044040000
        let mut f = line.split_whitespace();
        let (Some(ts), Some(_), Some(frame)) = (f.next(), f.next(), f.next()) else { continue };
        let Some(ts) = ts.trim_matches(|c| c == '(' || c == ')').parse::<f64>().ok() else { continue };
        let Some((id, hex)) = frame.split_once('#') else { continue };
        let (Ok(id), Some(data)) = (u32::from_str_radix(id, 16), (0..hex.len() / 2).map(|i| u8::from_str_radix(&hex[2 * i..2 * i + 2], 16).ok()).collect::<Option<Vec<u8>>>()) else { continue };
        let ts_us = (ts * 1e6) as i64;
        d.frame(id, &data, ts_us, &mut out);
        if ts_us - last_tick >= 10_000 {
            d.tick(ts_us, &mut out);
            last_tick = ts_us;
        }
        for o in out.drain(..) {
            let mut e = json!({ "t": o.ts_us as f64 / 1e6, "signal": names[o.sig], "value": o.value });
            if o.stale {
                e["quality"] = json!("stale");
            }
            println!("{e}");
        }
    }
    ExitCode::SUCCESS
}

fn main() -> ExitCode {
    let mut broker = "127.0.0.1:1883".to_string();
    let mut iface = "can_pcan".to_string();
    let mut args = std::env::args().skip(1);
    while let Some(a) = args.next() {
        match a.as_str() {
            "--replay" => return replay(),
            "--list" => {
                for l in layouts() {
                    println!("{:<20} 0x{:03X} byte {} {}", l.signal, l.id, l.byte, match l.length {
                        1 => format!("bit {}", l.bit),
                        n => format!("{n} bits{} ×{} +{}", if n == 16 { if l.big { " big" } else { " little" } } else { "" }, l.scale, l.offset),
                    });
                }
                return ExitCode::SUCCESS;
            }
            "--broker" => broker = args.next().unwrap_or_default(),
            "--can" => iface = args.next().unwrap_or_default(),
            _ => {
                eprintln!("decoder: unknown option {a}\n{USAGE}");
                return ExitCode::from(2);
            }
        }
    }

    let units = units();
    let mut d = Decoder::new(layouts());
    let names: Vec<String> = d.sigs.iter().map(|s| s.layout.signal.clone()).collect();
    let topics: Vec<String> = names.iter().map(|n| format!("yukart/vehicle/{n}")).collect();
    let unit_of: Vec<String> = names
        .iter()
        .map(|n| units.get(n).unwrap_or_else(|| panic!("signal {n} is not in contracts signals.json")).clone())
        .collect();

    let Some((host, port)) = broker.rsplit_once(':').and_then(|(h, p)| Some((h.to_string(), p.parse::<u16>().ok()?)))
    else {
        eprintln!("decoder: --broker must be HOST:PORT\n{USAGE}");
        return ExitCode::from(2);
    };
    let mut opts = MqttOptions::new("decoder", host, port);
    opts.set_keep_alive(Duration::from_secs(5));
    opts.set_last_will(LastWill::new(
        SYSTEM_TOPIC,
        json!({ "state": "stopping", "detail": "connection lost" }).to_string(),
        QoS::AtLeastOnce,
        true,
    ));
    let (client, mut connection) = Client::new(opts, 1024);

    // The event loop runs on its own thread and reconnects forever; publishing never waits on it.
    let connected = Arc::new(AtomicBool::new(false));
    let announce = Arc::new(AtomicBool::new(false));
    {
        let (connected, announce) = (connected.clone(), announce.clone());
        thread::spawn(move || {
            for ev in connection.iter() {
                match ev {
                    Ok(Event::Incoming(Packet::ConnAck(_))) => {
                        eprintln!("decoder: connected to the broker");
                        connected.store(true, Ordering::Relaxed);
                        announce.store(true, Ordering::Relaxed);
                    }
                    Ok(_) => {}
                    Err(e) => {
                        if connected.swap(false, Ordering::Relaxed) {
                            eprintln!("decoder: broker: {e}; retrying");
                        }
                        thread::sleep(Duration::from_secs(1));
                    }
                }
            }
        });
    }

    eprintln!("decoder {} on {iface}: {} signals from {} frame IDs, publishing to {broker}", env!("CARGO_PKG_VERSION"), names.len(), d.ids().len());
    let ids = d.ids();
    let mut sock: Option<socket::RxSocket> = None;
    let mut out = Vec::new();
    let mut last_state = String::new();
    let mut last_announce = Instant::now();
    let mut last_open_try = Instant::now() - Duration::from_secs(10);
    let mut last_tick = Instant::now();
    let mut last_frame_us = 0i64;
    loop {
        if sock.is_none() && last_open_try.elapsed() >= Duration::from_secs(2) {
            last_open_try = Instant::now();
            // Absent or down: the state says so, retried every 2 s.
            if let Ok(s) = socket::RxSocket::open(&iface, &ids, Duration::from_millis(100)) {
                eprintln!("decoder: {iface} open");
                sock = Some(s);
            }
        }
        match sock.as_ref().map(|s| s.recv()) {
            Some(Ok(Some(f))) => {
                last_frame_us = f.ts_us;
                d.frame(f.id, &f.data, f.ts_us, &mut out);
            }
            Some(Ok(None)) => {}
            Some(Err(e)) => {
                eprintln!("decoder: {iface}: {e}; reopening");
                sock = None;
            }
            None => thread::sleep(Duration::from_millis(100)),
        }
        if last_tick.elapsed() >= Duration::from_millis(10) {
            last_tick = Instant::now();
            d.tick(now_us(), &mut out);
        }
        for o in out.drain(..) {
            let mut e = json!({ "timestamp": o.ts_us, "monotonic": mono_ns(), "value": o.value, "unit": unit_of[o.sig] });
            if o.stale {
                e["quality"] = json!("stale");
            }
            // try_publish: a full queue drops a value rather than stalling the reads.
            let _ = client.try_publish(&topics[o.sig], QoS::AtMostOnce, true, e.to_string());
        }

        if connected.load(Ordering::Relaxed) {
            let state = if sock.is_none() {
                json!({ "state": "degraded", "detail": format!("{iface} absent or down") })
            } else if now_us() - last_frame_us > STALE_AFTER_US {
                json!({ "state": "degraded", "detail": "bus silent" })
            } else {
                json!({ "state": "ready" })
            }
            .to_string();
            if state != last_state || announce.swap(false, Ordering::Relaxed) || last_announce.elapsed() > Duration::from_secs(60) {
                if state != last_state {
                    eprintln!("decoder: {state}");
                }
                let _ = client.try_publish(SYSTEM_TOPIC, QoS::AtLeastOnce, true, state.clone());
                last_state = state;
                last_announce = Instant::now();
            }
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    fn dec() -> Decoder {
        Decoder::new(layout::load(REGISTRY).unwrap())
    }

    fn named<'a>(d: &'a Decoder, out: &'a [Out]) -> Vec<(&'a str, Value, bool)> {
        out.iter().map(|o| (d.sigs[o.sig].layout.signal.as_str(), o.value.clone(), o.stale)).collect()
    }

    #[test]
    fn numbers_are_throttled_and_flushed() {
        let mut d = dec();
        let id = d.sigs.iter().find(|s| s.layout.signal == "engine_rpm").unwrap().layout.id;
        let mut out = Vec::new();
        let rpm = |r: u16| {
            let mut f = [0u8; 8];
            let s = d.sigs.iter().find(|s| s.layout.signal == "engine_rpm").unwrap();
            let b = if s.layout.big { r.to_be_bytes() } else { r.to_le_bytes() };
            f[s.layout.byte..s.layout.byte + 2].copy_from_slice(&b);
            f
        };
        let (a, b) = (rpm(800), rpm(820));
        d.frame(id, &a, 1_000_000, &mut out);
        d.frame(id, &b, 1_020_000, &mut out); // 20 ms later: held back
        assert_eq!(out.iter().filter(|o| d.sigs[o.sig].layout.signal == "engine_rpm").count(), 1);
        out.clear();
        d.tick(1_060_000, &mut out);
        assert!(named(&d, &out).contains(&("engine_rpm", json!(820.0), false)));
    }

    #[test]
    fn silence_makes_stale_and_a_frame_makes_ok() {
        let mut d = dec();
        let s = d.sigs.iter().find(|s| s.layout.signal == "brake_pedal").unwrap().layout.clone();
        let mut f = [0u8; 8];
        f[s.byte] = 1 << s.bit;
        let mut out = Vec::new();
        d.frame(s.id, &f, 1_000_000, &mut out);
        d.frame(s.id, &f, 1_600_000, &mut out);
        out.clear();
        d.tick(2_000_100, &mut out); // frames still coming, a second unchanged: republished
        assert!(named(&d, &out).contains(&("brake_pedal", json!(true), false)));
        out.clear();
        d.tick(2_700_000, &mut out); // then silent past a second: stale
        assert!(named(&d, &out).contains(&("brake_pedal", json!(true), true)));
        out.clear();
        d.frame(s.id, &f, 2_800_000, &mut out);
        assert!(named(&d, &out).contains(&("brake_pedal", json!(true), false)));
    }
}