1use crate::live::KeyedDiff;
40use crate::util::{py_float, Json};
41use cumulus3d_core::io::PointCloud;
42use std::path::{Path, PathBuf};
43
44pub use crate::live::{KeyHasher, POSITION_EPS};
45
46pub const FORMAT_VERSION: i64 = 4;
48
49pub const ADD_BYTES: usize = 20;
51pub const MOVE_BYTES: usize = 20;
53pub const REMOVE_BYTES: usize = 4;
55
56#[derive(Clone, Copy, Debug, PartialEq)]
58pub struct LiveStream {
59 pub enu: bool,
61 pub origin_gps: Option<[f64; 3]>,
63}
64
65#[derive(Clone, Copy, Debug, PartialEq, Eq)]
67pub struct EventStats {
68 pub n_add: usize,
70 pub n_move: usize,
72 pub n_remove: usize,
74 pub points: usize,
76 pub bytes: usize,
78}
79
80pub struct DiffStream {
82 dir: PathBuf,
83 cfg: LiveStream,
84 diff: KeyedDiff,
85 events: Vec<Json>,
87}
88
89impl DiffStream {
90 pub fn new(dir: PathBuf, cfg: LiveStream) -> Self {
92 Self { dir, cfg, diff: KeyedDiff::new(), events: Vec::new() }
93 }
94
95 pub fn events_written(&self) -> i64 {
97 self.diff.seq() as i64
98 }
99
100 pub fn dir(&self) -> &Path {
102 &self.dir
103 }
104
105 pub fn write_event(&mut self, t: f64, zone: usize, kind: &str, cloud: &PointCloud, zones: &[usize]) -> Result<EventStats, String> {
109 let pending = self.diff.prepare(cloud, zones)?;
110 let d = pending.delta();
111 let (n_add, n_move, n_remove) = (d.adds.len(), d.moves.len(), d.removes.len());
112 let bytes = n_add * ADD_BYTES + n_move * MOVE_BYTES + n_remove * REMOVE_BYTES;
113 let mut pack: Vec<u8> = Vec::with_capacity(bytes);
114 for a in &d.adds {
115 pack.extend_from_slice(&a.id.to_le_bytes());
116 for v in a.xyz {
117 pack.extend_from_slice(&v.to_le_bytes());
118 }
119 pack.extend_from_slice(&a.rgb);
120 pack.push(a.zone);
121 }
122 for m in &d.moves {
123 pack.extend_from_slice(&m.id.to_le_bytes());
124 for v in m.xyz {
125 pack.extend_from_slice(&v.to_le_bytes());
126 }
127 pack.extend_from_slice(&m.rgb);
128 pack.push(0);
129 }
130 for id in &d.removes {
131 pack.extend_from_slice(&id.to_le_bytes());
132 }
133 let seq = d.seq as i64;
134 let pack_name = (bytes > 0).then(|| format!("packs/e_{seq:04}.bin"));
135 let ev = Json::obj(vec![
136 ("seq", Json::Int(seq)),
137 ("t", Json::f(t, 2)),
138 ("zone", Json::Int(zone as i64)),
139 ("kind", Json::Str(kind.to_string())),
140 ("points", Json::Int(d.total_points as i64)),
141 ("pack", pack_name.clone().map_or(Json::Null, Json::Str)),
142 ("n_add", Json::Int(n_add as i64)),
143 ("n_move", Json::Int(n_move as i64)),
144 ("n_remove", Json::Int(n_remove as i64)),
145 ("bytes", Json::Int(bytes as i64)),
146 ]);
147 let written = match &pack_name {
148 Some(name) => atomic_write(&self.dir.join(name), &pack),
149 None => Ok(()),
150 };
151 self.events.push(ev);
152 let written = written.and_then(|_| self.write_manifest());
153 if let Err(e) = written {
154 self.events.pop();
155 self.diff.abort(pending);
156 return Err(e);
157 }
158 self.diff.commit(pending);
159 Ok(EventStats { n_add, n_move, n_remove, points: self.diff.len(), bytes })
160 }
161
162 fn write_manifest(&self) -> Result<(), String> {
163 let origin = match self.cfg.origin_gps {
164 Some(o) => Json::Arr(o.iter().map(|v| Json::Num(py_float(*v, 9))).collect()),
165 None => Json::Null,
166 };
167 let m = Json::obj(vec![
168 ("version", Json::Int(FORMAT_VERSION)),
169 ("frame", Json::Str(if self.cfg.enu { "enu" } else { "display" }.into())),
170 ("origin_gps", origin),
171 ("capacity", Json::Int(self.diff.capacity() as i64)),
172 ("events", Json::Arr(self.events.clone())),
173 ]);
174 atomic_write(&self.dir.join("manifest.json"), m.dump().as_bytes())
175 }
176}
177
178#[derive(Clone, Debug, Default)]
180pub struct Heartbeat {
181 pub dir: PathBuf,
183 pub stage: &'static str,
185 pub zone: usize,
187 pub positions: usize,
189 pub positions_total: Option<usize>,
191 pub images: usize,
193}
194
195impl Heartbeat {
196 pub fn write(&self, t: f64) -> Result<(), String> {
198 let total = self.positions_total.map_or(Json::Null, |n| Json::Int(n as i64));
199 let m = Json::obj(vec![
200 ("t", Json::f(t, 1)),
201 ("stage", Json::Str(self.stage.into())),
202 ("zone", Json::Int(self.zone as i64)),
203 ("positions", Json::Int(self.positions as i64)),
204 ("positions_total", total),
205 ("images", Json::Int(self.images as i64)),
206 ]);
207 atomic_write(&self.dir.join("status.json"), m.dump().as_bytes())
208 }
209}
210
211pub(crate) fn atomic_write(path: &Path, bytes: &[u8]) -> Result<(), String> {
213 if let Some(d) = path.parent() {
214 std::fs::create_dir_all(d).map_err(|e| format!("{}: {e}", d.display()))?;
215 }
216 let mut tmp = path.as_os_str().to_owned();
217 tmp.push(".tmp");
218 let tmp = PathBuf::from(tmp);
219 std::fs::write(&tmp, bytes).map_err(|e| format!("{}: {e}", tmp.display()))?;
220 std::fs::rename(&tmp, path).map_err(|e| format!("{}: {e}", path.display()))
221}
222
223#[cfg(test)]
224mod tests {
225 use super::*;
226
227 const FROZEN_HASH: u64 = 0x3f8de9f327bd3d56;
228 const FROZEN_LENS: [usize; 4] = [5760, 628, 920, 0];
229
230 fn tmpdir(name: &str) -> PathBuf {
231 let d = std::env::temp_dir().join(format!("cumulus3d_diff_{}_{name}", std::process::id()));
232 let _ = std::fs::remove_dir_all(&d);
233 d
234 }
235
236 fn cfg() -> LiveStream {
237 LiveStream { enu: true, origin_gps: Some([37.5, 127.0, 30.0]) }
238 }
239
240 fn lattice() -> (PointCloud, Vec<usize>) {
242 let mut c = PointCloud::default();
243 let mut zones = Vec::new();
244 for i in 0..12 {
245 for j in 0..12 {
246 for k in 0..2 {
247 c.positions.push([i as f32 * 0.7 - 3.0, j as f32 * 0.7 - 3.0, k as f32 * 0.7]);
248 c.colors.push([(i * 20) as u8, (j * 20) as u8, 7]);
249 c.keys.push(c.positions.len() as u64);
250 zones.push((i / 6) as usize);
251 }
252 }
253 }
254 (c, zones)
255 }
256
257 fn pick(c: &PointCloud, z: &[usize], idx: &[usize]) -> (PointCloud, Vec<usize>) {
259 let mut o = PointCloud::default();
260 let mut oz = Vec::new();
261 for &i in idx {
262 o.positions.push(c.positions[i]);
263 o.colors.push(c.colors[i]);
264 o.keys.push(c.keys[i]);
265 oz.push(z[i]);
266 }
267 (o, oz)
268 }
269
270 type Ids = (Vec<u32>, Vec<u32>);
271 type Slot = Option<([f32; 3], [u8; 3], u8)>;
272
273 struct Ev {
275 pack: Option<String>,
276 n: [usize; 3],
277 bytes: usize,
278 points: usize,
279 }
280
281 fn manifest(dir: &Path) -> (Json, Vec<Ev>) {
282 let m = Json::parse(&std::fs::read_to_string(dir.join("manifest.json")).unwrap()).unwrap();
283 let Some(Json::Arr(events)) = m.get("events") else { panic!("no events") };
284 let num = |e: &Json, k: &str| e.get(k).unwrap().as_f64().unwrap() as usize;
285 let evs = events
286 .iter()
287 .map(|e| Ev {
288 pack: match e.get("pack") {
289 Some(Json::Str(p)) => Some(p.clone()),
290 _ => None,
291 },
292 n: [num(e, "n_add"), num(e, "n_move"), num(e, "n_remove")],
293 bytes: num(e, "bytes"),
294 points: num(e, "points"),
295 })
296 .collect();
297 (m, evs)
298 }
299
300 fn apply(dir: &Path, slots: &mut Vec<Slot>, ev: &Ev) -> (Vec<u32>, Vec<u32>) {
302 let Some(p) = &ev.pack else { return (vec![], vec![]) };
303 let b = std::fs::read(dir.join(p)).unwrap();
304 assert_eq!(b.len(), ev.bytes);
305 assert_eq!(b.len(), ev.n[0] * ADD_BYTES + ev.n[1] * MOVE_BYTES + ev.n[2] * REMOVE_BYTES);
306 let u = |o: usize| u32::from_le_bytes(b[o..o + 4].try_into().unwrap());
307 let f = |o: usize| f32::from_le_bytes(b[o..o + 4].try_into().unwrap());
308 let (mut added, mut removed) = (vec![], vec![]);
309 let mut o = 0;
310 let rm0 = ev.n[0] * ADD_BYTES + ev.n[1] * MOVE_BYTES;
312 for r in 0..ev.n[2] {
313 let id = u(rm0 + r * 4);
314 removed.push(id);
315 slots[id as usize] = None;
316 }
317 for _ in 0..ev.n[0] {
318 let id = u(o) as usize;
319 if slots.len() <= id {
320 slots.resize(id + 1, None);
321 }
322 assert!(slots[id].is_none(), "add into a live slot {id}");
323 slots[id] = Some(([f(o + 4), f(o + 8), f(o + 12)], [b[o + 16], b[o + 17], b[o + 18]], b[o + 19]));
324 added.push(id as u32);
325 o += ADD_BYTES;
326 }
327 for _ in 0..ev.n[1] {
328 let id = u(o) as usize;
329 let s = slots[id].as_mut().expect("move of an empty slot");
330 s.0 = [f(o + 4), f(o + 8), f(o + 12)];
331 s.1 = [b[o + 16], b[o + 17], b[o + 18]];
332 assert_eq!(b[o + 19], 0);
333 o += MOVE_BYTES;
334 }
335 (added, removed)
336 }
337
338 fn replay(dir: &Path) -> (Vec<Slot>, Vec<Ids>) {
339 let (_, evs) = manifest(dir);
340 let mut slots = Vec::new();
341 let ids = evs.iter().map(|e| apply(dir, &mut slots, e)).collect();
342 (slots, ids)
343 }
344
345 fn shown(slots: &[Slot]) -> Vec<([u32; 3], [u8; 3], u8)> {
346 let mut v: Vec<_> = slots.iter().flatten().map(|s| (s.0.map(f32::to_bits), s.1, s.2)).collect();
347 v.sort();
348 v
349 }
350
351 fn expect(c: &PointCloud, z: &[usize]) -> Vec<([u32; 3], [u8; 3], u8)> {
352 let mut v: Vec<_> = (0..c.len()).map(|i| (c.positions[i].map(f32::to_bits), c.colors[i], z[i] as u8)).collect();
353 v.sort();
354 v
355 }
356
357 #[test]
358 fn packs_in_order_reproduce_the_composed_cloud() {
359 let dir = tmpdir("repro");
360 let (c, z) = lattice();
361 let mut ds = DiffStream::new(dir.clone(), cfg());
362 let st = ds.write_event(1.5, 0, "refined", &c, &z).unwrap();
363 assert_eq!((st.n_add, st.n_move, st.n_remove, st.points), (c.len(), 0, 0, c.len()));
364 let keep: Vec<usize> = (0..c.len()).filter(|i| i % 5 != 0).collect();
366 let (mut c2, mut z2) = pick(&c, &z, &keep);
367 c2.positions[3][1] += 0.5;
368 c2.positions[40][2] -= 0.25;
369 for j in 0..30u64 {
370 c2.positions.push([50.0 + j as f32, 0.0, 1.0]);
371 c2.colors.push([1, 2, 3]);
372 c2.keys.push(10_000 + j);
373 z2.push(2);
374 }
375 ds.write_event(2.5, 1, "replace", &c2, &z2).unwrap();
376 let (slots, _) = replay(&dir);
377 assert_eq!(shown(&slots), expect(&c2, &z2));
378 let (m, evs) = manifest(&dir);
379 assert_eq!(m.get("version").and_then(Json::as_f64), Some(4.0));
380 assert!(matches!(m.get("frame"), Some(Json::Str(s)) if s == "enu"));
381 assert!(matches!(m.get("origin_gps"), Some(Json::Arr(a)) if a.len() == 3));
382 assert_eq!(m.get("capacity").and_then(Json::as_f64), Some(slots.len() as f64));
383 assert_eq!(evs[0].pack.as_deref(), Some("packs/e_0001.bin"));
384 assert_eq!(evs[0].bytes, c.len() * ADD_BYTES);
385 assert_eq!(evs[1].points, c2.len());
386 assert_eq!(evs[1].n, [30, 2, c.len() - keep.len()]);
387 assert!(!dir.join("manifest.json.tmp").exists());
388 let _ = std::fs::remove_dir_all(&dir);
389 }
390
391 #[test]
392 fn identical_event_writes_no_pack() {
393 let dir = tmpdir("same");
394 let (c, z) = lattice();
395 let mut ds = DiffStream::new(dir.clone(), cfg());
396 ds.write_event(1.0, 0, "refined", &c, &z).unwrap();
397 let mut c2 = c.clone();
399 c2.positions[5][0] += 5e-5;
400 let st = ds.write_event(2.0, 0, "replace", &c2, &z).unwrap();
401 assert_eq!((st.n_add, st.n_move, st.n_remove, st.bytes), (0, 0, 0, 0));
402 let (_, evs) = manifest(&dir);
403 assert_eq!(evs.len(), 2);
404 assert!(evs[1].pack.is_none());
405 assert_eq!(evs[1].n, [0, 0, 0]);
406 assert_eq!(evs[1].points, c.len());
407 assert!(!dir.join("packs/e_0002.bin").exists());
408 let _ = std::fs::remove_dir_all(&dir);
409 }
410
411 #[test]
412 fn moving_points_only_moves() {
413 let dir = tmpdir("move");
414 let (c, z) = lattice();
415 let mut ds = DiffStream::new(dir.clone(), cfg());
416 ds.write_event(1.0, 0, "refined", &c, &z).unwrap();
417 let mut c2 = c.clone();
418 for i in [10usize, 200, 101] {
419 c2.positions[i][0] += 0.3;
420 }
421 let st = ds.write_event(2.0, 1, "replace", &c2, &z).unwrap();
422 assert_eq!((st.n_add, st.n_move, st.n_remove), (0, 3, 0));
423 assert_eq!(st.bytes, 3 * MOVE_BYTES);
424 let (slots, _) = replay(&dir);
425 assert_eq!(shown(&slots), expect(&c2, &z));
426 let _ = std::fs::remove_dir_all(&dir);
427 }
428
429 #[test]
430 fn freed_ids_are_reused_only_in_the_next_event() {
431 let dir = tmpdir("reuse");
432 let (c, z) = lattice();
433 let n = c.len();
434 let mut ds = DiffStream::new(dir.clone(), cfg());
435 ds.write_event(1.0, 0, "refined", &c, &z).unwrap();
436 let (mut c2, mut z2) = pick(&c, &z, &(10..n).collect::<Vec<_>>());
438 let new_pt = |c: &mut PointCloud, z: &mut Vec<usize>, key: u64| {
439 c.positions.push([40.0 + key as f32, 1.0, 2.0]);
440 c.colors.push([9, 9, 9]);
441 c.keys.push(key);
442 z.push(0);
443 };
444 for k in 0..10 {
445 new_pt(&mut c2, &mut z2, 5000 + k);
446 }
447 let st = ds.write_event(2.0, 0, "replace", &c2, &z2).unwrap();
448 assert_eq!((st.n_add, st.n_remove), (10, 10));
449 let (_, ids) = replay(&dir);
450 let (added2, removed2) = &ids[1];
451 assert!(added2.iter().all(|a| !removed2.contains(a)), "freed id reused in the same event");
452 assert!(added2.iter().all(|&a| a as usize >= n), "adds must take new ids");
453 let (mut c3, mut z3) = (c2.clone(), z2.clone());
455 for k in 0..10 {
456 new_pt(&mut c3, &mut z3, 6000 + k);
457 }
458 let st = ds.write_event(3.0, 0, "replace", &c3, &z3).unwrap();
459 assert_eq!((st.n_add, st.n_remove), (10, 0));
460 let (slots, ids) = replay(&dir);
461 let mut want = removed2.clone();
462 want.sort();
463 let mut got = ids[2].0.clone();
464 got.sort();
465 assert_eq!(got, want);
466 assert_eq!(ids[2].0, want, "lowest free id first");
467 assert_eq!(slots.len(), n + 10, "capacity does not grow when ids are recycled");
468 assert_eq!(shown(&slots), expect(&c3, &z3));
469 let _ = std::fs::remove_dir_all(&dir);
470 }
471
472 #[test]
473 fn colour_change_is_a_move_with_the_same_id() {
474 let dir = tmpdir("colour");
475 let (c, z) = lattice();
476 let mut ds = DiffStream::new(dir.clone(), cfg());
477 ds.write_event(1.0, 0, "refined", &c, &z).unwrap();
478 let mut c2 = c.clone();
479 c2.colors[7] = [1, 1, 1];
480 c2.colors[9] = [2, 2, 2];
481 c2.positions[9][0] += 0.2;
482 let st = ds.write_event(2.0, 0, "replace", &c2, &z).unwrap();
483 assert_eq!((st.n_add, st.n_move, st.n_remove, st.points), (0, 2, 0, c.len()));
484 assert_eq!(st.bytes, 2 * MOVE_BYTES);
485 let (slots, _) = replay(&dir);
486 assert_eq!(slots.len(), c.len());
487 assert_eq!(shown(&slots), expect(&c2, &z));
488 let _ = std::fs::remove_dir_all(&dir);
489 }
490
491 #[test]
492 fn zone_change_is_remove_plus_add_with_a_new_id() {
493 let dir = tmpdir("zone");
494 let (c, z) = lattice();
495 let mut ds = DiffStream::new(dir.clone(), cfg());
496 ds.write_event(1.0, 0, "refined", &c, &z).unwrap();
497 let mut z2 = z.clone();
498 z2[7] = 5;
499 z2[8] = 6;
500 let st = ds.write_event(2.0, 0, "replace", &c, &z2).unwrap();
501 assert_eq!((st.n_add, st.n_move, st.n_remove, st.points), (2, 0, 2, c.len()));
502 let (slots, ids) = replay(&dir);
503 assert_eq!(ids[1].1, vec![7, 8]);
504 assert!(ids[1].0.iter().all(|&a| a as usize >= c.len()));
505 assert_eq!(shown(&slots), expect(&c, &z2));
506 let _ = std::fs::remove_dir_all(&dir);
507 }
508
509 #[test]
510 fn rejects_clouds_without_keys() {
511 let dir = tmpdir("nokeys");
512 let mut c = PointCloud::default();
513 c.positions.push([0.0; 3]);
514 let mut ds = DiffStream::new(dir.clone(), cfg());
515 assert!(ds.write_event(0.0, 0, "refined", &c, &[0]).is_err());
516 assert!(!dir.join("manifest.json").exists());
517 let _ = std::fs::remove_dir_all(&dir);
518 }
519
520 fn fnv(h: u64, b: &[u8]) -> u64 {
522 b.iter().fold(h, |h, &x| (h ^ x as u64).wrapping_mul(0x0000_0100_0000_01b3))
523 }
524
525 #[test]
528 fn pack_bytes_are_frozen() {
529 let dir = tmpdir("frozen");
530 let (c, z) = lattice();
531 let n = c.len();
532 let mut ds = DiffStream::new(dir.clone(), cfg());
533 ds.write_event(1.0, 0, "refined", &c, &z).unwrap();
534 let keep: Vec<usize> = (0..n).filter(|i| i % 7 != 3).collect();
535 let (mut c2, mut z2) = pick(&c, &z, &keep);
536 c2.positions[5][0] += 0.4;
537 c2.colors[9] = [3, 4, 5];
538 z2[11] = 4;
539 for j in 0..20u64 {
540 c2.positions.push([60.0 + j as f32, 2.0, 1.5]);
541 c2.colors.push([j as u8, 2, 3]);
542 c2.keys.push(20_000 + j);
543 z2.push(1);
544 }
545 ds.write_event(2.0, 1, "replace", &c2, &z2).unwrap();
546 let (mut c3, mut z3) = pick(&c2, &z2, &(0..c2.len()).filter(|i| i % 11 != 0).collect::<Vec<_>>());
547 c3.positions[2][2] -= 1.0;
548 for j in 0..40u64 {
549 c3.positions.push([-60.0 - j as f32, 2.0, 1.5]);
550 c3.colors.push([9, j as u8, 3]);
551 c3.keys.push(30_000 + j);
552 z3.push(2);
553 }
554 ds.write_event(3.0, 2, "replace", &c3, &z3).unwrap();
555 ds.write_event(4.0, 2, "final", &c3, &z3).unwrap();
556 let mut h = 0xcbf2_9ce4_8422_2325u64;
557 let mut lens = Vec::new();
558 for s in 1..=4 {
559 let f = dir.join(format!("packs/e_{s:04}.bin"));
560 let b = std::fs::read(&f).unwrap_or_default();
561 lens.push(b.len());
562 h = fnv(h, &b);
563 }
564 assert_eq!((h, lens), (FROZEN_HASH, FROZEN_LENS.to_vec()));
565 let _ = std::fs::remove_dir_all(&dir);
566 }
567}