diff --git a/Cargo.lock b/Cargo.lock
index 31ac5a0a..cf237db0 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -2509,6 +2509,16 @@ version = "1.2.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "75b325c5dbd37f80359721ad39aca5a29fb04c89279657cffdda8736d0c0b9d2"
 
+[[package]]
+name = "duplicate"
+version = "1.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "de78e66ac9061e030587b2a2e75cc88f22304913c907b11307bca737141230cb"
+dependencies = [
+ "heck 0.4.1",
+ "proc-macro-error",
+]
+
 [[package]]
 name = "ecolor"
 version = "0.26.2"
@@ -2540,6 +2550,17 @@ dependencies = [
  "egui",
 ]
 
+[[package]]
+name = "egui_dock"
+version = "0.11.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9a062ac200c9f3ddf120ffcc5582f9fbd5d8fbd046d2eed215ed5426f56513d0"
+dependencies = [
+ "duplicate",
+ "egui",
+ "paste",
+]
+
 [[package]]
 name = "egui_extras"
 version = "0.26.2"
@@ -2576,6 +2597,19 @@ dependencies = [
  "egui",
 ]
 
+[[package]]
+name = "egui_tiles"
+version = "0.7.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1bb10cef7bdbd1adb158aec9cca20f34779fd40ea126e02662ab558189f1c435"
+dependencies = [
+ "ahash 0.8.11",
+ "egui",
+ "itertools",
+ "log",
+ "serde",
+]
+
 [[package]]
 name = "ehttp"
 version = "0.4.0"
@@ -3048,9 +3082,11 @@ dependencies = [
  "dark-light",
  "derive_more",
  "directories",
+ "egui_dock",
  "egui_extras",
  "egui_graphs",
  "egui_plot",
+ "egui_tiles",
  "embed-resource 2.4.2",
  "gbp_linalg",
  "gbp_multivariate_normal",
@@ -4557,6 +4593,30 @@ dependencies = [
  "toml_edit 0.21.1",
 ]
 
+[[package]]
+name = "proc-macro-error"
+version = "1.0.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c"
+dependencies = [
+ "proc-macro-error-attr",
+ "proc-macro2",
+ "quote",
+ "syn 1.0.109",
+ "version_check",
+]
+
+[[package]]
+name = "proc-macro-error-attr"
+version = "1.0.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "version_check",
+]
+
 [[package]]
 name = "proc-macro2"
 version = "1.0.79"
diff --git a/Cargo.toml b/Cargo.toml
index cf285903..75ef1763 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -4,7 +4,8 @@ cargo-features = [
 
 [workspace]
 resolver = "2" # Important! wgpu/bevy needs this!
-members = [ "crates/angle",
+members = [
+  "crates/angle",
   "crates/min_len_vec",
   "crates/percentage",
   "crates/unit_interval",
@@ -13,7 +14,11 @@ members = [ "crates/angle",
   # "crates/bevy_undo_redo",
   "crates/gbpplanner-rs",
   "crates/bevy_notify",
-  "crates/units", "crates/repeating_array",
+  "crates/units",
+  "crates/repeating_array",
+  "crates/units",
+  "crates/units",
+  "crates/bevy_fullscreen",
 ]
 
 [workspace.package]
diff --git a/crates/bevy_notify/src/lib.rs b/crates/bevy_notify/src/lib.rs
index d23cebfd..63f1e380 100644
--- a/crates/bevy_notify/src/lib.rs
+++ b/crates/bevy_notify/src/lib.rs
@@ -45,6 +45,7 @@ pub mod prelude {
 /// Adds event `ToastEvent` to be used in systems.
 /// Uses a `Update` system to render the toasts on the screen at the specified
 /// anchor
+#[derive(Debug)]
 pub struct NotifyPlugin {
     /// Anchor for where to render the toasts in the main window
     /// Defaults to `Anchor::TopCenter`
@@ -108,7 +109,7 @@ impl Toasts {
 }
 
 /// Event for creating a toast
-#[derive(Event)]
+#[derive(Debug, Event)]
 pub struct ToastEvent {
     /// The caption of the toast
     pub caption: String,
diff --git a/crates/gbp_linalg/src/lib.rs b/crates/gbp_linalg/src/lib.rs
index 7039ce17..3292127c 100644
--- a/crates/gbp_linalg/src/lib.rs
+++ b/crates/gbp_linalg/src/lib.rs
@@ -5,6 +5,8 @@ pub mod pretty_print;
 /// `use gbp_linalg::prelude::*` to import all the common symbols from this
 /// crate
 pub mod prelude {
+    // pub use ndarray::{array, concatenate, s, Axis};
+
     pub use super::{
         pretty_print::*, Float, GbpFloat, Matrix, MatrixView, NdarrayVectorExt, Vector, VectorNorm,
         VectorView,
@@ -41,12 +43,16 @@ pub type VectorView<'a, T> = ndarray::ArrayView1<'a, T>;
 /// Type alias for `ndarray::ArrayView2<T>`
 pub type MatrixView<'a, T> = ndarray::ArrayView2<'a, T>;
 
-///
+/// Trait for vector norms
 pub trait VectorNorm {
+    /// The scalar type of the vector
     type Scalar: GbpFloat;
+    /// Calculate the Euclidean norm of the vector
     fn euclidean_norm(&self) -> Self::Scalar;
+    /// Calculate the L1 norm of the vector
     fn l1_norm(&self) -> Self::Scalar;
 
+    /// Calculate the L2 norm of the vector
     #[inline(always)]
     fn l2_norm(&self) -> Self::Scalar {
         self.euclidean_norm()
@@ -73,8 +79,11 @@ macro_rules! vector_norm_trait_impl {
 vector_norm_trait_impl!(f32);
 vector_norm_trait_impl!(f64);
 
+/// Extension trait for `ndarray::Array1<T>`
 pub trait NdarrayVectorExt: Clone + VectorNorm {
+    /// The scalar type of the vector
     type Scalar: GbpFloat;
+    /// Normalize the vector in place.
     fn normalize(&mut self);
     /// Return a normalized copy of the vector.
     fn normalized(&self) -> Self {
diff --git a/crates/gbp_linalg/src/pretty_print.rs b/crates/gbp_linalg/src/pretty_print.rs
index f92bda21..23178868 100644
--- a/crates/gbp_linalg/src/pretty_print.rs
+++ b/crates/gbp_linalg/src/pretty_print.rs
@@ -63,6 +63,7 @@ pub fn num_of_integral_digits(mut f: f64) -> Option<usize> {
     Some(count)
 }
 
+/// Map a floating point number to a ansi color string.
 fn float_color(f: f64) -> &'static str {
     if f.is_nan() {
         MAGENTA_TEXT
@@ -190,6 +191,8 @@ pub fn _pretty_print_matrix<T, M>(
     );
 }
 
+/// Internal function to pretty print a vector
+/// Use by the [`pretty_print_vector!`] macro
 pub fn _pretty_print_vector<T, V>(
     vector: &V,
     name: Option<&str>,
@@ -302,6 +305,7 @@ pub fn _pretty_print_vector<T, V>(
     );
 }
 
+/// Extension trait that adds a [`pretty_print`] method to vectors
 pub trait PrettyPrintVector<T: GbpFloat>: Sized {
     /// Returns the length of the vector.
     #[allow(clippy::len_without_is_empty)]
@@ -309,6 +313,7 @@ pub trait PrettyPrintVector<T: GbpFloat>: Sized {
     /// Returns the element at index `i`.
     fn at(&self, i: usize) -> T;
 
+    /// Pretty prints the vector.
     #[inline(always)]
     fn pretty_print(&self) {
         _pretty_print_vector(self, None, None, None);
@@ -326,12 +331,14 @@ impl<T: GbpFloat> PrettyPrintVector<T> for Vector<T> {
         self[i]
     }
 }
+/// Extension trait that adds a [`pretty_print`] method to matrices
 pub trait PrettyPrintMatrix<T: GbpFloat>: Sized {
     /// Returns the shape of the matrix as a tuple `(nrows, ncols)`.
     fn shape(&self) -> (usize, usize);
     /// Returns the element at index `(i, j)`.
     fn at(&self, i: usize, j: usize) -> T;
 
+    /// Pretty prints the matrix.
     #[inline(always)]
     fn pretty_print(&self) {
         _pretty_print_matrix(self, None, None, None);
@@ -350,6 +357,7 @@ impl<T: GbpFloat> PrettyPrintMatrix<T> for Matrix<T> {
     }
 }
 
+/// Pretty prints a vector
 #[macro_export]
 macro_rules! pretty_print_vector {
     ($name:expr) => {
@@ -370,6 +378,7 @@ macro_rules! pretty_print_vector {
     };
 }
 
+/// Pretty prints a matrix
 #[macro_export]
 macro_rules! pretty_print_matrix {
     ($name:expr) => {
diff --git a/crates/gbp_multivariate_normal/src/lib.rs b/crates/gbp_multivariate_normal/src/lib.rs
index 3ffec224..28985c07 100644
--- a/crates/gbp_multivariate_normal/src/lib.rs
+++ b/crates/gbp_multivariate_normal/src/lib.rs
@@ -1,30 +1,38 @@
-// #![warn(missing_docs)]
+//! Multivariate normal distribution type
 use gbp_linalg::{Float, Matrix, Vector};
 use ndarray_inverse::Inverse;
 
+/// Error type use by this module
 #[derive(Debug, thiserror::Error)]
 pub enum MultivariateNormalError {
+    /// The precision matrix is not square
     #[error("the precision matrix is not square, it has shape {0}x{1}")]
     NonSquarePrecisionMatrix(usize, usize),
     #[error(
         "the length of the information vector ({0}) is not equal to the number of rows ({1}) or \
          columns ({2}) of the precision matrix"
     )]
+    /// The length of the information vector is not equal to the number of rows
+    /// or columns of the precision matrix
     VectorLengthNotEqualMatrixShape(usize, usize, usize),
     #[error(
         "the covariance matrix is not invertible, which is required to calculate the precision \
          matrix"
     )]
+    /// The covariance matrix is not invertible
     NonInvertibleCovarianceMatrix,
     #[error(
         "the precision matrix is not invertible, which is required to calculate the covariance \
          matrix"
     )]
+    /// The precision matrix is not invertible
     NonInvertiblePrecisionMatrix,
 }
 
+/// Result type used by this module
 pub type Result<T> = std::result::Result<T, MultivariateNormalError>;
 
+/// Multivariate normal distribution stored in information form
 #[allow(clippy::len_without_is_empty)]
 #[derive(Debug, Clone)]
 pub struct MultivariateNormal {
@@ -145,11 +153,15 @@ impl MultivariateNormal {
         &self.precision
     }
 
+    /// Set the information vector of the multivariate normal distribution
+    /// Updates the cached mean internally.
     pub fn update_information_vector(&mut self, value: &Vector<Float>) {
         self.information.clone_from(value);
         self.update();
     }
 
+    /// Set the precision matrix of the multivariate normal distribution
+    /// Updates the cached mean internally.
     pub fn update_precision_matrix(&mut self, value: &Matrix<Float>) -> Result<()> {
         // if value.det() == Float::zero() {
         if value.det() == 0.0 {
diff --git a/crates/gbpplanner-rs/Cargo.toml b/crates/gbpplanner-rs/Cargo.toml
index ceaa24cf..f03211cf 100644
--- a/crates/gbpplanner-rs/Cargo.toml
+++ b/crates/gbpplanner-rs/Cargo.toml
@@ -30,8 +30,11 @@ bevy = { version = "0.13.0", default-features = true, features = [
   # "wayland",
   # "dynamic_linking",
 ] }
+
 bevy-inspector-egui = "0.23.2"
-bevy_egui = "0.26.0"
+bevy_egui           = "0.26.0"
+egui_dock           = "0.11.4"
+egui_tiles          = "0.7.2"
 
 # bevy_egui = { git = "https://github.com/mvlabat/bevy_egui", branch = "main" } # 0.26 does not support egui 0.27, which egui-notify needs
 
@@ -104,6 +107,8 @@ serde_yaml       = "0.9.34"
 colored          = "2.1.0"
 directories      = "5.0.1"
 chrono           = "0.4.37"
+
+
 # font-kit = { version = "0.13.0", features = ["freetype"] }
 
 dark-light = "1.1.1"
diff --git a/crates/gbpplanner-rs/src/bevy_utils.rs b/crates/gbpplanner-rs/src/bevy_utils.rs
index 1eb21b01..d4bb95e1 100644
--- a/crates/gbpplanner-rs/src/bevy_utils.rs
+++ b/crates/gbpplanner-rs/src/bevy_utils.rs
@@ -58,3 +58,26 @@ impl BevyPluginExt for bevy::app::App {
         self
     }
 }
+
+pub mod run_conditions {
+    use bevy::{
+        ecs::system::Res,
+        input::{keyboard::KeyCode, ButtonInput},
+    };
+
+    //     pub fn any_input_just_pressed(
+    //         // inputs: impl IntoIterator<Item = ButtonInput<KeyCode>>,
+    //         // inputs: impl IntoIterator<Item = KeyCode>,
+    //         // inputs: Vec<KeyCode>,
+    //     ) -> impl Fn(Res<ButtonInput<KeyCode>>) -> bool
+    // // where
+    //     //     T: Copy + Eq + Send + Sync + 'static,
+    //     {
+    //         move |keyboard_input: Res<ButtonInput<KeyCode>>|
+    // keyboard_input.any_pressed(inputs)
+
+    //         // move |keyboard_input: Res<ButtonInput<T>>| {
+    //         //     inputs.into_iter().any(|it|
+    // keyboard_input.just_pressed(it))         // }
+    //     }
+}
diff --git a/crates/gbpplanner-rs/src/bin/console.rs b/crates/gbpplanner-rs/src/bin/console.rs
index 84ca2503..64daba06 100644
--- a/crates/gbpplanner-rs/src/bin/console.rs
+++ b/crates/gbpplanner-rs/src/bin/console.rs
@@ -1,3 +1,5 @@
+#![allow(missing_docs)]
+
 use bevy::{log::LogPlugin, prelude::*};
 use bevy_dev_console::prelude::*;
 
diff --git a/crates/gbpplanner-rs/src/bin/derive_more.rs b/crates/gbpplanner-rs/src/bin/derive_more.rs
deleted file mode 100644
index 0453bf25..00000000
--- a/crates/gbpplanner-rs/src/bin/derive_more.rs
+++ /dev/null
@@ -1,18 +0,0 @@
-#[derive(derive_more::Display)]
-#[display(fmt = "hello")]
-struct Foo;
-
-#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, derive_more::From)]
-struct VariableIndex(usize);
-
-fn main() -> Result<(), Box<dyn std::error::Error>> {
-    let foo = Foo;
-
-    println!("{}", foo);
-
-    let vi: VariableIndex = 42usize.into();
-
-    println!("{:?}", vi);
-
-    Ok(())
-}
diff --git a/crates/gbpplanner-rs/src/bin/draw2d.rs b/crates/gbpplanner-rs/src/bin/draw2d.rs
index 89cc067a..baf0578e 100644
--- a/crates/gbpplanner-rs/src/bin/draw2d.rs
+++ b/crates/gbpplanner-rs/src/bin/draw2d.rs
@@ -1,3 +1,5 @@
+#![allow(missing_docs)]
+
 use bevy::{prelude::*, window::WindowResized};
 // use std::f32::consts::{PI, TAU};
 
diff --git a/crates/gbpplanner-rs/src/bin/gizmos.rs b/crates/gbpplanner-rs/src/bin/gizmos.rs
index 2836a2e1..645e1fd4 100644
--- a/crates/gbpplanner-rs/src/bin/gizmos.rs
+++ b/crates/gbpplanner-rs/src/bin/gizmos.rs
@@ -1,4 +1,4 @@
-#![allow(dead_code, unused_macros)]
+#![allow(dead_code, unused_macros, missing_docs)]
 
 use bevy::{prelude::*, window::WindowResized};
 // use std::f32::consts::{PI, TAU};
diff --git a/crates/gbpplanner-rs/src/bin/hover-ui.rs b/crates/gbpplanner-rs/src/bin/hover-ui.rs
deleted file mode 100644
index 93b8b7d5..00000000
--- a/crates/gbpplanner-rs/src/bin/hover-ui.rs
+++ /dev/null
@@ -1,199 +0,0 @@
-#![allow(missing_docs)]
-
-use bevy::{prelude::*, window::PrimaryWindow};
-
-fn main() -> Result<(), Box<dyn std::error::Error>> {
-    let mut app = App::new();
-
-    app.add_plugins(DefaultPlugins)
-        .add_systems(Startup, setup)
-        .add_systems(Update, move_hover_window);
-
-    app.run();
-    Ok(())
-}
-
-// #[inline]
-// fn px(logical_pixels: f32) -> Val {
-//     Val::Px(logical_pixels)
-// }
-
-trait ValExt {
-    fn px(self) -> Val;
-    fn percent(self) -> Val;
-}
-
-impl ValExt for f32 {
-    fn px(self) -> Val {
-        Val::Px(self)
-    }
-
-    fn percent(self) -> Val {
-        if !(0.0..=100.0).contains(&self) {
-            panic!("value outside interval [0.0, 100.0]");
-        }
-        Val::Percent(self)
-    }
-}
-
-impl ValExt for i32 {
-    fn px(self) -> Val {
-        Val::Px(self as f32)
-    }
-
-    fn percent(self) -> Val {
-        if !(0..=100).contains(&self) {
-            panic!("value outside interval [0, 100]");
-        }
-
-        Val::Percent(self as f32)
-    }
-}
-
-fn relative_cursor_position_in_window(window: &Window) -> Option<Vec2> {
-    let cursor_position = window.cursor_position()?;
-    let relative_cursor_position = Vec2::new(
-        cursor_position.x / window.width(),
-        cursor_position.y / window.height(),
-    );
-
-    Some(relative_cursor_position)
-}
-
-#[derive(Component)]
-pub struct HoverWindow;
-
-fn setup(mut commands: Commands, query_window: Query<&Window, With<PrimaryWindow>>) {
-    let Ok(window) = query_window.get_single() else {
-        error!("no primary window");
-        return;
-    };
-
-    let Some(relative_cursor_position) = relative_cursor_position_in_window(window) else {
-        warn!("cursor position is outside primary window");
-        return;
-    };
-
-    commands.spawn(Camera2dBundle::default());
-
-    let root = commands
-        .spawn((HoverWindow, NodeBundle {
-            style: Style {
-                position_type: PositionType::Absolute,
-                top: 5.0.percent(),
-                // top: percent(5.0),
-                left: 5.0.percent(),
-
-                width: 200.px(),
-                height: 400.px(),
-                ..default()
-            },
-            background_color: Color::RED.into(),
-            border_color: Color::GREEN.into(),
-            ..default()
-        }))
-        .id();
-
-    // all the different combinations of border edges
-    // these correspond to the labels above
-    let borders = [
-        UiRect::default(),
-        UiRect::all(Val::Px(10.)),
-        UiRect::left(Val::Px(10.)),
-        UiRect::right(Val::Px(10.)),
-        UiRect::top(Val::Px(10.)),
-        UiRect::bottom(Val::Px(10.)),
-        UiRect::horizontal(Val::Px(10.)),
-        UiRect::vertical(Val::Px(10.)),
-        UiRect {
-            left: Val::Px(10.),
-            top: Val::Px(10.),
-            ..Default::default()
-        },
-        UiRect {
-            left: Val::Px(10.),
-            bottom: Val::Px(10.),
-            ..Default::default()
-        },
-        UiRect {
-            right: Val::Px(10.),
-            top: Val::Px(10.),
-            ..Default::default()
-        },
-        UiRect {
-            right: Val::Px(10.),
-            bottom: Val::Px(10.),
-            ..Default::default()
-        },
-        UiRect {
-            right: Val::Px(10.),
-            top: Val::Px(10.),
-            bottom: Val::Px(10.),
-            ..Default::default()
-        },
-        UiRect {
-            left: Val::Px(10.),
-            top: Val::Px(10.),
-            bottom: Val::Px(10.),
-            ..Default::default()
-        },
-        UiRect {
-            left: Val::Px(10.),
-            right: Val::Px(10.),
-            top: Val::Px(10.),
-            ..Default::default()
-        },
-        UiRect {
-            left: Val::Px(10.),
-            right: Val::Px(10.),
-            bottom: Val::Px(10.),
-            ..Default::default()
-        },
-    ];
-
-    let title = commands
-        .spawn(TextBundle::from_section("variable", TextStyle {
-            font_size: 10.0,
-            ..Default::default()
-        }))
-        .id();
-
-    let container = commands
-        .spawn(NodeBundle {
-            style: Style {
-                flex_direction: FlexDirection::Column,
-                align_items: AlignItems::Center,
-                ..Default::default()
-            },
-            ..Default::default()
-        })
-        .push_children(&[title])
-        .id();
-
-    commands.entity(root).add_child(container);
-}
-
-fn primary_window_exists(query_window: Query<&Window, With<PrimaryWindow>>) -> bool {
-    query_window.get_single().is_ok()
-}
-
-fn move_hover_window(
-    mut query_hover_window: Query<&mut Transform, With<HoverWindow>>,
-    query_window: Query<&Window, With<PrimaryWindow>>,
-) {
-    let window = query_window.single();
-
-    let Some(relative_cursor_position) = relative_cursor_position_in_window(window) else {
-        warn!("cursor outside primary window");
-        return;
-    };
-
-    let mut transform = query_hover_window.single_mut();
-
-    let x = relative_cursor_position.x * window.width();
-    let y = relative_cursor_position.y * window.height();
-
-    info!("x = {}, y = {}", x, y);
-
-    transform.translation = Vec3::new(x, 0.0, y);
-}
diff --git a/crates/gbpplanner-rs/src/bin/many-animated-sprites.rs b/crates/gbpplanner-rs/src/bin/many-animated-sprites.rs
deleted file mode 100644
index 0b63fd12..00000000
--- a/crates/gbpplanner-rs/src/bin/many-animated-sprites.rs
+++ /dev/null
@@ -1,152 +0,0 @@
-//! Renders a lot of animated sprites to allow performance testing.
-//!
-//! This example sets up many animated sprites in different sizes, rotations,
-//! and scales in the world. It also moves the camera over them to see how well
-//! frustum culling works.
-
-use std::time::Duration;
-
-use bevy::{
-    diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin},
-    math::Quat,
-    prelude::*,
-    render::camera::Camera,
-    window::{PresentMode, WindowResolution},
-    winit::{UpdateMode, WinitSettings},
-};
-use rand::Rng;
-
-const CAMERA_SPEED: f32 = 1000.0;
-
-fn main() {
-    App::new()
-        // Since this is also used as a benchmark, we want it to display performance data.
-        .add_plugins((
-            LogDiagnosticsPlugin::default(),
-            FrameTimeDiagnosticsPlugin,
-            DefaultPlugins.set(WindowPlugin {
-                primary_window: Some(Window {
-                    present_mode: PresentMode::AutoNoVsync,
-                    resolution: WindowResolution::new(1920.0, 1080.0)
-                        .with_scale_factor_override(1.0),
-                    ..default()
-                }),
-                ..default()
-            }),
-        ))
-        .insert_resource(WinitSettings {
-            focused_mode: UpdateMode::Continuous,
-            unfocused_mode: UpdateMode::Continuous,
-        })
-        .add_systems(Startup, setup)
-        .add_systems(
-            Update,
-            (
-                animate_sprite,
-                print_sprite_count,
-                move_camera.after(print_sprite_count),
-            ),
-        )
-        .run();
-}
-
-fn setup(
-    mut commands: Commands,
-    assets: Res<AssetServer>,
-    mut texture_atlases: ResMut<Assets<TextureAtlasLayout>>,
-) {
-    // warn!(include_str!("warning_string.txt"));
-
-    let mut rng = rand::thread_rng();
-
-    let tile_size = Vec2::splat(64.0);
-    let map_size = Vec2::splat(320.0);
-
-    let half_x = (map_size.x / 2.0) as i32;
-    let half_y = (map_size.y / 2.0) as i32;
-
-    let texture_handle = assets.load("textures/rpg/chars/gabe/gabe-idle-run.png");
-    let texture_atlas = TextureAtlasLayout::from_grid(Vec2::new(24.0, 24.0), 7, 1, None, None);
-    let texture_atlas_handle = texture_atlases.add(texture_atlas);
-
-    // Spawns the camera
-
-    commands.spawn(Camera2dBundle::default());
-
-    // Builds and spawns the sprites
-    for y in -half_y..half_y {
-        for x in -half_x..half_x {
-            let position = Vec2::new(x as f32, y as f32);
-            let translation = (position * tile_size).extend(rng.gen::<f32>());
-            let rotation = Quat::from_rotation_z(rng.gen::<f32>());
-            let scale = Vec3::splat(rng.gen::<f32>() * 2.0);
-            let mut timer = Timer::from_seconds(0.1, TimerMode::Repeating);
-            timer.set_elapsed(Duration::from_secs_f32(rng.gen::<f32>()));
-
-            commands.spawn((
-                SpriteSheetBundle {
-                    texture: texture_handle.clone(),
-                    atlas: TextureAtlas {
-                        layout: texture_atlas_handle.clone(),
-                        ..Default::default()
-                    },
-                    transform: Transform {
-                        translation,
-                        rotation,
-                        scale,
-                    },
-                    sprite: Sprite {
-                        custom_size: Some(tile_size),
-                        ..default()
-                    },
-                    ..default()
-                },
-                AnimationTimer(timer),
-            ));
-        }
-    }
-}
-
-// System for rotating and translating the camera
-fn move_camera(time: Res<Time>, mut camera_query: Query<&mut Transform, With<Camera>>) {
-    let mut camera_transform = camera_query.single_mut();
-    camera_transform.rotate(Quat::from_rotation_z(time.delta_seconds() * 0.5));
-    *camera_transform = *camera_transform
-        * Transform::from_translation(Vec3::X * CAMERA_SPEED * time.delta_seconds());
-}
-
-#[derive(Component, Deref, DerefMut)]
-struct AnimationTimer(Timer);
-
-fn animate_sprite(
-    time: Res<Time>,
-    texture_atlases: Res<Assets<TextureAtlasLayout>>,
-    mut query: Query<(&mut AnimationTimer, &mut TextureAtlas)>,
-) {
-    for (mut timer, mut sheet) in query.iter_mut() {
-        timer.tick(time.delta());
-        if timer.just_finished() {
-            error!("timer finished");
-            let texture_atlas = texture_atlases.get(&sheet.layout).unwrap();
-            sheet.index = (sheet.index + 1) % texture_atlas.textures.len();
-        }
-    }
-}
-
-#[derive(Deref, DerefMut)]
-struct PrintingTimer(Timer);
-
-impl Default for PrintingTimer {
-    fn default() -> Self {
-        Self(Timer::from_seconds(1.0, TimerMode::Repeating))
-    }
-}
-
-// System for printing the number of sprites on every tick of the timer
-fn print_sprite_count(time: Res<Time>, mut timer: Local<PrintingTimer>, sprites: Query<&Sprite>) {
-    timer.tick(time.delta());
-
-    if timer.just_finished() {
-        info!("Sprites: {}", sprites.iter().count());
-    }
-}
diff --git a/crates/gbpplanner-rs/src/bin/notify.rs b/crates/gbpplanner-rs/src/bin/notify.rs
deleted file mode 100644
index 6df52b91..00000000
--- a/crates/gbpplanner-rs/src/bin/notify.rs
+++ /dev/null
@@ -1,275 +0,0 @@
-use std::{collections::VecDeque, time::Duration};
-
-use bevy::{input::common_conditions::*, prelude::*};
-
-fn main() -> Result<(), Box<dyn std::error::Error>> {
-    if let Ok(rust_log) = std::env::var("RUST_LOG") {
-        println!("RUST_LOG = {}", rust_log);
-    }
-    App::new()
-        .add_plugins(DefaultPlugins)
-        .add_event::<CreateNotificationEvent>()
-        .add_event::<DespawnNotificationEvent>()
-        .init_resource::<NotificationStore>()
-        .add_systems(Startup, setup)
-        .add_systems(
-            Update,
-            (
-                advance_time,
-                send_notification.run_if(input_just_pressed(KeyCode::Space)),
-                listen_for_notifications,
-                remove_notifications,
-            ),
-        )
-        .run();
-
-    Ok(())
-}
-
-#[derive(Debug, Component)]
-pub struct NotificationCountdown(Timer);
-
-impl NotificationCountdown {
-    pub fn new(duration: Duration) -> Self {
-        Self(Timer::from_seconds(duration.as_secs_f32(), TimerMode::Once))
-    }
-}
-
-#[derive(Debug, Default)]
-struct NotificationIdGenerator {
-    current: NotificationId,
-}
-
-impl NotificationIdGenerator {
-    fn next_id(&mut self) -> NotificationId {
-        let next = self.current;
-        self.current += 1;
-        next
-    }
-}
-
-#[derive(Resource, Default)]
-struct NotificationStore {
-    notifications: VecDeque<Notification>,
-    id_generator:  NotificationIdGenerator,
-}
-
-impl NotificationStore {
-    pub fn len(&self) -> usize {
-        self.notifications.len()
-    }
-
-    pub fn is_empty(&self) -> bool {
-        self.notifications.is_empty()
-    }
-
-    pub fn insert(&mut self, event: CreateNotificationEvent) -> NotificationId {
-        let id = self.id_generator.next_id();
-        let notification =
-            Notification::new(id, event.title, event.body, event.category, event.duration);
-        self.notifications.push_back(notification);
-        id
-    }
-
-    pub fn remove(&mut self, id: NotificationId) -> Option<Notification> {
-        for i in 0..self.len() {
-            if self.notifications[i].id == id {
-                return self.notifications.remove(i);
-            }
-        }
-        None
-    }
-}
-
-type NotificationId = usize;
-
-#[derive(Debug)]
-struct Notification {
-    id:       NotificationId,
-    title:    String,
-    body:     String,
-    category: NotificationCategory,
-    duration: Duration,
-}
-
-impl Notification {
-    fn new(
-        id: NotificationId,
-        title: impl Into<String>,
-        body: impl Into<String>,
-        category: NotificationCategory,
-        duration: Duration,
-    ) -> Self {
-        Self {
-            id,
-            title: title.into(),
-            body: body.into(),
-            category,
-            duration,
-        }
-    }
-}
-
-#[derive(Debug, Event, Clone)]
-pub struct CreateNotificationEvent {
-    title:    String,
-    body:     String,
-    category: NotificationCategory,
-    duration: Duration,
-}
-
-#[derive(Debug, Default, PartialEq, Clone, Copy)]
-pub enum NotificationCategory {
-    #[default]
-    Info,
-    Warn,
-    Error,
-}
-
-impl CreateNotificationEvent {
-    pub fn new(
-        title: impl Into<String>,
-        body: impl Into<String>,
-        category: NotificationCategory,
-        duration: Duration,
-    ) -> Self {
-        Self {
-            title: title.into(),
-            body: body.into(),
-            category,
-            duration,
-        }
-    }
-}
-
-fn setup(mut commands: Commands) {
-    commands.spawn(Camera2dBundle::default());
-    // root node
-    // commands.spawn(NodeBundle {
-    //     style: Style {
-    //         width: Val::Percent(100.0),
-    //         height: Val::Percent(100.0),
-    //         justify_content: JustifyContent::SpaceBetween,
-    //         ..default()
-    //     },
-    //     ..default()
-    // });
-}
-
-fn send_notification(mut ev_notification: EventWriter<CreateNotificationEvent>) {
-    println!("sending notification");
-    ev_notification.send(CreateNotificationEvent::new(
-        "title",
-        "body",
-        NotificationCategory::Info,
-        Duration::from_secs(5),
-    ));
-}
-
-fn listen_for_notifications(
-    mut commands: Commands,
-    mut ev_notification: EventReader<CreateNotificationEvent>,
-    mut store: ResMut<NotificationStore>,
-) {
-    for notification in ev_notification.read() {
-        println!("received notification: {:?}", notification);
-        let id = store.insert(notification.clone());
-        println!("inserted notification into store with id: {:?}", id);
-        println!("the store currently contains {} notifications", store.len());
-
-        commands
-            .spawn((
-                NotificationCountdown::new(notification.duration),
-                NotificationComponent(id),
-                NodeBundle {
-                    style: Style {
-                        width: Val::Percent(100.0),
-                        height: Val::Percent(100.0),
-                        justify_content: JustifyContent::SpaceBetween,
-                        ..default()
-                    },
-                    ..default()
-                },
-            ))
-            .with_children(|parent| {
-                // left vertical fill (border)
-                parent.spawn(NodeBundle {
-                    style: Style {
-                        width: Val::Px(200.),
-                        height: Val::Px(200.0),
-                        border: UiRect::all(Val::Px(2.)),
-                        ..default()
-                    },
-                    background_color: Color::rgb(0.65, 0.65, 0.65).into(),
-                    ..default()
-                });
-            });
-    }
-}
-
-#[derive(Debug, Clone, Copy, PartialEq, Eq)]
-pub enum NotificationColumnPlacement {
-    Left,
-    Center,
-    Right,
-}
-
-#[derive(Debug, Clone, Copy, PartialEq, Eq)]
-pub enum NotificationRowPlacement {
-    Top,
-    Bottom,
-}
-
-#[derive(Debug, Clone, Copy, PartialEq, Eq)]
-pub struct NotificationPlacement {
-    row:    NotificationRowPlacement,
-    column: NotificationColumnPlacement,
-}
-
-// #[derive(Debug, Component, Clone, Copy)]
-#[derive(Component, Clone, Copy)]
-pub struct NotificationComponent(pub NotificationId);
-
-// #[derive(Component)]
-// pub struct Foo;
-
-#[derive(Debug, Event, Clone, Copy)]
-pub struct DespawnNotificationEvent(NotificationId);
-
-fn advance_time(
-    time: Res<Time>,
-    mut query: Query<(&NotificationComponent, &mut NotificationCountdown)>,
-    // query: Query<NotificationComponen>,
-    // query: Query<&Foo>,
-    mut ev_despawn_notification: EventWriter<DespawnNotificationEvent>,
-) {
-    for (NotificationComponent(id), mut countdown) in query.iter_mut() {
-        // for (component, mut countdown) in query.iter_mut() {
-        countdown.0.tick(time.delta());
-
-        if countdown.0.just_finished() {
-            println!("countdown finished sending despawn event");
-            ev_despawn_notification.send(DespawnNotificationEvent(*id));
-        }
-    }
-}
-
-fn remove_notifications(
-    mut commands: Commands,
-    mut ev_despawn_notification: EventReader<DespawnNotificationEvent>,
-    mut store: ResMut<NotificationStore>,
-) {
-    for ev in ev_despawn_notification.read() {
-        let Some(notification) = store.remove(ev.0) else {
-            error!(
-                "attempted to remove a notification {:?} that does not exist!",
-                ev.0
-            );
-            continue;
-        };
-
-        // commands.get_entity(entity).despawn();
-
-        println!("removed notification: {:?}", notification);
-    }
-}
diff --git a/crates/gbpplanner-rs/src/bin/repeating-timers.rs b/crates/gbpplanner-rs/src/bin/repeating-timers.rs
index c04ad5db..c36a4e11 100644
--- a/crates/gbpplanner-rs/src/bin/repeating-timers.rs
+++ b/crates/gbpplanner-rs/src/bin/repeating-timers.rs
@@ -1,3 +1,5 @@
+#![allow(missing_docs)]
+
 use std::time::Duration;
 
 use bevy::prelude::*;
diff --git a/crates/gbpplanner-rs/src/config/mod.rs b/crates/gbpplanner-rs/src/config/mod.rs
index 9e9d3df7..a42b2fbf 100644
--- a/crates/gbpplanner-rs/src/config/mod.rs
+++ b/crates/gbpplanner-rs/src/config/mod.rs
@@ -1,6 +1,7 @@
 pub mod environment;
 pub mod formation;
 pub mod geometry;
+
 pub mod reader;
 
 use std::num::NonZeroUsize;
diff --git a/crates/gbpplanner-rs/src/diagnostic/robot.rs b/crates/gbpplanner-rs/src/diagnostic/robot.rs
index 7b52866e..bc448f66 100644
--- a/crates/gbpplanner-rs/src/diagnostic/robot.rs
+++ b/crates/gbpplanner-rs/src/diagnostic/robot.rs
@@ -12,7 +12,7 @@ use bevy::{
 use typed_floats::StrictlyPositiveFinite;
 use units::sample_rate::SampleRate;
 
-use crate::planner::{FactorGraph, RobotState};
+use crate::{factorgraph::prelude::FactorGraph, planner::RobotState};
 
 // /// Newtype representing a sample rate in seconds.
 // /// The newtype wraps a `std::time::Duration` to ensure the invariant that
@@ -79,6 +79,7 @@ impl Plugin for RobotDiagnosticsPlugin {
         app.register_diagnostic(Diagnostic::new(Self::ROBOT_COUNT))
             .register_diagnostic(Diagnostic::new(Self::VARIABLE_COUNT))
             .register_diagnostic(Diagnostic::new(Self::FACTOR_COUNT))
+            .register_diagnostic(Diagnostic::new(Self::EXTERNAL_MESSAGES_SENT_COUNT))
             .register_diagnostic(Diagnostic::new(Self::MESSAGES_SENT_COUNT));
 
         let sample_schedule = PostUpdate;
@@ -150,6 +151,8 @@ impl Plugin for RobotDiagnosticsPlugin {
 }
 
 impl RobotDiagnosticsPlugin {
+    pub const EXTERNAL_MESSAGES_SENT_COUNT: DiagnosticPath =
+        DiagnosticPath::const_new("external_messages_sent_count");
     pub const FACTOR_COUNT: DiagnosticPath = DiagnosticPath::const_new("factor_count");
     pub const MESSAGES_SENT_COUNT: DiagnosticPath =
         DiagnosticPath::const_new("messages_sent_count");
@@ -194,4 +197,15 @@ impl RobotDiagnosticsPlugin {
             *messages_sent_in_total as f64
         })
     }
+
+    // fn count_external_messages_sent(
+    //     mut diagnostics: Diagnostics,
+    //     mut messages_sent_in_total: Local<usize>,
+    //     mut factorgraphs: Query<&mut FactorGraph, With<RobotState>>,
+    // ) {
+    //     diagnostics.add_measurement(&Self::EXTERNAL_MESSAGES_SENT_COUNT, || {
+    //         *messages_sent_in_total += *messages_sent_in_total;
+    //         *messages_sent_in_total as f64
+    //     })
+    // }
 }
diff --git a/crates/gbpplanner-rs/src/factorgraph/factor/dynamic.rs b/crates/gbpplanner-rs/src/factorgraph/factor/dynamic.rs
new file mode 100644
index 00000000..b795a16c
--- /dev/null
+++ b/crates/gbpplanner-rs/src/factorgraph/factor/dynamic.rs
@@ -0,0 +1,84 @@
+//!
+
+use gbp_linalg::prelude::*;
+use ndarray::{concatenate, Axis};
+
+use super::{FactorState, IFactor};
+use crate::factorgraph::DOFS;
+
+/// Dynamic factor: constant velocity model
+#[derive(Debug)]
+pub struct DynamicFactor {
+    cached_jacobian: Matrix<Float>,
+}
+
+impl DynamicFactor {
+    pub const NEIGHBORS: usize = 2;
+
+    #[must_use]
+    pub fn new(state: &mut FactorState, delta_t: Float) -> Self {
+        let eye = Matrix::<Float>::eye(DOFS / 2);
+        let zeros = Matrix::<Float>::zeros((DOFS / 2, DOFS / 2));
+        #[allow(clippy::similar_names)]
+        let qc_inv = Float::powi(state.strength, -2) * &eye;
+
+        #[allow(clippy::similar_names)]
+        let qi_inv = concatenate![
+            Axis(0),
+            concatenate![
+                Axis(1),
+                12.0 * Float::powi(delta_t, -3) * &qc_inv,
+                -6.0 * Float::powi(delta_t, -2) * &qc_inv
+            ],
+            concatenate![
+                Axis(1),
+                -6.0 * Float::powi(delta_t, -2) * &qc_inv,
+                (4.0 / delta_t) * &qc_inv
+            ]
+        ];
+        debug_assert_eq!(qi_inv.shape(), &[DOFS, DOFS]);
+
+        state.measurement_precision = qi_inv;
+
+        let cached_jacobian = concatenate![
+            Axis(0),
+            concatenate![Axis(1), eye, delta_t * &eye, -1.0 * &eye, zeros],
+            concatenate![Axis(1), zeros, eye, zeros, -1.0 * &eye]
+        ];
+        debug_assert_eq!(cached_jacobian.shape(), &[DOFS, DOFS * 2]);
+
+        Self { cached_jacobian }
+    }
+}
+
+impl IFactor for DynamicFactor {
+    #[inline]
+    fn name(&self) -> &'static str {
+        "DynamicFactor"
+    }
+
+    #[inline]
+    fn jacobian(&mut self, _state: &FactorState, _x: &Vector<Float>) -> Matrix<Float> {
+        self.cached_jacobian.clone()
+    }
+
+    #[inline(always)]
+    fn measure(&mut self, _state: &FactorState, x: &Vector<Float>) -> Vector<Float> {
+        self.cached_jacobian.dot(x)
+    }
+
+    #[inline(always)]
+    fn skip(&mut self, _state: &FactorState) -> bool {
+        false
+    }
+
+    #[inline(always)]
+    fn jacobian_delta(&self) -> Float {
+        1e-8
+    }
+
+    #[inline(always)]
+    fn linear(&self) -> bool {
+        true
+    }
+}
diff --git a/crates/gbpplanner-rs/src/factorgraph/factor/interrobot.rs b/crates/gbpplanner-rs/src/factorgraph/factor/interrobot.rs
new file mode 100644
index 00000000..ec82b5a4
--- /dev/null
+++ b/crates/gbpplanner-rs/src/factorgraph/factor/interrobot.rs
@@ -0,0 +1,207 @@
+use std::ops::Sub;
+
+use bevy::log::info;
+use gbp_linalg::prelude::*;
+use ndarray::s;
+use typed_floats::StrictlyPositiveFinite;
+
+use super::{FactorState, IFactor};
+use crate::factorgraph::{
+    factorgraph::{FactorGraphId, NodeIndex, VariableIndex},
+    id::{FactorId, VariableId},
+    DOFS,
+};
+
+#[derive(Debug, Clone, Copy)]
+pub struct ExternalVariableId {
+    pub factorgraph_id: FactorGraphId,
+    pub variable_index: VariableIndex,
+}
+
+impl ExternalVariableId {
+    /// Create a new `ExternalVariableId`
+    pub fn new(factorgraph_id: FactorGraphId, variable_index: VariableIndex) -> Self {
+        Self {
+            factorgraph_id,
+            variable_index,
+        }
+    }
+}
+
+/// Interrobot factor: for avoidance of other robots
+/// This factor results in a high energy or cost if two robots are planning to
+/// be in the same position at the same timestep (collision). This factor is
+/// created between variables of two robots. The factor has 0 energy if the
+/// variables are further away than the safety distance.
+#[derive(Debug, Clone)]
+pub struct InterRobotFactor {
+    safety_distance: Float,
+    skip: bool,
+    pub external_variable: ExternalVariableId,
+}
+
+impl InterRobotFactor {
+    pub const NEIGHBORS: usize = 2;
+
+    #[must_use]
+    pub fn new(
+        robot_radius: StrictlyPositiveFinite<Float>,
+        external_variable: ExternalVariableId,
+    ) -> Self {
+        let robot_radius = robot_radius.get();
+        let epsilon = 0.2 * robot_radius;
+
+        Self {
+            safety_distance: 2.0 * robot_radius + epsilon,
+            skip: false,
+            external_variable,
+        }
+    }
+
+    /// Get the safety distance
+    #[inline(always)]
+    pub fn safety_distance(&self) -> Float {
+        self.safety_distance
+    }
+}
+
+impl IFactor for InterRobotFactor {
+    #[inline(always)]
+    fn name(&self) -> &'static str {
+        "InterRobotFactor"
+    }
+
+    fn jacobian(&mut self, state: &FactorState, x: &Vector<Float>) -> Matrix<Float> {
+        let mut jacobian = Matrix::<Float>::zeros((state.initial_measurement.len(), DOFS * 2));
+        let x_diff = {
+            let offset = DOFS / 2;
+            let mut x_diff = x.slice(s![..offset]).sub(&x.slice(s![DOFS..DOFS + offset]));
+
+            // NOTE: In gbplanner, they weight this by the robot id, why they do this is
+            // unclear as a robot id should be unique, and not have any
+            // semantics of distance/weight.
+            for i in 0..offset {
+                // x_diff[i] += 1e-6 * self.connection.id_of_robot_connected_with.index() as
+                // Float;
+                // x_diff[i] += 1e-6 * self.connection.other_variable.factorgraph_id.index() as
+                // Float;
+                x_diff[i] += 1e-6 * self.external_variable.factorgraph_id.index() as Float;
+                // Add a tiny random offset to avoid div/0 errors
+            }
+            x_diff
+        };
+
+        let radius = x_diff.euclidean_norm();
+        if radius <= self.safety_distance {
+            // TODO: why do we change the Jacobian if we are not outside the safety
+            // distance?
+
+            // J(0, seqN(0, n_dofs_ / 2)) = -1.f / safety_distance_ / r * X_diff;
+            jacobian
+                .slice_mut(s![0, ..DOFS / 2])
+                .assign(&(-1.0 / self.safety_distance / radius * &x_diff));
+
+            // J(0, seqN(n_dofs_, n_dofs_ / 2)) = 1.f / safety_distance_ / r * X_diff;
+            jacobian
+                .slice_mut(s![0, DOFS..DOFS + (DOFS / 2)])
+                .assign(&(1.0 / self.safety_distance / radius * &x_diff));
+        }
+        jacobian
+    }
+
+    fn measure(&mut self, state: &FactorState, x: &Vector<Float>) -> Vector<Float> {
+        let mut h = Vector::<Float>::zeros(state.initial_measurement.len());
+        let x_diff = {
+            let offset = DOFS / 2;
+            let mut x_diff = x.slice(s![..offset]).sub(&x.slice(s![DOFS..DOFS + offset]));
+            // NOTE: In gbplanner, they weight this by the robot id, why they do this is
+            // unclear as a robot id should be unique, and not have any
+            // semantics of distance/weight.
+            for i in 0..offset {
+                // Add a tiny random offset to avoid div/0 errors
+                // x_diff[i] += 1e-6 * self.connection.id_of_robot_connected_with.index() as
+                // Float;
+                x_diff[i] += 1e-6 * self.external_variable.factorgraph_id.index() as Float;
+            }
+            x_diff
+        };
+
+        let radius = x_diff.euclidean_norm();
+        let within_safety_distance = radius <= self.safety_distance;
+        // match (self.skip, within_safety_distance) {
+        //     (Skip(true), true) => {}
+        //     (Skip(true), false) => {}
+        //     (Skip(false), true) => {
+        //         self.skip = Skip(true);
+        //         info!("skip = true, radius = {}", radius);
+        //     }
+        //     (Skip(false), false) => {}
+        // };
+        if radius <= self.safety_distance {
+            if self.skip {
+                info!(
+                    "within safety distance, radius = {}, setting self.skip to false",
+                    radius
+                );
+            }
+            self.skip = false;
+            // gbpplanner: h(0) = 1.f*(1 - r/safety_distance_);
+            // NOTE: in Eigen, indexing a matrix with a single index corresponds to indexing
+            // the matrix as a flattened array in column-major order.
+            // h[(0, 0)] = 1.0 * (1.0 - radius / self.safety_distance);
+            h[0] = 1.0 * (1.0 - radius / self.safety_distance);
+            // println!("h = {}", h);
+        } else {
+            if !self.skip {
+                // info!(
+                //     "outside safety distance, radius = {}, setting self.skip
+                // to true",     radius
+                // );
+            }
+            self.skip = true;
+        }
+
+        h
+    }
+
+    #[inline(always)]
+    fn jacobian_delta(&self) -> Float {
+        1e-2
+    }
+
+    fn skip(&mut self, state: &FactorState) -> bool {
+        // this->skip_flag = ( (X_(seqN(0,n_dofs_/2)) - X_(seqN(n_dofs_,
+        // n_dofs_/2))).squaredNorm() >= safety_distance_*safety_distance_ );
+        let offset = DOFS / 2;
+
+        // [..offset] is the position of the first variable
+        // [dofs..dofs + offset] is the position of the other variable
+
+        let difference_between_estimated_positions = state
+            .linearisation_point
+            .slice(s![..offset])
+            .sub(&state.linearisation_point.slice(s![DOFS..DOFS + offset]));
+        let squared_norm = difference_between_estimated_positions
+            .mapv(|x| x.powi(2))
+            .sum();
+
+        let skip = squared_norm >= self.safety_distance.powi(2);
+        // let skip = squared_norm >= Float::powi(self.safety_distance, 2);
+        if self.skip != skip {
+            // warn!(
+            //     "skip = {}, squared_norm = {} safety_distance^2 = {}",
+            //     skip,
+            //     squared_norm,
+            //     Float::powi(self.safety_distance, 2)
+            // );
+        }
+        self.skip = skip;
+        // self.skip = squared_norm >= Float::powi(self.safety_distance, 2);
+        self.skip
+    }
+
+    #[inline(always)]
+    fn linear(&self) -> bool {
+        false
+    }
+}
diff --git a/crates/gbpplanner-rs/src/planner/marginalise_factor_distance.rs b/crates/gbpplanner-rs/src/factorgraph/factor/marginalise_factor_distance.rs
similarity index 60%
rename from crates/gbpplanner-rs/src/planner/marginalise_factor_distance.rs
rename to crates/gbpplanner-rs/src/factorgraph/factor/marginalise_factor_distance.rs
index 99a870ad..daf80a5a 100644
--- a/crates/gbpplanner-rs/src/planner/marginalise_factor_distance.rs
+++ b/crates/gbpplanner-rs/src/factorgraph/factor/marginalise_factor_distance.rs
@@ -2,14 +2,11 @@ use gbp_linalg::prelude::*;
 use ndarray::prelude::*;
 use ndarray_inverse::Inverse;
 
-use super::message::Message;
-use crate::planner::message::{Eta, Lam, Mu};
-
-/// Utility function to create `start..start + n`
-/// Similar to `Eigen::seqN`
-const fn seq_n(start: usize, n: usize) -> std::ops::Range<usize> {
-    start..start + n
-}
+use crate::factorgraph::{
+    message::{InformationVec, Mean, PrecisionMatrix},
+    prelude::Message,
+    DOFS,
+};
 
 type Aa<'a, T> = MatrixView<'a, T>;
 type Ab<'a, T> = MatrixView<'a, T>;
@@ -18,29 +15,28 @@ type Bb<'a, T> = MatrixView<'a, T>;
 
 fn extract_submatrices_from_precision_matrix<T: GbpFloat>(
     precision_matrix: &Matrix<T>,
-    dofs: usize,
     marg_idx: usize,
 ) -> (Aa<T>, Ab<T>, Ba<T>, Bb<T>) {
     debug_assert!(precision_matrix.is_square());
-    debug_assert_eq!(precision_matrix.nrows() % dofs, 0);
-    debug_assert_eq!(precision_matrix.ncols() % dofs, 0);
+    debug_assert_eq!(precision_matrix.nrows() % DOFS, 0);
+    debug_assert_eq!(precision_matrix.ncols() % DOFS, 0);
 
-    let aa = precision_matrix.slice(s![seq_n(marg_idx, dofs), seq_n(marg_idx, dofs)]);
+    let aa = precision_matrix.slice(s![seq_n(marg_idx, DOFS), seq_n(marg_idx, DOFS)]);
 
     let ab = if marg_idx == 0 {
-        precision_matrix.slice(s![seq_n(marg_idx, dofs), marg_idx + dofs..])
+        precision_matrix.slice(s![seq_n(marg_idx, DOFS), marg_idx + DOFS..])
     } else {
-        precision_matrix.slice(s![seq_n(marg_idx, dofs), ..marg_idx])
+        precision_matrix.slice(s![seq_n(marg_idx, DOFS), ..marg_idx])
     };
 
     let ba = if marg_idx == 0 {
-        precision_matrix.slice(s![marg_idx + dofs.., seq_n(marg_idx, dofs)])
+        precision_matrix.slice(s![marg_idx + DOFS.., seq_n(marg_idx, DOFS)])
     } else {
-        precision_matrix.slice(s![..marg_idx, seq_n(marg_idx, dofs)])
+        precision_matrix.slice(s![..marg_idx, seq_n(marg_idx, DOFS)])
     };
 
     let bb = if marg_idx == 0 {
-        precision_matrix.slice(s![marg_idx + dofs.., marg_idx + dofs..])
+        precision_matrix.slice(s![marg_idx + DOFS.., marg_idx + DOFS..])
     } else {
         precision_matrix.slice(s![..marg_idx, ..marg_idx])
     };
@@ -51,132 +47,53 @@ fn extract_submatrices_from_precision_matrix<T: GbpFloat>(
 pub fn marginalise_factor_distance(
     information_vector: Vector<Float>,
     precision_matrix: Matrix<Float>,
-    variable_dofs: usize,
     marginalisation_idx: usize,
 ) -> Result<Message, &'static str> {
-    let ndofs = variable_dofs;
+    // let ndofs = variable_dofs;
     let marg_idx = marginalisation_idx;
 
     debug_assert_eq!(information_vector.len(), precision_matrix.nrows());
     debug_assert_eq!(precision_matrix.nrows(), precision_matrix.ncols());
-    // pretty_print_vector!(&information_vector);
-    // pretty_print_matrix!(&precision_matrix);
 
-    let factor_only_connected_to_one_variable = information_vector.len() == variable_dofs;
+    let factor_only_connected_to_one_variable = information_vector.len() == DOFS;
     if factor_only_connected_to_one_variable {
         let mu = Vector::<Float>::zeros(information_vector.len());
         return Ok(Message::new(
-            Eta(information_vector),
-            Lam(precision_matrix),
-            Mu(mu),
+            InformationVec(information_vector),
+            PrecisionMatrix(precision_matrix),
+            Mean(mu),
         ));
-        // dbg!(&information_vector);
-        // dbg!(&precision_matrix);
-        // TODO: return None
-        // let mvn = MultivariateNormal::from_information_and_precision(
-        //     information_vector,
-        //     precision_matrix,
-        // )
-        // .inspect_err(|_| {
-        //     // pretty_print_matrix!(&precision_matrix);
-        // })
-        // .expect(
-        //     "the given information vector and precision matrix is a valid
-        // multivariate gaussian", );
-        // return Ok(Message::new(mvn));
     }
 
-    // eprintln!(
-    //     "information_vector shape = {:?}, ndofs = {:?}",
-    //     information_vector.shape(),
-    //     variable_dofs
-    // );
-    // eprintln!("precision_matrix shape = {:?}", precision_matrix.shape());
-
-    // eprintln!("show me precision_matrix = \n{:?}", precision_matrix);
-
-    // let iv = &information_vector;
-    // let pm = &precision_matrix;
-
-    let eta_a = information_vector.slice(s![seq_n(marg_idx, ndofs)]);
-    assert_eq!(eta_a.len(), ndofs);
+    let eta_a = information_vector.slice(s![seq_n(marg_idx, DOFS)]);
+    assert_eq!(eta_a.len(), DOFS);
 
     let eta_b = if marg_idx == 0 {
-        information_vector.slice(s![ndofs..])
+        information_vector.slice(s![DOFS..])
     } else {
         information_vector.slice(s![..marg_idx])
     };
-    assert_eq!(eta_b.len(), information_vector.len() - ndofs);
+    assert_eq!(eta_b.len(), information_vector.len() - DOFS);
 
     let (lam_aa, lam_ab, lam_ba, lam_bb) =
-        extract_submatrices_from_precision_matrix(&precision_matrix, ndofs, marg_idx);
-
-    // let lam_aa = precision_matrix.slice(s![seq_n(marg_idx, ndofs),
-    // seq_n(marg_idx, ndofs)]);
-
-    // let lam_ab = if marg_idx == 0 {
-    //     precision_matrix.slice(s![seq_n(marg_idx, ndofs), marg_idx + ndofs..])
-    // } else {
-    //     precision_matrix.slice(s![seq_n(marg_idx, ndofs), ..marg_idx])
-    // };
-
-    // assert_eq!(lam_ab.shape(), &[ndofs, precision_matrix.ncols() - ndofs]);
-
-    // // eprintln!("margin_idx = {}", marg_idx);
-
-    // let lam_ba = if marg_idx == 0 {
-    //     precision_matrix.slice(s![marg_idx + ndofs.., seq_n(marg_idx, ndofs)])
-    // } else {
-    //     precision_matrix.slice(s![..marg_idx, seq_n(marg_idx, ndofs)])
-    // };
-
-    // let lam_bb = if marg_idx == 0 {
-    //     precision_matrix.slice(s![marg_idx + ndofs.., marg_idx + ndofs..])
-    // } else {
-    //     precision_matrix.slice(s![..marg_idx, ..marg_idx])
-    // };
-
-    // assert_eq!(
-    //     lam_bb.shape(),
-    //     &[
-    //         precision_matrix.shape()[0] - ndofs,
-    //         precision_matrix.shape()[1] - ndofs
-    //     ]
-    // );
-
-    // eprintln!("lam_bb = {:?}", lam_bb);
+        extract_submatrices_from_precision_matrix(&precision_matrix, marg_idx);
 
     let Some(lam_bb_inv) = lam_bb.to_owned().inv() else {
-        return Ok(Message::empty(ndofs));
+        return Ok(Message::empty());
     };
 
-    // let lam_bb_inv = lam_bb.to_owned().inv().expect("should have an inverse");
-    // let information_vector = (&eta_a - &lam_ab).dot(&lam_bb_inv).dot(&eta_b);
     let information_vector = &eta_a - &lam_ab.dot(&lam_bb_inv).dot(&eta_b);
-    // eprintln!("information_vector = {:?}", information_vector);
     let precision_matrix = &lam_aa - &lam_ab.dot(&lam_bb_inv).dot(&lam_ba);
-    // eprintln!("precision_matrix = {:?}", precision_matrix);
 
     if precision_matrix.iter().any(|elem| elem.is_infinite()) {
-        Ok(Message::empty(information_vector.len()))
-        // Message::with_dofs(information_vector.len())
-        // Message::zeros(information_vector.len())
+        Ok(Message::empty())
     } else {
         let mu = Vector::<Float>::zeros(information_vector.len());
         Ok(Message::new(
-            Eta(information_vector),
-            Lam(precision_matrix),
-            Mu(mu),
+            InformationVec(information_vector),
+            PrecisionMatrix(precision_matrix),
+            Mean(mu),
         ))
-
-        // let mvn = MultivariateNormal::from_information_and_precision(
-        //     information_vector,
-        //     precision_matrix,
-        // )
-        // .expect(
-        //     "the given information vector and precision matrix is a valid
-        // multivariate gaussian", );
-        // Ok(Message::new(mvn))
     }
 }
 
@@ -240,7 +157,7 @@ mod tests {
 
         assert!(precision_matrix.is_square());
 
-        let (aa, ab, ba, bb) = extract_submatrices_from_precision_matrix(&precision_matrix, 4, 0);
+        let (aa, ab, ba, bb) = extract_submatrices_from_precision_matrix(&precision_matrix, 0);
 
         assert_eq!(aa, upper_left);
         assert_eq!(ab, upper_right);
@@ -255,7 +172,7 @@ mod tests {
 
         assert!(precision_matrix.is_square());
 
-        let (aa, ab, ba, bb) = extract_submatrices_from_precision_matrix(&precision_matrix, 4, 4);
+        let (aa, ab, ba, bb) = extract_submatrices_from_precision_matrix(&precision_matrix, 4);
 
         assert_eq!(aa, lower_right);
         assert_eq!(ab, lower_left);
@@ -272,21 +189,19 @@ mod tests {
                 0., 0., 0.3, 5.
             ]];
 
-        let ndofs = 4;
         let marginalisation_idx = 0;
 
         let mut marginalised_msg = marginalise_factor_distance(
             information_vector.clone(),
             precision_matrix.clone(),
-            ndofs,
             marginalisation_idx,
         )
         .unwrap();
 
         let payload = marginalised_msg.take().unwrap();
 
-        assert_eq!(payload.eta, information_vector);
-        assert_eq!(payload.lam, precision_matrix);
+        assert_eq!(payload.information_factor, information_vector);
+        assert_eq!(payload.precision_matrix, precision_matrix);
     }
 
     // #[test]
@@ -333,3 +248,10 @@ mod tests {
     //     .collect::<Vec<_>>();
     // }
 }
+
+/// Utility function to create `start..start + n`
+/// Similar to `Eigen::seqN`
+#[inline]
+const fn seq_n(start: usize, n: usize) -> std::ops::Range<usize> {
+    start..start + n
+}
diff --git a/crates/gbpplanner-rs/src/factorgraph/factor/mod.rs b/crates/gbpplanner-rs/src/factorgraph/factor/mod.rs
new file mode 100644
index 00000000..eebb52a6
--- /dev/null
+++ b/crates/gbpplanner-rs/src/factorgraph/factor/mod.rs
@@ -0,0 +1,497 @@
+use std::ops::AddAssign;
+
+use bevy::render::texture::Image;
+use gbp_linalg::prelude::*;
+use ndarray::{array, concatenate, prelude::*, s, Axis};
+use typed_floats::StrictlyPositiveFinite;
+
+use self::{
+    dynamic::DynamicFactor, interrobot::InterRobotFactor, obstacle::ObstacleFactor,
+    pose::PoseFactor,
+};
+use super::{
+    factorgraph::NodeIndex,
+    id::VariableId,
+    message::{MessagesToFactors, MessagesToVariables},
+    node::FactorGraphNode,
+    prelude::Message,
+    MessageCount, DOFS,
+};
+use crate::factorgraph::node::RemoveConnectionToError;
+
+pub(in crate::factorgraph) mod dynamic;
+pub(in crate::factorgraph) mod interrobot;
+mod marginalise_factor_distance;
+pub(in crate::factorgraph) mod obstacle;
+pub(in crate::factorgraph) mod pose;
+
+use marginalise_factor_distance::marginalise_factor_distance;
+
+// pub use crate::factorgraph::factor::interrobot::InterRobotFactorConnection;
+pub use crate::factorgraph::factor::interrobot::ExternalVariableId;
+
+// TODO: make generic over f32 | f64
+// TODO: hide the state parameter from the public API, by having the `Factor`
+// struct expose similar methods that dispatch to the `FactorState` struct.
+pub trait IFactor {
+    /// The name of the factor. Used for debugging and visualization.
+    fn name(&self) -> &'static str;
+
+    fn jacobian_delta(&self) -> Float;
+
+    /// Whether to skip this factor in the update step
+    /// In gbpplanner, this is only used for the interrobot factor.
+    /// The other factors are always included in the update step.
+    fn skip(&mut self, state: &FactorState) -> bool;
+
+    /// Whether the factor is linear or non-linear
+    fn linear(&self) -> bool;
+
+    #[must_use]
+    #[inline]
+    fn jacobian(&mut self, state: &FactorState, x: &Vector<Float>) -> Matrix<Float> {
+        self.first_order_jacobian(state, x.clone())
+    }
+
+    /// Measurement function
+    /// **Note**: This method takes a mutable reference to self, because the
+    /// interrobot factor
+    #[must_use]
+    fn measure(&mut self, state: &FactorState, x: &Vector<Float>) -> Vector<Float>;
+
+    fn first_order_jacobian(&mut self, state: &FactorState, mut x: Vector<Float>) -> Matrix<Float> {
+        let h0 = self.measure(state, &x); // value at linearization point
+        let mut jacobian = Matrix::<Float>::zeros((h0.len(), x.len()));
+
+        let delta = self.jacobian_delta();
+
+        for i in 0..x.len() {
+            x[i] += delta; // perturb by delta
+            let derivatives = (self.measure(state, &x) - &h0) / delta;
+            jacobian.column_mut(i).assign(&derivatives);
+            x[i] -= delta; // reset the perturbation
+        }
+
+        jacobian
+    }
+}
+
+#[derive(Debug)]
+pub struct Factor {
+    /// Unique identifier that associates the variable with the factorgraph it
+    /// is part of.
+    pub node_index: Option<NodeIndex>,
+    /// State common between all factor kinds
+    pub state:      FactorState,
+    /// Variant storing the specialized behavior of each Factor kind.
+    pub kind:       FactorKind,
+    /// Mailbox for incoming message storage
+    pub inbox:      MessagesToVariables,
+
+    message_count: MessageCount,
+}
+
+impl Factor {
+    fn new(state: FactorState, kind: FactorKind) -> Self {
+        Self {
+            node_index: None,
+            state,
+            kind,
+            inbox: MessagesToVariables::new(),
+            message_count: MessageCount::default(),
+        }
+    }
+
+    /// Returns the node index of the factor
+    ///
+    /// # Panics
+    ///
+    /// Panics if the node index has not been set, which should not happen.
+    #[inline]
+    pub fn node_index(&self) -> NodeIndex {
+        if self.node_index.is_none() {
+            panic!("The node index has not been set");
+        }
+        self.node_index.unwrap()
+    }
+
+    /// Sets the node index of the factor
+    ///
+    /// # Panics
+    ///
+    /// Panics if the node index has already been set.
+    pub fn set_node_index(&mut self, node_index: NodeIndex) {
+        if self.node_index.is_some() {
+            panic!("The node index is already set");
+        }
+        self.node_index = Some(node_index);
+    }
+
+    pub fn new_dynamic_factor(strength: Float, measurement: Vector<Float>, delta_t: Float) -> Self {
+        let mut state = FactorState::new(measurement, strength, DynamicFactor::NEIGHBORS);
+        let dynamic_factor = DynamicFactor::new(&mut state, delta_t);
+        let kind = FactorKind::Dynamic(dynamic_factor);
+        Self::new(state, kind)
+    }
+
+    pub fn new_interrobot_factor(
+        strength: Float,
+        measurement: Vector<Float>,
+        safety_radius: StrictlyPositiveFinite<Float>,
+        external_variable: ExternalVariableId,
+    ) -> Self {
+        let interrobot_factor = InterRobotFactor::new(safety_radius, external_variable);
+        let kind = FactorKind::InterRobot(interrobot_factor);
+        let state = FactorState::new(measurement, strength, InterRobotFactor::NEIGHBORS);
+
+        Self::new(state, kind)
+    }
+
+    pub fn new_pose_factor() -> Self {
+        unimplemented!("the pose factor is stored in the variable")
+    }
+
+    pub fn new_obstacle_factor(
+        strength: Float,
+        measurement: Vector<Float>,
+        obstacle_sdf: &'static Image,
+        world_size: Float,
+    ) -> Self {
+        let state = FactorState::new(measurement, strength, ObstacleFactor::NEIGHBORS);
+        let obstacle_factor = ObstacleFactor::new(obstacle_sdf, world_size);
+        let kind = FactorKind::Obstacle(obstacle_factor);
+        Self::new(state, kind)
+    }
+
+    #[inline(always)]
+    pub fn variant(&self) -> &'static str {
+        self.kind.name()
+    }
+
+    #[inline(always)]
+    fn jacobian(&mut self, x: &Vector<Float>) -> Matrix<Float> {
+        self.kind.jacobian(&self.state, x)
+    }
+
+    fn measure(&mut self, x: &Vector<Float>) -> Vector<Float> {
+        self.state.cached_measurement = self.kind.measure(&self.state, x);
+        self.state.cached_measurement.clone()
+    }
+
+    /// Check if the factor should be skipped in the update step
+    #[inline(always)]
+    fn skip(&mut self) -> bool {
+        self.kind.skip(&self.state)
+    }
+
+    pub fn receive_message_from(&mut self, from: VariableId, message: Message) {
+        let _ = self.inbox.insert(from, message);
+        self.message_count.received += 1;
+    }
+
+    #[inline(always)]
+    pub fn read_message_from(&mut self, from: VariableId) -> Option<&Message> {
+        self.inbox.get(&from)
+    }
+
+    /// Calculates the residual between the current measurement and the initial
+    /// measurement
+    #[inline(always)]
+    #[must_use]
+    fn residual(&self) -> Vector<Float> {
+        &self.state.initial_measurement - &self.state.cached_measurement
+    }
+
+    #[must_use]
+    pub fn update(&mut self) -> MessagesToVariables {
+        debug_assert_eq!(
+            self.state.linearisation_point.len(),
+            DOFS * self.inbox.len()
+        );
+
+        let zero_mean = Vector::<Float>::zeros(DOFS);
+
+        for (i, (_, message)) in self.inbox.iter().enumerate() {
+            let mean = message.mean().unwrap_or(&zero_mean);
+            self.state
+                .linearisation_point
+                .slice_mut(s![i * DOFS..(i + 1) * DOFS])
+                .assign(mean);
+        }
+
+        if self.skip() {
+            let messages: MessagesToVariables = self
+                .inbox
+                .iter()
+                .map(|(variable_id, _)| (*variable_id, Message::empty()))
+                .collect();
+            self.message_count.sent += messages.len();
+            return messages;
+        }
+
+        let meas = self.measure(&self.state.linearisation_point.clone());
+        let jacobian = self.jacobian(&self.state.linearisation_point.clone());
+
+        let potential_precision_matrix = jacobian
+            .t()
+            .dot(&self.state.measurement_precision)
+            .dot(&jacobian);
+        let potential_information_vec = jacobian
+            .t()
+            .dot(&self.state.measurement_precision)
+            .dot(&(jacobian.dot(&self.state.linearisation_point) + self.residual()));
+
+        self.state.initialized = true;
+
+        let mut marginalisation_idx = 0;
+        let mut messages = MessagesToVariables::new();
+
+        let zero_precision = Matrix::<Float>::zeros((DOFS, DOFS));
+
+        for variable_id in self.inbox.keys() {
+            let mut information_vec = potential_information_vec.clone();
+            let mut precision_matrix = potential_precision_matrix.clone();
+
+            for (j, (other_variable_id, other_message)) in self.inbox.iter().enumerate() {
+                if other_variable_id == variable_id {
+                    // Do not aggregate data from the variable we're sending to
+                    continue;
+                }
+
+                let message_eta = other_message
+                    .information_vector()
+                    .expect("it better be there");
+
+                // let message_precision =
+                // other_message.precision_matrix().unwrap_or(&zero_precision);
+                let message_precision = other_message
+                    .precision_matrix()
+                    .unwrap_or_else(|| &zero_precision);
+                // .unwrap_or_else(|| &Matrix::<Float>::zeros((DOFS, DOFS)));
+
+                information_vec
+                    .slice_mut(s![j * DOFS..(j + 1) * DOFS])
+                    .add_assign(message_eta);
+                precision_matrix
+                    .slice_mut(s![j * DOFS..(j + 1) * DOFS, j * DOFS..(j + 1) * DOFS])
+                    .add_assign(message_precision);
+            }
+
+            let message =
+                marginalise_factor_distance(information_vec, precision_matrix, marginalisation_idx)
+                    .expect("marginalise_factor_distance should not fail");
+            messages.insert(*variable_id, message);
+            marginalisation_idx += DOFS;
+        }
+
+        self.message_count.sent += messages.len();
+        messages
+    }
+
+    /// Check if the factor is an [`InterRobotFactor`]
+    #[inline(always)]
+    pub fn is_inter_robot(&self) -> bool {
+        self.kind.is_inter_robot()
+    }
+
+    /// Check if the factor is a [`DynamicFactor`]
+    #[inline(always)]
+    pub fn is_dynamic(&self) -> bool {
+        self.kind.is_dynamic()
+    }
+
+    /// Check if the factor is an [`ObstacleFactor`]
+    #[inline(always)]
+    pub fn is_obstacle(&self) -> bool {
+        self.kind.is_obstacle()
+    }
+
+    /// Check if the factor is a [`PoseFactor`]
+    #[inline(always)]
+    pub fn is_pose(&self) -> bool {
+        self.kind.is_pose()
+    }
+}
+
+#[derive(Debug, derive_more::IsVariant)]
+pub enum FactorKind {
+    Pose(PoseFactor),
+    InterRobot(InterRobotFactor),
+    Dynamic(DynamicFactor),
+    Obstacle(ObstacleFactor),
+}
+
+impl FactorKind {
+    /// Returns `Some(&InterRobotFactor)` if self is [`InterRobot`], otherwise
+    pub fn as_inter_robot(&self) -> Option<&InterRobotFactor> {
+        if let Self::InterRobot(v) = self {
+            Some(v)
+        } else {
+            None
+        }
+    }
+
+    /// Returns `Some(&DynamicFactor)` if self is [`Dynamic`], otherwise
+    pub fn as_dynamic(&self) -> Option<&DynamicFactor> {
+        if let Self::Dynamic(v) = self {
+            Some(v)
+        } else {
+            None
+        }
+    }
+
+    /// Returns `Some(&ObstacleFactor)` if self is [`Obstacle`], otherwise
+    pub fn as_obstacle(&self) -> Option<&ObstacleFactor> {
+        if let Self::Obstacle(v) = self {
+            Some(v)
+        } else {
+            None
+        }
+    }
+
+    /// Returns `Some(&PoseFactor)` if self is [`Pose`], otherwise `None`.
+    pub fn as_pose(&self) -> Option<&PoseFactor> {
+        if let Self::Pose(v) = self {
+            Some(v)
+        } else {
+            None
+        }
+    }
+}
+
+impl IFactor for FactorKind {
+    fn name(&self) -> &'static str {
+        match self {
+            FactorKind::Pose(f) => f.name(),
+            FactorKind::InterRobot(f) => f.name(),
+            FactorKind::Dynamic(f) => f.name(),
+            FactorKind::Obstacle(f) => f.name(),
+        }
+    }
+
+    fn jacobian(&mut self, state: &FactorState, x: &Vector<Float>) -> Matrix<Float> {
+        match self {
+            FactorKind::Pose(f) => f.jacobian(state, x),
+            FactorKind::InterRobot(f) => f.jacobian(state, x),
+            FactorKind::Dynamic(f) => f.jacobian(state, x),
+            FactorKind::Obstacle(f) => f.jacobian(state, x),
+        }
+    }
+
+    fn measure(&mut self, state: &FactorState, x: &Vector<Float>) -> Vector<Float> {
+        match self {
+            FactorKind::Pose(f) => f.measure(state, x),
+            FactorKind::InterRobot(f) => f.measure(state, x),
+            FactorKind::Dynamic(f) => f.measure(state, x),
+            FactorKind::Obstacle(f) => f.measure(state, x),
+        }
+    }
+
+    fn skip(&mut self, state: &FactorState) -> bool {
+        match self {
+            FactorKind::Pose(f) => f.skip(state),
+            FactorKind::InterRobot(f) => f.skip(state),
+            FactorKind::Dynamic(f) => f.skip(state),
+            FactorKind::Obstacle(f) => f.skip(state),
+        }
+    }
+
+    fn jacobian_delta(&self) -> Float {
+        match self {
+            FactorKind::Pose(f) => f.jacobian_delta(),
+            FactorKind::InterRobot(f) => f.jacobian_delta(),
+            FactorKind::Dynamic(f) => f.jacobian_delta(),
+            FactorKind::Obstacle(f) => f.jacobian_delta(),
+        }
+    }
+
+    fn linear(&self) -> bool {
+        match self {
+            FactorKind::Pose(f) => f.linear(),
+            FactorKind::InterRobot(f) => f.linear(),
+            FactorKind::Dynamic(f) => f.linear(),
+            FactorKind::Obstacle(f) => f.linear(),
+        }
+    }
+}
+
+#[derive(Debug, Clone)]
+pub struct FactorState {
+    /// called `z_` in **gbpplanner**
+    pub initial_measurement: Vector<Float>,
+    /// called `meas_model_lambda_` in **gbpplanner**
+    pub measurement_precision: Matrix<Float>,
+    /// Stored linearisation point
+    /// called `X_` in **gbpplanner**, they use `Eigen::MatrixXd` instead
+    pub linearisation_point: Vector<Float>,
+    /// Strength of the factor. Called `sigma` in gbpplanner.
+    /// The factor precision $Lambda = sigma^-2 * Identify$
+    pub strength: Float,
+
+    /// Cached value of the factors jacobian function
+    /// called `J_` in **gbpplanner**
+    /// TODO: wrap in Option<>
+    pub cached_jacobian: Matrix<Float>,
+
+    /// Cached value of the factors jacobian function
+    /// called `h_` in **gbpplanner**
+    /// TODO: wrap in Option<>
+    pub cached_measurement: Vector<Float>,
+    /// Set to true after the first call to self.update()
+    initialized: bool,
+}
+
+impl FactorState {
+    /// Create a new [`FactorState`]
+    fn new(initial_measurement: Vector<Float>, strength: Float, neighbor_amount: usize) -> Self {
+        // Initialise precision of the measurement function
+        // this->meas_model_lambda_ = Eigen::MatrixXd::Identity(z_.rows(), z_.rows()) /
+        // pow(sigma,2.);
+        let measurement_precision =
+            Matrix::<Float>::eye(initial_measurement.len()) / Float::powi(strength, 2);
+
+        Self {
+            initial_measurement,
+            measurement_precision,
+            linearisation_point: Vector::<Float>::zeros(DOFS * neighbor_amount),
+            strength,
+            cached_jacobian: array![[]],
+            cached_measurement: array![],
+            initialized: false,
+        }
+    }
+}
+
+impl FactorGraphNode for Factor {
+    fn remove_connection_to(
+        &mut self,
+        factorgraph_id: super::factorgraph::FactorGraphId,
+    ) -> Result<(), RemoveConnectionToError> {
+        let connections_before = self.inbox.len();
+        self.inbox
+            .retain(|variable_id, v| variable_id.factorgraph_id != factorgraph_id);
+        let connections_after = self.inbox.len();
+
+        let no_connections_removed = connections_before == connections_after;
+        if no_connections_removed {
+            Err(RemoveConnectionToError)
+        } else {
+            Ok(())
+        }
+    }
+
+    #[inline(always)]
+    fn messages_sent(&self) -> usize {
+        self.message_count.sent
+    }
+
+    #[inline(always)]
+    fn messages_received(&self) -> usize {
+        self.message_count.received
+    }
+
+    #[inline(always)]
+    fn reset_message_count(&mut self) {
+        self.message_count.reset();
+    }
+}
diff --git a/crates/gbpplanner-rs/src/factorgraph/factor/obstacle.rs b/crates/gbpplanner-rs/src/factorgraph/factor/obstacle.rs
new file mode 100644
index 00000000..e312c491
--- /dev/null
+++ b/crates/gbpplanner-rs/src/factorgraph/factor/obstacle.rs
@@ -0,0 +1,128 @@
+//! Obstacle factor
+
+use bevy::{log::warn, render::texture::Image};
+use gbp_linalg::{prelude::*, Float};
+use ndarray::{array, concatenate, s};
+
+use super::{FactorState, IFactor};
+
+#[derive(Clone)]
+pub struct ObstacleFactor {
+    /// The signed distance field of the environment
+    obstacle_sdf: &'static Image,
+    /// Copy of the `WORLD_SZ` setting from **gbpplanner**, that we store a copy
+    /// of here since `ObstacleFactor` needs this information to calculate
+    /// `.jacobian_delta()` and `.measurement()`
+    world_size:   Float,
+}
+
+impl std::fmt::Debug for ObstacleFactor {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        f.debug_struct("ObstacleFactor")
+            // .field("obstacle_sdf", &self.obstacle_sdf)
+            .field("world_size", &self.world_size)
+            .finish()
+    }
+}
+
+impl ObstacleFactor {
+    pub const NEIGHBORS: usize = 1;
+
+    /// Creates a new [`ObstacleFactor`].
+    #[must_use]
+    pub fn new(obstacle_sdf: &'static Image, world_size: Float) -> Self {
+        Self {
+            obstacle_sdf,
+            world_size,
+        }
+    }
+}
+
+impl IFactor for ObstacleFactor {
+    #[inline]
+    fn name(&self) -> &'static str {
+        "ObstacleFactor"
+    }
+
+    #[inline]
+    fn jacobian(&mut self, state: &FactorState, x: &Vector<Float>) -> Matrix<Float> {
+        // Same as PoseFactor
+        // TODO: change to not clone x
+        self.first_order_jacobian(state, x.clone())
+    }
+
+    fn measure(&mut self, _state: &FactorState, x: &Vector<Float>) -> Vector<Float> {
+        // pretty_print_vector!(x);
+        debug_assert!(x.len() >= 2, "x.len() = {}", x.len());
+        // White areas are obstacles, so h(0) should return a 1 for these regions.
+        let scale = self.obstacle_sdf.width() as Float / self.world_size;
+        // let offset = (self.world_size / 2.0) as usize;
+        let offset = self.world_size / 2.0;
+        if (x[0] + offset) * scale > self.obstacle_sdf.width() as Float {
+            // warn!(
+            //     "x[0] + offset = {}, scale = {}, width = {}",
+            //     (x[0] + offset) * scale,
+            //     scale,
+            //     self.obstacle_sdf.width()
+            // );
+            return array![0.0];
+        }
+        if (x[1] + offset) * scale > self.obstacle_sdf.height() as Float {
+            // warn!(
+            //     "x[1] + offset = {}, scale = {}, height = {}",
+            //     (x[1] + offset) * scale,
+            //     scale,
+            //     self.obstacle_sdf.height()
+            // );
+            return array![0.0];
+        }
+        // dbg!(offset);
+        let pixel_x = ((x[0] + offset) * scale) as u32;
+        let pixel_y = ((x[1] + offset) * scale) as u32;
+        // println!("pixel_x = {}, pixel_y = {}", pixel_x, pixel_y);
+        // dbg!(pixel_x, pixel_y);
+        // assert_eq!((self.obstacle_sdf.width() * self.obstacle_sdf.height() * 4) as
+        // usize, self.obstacle_sdf.data.len()); multiply by 4 because the image
+        // is in RGBA format, and we simply use th R channel to determine value,
+        // as the image is grayscale
+        // TODO: assert that the image's data is laid out in row-major order
+        let linear_index = ((self.obstacle_sdf.width() * pixel_y + pixel_x) * 4) as usize;
+        if linear_index >= self.obstacle_sdf.data.len() {
+            warn!(
+                "linear_index = {}, obstacle_sdf.data.len() = {}",
+                linear_index,
+                self.obstacle_sdf.data.len()
+            );
+            return array![0.0];
+        }
+        let red = self.obstacle_sdf.data[linear_index];
+        // NOTE: do 1.0 - red to invert the value, as the obstacle sdf is white where
+        // there are obstacles in gbpplanner, they do not do the inversion here,
+        // but instead invert the entire image, when they load it from disk.
+        let hsv_value = 1.0 - red as Float / 255.0;
+        // let hsv_value = pixel as Float / 255.0;
+        // if hsv_value <= 0.5 {
+        //     println!("image(x={}, y={}).z {} (scale = {})", pixel_x, pixel_y,
+        // hsv_value, scale); }
+        // dbg!(hsv_value);
+
+        // println!("hsv_value = {}", hsv_value);
+
+        array![hsv_value]
+    }
+
+    #[inline(always)]
+    fn jacobian_delta(&self) -> Float {
+        self.world_size / self.obstacle_sdf.width() as Float
+    }
+
+    #[inline(always)]
+    fn skip(&mut self, _state: &FactorState) -> bool {
+        false
+    }
+
+    #[inline(always)]
+    fn linear(&self) -> bool {
+        false
+    }
+}
diff --git a/crates/gbpplanner-rs/src/factorgraph/factor/pose.rs b/crates/gbpplanner-rs/src/factorgraph/factor/pose.rs
new file mode 100644
index 00000000..f8cc2b1f
--- /dev/null
+++ b/crates/gbpplanner-rs/src/factorgraph/factor/pose.rs
@@ -0,0 +1,41 @@
+//! Pose factor
+
+use gbp_linalg::prelude::*;
+
+use super::{FactorState, IFactor};
+
+#[derive(Debug)]
+pub struct PoseFactor;
+
+impl IFactor for PoseFactor {
+    #[inline(always)]
+    fn name(&self) -> &'static str {
+        "PoseFactor"
+    }
+
+    /// Default jacobian is the first order taylor series jacobian
+    fn jacobian(&mut self, state: &FactorState, x: &Vector<Float>) -> Matrix<Float> {
+        self.first_order_jacobian(state, x.clone())
+    }
+
+    /// Default measurement function is the identity function
+    #[inline(always)]
+    fn measure(&mut self, _state: &FactorState, x: &Vector<Float>) -> Vector<Float> {
+        x.clone()
+    }
+
+    #[inline(always)]
+    fn skip(&mut self, _state: &FactorState) -> bool {
+        false
+    }
+
+    #[inline(always)]
+    fn jacobian_delta(&self) -> Float {
+        1e-8
+    }
+
+    #[inline(always)]
+    fn linear(&self) -> bool {
+        false
+    }
+}
diff --git a/crates/gbpplanner-rs/src/planner/factorgraph.rs b/crates/gbpplanner-rs/src/factorgraph/factorgraph.rs
similarity index 52%
rename from crates/gbpplanner-rs/src/planner/factorgraph.rs
rename to crates/gbpplanner-rs/src/factorgraph/factorgraph.rs
index d74b1711..7cb6ad7c 100644
--- a/crates/gbpplanner-rs/src/planner/factorgraph.rs
+++ b/crates/gbpplanner-rs/src/factorgraph/factorgraph.rs
@@ -1,445 +1,84 @@
-#![warn(missing_docs)]
-use std::{collections::BTreeMap, ops::Range};
+use std::ops::Range;
 
-use bevy::prelude::*;
-use gbp_linalg::{Float, Vector};
-// use itertools::Itertools;
+use bevy::{
+    ecs::{component::Component, entity::Entity},
+    log::{debug, info},
+};
+// use gbp_linalg::Float;
+use gbp_linalg::prelude::*;
 use petgraph::Undirected;
-use tap::Tap;
 
 use super::{
-    factor::{Factor, FactorKind},
-    message::Message,
-    robot::RobotId,
+    factor::{interrobot::InterRobotFactor, Factor, FactorKind},
+    id::{FactorId, VariableId},
+    message::{
+        FactorToVariableMessage, InformationVec, Mean, PrecisionMatrix, VariableToFactorMessage,
+    },
+    node::{FactorGraphNode, Node, NodeKind, RemoveConnectionToError},
+    prelude::Message,
     variable::Variable,
 };
-use crate::{
-    escape_codes::*,
-    planner::message::{Eta, Lam, Mu},
-};
-
-#[derive(Debug, derive_more::Display)]
-#[display(fmt = "no connection to the given factorgraph")]
-pub struct RemoveConnectionToError;
-
-impl std::error::Error for RemoveConnectionToError {}
 
-// pub
-
-pub(super) trait FactorGraphNode {
-    #[must_use]
-    fn remove_connection_to(
-        &mut self,
-        factorgraph_id: FactorGraphId,
-    ) -> Result<(), RemoveConnectionToError>;
+/// type alias used to represent the id of the factorgraph
+/// Since we use **Bevy** we can use the `Entity` id of the whatever entity the
+/// the factorgraph is attached to as a Component, as its unique identifier.
+pub type FactorGraphId = Entity;
 
-    fn messages_sent(&self) -> usize;
-    fn messages_received(&self) -> usize;
+/// The type used to represent indices into the nodes of the factorgraph.
+/// This is just a type alias for `petgraph::graph::NodeIndex`, but
+/// we make an alias for it here, such that it is easier to use the same
+/// index type across modules, as the various node index types `petgraph`
+/// are not interchangeable.
+pub(crate) type NodeIndex = petgraph::stable_graph::NodeIndex;
+/// The type used to represent indices into the nodes of the factorgraph.
+pub type EdgeIndex = petgraph::stable_graph::EdgeIndex;
+/// A factorgraph is an undirected graph
+pub type Graph = petgraph::stable_graph::StableGraph<Node, (), Undirected, u32>;
 
-    fn reset_message_count(&mut self);
-}
+/// A newtype used to enforce type safety of the indices of the factors in the
+/// factorgraph.
+#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, derive_more::From)]
+pub struct FactorIndex(pub NodeIndex);
 
-#[derive(Debug, Default, Clone, Copy)]
-pub(super) struct MessageCount {
-    pub sent:     usize,
-    pub received: usize,
-}
+impl std::ops::Deref for FactorIndex {
+    type Target = NodeIndex;
 
-impl MessageCount {
-    pub fn reset(&mut self) {
-        self.sent = 0;
-        self.received = 0;
+    #[inline(always)]
+    fn deref(&self) -> &Self::Target {
+        &self.0
     }
 }
 
-impl std::ops::Add for MessageCount {
-    type Output = Self;
-
-    fn add(self, rhs: Self) -> Self::Output {
-        MessageCount {
-            sent:     self.sent + rhs.sent,
-            received: self.received + rhs.received,
-        }
+impl From<FactorIndex> for usize {
+    fn from(index: FactorIndex) -> Self {
+        index.0.index()
     }
 }
 
-// /// How the messages are passed between factors and variables in the
-// connected /// factorgraphs.
-// #[derive(Debug, Clone, Copy)]
-// pub enum MessagePassingMode {
-//     /// Messages are passed within a robot's own factorgraph.
-//     Internal,
-//     /// Messages are passed between a robot factorgraph and other robots
-//     /// factorgraphs.
-//     External,
-// }
-
-/// A newtype used to enforce type safety of the indices of the factors in the
-/// factorgraph.
-#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, derive_more::From)]
-pub struct FactorIndex(NodeIndex);
 /// A newtype used to enforce type safety of the indices of the variables in the
 /// factorgraph.
 #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, derive_more::From)]
-pub struct VariableIndex(NodeIndex);
-
-// impl From<NodeIndex> for FactorIndex {
-//     fn from(index: NodeIndex) -> Self {
-//         Self(index)
-//     }
-// }
-
-// impl From<NodeIndex> for VariableIndex {
-//     fn from(index: NodeIndex) -> Self {
-//         Self(index)
-//     }
-// }
-
-pub trait AsNodeIndex {
-    fn as_node_index(&self) -> NodeIndex;
-}
+pub struct VariableIndex(pub NodeIndex);
 
-impl From<FactorIndex> for usize {
-    #[inline]
-    fn from(index: FactorIndex) -> Self {
-        index.0.index()
+impl std::ops::Deref for VariableIndex {
+    type Target = NodeIndex;
+
+    #[inline(always)]
+    fn deref(&self) -> &Self::Target {
+        &self.0
     }
 }
 
 impl From<VariableIndex> for usize {
-    #[inline]
     fn from(index: VariableIndex) -> Self {
         index.0.index()
     }
 }
 
-impl AsNodeIndex for FactorIndex {
-    #[inline]
-    fn as_node_index(&self) -> NodeIndex {
-        self.0
-    }
-}
-
-impl AsNodeIndex for VariableIndex {
-    #[inline]
-    fn as_node_index(&self) -> NodeIndex {
-        self.0
-    }
-}
-
-#[derive(Debug, Clone, Copy)]
-pub struct FactorId {
-    pub factorgraph_id: FactorGraphId,
-    pub factor_index: FactorIndex,
-    pub color: &'static str,
-}
-
-impl std::cmp::PartialOrd for FactorId {
-    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
-        if self.factorgraph_id < other.factorgraph_id {
-            Some(std::cmp::Ordering::Less)
-        } else if self.factorgraph_id == other.factorgraph_id
-            && self.factor_index.0 < other.factor_index.0
-        {
-            Some(std::cmp::Ordering::Less)
-        } else if self.factorgraph_id == other.factorgraph_id
-            && self.factor_index == other.factor_index
-        {
-            Some(std::cmp::Ordering::Equal)
-        } else {
-            Some(std::cmp::Ordering::Greater)
-        }
-    }
-}
-
-impl std::cmp::Ord for FactorId {
-    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
-        self.partial_cmp(other)
-            .expect("every branch in partial_cmp() returns Some()")
-    }
-}
-
-// implement PartialEq and Eq manually
-impl std::cmp::PartialEq for FactorId {
-    // No need to compare color
-    fn eq(&self, other: &Self) -> bool {
-        self.factorgraph_id == other.factorgraph_id && self.factor_index == other.factor_index
-    }
-}
-
-impl std::cmp::Eq for FactorId {}
-
-macro_rules! impl_factor_id_constructor {
-    ($name:ident, color = $color:expr) => {
-        paste::paste! {
-
-            #[doc = "Create a new `FactorId` for an " $name " factor."]
-            pub fn [<new_ $name>](factorgraph_id: FactorGraphId, factor_index: FactorIndex) -> Self {
-                Self {
-                    factorgraph_id,
-                    factor_index,
-                    color: $color,
-                }
-            }
-        }
-    };
-}
-
-impl FactorId {
-    impl_factor_id_constructor!(ambiguous, color = CYAN);
-
-    impl_factor_id_constructor!(obstacle, color = RED);
-
-    impl_factor_id_constructor!(interrobot, color = GREEN);
-
-    impl_factor_id_constructor!(pose, color = MAGENTA);
-
-    impl_factor_id_constructor!(dynamic, color = BLUE);
-
-    #[inline]
-    pub fn global_id(&self) -> String {
-        format!("{:?}-{}", self.factorgraph_id, self.factor_index.0.index())
-    }
-
-    #[inline]
-    pub fn get_factor_graph_id(&self) -> FactorGraphId {
-        self.factorgraph_id
-    }
-}
-
-#[derive(Debug, Clone, Copy)]
-pub struct VariableId {
-    pub factorgraph_id: FactorGraphId,
-    pub variable_index: VariableIndex,
-    pub color: &'static str,
-}
-
-impl std::cmp::PartialOrd for VariableId {
-    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
-        if self.factorgraph_id < other.factorgraph_id {
-            Some(std::cmp::Ordering::Less)
-        } else if self.factorgraph_id == other.factorgraph_id
-            && self.variable_index.0 < other.variable_index.0
-        {
-            Some(std::cmp::Ordering::Less)
-        } else if self.factorgraph_id == other.factorgraph_id
-            && self.variable_index == other.variable_index
-        {
-            Some(std::cmp::Ordering::Equal)
-        } else {
-            Some(std::cmp::Ordering::Greater)
-        }
-    }
-}
-
-impl std::cmp::Ord for VariableId {
-    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
-        self.partial_cmp(other).unwrap()
-    }
-}
-
-// implement PartialEq and Eq manually
-impl std::cmp::PartialEq for VariableId {
-    fn eq(&self, other: &Self) -> bool {
-        self.factorgraph_id == other.factorgraph_id && self.variable_index == other.variable_index
-    }
-}
-impl std::cmp::Eq for VariableId {}
-
-impl VariableId {
-    pub fn new(factorgraph_id: FactorGraphId, variable_index: VariableIndex) -> Self {
-        Self {
-            factorgraph_id,
-            variable_index,
-            color: YELLOW,
-        }
-    }
-
-    pub fn global_id(&self) -> String {
-        format!(
-            "{:?}-{}",
-            self.factorgraph_id,
-            self.variable_index.0.index()
-        )
-    }
-
-    pub fn get_factor_graph_id(&self) -> FactorGraphId {
-        self.factorgraph_id
-    }
-}
-
-#[derive(Debug)]
-pub struct VariableToFactorMessage {
-    pub from:    VariableId,
-    pub to:      FactorId,
-    pub message: Message,
-}
-
-#[derive(Debug)]
-pub struct FactorToVariableMessage {
-    pub from:    FactorId,
-    pub to:      VariableId,
-    pub message: Message,
-}
-
-pub type MessagesFromVariables = BTreeMap<FactorId, Message>;
-pub type MessagesFromFactors = BTreeMap<VariableId, Message>;
-
-pub type MessagesToVariables = BTreeMap<VariableId, Message>;
-pub type MessagesToFactors = BTreeMap<FactorId, Message>;
-
-#[derive(Debug)]
-pub enum NodeKind {
-    Factor(Factor),
-    // TODO: wrap in Box<>
-    Variable(Variable),
-}
-
-#[derive(Debug)]
-pub struct Node {
-    // TODO: change to factorgraph_id
-    robot_id: RobotId,
-    kind:     NodeKind,
-}
-
-// #[derive(Debug, Clone, Copy)]
-// pub enum EdgeConnection {
-//     Inter,
-//     Intra,
-// }
-
-impl Node {
-    // pub fn set_node_index(&mut self, index: NodeIndex) {
-    //     match self {
-    //         Self::Factor(factor) => factor.set_node_index(index),
-    //         Self::Variable(variable) => variable.set_node_index(index),
-    //     }
-    // }
-    // pub fn get_node_index(&mut self) -> NodeIndex {
-    //     match self {
-    //         Self::Factor(factor) => factor.get_node_index(),
-    //         Self::Variable(variable) => variable.get_node_index(),
-    //     }
-    // }
-
-    /// Returns `true` if the node is [`Factor`].
-    ///
-    /// [`Factor`]: Node::Factor
-    #[must_use]
-    pub fn is_factor(&self) -> bool {
-        matches!(self.kind, NodeKind::Factor(..))
-    }
-
-    /// Returns `Some(&Factor)` if the node]s variant is [`Factor`], otherwise
-    /// `None`.
-    pub fn as_factor(&self) -> Option<&Factor> {
-        if let NodeKind::Factor(ref v) = self.kind {
-            Some(v)
-        } else {
-            None
-        }
-    }
-
-    /// Returns `Some(&mut Factor)` if the node]s variant is [`Factor`],
-    /// otherwise `None`.
-    pub fn as_factor_mut(&mut self) -> Option<&mut Factor> {
-        if let NodeKind::Factor(ref mut v) = self.kind {
-            Some(v)
-        } else {
-            None
-        }
-    }
-
-    /// Returns `true` if the node is [`Variable`].
-    ///
-    /// [`Variable`]: Node::Variable
-    #[must_use]
-    pub fn is_variable(&self) -> bool {
-        matches!(self.kind, NodeKind::Variable(..))
-    }
-
-    /// Returns `Some(&Variable)` if the node]s variant is [`Variable`],
-    /// otherwise `None`.
-    pub fn as_variable(&self) -> Option<&Variable> {
-        if let NodeKind::Variable(ref v) = self.kind {
-            Some(v)
-        } else {
-            None
-        }
-    }
-
-    /// Returns `Some(&mut Variable)` if the node]s variant is [`Variable`],
-    /// otherwise `None`.
-    pub fn as_variable_mut(&mut self) -> Option<&mut Variable> {
-        if let NodeKind::Variable(ref mut v) = self.kind {
-            Some(v)
-        } else {
-            None
-        }
-    }
-}
-
-impl FactorGraphNode for Node {
-    fn remove_connection_to(
-        &mut self,
-        factorgraph_id: FactorGraphId,
-    ) -> Result<(), RemoveConnectionToError> {
-        match self.kind {
-            NodeKind::Factor(ref mut factor) => factor.remove_connection_to(factorgraph_id),
-            NodeKind::Variable(ref mut variable) => variable.remove_connection_to(factorgraph_id),
-        }
-    }
-
-    fn messages_sent(&self) -> usize {
-        match self.kind {
-            NodeKind::Factor(ref factor) => factor.messages_sent(),
-            NodeKind::Variable(ref variable) => variable.messages_sent(),
-        }
-    }
-
-    fn messages_received(&self) -> usize {
-        match self.kind {
-            NodeKind::Factor(ref factor) => factor.messages_received(),
-            NodeKind::Variable(ref variable) => variable.messages_received(),
-        }
-    }
-
-    fn reset_message_count(&mut self) {
-        match self.kind {
-            NodeKind::Factor(ref mut factor) => factor.reset_message_count(),
-            NodeKind::Variable(ref mut variable) => variable.reset_message_count(),
-        };
-    }
-}
-
-/// The type used to represent indices into the nodes of the factorgraph.
-/// This is just a type alias for `petgraph::graph::NodeIndex`, but
-/// we make an alias for it here, such that it is easier to use the same
-/// index type across modules, as the various node index types `petgraph`
-/// are not interchangeable.
-pub type NodeIndex = petgraph::stable_graph::NodeIndex;
-// pub type VariableIndex = NodeIndex;
-// pub type FactorIndex = NodeIndex;
-/// The type used to represent indices into the nodes of the factorgraph.
-pub type EdgeIndex = petgraph::stable_graph::EdgeIndex;
-/// A factorgraph is an undirected graph
-// pub type Graph = petgraph::graph::Graph<Node, (), Undirected, u32>;
-pub type Graph = petgraph::stable_graph::StableGraph<Node, (), Undirected, u32>;
-
-/// Record type used to keep track of how many factors and variables
-/// there are in the factorgraph. We keep track of these counts internally in
-/// the factorgraph, such a query for the counts, is **O(1)**.
-#[derive(Debug, Clone, Copy, Default)]
-pub struct NodeCount {
-    pub factors:   usize,
-    pub variables: usize,
-}
-
-pub type FactorGraphId = Entity;
-
 /// A factor graph is a bipartite graph consisting of two types of nodes:
 /// factors and variables.
 #[derive(Component, Debug)]
+// #[cfg_attr(feature = "bevy", derive(Component))]
 pub struct FactorGraph {
     /// The id of the factorgraph. We store a copy of it here, for convenience.
     /// **Invariants**:
@@ -452,132 +91,31 @@ pub struct FactorGraph {
     /// In **gbpplanner** the sequence in which variables are inserted/created
     /// in the graph is meaningful. `self.graph` does not capture this
     /// ordering, so we use an extra vector to manage the order in which
-    /// variables are inserted/removed from the graph. **IMPORTANT** we have
-    /// to manually ensure the invariant that `self.graph` and this field is
-    /// consistent at all time.
+    /// variables are inserted/removed from the graph.
+    ///
+    /// **IMPORTANT** we have  to manually ensure the invariant that
+    /// `self.graph` and this field is consistent at all time.
     variable_indices: Vec<NodeIndex>,
     /// List of indices of the factors in the graph. Order is not important.
     /// Used to speed up iteration over factors.
     factor_indices: Vec<NodeIndex>,
-}
-
-pub struct Factors<'a> {
-    graph: &'a Graph,
-    factor_indices: std::slice::Iter<'a, NodeIndex>,
-}
-
-impl<'a> Factors<'a> {
-    #[must_use]
-    pub fn new(graph: &'a Graph, factor_indices: &'a [NodeIndex]) -> Self {
-        Self {
-            graph,
-            factor_indices: factor_indices.iter(),
-        }
-    }
-}
-
-impl<'a> Iterator for Factors<'a> {
-    type Item = (NodeIndex, &'a Factor);
-
-    fn next(&mut self) -> Option<Self::Item> {
-        let &index = self.factor_indices.next()?;
-        let node = &self.graph[index];
-        node.as_factor().map(|factor| (index, factor))
-    }
-}
-
-pub struct Variables<'a> {
-    graph: &'a Graph,
-    variable_indices: std::slice::Iter<'a, NodeIndex>,
-}
-
-impl<'a> Variables<'a> {
-    pub fn new(graph: &'a Graph, variable_indices: &'a [NodeIndex]) -> Self {
-        Self {
-            graph,
-            variable_indices: variable_indices.iter(),
-        }
-    }
-}
 
-impl<'a> Iterator for Variables<'a> {
-    type Item = (VariableIndex, &'a Variable);
-
-    fn next(&mut self) -> Option<Self::Item> {
-        let &index = self.variable_indices.next()?;
-        let node = &self.graph[index];
-        node.as_variable()
-            .map(|variable| (VariableIndex(index), variable))
-    }
+    /// List of indices of the interrobot factors in the graph. Order is not
+    /// important. Used to speed up iteration over interrobot factors.
+    /// When querying for number of external messages sent
+    interrobot_factor_indices: Vec<NodeIndex>,
 }
 
-// pub struct VariablesMut<'a> {
-//     graph: &'a mut Graph,
-//     variable_indices: std::slice::Iter<'a, NodeIndex>,
-// }
-
-// impl<'a> VariablesMut<'a> {
-//     pub fn new(graph: &'a mut Graph, variable_indices: &'a [NodeIndex]) ->
-// Self {         Self {
-//             graph,
-//             variable_indices: variable_indices.iter(),
-//         }
-//     }
-// }
-
-// impl<'a> Iterator for VariablesMut<'a> {
-//     type Item = (VariableIndex, &'a mut Variable);
-
-//     fn next(&mut self) -> Option<Self::Item> {
-//         let &index = self.variable_indices.next()?;
-//         let node = &mut self.graph[index];
-//         let NodeKind::Variable(ref mut variable) = &mut node.kind else {
-//             panic!("A variable index either does not exist or does not point
-// to a variable node");         };
-//         Some((VariableIndex(index), variable))
-//     }
-// }
-
-// struct AdjacentVariables<'a> {
-//     graph: &'a Graph,
-//     adjacent_variables: petgraph::stable_graph::Neighbors<'a, ()>,
-// }
-//
-// impl<'a> AdjacentVariables<'a> {
-//     pub fn new(graph: &'a Graph, factor_index: FactorIndex) -> Self {
-//         Self {
-//             graph,
-//             adjacent_variables:
-// graph.neighbors(factor_index.as_node_index()),         }
-//     }
-// }
-//
-// impl<'a> Iterator for AdjacentVariables<'a> {
-//     // type Item = (RobotId, &'a Variable);
-//     type Item = VariableId;
-//
-//     fn next(&mut self) -> Option<Self::Item> {
-//         self.adjacent_variables.next().map(|node_index| {
-//             let node = &self.graph[node_index];
-//             if node.is_variable() {
-//                 VariableId::new(node.robot_id, node_index.into())
-//             } else {
-//                 panic!("A factor can only have variables as neighbors");
-//             }
-//         })
-//     }
-// }
-
 impl FactorGraph {
     /// Construct a new empty factorgraph with a given id
     #[must_use]
     pub fn new(id: FactorGraphId) -> Self {
         Self {
             id,
-            // graph: Graph::new_undirected(),
             graph: Graph::with_capacity(0, 0),
             variable_indices: Vec::new(),
             factor_indices: Vec::new(),
+            interrobot_factor_indices: Vec::new(),
         }
     }
 
@@ -590,6 +128,7 @@ impl FactorGraph {
             graph: Graph::with_capacity(nodes, edges),
             variable_indices: Vec::with_capacity(nodes),
             factor_indices: Vec::with_capacity(edges),
+            interrobot_factor_indices: Vec::new(),
         }
     }
 
@@ -599,41 +138,26 @@ impl FactorGraph {
         self.id
     }
 
-    /// Returns an `Iterator` over the variable nodes in the factorgraph.
-    #[inline]
-    pub fn variables(&self) -> Variables<'_> {
-        Variables::new(&self.graph, &self.variable_indices)
-    }
-
-    /// Returns an `Iterator` over the factor nodes in the factorgraph.
-    #[inline]
-    pub fn factors(&self) -> Factors<'_> {
-        Factors::new(&self.graph, &self.factor_indices)
-    }
-
+    /// Adds a variable to the factorgraph
+    /// Returns the index of the variable in the factorgraph
     pub fn add_variable(&mut self, variable: Variable) -> VariableIndex {
-        let node = Node {
-            robot_id: self.id,
-            kind:     NodeKind::Variable(variable),
-        };
+        let node = Node::new(self.id, NodeKind::Variable(variable));
         let node_index = self.graph.add_node(node);
         self.variable_indices.push(node_index);
         self.graph[node_index]
             .as_variable_mut()
             .expect("just added the variable to the graph in the previous statement")
             .set_node_index(node_index);
-        // info!(
-        //     "added a variable with node_index: {:?} to factorgraph: {:?}",
-        //     node_index, self.id
-        // );
+        debug!(
+            "added a variable with node_index: {:?} to factorgraph: {:?}",
+            node_index, self.id
+        );
         node_index.into()
     }
 
     pub fn add_factor(&mut self, factor: Factor) -> FactorIndex {
-        let node = Node {
-            robot_id: self.id,
-            kind:     NodeKind::Factor(factor),
-        };
+        let is_interrobot = factor.is_inter_robot();
+        let node = Node::new(self.id, NodeKind::Factor(factor));
         let node_index = self.graph.add_node(node);
         self.graph[node_index]
             .as_factor_mut()
@@ -648,75 +172,96 @@ impl FactorGraph {
             // })
             .set_node_index(node_index);
         self.factor_indices.push(node_index);
+        if is_interrobot {
+            self.interrobot_factor_indices.push(node_index);
+        }
 
         node_index.into()
     }
 
-    /// Add an edge between nodes `a` and `b` in the factorgraph.
+    /// Number of nodes in the factorgraph
     ///
-    /// **invariants**:
-    /// - Both `a` and `b` must already be in the factorgraph. Panics if any of
-    ///   the nodes does not exist.
-    pub fn add_internal_edge(
-        &mut self,
-        // variable_index: VariableIndex,
-        // factor_index: FactorIndex,
-        variable_id: VariableId,
-        factor_id: FactorId,
-    ) -> EdgeIndex {
-        let dofs = 4;
-
-        let message_to_factor = {
-            let Some(variable) =
-                self.graph[variable_id.variable_index.as_node_index()].as_variable_mut()
-            else {
-                panic!(
-                    "the variable index either does not exist or does not point to a variable node"
+    /// **Computes in O(1) time**
+    #[inline]
+    pub fn len(&self) -> usize {
+        self.graph.node_count()
+    }
+
+    /// A count over the number of variables and factors in the factorgraph
+    ///
+    /// **Computes in O(1) time**
+    #[must_use]
+    pub fn node_count(&self) -> NodeCount {
+        NodeCount {
+            factors:   self.factor_indices.len(),
+            variables: self.variable_indices.len(),
+        }
+    }
+
+    /// go through all nodes, and remove their individual connection to the
+    /// other factorgraph if none of the nodes has a connection to the other
+    /// factorgraph, then return and Error.
+    pub fn remove_connection_to(
+        &mut self,
+        factorgraph_id: FactorGraphId,
+    ) -> Result<(), RemoveConnectionToError> {
+        let mut connections_removed: usize = 0;
+        for node in self.graph.node_weights_mut() {
+            if node.remove_connection_to(factorgraph_id).is_ok() {
+                connections_removed += 1;
+            }
+        }
+
+        if connections_removed == 0 {
+            Err(RemoveConnectionToError)
+        } else {
+            Ok(())
+        }
+    }
+
+    /// Add an edge between nodes `a` and `b` in the factorgraph.
+    ///
+    /// **invariants**:
+    /// - Both `a` and `b` must already be in the factorgraph. Panics if any of
+    ///   the nodes does not exist.
+    pub fn add_internal_edge(&mut self, variable_id: VariableId, factor_id: FactorId) -> EdgeIndex {
+        let message_to_factor = {
+            let Some(variable) = self.graph[variable_id.variable_index.0].as_variable_mut() else {
+                panic!(
+                    "the variable index either does not exist or does not point to a variable node"
                 );
             };
             // TODO: explain why we send an empty message
-            variable.receive_message_from(factor_id, Message::empty(dofs));
-            // .receive_message_from(FactorId::new(self.id, factor_index),
-            // Message::empty(dofs));
+            variable.receive_message_from(factor_id, Message::empty());
 
             Message::new(
-                Eta(variable.belief.eta.clone()),
-                Lam(variable.belief.lambda.clone()),
-                Mu(variable.belief.mu.clone()),
+                InformationVec(variable.belief.information_vector.clone()),
+                PrecisionMatrix(variable.belief.precision_matrix.clone()),
+                Mean(variable.belief.mean.clone()),
             )
         };
 
-        let node = &mut self.graph[factor_id.factor_index.as_node_index()];
+        let node = &mut self.graph[factor_id.factor_index.0];
         match node.kind {
             NodeKind::Factor(ref mut factor) => {
                 // NOTE: If this message were not empty, half a variable iteration will have
                 // happened manually in secret, which is not wanted
-                factor.receive_message_from(variable_id, Message::empty(dofs))
-                // factor.receive_message_from(variable_id, message_to_factor)
+                factor.receive_message_from(variable_id, Message::empty())
             }
-            // .receive_message_from(VariableId::new(self.id, variable_index), message_to_factor),
             NodeKind::Variable(_) => {
                 panic!("the factor index either does not exist or does not point to a factor node")
             }
         }
 
-        // info!(
-        //     "adding internal edge from {:?} to {:?}",
-        //     variable_id, factor_id
-        // );
-
-        self.graph.add_edge(
-            variable_id.variable_index.as_node_index(),
-            factor_id.factor_index.as_node_index(),
-            (),
-        )
+        self.graph
+            .add_edge(variable_id.variable_index.0, factor_id.factor_index.0, ())
     }
 
     pub fn add_external_edge(&mut self, factor_id: FactorId, nth_variable_index: usize) {
         let variable_index = self
             .nth_variable_index(nth_variable_index)
             .expect("The variable index does not exist");
-        let variable = self.graph[variable_index.as_node_index()]
+        let variable = self.graph[variable_index.0]
             .as_variable_mut()
             .expect("The variable index does not point to a variable node");
 
@@ -725,15 +270,99 @@ impl FactorGraph {
             "adding external edge from {:?} to {:?} in factorgraph {:?}",
             variable_index, factor_id, self.id
         );
-        variable.receive_message_from(factor_id, Message::empty(dofs));
+        variable.receive_message_from(factor_id, Message::empty());
     }
 
-    /// Number of nodes in the factorgraph
-    ///
-    /// **Computes in O(1) time**
     #[inline]
-    pub fn len(&self) -> usize {
-        self.graph.node_count()
+    pub fn nth_variable_index(&self, index: usize) -> Option<VariableIndex> {
+        self.variable_indices.get(index).copied().map(VariableIndex)
+    }
+
+    pub fn nth_variable(&self, index: usize) -> Option<(VariableIndex, &Variable)> {
+        let variable_index = self.nth_variable_index(index)?;
+        let node = &self.graph[variable_index.0];
+        let variable = node.as_variable()?;
+        Some((variable_index, variable))
+    }
+
+    pub fn nth_variable_mut(&mut self, index: usize) -> Option<(VariableIndex, &mut Variable)> {
+        let variable_index = self.nth_variable_index(index)?;
+        let node = &mut self.graph[variable_index.0];
+        let variable = node.as_variable_mut()?;
+        Some((variable_index, variable))
+    }
+
+    pub(crate) fn delete_interrobot_factors_connected_to(
+        &mut self,
+        other: FactorGraphId,
+    ) -> Result<(), &'static str> {
+        // 1. Find all interrobot factors connected to the robot with id `other`
+        // and remove them from the graph
+
+        let mut factor_indices_to_remove = Vec::new();
+
+        for node_index in self.graph.node_indices().collect::<Vec<_>>() {
+            let node = &mut self.graph[node_index];
+            if node.is_variable() {
+                // 2. remove the the message from the external factor
+                let variable = node
+                    .as_variable_mut()
+                    .expect("A variable index should point to a Variable in the graph");
+                variable
+                    .inbox
+                    .retain(|factor_id, _| factor_id.factorgraph_id != other);
+                continue;
+            }
+
+            let factor = node
+                .as_factor()
+                .expect("A factor index should point to a Factor in the graph");
+            let Some(interrobot) = factor.kind.as_inter_robot() else {
+                continue;
+            };
+
+            if interrobot.external_variable.factorgraph_id == other {
+                self.graph.remove_node(node_index).expect(
+                    "The node index was retrieved from the graph in the previous statement",
+                );
+
+                self.factor_indices.retain(|&idx| idx != node_index);
+
+                factor_indices_to_remove.push(FactorIndex(node_index));
+            }
+        }
+
+        for node_index in self.graph.node_indices().collect::<Vec<_>>() {
+            let node = &mut self.graph[node_index];
+            if !node.is_variable() {
+                continue;
+            }
+
+            let variable = node
+                .as_variable_mut()
+                .expect("A variable index should point to a Variable in the graph");
+
+            for factor_index in &factor_indices_to_remove {
+                variable
+                    .inbox
+                    .remove(&FactorId::new(self.id, *factor_index));
+            }
+        }
+
+        Ok(())
+    }
+
+    pub(crate) fn delete_messages_from_interrobot_factor_at(&mut self, other: FactorGraphId) {
+        // PERF: avoid allocation
+        for node_index in self.graph.node_indices().collect::<Vec<_>>() {
+            let node = &mut self.graph[node_index];
+            let Some(variable) = node.as_variable_mut() else {
+                continue;
+            };
+            variable
+                .inbox
+                .retain(|factor_id, _| factor_id.factorgraph_id != other);
+        }
     }
 
     /// Return an ordered interval of variables indices.
@@ -759,34 +388,63 @@ impl FactorGraph {
         }
     }
 
-    /// A count over the number of variables and factors in the factorgraph
-    ///
-    /// **Computes in O(1) time**
-    #[must_use]
-    pub fn node_count(&self) -> NodeCount {
-        NodeCount {
-            factors:   self.factor_indices.len(),
-            variables: self.variable_indices.len(),
+    pub fn change_prior_of_variable(
+        &mut self,
+        variable_index: VariableIndex,
+        new_mean: Vector<Float>,
+    ) -> Vec<VariableToFactorMessage> {
+        let variable_id = VariableId::new(self.id, variable_index);
+        let Some(variable) = self.get_variable_mut(variable_id.variable_index) else {
+            panic!("the variable index either does not exist or does not point to a variable node");
+        };
+        // let variable = self.variable_mut(variable_id.variable_index);
+
+        let factor_messages = variable.change_prior(new_mean);
+        let mut messages_to_external_factors: Vec<VariableToFactorMessage> = Vec::new();
+
+        for (factor_id, message) in factor_messages {
+            let in_internal_graph = factor_id.factorgraph_id == self.id;
+            if in_internal_graph {
+                // If the factor is an interrobot factor, it can be missing if the robot the
+                // graph is connected to despawns, so we only have the factor
+                // receive the message if it exists
+                if let Some(factor) = self.get_factor_mut(factor_id.factor_index) {
+                    factor.receive_message_from(variable_id, message);
+                }
+            } else {
+                messages_to_external_factors.push(VariableToFactorMessage {
+                    from: variable_id,
+                    to: factor_id,
+                    message,
+                });
+            }
         }
+
+        messages_to_external_factors
     }
 
-    #[inline]
-    pub fn nth_variable_index(&self, index: usize) -> Option<VariableIndex> {
-        self.variable_indices.get(index).copied().map(VariableIndex)
+    pub fn get_factor(&self, index: FactorIndex) -> Option<&Factor> {
+        self.graph
+            .node_weight(index.0)
+            .and_then(|node| node.as_factor())
     }
 
-    pub fn nth_variable(&self, index: usize) -> Option<(VariableIndex, &Variable)> {
-        let variable_index = self.nth_variable_index(index)?;
-        let node = &self.graph[variable_index.as_node_index()];
-        let variable = node.as_variable()?;
-        Some((variable_index, variable))
+    pub fn get_factor_mut(&mut self, index: FactorIndex) -> Option<&mut Factor> {
+        self.graph
+            .node_weight_mut(*index)
+            .and_then(|node| node.as_factor_mut())
     }
 
-    pub fn nth_variable_mut(&mut self, index: usize) -> Option<(VariableIndex, &mut Variable)> {
-        let variable_index = self.nth_variable_index(index)?;
-        let node = &mut self.graph[variable_index.as_node_index()];
-        let variable = node.as_variable_mut()?;
-        Some((variable_index, variable))
+    pub fn get_variable(&self, index: VariableIndex) -> Option<&Variable> {
+        self.graph
+            .node_weight(*index)
+            .and_then(|node| node.as_variable())
+    }
+
+    pub fn get_variable_mut(&mut self, index: VariableIndex) -> Option<&mut Variable> {
+        self.graph
+            .node_weight_mut(*index)
+            .and_then(|node| node.as_variable_mut())
     }
 
     #[inline(always)]
@@ -810,99 +468,82 @@ impl FactorGraph {
         }
     }
 
-    pub fn factor(&self, index: FactorIndex) -> &Factor {
-        self.graph[index.as_node_index()]
-            .as_factor()
-            .expect("A factor index should point to a Factor in the graph")
-    }
+    /// Variable Iteration in Gaussian Belief Propagation (GBP).
+    /// For each variable in the factorgraph:
+    /// 1. Use received messages from connected factors to update the variable
+    ///    belief
+    /// 2. Create and send outgoing messages to the connected factors
+    /// # Arguments
+    /// * `robot_id` - The id of the robot that this factorgraph belongs to
+    /// # Returns
+    /// Messages that need to be sent to any externally connected factors
+    /// This can be empty if there are no externally connected factors
+    /// A [`FactorGraph`] does not have a handle to the factorgraphs of other
+    /// robots, so it cannot send messages to them. It is up to the caller
+    /// of this method to send the messages to the correct robot. # Panics
+    /// This method panics if a variable has not received any messages from its
+    /// connected factors. As this indicates that the factorgraph is not
+    /// correctly constructed.
+    pub fn variable_iteration(&mut self) -> Vec<VariableToFactorMessage> {
+        let mut messages_to_external_factors: Vec<VariableToFactorMessage> = Vec::new();
 
-    pub fn factor_mut(&mut self, index: FactorIndex) -> &mut Factor {
-        self.graph[index.as_node_index()]
-            .as_factor_mut()
-            .expect("A factor index should point to a Factor in the graph")
-    }
+        for &node_index in self.variable_indices.iter() {
+            let node = &mut self.graph[node_index];
+            let variable = node.as_variable_mut().expect(
+                "self.variable_indices should only contain indices that point to Variables in the \
+                 graph",
+            );
+            let variable_index = VariableIndex(node_index);
 
-    pub fn variable(&self, index: VariableIndex) -> &Variable {
-        self.graph[index.as_node_index()]
-            .as_variable()
-            .expect("A variable index should point to a Variable in the graph")
-    }
+            let factor_messages = variable.update_belief_and_create_factor_responses();
+            if factor_messages.is_empty() {
+                panic!(
+                    "The factorgraph {:?} with variable {:?} did not receive any messages from \
+                     its connected factors",
+                    self.id, variable_index
+                );
+            }
 
-    pub fn get_factor(&self, index: FactorIndex) -> Option<&Factor> {
-        self.graph
-            .node_weight(index.as_node_index())
-            .and_then(|node| node.as_factor())
-    }
+            let variable_id = VariableId::new(self.id, variable_index);
+            for (factor_id, message) in factor_messages {
+                let in_internal_graph = factor_id.factorgraph_id == self.id;
+                if in_internal_graph {
+                    // Send the messages to the connected factors within the same factorgraph
+                    // self.graph.
+                    if !self.factor_indices.contains(&factor_id.factor_index.0) {
+                        info!(
+                            "factor_id: {:?} does not exist in the factorgraph {:?}",
+                            factor_id, self.id
+                        );
+                        continue;
+                    }
 
-    pub fn get_factor_mut(&mut self, index: FactorIndex) -> Option<&mut Factor> {
-        self.graph
-            .node_weight_mut(index.as_node_index())
-            .and_then(|node| node.as_factor_mut())
-    }
+                    self.graph[factor_id.factor_index.0]
+                        .as_factor_mut()
+                        .expect("A factor can only have variables as neighbours")
+                        .receive_message_from(variable_id, message);
 
-    pub fn get_variable(&self, index: VariableIndex) -> Option<&Variable> {
-        self.graph
-            .node_weight(index.as_node_index())
-            .and_then(|node| node.as_variable())
-    }
-
-    pub fn get_variable_mut(&mut self, index: VariableIndex) -> Option<&mut Variable> {
-        self.graph
-            .node_weight_mut(index.as_node_index())
-            .and_then(|node| node.as_variable_mut())
-    }
-
-    pub fn variable_mut(&mut self, index: VariableIndex) -> &mut Variable {
-        self.graph[index.as_node_index()]
-            .as_variable_mut()
-            .expect("A variable index should point to a Variable in the graph")
-    }
-
-    pub fn export_data(&self) -> (Vec<graphviz::Node>, Vec<graphviz::Edge>) {
-        let nodes = self
-            .graph
-            .node_indices()
-            .map(|node_index| {
-                let node = &self.graph[node_index];
-                graphviz::Node {
-                    index: node_index.index(),
-                    kind:  match &node.kind {
-                        NodeKind::Factor(factor) => match factor.kind {
-                            FactorKind::Dynamic(_) => graphviz::NodeKind::DynamicFactor,
-                            FactorKind::Obstacle(_) => graphviz::NodeKind::ObstacleFactor,
-                            FactorKind::Pose(_) => graphviz::NodeKind::PoseFactor,
-                            FactorKind::InterRobot(ref inner) => {
-                                graphviz::NodeKind::InterRobotFactor(inner.connection)
-                            }
-                        },
-                        NodeKind::Variable(variable) => {
-                            // let mean = variable.belief.mean();
-                            // let mean = &variable.mu;
-                            let [x, y] = variable.estimated_position();
-                            graphviz::NodeKind::Variable {
-                                x: x as f32,
-                                y: y as f32,
-                            }
-                        }
-                    },
+                    let factor = self.graph[factor_id.factor_index.0]
+                        .as_factor()
+                        .expect("A factor index should point to a Factor in the graph");
+                } else {
+                    // error!(
+                    //     "message from factor_id: {:?} to variable_id: {:?} is external",
+                    //     factor_id, variable_id
+                    // );
+                    messages_to_external_factors.push(VariableToFactorMessage {
+                        from: variable_id,
+                        to: factor_id,
+                        message,
+                    });
                 }
-            })
-            .collect::<Vec<_>>();
-
-        let edges = self
-            .graph
-            .edge_indices()
-            .filter_map(|edge_index| {
-                self.graph
-                    .edge_endpoints(edge_index)
-                    .map(|(from, to)| graphviz::Edge {
-                        from: from.index(),
-                        to:   to.index(),
-                    })
-            })
-            .collect::<Vec<_>>();
+            }
+        }
 
-        (nodes, edges)
+        // Return the messages to be sent to the connected factors in other factorgraphs
+        // The caller is responsible for sending these messages to the correct
+        // factorgraphs
+        messages_to_external_factors
     }
 
     /// Aggregate and marginalise over all adjacent variables, and send.
@@ -915,7 +556,6 @@ impl FactorGraph {
             let Some(factor) = node.as_factor_mut() else {
                 continue;
             };
-            // let factor_variant = factor.variant();
 
             let variable_messages = factor.update();
             if variable_messages.is_empty() {
@@ -926,39 +566,17 @@ impl FactorGraph {
                 );
             }
 
-            let factor_id = match factor.kind {
-                FactorKind::Dynamic(_) => FactorId::new_dynamic(self.id, FactorIndex(node_index)),
-                FactorKind::Obstacle(_) => FactorId::new_obstacle(self.id, FactorIndex(node_index)),
-                FactorKind::Pose(_) => FactorId::new_pose(self.id, FactorIndex(node_index)),
-                FactorKind::InterRobot(_) => {
-                    FactorId::new_interrobot(self.id, FactorIndex(node_index))
-                }
-            };
-            // let factor_id = FactorId::new(self.id, FactorIndex(node_index));
+            let factor_id = FactorId::new(self.id, FactorIndex(node_index));
+
             for (variable_id, message) in variable_messages {
                 let in_internal_graph = variable_id.factorgraph_id == self.id;
                 if in_internal_graph {
-                    let variable = self.graph[variable_id.variable_index.as_node_index()]
+                    let variable = self.graph[variable_id.variable_index.0]
                         .as_variable_mut()
                         .expect("A factor can only have variables as neighbors");
 
-                    // info!(
-                    //     "{:?}\t->\tvariable {:?}\tfactor variant: {:?}",
-                    //     factor_id, variable_id, factor_variant
-                    // );
-
-                    // let factor_global_identifier = factor_id.global_id();
-                    // let variable_global_identifier = variable_id.global_id();
-                    // pretty_print_message!(factor_global_identifier, variable_global_identifier);
-                    // NOTE: pretty print the message
-                    // pretty_print_message!(factor_id, variable_id, "factor iteration");
-
                     variable.receive_message_from(factor_id, message);
                 } else {
-                    // error!(
-                    //     "message from factor_id: {:?} to variable_id: {:?} is external",
-                    //     factor_id, variable_id
-                    // );
                     messages_to_external_variables.push(FactorToVariableMessage {
                         from: factor_id,
                         to: variable_id,
@@ -974,309 +592,219 @@ impl FactorGraph {
         messages_to_external_variables
     }
 
-    /// Variable Iteration in Gaussian Belief Propagation (GBP).
-    /// For each variable in the factorgraph:
-    /// 1. Use received messages from connected factors to update the variable
-    ///    belief
-    /// 2. Create and send outgoing messages to the connected factors
-    /// # Arguments
-    /// * `robot_id` - The id of the robot that this factorgraph belongs to
-    /// # Returns
-    /// Messages that need to be sent to any externally connected factors
-    /// This can be empty if there are no externally connected factors
-    /// A [`FactorGraph`] does not have a handle to the factorgraphs of other
-    /// robots, so it cannot send messages to them. It is up to the caller
-    /// of this method to send the messages to the correct robot. # Panics
-    /// This method panics if a variable has not received any messages from its
-    /// connected factors. As this indicates that the factorgraph is not
-    /// correctly constructed.
-    pub fn variable_iteration(&mut self) -> Vec<VariableToFactorMessage> {
-        let mut messages_to_external_factors: Vec<VariableToFactorMessage> = Vec::new();
-
-        for &node_index in self.variable_indices.iter() {
-            let node = &mut self.graph[node_index];
-            let variable = node.as_variable_mut().expect(
-                "self.variable_indices should only contain indices that point to Variables in the \
-                 graph",
-            );
-            let variable_index = VariableIndex(node_index);
-
-            let factor_messages = variable.update_belief_and_create_factor_responses();
-            // dbg!(&factor_messages);
-            if factor_messages.is_empty() {
-                panic!(
-                    "The factorgraph {:?} with variable {:?} did not receive any messages from \
-                     its connected factors",
-                    self.id, variable_index
-                );
-            }
-
-            let variable_id = VariableId::new(self.id, variable_index);
-            for (factor_id, message) in factor_messages {
-                let in_internal_graph = factor_id.factorgraph_id == self.id;
-                if in_internal_graph {
-                    // Send the messages to the connected factors within the same factorgraph
-                    // self.graph.
-                    if !self
-                        .factor_indices
-                        .contains(&factor_id.factor_index.as_node_index())
-                    {
-                        info!(
-                            "factor_id: {:?} does not exist in the factorgraph {:?}",
-                            factor_id, self.id
-                        );
-                        continue;
-                    }
-
-                    self.graph[factor_id.factor_index.as_node_index()]
-                        .as_factor_mut()
-                        .expect("A factor can only have variables as neighbours")
-                        .receive_message_from(variable_id, message);
-
-                    let factor = self.graph[factor_id.factor_index.as_node_index()]
-                        .as_factor()
-                        .expect("A factor index should point to a Factor in the graph");
-                    // info!(
-                    //     "Message variable {:?}\t->\tfactor {:?}\tfactor
-                    // variant: {:?}",     variable_id,
-                    //     factor_id,
-                    //     factor.variant()
-                    // );
-                    // NOTE: pretty print the message
-                    // pretty_print_message!(variable_id, factor_id, "variable
-                    // iteration");
-                } else {
-                    // error!(
-                    //     "message from factor_id: {:?} to variable_id: {:?} is external",
-                    //     factor_id, variable_id
-                    // );
-                    messages_to_external_factors.push(VariableToFactorMessage {
-                        from: variable_id,
-                        to: factor_id,
-                        message,
-                    });
-                }
-            }
-        }
-
-        // std::process::exit(0);
+    #[must_use]
+    pub fn messages_sent(&mut self) -> usize {
+        self.graph
+            .node_weights_mut()
+            .map(|node| {
+                let messages_sent = node.messages_sent();
 
-        // Return the messages to be sent to the connected factors in other factorgraphs
-        // The caller is responsible for sending these messages to the correct
-        // factorgraphs
-        messages_to_external_factors
+                node.reset_message_count();
+                messages_sent
+            })
+            .sum()
     }
+}
 
-    pub fn change_prior_of_variable(
-        &mut self,
-        variable_index: VariableIndex,
-        new_mean: Vector<Float>,
-    ) -> Vec<VariableToFactorMessage> {
-        let variable_id = VariableId::new(self.id, variable_index);
-        let variable = self.variable_mut(variable_id.variable_index);
+/// Record type used to keep track of how many factors and variables
+/// there are in the factorgraph. We keep track of these counts internally in
+/// the factorgraph, such a query for the counts, is **O(1)**.
+#[derive(Debug, Clone, Copy, Default)]
+pub struct NodeCount {
+    pub factors:   usize,
+    pub variables: usize,
+}
 
-        let factor_messages = variable.change_prior(new_mean);
-        let mut messages_to_external_factors: Vec<VariableToFactorMessage> = Vec::new();
+/// Iterator over the factors in the factorgraph.
+///
+/// Iterator element type is `(FactorIndex, &'a Factor)`.
+///
+/// Created with [`.factors()`][1]
+///
+/// [1]: struct.FactorGraph.html#method.factors
+pub struct Factors<'a> {
+    graph: &'a Graph,
+    factor_indices: std::slice::Iter<'a, NodeIndex>,
+}
 
-        for (factor_id, message) in factor_messages {
-            let in_internal_graph = factor_id.factorgraph_id == self.id;
-            if in_internal_graph {
-                // If the factor is an interrobot factor, it can be missing if the robot the
-                // graph is connected to despawns, so we only have the factor
-                // receive the message if it exists
-                if let Some(factor) = self.get_factor_mut(factor_id.factor_index) {
-                    factor.receive_message_from(variable_id, message);
-                }
-            } else {
-                messages_to_external_factors.push(VariableToFactorMessage {
-                    from: variable_id,
-                    to: factor_id,
-                    message,
-                });
-            }
+impl<'a> Factors<'a> {
+    #[must_use]
+    pub fn new(graph: &'a Graph, factor_indices: &'a [NodeIndex]) -> Self {
+        Self {
+            graph,
+            factor_indices: factor_indices.iter(),
         }
-
-        messages_to_external_factors
     }
+}
 
-    pub(crate) fn delete_interrobot_factors_connected_to(
-        &mut self,
-        other: RobotId,
-    ) -> Result<(), &'static str> {
-        // 1. Find all interrobot factors connected to the robot with id `other`
-        // and remove them from the graph
-
-        let mut factor_indices_to_remove = Vec::new();
-
-        for node_index in self.graph.node_indices().collect::<Vec<_>>() {
-            let node = &mut self.graph[node_index];
-            if node.is_variable() {
-                // 2. remove the the message from the external factor
-                let variable = node
-                    .as_variable_mut()
-                    .expect("A variable index should point to a Variable in the graph");
-                variable
-                    .inbox
-                    .retain(|factor_id, _| factor_id.factorgraph_id != other);
-                continue;
-            }
+impl FactorGraph {
+    /// Returns an iterator over the factors in the factorgraph.
+    #[inline]
+    #[must_use]
+    pub fn factors(&self) -> Factors<'_> {
+        Factors::new(&self.graph, &self.factor_indices)
+    }
+}
 
-            let factor = node
-                .as_factor()
-                .expect("A factor index should point to a Factor in the graph");
-            let Some(interrobot) = factor.kind.as_inter_robot() else {
-                continue;
-            };
+impl<'a> Iterator for Factors<'a> {
+    type Item = (NodeIndex, &'a Factor);
 
-            if interrobot.connection.id_of_robot_connected_with == other {
-                // info!("deleting interrobot factor {:?}", node_index);
-                self.graph.remove_node(node_index).expect(
-                    "The node index was retrieved from the graph in the previous statement",
-                );
+    fn next(&mut self) -> Option<Self::Item> {
+        let &index = self.factor_indices.next()?;
+        let node = &self.graph[index];
+        node.as_factor().map(|factor| (index, factor))
+    }
+}
 
-                self.factor_indices.retain(|&idx| idx != node_index);
+/// Iterator over the variables in the factorgraph.
+///
+/// Iterator element type is `(VariableIndex, &'a Variable)`.
+///
+/// Created with [`.variables()`][1]
+///
+/// [1]: struct.FactorGraph.html#method.variables
+pub struct Variables<'a> {
+    graph: &'a Graph,
+    variable_indices: std::slice::Iter<'a, NodeIndex>,
+}
 
-                factor_indices_to_remove.push(FactorIndex(node_index));
-            }
+impl<'a> Variables<'a> {
+    pub fn new(graph: &'a Graph, variable_indices: &'a [NodeIndex]) -> Self {
+        Self {
+            graph,
+            variable_indices: variable_indices.iter(),
         }
+    }
+}
 
-        for node_index in self.graph.node_indices().collect::<Vec<_>>() {
-            let node = &mut self.graph[node_index];
-            if !node.is_variable() {
-                continue;
-            }
-
-            let variable = node
-                .as_variable_mut()
-                .expect("A variable index should point to a Variable in the graph");
-
-            for factor_index in &factor_indices_to_remove {
-                variable
-                    .inbox
-                    .remove(&FactorId::new_ambiguous(self.id, *factor_index));
-            }
-        }
+impl<'a> Iterator for Variables<'a> {
+    type Item = (VariableIndex, &'a Variable);
 
-        Ok(())
+    fn next(&mut self) -> Option<Self::Item> {
+        let &index = self.variable_indices.next()?;
+        let node = &self.graph[index];
+        node.as_variable()
+            .map(|variable| (VariableIndex(index), variable))
     }
+}
 
-    pub(crate) fn delete_messages_from_interrobot_factor_at(&mut self, other: RobotId) {
-        for node_index in self.graph.node_indices().collect::<Vec<_>>() {
-            let node = &mut self.graph[node_index];
-            let Some(variable) = node.as_variable_mut() else {
-                continue;
-            };
-            // println!("robot id {:?}", other);
-            // println!("before {:?}", variable.inbox.keys().collect::<Vec<_>>());
-            variable
-                .inbox
-                .retain(|factor_id, _| factor_id.factorgraph_id != other);
-            // println!("after  {:?}",
-            // variable.inbox.keys().collect::<Vec<_>>());
-        }
+impl FactorGraph {
+    /// Returns an iterator over the variables in the factorgraph.
+    #[inline]
+    #[must_use]
+    pub fn variables(&self) -> Variables<'_> {
+        Variables::new(&self.graph, &self.variable_indices)
     }
+}
 
-    pub fn remove_connection_to(
-        &mut self,
-        factorgraph_id: FactorGraphId,
-    ) -> Result<(), RemoveConnectionToError> {
-        // go through all nodes, and remove their individual connection to the other
-        // factorgraph if none of the nodes has a connection to the other
-        // factorgraph, then return and Error.
-
-        let mut connections_removed: usize = 0;
-        for node in self.graph.node_weights_mut() {
-            if node.remove_connection_to(factorgraph_id).is_ok() {
-                connections_removed += 1;
-            }
-        }
+/// Iterator over the interrobot factors in the factorgraph.
+///
+/// Iterator element type is `(FactorIndex, &'a InterRobotFactor)`.
+///
+/// Created with [`.inter_robot_factors()`][1]
+pub struct InterRobotFactors<'a> {
+    graph: &'a Graph,
+    factor_indices: std::slice::Iter<'a, NodeIndex>,
+}
 
-        if connections_removed == 0 {
-            Err(RemoveConnectionToError)
-        } else {
-            Ok(())
+impl<'a> InterRobotFactors<'a> {
+    pub fn new(graph: &'a Graph, factor_indices: &'a [NodeIndex]) -> Self {
+        Self {
+            graph,
+            factor_indices: factor_indices.iter(),
         }
     }
+}
 
-    #[must_use]
-    pub fn messages_sent(&mut self) -> usize {
-        self.graph
-            .node_weights_mut()
-            .map(|node| {
-                let messages_sent = node.messages_sent();
+impl<'a> Iterator for InterRobotFactors<'a> {
+    type Item = (NodeIndex, &'a InterRobotFactor);
 
-                node.reset_message_count();
-                messages_sent
-            })
-            .sum()
+    fn next(&mut self) -> Option<Self::Item> {
+        let &index = self.factor_indices.next()?;
+        let node = &self.graph[index];
+        node.as_factor()
+            .and_then(|factor| factor.kind.as_inter_robot())
+            .map(|interrobot| (index, interrobot))
     }
 }
 
-pub mod graphviz {
-    use crate::planner::factor::InterRobotConnection;
-
-    pub struct Node {
-        pub index: usize,
-        pub kind:  NodeKind,
+impl FactorGraph {
+    #[inline]
+    #[must_use]
+    pub fn inter_robot_factors(&self) -> InterRobotFactors<'_> {
+        InterRobotFactors::new(&self.graph, &self.interrobot_factor_indices)
     }
+}
 
-    impl Node {
-        pub fn color(&self) -> &'static str {
-            self.kind.color()
-        }
+impl std::ops::Index<FactorIndex> for FactorGraph {
+    type Output = Factor;
 
-        pub fn shape(&self) -> &'static str {
-            self.kind.shape()
-        }
+    // type Output = Option<Factor>;
 
-        pub fn width(&self) -> f64 {
-            self.kind.width()
-        }
+    fn index(&self, index: FactorIndex) -> &Self::Output {
+        let node = &self.graph[index.0];
+        node.as_factor()
+            .expect("a factor index points to a factor node in the graph")
     }
+}
+
+impl std::ops::Index<VariableIndex> for FactorGraph {
+    type Output = Variable;
 
-    pub enum NodeKind {
-        Variable { x: f32, y: f32 },
-        InterRobotFactor(InterRobotConnection),
-        // InterRobotFactor {
-        //     /// The id of the robot the interrobot factor is connected to
-        //     other_robot_id: RobotId,
-        //     /// The index of the variable in the other robots factorgraph, that the interrobot
-        // factor is connected with     variable_index_in_other_robot: usize,
-        // },
-        DynamicFactor,
-        ObstacleFactor,
-        PoseFactor,
+    fn index(&self, index: VariableIndex) -> &Self::Output {
+        self.graph[index.0]
+            .as_variable()
+            .expect("a variable index points to a variable node in the graph")
     }
+}
 
-    impl NodeKind {
-        pub fn color(&self) -> &'static str {
-            match self {
-                Self::Variable { .. } => "#eff1f5",         // latte base (white)
-                Self::InterRobotFactor { .. } => "#a6da95", // green
-                Self::DynamicFactor => "#8aadf4",           // blue
-                // Self::ObstacleFactor => "#c6a0f6",          // mauve (purple)
-                Self::ObstacleFactor => "#ee99a0", // mauve (purple)
-                Self::PoseFactor => "#c6aof6",     // maroon (red)
-            }
-        }
+use super::graphviz;
 
-        pub fn shape(&self) -> &'static str {
-            match self {
-                Self::Variable { .. } => "circle",
-                _ => "square",
-            }
-        }
+impl graphviz::Graph for FactorGraph {
+    fn export_data(&self) -> (Vec<super::graphviz::Node>, Vec<super::graphviz::Edge>) {
+        let nodes = self
+            .graph
+            .node_indices()
+            .map(|node_index| {
+                let node = &self.graph[node_index];
+                graphviz::Node {
+                    index: node_index.index(),
+                    kind:  match &node.kind {
+                        NodeKind::Factor(factor) => match factor.kind {
+                            FactorKind::Dynamic(_) => graphviz::NodeKind::DynamicFactor,
+                            FactorKind::Obstacle(_) => graphviz::NodeKind::ObstacleFactor,
+                            FactorKind::Pose(_) => graphviz::NodeKind::PoseFactor,
+                            FactorKind::InterRobot(ref inner) => {
+                                graphviz::NodeKind::InterRobotFactor(inner.external_variable)
+                            }
+                        },
+                        NodeKind::Variable(variable) => {
+                            // let mean = variable.belief.mean();
+                            // let mean = &variable.mu;
+                            let [x, y] = variable.estimated_position();
+                            graphviz::NodeKind::Variable {
+                                x: x as f32,
+                                y: y as f32,
+                            }
+                        }
+                    },
+                }
+            })
+            .collect::<Vec<_>>();
 
-        pub fn width(&self) -> f64 {
-            match self {
-                Self::Variable { .. } => 0.8,
-                _ => 0.2,
-            }
-        }
-    }
+        let edges = self
+            .graph
+            .edge_indices()
+            .filter_map(|edge_index| {
+                self.graph
+                    .edge_endpoints(edge_index)
+                    .map(|(from, to)| graphviz::Edge {
+                        from: from.index(),
+                        to:   to.index(),
+                    })
+            })
+            .collect::<Vec<_>>();
 
-    pub struct Edge {
-        pub from: usize,
-        pub to:   usize,
+        (nodes, edges)
     }
 }
diff --git a/crates/gbpplanner-rs/src/factorgraph/graphviz.rs b/crates/gbpplanner-rs/src/factorgraph/graphviz.rs
new file mode 100644
index 00000000..a297ab5d
--- /dev/null
+++ b/crates/gbpplanner-rs/src/factorgraph/graphviz.rs
@@ -0,0 +1,70 @@
+use super::factor::ExternalVariableId;
+
+pub struct Node {
+    pub index: usize,
+    pub kind:  NodeKind,
+}
+
+impl Node {
+    pub fn color(&self) -> &'static str {
+        self.kind.color()
+    }
+
+    pub fn shape(&self) -> &'static str {
+        self.kind.shape()
+    }
+
+    pub fn width(&self) -> f64 {
+        self.kind.width()
+    }
+}
+
+pub enum NodeKind {
+    Variable { x: f32, y: f32 },
+    InterRobotFactor(ExternalVariableId),
+    // InterRobotFactor {
+    //     /// The id of the robot the interrobot factor is connected to
+    //     other_robot_id: RobotId,
+    //     /// The index of the variable in the other robots factorgraph, that the interrobot
+    // factor is connected with     variable_index_in_other_robot: usize,
+    // },
+    DynamicFactor,
+    ObstacleFactor,
+    PoseFactor,
+}
+
+impl NodeKind {
+    pub fn color(&self) -> &'static str {
+        match self {
+            Self::Variable { .. } => "#eff1f5",         // latte base (white)
+            Self::InterRobotFactor { .. } => "#a6da95", // green
+            Self::DynamicFactor => "#8aadf4",           // blue
+            // Self::ObstacleFactor => "#c6a0f6",          // mauve (purple)
+            Self::ObstacleFactor => "#ee99a0", // mauve (purple)
+            Self::PoseFactor => "#c6aof6",     // maroon (red)
+        }
+    }
+
+    pub fn shape(&self) -> &'static str {
+        match self {
+            Self::Variable { .. } => "circle",
+            _ => "square",
+        }
+    }
+
+    pub fn width(&self) -> f64 {
+        match self {
+            Self::Variable { .. } => 0.8,
+            _ => 0.2,
+        }
+    }
+}
+
+pub struct Edge {
+    pub from: usize,
+    pub to:   usize,
+}
+
+pub trait Graph {
+    fn export_data(&self) -> (Vec<Node>, Vec<Edge>);
+}
diff --git a/crates/gbpplanner-rs/src/factorgraph/id.rs b/crates/gbpplanner-rs/src/factorgraph/id.rs
new file mode 100644
index 00000000..742a4ade
--- /dev/null
+++ b/crates/gbpplanner-rs/src/factorgraph/id.rs
@@ -0,0 +1,133 @@
+use super::factorgraph::{FactorGraphId, FactorIndex, VariableIndex};
+
+/// Unique identifier of a factor in the world.
+#[derive(Debug, Clone, Copy, PartialEq, Eq, derive_more::Display)]
+#[display(fmt = "{:?}-{}", factorgraph_id, "factor_index.0.index()")]
+pub struct FactorId {
+    /// The id of the factorgraph that the factor belongs to.
+    pub factorgraph_id: FactorGraphId,
+    /// The index of the factor in the factorgraph.
+    pub factor_index:   FactorIndex,
+}
+
+impl FactorId {
+    /// Create a new `FactorId`.
+    #[must_use]
+    pub fn new(factorgraph_id: FactorGraphId, factor_index: FactorIndex) -> Self {
+        Self {
+            factorgraph_id,
+            factor_index,
+        }
+    }
+}
+
+impl std::cmp::PartialOrd for FactorId {
+    /// Returns `Some(std::cmp::ordering::Equal)` if `self` and `other` are
+    /// equal, `Some(std::cmp::ordering::Less)` if `other.factorgraph_id` is
+    /// greater than `self.factorgraph_id`, or if the factors belong to the
+    /// same factorgraph and `other.factor_index` is greater than
+    /// `self.factor_index`. Returns `Some(std::cmp::ordering::Greater)`
+    /// otherwise.
+    ///
+    /// # NOTE
+    ///
+    /// This ordering is really important for the **gbpplanner** algorithm, to
+    /// work correctly.
+    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
+        if self.factorgraph_id < other.factorgraph_id {
+            Some(std::cmp::Ordering::Less)
+        } else if self.factorgraph_id == other.factorgraph_id
+            && self.factor_index.0 < other.factor_index.0
+        {
+            Some(std::cmp::Ordering::Less)
+        } else if self.factorgraph_id == other.factorgraph_id
+            && self.factor_index == other.factor_index
+        {
+            Some(std::cmp::Ordering::Equal)
+        } else {
+            Some(std::cmp::Ordering::Greater)
+        }
+    }
+}
+
+impl std::cmp::Ord for FactorId {
+    #[inline]
+    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
+        self.partial_cmp(other)
+            .expect("every branch in Self::partial_cmp() returns Some()")
+    }
+}
+
+// implement PartialEq and Eq manually
+// impl std::cmp::PartialEq for FactorId {
+//     fn eq(&self, other: &Self) -> bool {
+//         self.factorgraph_id == other.factorgraph_id && self.factor_index ==
+// other.factor_index     }
+// }
+//
+// impl std::cmp::Eq for FactorId {}
+
+/// Unique identifier of a variable in the world.
+#[derive(Debug, Clone, Copy, PartialEq, Eq, derive_more::Display)]
+#[display(fmt = "{:?}-{}", factorgraph_id, "variable_index.0.index()")]
+pub struct VariableId {
+    /// The id of the factorgraph that the variable belongs to.
+    pub factorgraph_id: FactorGraphId,
+    /// The index of the variable in the factorgraph.
+    pub variable_index: VariableIndex,
+}
+
+impl std::cmp::PartialOrd for VariableId {
+    /// Returns `Some(std::cmp::ordering::Equal)` if `self` and `other` are
+    /// equal, `Some(std::cmp::ordering::Less)` if `other.factorgraph_id` is
+    /// greater than `self.factorgraph_id`, or if the factors belong to the
+    /// same factorgraph and `other.variable_index` is greater than
+    /// `self.variable_index`. Returns `Some(std::cmp::ordering::Greater)`
+    /// otherwise.
+    ///
+    /// # NOTE
+    ///
+    /// This ordering is really important for the **gbpplanner** algorithm, to
+    /// work correctly.
+    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
+        if self.factorgraph_id < other.factorgraph_id {
+            Some(std::cmp::Ordering::Less)
+        } else if self.factorgraph_id == other.factorgraph_id
+            && self.variable_index.0 < other.variable_index.0
+        {
+            Some(std::cmp::Ordering::Less)
+        } else if self.factorgraph_id == other.factorgraph_id
+            && self.variable_index == other.variable_index
+        {
+            Some(std::cmp::Ordering::Equal)
+        } else {
+            Some(std::cmp::Ordering::Greater)
+        }
+    }
+}
+
+impl std::cmp::Ord for VariableId {
+    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
+        self.partial_cmp(other)
+            .expect("every branch in Self::partial_cmp() returns Some()")
+    }
+}
+
+// implement PartialEq and Eq manually
+// impl std::cmp::PartialEq for VariableId {
+//     fn eq(&self, other: &Self) -> bool {
+//         self.factorgraph_id == other.factorgraph_id && self.variable_index ==
+// other.variable_index     }
+// }
+// impl std::cmp::Eq for VariableId {}
+
+impl VariableId {
+    /// Create a new `VariableId`.
+    #[must_use]
+    pub fn new(factorgraph_id: FactorGraphId, variable_index: VariableIndex) -> Self {
+        Self {
+            factorgraph_id,
+            variable_index,
+        }
+    }
+}
diff --git a/crates/gbpplanner-rs/src/factorgraph/message.rs b/crates/gbpplanner-rs/src/factorgraph/message.rs
new file mode 100644
index 00000000..6abcd380
--- /dev/null
+++ b/crates/gbpplanner-rs/src/factorgraph/message.rs
@@ -0,0 +1,165 @@
+#![deny(missing_docs)]
+//! Message module.
+//!
+//! Contains the message struct that variables and factors send between each
+//! other in the factorgraph.
+
+use std::collections::BTreeMap;
+
+use gbp_linalg::prelude::*;
+
+use super::{
+    id::{FactorId, VariableId},
+    DOFS,
+};
+
+/// Payload of a message
+#[derive(Debug, Clone)]
+pub struct Payload {
+    /// Information vector of a multivariate gaussian
+    pub information_factor: Vector<Float>,
+    /// Precision matrix of a multivariate gaussian
+    pub precision_matrix: Matrix<Float>,
+    /// Mean vector of a multivariate gaussian
+    /// The mean can be computed from the information vector and the precision
+    /// matrix but the mean vector is stored here to trade some memory, for
+    /// having to compute it multiple times
+    pub mean: Vector<Float>,
+}
+
+/// Newtype used to make prevent the caller of `Message::new()` from mixing up
+/// the information vector and mean vector argument.
+pub struct InformationVec(pub Vector<Float>);
+/// Newtype used to make make it clear for the caller that the matrix argument
+/// for `Message::new()` has to be a precision matrix, and not a covariance.
+pub struct PrecisionMatrix(pub Matrix<Float>);
+/// Newtype used to make prevent the caller of `Message::new()` from mixing up
+/// the information vector and mean vector argument.
+pub struct Mean(pub Vector<Float>);
+
+#[derive(Debug, Clone)]
+pub struct Message {
+    payload: Option<Payload>,
+}
+
+impl Message {
+    /// Returns a reference to the mean
+    /// or `None` if the message is empty.
+    #[inline]
+    pub fn mean(&self) -> Option<&Vector<Float>> {
+        self.payload.as_ref().map(|payload| &payload.mean)
+    }
+
+    /// Returns a reference to the precision matrix
+    /// or `None` if the message is empty.
+    #[inline]
+    pub fn precision_matrix(&self) -> Option<&Matrix<Float>> {
+        self.payload
+            .as_ref()
+            .map(|payload| &payload.precision_matrix)
+    }
+
+    /// Returns a reference to the information vector
+    /// or `None` if the message is empty.
+    #[inline]
+    pub fn information_vector(&self) -> Option<&Vector<Float>> {
+        self.payload
+            .as_ref()
+            .map(|payload| &payload.information_factor)
+    }
+
+    /// Returns `true` if the message is [`Empty`].
+    #[inline]
+    pub fn is_empty(&self) -> bool {
+        self.payload.is_none()
+    }
+
+    /// Take the inner `MultivariateNormal` from the message.
+    /// Leaving the message in an empty state.
+    #[inline]
+    pub fn take(&mut self) -> Option<Payload> {
+        self.payload.take()
+    }
+
+    /// Access the payload of the message.
+    /// Returns `None` if the message is empty.
+    #[inline]
+    pub fn payload(&self) -> Option<&Payload> {
+        self.payload.as_ref()
+    }
+
+    /// Create an empty message
+    #[must_use]
+    pub fn empty() -> Self {
+        Self {
+            payload: Some(Payload {
+                information_factor: Vector::<Float>::zeros(DOFS),
+                precision_matrix: Matrix::<Float>::zeros((DOFS, DOFS)),
+                mean: Vector::<Float>::zeros(DOFS),
+            }),
+        }
+    }
+
+    /// Create a new message
+    ///
+    /// # Panics
+    ///
+    /// - if `eta.0.len() != DOFS`
+    /// - if `lam.0.nrows() != DOFS`
+    /// - if `lam.0.ncols() != DOFS`
+    /// - if `mu.0.len() != DOFS`
+    #[must_use]
+    pub fn new(eta: InformationVec, lam: PrecisionMatrix, mu: Mean) -> Self {
+        debug_assert_eq!(eta.0.len(), DOFS);
+        debug_assert_eq!(lam.0.nrows(), DOFS);
+        debug_assert_eq!(lam.0.ncols(), DOFS);
+        debug_assert_eq!(mu.0.len(), DOFS);
+
+        Self {
+            payload: Some(Payload {
+                information_factor: eta.0,
+                precision_matrix: lam.0,
+                mean: mu.0,
+            }),
+        }
+    }
+}
+
+/// A message from a factor to a variable
+#[derive(Debug)]
+pub struct VariableToFactorMessage {
+    /// The factor that sends the message
+    pub from:    VariableId,
+    /// The variable that receives the message
+    pub to:      FactorId,
+    /// The message
+    pub message: Message,
+}
+
+/// A message from a variable to a factor
+#[derive(Debug)]
+pub struct FactorToVariableMessage {
+    /// The variable that sends the message
+    pub from:    FactorId,
+    /// The factor that receives the message
+    pub to:      VariableId,
+    /// The message
+    pub message: Message,
+}
+
+// pub type MessagesFromVariables = BTreeMap<FactorId, Message>;
+// pub type MessagesFromFactors = BTreeMap<VariableId, Message>;
+
+/// Type alias for a map of messages from factors connected to a variable
+/// A (`BTreeMap`)[std::collections::BTreeMap] is used, instead of a
+/// (`HashMap`)[std::collections::HashMap] to ensure that the messages are
+/// stored in a consistent order This is necessary for the **gbpplanner**
+/// algorithm to work correctly.
+pub type MessagesToFactors = BTreeMap<FactorId, Message>;
+
+/// Type alias for a map of messages from variables connected to a factor
+/// A (`BTreeMap`)[std::collections::BTreeMap] is used, instead of a
+/// (`HashMap`)[std::collections::HashMap] to ensure that the messages are
+/// stored in a consistent order This is necessary for the **gbpplanner**
+/// algorithm to work correctly.
+pub type MessagesToVariables = BTreeMap<VariableId, Message>;
diff --git a/crates/gbpplanner-rs/src/factorgraph/mod.rs b/crates/gbpplanner-rs/src/factorgraph/mod.rs
new file mode 100644
index 00000000..b98e24ea
--- /dev/null
+++ b/crates/gbpplanner-rs/src/factorgraph/mod.rs
@@ -0,0 +1,51 @@
+#![deny(missing_docs)]
+//! ...
+
+use bevy::ecs::entity::Entity;
+use petgraph::Undirected;
+
+pub(super) mod factor;
+pub(super) mod factorgraph;
+pub(super) mod graphviz;
+pub(crate) mod id;
+pub(crate) mod message;
+pub(crate) mod node;
+pub(crate) mod variable;
+
+/// Degrees of Freedom of the ground robot.
+/// The robot has 4 degrees, of freedom:
+/// 1. position.x
+/// 2. position.y
+/// 3. velocity.x
+/// 4. velocity.y
+/// [x, y, x', y']
+pub const DOFS: usize = 4;
+
+/// prelude module bringing entire public API into score
+pub mod prelude {
+    pub use super::{factorgraph::FactorGraph, message::Message, DOFS};
+}
+
+#[derive(Debug, Default, Clone, Copy)]
+pub(super) struct MessageCount {
+    pub sent:     usize,
+    pub received: usize,
+}
+
+impl MessageCount {
+    pub fn reset(&mut self) {
+        self.sent = 0;
+        self.received = 0;
+    }
+}
+
+impl std::ops::Add for MessageCount {
+    type Output = Self;
+
+    fn add(self, rhs: Self) -> Self::Output {
+        MessageCount {
+            sent:     self.sent + rhs.sent,
+            received: self.received + rhs.received,
+        }
+    }
+}
diff --git a/crates/gbpplanner-rs/src/factorgraph/node.rs b/crates/gbpplanner-rs/src/factorgraph/node.rs
new file mode 100644
index 00000000..5e9956a0
--- /dev/null
+++ b/crates/gbpplanner-rs/src/factorgraph/node.rs
@@ -0,0 +1,134 @@
+use super::{factor::Factor, factorgraph::FactorGraphId, variable::Variable};
+
+#[derive(Debug, derive_more::Display)]
+#[display(fmt = "no connection to the given factorgraph")]
+pub(crate) struct RemoveConnectionToError;
+
+impl std::error::Error for RemoveConnectionToError {}
+
+pub(in crate::factorgraph) trait FactorGraphNode {
+    #[must_use]
+    fn remove_connection_to(
+        &mut self,
+        factorgraph_id: FactorGraphId,
+    ) -> Result<(), RemoveConnectionToError>;
+
+    fn messages_sent(&self) -> usize;
+    fn messages_received(&self) -> usize;
+
+    fn reset_message_count(&mut self);
+}
+
+#[derive(Debug, derive_more::IsVariant)]
+pub enum NodeKind {
+    Factor(Factor),
+    // TODO: wrap in Box<>
+    Variable(Variable),
+}
+
+#[derive(Debug)]
+pub struct Node {
+    factorgraph_id: FactorGraphId,
+    pub kind:       NodeKind,
+}
+
+impl Node {
+    /// Construct a new node
+    pub fn new(factorgraph_id: FactorGraphId, kind: NodeKind) -> Self {
+        Self {
+            factorgraph_id,
+            kind,
+        }
+    }
+
+    /// Returns `true` if the node is [`Factor`].
+    ///
+    /// [`Factor`]: Node::Factor
+    #[must_use]
+    #[inline]
+    pub fn is_factor(&self) -> bool {
+        self.kind.is_factor()
+    }
+
+    /// Returns `Some(&Factor)` if the node]s variant is [`Factor`], otherwise
+    /// `None`.
+    pub fn as_factor(&self) -> Option<&Factor> {
+        if let NodeKind::Factor(ref v) = self.kind {
+            Some(v)
+        } else {
+            None
+        }
+    }
+
+    /// Returns `Some(&mut Factor)` if the node]s variant is [`Factor`],
+    /// otherwise `None`.
+    pub fn as_factor_mut(&mut self) -> Option<&mut Factor> {
+        if let NodeKind::Factor(ref mut v) = self.kind {
+            Some(v)
+        } else {
+            None
+        }
+    }
+
+    /// Returns `true` if the node is [`Variable`].
+    ///
+    /// [`Variable`]: Node::Variable
+    #[must_use]
+    #[inline]
+    pub fn is_variable(&self) -> bool {
+        self.kind.is_variable()
+    }
+
+    /// Returns `Some(&Variable)` if the node]s variant is [`Variable`],
+    /// otherwise `None`.
+    pub fn as_variable(&self) -> Option<&Variable> {
+        if let NodeKind::Variable(ref v) = self.kind {
+            Some(v)
+        } else {
+            None
+        }
+    }
+
+    /// Returns `Some(&mut Variable)` if the node]s variant is [`Variable`],
+    /// otherwise `None`.
+    pub fn as_variable_mut(&mut self) -> Option<&mut Variable> {
+        if let NodeKind::Variable(ref mut v) = self.kind {
+            Some(v)
+        } else {
+            None
+        }
+    }
+}
+
+impl FactorGraphNode for Node {
+    fn remove_connection_to(
+        &mut self,
+        factorgraph_id: FactorGraphId,
+    ) -> Result<(), RemoveConnectionToError> {
+        match self.kind {
+            NodeKind::Factor(ref mut factor) => factor.remove_connection_to(factorgraph_id),
+            NodeKind::Variable(ref mut variable) => variable.remove_connection_to(factorgraph_id),
+        }
+    }
+
+    fn messages_sent(&self) -> usize {
+        match self.kind {
+            NodeKind::Factor(ref factor) => factor.messages_sent(),
+            NodeKind::Variable(ref variable) => variable.messages_sent(),
+        }
+    }
+
+    fn messages_received(&self) -> usize {
+        match self.kind {
+            NodeKind::Factor(ref factor) => factor.messages_received(),
+            NodeKind::Variable(ref variable) => variable.messages_received(),
+        }
+    }
+
+    fn reset_message_count(&mut self) {
+        match self.kind {
+            NodeKind::Factor(ref mut factor) => factor.reset_message_count(),
+            NodeKind::Variable(ref mut variable) => variable.reset_message_count(),
+        };
+    }
+}
diff --git a/crates/gbpplanner-rs/src/factorgraph/variable.rs b/crates/gbpplanner-rs/src/factorgraph/variable.rs
new file mode 100644
index 00000000..5101b412
--- /dev/null
+++ b/crates/gbpplanner-rs/src/factorgraph/variable.rs
@@ -0,0 +1,330 @@
+#![deny(missing_docs)]
+
+use bevy::log::{debug, error};
+use gbp_linalg::{pretty_print_matrix, pretty_print_vector, Float, Matrix, Vector};
+use ndarray_inverse::Inverse;
+use tap::Tap;
+
+use super::{
+    factorgraph::NodeIndex,
+    id::FactorId,
+    message::{InformationVec, Mean, Message, MessagesToFactors, PrecisionMatrix},
+    node::{FactorGraphNode, RemoveConnectionToError},
+    MessageCount,
+};
+use crate::{
+    escape_codes::*, pretty_print_line, pretty_print_message, pretty_print_subtitle,
+    pretty_print_title,
+};
+
+#[derive(Debug, Clone)]
+pub struct VariablePrior {
+    information_vector: Vector<Float>,
+    precision_matrix:   Matrix<Float>,
+}
+
+impl VariablePrior {
+    #[must_use]
+    fn new(information_vector: Vector<Float>, precision_matrix: Matrix<Float>) -> Self {
+        Self {
+            information_vector,
+            precision_matrix,
+        }
+    }
+}
+
+// TODO: use pretty_print_matrix!
+// impl std::fmt::Display for VariablePrior {
+//     fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+//         todo!()
+//     }
+// }
+
+/// PERF: use fixed size vectors and matrices, either bevy Vec4, or nalgebra
+/// Vec4
+#[derive(Debug, Clone)]
+pub struct VariableBelief {
+    pub information_vector: Vector<Float>,
+    pub precision_matrix: Matrix<Float>,
+    pub mean: Vector<Float>,
+
+    pub covariance_matrix: Matrix<Float>,
+    /// Flag to indicate if the variable's covariance is finite, i.e. it does
+    /// not contain NaNs or Infs In gbpplanner it is used to control if a
+    /// variable can be rendered.
+    valid: bool,
+}
+
+impl VariableBelief {
+    fn new(
+        information_vector: Vector<Float>,
+        precision_matrix: Matrix<Float>,
+        mean: Vector<Float>,
+        covariance_matrix: Matrix<Float>,
+    ) -> Self {
+        let valid = covariance_matrix.iter().all(|x| x.is_finite());
+        Self {
+            information_vector,
+            precision_matrix,
+            mean,
+            covariance_matrix,
+            valid,
+        }
+    }
+}
+
+impl From<VariableBelief> for Message {
+    fn from(value: VariableBelief) -> Self {
+        Message::new(
+            InformationVec(value.information_vector),
+            PrecisionMatrix(value.precision_matrix),
+            Mean(value.mean),
+        )
+    }
+}
+
+/// A variable in the factor graph.
+#[derive(Debug)]
+pub struct Variable {
+    pub prior:  VariablePrior,
+    pub belief: VariableBelief,
+
+    // / Flag to indicate if the variable's covariance is finite, i.e. it does
+    // / not contain NaNs or Infs In gbpplanner it is used to control if a
+    // / variable can be rendered.
+    // pub valid: bool,
+    /// Mailbox for incoming message storage
+    pub inbox: MessagesToFactors,
+
+    /// index
+    node_index: Option<NodeIndex>,
+
+    message_count: MessageCount,
+}
+
+impl Variable {
+    /// Returns the node index of the variable
+    ///
+    /// # Panics
+    ///
+    /// Panics if the node index has not been set, which should not happen.
+    #[inline]
+    pub fn node_index(&self) -> NodeIndex {
+        if self.node_index.is_none() {
+            panic!("The node index has not been set");
+        }
+        self.node_index.unwrap()
+    }
+
+    /// Returns the variables belief about its position
+    #[inline]
+    pub fn estimated_position(&self) -> [Float; 2] {
+        [self.belief.mean[0], self.belief.mean[1]]
+    }
+
+    /// Returns the variables belief about its velocity
+    #[inline]
+    pub fn estimated_velocity(&self) -> [Float; 2] {
+        [self.belief.mean[2], self.belief.mean[3]]
+    }
+
+    #[must_use]
+    pub fn new(
+        prior_mean: Vector<Float>,
+        mut prior_precision_matrix: Matrix<Float>,
+        dofs: usize,
+    ) -> Self {
+        if !prior_precision_matrix.iter().all(|x| x.is_finite()) {
+            prior_precision_matrix.fill(0.0);
+        }
+
+        let eta_prior = prior_precision_matrix.dot(&prior_mean);
+
+        let sigma = prior_precision_matrix
+            .inv()
+            .unwrap_or_else(|| Matrix::<Float>::zeros((dofs, dofs)));
+        let eta = eta_prior.clone();
+        let lam = prior_precision_matrix.clone();
+
+        Self {
+            prior: VariablePrior::new(eta_prior, prior_precision_matrix),
+            belief: VariableBelief::new(eta, lam, prior_mean, sigma),
+            inbox: MessagesToFactors::new(),
+            node_index: None,
+            message_count: MessageCount::default(),
+        }
+    }
+
+    pub fn set_node_index(&mut self, index: NodeIndex) {
+        if self.node_index.is_some() {
+            panic!("The node index is already set");
+        }
+        self.node_index = Some(index);
+    }
+
+    pub fn receive_message_from(&mut self, from: FactorId, message: Message) {
+        debug!("variable ? received message from {:?}", from);
+        if message.is_empty() {
+            // warn!("Empty message received from factor {:?}", from);
+        }
+        let _ = self.inbox.insert(from, message);
+        self.message_count.received += 1;
+    }
+
+    // // TODO: why never used?
+    // #[inline]
+    // pub fn read_message_from(&mut self, from: FactorId) -> Option<&Message> {
+    //     self.inbox.get(&from)
+    // }
+
+    /// Change the prior of the variable.
+    /// It updates the belief of the variable.
+    /// The prior acts as the pose factor
+    /// Called `Variable::change_variable_prior` in **gbpplanner**
+    pub fn change_prior(&mut self, mean: Vector<Float>) -> MessagesToFactors {
+        self.prior.information_vector = self.prior.precision_matrix.dot(&mean);
+        self.belief.mean = mean;
+
+        // FIXME: forgot this line in the original code
+        // this->belief_ = Message {this->eta_, this->lam_, this->mu_};
+
+        let messages: MessagesToFactors = self
+            .inbox
+            .keys()
+            .map(|factor_id| (*factor_id, self.belief.clone().into()))
+            .collect();
+
+        for message in self.inbox.values_mut() {
+            *message = Message::empty();
+        }
+
+        messages
+    }
+
+    pub fn prepare_message(&self) -> Message {
+        Message::new(
+            InformationVec(self.belief.information_vector.clone()),
+            PrecisionMatrix(self.belief.precision_matrix.clone()),
+            Mean(self.belief.mean.clone()),
+        )
+    }
+
+    // /****************************************************************************
+    // *******************************/ // Variable belief update step:
+    // // Aggregates all the messages from its connected factors (begins with the
+    // prior, as this is effectively a unary factor) // The valid_ flag is
+    // useful for drawing the variable. // Finally the outgoing messages to
+    // factors is created. /****************************************************
+    // *******************************************************/
+    /// Variable Belief Update step (Step 1 in the GBP algorithm)
+    /// called `Variable::update_belief` in **gbpplanner**
+    pub fn update_belief_and_create_factor_responses(&mut self) -> MessagesToFactors {
+        // Collect messages from all other factors, begin by "collecting message from
+        // pose factor prior"
+        self.belief.information_vector = self.prior.information_vector.clone();
+        self.belief.precision_matrix = self.prior.precision_matrix.clone();
+
+        // Go through received messages and update belief
+        for (_, message) in self.inbox.iter() {
+            let Some(payload) = message.payload() else {
+                continue;
+            };
+            self.belief.information_vector =
+                &self.belief.information_vector + &payload.information_factor;
+            self.belief.precision_matrix =
+                &self.belief.precision_matrix + &payload.precision_matrix;
+        }
+
+        // Update belief
+        // NOTE: This might not be correct, but it seems the `.inv()` method doesn't
+        // catch and all-zero matrix
+        let lam_not_zero = self.belief.precision_matrix.iter().any(|x| *x - 1e-6 > 0.0);
+        if lam_not_zero {
+            if let Some(sigma) = self.belief.precision_matrix.inv() {
+                self.belief.covariance_matrix = sigma;
+                self.belief.valid = self.belief.covariance_matrix.iter().all(|x| x.is_finite());
+                if self.belief.valid {
+                    self.belief.mean = self
+                        .belief
+                        .covariance_matrix
+                        .dot(&self.belief.information_vector);
+                } else {
+                    println!(
+                        "{}:{},Variable covariance is not finite",
+                        file!().split('/').last().unwrap(),
+                        line!()
+                    );
+                }
+            }
+        }
+
+        let messages: MessagesToFactors = self
+            .inbox
+            .iter()
+            .map(|(&factor_id, received_message)| {
+                let response = received_message.payload().map_or_else(
+                    || self.prepare_message(),
+                    |message_from_factor| {
+                        let msg = Message::new(
+                            InformationVec(
+                                &self.belief.information_vector
+                                    - &message_from_factor.information_factor,
+                            ),
+                            PrecisionMatrix(
+                                &self.belief.precision_matrix
+                                    - &message_from_factor.precision_matrix,
+                            ),
+                            Mean(&self.belief.mean - &message_from_factor.mean),
+                        );
+                        msg
+                    },
+                );
+                (factor_id, response)
+            })
+            .collect();
+
+        self.message_count.sent += messages.len();
+
+        messages
+    }
+
+    /// Returns `true` if the covariance matrix is finite, `false` otherwise.
+    #[inline]
+    pub fn finite_covariance(&self) -> bool {
+        self.belief.valid
+    }
+}
+
+impl FactorGraphNode for Variable {
+    fn remove_connection_to(
+        &mut self,
+        factorgraph_id: super::factorgraph::FactorGraphId,
+    ) -> Result<(), RemoveConnectionToError> {
+        let connections_before = self.inbox.len();
+        self.inbox
+            .retain(|factor_id, v| factor_id.factorgraph_id != factorgraph_id);
+        let connections_after = self.inbox.len();
+
+        let no_connections_removed = connections_before == connections_after;
+        if no_connections_removed {
+            Err(RemoveConnectionToError)
+        } else {
+            Ok(())
+        }
+    }
+
+    #[inline(always)]
+    fn messages_sent(&self) -> usize {
+        self.message_count.sent
+    }
+
+    #[inline(always)]
+    fn messages_received(&self) -> usize {
+        self.message_count.received
+    }
+
+    #[inline(always)]
+    fn reset_message_count(&mut self) {
+        self.message_count.reset();
+    }
+}
diff --git a/crates/gbpplanner-rs/src/input/general.rs b/crates/gbpplanner-rs/src/input/general.rs
index 35ddafc5..931aa711 100644
--- a/crates/gbpplanner-rs/src/input/general.rs
+++ b/crates/gbpplanner-rs/src/input/general.rs
@@ -15,7 +15,12 @@ use tap::Tap;
 use super::{super::theme::CycleTheme, screenshot::TakeScreenshot};
 use crate::{
     config::Config,
-    planner::{FactorGraph, NodeKind, PausePlayEvent, RobotId, RobotState},
+    factorgraph::{
+        graphviz::{Graph, NodeKind},
+        prelude::FactorGraph,
+    },
+    pause_play::PausePlay,
+    planner::{RobotId, RobotState},
     theme::CatppuccinTheme,
     ui::{ChangingBinding, ExportGraphEvent},
 };
@@ -105,7 +110,6 @@ fn bind_general_input(mut commands: Commands) {
 fn export_factorgraphs_as_graphviz(
     query: Query<(Entity, &FactorGraph), With<RobotState>>,
     config: &Config,
-    // config: Res<Config>,
 ) -> Option<String> {
     if query.is_empty() {
         // There are no factorgraph in the scene/world
@@ -181,13 +185,15 @@ fn export_factorgraphs_as_graphviz(
         let external_connections: HashMap<usize, (RobotId, usize)> = nodes
             .into_iter()
             .filter_map(|node| match node.kind {
-                NodeKind::InterRobotFactor(connection) => Some((
+                NodeKind::InterRobotFactor(external_variable) => Some((
                     node.index,
                     (
-                        connection.id_of_robot_connected_with,
-                        connection
-                            .index_of_connected_variable_in_other_robots_factorgraph
-                            .index(),
+                        external_variable.factorgraph_id,
+                        external_variable.variable_index.index(),
+                        // connection.id_of_robot_connected_with,
+                        // connection
+                        //     .index_of_connected_variable_in_other_robots_factorgraph
+                        //     .index(),
                     ),
                 )),
                 _ => None,
@@ -374,7 +380,7 @@ fn general_actions_system(
     // mut app_exit_event: EventWriter<AppExit>,
     mut quit_application_event: EventWriter<QuitApplicationEvent>,
     export_graph_finished_event: EventWriter<ExportGraphFinishedEvent>,
-    mut pause_play_event: EventWriter<PausePlayEvent>,
+    mut pause_play_event: EventWriter<PausePlay>,
     mut toast_event: EventWriter<ToastEvent>,
 ) {
     if currently_changing.on_cooldown() || currently_changing.is_changing() {
@@ -402,7 +408,7 @@ fn general_actions_system(
         // app_exit_event.send(AppExit);
     } else if action_state.just_pressed(&GeneralAction::PausePlaySimulation) {
         info!("toggling pause/play simulation");
-        pause_play_event.send(PausePlayEvent::Toggle);
+        pause_play_event.send(PausePlay::Toggle);
     }
 }
 
diff --git a/crates/gbpplanner-rs/src/main.rs b/crates/gbpplanner-rs/src/main.rs
index 298bee8e..2a895d27 100644
--- a/crates/gbpplanner-rs/src/main.rs
+++ b/crates/gbpplanner-rs/src/main.rs
@@ -7,9 +7,11 @@ mod cli;
 pub(crate) mod config;
 mod diagnostic;
 mod environment;
+mod factorgraph;
 mod input;
 mod moveable_object;
 mod movement;
+pub(crate) mod pause_play;
 mod planner;
 mod robot_spawner;
 mod scene;
@@ -41,6 +43,7 @@ use crate::{
     environment::EnvironmentPlugin,
     input::InputPlugin,
     movement::MovementPlugin,
+    pause_play::PausePlayPlugin,
     planner::PlannerPlugin,
     robot_spawner::RobotSpawnerPlugin,
     scene::ScenePlugin,
@@ -232,6 +235,7 @@ fn main() -> anyhow::Result<()> {
         // our plugins
         .add_plugins((
             DefaultPickingPlugins,
+            PausePlayPlugin::default(),
             ThemePlugin,       // Custom
             AssetLoaderPlugin, // Custom
             EnvironmentPlugin, // Custom
diff --git a/crates/gbpplanner-rs/src/pause_play.rs b/crates/gbpplanner-rs/src/pause_play.rs
new file mode 100644
index 00000000..606478a1
--- /dev/null
+++ b/crates/gbpplanner-rs/src/pause_play.rs
@@ -0,0 +1,58 @@
+//! Module for pausing and resuming the simulation.
+
+use bevy::prelude::*;
+
+/// Plugin for pausing and resuming the simulation.
+#[derive(Default)]
+pub struct PausePlayPlugin;
+
+impl Plugin for PausePlayPlugin {
+    fn build(&self, app: &mut App) {
+        app.insert_state(PausedState::default())
+            .add_event::<PausePlay>()
+            .add_systems(PreUpdate, pause_play_virtual_time);
+    }
+}
+
+/// State keeping track of whether the simulation is paused or not.
+#[derive(States, Default, Debug, Clone, PartialEq, Eq, Hash, derive_more::IsVariant)]
+pub enum PausedState {
+    #[default]
+    Running,
+    Paused,
+}
+
+/// Event for pausing and resuming the simulation.
+#[derive(Debug, Clone, Copy, Default, Event)]
+pub enum PausePlay {
+    #[default]
+    Toggle,
+    Pause,
+    Play,
+}
+
+/// System that reacts to events for pausing and resuming the simulation.
+fn pause_play_virtual_time(
+    state: ResMut<State<PausedState>>,
+    mut next_state: ResMut<NextState<PausedState>>,
+    mut pause_play_event: EventReader<PausePlay>,
+    mut time: ResMut<Time<Virtual>>,
+) {
+    for pause_play_event in pause_play_event.read() {
+        debug!("received event: {:?}", pause_play_event);
+        match (pause_play_event, state.get()) {
+            (PausePlay::Pause, PausedState::Paused) | (PausePlay::Play, PausedState::Running) => {
+                warn!("ignoring duplicate event: {:?}", pause_play_event);
+                continue;
+            }
+            (PausePlay::Pause | PausePlay::Toggle, PausedState::Running) => {
+                next_state.set(PausedState::Paused);
+                time.pause();
+            }
+            (PausePlay::Play | PausePlay::Toggle, PausedState::Paused) => {
+                next_state.set(PausedState::Running);
+                time.unpause();
+            }
+        };
+    }
+}
diff --git a/crates/gbpplanner-rs/src/planner/debug.rs b/crates/gbpplanner-rs/src/planner/debug.rs
deleted file mode 100644
index 875cc07f..00000000
--- a/crates/gbpplanner-rs/src/planner/debug.rs
+++ /dev/null
@@ -1,23 +0,0 @@
-use bevy::prelude::*;
-
-use super::{FactorGraph, RobotState};
-
-pub struct PlannerDebugPlugin;
-
-impl Plugin for PlannerDebugPlugin {
-    fn build(&self, app: &mut App) {
-        app.add_systems(Update, debug);
-    }
-}
-
-fn debug(q: Query<(Entity, &RobotState, &FactorGraph)>) {
-    for (entity, state, graph) in q.iter() {
-        // println!("{:?}", entity);
-        // println!("{:?}", state);
-        // println!("{:?}", graph);
-        graph.variables().for_each(|(index, variable)| {
-            // println!("variable {:?}: mu {}", index, variable.belief.mu);
-        })
-    }
-    // std::process
-}
diff --git a/crates/gbpplanner-rs/src/planner/factor.rs b/crates/gbpplanner-rs/src/planner/factor.rs
deleted file mode 100644
index 9125d937..00000000
--- a/crates/gbpplanner-rs/src/planner/factor.rs
+++ /dev/null
@@ -1,968 +0,0 @@
-use std::{
-    collections::{BTreeMap, HashMap},
-    num::NonZeroUsize,
-    ops::{AddAssign, Sub},
-};
-
-use bevy::{
-    log::{info, warn},
-    render::texture::Image,
-};
-use gbp_linalg::{pretty_print_matrix, pretty_print_vector, Float, Matrix, Vector, VectorNorm};
-use ndarray::{array, concatenate, s, Axis};
-use petgraph::prelude::NodeIndex;
-use typed_floats::StrictlyPositiveFinite;
-
-use super::{
-    factorgraph::{
-        FactorGraphNode, MessageCount, MessagesFromFactors, MessagesToVariables, VariableId,
-    },
-    message::Message,
-    robot::RobotId,
-};
-use crate::{
-    escape_codes::*,
-    planner::{factorgraph::FactorId, marginalise_factor_distance::marginalise_factor_distance},
-    pretty_print_message, pretty_print_subtitle,
-};
-
-// TODO: make generic over f32 | f64
-// TODO: hide the state parameter from the public API, by having the `Factor`
-// struct expose similar methods that dispatch to the `FactorState` struct.
-trait Model {
-    /// The name of the factor. Used for debugging and visualization.
-    fn name(&self) -> &'static str;
-
-    fn jacobian_delta(&self) -> Float;
-
-    /// Whether to skip this factor in the update step
-    /// In gbpplanner, this is only used for the interrobot factor.
-    /// The other factors are always included in the update step.
-    fn skip(&mut self, state: &FactorState) -> bool;
-
-    /// Whether the factor is linear or non-linear
-    fn linear(&self) -> bool;
-
-    #[must_use]
-    #[inline]
-    fn jacobian(&mut self, state: &FactorState, x: &Vector<Float>) -> Matrix<Float> {
-        self.first_order_jacobian(state, x.clone())
-    }
-
-    /// Measurement function
-    /// **Note**: This method takes a mutable reference to self, because the
-    /// interrobot factor
-    fn measure(&mut self, state: &FactorState, x: &Vector<Float>) -> Vector<Float>;
-
-    fn first_order_jacobian(&mut self, state: &FactorState, mut x: Vector<Float>) -> Matrix<Float> {
-        let h0 = self.measure(state, &x); // value at linearization point
-        let mut jacobian = Matrix::<Float>::zeros((h0.len(), x.len()));
-
-        for i in 0..x.len() {
-            x[i] += self.jacobian_delta(); // perturb by delta
-            let derivatives = (self.measure(state, &x) - &h0) / self.jacobian_delta();
-            jacobian.column_mut(i).assign(&derivatives);
-            x[i] -= self.jacobian_delta(); // reset the perturbation
-        }
-
-        jacobian
-    }
-}
-
-#[derive(Debug, Clone, Copy)]
-pub struct InterRobotConnection {
-    pub id_of_robot_connected_with: RobotId,
-    pub index_of_connected_variable_in_other_robots_factorgraph: NodeIndex,
-}
-
-impl InterRobotConnection {
-    #[must_use]
-    pub fn new(
-        id_of_robot_connected_with: RobotId,
-        index_of_connected_variable_in_other_robots_factorgraph: NodeIndex,
-    ) -> Self {
-        Self {
-            id_of_robot_connected_with,
-            index_of_connected_variable_in_other_robots_factorgraph,
-        }
-    }
-}
-
-// #[derive(Debug, Clone, Copy, PartialEq, Eq)]
-// pub struct Skip(bool);
-
-/// Interrobot factor: for avoidance of other robots
-/// This factor results in a high energy or cost if two robots are planning to
-/// be in the same position at the same timestep (collision). This factor is
-/// created between variables of two robots. The factor has 0 energy if the
-/// variables are further away than the safety distance.
-#[derive(Debug, Clone)]
-pub struct InterRobotFactor {
-    safety_distance: Float,
-    skip: bool,
-    pub connection: InterRobotConnection,
-}
-
-#[derive(Debug, thiserror::Error)]
-pub enum InterRobotFactorError {
-    #[error("The robot radius must be positive, but it was {0}")]
-    NegativeRobotRadius(Float),
-}
-
-impl InterRobotFactor {
-    #[must_use]
-    pub fn new(
-        robot_radius: Float,
-        connection: InterRobotConnection,
-    ) -> Result<Self, InterRobotFactorError> {
-        if robot_radius < 0.0 {
-            return Err(InterRobotFactorError::NegativeRobotRadius(robot_radius));
-        }
-        let epsilon = 0.2 * robot_radius;
-
-        Ok(Self {
-            safety_distance: 2.0 * robot_radius + epsilon,
-            skip: false,
-            connection,
-        })
-    }
-}
-
-impl Model for InterRobotFactor {
-    #[inline(always)]
-    fn name(&self) -> &'static str {
-        "InterRobotFactor"
-    }
-
-    fn jacobian(&mut self, state: &FactorState, x: &Vector<Float>) -> Matrix<Float> {
-        let dofs = state.dofs.get();
-        let mut jacobian = Matrix::<Float>::zeros((state.initial_measurement.len(), dofs * 2));
-        let x_diff = {
-            let offset = dofs / 2;
-            let mut x_diff = x.slice(s![..offset]).sub(&x.slice(s![dofs..dofs + offset]));
-
-            // NOTE: In gbplanner, they weight this by the robot id, why they do this is
-            // unclear as a robot id should be unique, and not have any
-            // semantics of distance/weight.
-            for i in 0..offset {
-                x_diff[i] += 1e-6 * self.connection.id_of_robot_connected_with.index() as Float;
-                // Add a tiny random offset to avoid div/0 errors
-            }
-            x_diff
-        };
-
-        let radius = x_diff.euclidean_norm();
-        if radius <= self.safety_distance {
-            // TODO: why do we change the Jacobian if we are not outside the safety
-            // distance?
-
-            // J(0, seqN(0, n_dofs_ / 2)) = -1.f / safety_distance_ / r * X_diff;
-            jacobian
-                .slice_mut(s![0, ..dofs / 2])
-                .assign(&(-1.0 / self.safety_distance / radius * &x_diff));
-
-            // J(0, seqN(n_dofs_, n_dofs_ / 2)) = 1.f / safety_distance_ / r * X_diff;
-            jacobian
-                .slice_mut(s![0, dofs..dofs + (dofs / 2)])
-                .assign(&(1.0 / self.safety_distance / radius * &x_diff));
-        }
-        jacobian
-    }
-
-    fn measure(&mut self, state: &FactorState, x: &Vector<Float>) -> Vector<Float> {
-        let dofs = state.dofs.get();
-        let mut h = Vector::<Float>::zeros(state.initial_measurement.len());
-        let x_diff = {
-            let offset = dofs / 2;
-            let mut x_diff = x.slice(s![..offset]).sub(&x.slice(s![dofs..dofs + offset]));
-            // NOTE: In gbplanner, they weight this by the robot id, why they do this is
-            // unclear as a robot id should be unique, and not have any
-            // semantics of distance/weight.
-            for i in 0..offset {
-                // Add a tiny random offset to avoid div/0 errors
-                x_diff[i] += 1e-6 * self.connection.id_of_robot_connected_with.index() as Float;
-            }
-            x_diff
-        };
-
-        let radius = x_diff.euclidean_norm();
-        let within_safety_distance = radius <= self.safety_distance;
-        // match (self.skip, within_safety_distance) {
-        //     (Skip(true), true) => {}
-        //     (Skip(true), false) => {}
-        //     (Skip(false), true) => {
-        //         self.skip = Skip(true);
-        //         info!("skip = true, radius = {}", radius);
-        //     }
-        //     (Skip(false), false) => {}
-        // };
-        if radius <= self.safety_distance {
-            if self.skip {
-                info!(
-                    "within safety distance, radius = {}, setting self.skip to false",
-                    radius
-                );
-            }
-            self.skip = false;
-            // gbpplanner: h(0) = 1.f*(1 - r/safety_distance_);
-            // NOTE: in Eigen, indexing a matrix with a single index corresponds to indexing
-            // the matrix as a flattened array in column-major order.
-            // h[(0, 0)] = 1.0 * (1.0 - radius / self.safety_distance);
-            h[0] = 1.0 * (1.0 - radius / self.safety_distance);
-            // println!("h = {}", h);
-        } else {
-            if !self.skip {
-                // info!(
-                //     "outside safety distance, radius = {}, setting self.skip
-                // to true",     radius
-                // );
-            }
-            self.skip = true;
-        }
-
-        h
-    }
-
-    #[inline(always)]
-    fn jacobian_delta(&self) -> Float {
-        1e-2
-    }
-
-    fn skip(&mut self, state: &FactorState) -> bool {
-        // this->skip_flag = ( (X_(seqN(0,n_dofs_/2)) - X_(seqN(n_dofs_,
-        // n_dofs_/2))).squaredNorm() >= safety_distance_*safety_distance_ );
-        let dofs = state.dofs.get();
-        let offset = dofs / 2;
-
-        // [..offset] is the position of the first variable
-        // [dofs..dofs + offset] is the position of the other variable
-
-        let difference_between_estimated_positions = state
-            .linearisation_point
-            .slice(s![..offset])
-            .sub(&state.linearisation_point.slice(s![dofs..dofs + offset]));
-        let squared_norm = difference_between_estimated_positions
-            .mapv(|x| x.powi(2))
-            .sum();
-
-        let skip = squared_norm >= self.safety_distance.powi(2);
-        // let skip = squared_norm >= Float::powi(self.safety_distance, 2);
-        if self.skip != skip {
-            // warn!(
-            //     "skip = {}, squared_norm = {} safety_distance^2 = {}",
-            //     skip,
-            //     squared_norm,
-            //     Float::powi(self.safety_distance, 2)
-            // );
-        }
-        self.skip = skip;
-        // self.skip = squared_norm >= Float::powi(self.safety_distance, 2);
-        self.skip
-    }
-
-    #[inline(always)]
-    fn linear(&self) -> bool {
-        false
-    }
-}
-
-/// Dynamic factor: constant velocity model
-#[derive(Debug)]
-pub struct DynamicFactor {
-    cached_jacobian: Matrix<Float>,
-}
-
-// TODO: constrain within the limits that a differential drive robot can turn
-
-impl DynamicFactor {
-    #[must_use]
-    pub fn new(state: &mut FactorState, delta_t: Float) -> Self {
-        let dofs = state.dofs.get();
-        let eye = Matrix::<Float>::eye(dofs / 2);
-        let zeros = Matrix::<Float>::zeros((dofs / 2, dofs / 2));
-        #[allow(clippy::similar_names)]
-        let qc_inv = Float::powi(state.strength, -2) * &eye;
-        // pretty_print_matrix!(&qc_inv);
-
-        #[allow(clippy::similar_names)]
-        let qi_inv = concatenate![
-            Axis(0),
-            concatenate![
-                Axis(1),
-                12.0 * Float::powi(delta_t, -3) * &qc_inv,
-                -6.0 * Float::powi(delta_t, -2) * &qc_inv
-            ],
-            concatenate![
-                Axis(1),
-                -6.0 * Float::powi(delta_t, -2) * &qc_inv,
-                (4.0 / delta_t) * &qc_inv
-            ]
-        ];
-        debug_assert_eq!(qi_inv.shape(), &[dofs, dofs]);
-
-        // pretty_print_matrix!(&qi_inv);
-
-        // std::process::exit(1);
-
-        state.measurement_precision = qi_inv;
-
-        let cached_jacobian = concatenate![
-            Axis(0),
-            concatenate![Axis(1), eye, delta_t * &eye, -1.0 * &eye, zeros],
-            concatenate![Axis(1), zeros, eye, zeros, -1.0 * &eye]
-        ];
-        debug_assert_eq!(cached_jacobian.shape(), &[dofs, dofs * 2]);
-
-        // pretty_print_matrix!(&cached_jacobian);
-
-        // std::process::exit(1);
-
-        Self { cached_jacobian }
-    }
-}
-
-impl Model for DynamicFactor {
-    #[inline]
-    fn name(&self) -> &'static str {
-        "DynamicFactor"
-    }
-
-    #[inline]
-    fn jacobian(&mut self, _state: &FactorState, _x: &Vector<Float>) -> Matrix<Float> {
-        self.cached_jacobian.clone()
-    }
-
-    #[inline(always)]
-    fn measure(&mut self, _state: &FactorState, x: &Vector<Float>) -> Vector<Float> {
-        self.cached_jacobian.dot(x)
-    }
-
-    #[inline(always)]
-    fn skip(&mut self, _state: &FactorState) -> bool {
-        false
-    }
-
-    #[inline(always)]
-    fn jacobian_delta(&self) -> Float {
-        1e-8
-    }
-
-    #[inline(always)]
-    fn linear(&self) -> bool {
-        true
-    }
-}
-
-#[derive(Debug)]
-pub struct PoseFactor;
-
-impl Model for PoseFactor {
-    #[inline(always)]
-    fn name(&self) -> &'static str {
-        "PoseFactor"
-    }
-
-    /// Default jacobian is the first order taylor series jacobian
-    fn jacobian(&mut self, state: &FactorState, x: &Vector<Float>) -> Matrix<Float> {
-        self.first_order_jacobian(state, x.clone())
-    }
-
-    /// Default measurement function is the identity function
-    #[inline(always)]
-    fn measure(&mut self, _state: &FactorState, x: &Vector<Float>) -> Vector<Float> {
-        x.clone()
-    }
-
-    #[inline(always)]
-    fn skip(&mut self, _state: &FactorState) -> bool {
-        false
-    }
-
-    #[inline(always)]
-    fn jacobian_delta(&self) -> Float {
-        1e-8
-    }
-
-    #[inline(always)]
-    fn linear(&self) -> bool {
-        false
-    }
-}
-
-#[derive(Clone)]
-pub struct ObstacleFactor {
-    /// The signed distance field of the environment
-    obstacle_sdf: &'static Image,
-    /// Copy of the `WORLD_SZ` setting from **gbpplanner**, that we store a copy
-    /// of here since `ObstacleFactor` needs this information to calculate
-    /// `.jacobian_delta()` and `.measurement()`
-    world_size:   Float,
-}
-
-impl std::fmt::Debug for ObstacleFactor {
-    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
-        f.debug_struct("ObstacleFactor")
-            // .field("obstacle_sdf", &self.obstacle_sdf)
-            .field("world_size", &self.world_size)
-            .finish()
-    }
-}
-
-impl ObstacleFactor {
-    /// Creates a new [`ObstacleFactor`].
-    #[must_use]
-    fn new(obstacle_sdf: &'static Image, world_size: Float) -> Self {
-        Self {
-            obstacle_sdf,
-            world_size,
-        }
-    }
-}
-
-impl Model for ObstacleFactor {
-    #[inline]
-    fn name(&self) -> &'static str {
-        "ObstacleFactor"
-    }
-
-    #[inline]
-    fn jacobian(&mut self, state: &FactorState, x: &Vector<Float>) -> Matrix<Float> {
-        // Same as PoseFactor
-        // TODO: change to not clone x
-        self.first_order_jacobian(state, x.clone())
-    }
-
-    fn measure(&mut self, _state: &FactorState, x: &Vector<Float>) -> Vector<Float> {
-        // pretty_print_vector!(x);
-        debug_assert!(x.len() >= 2, "x.len() = {}", x.len());
-        // White areas are obstacles, so h(0) should return a 1 for these regions.
-        let scale = self.obstacle_sdf.width() as Float / self.world_size;
-        // let offset = (self.world_size / 2.0) as usize;
-        let offset = self.world_size / 2.0;
-        if (x[0] + offset) * scale > self.obstacle_sdf.width() as Float {
-            // warn!(
-            //     "x[0] + offset = {}, scale = {}, width = {}",
-            //     (x[0] + offset) * scale,
-            //     scale,
-            //     self.obstacle_sdf.width()
-            // );
-            return array![0.0];
-        }
-        if (x[1] + offset) * scale > self.obstacle_sdf.height() as Float {
-            // warn!(
-            //     "x[1] + offset = {}, scale = {}, height = {}",
-            //     (x[1] + offset) * scale,
-            //     scale,
-            //     self.obstacle_sdf.height()
-            // );
-            return array![0.0];
-        }
-        // dbg!(offset);
-        let pixel_x = ((x[0] + offset) * scale) as u32;
-        let pixel_y = ((x[1] + offset) * scale) as u32;
-        // println!("pixel_x = {}, pixel_y = {}", pixel_x, pixel_y);
-        // dbg!(pixel_x, pixel_y);
-        // assert_eq!((self.obstacle_sdf.width() * self.obstacle_sdf.height() * 4) as
-        // usize, self.obstacle_sdf.data.len()); multiply by 4 because the image
-        // is in RGBA format, and we simply use th R channel to determine value,
-        // as the image is grayscale
-        // TODO: assert that the image's data is laid out in row-major order
-        let linear_index = ((self.obstacle_sdf.width() * pixel_y + pixel_x) * 4) as usize;
-        if linear_index >= self.obstacle_sdf.data.len() {
-            warn!(
-                "linear_index = {}, obstacle_sdf.data.len() = {}",
-                linear_index,
-                self.obstacle_sdf.data.len()
-            );
-            return array![0.0];
-        }
-        let red = self.obstacle_sdf.data[linear_index];
-        // NOTE: do 1.0 - red to invert the value, as the obstacle sdf is white where
-        // there are obstacles in gbpplanner, they do not do the inversion here,
-        // but instead invert the entire image, when they load it from disk.
-        let hsv_value = 1.0 - red as Float / 255.0;
-        // let hsv_value = pixel as Float / 255.0;
-        // if hsv_value <= 0.5 {
-        //     println!("image(x={}, y={}).z {} (scale = {})", pixel_x, pixel_y,
-        // hsv_value, scale); }
-        // dbg!(hsv_value);
-
-        // println!("hsv_value = {}", hsv_value);
-
-        array![hsv_value]
-    }
-
-    #[inline(always)]
-    fn jacobian_delta(&self) -> Float {
-        self.world_size / self.obstacle_sdf.width() as Float
-    }
-
-    #[inline(always)]
-    fn skip(&mut self, _state: &FactorState) -> bool {
-        false
-    }
-
-    #[inline(always)]
-    fn linear(&self) -> bool {
-        false
-    }
-}
-
-#[derive(Debug, derive_more::IsVariant)]
-pub enum FactorKind {
-    Pose(PoseFactor),
-    InterRobot(InterRobotFactor),
-    Dynamic(DynamicFactor),
-    Obstacle(ObstacleFactor),
-}
-
-impl FactorKind {
-    // /// Returns `true` if the factor kind is [`Obstacle`].
-    // ///
-    // /// [`Obstacle`]: FactorKind::Obstacle
-    // #[must_use]
-    // pub fn is_obstacle(&self) -> bool {
-    //     matches!(self, Self::Obstacle(..))
-    // }
-
-    // /// Returns `true` if the factor kind is [`Dynamic`].
-    // ///
-    // /// [`Dynamic`]: FactorKind::Dynamic
-    // #[must_use]
-    // pub fn is_dynamic(&self) -> bool {
-    //     matches!(self, Self::Dynamic(..))
-    // }
-
-    // /// Returns `true` if the factor kind is [`InterRobot`].
-    // ///
-    // /// [`InterRobot`]: FactorKind::InterRobot
-    // #[must_use]
-    // pub fn is_inter_robot(&self) -> bool {
-    //     matches!(self, Self::InterRobot(..))
-    // }
-
-    // /// Returns `true` if the factor kind is [`Pose`].
-    // ///
-    // /// [`Pose`]: FactorKind::Pose
-    // #[must_use]
-    // pub fn is_pose(&self) -> bool {
-    //     matches!(self, Self::Pose(..))
-    // }
-
-    pub fn as_pose(&self) -> Option<&PoseFactor> {
-        if let Self::Pose(v) = self {
-            Some(v)
-        } else {
-            None
-        }
-    }
-
-    pub fn as_inter_robot(&self) -> Option<&InterRobotFactor> {
-        if let Self::InterRobot(v) = self {
-            Some(v)
-        } else {
-            None
-        }
-    }
-
-    pub fn as_dynamic(&self) -> Option<&DynamicFactor> {
-        if let Self::Dynamic(v) = self {
-            Some(v)
-        } else {
-            None
-        }
-    }
-
-    pub fn as_obstacle(&self) -> Option<&ObstacleFactor> {
-        if let Self::Obstacle(v) = self {
-            Some(v)
-        } else {
-            None
-        }
-    }
-}
-
-// TODO(kpbaks): use enum_dispatch crate
-
-impl Model for FactorKind {
-    fn name(&self) -> &'static str {
-        match self {
-            FactorKind::Pose(f) => f.name(),
-            FactorKind::InterRobot(f) => f.name(),
-            FactorKind::Dynamic(f) => f.name(),
-            FactorKind::Obstacle(f) => f.name(),
-        }
-    }
-
-    fn jacobian(&mut self, state: &FactorState, x: &Vector<Float>) -> Matrix<Float> {
-        match self {
-            FactorKind::Pose(f) => f.jacobian(state, x),
-            FactorKind::InterRobot(f) => f.jacobian(state, x),
-            FactorKind::Dynamic(f) => f.jacobian(state, x),
-            FactorKind::Obstacle(f) => f.jacobian(state, x),
-        }
-    }
-
-    fn measure(&mut self, state: &FactorState, x: &Vector<Float>) -> Vector<Float> {
-        match self {
-            FactorKind::Pose(f) => f.measure(state, x),
-            FactorKind::InterRobot(f) => f.measure(state, x),
-            FactorKind::Dynamic(f) => f.measure(state, x),
-            FactorKind::Obstacle(f) => f.measure(state, x),
-        }
-    }
-
-    fn skip(&mut self, state: &FactorState) -> bool {
-        match self {
-            FactorKind::Pose(f) => f.skip(state),
-            FactorKind::InterRobot(f) => f.skip(state),
-            FactorKind::Dynamic(f) => f.skip(state),
-            FactorKind::Obstacle(f) => f.skip(state),
-        }
-    }
-
-    fn jacobian_delta(&self) -> Float {
-        match self {
-            FactorKind::Pose(f) => f.jacobian_delta(),
-            FactorKind::InterRobot(f) => f.jacobian_delta(),
-            FactorKind::Dynamic(f) => f.jacobian_delta(),
-            FactorKind::Obstacle(f) => f.jacobian_delta(),
-        }
-    }
-
-    fn linear(&self) -> bool {
-        match self {
-            FactorKind::Pose(f) => f.linear(),
-            FactorKind::InterRobot(f) => f.linear(),
-            FactorKind::Dynamic(f) => f.linear(),
-            FactorKind::Obstacle(f) => f.linear(),
-        }
-    }
-}
-
-// TODO: make generic over f32 | f64
-// <T: nalgebra::Scalar + Copy>
-#[derive(Debug, Clone)]
-pub struct FactorState {
-    /// called `z_` in **gbpplanner**
-    pub initial_measurement: Vector<Float>,
-    /// called `meas_model_lambda_` in **gbpplanner**
-    pub measurement_precision: Matrix<Float>,
-    /// Stored linearisation point
-    /// called `X_` in **gbpplanner**, they use `Eigen::MatrixXd` instead
-    pub linearisation_point: Vector<Float>,
-    /// Strength of the factor. Called `sigma` in gbpplanner.
-    /// The factor precision $Lambda = sigma^-2 * Identify$
-    pub strength: Float,
-    /// Number of degrees of freedom e.g. 4 [x, y, x', y']
-    pub dofs: NonZeroUsize,
-
-    /// Cached value of the factors jacobian function
-    /// called `J_` in **gbpplanner**
-    pub cached_jacobian: Matrix<Float>,
-
-    /// Cached value of the factors jacobian function
-    /// called `h_` in **gbpplanner**
-    pub cached_measurement: Vector<Float>,
-    /// Set to true after the first call to self.update()
-    /// TODO: move to FactorState
-    initialized: bool,
-}
-
-impl FactorState {
-    fn new(
-        measurement: Vector<Float>,
-        strength: Float,
-        dofs: NonZeroUsize,
-        neighbor_amount: usize,
-    ) -> Self {
-        // Initialise precision of the measurement function
-        // this->meas_model_lambda_ = Eigen::MatrixXd::Identity(z_.rows(), z_.rows()) /
-        // pow(sigma,2.);
-        let measurement_precision =
-            Matrix::<Float>::eye(measurement.len()) / Float::powi(strength, 2);
-
-        Self {
-            initial_measurement: measurement,
-            measurement_precision,
-            linearisation_point: Vector::<Float>::zeros(dofs.get() * neighbor_amount),
-            strength,
-            dofs,
-            cached_jacobian: array![[]],
-            cached_measurement: array![],
-            initialized: false,
-        }
-    }
-}
-
-#[derive(Debug)]
-pub struct Factor {
-    /// Unique identifier that associates the variable with a factorgraph/robot.
-    pub node_index: Option<NodeIndex>,
-    /// State common between all factor kinds
-    pub state:      FactorState,
-    /// Variant storing the specialized behavior of each Factor kind.
-    pub kind:       FactorKind,
-    /// Mailbox for incoming message storage
-    pub inbox:      MessagesFromFactors,
-
-    message_count: MessageCount,
-}
-
-impl Factor {
-    fn new(state: FactorState, kind: FactorKind) -> Self {
-        Self {
-            node_index: None,
-            state,
-            kind,
-            inbox: MessagesFromFactors::new(),
-            message_count: MessageCount::default(),
-        }
-    }
-
-    pub fn new_dynamic_factor(
-        strength: Float,
-        measurement: Vector<Float>,
-        dofs: NonZeroUsize,
-        delta_t: Float,
-    ) -> Self {
-        let mut state = FactorState::new(measurement, strength, dofs, 2); // Dynamic factors have 2 neighbors
-        let dynamic_factor = DynamicFactor::new(&mut state, delta_t);
-        let kind = FactorKind::Dynamic(dynamic_factor);
-        Self::new(state, kind)
-    }
-
-    // TODO: need to store the id of the variable in the other robots factorgraph,
-    // so we can visualize it with graphviz
-    pub fn new_interrobot_factor(
-        strength: Float,
-        measurement: Vector<Float>,
-        dofs: NonZeroUsize,
-        safety_radius: StrictlyPositiveFinite<Float>,
-        connection: InterRobotConnection,
-    ) -> Result<Self, InterRobotFactorError> {
-        let state = FactorState::new(measurement, strength, dofs, 2); // Interrobot factors have 2 neighbors
-        let interrobot_factor = InterRobotFactor::new(safety_radius.get(), connection)?;
-        let kind = FactorKind::InterRobot(interrobot_factor);
-
-        Ok(Self::new(state, kind))
-    }
-
-    pub fn new_pose_factor() -> Self {
-        todo!()
-    }
-
-    pub fn new_obstacle_factor(
-        strength: Float,
-        measurement: Vector<Float>,
-        dofs: NonZeroUsize,
-        obstacle_sdf: &'static Image,
-        world_size: Float,
-    ) -> Self {
-        let state = FactorState::new(measurement, strength, dofs, 1); // Obstacle factors have 1 neighbor
-                                                                      // let obstacle_factor = ObstacleFactor::new(obstacle_sdf, world_size);
-        let obstacle_factor = ObstacleFactor::new(obstacle_sdf, world_size);
-        let kind = FactorKind::Obstacle(obstacle_factor);
-        Self::new(state, kind)
-    }
-
-    #[inline(always)]
-    pub fn variant(&self) -> &'static str {
-        self.kind.name()
-    }
-
-    #[inline(always)]
-    fn jacobian(&mut self, x: &Vector<Float>) -> Matrix<Float> {
-        self.kind.jacobian(&self.state, x)
-    }
-
-    fn measure(&mut self, x: &Vector<Float>) -> Vector<Float> {
-        self.state.cached_measurement = self.kind.measure(&self.state, x);
-        self.state.cached_measurement.clone()
-    }
-
-    #[inline(always)]
-    fn skip(&mut self) -> bool {
-        self.kind.skip(&self.state)
-    }
-
-    pub fn set_node_index(&mut self, node_index: NodeIndex) {
-        if self.node_index.is_some() {
-            panic!("The node index is already set");
-        }
-        self.node_index = Some(node_index);
-    }
-
-    pub fn get_node_index(&self) -> NodeIndex {
-        if self.node_index.is_none() {
-            panic!("The node index has not been set");
-        }
-        self.node_index.expect("I checked it was there 3 lines ago")
-    }
-
-    pub fn receive_message_from(&mut self, from: VariableId, message: Message) {
-        if message.is_empty() {
-            // warn!("received an empty message from {:?}", from);
-        }
-        let _ = self.inbox.insert(from, message);
-        self.message_count.received += 1;
-    }
-
-    #[inline(always)]
-    pub fn read_message_from(&mut self, from: VariableId) -> Option<&Message> {
-        self.inbox.get(&from)
-    }
-
-    fn residual(&self) -> Vector<Float> {
-        &self.state.initial_measurement - &self.state.cached_measurement
-    }
-
-    #[must_use]
-    pub fn update(&mut self) -> MessagesToVariables {
-        let dofs = 4;
-        debug_assert_eq!(
-            self.state.linearisation_point.len(),
-            dofs * self.inbox.len()
-        );
-        let zero_mean = Vector::<Float>::zeros(dofs);
-        for (i, (_, message)) in self.inbox.iter().enumerate() {
-            let mean = message.mean().unwrap_or(&zero_mean);
-            self.state
-                .linearisation_point
-                .slice_mut(s![i * dofs..(i + 1) * dofs])
-                .assign(mean);
-        }
-
-        if self.skip() {
-            let messages = self
-                .inbox
-                .iter()
-                .map(|(variable_id, _)| (*variable_id, Message::empty(dofs)))
-                .collect::<BTreeMap<_, _>>();
-            self.message_count.sent += messages.len();
-            return messages;
-        }
-
-        let meas = self.measure(&self.state.linearisation_point.clone());
-        let jacobian = self.jacobian(&self.state.linearisation_point.clone());
-
-        let factor_lambda_potential = jacobian
-            .t()
-            .dot(&self.state.measurement_precision)
-            .dot(&jacobian);
-        let factor_eta_potential = jacobian
-            .t()
-            .dot(&self.state.measurement_precision)
-            .dot(&(jacobian.dot(&self.state.linearisation_point) + self.residual()));
-
-        // if matches!(self.kind, FactorKind::Dynamic(_)) {
-        //     // let subtitle = format!("Factor update: {}", self.kind.name());
-        //     // include name and index of the factor
-        //     let subtitle = format!(
-        //         "Factor update: {}({}) {}{}{}",
-        //         YELLOW,
-        //         self.node_index.unwrap().index(),
-        //         BLUE,
-        //         self.kind.name(),
-        //         RESET
-        //     );
-        //     pretty_print_subtitle!(subtitle);
-        //     pretty_print_vector!(&self.state.linearisation_point);
-        //     pretty_print_vector!(&meas);
-        //     pretty_print_vector!(&self.residual());
-        // }
-        // pretty_print_vector!(&factor_eta_potential);
-
-        self.state.initialized = true;
-
-        let mut marginalisation_idx = 0;
-        let mut messages = MessagesToVariables::new();
-
-        let zero_precision = Matrix::<Float>::zeros((dofs, dofs));
-
-        // let color_code = match self.kind {
-        //     FactorKind::Pose(_) => MAGENTA,
-        //     FactorKind::InterRobot(_) => GREEN,
-        //     FactorKind::Dynamic(_) => BLUE,
-        //     FactorKind::Obstacle(_) => RED,
-        // };
-        // println!(
-        //     "{}{}{} UPDATE",
-        //     color_code,
-        //     self.node_index.unwrap().index(),
-        //     RESET
-        // );
-        for variable_id in self.inbox.keys() {
-            let mut factor_eta = factor_eta_potential.clone();
-            let mut factor_lambda = factor_lambda_potential.clone();
-
-            for (j, (other_variable_id, other_message)) in self.inbox.iter().enumerate() {
-                // Do not aggregate data from the variable we're sending to
-                if other_variable_id == variable_id {
-                    continue;
-                }
-
-                let message_eta = other_message
-                    .information_vector()
-                    .expect("it better be there");
-
-                // println!("{}{}{} eta = ", YELLOW, variable_id.global_id(), RESET);
-                // pretty_print_vector!(message_eta);
-
-                let message_precision = other_message.precision_matrix().unwrap_or(&zero_precision);
-
-                factor_eta
-                    .slice_mut(s![j * dofs..(j + 1) * dofs])
-                    .add_assign(message_eta);
-                factor_lambda
-                    .slice_mut(s![j * dofs..(j + 1) * dofs, j * dofs..(j + 1) * dofs])
-                    .add_assign(message_precision);
-            }
-
-            // pretty_print_vector!(&factor_eta);
-
-            let message =
-                marginalise_factor_distance(factor_eta, factor_lambda, dofs, marginalisation_idx)
-                    .expect("marginalise_factor_distance should not fail");
-            messages.insert(*variable_id, message);
-            marginalisation_idx += dofs;
-        }
-
-        self.message_count.sent += messages.len();
-        messages
-    }
-}
-
-impl FactorGraphNode for Factor {
-    fn remove_connection_to(
-        &mut self,
-        factorgraph_id: super::factorgraph::FactorGraphId,
-    ) -> Result<(), super::factorgraph::RemoveConnectionToError> {
-        let connections_before = self.inbox.len();
-        self.inbox
-            .retain(|variable_id, v| variable_id.factorgraph_id != factorgraph_id);
-        let connections_after = self.inbox.len();
-
-        let no_connections_removed = connections_before == connections_after;
-        if no_connections_removed {
-            Err(super::factorgraph::RemoveConnectionToError)
-        } else {
-            Ok(())
-        }
-    }
-
-    #[inline]
-    fn messages_sent(&self) -> usize {
-        self.message_count.sent
-    }
-
-    #[inline]
-    fn messages_received(&self) -> usize {
-        self.message_count.received
-    }
-
-    #[inline(always)]
-    fn reset_message_count(&mut self) {
-        self.message_count.reset();
-    }
-}
diff --git a/crates/gbpplanner-rs/src/planner/message.rs b/crates/gbpplanner-rs/src/planner/message.rs
deleted file mode 100644
index 04540f49..00000000
--- a/crates/gbpplanner-rs/src/planner/message.rs
+++ /dev/null
@@ -1,130 +0,0 @@
-#![deny(missing_docs)]
-//!
-
-use gbp_linalg::prelude::*;
-
-#[derive(Debug, Clone)]
-pub struct Payload {
-    pub eta: Vector<Float>,
-    pub lam: Matrix<Float>,
-    pub mu:  Vector<Float>,
-}
-
-pub struct Eta(pub Vector<Float>);
-pub struct Lam(pub Matrix<Float>);
-pub struct Mu(pub Vector<Float>);
-
-#[derive(Debug, Clone)]
-pub struct Message {
-    payload: Option<Payload>,
-    dofs:    usize,
-}
-
-impl Message {
-    pub fn mean(&self) -> Option<&Vector<Float>> {
-        // self.payload.as_ref().map(|gaussian| gaussian.mean())
-        self.payload.as_ref().map(|payload| &payload.mu)
-    }
-
-    pub fn precision_matrix(&self) -> Option<&Matrix<Float>> {
-        self.payload
-            .as_ref()
-            // .map(|gaussian| gaussian.precision_matrix())
-            .map(|payload| &payload.lam)
-    }
-
-    pub fn information_vector(&self) -> Option<&Vector<Float>> {
-        self.payload
-            .as_ref()
-            // .map(|gaussian| gaussian.information_vector())
-            .map(|payload| &payload.eta)
-    }
-
-    // pub fn mean(&self) -> Vector<Float> {
-    //     match self {
-    //         Self::Content { gaussian } => gaussian.mean().clone(),
-    //         Self::Empty(dofs) => Vector::<Float>::zeros(*dofs),
-    //     }
-    // }
-
-    // pub fn precision_matrix(&self) -> Matrix<Float> {
-    //     match self {
-    //         Self::Content { gaussian } => gaussian.precision_matrix().clone(),
-    //         Self::Empty(dofs) => Matrix::<Float>::zeros((*dofs, *dofs)),
-    //     }
-    // }
-
-    // pub fn information_vector(&self) -> Vector<Float> {
-    //     match self {
-    //         Self::Content { gaussian } => gaussian.information_vector().clone(),
-    //         Self::Empty(dofs) => Vector::<Float>::zeros(*dofs),
-    //     }
-    // }
-
-    // /// Returns `true` if the message is [`Content`].
-    // ///
-    // /// [`Content`]: Message::Content
-    // #[must_use]
-    // pub fn is_content(&self) -> bool {
-    //     matches!(self, Self::Content { .. })
-    // }
-
-    // /// Returns `true` if the message is [`Empty`].
-    // ///
-    // /// [`Empty`]: Message::Empty
-    // #[must_use]
-    // pub fn is_empty(&self) -> bool {
-    //     matches!(self, Self::Empty(..))
-    // }
-
-    #[inline]
-    pub fn is_empty(&self) -> bool {
-        self.payload.is_none()
-    }
-
-    #[inline]
-    pub fn len(&self) -> usize {
-        self.dofs
-    }
-
-    /// Take the inner `MultivariateNormal` from the message.
-    /// Leaving the message in an empty state.
-    #[inline]
-    pub fn take(&mut self) -> Option<Payload> {
-        self.payload.take()
-    }
-
-    #[inline]
-    pub fn payload(&self) -> Option<&Payload> {
-        self.payload.as_ref()
-    }
-
-    /// Create an empty message
-    pub fn empty(dofs: usize) -> Self {
-        // Self {
-        //     payload: None,
-        //     dofs,
-        // }
-
-        Self {
-            payload: Some(Payload {
-                eta: Vector::<Float>::zeros(dofs),
-                lam: Matrix::<Float>::zeros((dofs, dofs)),
-                mu:  Vector::<Float>::zeros(dofs),
-            }),
-            dofs,
-        }
-    }
-
-    pub fn new(eta: Eta, lam: Lam, mu: Mu) -> Self {
-        let dofs = eta.0.len();
-        Self {
-            payload: Some(Payload {
-                eta: eta.0,
-                lam: lam.0,
-                mu:  mu.0,
-            }),
-            dofs,
-        }
-    }
-}
diff --git a/crates/gbpplanner-rs/src/planner/mod.rs b/crates/gbpplanner-rs/src/planner/mod.rs
index 9d910983..76c818af 100644
--- a/crates/gbpplanner-rs/src/planner/mod.rs
+++ b/crates/gbpplanner-rs/src/planner/mod.rs
@@ -1,121 +1,31 @@
-mod debug;
-mod factor;
-mod factorgraph;
-mod marginalise_factor_distance;
-mod message;
+// mod debug;
+// mod factor;
+// mod factorgraph;
+// mod marginalise_factor_distance;
+// mod message;
 pub mod robot;
 mod spawner;
-mod variable;
+// mod variable;
 mod visualiser;
 
 use bevy::prelude::*;
-pub use factorgraph::{graphviz::NodeKind, FactorGraph, NodeIndex};
+// pub use factorgraph::{graphviz::NodeKind, FactorGraph, NodeIndex};
 pub use robot::{RobotId, RobotState};
 pub use visualiser::{
     factorgraphs::VariableVisualiser, waypoints::WaypointVisualiser, RobotTracker,
 };
 
-use self::{
-    debug::PlannerDebugPlugin, robot::RobotPlugin, spawner::SpawnerPlugin,
-    visualiser::VisualiserPlugin,
-};
+use self::{robot::RobotPlugin, spawner::SpawnerPlugin, visualiser::VisualiserPlugin};
 
 pub struct PlannerPlugin;
 
 impl Plugin for PlannerPlugin {
     fn build(&self, app: &mut App) {
-        app.insert_state(PausedState::Running)
-            .add_event::<PausePlayEvent>()
-            .add_systems(PreUpdate, pause_play_simulation)
-            .add_plugins((
-                RobotPlugin,
-                SpawnerPlugin,
-                VisualiserPlugin,
-                PlannerDebugPlugin,
-            ));
-    }
-}
-
-// TODO(kpbaks): move out of planner
-
-// /// **Bevy** [`Resource`] for pausing or playing the simulation
-// #[derive(Default, Resource)]
-// pub struct PausePlay(bool);
-
-#[derive(States, Default, Debug, Clone, PartialEq, Eq, Hash)]
-pub enum PausedState {
-    #[default]
-    Running,
-    Paused,
-}
-
-impl PausedState {
-    /// Returns `true` if the paused state is [`Running`].
-    ///
-    /// [`Running`]: PausedState::Running
-    #[must_use]
-    pub fn is_running(&self) -> bool {
-        matches!(self, Self::Running)
-    }
-
-    /// Returns `true` if the paused state is [`Paused`].
-    ///
-    /// [`Paused`]: PausedState::Paused
-    #[must_use]
-    pub fn is_paused(&self) -> bool {
-        matches!(self, Self::Paused)
-    }
-}
-
-// impl PausePlay {
-//     pub fn pause(&mut self) {
-//         self.0 = false;
-//     }
-
-//     pub fn play(&mut self) {
-//         self.0 = true;
-//     }
-
-//     pub fn toggle(&mut self) {
-//         self.0 = !self.0;
-//     }
-
-//     pub fn is_paused(&self) -> bool {
-//         !self.0
-//     }
-// }
-
-#[allow(dead_code)]
-#[derive(Event, Clone, Debug, Default)]
-pub enum PausePlayEvent {
-    #[default]
-    Toggle,
-    Pause,
-    Play,
-}
-
-fn pause_play_simulation(
-    state: ResMut<State<PausedState>>,
-    mut next_state: ResMut<NextState<PausedState>>,
-    mut pause_play_event_reader: EventReader<PausePlayEvent>,
-    mut time: ResMut<Time<Virtual>>,
-) {
-    for pause_play_event in pause_play_event_reader.read() {
-        info!("received event: {:?}", pause_play_event);
-        match (pause_play_event, state.get()) {
-            (PausePlayEvent::Pause, PausedState::Paused)
-            | (PausePlayEvent::Play, PausedState::Running) => {
-                warn!("ignoring duplicate event: {:?}", pause_play_event);
-                continue;
-            }
-            (PausePlayEvent::Pause | PausePlayEvent::Toggle, PausedState::Running) => {
-                next_state.set(PausedState::Paused);
-                time.pause();
-            }
-            (PausePlayEvent::Play | PausePlayEvent::Toggle, PausedState::Paused) => {
-                next_state.set(PausedState::Running);
-                time.unpause();
-            }
-        };
+        app.add_plugins((
+            RobotPlugin,
+            SpawnerPlugin,
+            VisualiserPlugin,
+            // PlannerDebugPlugin,
+        ));
     }
 }
diff --git a/crates/gbpplanner-rs/src/planner/robot.rs b/crates/gbpplanner-rs/src/planner/robot.rs
index 1b39c13c..a990ab8f 100644
--- a/crates/gbpplanner-rs/src/planner/robot.rs
+++ b/crates/gbpplanner-rs/src/planner/robot.rs
@@ -8,122 +8,38 @@ use bevy::{
 use gbp_linalg::{prelude::*, pretty_print_matrix, pretty_print_vector};
 use ndarray::{array, concatenate, s, Axis};
 
-use super::{
-    factor::{Factor, InterRobotConnection},
-    factorgraph::{FactorGraph, FactorId, VariableId},
-    variable::Variable,
-    NodeIndex, PausePlayEvent,
+// use super::{
+//     factor::{Factor, InterRobotConnection},
+//     factorgraph::{FactorGraph, FactorId, VariableId},
+//     variable::Variable,
+//     NodeIndex,
+// };
+use crate::factorgraph::{
+    factor::{ExternalVariableId, Factor},
+    factorgraph::{FactorGraph, NodeIndex, VariableIndex},
+    id::{FactorId, VariableId},
 };
 use crate::{
-    boolean_bevy_resource, config::Config, pretty_print_subtitle, pretty_print_title,
+    boolean_bevy_resource,
+    config::Config,
+    factorgraph::{factor, variable::Variable, DOFS},
+    pause_play::PausePlay,
+    pretty_print_subtitle, pretty_print_title,
     utils::get_variable_timesteps,
 };
 
-pub struct RobotPlugin;
-
-boolean_bevy_resource!(ManualMode, default = false);
-
-#[derive(States, Default, Debug, Clone, PartialEq, Eq, Hash)]
-pub enum ManualModeState {
-    #[default]
-    Disabled,
-    Enabled {
-        iterations_remaining: usize,
-    },
-}
-
-impl ManualModeState {
-    pub fn enabled(state: Res<State<ManualModeState>>) -> bool {
-        matches!(state.get(), Self::Enabled { .. })
-    }
-
-    pub fn disabled(state: Res<State<ManualModeState>>) -> bool {
-        matches!(state.get(), Self::Disabled)
-    }
-}
-
-// fn keyboard_input_is<M>(key_code: KeyCode, state: ButtonState) -> impl
-// Condition<M> {     move |mut keyboard_input_events:
-// EventReader<KeyboardInput>| {         for event in
-// keyboard_input_events.read() {             if (event.key_code, event.state)
-// == (key_code, state) {                 return true;
-//             }
-//         }
-//         false;
-//     }
-// }
-
-fn start_manual_step(
-    // mut mode: ResMut<ManualMode>,
-    state: Res<State<ManualModeState>>,
-    mut next_state: ResMut<NextState<ManualModeState>>,
-    mut keyboard_input_events: EventReader<KeyboardInput>,
-    mut pause_play_event: EventWriter<PausePlayEvent>,
-    config: Res<Config>,
-) {
-    for event in keyboard_input_events.read() {
-        let (KeyCode::KeyM, ButtonState::Pressed) = (event.key_code, event.state) else {
-            continue;
-        };
-
-        match state.get() {
-            ManualModeState::Disabled => {
-                next_state.set(ManualModeState::Enabled {
-                    iterations_remaining: config.manual.timesteps_per_step.into(),
-                });
-                pause_play_event.send(PausePlayEvent::Play);
-            }
-            ManualModeState::Enabled { .. } => {
-                warn!("manual step already in progress");
-            }
-        }
-
-        // mode.enable();
-        // pause_play_event.send(PausePlayEvent::Play);
-        // debug!("starting manual step");
-    }
-}
-
-fn finish_manual_step(
-    // mut mode: ResMut<ManualMode>,
-    state: Res<State<ManualModeState>>,
-    mut next_state: ResMut<NextState<ManualModeState>>,
-    mut pause_play_event: EventWriter<PausePlayEvent>,
-) {
-    match state.get() {
-        ManualModeState::Enabled {
-            iterations_remaining,
-        } if (0..=1).contains(iterations_remaining) => {
-            next_state.set(ManualModeState::Disabled);
-            pause_play_event.send(PausePlayEvent::Pause);
-        }
-        ManualModeState::Enabled {
-            iterations_remaining,
-        } => {
-            next_state.set(ManualModeState::Enabled {
-                iterations_remaining: iterations_remaining - 1,
-            });
-        }
-        ManualModeState::Disabled => {
-            error!("manual step not in progress");
-        }
-    };
-
-    // mode.disable();
-    // pause_play_event.send(PausePlayEvent::Pause);
-}
+pub type RobotId = Entity;
 
-#[derive(SystemSet, Debug, Clone, PartialEq, Eq, Hash)]
-pub struct GbpSet;
+pub struct RobotPlugin;
 
 impl Plugin for RobotPlugin {
     fn build(&self, app: &mut App) {
         app.init_resource::<VariableTimesteps>()
             .init_resource::<ManualMode>()
             .insert_state(ManualModeState::Disabled)
-            .add_event::<SpawnRobotEvent>()
-            .add_event::<DespawnRobotEvent>()
-            .add_event::<RobotReachedWaypointEvent>()
+            .add_event::<RobotSpawned>()
+            .add_event::<RobotDespawned>()
+            .add_event::<RobotReachedWaypoint>()
             .add_systems(PreUpdate, start_manual_step.run_if(time_is_paused))
             .add_systems(
                 FixedUpdate,
@@ -143,55 +59,58 @@ impl Plugin for RobotPlugin {
                 )
                     .chain()
                     .run_if(not(time_is_paused)),
-            )
-            .configure_sets(Update, GbpSet.run_if(time_changed));
+            );
+        // .configure_sets(Update, GbpSet.run_if(time_changed));
     }
 }
 
-fn time_changed(time: Res<Time<Fixed>>) -> bool {
-    // error!("time changed: {}", time.delta_seconds());
-    time.delta_seconds() > 0.
-    // time.is_changed()
-}
+// fn time_changed(time: Res<Time<Fixed>>) -> bool {
+//     // error!("time changed: {}", time.delta_seconds());
+//     time.delta_seconds() > 0.
+//     // time.is_changed()
+// }
 
-fn time_is_not_paused_and_manual_mode_is_disabled(
-    time: Res<Time<Virtual>>,
-    mode: Res<ManualMode>,
-) -> bool {
-    !time.is_paused() && ManualMode::disabled(mode)
-}
+// fn time_is_not_paused_and_manual_mode_is_disabled(
+//     time: Res<Time<Virtual>>,
+//     mode: Res<ManualMode>,
+// ) -> bool {
+//     !time.is_paused() && ManualMode::disabled(mode)
+// }
 
-fn manual_mode(mode: Res<ManualMode>) -> bool {
-    mode.0
-}
+// fn manual_mode(mode: Res<ManualMode>) -> bool {
+//     mode.0
+// }
+//
 
 /// run criteria if time is not paused
+#[inline]
 fn time_is_paused(time: Res<Time<Virtual>>) -> bool {
     time.is_paused()
 }
 
-pub type RobotId = Entity;
+/// Event emitted when a robot is spawned
+#[derive(Debug, Event)]
+pub struct RobotSpawned(pub RobotId);
 
-#[derive(Event)]
-pub struct DespawnRobotEvent(pub RobotId);
-
-#[derive(Event)]
-pub struct SpawnRobotEvent(pub RobotId);
+/// Event emitted when a robot is despawned
+#[derive(Debug, Event)]
+pub struct RobotDespawned(pub RobotId);
 
+/// Event emitted when a robot reaches a waypoint
 #[derive(Event)]
-pub struct RobotReachedWaypointEvent {
+pub struct RobotReachedWaypoint {
     pub robot_id:       RobotId,
     pub waypoint_index: usize,
-    // pub waypoint_id: Entity,
 }
 
 fn despawn_robots(
     mut commands: Commands,
+    // TODO: change query
     mut query: Query<(Entity, &mut FactorGraph, &mut RobotState)>,
-    mut despawn_robot_event: EventReader<DespawnRobotEvent>,
+    mut despawn_robot_event: EventReader<RobotDespawned>,
 ) {
-    for DespawnRobotEvent(robot_id) in despawn_robot_event.read() {
-        for (entity, mut factorgraph, mut robotstate) in query.iter_mut() {
+    for RobotDespawned(robot_id) in despawn_robot_event.read() {
+        for (_, mut factorgraph, _) in query.iter_mut() {
             let _ = factorgraph.remove_connection_to(*robot_id);
         }
 
@@ -208,13 +127,32 @@ fn despawn_robots(
     }
 }
 
-// /// Sigma for Unary pose factor on current and horizon states
-// /// from **gbpplanner** `Globals.h`
-// const SIGMA_POSE_FIXED: f32 = 1e-6;
-
+/// Resource that stores the horizon timesteps sequence
 #[derive(Resource, Debug)]
 pub struct VariableTimesteps {
-    pub timesteps: Vec<u32>,
+    timesteps: Vec<u32>,
+}
+
+impl VariableTimesteps {
+    /// Returns the number of timesteps
+    #[inline(always)]
+    pub fn len(&self) -> usize {
+        self.timesteps.len()
+    }
+
+    /// Extracts a slice containing the entire vector.
+    #[inline(always)]
+    pub fn as_slice(&self) -> &[u32] {
+        self.timesteps.as_slice()
+    }
+}
+
+impl std::ops::Index<usize> for VariableTimesteps {
+    type Output = u32;
+
+    fn index(&self, index: usize) -> &Self::Output {
+        &self.timesteps[index]
+    }
 }
 
 impl FromWorld for VariableTimesteps {
@@ -234,7 +172,6 @@ impl FromWorld for VariableTimesteps {
 }
 
 #[derive(Debug, thiserror::Error)]
-
 pub enum RobotInitError {
     #[error("No waypoints were provided")]
     NoWaypoints,
@@ -242,19 +179,16 @@ pub enum RobotInitError {
     NoVariableTimesteps,
 }
 
-/// A component to encapsulate the idea of a radius.
+/// Component for entities with a radius, used for robots
 #[derive(Component, Debug)]
-
 pub struct Radius(pub f32);
 
-/// A waypoint is a position that the robot should move to.
+/// A waypoint is a position and velocity that the robot should move to and
+/// achieve.
 #[allow(clippy::similar_names)]
 #[derive(Component, Debug)]
-
 pub struct Waypoints(pub VecDeque<Vec4>);
 
-// pub struct Waypoints(pub VecDeque<Vec2>);
-
 /// A robot's state, consisting of other robots within communication range,
 /// and other robots that are connected via inter-robot factors.
 #[derive(Component, Debug)]
@@ -272,6 +206,7 @@ pub struct RobotState {
 }
 
 impl RobotState {
+    /// Create a new `RobotState`
     #[must_use]
     pub fn new() -> Self {
         Self {
@@ -294,17 +229,10 @@ pub struct RobotBundle {
     pub radius:      Radius,
     /// The current state of the robot
     pub state:       RobotState,
-    // pub transform: Transform,
     /// Waypoints used to instruct the robot to move to a specific position.
     /// A VecDeque is used to allow for efficient pop_front operations, and
     /// push_back operations.
     pub waypoints:   Waypoints,
-    // NOTE: Using the **Bevy** entity id as the robot id
-    // pub id: RobotId,
-    // NOTE: These are accessible as **Bevy** resources
-    // obstacle_sdf: Option<Rc<image::RgbImage>>,
-    // settings: &'a RobotSettings,
-    // id_generator: Rc<RefCell<IdGenerator>>,
 }
 
 impl RobotBundle {
@@ -342,8 +270,6 @@ impl RobotBundle {
                 (config.robot.planning_horizon * config.robot.max_speed).get(),
             ) * start2goal.normalize();
 
-        let ndofs = 4; // [x, y, x', y']
-
         let mut factorgraph = FactorGraph::new(robot_id);
         let last_variable_timestep = *variable_timesteps
             .last()
@@ -371,8 +297,7 @@ impl RobotBundle {
                 Float::INFINITY
             };
 
-            let precision_matrix = Matrix::<Float>::from_diag_elem(ndofs, sigma);
-            // pretty_print_matrix!(&precision_matrix);
+            let precision_matrix = Matrix::<Float>::from_diag_elem(DOFS, sigma);
 
             let mean = array![
                 mean.x as Float,
@@ -380,15 +305,12 @@ impl RobotBundle {
                 mean.z as Float,
                 mean.w as Float
             ];
-            // pretty_print_vector!(&mean);
 
-            let variable = Variable::new(mean, precision_matrix, ndofs);
+            let variable = Variable::new(mean, precision_matrix, DOFS);
             let variable_index = factorgraph.add_variable(variable);
             variable_node_indices.push(variable_index);
         }
 
-        // std::process::exit(1);
-
         // Create Dynamics factors between variables
         for i in 0..variable_timesteps.len() - 1 {
             // T0 is the timestep between the current state and the first planned state.
@@ -400,12 +322,11 @@ impl RobotBundle {
             let dynamic_factor = Factor::new_dynamic_factor(
                 config.gbp.sigma_factor_dynamics as Float,
                 measurement,
-                config.robot.dofs,
                 delta_t as Float,
             );
 
             let factor_node_index = factorgraph.add_factor(dynamic_factor);
-            let factor_id = FactorId::new_dynamic(factorgraph.id(), factor_node_index);
+            let factor_id = FactorId::new(factorgraph.id(), factor_node_index);
             // A dynamic factor connects two variables
             let _ = factorgraph.add_internal_edge(
                 VariableId::new(factorgraph.id(), variable_node_indices[i + 1]),
@@ -416,7 +337,6 @@ impl RobotBundle {
                 factor_id,
             );
         }
-        // std::process::exit(0);
 
         // Create Obstacle factors for all variables excluding start, excluding horizon
         #[allow(clippy::needless_range_loop)]
@@ -424,19 +344,17 @@ impl RobotBundle {
             let obstacle_factor = Factor::new_obstacle_factor(
                 config.gbp.sigma_factor_obstacle as Float,
                 array![0.0],
-                config.robot.dofs,
                 obstacle_sdf,
                 config.simulation.world_size.get() as Float,
             );
 
             let factor_node_index = factorgraph.add_factor(obstacle_factor);
-            let factor_id = FactorId::new_obstacle(factorgraph.id(), factor_node_index);
+            let factor_id = FactorId::new(factorgraph.id(), factor_node_index);
             let _ = factorgraph.add_internal_edge(
                 VariableId::new(factorgraph.id(), variable_node_indices[i]),
                 factor_id,
             );
         }
-        // std::process::exit(0);
 
         Ok(Self {
             factorgraph,
@@ -592,24 +510,26 @@ fn create_interrobot_factors(
                 .expect("the key is in the map");
 
             for i in 1..number_of_variables {
-                let dofs = 4;
-                let z = Vector::<Float>::zeros(dofs);
+                let z = Vector::<Float>::zeros(DOFS);
                 let eps = 0.2 * config.robot.radius.get();
                 let safety_radius = 2.0 * config.robot.radius.get() + eps;
                 // TODO: should it be i - 1 or i?
-                let connection =
-                    InterRobotConnection::new(*other_robot_id, other_variable_indices[i - 1]);
-
+                let external_variable_id = ExternalVariableId::new(
+                    *other_robot_id,
+                    VariableIndex(other_variable_indices[i - 1]),
+                );
+                // let connection =
+                //     InterRobotFactorConnection::new(*other_robot_id, other_variable_indices[i
+                // - 1]);
+                //
                 let interrobot_factor = Factor::new_interrobot_factor(
                     config.gbp.sigma_factor_interrobot as Float,
                     z,
-                    dofs.try_into().expect("dofs > 0"),
                     (safety_radius as Float)
                         .try_into()
                         .expect("safe radius is positive and finite"),
-                    connection,
-                )
-                .expect("safe radius is positive and finite");
+                    external_variable_id,
+                );
 
                 let factor_index = factorgraph.add_factor(interrobot_factor);
 
@@ -617,7 +537,7 @@ fn create_interrobot_factors(
                     .nth_variable_index(i)
                     .expect("there should be an i'th variable");
 
-                let factor_id = FactorId::new_interrobot(robot_id, factor_index);
+                let factor_id = FactorId::new(robot_id, factor_index);
                 let graph_id = factorgraph.id();
                 factorgraph.add_internal_edge(VariableId::new(graph_id, variable_index), factor_id);
                 external_edges_to_add.push((robot_id, factor_index, *other_robot_id, i));
@@ -638,7 +558,7 @@ fn create_interrobot_factors(
             .expect("the other_robot_id should be in the query")
             .1;
 
-        other_factorgraph.add_external_edge(FactorId::new_interrobot(robot_id, factor_index), i);
+        other_factorgraph.add_external_edge(FactorId::new(robot_id, factor_index), i);
 
         let (nth_variable_index, nth_variable) = other_factorgraph
             .nth_variable(i)
@@ -657,13 +577,14 @@ fn create_interrobot_factors(
             .expect("the robot_id should be in the query")
             .1;
 
-        factorgraph
-            .factor_mut(factor_index)
-            .receive_message_from(variable_id, variable_message);
-        // info!(
-        //     "sent initial message from {:?} to {:?}",
-        //     robot_id, variable_id
-        // );
+        if let Some(factor) = factorgraph.get_factor_mut(factor_index) {
+            factor.receive_message_from(variable_id, variable_message.clone());
+        } else {
+            error!(
+                "factorgraph {:?} has no factor with index {:?}",
+                robot_id, factor_index
+            );
+        }
     }
 }
 
@@ -699,14 +620,15 @@ fn iterate_gbp(
                 .find(|(id, _)| *id == message.to.factorgraph_id)
                 .expect("the factorgraph_id of the receiving variable should exist in the world");
 
-            // info!(
-            //     "Ext message factor {:?}\t->\tvariable {:?}",
-            //     message.from, message.to
-            // );
-
-            external_factorgraph
-                .variable_mut(message.to.variable_index)
-                .receive_message_from(message.from, message.message.clone());
+            if let Some(variable) = external_factorgraph.get_variable_mut(message.to.variable_index)
+            {
+                variable.receive_message_from(message.from, message.message.clone());
+            } else {
+                error!(
+                    "variablegraph {:?} has no variable with index {:?}",
+                    message.to.factorgraph_id, message.to.variable_index
+                );
+            }
         }
 
         // ╭────────────────────────────────────────────────────────────────────────────────────────
@@ -723,14 +645,13 @@ fn iterate_gbp(
                 .find(|(id, _)| *id == message.to.factorgraph_id)
                 .expect("the factorgraph_id of the receiving factor should exist in the world");
 
-            // info!(
-            //     "Ext message variable {:?}\t->\tfactor {:?}",
-            //     message.from, message.to
-            // );
+            if let Some(factor) = external_factorgraph.get_factor_mut(message.to.factor_index) {
+                factor.receive_message_from(message.from, message.message.clone());
+            }
 
-            external_factorgraph
-                .factor_mut(message.to.factor_index)
-                .receive_message_from(message.from, message.message.clone());
+            // external_factorgraph
+            //     .factor_mut(message.to.factor_index)
+            //     .receive_message_from(message.from, message.message.clone());
         }
     }
 }
@@ -740,8 +661,8 @@ fn update_prior_of_horizon_state(
     mut query: Query<(Entity, &mut FactorGraph, &mut Waypoints), With<RobotState>>,
     config: Res<Config>,
     time: Res<Time>,
-    mut despawn_robot_event: EventWriter<DespawnRobotEvent>,
-    mut robot_reached_waypoint_event: EventWriter<RobotReachedWaypointEvent>,
+    mut despawn_robot_event: EventWriter<RobotDespawned>,
+    mut robot_reached_waypoint_event: EventWriter<RobotReachedWaypoint>,
 ) {
     let delta_t = time.delta_seconds();
 
@@ -766,10 +687,10 @@ fn update_prior_of_horizon_state(
                 .last_variable()
                 .expect("factorgraph has a horizon variable");
 
-            debug_assert_eq!(horizon_variable.belief.mu.len(), 4);
+            debug_assert_eq!(horizon_variable.belief.mean.len(), 4);
 
-            let estimated_position = horizon_variable.belief.mu.slice(s![..2]); // the mean is a 4x1 vector with [x, y, x', y']
-                                                                                // dbg!(&estimated_position);
+            let estimated_position = horizon_variable.belief.mean.slice(s![..2]); // the mean is a 4x1 vector with [x, y, x', y']
+                                                                                  // dbg!(&estimated_position);
 
             let horizon2goal_dir = current_waypoint - estimated_position;
 
@@ -806,7 +727,7 @@ fn update_prior_of_horizon_state(
             .last_variable_mut()
             .expect("factorgraph has a horizon variable");
 
-        horizon_variable.belief.mu.clone_from(&new_mean);
+        horizon_variable.belief.mean.clone_from(&new_mean);
 
         let messages_to_external_factors =
             factorgraph.change_prior_of_variable(variable_index, new_mean);
@@ -822,7 +743,7 @@ fn update_prior_of_horizon_state(
             // info!("robot {:?}, has reached its waypoint", robot_id);
 
             waypoints.0.pop_front();
-            robot_reached_waypoint_event.send(RobotReachedWaypointEvent {
+            robot_reached_waypoint_event.send(RobotReachedWaypoint {
                 robot_id,
                 waypoint_index: 0,
             });
@@ -836,13 +757,16 @@ fn update_prior_of_horizon_state(
             .find(|(id, _, _)| *id == message.to.factorgraph_id)
             .expect("the factorgraph_id of the receiving factor should exist in the world");
 
-        external_factorgraph
-            .factor_mut(message.to.factor_index)
-            .receive_message_from(message.from, message.message.clone());
+        if let Some(factor) = external_factorgraph.get_factor_mut(message.to.factor_index) {
+            factor.receive_message_from(message.from, message.message.clone());
+        }
+        // external_factorgraph
+        //     .factor_mut(message.to.factor_index)
+        //     .receive_message_from(message.from, message.message.clone());
     }
 
     if !ids_of_robots_to_despawn.is_empty() {
-        despawn_robot_event.send_batch(ids_of_robots_to_despawn.into_iter().map(DespawnRobotEvent));
+        despawn_robot_event.send_batch(ids_of_robots_to_despawn.into_iter().map(RobotDespawned));
     }
 }
 
@@ -861,9 +785,9 @@ fn update_prior_of_current_state(
         let (_, next_variable) = factorgraph
             .nth_variable(1)
             .expect("factorgraph should have a next variable");
-        let mean_of_current_variable = current_variable.belief.mu.clone();
+        let mean_of_current_variable = current_variable.belief.mean.clone();
         let change_in_position =
-            scale as Float * (&next_variable.belief.mu - &mean_of_current_variable);
+            scale as Float * (&next_variable.belief.mean - &mean_of_current_variable);
 
         factorgraph.change_prior_of_variable(
             current_variable_index,
@@ -885,3 +809,83 @@ fn update_prior_of_current_state(
         transform.translation += increment;
     }
 }
+
+boolean_bevy_resource!(ManualMode, default = false);
+
+#[derive(States, Default, Debug, Clone, PartialEq, Eq, Hash)]
+pub enum ManualModeState {
+    #[default]
+    Disabled,
+    Enabled {
+        iterations_remaining: usize,
+    },
+}
+
+impl ManualModeState {
+    pub fn enabled(state: Res<State<ManualModeState>>) -> bool {
+        matches!(state.get(), Self::Enabled { .. })
+    }
+
+    pub fn disabled(state: Res<State<ManualModeState>>) -> bool {
+        matches!(state.get(), Self::Disabled)
+    }
+}
+
+fn start_manual_step(
+    // mut mode: ResMut<ManualMode>,
+    state: Res<State<ManualModeState>>,
+    mut next_state: ResMut<NextState<ManualModeState>>,
+    mut keyboard_input_events: EventReader<KeyboardInput>,
+    mut pause_play_event: EventWriter<PausePlay>,
+    config: Res<Config>,
+) {
+    for event in keyboard_input_events.read() {
+        let (KeyCode::KeyM, ButtonState::Pressed) = (event.key_code, event.state) else {
+            continue;
+        };
+
+        match state.get() {
+            ManualModeState::Disabled => {
+                next_state.set(ManualModeState::Enabled {
+                    iterations_remaining: config.manual.timesteps_per_step.into(),
+                });
+                pause_play_event.send(PausePlay::Play);
+            }
+            ManualModeState::Enabled { .. } => {
+                warn!("manual step already in progress");
+            }
+        }
+    }
+}
+
+fn finish_manual_step(
+    // mut mode: ResMut<ManualMode>,
+    state: Res<State<ManualModeState>>,
+    mut next_state: ResMut<NextState<ManualModeState>>,
+    mut pause_play_event: EventWriter<PausePlay>,
+) {
+    match state.get() {
+        ManualModeState::Enabled {
+            iterations_remaining,
+        } if (0..=1).contains(iterations_remaining) => {
+            next_state.set(ManualModeState::Disabled);
+            pause_play_event.send(PausePlay::Pause);
+        }
+        ManualModeState::Enabled {
+            iterations_remaining,
+        } => {
+            next_state.set(ManualModeState::Enabled {
+                iterations_remaining: iterations_remaining - 1,
+            });
+        }
+        ManualModeState::Disabled => {
+            error!("manual step not in progress");
+        }
+    };
+
+    // mode.disable();
+    // pause_play_event.send(PausePlayEvent::Pause);
+}
+
+#[derive(SystemSet, Debug, Clone, PartialEq, Eq, Hash)]
+pub struct GbpSet;
diff --git a/crates/gbpplanner-rs/src/planner/spawner.rs b/crates/gbpplanner-rs/src/planner/spawner.rs
index 432da4a8..47bea46a 100644
--- a/crates/gbpplanner-rs/src/planner/spawner.rs
+++ b/crates/gbpplanner-rs/src/planner/spawner.rs
@@ -10,8 +10,8 @@ use strum::IntoEnumIterator;
 use typed_floats::StrictlyPositiveFinite;
 
 use super::{
-    robot::{SpawnRobotEvent, VariableTimesteps},
-    PausePlayEvent, RobotId,
+    robot::{RobotSpawned, VariableTimesteps},
+    RobotId,
 };
 use crate::{
     asset_loader::SceneAssets,
@@ -19,6 +19,7 @@ use crate::{
         geometry::{Point, RelativePoint, Shape},
         Config, FormationGroup,
     },
+    pause_play::PausePlay,
     planner::robot::RobotBundle,
     theme::{CatppuccinTheme, ColorAssociation, ColorFromCatppuccinColourExt, DisplayColour},
 };
@@ -183,7 +184,7 @@ fn advance_time(
     time: Res<Time>,
     mut query: Query<&mut FormationSpawnerCountdown>,
     mut spawn_event_writer: EventWriter<FormationSpawnEvent>,
-    mut pause_play_event: EventWriter<PausePlayEvent>,
+    mut pause_play_event: EventWriter<PausePlay>,
     config: Res<Config>,
 ) {
     for mut countdown in query.iter_mut() {
@@ -197,7 +198,7 @@ fn advance_time(
             //     countdown.formation_group_index
             // );
             if config.simulation.pause_on_spawn {
-                pause_play_event.send(PausePlayEvent::Pause);
+                pause_play_event.send(PausePlay::Pause);
             }
         }
     }
@@ -207,7 +208,7 @@ fn advance_time(
 fn spawn_formation(
     mut commands: Commands,
     mut spawn_event_reader: EventReader<FormationSpawnEvent>,
-    mut spawn_robot_event: EventWriter<SpawnRobotEvent>,
+    mut spawn_robot_event: EventWriter<RobotSpawned>,
     mut create_waypoint_event: EventWriter<CreateWaypointEvent>,
     mut materials: ResMut<Assets<StandardMaterial>>,
     config: Res<Config>,
@@ -323,7 +324,7 @@ fn spawn_formation(
                 //     .iter()
                 //     .map(|p| Vec4::new(p.x, p.y, max_speed, 0.0))
                 //     .collect::<VecDeque<_>>(),
-                variable_timesteps.timesteps.as_slice(),
+                variable_timesteps.as_slice(),
                 &config,
                 OBSTACLE_IMAGE
                     .get()
@@ -381,7 +382,7 @@ fn spawn_formation(
                     .with_attached(true),
             ));
 
-            spawn_robot_event.send(SpawnRobotEvent(robot_id));
+            spawn_robot_event.send(RobotSpawned(robot_id));
         }
         // info!("spawning formation group {}", event.formation_group_index);
     }
diff --git a/crates/gbpplanner-rs/src/planner/variable.rs b/crates/gbpplanner-rs/src/planner/variable.rs
deleted file mode 100644
index 9e86b97a..00000000
--- a/crates/gbpplanner-rs/src/planner/variable.rs
+++ /dev/null
@@ -1,405 +0,0 @@
-use bevy::log::{debug, error};
-use gbp_linalg::{pretty_print_matrix, pretty_print_vector, Float, Matrix, Vector};
-use ndarray_inverse::Inverse;
-use tap::Tap;
-
-use super::{
-    factorgraph::{
-        FactorGraphNode, FactorId, MessageCount, MessagesFromVariables, MessagesToFactors,
-        VariableId,
-    },
-    message::{Eta, Lam, Message, Mu},
-};
-use crate::{
-    escape_codes::*,
-    planner::{factorgraph::VariableIndex, NodeIndex},
-    pretty_print_line, pretty_print_message, pretty_print_subtitle, pretty_print_title,
-};
-
-#[derive(Debug, Clone)]
-pub struct VariablePrior {
-    eta:    Vector<Float>,
-    lambda: Matrix<Float>,
-}
-
-impl VariablePrior {
-    fn new(eta: Vector<Float>, lam: Matrix<Float>) -> Self {
-        Self { eta, lambda: lam }
-    }
-}
-
-#[derive(Debug, Clone)]
-pub struct VariableBelief {
-    pub eta:    Vector<Float>,
-    pub lambda: Matrix<Float>,
-    pub mu:     Vector<Float>,
-    pub sigma:  Matrix<Float>,
-    /// Flag to indicate if the variable's covariance is finite, i.e. it does
-    /// not contain NaNs or Infs In gbpplanner it is used to control if a
-    /// variable can be rendered.
-    valid:      bool,
-}
-
-impl VariableBelief {
-    fn new(
-        eta: Vector<Float>,
-        lam: Matrix<Float>,
-        mu: Vector<Float>,
-        sigma: Matrix<Float>,
-        valid: bool,
-    ) -> Self {
-        Self {
-            eta,
-            lambda: lam,
-            mu,
-            sigma,
-            valid,
-        }
-    }
-}
-
-impl From<VariableBelief> for Message {
-    fn from(value: VariableBelief) -> Self {
-        Message::new(Eta(value.eta), Lam(value.lambda), Mu(value.mu))
-    }
-}
-
-/// A variable in the factor graph.
-#[derive(Debug)]
-pub struct Variable {
-    /// Degrees of freedom. For 2D case n_dofs_ = 4 ([x,y,xdot,ydot])
-    pub dofs:   usize,
-    pub prior:  VariablePrior,
-    pub belief: VariableBelief,
-
-    // pub eta_prior: Vector<Float>,
-    // pub lam_prior: Matrix<Float>,
-    // pub eta: Vector<Float>,
-    // pub lam: Matrix<Float>,
-    // pub mu: Vector<Float>,
-    // pub sigma: Matrix<Float>,
-
-    // / Flag to indicate if the variable's covariance is finite, i.e. it does
-    // / not contain NaNs or Infs In gbpplanner it is used to control if a
-    // / variable can be rendered.
-    // pub valid: bool,
-    /// Mailbox for incoming message storage
-    pub inbox: MessagesToFactors,
-
-    /// index
-    pub node_index: Option<NodeIndex>,
-
-    message_count: MessageCount,
-}
-
-impl Variable {
-    /// Returns the variables belief about its position
-    #[inline]
-    pub fn estimated_position(&self) -> [Float; 2] {
-        [self.belief.mu[0], self.belief.mu[1]]
-    }
-
-    /// Returns the variables belief about its velocity
-    #[inline]
-    pub fn estimated_velocity(&self) -> [Float; 2] {
-        [self.belief.mu[2], self.belief.mu[3]]
-    }
-
-    // pub fn new(prior: MultivariateNormal, dofs: usize) -> UninsertedVariable {
-    //     UninsertedVariable { prior, dofs }
-    //     // Self {
-    //     //     node_index: None,
-    //     //     prior: prior.clone(),
-    //     //     belief: prior,
-    //     //     dofs,
-    //     //     inbox: Inbox::new(),
-    //     // }
-    // }
-
-    #[must_use]
-    pub fn new(mu_prior: Vector<Float>, mut lam_prior: Matrix<Float>, dofs: usize) -> Self {
-        // if (!lam_prior_.allFinite()) lam_prior_.setZero();
-        // if !prior.precision_matrix().iter().all(|x| x.is_finite()) {
-        //     prior.precision_matrix().fill(0.0);
-        // }
-        if !lam_prior.iter().all(|x| x.is_finite()) {
-            lam_prior.fill(0.0);
-        }
-
-        // pretty_print_matrix!(&lam_prior);
-
-        let eta_prior = lam_prior.dot(&mu_prior);
-
-        // let subtitle = format!("{}{}{}", RED, "Creating variable", RESET);
-        // pretty_print_subtitle!(subtitle);
-        // pretty_print_vector!(&mu_prior);
-        // pretty_print_matrix!(&lam_prior);
-        // pretty_print_vector!(&eta_prior);
-
-        let sigma = lam_prior
-            .inv()
-            .unwrap_or_else(|| Matrix::<Float>::zeros((dofs, dofs)));
-        let eta = eta_prior.clone();
-        let lam = lam_prior.clone();
-
-        Self {
-            dofs,
-            prior: VariablePrior::new(eta_prior, lam_prior),
-            belief: VariableBelief::new(eta, lam, mu_prior, sigma, false),
-            // eta_prior,
-            // lam_prior,
-            // eta,
-            // lam,
-            // mu: mu_prior,
-            // sigma,
-            // valid: false,
-            inbox: MessagesToFactors::new(),
-            node_index: None,
-            message_count: MessageCount::default(),
-        }
-
-        // Self {
-        //     prior: prior.clone(),
-        //     belief: prior,
-        //     dofs,
-        //     inbox: Inbox::new(),
-        // }
-    }
-
-    pub fn set_node_index(&mut self, index: NodeIndex) {
-        if self.node_index.is_some() {
-            panic!("The node index is already set");
-        }
-        self.node_index = Some(index);
-    }
-
-    // pub fn new(mut mu_prior: Vector<Float>, mut
-
-    // pub fn set_node_index(&mut self, node_index: NodeIndex) {
-    //     match self.node_index {
-    //         Some(_) => panic!("The node index is already set"),
-    //         None => self.node_index = Some(node_index),
-    //     }
-    // }
-    //
-    // pub fn get_node_index(&self) -> NodeIndex {
-    //     match self.node_index {
-    //         Some(node_index) => node_index,
-    //         None => panic!("The node index has not been set"),
-    //     }
-    // }
-
-    pub fn receive_message_from(&mut self, from: FactorId, message: Message) {
-        debug!("variable ? received message from {:?}", from);
-        if message.is_empty() {
-            // warn!("Empty message received from factor {:?}", from);
-        }
-        let _ = self.inbox.insert(from, message);
-        self.message_count.received += 1;
-    }
-
-    // TODO: why never used?
-    pub fn read_message_from(&mut self, from: FactorId) -> Option<&Message> {
-        self.inbox.get(&from)
-    }
-
-    /// Change the prior of the variable.
-    /// It updates the belief of the variable.
-    /// The prior acts as the pose factor
-    /// Called `Variable::change_variable_prior` in **gbpplanner**
-    pub fn change_prior(&mut self, mean: Vector<Float>) -> MessagesFromVariables {
-        // let subtitle = format!("{}{}{}", RED, "Changing prior", RESET);
-        // pretty_print_subtitle!(subtitle);
-        // pretty_print_matrix!(&self.prior.lambda);
-        // pretty_print_vector!(&mean);
-        self.prior.eta = self.prior.lambda.dot(&mean);
-        // pretty_print_vector!(&self.prior.eta);
-        // pretty_print_line!();
-        // self.eta_prior = self.lam_prior.dot(&mean);
-        self.belief.mu = mean;
-        // dbg!(&self.mu);
-
-        // FIXME: forgot this line in the original code
-        // this->belief_ = Message {this->eta_, this->lam_, this->mu_};
-
-        let messages: MessagesFromVariables = self
-            .inbox
-            .keys()
-            .map(|factor_id| (*factor_id, self.prepare_message()))
-            .collect();
-
-        for message in self.inbox.values_mut() {
-            *message = Message::empty(self.dofs);
-        }
-
-        messages
-    }
-
-    pub fn prepare_message(&self) -> Message {
-        Message::new(
-            Eta(self.belief.eta.clone()),
-            Lam(self.belief.lambda.clone()),
-            Mu(self.belief.mu.clone()),
-        )
-    }
-
-    // /****************************************************************************
-    // *******************************/ // Variable belief update step:
-    // // Aggregates all the messages from its connected factors (begins with the
-    // prior, as this is effectively a unary factor) // The valid_ flag is
-    // useful for drawing the variable. // Finally the outgoing messages to
-    // factors is created. /****************************************************
-    // *******************************************************/
-    /// Variable Belief Update step (Step 1 in the GBP algorithm)
-    /// called `Variable::update_belief` in **gbpplanner**
-    pub fn update_belief_and_create_factor_responses(&mut self) -> MessagesFromVariables {
-        // Collect messages from all other factors, begin by "collecting message from
-        // pose factor prior"
-        self.belief.eta = self.prior.eta.clone();
-        self.belief.lambda = self.prior.lambda.clone();
-
-        // let mut title = format!("{}{}{}", YELLOW, "Variable belief BEFORE update:",
-        // RESET); pretty_print_subtitle!(title);
-        // pretty_print_vector!(&self.belief.eta);
-        // pretty_print_matrix!(&self.belief.lambda);
-        // pretty_print_vector!(&self.belief.mu);
-
-        // Go through received messages and update belief
-        for (_, message) in self.inbox.iter() {
-            let Some(payload) = message.payload() else {
-                // empty message
-                // info!("skipping empty message");
-                continue;
-            };
-            self.belief.eta = &self.belief.eta + &payload.eta;
-            self.belief.lambda = &self.belief.lambda + &payload.lam;
-        }
-
-        // Update belief
-        // NOTE: This might not be correct, but it seems the `.inv()` method doesn't
-        // catch and all-zero matrix
-        let lam_not_zero = self.belief.lambda.iter().any(|x| *x - 1e-6 > 0.0);
-        // println!("lam_not_zero: {}", lam_not_zero);
-        if lam_not_zero {
-            if let Some(sigma) = self.belief.lambda.inv() {
-                // pretty_print_matrix!(&sigma);
-                self.belief.sigma = sigma;
-                self.belief.valid = self.belief.sigma.iter().all(|x| x.is_finite());
-                if self.belief.valid {
-                    self.belief.mu = self.belief.sigma.dot(&self.belief.eta);
-                } else {
-                    println!(
-                        "{}:{},Variable covariance is not finite",
-                        file!().split('/').last().unwrap(),
-                        line!()
-                    );
-                }
-            }
-        }
-
-        // let title = format!("{}{}{}", YELLOW, "Variable belief update:", RESET);
-        // pretty_print_subtitle!(title);
-        // pretty_print_vector!(&self.belief.eta);
-        // pretty_print_matrix!(&self.belief.lambda);
-        // pretty_print_vector!(&self.belief.mu);
-        // pretty_print_line!();
-
-        let messages = self
-            .inbox
-            .iter()
-            .map(|(&factor_id, received_message)| {
-                let response = received_message.payload().map_or_else(
-                    || self.prepare_message(),
-                    |message_from_factor| {
-                        // pretty_print_subtitle!("BEFORE FACTOR SUBSTRACTION");
-                        // pretty_print_vector!(&self.belief.eta);
-                        // pretty_print_matrix!(&self.belief.lam);
-                        // pretty_print_vector!(&self.belief.mu);
-                        // pretty_print_line!();
-                        let msg = Message::new(
-                            Eta(&self.belief.eta - &message_from_factor.eta),
-                            Lam(&self.belief.lambda - &message_from_factor.lam),
-                            Mu(&self.belief.mu - &message_from_factor.mu),
-                        );
-                        // pretty_print_subtitle!("AFTER FACTOR SUBSTRACTION");
-                        // pretty_print_vector!(&self.belief.eta);
-                        // pretty_print_matrix!(&self.belief.lam);
-                        // pretty_print_vector!(&self.belief.mu);
-                        msg
-                    },
-                );
-                (factor_id, response)
-            })
-            .collect::<MessagesFromVariables>();
-
-        // messages.iter().for_each(|(factor_id, message)| {
-        //     pretty_print_message!(
-        //         VariableId::new(
-        //             factor_id.get_factor_graph_id(),
-        //             self.node_index.unwrap().into()
-        //         ),
-        //         factor_id,
-        //         ""
-        //     );
-        //     pretty_print_vector!(message.information_vector().unwrap());
-        //     pretty_print_matrix!(message.precision_matrix().unwrap());
-        //     pretty_print_vector!(message.mean().unwrap());
-        // });
-
-        self.message_count.sent += messages.len();
-
-        messages
-
-        // self.inbox
-        //     .iter()
-        //     .map(|(&factor_id, received_message)| {
-        //         let response = Message::new(
-        //             Eta(&self.eta - &received_message.eta),
-        //             Lam(&self.lam - &received_message.lam),
-        //             Mu(&self.mu - &received_message.mu),
-        //         );
-        //         (factor_id, response)
-        //     })
-        //     .collect()
-    }
-
-    /// Returns `true` if the covariance matrix is finite, `false` otherwise.
-    #[inline]
-    pub fn finite_covariance(&self) -> bool {
-        self.belief.valid
-    }
-}
-
-impl FactorGraphNode for Variable {
-    fn remove_connection_to(
-        &mut self,
-        factorgraph_id: super::factorgraph::FactorGraphId,
-    ) -> Result<(), super::factorgraph::RemoveConnectionToError> {
-        let connections_before = self.inbox.len();
-        self.inbox
-            .retain(|factor_id, v| factor_id.factorgraph_id != factorgraph_id);
-        let connections_after = self.inbox.len();
-
-        let no_connections_removed = connections_before == connections_after;
-        if no_connections_removed {
-            Err(super::factorgraph::RemoveConnectionToError)
-        } else {
-            Ok(())
-        }
-    }
-
-    #[inline(always)]
-    fn messages_sent(&self) -> usize {
-        self.message_count.sent
-    }
-
-    #[inline(always)]
-    fn messages_received(&self) -> usize {
-        self.message_count.received
-    }
-
-    #[inline(always)]
-    fn reset_message_count(&mut self) {
-        self.message_count.reset();
-    }
-}
diff --git a/crates/gbpplanner-rs/src/planner/visualiser/factorgraphs.rs b/crates/gbpplanner-rs/src/planner/visualiser/factorgraphs.rs
index e227d82f..c0599e1c 100644
--- a/crates/gbpplanner-rs/src/planner/visualiser/factorgraphs.rs
+++ b/crates/gbpplanner-rs/src/planner/visualiser/factorgraphs.rs
@@ -3,13 +3,13 @@ use bevy_mod_picking::prelude::*;
 use gbp_linalg::{pretty_print_matrix, pretty_print_vector};
 use itertools::Itertools;
 
-use super::{super::FactorGraph, RobotTracker};
+use super::RobotTracker;
 use crate::{
     asset_loader::SceneAssets,
     config::{Config, DrawSetting},
-    // factorgraph,
+    factorgraph::prelude::FactorGraph,
     planner::{
-        robot::{DespawnRobotEvent, SpawnRobotEvent},
+        robot::{RobotDespawned, RobotSpawned},
         RobotState,
     },
     pretty_print_title,
@@ -42,9 +42,9 @@ impl Plugin for FactorGraphVisualiserPlugin {
 fn remove_rendered_factorgraph_when_robot_despawns(
     mut commands: Commands,
     query: Query<(Entity, &RobotTracker)>,
-    mut despawn_robot_event: EventReader<DespawnRobotEvent>,
+    mut despawn_robot_event: EventReader<RobotDespawned>,
 ) {
-    for DespawnRobotEvent(robot_id) in despawn_robot_event.read() {
+    for RobotDespawned(robot_id) in despawn_robot_event.read() {
         // info!(
         //     "received DespawnRobotEvent({:?}), despawning the robots factorgraph
         // visualizer \      entities",
@@ -112,9 +112,9 @@ fn on_variable_clicked(
             return;
         };
 
-        pretty_print_vector!(&variable.belief.eta);
-        pretty_print_matrix!(&variable.belief.lambda);
-        pretty_print_vector!(&variable.belief.mu);
+        pretty_print_vector!(&variable.belief.information_vector);
+        pretty_print_matrix!(&variable.belief.precision_matrix);
+        pretty_print_vector!(&variable.belief.mean);
     }
 }
 
@@ -125,14 +125,14 @@ fn on_variable_clicked(
 /// be ignored
 fn create_factorgraph_visualizer(
     mut commands: Commands,
-    mut spawn_robot_event: EventReader<SpawnRobotEvent>,
+    mut spawn_robot_event: EventReader<RobotSpawned>,
     mut matierals: ResMut<Assets<StandardMaterial>>,
     query: Query<(Entity, &FactorGraph, &ColorAssociation), With<RobotState>>,
     config: Res<Config>,
     scene_assets: Res<SceneAssets>,
     theme: Res<CatppuccinTheme>,
 ) {
-    for SpawnRobotEvent(robot_id) in spawn_robot_event.read() {
+    for RobotSpawned(robot_id) in spawn_robot_event.read() {
         let Some((_, factorgraph, color_association)) =
             query.iter().find(|(entity, _, _)| entity == robot_id)
         else {
@@ -147,7 +147,7 @@ fn create_factorgraph_visualizer(
             let transform = Vec3::new(x as f32, config.visualisation.height.objects, y as f32);
             let robottracker = RobotTracker {
                 robot_id: *robot_id,
-                variable_index: index.into(),
+                variable_index: index.index(),
                 order: i,
             };
 
diff --git a/crates/gbpplanner-rs/src/planner/visualiser/tracer.rs b/crates/gbpplanner-rs/src/planner/visualiser/tracer.rs
index 777567eb..c3e21a41 100644
--- a/crates/gbpplanner-rs/src/planner/visualiser/tracer.rs
+++ b/crates/gbpplanner-rs/src/planner/visualiser/tracer.rs
@@ -10,7 +10,7 @@ const SAMPLE_DELAY: f32 = 0.5;
 use crate::{
     config::Config,
     planner::{
-        robot::{DespawnRobotEvent, SpawnRobotEvent},
+        robot::{RobotDespawned, RobotSpawned},
         RobotId, RobotState,
     },
     robot_spawner::RobotSpawnedEvent,
@@ -47,9 +47,9 @@ pub struct Traces(pub BTreeMap<RobotId, Trace>);
 
 fn remove_trace_of_despawned_robot(
     mut traces: ResMut<Traces>,
-    mut despawn_robot_event: EventReader<DespawnRobotEvent>,
+    mut despawn_robot_event: EventReader<RobotDespawned>,
 ) {
-    for DespawnRobotEvent(robot_id) in despawn_robot_event.read() {
+    for RobotDespawned(robot_id) in despawn_robot_event.read() {
         match traces.0.remove(robot_id) {
             Some(_) => info!("removed trace of robot: {:?}", robot_id),
             None => error!(
@@ -63,27 +63,25 @@ fn remove_trace_of_despawned_robot(
 fn track_initial_robot_positions(
     query: Query<(RobotId, &Transform, &ColorAssociation), With<RobotState>>,
     mut traces: ResMut<Traces>,
-    mut spawn_robot_event: EventReader<SpawnRobotEvent>,
+    mut spawn_robot_event: EventReader<RobotSpawned>,
     theme: Res<CatppuccinTheme>,
 ) {
-    spawn_robot_event
-        .read()
-        .for_each(|SpawnRobotEvent(robot_id)| {
-            for (other_robot_id, transform, color_association) in query.iter() {
-                // initialise the first position of the robot into the ring buffer
-                let mut ring_buffer = StaticRb::default();
-                let _ = ring_buffer.push_overwrite(transform.translation);
-
-                if other_robot_id == *robot_id {
-                    let _ = traces.0.entry(*robot_id).or_insert(Trace {
-                        color: Color::from_catppuccin_colour(
-                            theme.get_display_colour(&color_association.name),
-                        ),
-                        ring_buffer,
-                    });
-                }
+    spawn_robot_event.read().for_each(|RobotSpawned(robot_id)| {
+        for (other_robot_id, transform, color_association) in query.iter() {
+            // initialise the first position of the robot into the ring buffer
+            let mut ring_buffer = StaticRb::default();
+            let _ = ring_buffer.push_overwrite(transform.translation);
+
+            if other_robot_id == *robot_id {
+                let _ = traces.0.entry(*robot_id).or_insert(Trace {
+                    color: Color::from_catppuccin_colour(
+                        theme.get_display_colour(&color_association.name),
+                    ),
+                    ring_buffer,
+                });
             }
-        });
+        }
+    });
 }
 
 /// **Bevy** [`Update`] system
diff --git a/crates/gbpplanner-rs/src/planner/visualiser/uncertainty.rs b/crates/gbpplanner-rs/src/planner/visualiser/uncertainty.rs
index 85450342..0a1c9a86 100644
--- a/crates/gbpplanner-rs/src/planner/visualiser/uncertainty.rs
+++ b/crates/gbpplanner-rs/src/planner/visualiser/uncertainty.rs
@@ -2,10 +2,11 @@
 use bevy::prelude::*;
 
 // use gbp_linalg::pretty_print_matrix;
-use super::{super::FactorGraph, RobotTracker, Z_FIGHTING_OFFSET};
+use super::{RobotTracker, Z_FIGHTING_OFFSET};
 use crate::{
     asset_loader::SceneAssets,
     config::Config,
+    factorgraph::prelude::FactorGraph,
     theme::{self, CatppuccinTheme, ColorAssociation, ColorFromCatppuccinColourExt},
 };
 
@@ -97,7 +98,7 @@ fn init_uncertainty(
                 //  [b, c, _, _],
                 //  [_, _, _, _],
                 //  [_, _, _, _]]
-                let covariance = &v.belief.sigma;
+                let covariance = &v.belief.covariance_matrix;
 
                 // half major axis λ₁ and half minor axis λ₂
                 // λ₁ = (a + c) / 2 + √((a - c)² / 4 + b²)
@@ -220,8 +221,8 @@ fn update_uncertainty(
                     continue;
                 }
 
-                let mean = &v.belief.mu;
-                let covariance = &v.belief.sigma;
+                let mean = &v.belief.mean;
+                let covariance = &v.belief.covariance_matrix;
                 // pretty_print_matrix!(covariance);
 
                 let mut attenable = true;
diff --git a/crates/gbpplanner-rs/src/planner/visualiser/waypoints.rs b/crates/gbpplanner-rs/src/planner/visualiser/waypoints.rs
index 204d965f..0e128561 100644
--- a/crates/gbpplanner-rs/src/planner/visualiser/waypoints.rs
+++ b/crates/gbpplanner-rs/src/planner/visualiser/waypoints.rs
@@ -7,7 +7,7 @@ use crate::{
     asset_loader::SceneAssets,
     config::{Config, DrawSetting},
     planner::{
-        robot::RobotReachedWaypointEvent,
+        robot::RobotReachedWaypoint,
         spawner::{CreateWaypointEvent, DeleteWaypointEvent},
         RobotId,
     },
@@ -31,7 +31,7 @@ impl Plugin for WaypointVisualiserPlugin {
 }
 
 fn listen_for_robot_reached_waypoint_event(
-    mut robot_reached_waypoint_event: EventReader<RobotReachedWaypointEvent>,
+    mut robot_reached_waypoint_event: EventReader<RobotReachedWaypoint>,
     mut delete_waypoint_event: EventWriter<DeleteWaypointEvent>,
     query_waypoints: Query<(Entity, &AssociatedWithRobot), With<WaypointVisualiser>>,
 ) {
diff --git a/crates/gbpplanner-rs/src/ui/mod.rs b/crates/gbpplanner-rs/src/ui/mod.rs
index 10a6c461..8ab38d25 100644
--- a/crates/gbpplanner-rs/src/ui/mod.rs
+++ b/crates/gbpplanner-rs/src/ui/mod.rs
@@ -23,7 +23,7 @@ use self::{
     controls::ControlsPanelPlugin, data::DataPanelPlugin, metrics::MetricsPlugin,
     scale::ScaleUiPlugin, settings::SettingsPanelPlugin,
 };
-use crate::theme::CatppuccinThemeVisualsExt;
+use crate::{theme::CatppuccinThemeVisualsExt, AppState};
 
 //  _     _ _______ _______  ______
 //  |     | |______ |______ |_____/
@@ -62,6 +62,7 @@ impl Plugin for EguiInterfacePlugin {
                 MetricsPlugin::default()            ))
             // .add_systems(OnEnter(SimulationState::Loading), load_fonts)
             .add_systems(Startup, load_fonts)
+            // .add_systems(OnEnter(AppState::Loading), load_fonts)
             .add_systems(Startup, configure_visuals)
             .add_systems(Update, action_block)
             .add_systems(
diff --git a/crates/gbpplanner-rs/src/ui/settings.rs b/crates/gbpplanner-rs/src/ui/settings.rs
index 18ea6b0c..05feadfa 100644
--- a/crates/gbpplanner-rs/src/ui/settings.rs
+++ b/crates/gbpplanner-rs/src/ui/settings.rs
@@ -23,7 +23,7 @@ use crate::{
     config::{Config, DrawSection, DrawSetting},
     environment::cursor::CursorCoordinates,
     input::screenshot::TakeScreenshot,
-    planner::{PausePlayEvent, PausedState},
+    pause_play::{PausePlay, PausedState},
     theme::{CatppuccinTheme, CycleTheme, FromCatppuccinColourExt},
 };
 
@@ -334,19 +334,20 @@ fn ui_settings_panel(
                         ui.label("Simulation Time");
                         let progress =
                             time_fixed.elapsed_seconds() / config.simulation.max_time.get();
-                        let progressbar =
-                            egui::widgets::ProgressBar::new(progress).fill(Color32::RED);
-                        ui.add(progressbar);
-
-                        // custom::rect_label(
-                        //     ui,
-                        //     format!(
-                        //         "{:.2} / {:.2}",
-                        //         time_fixed.elapsed_seconds(),
-                        //         config.simulation.max_time.get()
-                        //     ),
-                        //     None,
-                        // );
+                        // let progressbar =
+                        //     egui::widgets::ProgressBar::new(progress).fill(Color32::RED);
+                        // ui.add(progressbar);
+
+                        custom::rect_label(
+                            ui,
+                            format!(
+                                "{:.2} / {:.2}",
+                                // time_fixed.elapsed_seconds(),
+                                time_virtual.elapsed_seconds(),
+                                config.simulation.max_time.get()
+                            ),
+                            None,
+                        );
                         ui.end_row();
 
                         // slider for simulation time between 0 and 100
@@ -395,7 +396,7 @@ fn ui_settings_panel(
                                     .on_hover_text("Play or pause the simulation")
                                     .clicked()
                                 {
-                                    world.send_event::<PausePlayEvent>(PausePlayEvent::Toggle);
+                                    world.send_event::<PausePlay>(PausePlay::Toggle);
                                 }
                             });
 
diff --git a/crates/units/src/sample_rate.rs b/crates/units/src/sample_rate.rs
index 1b7c1d5a..878c8580 100644
--- a/crates/units/src/sample_rate.rs
+++ b/crates/units/src/sample_rate.rs
@@ -85,13 +85,13 @@ impl std::ops::Deref for SampleRate {
     }
 }
 
-#[cfg(test)]
-mod tests {
-    use super::*;
-    // use pretty_assertions::{assert_eq, assert_ne};
-
-    // #[test]
-    // fn () {
-
-    // }
-}
+// #[cfg(test)]
+// mod tests {
+//     use super::*;
+//     // use pretty_assertions::{assert_eq, assert_ne};
+//
+//     // #[test]
+//     // fn () {
+//
+//     // }
+// }