use crate::events::{Command, Event, EventKind, Frame, Meta, ZoneRange};
use cumulus3d_core::io::ply::PointCloud;
use std::collections::{HashMap, VecDeque};
use std::panic::{catch_unwind, AssertUnwindSafe};
use std::sync::atomic::{AtomicUsize, Ordering};
use std::sync::mpsc::{channel, Receiver, Sender};
use std::sync::{Arc, Condvar, Mutex};
use std::thread::JoinHandle;
use std::time::SystemTime;
pub trait Reducer {
type Input;
fn step(&mut self, input: Self::Input) -> Vec<Event>;
fn prelude(&mut self, _input: &Self::Input) -> Vec<Event> {
Vec::new()
}
fn poll(&mut self) -> Vec<Event>;
fn command(&mut self, c: Command) -> Vec<Event>;
fn finish(&mut self) -> Vec<Event> {
self.poll()
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
pub enum QueuePolicy {
#[default]
KeepAll,
LatestPerKey,
LatestOnly,
}
#[derive(Clone, Debug, Default)]
pub struct Summary {
pub events: usize,
pub by_kind: HashMap<EventKind, usize>,
pub hook_calls: usize,
pub hook_panics: usize,
pub superseded: usize,
}
type Hook = Box<dyn FnMut(&Event) + Send + 'static>;
type HookList = Arc<Mutex<Vec<Hook>>>;
type LaneKey = Option<EventKind>;
struct Shared {
subscribers: Mutex<Vec<Sender<Event>>>,
hook_calls: AtomicUsize,
hook_panics: AtomicUsize,
superseded: AtomicUsize,
}
impl Shared {
fn broadcast(&self, ev: &Event) {
let mut subs = self.subscribers.lock().unwrap_or_else(|p| p.into_inner());
subs.retain(|tx| tx.send(ev.clone()).is_ok());
}
fn run_hooks(&self, hooks: &HookList, ev: &Event) {
let mut hooks = hooks.lock().unwrap_or_else(|p| p.into_inner());
for hook in hooks.iter_mut() {
self.hook_calls.fetch_add(1, Ordering::Relaxed);
let r = catch_unwind(AssertUnwindSafe(|| hook(ev)));
if let Err(payload) = r {
self.hook_panics.fetch_add(1, Ordering::Relaxed);
let what = if let Some(s) = payload.downcast_ref::<&str>() {
(*s).to_string()
} else if let Some(s) = payload.downcast_ref::<String>() {
s.clone()
} else {
"알 수 없는 패닉".to_string()
};
let err = Event::Error {
meta: Meta { version: ev.meta().version, at: SystemTime::now() },
message: format!("{:?} 훅 패닉: {what}", ev.kind()),
};
self.broadcast(&err);
}
}
}
}
struct LaneQueue {
items: VecDeque<Event>,
busy: bool,
closed: bool,
}
struct LaneState {
q: Mutex<LaneQueue>,
cv: Condvar,
}
struct Lane {
hooks: HookList,
state: Arc<LaneState>,
worker: Option<JoinHandle<()>>,
}
impl Lane {
fn new() -> Self {
Lane {
hooks: Arc::new(Mutex::new(Vec::new())),
state: Arc::new(LaneState {
q: Mutex::new(LaneQueue { items: VecDeque::new(), busy: false, closed: false }),
cv: Condvar::new(),
}),
worker: None,
}
}
fn ensure_worker(&mut self, name: String, shared: &Arc<Shared>) {
if self.worker.is_some() {
return;
}
let state = Arc::clone(&self.state);
let hooks = Arc::clone(&self.hooks);
let shared = Arc::clone(shared);
let h = std::thread::Builder::new()
.name(name)
.spawn(move || loop {
let ev = {
let mut q = state.q.lock().unwrap_or_else(|p| p.into_inner());
loop {
if let Some(ev) = q.items.pop_front() {
q.busy = true;
break Some(ev);
}
if q.closed {
break None;
}
q = state.cv.wait(q).unwrap_or_else(|p| p.into_inner());
}
};
let Some(ev) = ev else { return };
shared.run_hooks(&hooks, &ev);
let mut q = state.q.lock().unwrap_or_else(|p| p.into_inner());
q.busy = false;
state.cv.notify_all();
})
.expect("훅 워커 스레드 생성 실패");
self.worker = Some(h);
}
fn enqueue(&self, ev: Event, policy: QueuePolicy) -> usize {
let mut q = self.state.q.lock().unwrap_or_else(|p| p.into_inner());
let before = q.items.len();
match policy {
QueuePolicy::KeepAll => {}
QueuePolicy::LatestOnly => {
let k = ev.kind();
q.items.retain(|e| e.kind() != k);
}
QueuePolicy::LatestPerKey => {
let k = (ev.kind(), supersede_key(&ev));
q.items.retain(|e| (e.kind(), supersede_key(e)) != k);
}
}
let dropped = before - q.items.len();
q.items.push_back(ev);
self.state.cv.notify_all();
dropped
}
fn wait_idle(&self) {
if self.worker.is_none() {
return;
}
let mut q = self.state.q.lock().unwrap_or_else(|p| p.into_inner());
while !q.items.is_empty() || q.busy {
q = self.state.cv.wait(q).unwrap_or_else(|p| p.into_inner());
}
}
fn shutdown(&mut self) {
{
let mut q = self.state.q.lock().unwrap_or_else(|p| p.into_inner());
q.closed = true;
self.state.cv.notify_all();
}
if let Some(h) = self.worker.take() {
let _ = h.join();
}
}
}
fn supersede_key(ev: &Event) -> Option<usize> {
match ev {
Event::ZoneArrived { range, .. }
| Event::ZonePreview { range, .. }
| Event::ZoneRefinedPose { range, .. }
| Event::ZoneRefined { range, .. } => Some(range.zone),
Event::BaseAdopted { zone, .. }
| Event::Reanchored { zone, .. }
| Event::RefineStarted { zone, .. }
| Event::ZoneAdjusted { zone, .. }
| Event::ZoneInvalidated { zone, .. } => Some(*zone),
Event::Snapshot { index, .. } => Some(*index),
Event::PositionDone { position, .. } | Event::FrameIngested { position, .. } => Some(*position),
_ => None,
}
}
struct Lanes {
map: HashMap<LaneKey, Lane>,
}
impl Lanes {
fn shutdown(&mut self) {
for lane in self.map.values_mut() {
lane.shutdown();
}
}
}
impl Drop for Lanes {
fn drop(&mut self) {
self.shutdown();
}
}
type Deriver = Box<dyn FnMut(&Event) -> Vec<Event> + Send + 'static>;
pub struct Pipeline<R: Reducer> {
reducer: R,
derivers: Vec<Deriver>,
lanes: Lanes,
policies: HashMap<EventKind, QueuePolicy>,
shared: Arc<Shared>,
sync: bool,
events: usize,
by_kind: HashMap<EventKind, usize>,
}
impl<R: Reducer> Pipeline<R> {
pub fn new(reducer: R) -> Self {
Pipeline {
reducer,
derivers: Vec::new(),
lanes: Lanes { map: HashMap::new() },
policies: HashMap::new(),
shared: Arc::new(Shared {
subscribers: Mutex::new(Vec::new()),
hook_calls: AtomicUsize::new(0),
hook_panics: AtomicUsize::new(0),
superseded: AtomicUsize::new(0),
}),
sync: false,
events: 0,
by_kind: HashMap::new(),
}
}
pub fn sync(mut self, on: bool) -> Self {
if on && !self.sync {
self.flush();
}
self.sync = on;
self
}
pub fn policy(mut self, kind: EventKind, p: QueuePolicy) -> Self {
self.policies.insert(kind, p);
self
}
pub fn on<F>(mut self, kind: EventKind, f: F) -> Self
where
F: FnMut(&Event) + Send + 'static,
{
self.add_hook(Some(kind), Box::new(f));
self
}
pub fn on_any<F>(mut self, f: F) -> Self
where
F: FnMut(&Event) + Send + 'static,
{
self.add_hook(None, Box::new(f));
self
}
pub fn on_frame<F>(self, mut f: F) -> Self
where
F: FnMut(&Arc<Frame>) + Send + 'static,
{
self.on(EventKind::FrameDecoded, move |e| {
if let Event::FrameDecoded { frame, .. } = e {
f(frame)
}
})
}
pub fn on_zone_preview<F>(self, mut f: F) -> Self
where
F: FnMut(ZoneRange, &Arc<PointCloud>) + Send + 'static,
{
self.on(EventKind::ZonePreview, move |e| {
if let Event::ZonePreview { range, cloud, .. } = e {
f(*range, cloud)
}
})
}
pub fn on_zone_refined<F>(self, mut f: F) -> Self
where
F: FnMut(ZoneRange, &Arc<PointCloud>) + Send + 'static,
{
self.on(EventKind::ZoneRefined, move |e| {
if let Event::ZoneRefined { range, cloud, .. } = e {
f(*range, cloud)
}
})
}
pub fn on_snapshot<F>(self, mut f: F) -> Self
where
F: FnMut(usize, &Arc<PointCloud>) + Send + 'static,
{
self.on(EventKind::Snapshot, move |e| {
if let Event::Snapshot { index, cloud, .. } = e {
f(*index, cloud)
}
})
}
pub fn on_position_done<F>(self, mut f: F) -> Self
where
F: FnMut(usize, usize, usize) + Send + 'static,
{
self.on(EventKind::PositionDone, move |e| {
if let Event::PositionDone { position, registered, expected, .. } = e {
f(*position, *registered, *expected)
}
})
}
pub fn on_message<F>(self, f: F) -> Self
where
F: FnMut(EventKind, &str) + Send + 'static,
{
let f = Arc::new(Mutex::new(f));
let g = Arc::clone(&f);
self.on(EventKind::Warning, move |e| {
if let Event::Warning { message, .. } = e {
(f.lock().unwrap_or_else(|p| p.into_inner()))(EventKind::Warning, message)
}
})
.on(EventKind::Error, move |e| {
if let Event::Error { message, .. } = e {
(g.lock().unwrap_or_else(|p| p.into_inner()))(EventKind::Error, message)
}
})
}
pub fn derive<F>(mut self, f: F) -> Self
where
F: FnMut(&Event) -> Vec<Event> + Send + 'static,
{
self.derivers.push(Box::new(f));
self
}
fn add_hook(&mut self, key: LaneKey, hook: Hook) {
let lane = self.lanes.map.entry(key).or_insert_with(Lane::new);
lane.hooks.lock().unwrap_or_else(|p| p.into_inner()).push(hook);
}
pub fn subscribe(&self) -> Receiver<Event> {
let (tx, rx) = channel();
self.shared.subscribers.lock().unwrap_or_else(|p| p.into_inner()).push(tx);
rx
}
pub fn push(&mut self, input: R::Input) -> usize {
let pre = self.reducer.prelude(&input);
let n = self.dispatch(pre);
let evs = self.reducer.step(input);
n + self.dispatch(evs)
}
pub fn poll(&mut self) -> usize {
let evs = self.reducer.poll();
self.dispatch(evs)
}
pub fn command(&mut self, c: Command) -> usize {
let evs = self.reducer.command(c);
self.dispatch(evs)
}
pub fn flush(&mut self) {
for lane in self.lanes.map.values() {
lane.wait_idle();
}
}
pub fn reducer(&self) -> &R {
&self.reducer
}
pub fn reducer_mut(&mut self) -> &mut R {
&mut self.reducer
}
pub fn finish(mut self) -> (R, Summary) {
let evs = self.reducer.finish();
self.dispatch(evs);
self.lanes.shutdown();
self.shared.subscribers.lock().unwrap_or_else(|p| p.into_inner()).clear();
let summary = Summary {
events: self.events,
by_kind: std::mem::take(&mut self.by_kind),
hook_calls: self.shared.hook_calls.load(Ordering::Relaxed),
hook_panics: self.shared.hook_panics.load(Ordering::Relaxed),
superseded: self.shared.superseded.load(Ordering::Relaxed),
};
let Pipeline { reducer, .. } = self;
(reducer, summary)
}
fn dispatch(&mut self, evs: Vec<Event>) -> usize {
let mut n = 0;
let mut queue: VecDeque<Event> = evs.into();
while let Some(ev) = queue.pop_front() {
n += 1;
if !self.derivers.is_empty() {
let mut derived = Vec::new();
for d in self.derivers.iter_mut() {
match catch_unwind(AssertUnwindSafe(|| d(&ev))) {
Ok(v) => derived.extend(v),
Err(_) => {
self.shared.hook_panics.fetch_add(1, Ordering::Relaxed);
let meta = ev.meta().clone();
self.shared.broadcast(&Event::Error { meta, message: "derive: panic".to_string() });
}
}
}
for d in derived.into_iter().rev() {
queue.push_front(d);
}
}
self.events += 1;
*self.by_kind.entry(ev.kind()).or_insert(0) += 1;
self.shared.broadcast(&ev);
let kind = ev.kind();
if self.sync {
for key in [Some(kind), None] {
if let Some(lane) = self.lanes.map.get(&key) {
self.shared.run_hooks(&lane.hooks, &ev);
}
}
continue;
}
let policy = self.policies.get(&kind).copied().unwrap_or_default();
let has_kind = self.lanes.map.contains_key(&Some(kind));
let has_any = self.lanes.map.contains_key(&None);
let mut pending = Some(ev);
for (key, present, last) in [(Some(kind), has_kind, !has_any), (None, has_any, true)] {
if !present {
continue;
}
let ev = if last { pending.take() } else { pending.clone() };
let Some(ev) = ev else { break };
let name = match key {
Some(k) => format!("hook-{k:?}"),
None => "hook-any".to_string(),
};
let lane = self.lanes.map.get_mut(&key).expect("레인 존재 확인됨");
lane.ensure_worker(name, &self.shared);
let dropped = lane.enqueue(ev, policy);
self.shared.superseded.fetch_add(dropped, Ordering::Relaxed);
}
}
n
}
}
#[cfg(test)]
mod tests {
use super::*;
use cumulus3d_core::Reconstruction;
use std::sync::mpsc::RecvTimeoutError;
use std::time::Duration;
fn meta(v: u64) -> Meta {
Meta { version: v, at: SystemTime::now() }
}
fn range(z: usize) -> ZoneRange {
ZoneRange { zone: z, lo: z * 10, hi: z * 10 + 10 }
}
fn cloud() -> Arc<PointCloud> {
Arc::new(PointCloud::default())
}
fn preview(v: u64, z: usize) -> Event {
Event::ZonePreview {
meta: meta(v),
range: range(z),
cloud: cloud(),
model: Arc::new(Reconstruction::default()),
dense: true,
frame: "gps".to_string(),
}
}
fn refined(v: u64, z: usize) -> Event {
Event::ZoneRefined { meta: meta(v), range: range(z), cloud: cloud(), model: Arc::new(Reconstruction::default()), dense: true }
}
#[derive(Default)]
struct Fake {
v: u64,
zones: Vec<usize>,
commands: Vec<String>,
}
impl Reducer for Fake {
type Input = usize;
fn step(&mut self, n: usize) -> Vec<Event> {
self.v += 1;
let mut out = vec![Event::PositionDone { meta: meta(self.v), position: n, registered: n, expected: n }];
if n.is_multiple_of(2) {
out.push(preview(self.v, n / 2));
self.zones.push(n / 2);
}
out
}
fn poll(&mut self) -> Vec<Event> {
Vec::new()
}
fn command(&mut self, c: Command) -> Vec<Event> {
self.commands.push(format!("{c:?}"));
match c {
Command::InvalidateZone(z) => vec![Event::Warning { meta: meta(self.v), message: format!("invalidate {z}") }],
Command::ResetFrom(p) => vec![Event::Warning { meta: meta(self.v), message: format!("reset {p}") }],
}
}
fn finish(&mut self) -> Vec<Event> {
self.zones.drain(..).map(|z| refined(self.v, z)).collect()
}
}
fn log<T>() -> Arc<Mutex<Vec<T>>> {
Arc::new(Mutex::new(Vec::new()))
}
#[test]
fn pipeline_sync_hook_order() {
let l = log();
let (a, b, c) = (Arc::clone(&l), Arc::clone(&l), Arc::clone(&l));
let mut p = Pipeline::new(Fake::default())
.sync(true)
.on_any(move |e| a.lock().unwrap().push(format!("any:{:?}", e.kind())))
.on_position_done(move |pos, _, _| b.lock().unwrap().push(format!("pos:{pos}")))
.on_zone_preview(move |r, _| c.lock().unwrap().push(format!("prev:{}", r.zone)));
p.push(1);
p.push(2);
let (_, s) = p.finish();
let got = l.lock().unwrap().clone();
assert_eq!(got, vec!["pos:1", "any:PositionDone", "pos:2", "any:PositionDone", "prev:1", "any:ZonePreview", "any:ZoneRefined"]);
assert_eq!(s.events, 4);
assert_eq!(s.by_kind[&EventKind::PositionDone], 2);
}
#[test]
fn pipeline_async_order_within_kind() {
let l = log::<String>();
let any = log::<String>();
let (a, b) = (Arc::clone(&l), Arc::clone(&any));
let mut p = Pipeline::new(Fake::default())
.on_position_done(move |pos, _, _| {
if pos.is_multiple_of(7) {
std::thread::sleep(Duration::from_millis(1));
}
a.lock().unwrap().push(pos.to_string())
})
.on_any(move |e| b.lock().unwrap().push(format!("{:?}", e.kind())));
for i in 0..200 {
p.push(i);
}
let (_, s) = p.finish();
let got: Vec<usize> = l.lock().unwrap().iter().map(|s| s.parse().unwrap()).collect();
assert_eq!(got, (0..200).collect::<Vec<_>>());
assert_eq!(any.lock().unwrap().len(), s.events);
assert_eq!(s.superseded, 0);
}
#[test]
fn pipeline_latest_per_key_policy() {
let gate = Arc::new((Mutex::new(false), Condvar::new()));
let seen = log();
let (g, s2) = (Arc::clone(&gate), Arc::clone(&seen));
struct Previews(u64);
impl Reducer for Previews {
type Input = (usize, u64);
fn step(&mut self, (z, v): (usize, u64)) -> Vec<Event> {
self.0 += 1;
vec![preview(v, z)]
}
fn poll(&mut self) -> Vec<Event> {
Vec::new()
}
fn command(&mut self, _: Command) -> Vec<Event> {
Vec::new()
}
}
let mut p =
Pipeline::new(Previews(0)).policy(EventKind::ZonePreview, QueuePolicy::LatestPerKey).on(EventKind::ZonePreview, move |e| {
let (m, cv) = &*g;
let mut open = m.lock().unwrap();
while !*open {
open = cv.wait(open).unwrap();
}
if let Event::ZonePreview { meta, range, .. } = e {
s2.lock().unwrap().push(format!("{}@{}", range.zone, meta.version));
}
});
p.push((9, 0));
std::thread::sleep(Duration::from_millis(50));
for v in 1..=5 {
p.push((1, v));
p.push((2, v));
}
{
let (m, cv) = &*gate;
*m.lock().unwrap() = true;
cv.notify_all();
}
let (_, s) = p.finish();
assert_eq!(seen.lock().unwrap().clone(), vec!["9@0", "1@5", "2@5"]);
assert_eq!(s.superseded, 8);
assert_eq!(s.events, 11);
}
#[test]
fn pipeline_panic_isolated() {
let l = log();
let a = Arc::clone(&l);
let mut p = Pipeline::new(Fake::default())
.on_position_done(|pos, _, _| {
if pos == 1 {
panic!("boom");
}
})
.on_position_done(move |pos, _, _| a.lock().unwrap().push(pos));
let rx = p.subscribe();
for i in 0..4 {
p.push(i);
}
let (_, s) = p.finish();
assert_eq!(l.lock().unwrap().clone(), vec![0, 1, 2, 3]);
assert_eq!(s.hook_panics, 1);
let errs: Vec<String> = rx
.iter()
.filter_map(|e| match e {
Event::Error { message, meta } => {
assert_eq!(meta.version, 2);
Some(message)
}
_ => None,
})
.collect();
assert_eq!(errs.len(), 1);
assert!(errs[0].contains("boom"), "{}", errs[0]);
}
#[test]
fn pipeline_channel_receives_all() {
let mut p = Pipeline::new(Fake::default());
let rx = p.subscribe();
p.push(1);
p.push(2);
let kinds: Vec<EventKind> = rx.try_iter().map(|e| e.kind()).collect();
assert_eq!(kinds, vec![EventKind::PositionDone, EventKind::PositionDone, EventKind::ZonePreview]);
let dropped = p.subscribe();
drop(dropped);
p.push(3);
let (_, s) = p.finish();
let rest: Vec<EventKind> = rx.iter().map(|e| e.kind()).collect();
assert_eq!(rest, vec![EventKind::PositionDone, EventKind::ZoneRefined]);
assert_eq!(s.events, 5);
assert!(matches!(rx.recv_timeout(Duration::from_millis(1)), Err(RecvTimeoutError::Disconnected)));
}
#[test]
fn pipeline_finish_drains() {
let n = Arc::new(AtomicUsize::new(0));
let refined = log();
let (a, b) = (Arc::clone(&n), Arc::clone(&refined));
let mut p = Pipeline::new(Fake::default())
.on(EventKind::PositionDone, move |_| {
std::thread::sleep(Duration::from_micros(200));
a.fetch_add(1, Ordering::SeqCst);
})
.on_zone_refined(move |r, _| b.lock().unwrap().push(r.zone));
for i in 0..50 {
p.push(i);
}
let (r, s) = p.finish();
assert_eq!(n.load(Ordering::SeqCst), 50);
assert_eq!(refined.lock().unwrap().len(), 25);
assert!(r.zones.is_empty());
assert_eq!(s.hook_calls, 75);
}
#[test]
fn pipeline_command_forwarded() {
let msgs = log();
let a = Arc::clone(&msgs);
let mut p = Pipeline::new(Fake::default()).sync(true).on_message(move |k, m| a.lock().unwrap().push(format!("{k:?}:{m}")));
assert_eq!(p.command(Command::InvalidateZone(3)), 1);
p.command(Command::ResetFrom(7));
assert_eq!(p.reducer().commands, vec!["InvalidateZone(3)", "ResetFrom(7)"]);
let (_, _) = p.finish();
assert_eq!(msgs.lock().unwrap().clone(), vec!["Warning:invalidate 3", "Warning:reset 7"]);
}
}