diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 00000000..55ea4df5
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,7 @@
+{
+    "rust-analyzer.linkedProjects": [
+        "./gbp-rs/Cargo.toml",
+        "./gbp-rs/Cargo.toml",
+        "./gbp-rs/Cargo.toml"
+    ]
+}
\ No newline at end of file
diff --git a/Cargo.lock b/Cargo.lock
index 6703e272..5cb7c23d 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -1258,6 +1258,15 @@ version = "0.1.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0d8c1fef690941d3e7788d328517591fecc684c084084702d6ff1641e993699a"
 
+[[package]]
+name = "block-buffer"
+version = "0.10.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3078c7629b62d3f0439517fa394996acacc5cbc91c5a20d8c658e77abd503a71"
+dependencies = [
+ "generic-array",
+]
+
 [[package]]
 name = "block-sys"
 version = "0.1.0-beta.1"
@@ -1382,6 +1391,17 @@ version = "0.1.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "fd16c4719339c4530435d38e511904438d07cce7950afa3718a84ac36c10e89e"
 
+[[package]]
+name = "charming"
+version = "0.3.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f4c6b6990238a64b4ae139e7085ce2a11815cb67a0c066a3333ce40f3a329be3"
+dependencies = [
+ "handlebars",
+ "serde",
+ "serde_json",
+]
+
 [[package]]
 name = "clang-sys"
 version = "1.7.0"
@@ -1555,6 +1575,15 @@ dependencies = [
  "const_soft_float",
 ]
 
+[[package]]
+name = "convert_case"
+version = "0.6.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ec182b0ca2f35d8fc196cf3404988fd8b8c739a4d270ff118a398feb0cbec1ca"
+dependencies = [
+ "unicode-segmentation",
+]
+
 [[package]]
 name = "core-foundation"
 version = "0.9.4"
@@ -1640,6 +1669,15 @@ dependencies = [
  "windows 0.46.0",
 ]
 
+[[package]]
+name = "cpufeatures"
+version = "0.2.12"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "53fe5e26ff1b7aef8bca9c6080520cfb8d9333c7568e1829cef191a9723e5504"
+dependencies = [
+ "libc",
+]
+
 [[package]]
 name = "crc32fast"
 version = "1.3.2"
@@ -1683,6 +1721,16 @@ version = "0.8.19"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "248e3bacc7dc6baa3b21e405ee045c3047101a49145e7e9eca583ab4c2ca5345"
 
+[[package]]
+name = "crypto-common"
+version = "0.1.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1bfb12502f3fc46cca1bb51ac28df9d618d813cdc3d2f25b9fe775a34af26bb3"
+dependencies = [
+ "generic-array",
+ "typenum",
+]
+
 [[package]]
 name = "d3d12"
 version = "0.7.0"
@@ -1706,6 +1754,16 @@ version = "2.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "7e962a19be5cfc3f3bf6dd8f61eb50107f356ad6270fbb3ed41476571db78be5"
 
+[[package]]
+name = "digest"
+version = "0.10.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9ed9a281f7bc9b7576e61468ba615a66a5c8cfdff42420a70aa82701a3b1e292"
+dependencies = [
+ "block-buffer",
+ "crypto-common",
+]
+
 [[package]]
 name = "dispatch"
 version = "0.2.0"
@@ -2104,7 +2162,11 @@ dependencies = [
 name = "gbp-rs"
 version = "0.1.0"
 dependencies = [
+ "charming",
  "nalgebra",
+ "nutype",
+ "rand",
+ "rand_distr",
  "rayon",
  "rstest",
 ]
@@ -2123,6 +2185,16 @@ dependencies = [
  "toml",
 ]
 
+[[package]]
+name = "generic-array"
+version = "0.14.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "85649ca51fd72272d7821adaf274ad91c288277713d9c18820d8499a7ff69e9a"
+dependencies = [
+ "typenum",
+ "version_check",
+]
+
 [[package]]
 name = "gethostname"
 version = "0.3.0"
@@ -2329,6 +2401,20 @@ dependencies = [
  "svg_fmt",
 ]
 
+[[package]]
+name = "handlebars"
+version = "4.5.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "faa67bab9ff362228eb3d00bd024a4965d8231bbb7921167f0cfa66c6626b225"
+dependencies = [
+ "log",
+ "pest",
+ "pest_derive",
+ "serde",
+ "serde_json",
+ "thiserror",
+]
+
 [[package]]
 name = "hashbrown"
 version = "0.12.3"
@@ -2581,6 +2667,27 @@ dependencies = [
  "pkg-config",
 ]
 
+[[package]]
+name = "kinded"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ce4bdbb2f423660b19f0e9f7115182214732d8dd5f840cd0a3aee3e22562f34c"
+dependencies = [
+ "kinded_macros",
+]
+
+[[package]]
+name = "kinded_macros"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a13b4ddc5dcb32f45dac3d6f606da2a52fdb9964a18427e63cd5ef6c0d13288d"
+dependencies = [
+ "convert_case",
+ "proc-macro2",
+ "quote",
+ "syn 2.0.48",
+]
+
 [[package]]
 name = "ktx2"
 version = "0.3.0"
@@ -2639,6 +2746,12 @@ dependencies = [
  "windows-sys 0.48.0",
 ]
 
+[[package]]
+name = "libm"
+version = "0.2.8"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058"
+
 [[package]]
 name = "libredox"
 version = "0.0.2"
@@ -3023,6 +3136,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "39e3200413f237f41ab11ad6d161bc7239c84dcb631773ccd7de3dfe4b5c267c"
 dependencies = [
  "autocfg",
+ "libm",
 ]
 
 [[package]]
@@ -3067,6 +3181,28 @@ dependencies = [
  "syn 2.0.48",
 ]
 
+[[package]]
+name = "nutype"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "38fd38251d275b6a3cecbf4d2f8ce83b80609f0e33e2508a8bc3a7d94401d007"
+dependencies = [
+ "nutype_macros",
+]
+
+[[package]]
+name = "nutype_macros"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2b9dc33172cb6abafee47925b6a188f2d3c3726cb5df12f39132a53df98501d2"
+dependencies = [
+ "cfg-if",
+ "kinded",
+ "proc-macro2",
+ "quote",
+ "syn 2.0.48",
+]
+
 [[package]]
 name = "objc"
 version = "0.2.7"
@@ -3256,6 +3392,51 @@ version = "2.3.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "e3148f5046208a5d56bcfc03053e3ca6334e51da8dfb19b6cdc8b306fae3283e"
 
+[[package]]
+name = "pest"
+version = "2.7.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1f200d8d83c44a45b21764d1916299752ca035d15ecd46faca3e9a2a2bf6ad06"
+dependencies = [
+ "memchr",
+ "thiserror",
+ "ucd-trie",
+]
+
+[[package]]
+name = "pest_derive"
+version = "2.7.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bcd6ab1236bbdb3a49027e920e693192ebfe8913f6d60e294de57463a493cfde"
+dependencies = [
+ "pest",
+ "pest_generator",
+]
+
+[[package]]
+name = "pest_generator"
+version = "2.7.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2a31940305ffc96863a735bef7c7994a00b325a7138fdbc5bda0f1a0476d3275"
+dependencies = [
+ "pest",
+ "pest_meta",
+ "proc-macro2",
+ "quote",
+ "syn 2.0.48",
+]
+
+[[package]]
+name = "pest_meta"
+version = "2.7.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a7ff62f5259e53b78d1af898941cdcdccfae7385cf7d793a6e55de5d05bb4b7d"
+dependencies = [
+ "once_cell",
+ "pest",
+ "sha2",
+]
+
 [[package]]
 name = "petgraph"
 version = "0.6.4"
@@ -3317,6 +3498,12 @@ dependencies = [
  "unicode-xid",
 ]
 
+[[package]]
+name = "ppv-lite86"
+version = "0.2.17"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de"
+
 [[package]]
 name = "pretty-type-name"
 version = "1.0.1"
@@ -3363,6 +3550,46 @@ version = "0.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "17fd96390ed3feda12e1dfe2645ed587e0bea749e319333f104a33ff62f77a0b"
 
+[[package]]
+name = "rand"
+version = "0.8.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404"
+dependencies = [
+ "libc",
+ "rand_chacha",
+ "rand_core",
+]
+
+[[package]]
+name = "rand_chacha"
+version = "0.3.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88"
+dependencies = [
+ "ppv-lite86",
+ "rand_core",
+]
+
+[[package]]
+name = "rand_core"
+version = "0.6.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c"
+dependencies = [
+ "getrandom",
+]
+
+[[package]]
+name = "rand_distr"
+version = "0.4.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "32cb0b9bc82b0a0876c2dd994a7e7a2683d3e7390ca40e6886785ef0c7e3ee31"
+dependencies = [
+ "num-traits",
+ "rand",
+]
+
 [[package]]
 name = "range-alloc"
 version = "0.1.3"
@@ -3665,6 +3892,17 @@ dependencies = [
  "serde",
 ]
 
+[[package]]
+name = "sha2"
+version = "0.10.8"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "793db75ad2bcafc3ffa7c68b215fee268f537982cd901d132f89c6343f3a3dc8"
+dependencies = [
+ "cfg-if",
+ "cpufeatures",
+ "digest",
+]
+
 [[package]]
 name = "sharded-slab"
 version = "0.1.7"
@@ -4123,6 +4361,12 @@ version = "1.17.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825"
 
+[[package]]
+name = "ucd-trie"
+version = "0.1.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ed646292ffc8188ef8ea4d1e0e0150fb15a5c2e12ad9b8fc191ae7a8a7f3c4b9"
+
 [[package]]
 name = "unicode-bidi"
 version = "0.3.15"
@@ -4144,6 +4388,12 @@ dependencies = [
  "tinyvec",
 ]
 
+[[package]]
+name = "unicode-segmentation"
+version = "1.10.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1dd624098567895118886609431a7c3b8f516e41d30e0643f03d94592a147e36"
+
 [[package]]
 name = "unicode-width"
 version = "0.1.11"
diff --git a/gbp-rs/Cargo.toml b/gbp-rs/Cargo.toml
index d4dc1a88..c1736cbd 100644
--- a/gbp-rs/Cargo.toml
+++ b/gbp-rs/Cargo.toml
@@ -16,3 +16,7 @@ rayon = "1.8.1"
 rstest = "0.18.2"
 rand = "0.8.5"
 rand_distr = "0.4.3"
+
+
+[dev-dependencies]
+charming = "0.3.1"
diff --git a/gbp-rs/examples/1d_line_fitting.rs b/gbp-rs/examples/1d_line_fitting.rs
index 958c60c5..7f0dedd7 100644
--- a/gbp-rs/examples/1d_line_fitting.rs
+++ b/gbp-rs/examples/1d_line_fitting.rs
@@ -1,10 +1,7 @@
 /// Reimplementation of the 1D line fitting example from https://gaussianbp.github.io/
 /// jupyter notebook: https://colab.research.google.com/drive/1-nrE95X4UC9FBLR0-cTnsIP_XhA_PZKW?usp=sharing#scrollTo=kiAOHWV4uMGY
-
 use gbp_rs::prelude::*;
 
 fn main() {
-    
     let mut fg = FactorGraph::new();
-
 }
diff --git a/gbp-rs/src/factorgraph/factor.rs b/gbp-rs/src/factorgraph/factor.rs
index 02ca6c5a..00c34d11 100644
--- a/gbp-rs/src/factorgraph/factor.rs
+++ b/gbp-rs/src/factorgraph/factor.rs
@@ -1,26 +1,64 @@
+use super::message::Message;
 
+#[derive(Debug)]
+pub enum MeasurementModel {
+    Linear,
+    NonLinear,
+}
 
+pub trait Factor {
+    fn id(&self) -> super::NodeId;
+    fn compute_messages(&mut self) -> Vec<Message>;
+    fn energy(&self) -> f64;
+    fn residual(&self) -> nalgebra::DVector<f64>;
+    fn adj_means(&self) -> nalgebra::DVector<f64>;
+    fn compute(&self) -> f64;
+    fn robustify_loss(&self);
+    fn measurement_model(&self) -> MeasurementModel;
+    fn linerisation_point(&self) -> nalgebra::DVector<f64>;
+}
 
-pub trait Factor {}
+// [1,,3].iter().
 
-#[derive(Debug)]
-struct DefaultFactor;
-#[derive(Debug)]
-struct DynamicFactor;
-#[derive(Debug)]
-struct InterRobotFactor;
-#[derive(Debug)]
-struct ObstacleFactor;
+// #[derive(Debug)]
+// struct DynamicFactor {
+//     id: usize,
+//     // messages:
+// }
 
-impl Factor for DefaultFactor {}
-impl Factor for DynamicFactor {}
-impl Factor for InterRobotFactor {}
-impl Factor for ObstacleFactor {}
+// impl Factor for DynamicFactor {
+//     // type ComputedValue = f64;
 
-#[derive(Debug)]
-enum FactorType {
-    Default(DefaultFactor),
-    Dynamic(DynamicFactor),
-    InterRobot(InterRobotFactor),
-    Ocstacle(ObstacleFactor),
-}
+//     fn get_id(&self) -> usize {
+//         self.id
+//         // get_id_from_db()
+//     }
+
+//     fn compute_messages(&self) -> Vec<Message> {
+//         // Implementation goes here
+//         Vec::new()
+//     }
+
+//     fn compute(&self) -> Self::ComputedValue {
+//         // Implementation goes here
+//     }
+// }
+// let f = DynamicFactor {id : 2};
+
+// #[derive(Debug)]
+// struct InterRobotFactor;
+// #[derive(Debug)]
+// struct ObstacleFactor;
+
+// impl Factor for DefaultFactor {}
+// impl Factor for DynamicFactor {}
+// impl Factor for InterRobotFactor {}
+// impl Factor for ObstacleFactor {}
+
+// #[derive(Debug)]
+// enum FactorType {
+//     Default(DefaultFactor),
+//     Dynamic(DynamicFactor),
+//     InterRobot(InterRobotFactor),
+//     Ocstacle(ObstacleFactor),
+// }
diff --git a/gbp-rs/src/factorgraph/factorgraph.rs b/gbp-rs/src/factorgraph/factorgraph.rs
index 10f646a9..65a75cca 100644
--- a/gbp-rs/src/factorgraph/factorgraph.rs
+++ b/gbp-rs/src/factorgraph/factorgraph.rs
@@ -1,29 +1,36 @@
-
 use crate::factorgraph::factor::Factor;
 use crate::factorgraph::variable::Variable;
 
+pub struct GbpSettings {
+    pub beta: f64,
+}
 /// A factor graph is a bipartite graph representing the factorization of a function.
 /// It is composed of two types of nodes: factors and variables.
-/// 
-pub struct FactorGraph {
-    factors: Vec<Box<dyn Factor>>,
-    variables: Vec<Variable>,
+#[derive(Debug)]
+pub struct FactorGraph<F: Factor, V: Variable> {
+    // pub struct FactorGraph {
+    // factors: Vec<Box<dyn Factor>>,
+    factors: Vec<F>,
+    variables: Vec<V>,
+    gbp_settings: GbpSettings,
 }
 
+// std::unique_ptr
 
-impl FactorGraph {
-    pub fn new() -> Self {
+impl<F: Factor, V: Variable> FactorGraph<F, V> {
+    pub fn new(gbp_settings: GbpSettings) -> Self {
         Self {
             factors: Vec::new(),
             variables: Vec::new(),
+            gbp_settings,
         }
     }
 
-    pub fn add_factor(&mut self, factor: Box<dyn Factor>) {
+    pub fn add_factor(&mut self, factor: F) {
         self.factors.push(factor);
     }
 
-    pub fn add_variable(&mut self, variable: Variable) {
+    pub fn add_variable(&mut self, variable: V) {
         self.variables.push(variable);
     }
 
@@ -36,4 +43,31 @@ impl FactorGraph {
         //     factor.update_belief();
         // }
     }
+
+    // linearize_all_factors
+    fn compute_factors(&mut self) {
+        for factor in self.factors.iter() {
+            factor.compute();
+        }
+    }
+
+    fn robustify_all_factors(&mut self) {
+        for factor in self.factors.iter() {
+            factor.robustify_loss();
+        }
+    }
+
+    fn jit_linearisation(&mut self) {
+        for factor in self.factors.iter() {
+            match factor.measurement_model() {
+                MeasurementModel::NonLinear => {
+                    let adj_means = factor.adj_means();
+                    // factors.iters_since_relin += 1
+                    if ((adj_means - factor.linerisation_point()).norm() > self.gbp_settings.beta) {
+                        factor.compute();
+                    }
+                }
+            }
+        }
+    }
 }
diff --git a/gbp-rs/src/factorgraph/mod.rs b/gbp-rs/src/factorgraph/mod.rs
index 41b7978c..6c09eb85 100644
--- a/gbp-rs/src/factorgraph/mod.rs
+++ b/gbp-rs/src/factorgraph/mod.rs
@@ -1,4 +1,6 @@
 pub mod factor;
-pub mod variable;
 pub mod factorgraph;
 pub mod message;
+pub mod variable;
+
+type NodeId = usize;
diff --git a/gbp-rs/src/factorgraph/variable.rs b/gbp-rs/src/factorgraph/variable.rs
index 60ad707c..2cb529a0 100644
--- a/gbp-rs/src/factorgraph/variable.rs
+++ b/gbp-rs/src/factorgraph/variable.rs
@@ -1,21 +1,25 @@
-/// 
+///
 
 // pub trait Variable {}
 
 // struct RobotId(usize);
-type RobotId = usize;
+// type RobotId = usize;
 // struct NodeId(usize);
-type NodeId = usize;
 
+// #[derive(Debug, Copy, Clone, Eq, PartialEq, PartialOrd)]
+// pub struct Variable {
+//     node_id: NodeId,
+//     robot_id: RobotId,
+// }
 
-#[derive(Debug, Copy, Clone, Eq, PartialEq, PartialOrd)]
-pub struct Variable {
-    node_id: NodeId,
-    robot_id: RobotId,
-}
+// impl Variable {
+//     fn new(node_id: NodeId, robot_id: RobotId) -> Self {
+//         Self { node_id, robot_id }
+//     }
+// }
 
-impl Variable {
-    fn new(node_id: NodeId, robot_id: RobotId) -> Self {
-        Self { node_id, robot_id }
-    }
+pub trait Variable {
+    fn id(&self) -> super::NodeId;
+    fn update_belief(&mut self);
+    fn prior_energy(&self) -> f64;
 }
diff --git a/gbp-rs/src/lib.rs b/gbp-rs/src/lib.rs
index 0fa1a659..f48101a7 100644
--- a/gbp-rs/src/lib.rs
+++ b/gbp-rs/src/lib.rs
@@ -3,8 +3,6 @@ pub mod factorgraph;
 pub mod prelude {
     pub use crate::factorgraph::factor::Factor;
     pub use crate::factorgraph::factorgraph::FactorGraph;
-    pub use crate::factorgraph::variable::Variable;
     pub use crate::factorgraph::message::Message;
+    pub use crate::factorgraph::variable::Variable;
 }
-
-
diff --git a/gbpplanner-rs/src/shapes.rs b/gbpplanner-rs/src/shapes.rs
index 33bf97c7..3fbeb979 100644
--- a/gbpplanner-rs/src/shapes.rs
+++ b/gbpplanner-rs/src/shapes.rs
@@ -28,11 +28,11 @@ fn set_colors(
     mut query: Query<(&Transform, &Handle<ColorMaterial>)>,
     mut materials: ResMut<Assets<ColorMaterial>>,
 ) {
-    println!("Inside set_colors");
+    info!("Inside set_colors");
     for (i, (_, handle)) in query.iter_mut().enumerate() {
         // let material = materials.get_mut(handle).unwrap();
         if let Some(material) = materials.get_mut(handle) {
-            println!("Setting color to {:?}", colors.0[i]);
+            info!("Setting color to {:?}", colors.0[i]);
             let color = colors.0[i];
             material.color = color;
         }
diff --git a/hx.log b/hx.log
new file mode 100644
index 00000000..8feed474
--- /dev/null
+++ b/hx.log
@@ -0,0 +1,3 @@
+2024-02-01T13:21:37.825 helix_lsp::transport [ERROR] rust-analyzer err <- "error: 'rust-analyzer' is not installed for the toolchain 'stable-x86_64-unknown-linux-gnu'\n"
+2024-02-01T13:21:37.826 helix_lsp [ERROR] failed to initialize language server: server closed the stream
+2024-02-01T13:21:37.826 helix_lsp::transport [ERROR] rust-analyzer err: <- StreamClosed
diff --git a/shell.nix b/shell.nix
deleted file mode 100644
index ee8a26a8..00000000
--- a/shell.nix
+++ /dev/null
@@ -1,15 +0,0 @@
-{ pkgs ? import <nixpkgs> { } }:
-
-  with pkgs;
-
-mkShell rec {
-  nativeBuildInputs = [
-    pkg-config
-  ];
-  buildInputs = [
-    udev alsa-lib vulkan-loader
-    xorg.libX11 xorg.libXcursor xorg.libXi xorg.libXrandr # To use the x11 feature
-    libxkbcommon wayland # To use the wayland feature
-  ];
-  LD_LIBRARY_PATH = lib.makeLibraryPath buildInputs;
-}