diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..2f7896d
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+target/
diff --git a/.nvim.lua b/.nvim.lua
new file mode 100644
index 0000000..c026d8d
--- /dev/null
+++ b/.nvim.lua
@@ -0,0 +1,17 @@
+-- You must enable the exrc setting in neovim for this config file to be used.
+local rust_analyzer = {
+    cargo = {
+        target = "xtensa-esp32-none-elf",
+        allTargets = false,
+    },
+}
+rust_analyzer.cargo.extraEnv = { RUST_TOOLCHAIN = "esp" }
+rust_analyzer.check = { extraEnv = { RUST_TOOLCHAIN = "esp" } }
+rust_analyzer.server = { extraEnv = { RUST_TOOLCHAIN = "stable" } }
+
+-- Note the neovim name of the language server is rust_analyzer with an underscore.
+vim.lsp.config("rust_analyzer", {
+    settings = {
+        ["rust-analyzer"] = rust_analyzer
+    },
+})
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..2c4f731
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,63 @@
+
+
+
+set(CARGO_BUILD_TYPE "release")
+set(CARGO_BUILD_ARG "--release")
+if(CONFIG_IDF_TARGET_ARCH_RISCV)
+    set(CARGO_TARGET "riscv32imc-esp-espidf")
+    set(CARGO_FEATURES_ARG "")
+elseif(CONFIG_IDF_TARGET_ARCH_XTENSA)
+    set(CARGO_TARGET "xtensa-esp32-espidf")
+    set(CARGO_FEATURES_ARG "--features=std")
+endif()
+
+set(RUST_PROJECT_DIR "${CMAKE_CURRENT_LIST_DIR}")
+set(RUST_BUILD_DIR "${CMAKE_CURRENT_BINARY_DIR}")
+set(RUST_TARGET_DIR "${RUST_BUILD_DIR}/target")
+set(RUST_INCLUDE_DIR "${RUST_TARGET_DIR}")
+set(RUST_INCLUDE_HEADER "${RUST_INCLUDE_DIR}/drone_controller.h")
+set(RUST_STATIC_LIBRARY "${RUST_TARGET_DIR}/${CARGO_TARGET}/${CARGO_BUILD_TYPE}/libdrone_controller.a")
+
+
+idf_component_register(
+    SRCS "placeholder.c"
+    INCLUDE_DIRS "" "${RUST_INCLUDE_DIR}"
+    PRIV_REQUIRES "${RUST_DEPS}"
+)
+idf_build_get_property(sdkconfig SDKCONFIG)
+ExternalProject_Add(
+    drone_controller_project
+    PREFIX "${RUST_PROJECT_DIR}"
+    DOWNLOAD_COMMAND ""
+    CONFIGURE_COMMAND ${CMAKE_COMMAND} -E env
+        CARGO_BUILD_TARGET=${CARGO_TARGET}
+        CARGO_BUILD_TARGET_DIR=${RUST_TARGET_DIR}
+        cargo clean
+    BUILD_COMMAND ${CMAKE_COMMAND} -E env
+        CARGO_BUILD_TARGET=${CARGO_TARGET}
+        CARGO_BUILD_TARGET_DIR=${RUST_TARGET_DIR}
+        CARGO_CMAKE_BUILD_INCLUDES=$<TARGET_PROPERTY:${COMPONENT_LIB},INCLUDE_DIRECTORIES>
+        CARGO_CMAKE_BUILD_LINK_LIBRARIES=$<TARGET_PROPERTY:${COMPONENT_LIB},LINK_LIBRARIES>
+        CARGO_CMAKE_BUILD_SDKCONFIG=${sdkconfig}
+        CARGO_CMAKE_BUILD_COMPILER=${CMAKE_C_COMPILER}
+        cargo build ${CARGO_BUILD_ARG} ${CARGO_FEATURES_ARG}
+    INSTALL_COMMAND ""
+    BUILD_ALWAYS TRUE
+    TMP_DIR "${RUST_BUILD_DIR}/tmp"
+    STAMP_DIR "${RUST_BUILD_DIR}/stamp"
+    DOWNLOAD_DIR "${RUST_BUILD_DIR}"
+    SOURCE_DIR "${RUST_PROJECT_DIR}"
+    BINARY_DIR "${RUST_PROJECT_DIR}"
+    INSTALL_DIR "${RUST_BUILD_DIR}"
+    BUILD_BYPRODUCTS
+        "${RUST_INCLUDE_HEADER}"
+        "${RUST_STATIC_LIBRARY}"
+)
+
+set_source_files_properties("${RUST_INCLUDE_HEADER}" PROPERTIES GENERATED true)
+
+add_prebuilt_library(drone_controller_lib "${RUST_STATIC_LIBRARY}" PRIV_REQUIRES "${RUST_DEPS}")
+add_dependencies(drone_controller_lib drone_codrone_controllerr_project)
+
+target_include_directories(${COMPONENT_LIB} PUBLIC "${RUST_INCLUDE_DIR}")
+target_link_libraries(${COMPONENT_LIB} PRIVATE drone_controller_lib)
diff --git a/Cargo.lock b/Cargo.lock
index 25da3ce..9652985 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -2,18 +2,6 @@
 # It is not intended for manual editing.
 version = 4
 
-[[package]]
-name = "adler2"
-version = "2.0.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "320119579fcad9c21884f5c4861d16174d0e06250625266f50fe6898340abefa"
-
-[[package]]
-name = "allocator-api2"
-version = "0.2.21"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923"
-
 [[package]]
 name = "android_system_properties"
 version = "0.1.5"
@@ -73,6 +61,12 @@ dependencies = [
  "windows-sys",
 ]
 
+[[package]]
+name = "anyhow"
+version = "1.0.102"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7f202df86484c868dbad7eaa557ef785d5c66295e41b460ef922eca0723b842c"
+
 [[package]]
 name = "approx"
 version = "0.5.1"
@@ -82,12 +76,6 @@ dependencies = [
  "num-traits",
 ]
 
-[[package]]
-name = "arrayvec"
-version = "0.7.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7c02d123df017efcdfbd739ef81735b36c5ba83ec3c59c80a9d7ecc718f92e50"
-
 [[package]]
 name = "autocfg"
 version = "1.5.0"
@@ -100,18 +88,6 @@ version = "0.22.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "72b3254f16251a8381aa12e40e3c4d2f0199f8c6508fbecb9d91f575e0fbb8c6"
 
-[[package]]
-name = "bit-vec"
-version = "0.8.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5e764a1d40d510daf35e07be9eb06e75770908c27d411ee6c92109c9840eaaf7"
-
-[[package]]
-name = "bitflags"
-version = "1.3.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a"
-
 [[package]]
 name = "bitflags"
 version = "2.11.0"
@@ -131,10 +107,23 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c8efb64bd706a16a1bdde310ae86b351e4d21550d98d056f22f8a7f7a2183fec"
 
 [[package]]
-name = "byteorder"
-version = "1.5.0"
+name = "cbindgen"
+version = "0.29.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b"
+checksum = "befbfd072a8e81c02f8c507aefce431fe5e7d051f83d48a23ffc9b9fe5a11799"
+dependencies = [
+ "clap",
+ "heck",
+ "indexmap 2.13.0",
+ "log",
+ "proc-macro2",
+ "quote",
+ "serde",
+ "serde_json",
+ "syn",
+ "tempfile",
+ "toml",
+]
 
 [[package]]
 name = "cc"
@@ -204,12 +193,6 @@ version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "3a822ea5bc7590f9d40f1ba12c0dc3c2760f3482c6984db1573ad11031420831"
 
-[[package]]
-name = "color_quant"
-version = "1.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3d7b894f5411737b7867f4827955924d7c254fc9f4d91a6aad6b097804b1018b"
-
 [[package]]
 name = "colorchoice"
 version = "1.0.4"
@@ -222,15 +205,6 @@ version = "0.8.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "773648b94d0e5d620f64f280777445740e61fe701025087ec8b57f45c791888b"
 
-[[package]]
-name = "crc32fast"
-version = "1.5.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9481c1c90cbf2ac953f07c8d4a58aa3945c425b7185c9154d67a65e4230da511"
-dependencies = [
- "cfg-if",
-]
-
 [[package]]
 name = "darling"
 version = "0.21.3"
@@ -276,20 +250,13 @@ dependencies = [
  "serde_core",
 ]
 
-[[package]]
-name = "downcast-rs"
-version = "2.0.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "117240f60069e65410b3ae1bb213295bd828f707b5bec6596a1afc8793ce0cbc"
-
 [[package]]
 name = "drone_controller"
 version = "0.1.0"
 dependencies = [
+ "cbindgen",
  "clap",
- "macroquad",
  "nalgebra",
- "rapier3d",
  "serde",
  "serde_json",
  "serde_with",
@@ -302,21 +269,6 @@ version = "1.0.20"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "d0881ea181b1df73ff77ffaaf9c7544ecc11e82fba9b5f27b262a3c73a332555"
 
-[[package]]
-name = "either"
-version = "1.15.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719"
-
-[[package]]
-name = "ena"
-version = "0.14.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "eabffdaee24bd1bf95c5ef7cec31260444317e72ea56c4c91750e8b7ee58d5f1"
-dependencies = [
- "log",
-]
-
 [[package]]
 name = "equivalent"
 version = "1.0.2"
@@ -324,30 +276,27 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "877a4ace8713b0bcf2a4e7eec82529c029f1d0619886d18145fea96c3ffe5c0f"
 
 [[package]]
-name = "fdeflate"
-version = "0.3.7"
+name = "errno"
+version = "0.3.14"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1e6853b52649d4ac5c0bd02320cddc5ba956bdb407c4b75a2c6b75bf51500f8c"
+checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb"
 dependencies = [
- "simd-adler32",
+ "libc",
+ "windows-sys",
 ]
 
+[[package]]
+name = "fastrand"
+version = "2.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "37909eebbb50d72f9059c3b6d82c0463f2ff062c9e95845c43a6c9c0355411be"
+
 [[package]]
 name = "find-msvc-tools"
 version = "0.1.9"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "5baebc0774151f905a1a2cc41989300b1e6fbb29aff0ceffa1064fdd3088d582"
 
-[[package]]
-name = "flate2"
-version = "1.1.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "843fba2746e448b37e26a819579957415c8cef339bf08564fe8b7ddbd959573c"
-dependencies = [
- "crc32fast",
- "miniz_oxide",
-]
-
 [[package]]
 name = "fnv"
 version = "1.0.7"
@@ -361,19 +310,16 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "d9c4f5dac5e15c24eb999c26181a6ca40b39fe946cbe4c263c7209467bc83af2"
 
 [[package]]
-name = "foldhash"
-version = "0.2.0"
+name = "getrandom"
+version = "0.4.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "77ce24cb58228fbb8aa041425bb1050850ac19177686ea6e0f41a70416f56fdb"
-
-[[package]]
-name = "fontdue"
-version = "0.9.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2e57e16b3fe8ff4364c0661fdaac543fb38b29ea9bc9c2f45612d90adf931d2b"
+checksum = "0de51e6874e94e7bf76d726fc5d13ba782deca734ff60d5bb2fb2607c7406555"
 dependencies = [
- "hashbrown 0.15.5",
- "ttf-parser",
+ "cfg-if",
+ "libc",
+ "r-efi",
+ "wasip2",
+ "wasip3",
 ]
 
 [[package]]
@@ -472,15 +418,6 @@ version = "0.30.10"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "19fc433e8437a212d1b6f1e68c7824af3aed907da60afa994e7f542d18d12aa9"
 
-[[package]]
-name = "hash32"
-version = "0.3.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606"
-dependencies = [
- "byteorder",
-]
-
 [[package]]
 name = "hashbrown"
 version = "0.12.3"
@@ -493,9 +430,7 @@ version = "0.15.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "9229cfe53dfd69f0609a49f65461bd93001ea1ef889cd5529dd176593f5338a1"
 dependencies = [
- "allocator-api2",
- "equivalent",
- "foldhash 0.1.5",
+ "foldhash",
 ]
 
 [[package]]
@@ -503,19 +438,6 @@ name = "hashbrown"
 version = "0.16.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "841d1cc9bed7f9236f321df977030373f4a4163ae1a7dbfe1a51a2c1a51d9100"
-dependencies = [
- "foldhash 0.2.0",
-]
-
-[[package]]
-name = "heapless"
-version = "0.8.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad"
-dependencies = [
- "hash32",
- "stable_deref_trait",
-]
 
 [[package]]
 name = "heck"
@@ -553,25 +475,18 @@ dependencies = [
  "cc",
 ]
 
+[[package]]
+name = "id-arena"
+version = "2.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3d3067d79b975e8844ca9eb072e16b31c3c1c36928edf9c6789548c524d0d954"
+
 [[package]]
 name = "ident_case"
 version = "1.0.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39"
 
-[[package]]
-name = "image"
-version = "0.24.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5690139d2f55868e080017335e4b94cb7414274c74f1669c84fb5feba2c9f69d"
-dependencies = [
- "bytemuck",
- "byteorder",
- "color_quant",
- "num-traits",
- "png",
-]
-
 [[package]]
 name = "indexmap"
 version = "1.9.3"
@@ -617,6 +532,12 @@ dependencies = [
  "wasm-bindgen",
 ]
 
+[[package]]
+name = "leb128fmt"
+version = "0.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "09edd9e8b54e49e587e4f6295a7d29c3ea94d469cb40ab8ca70b288248a81db2"
+
 [[package]]
 name = "libc"
 version = "0.2.183"
@@ -624,10 +545,10 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b5b646652bf6661599e1da8901b3b9522896f01e736bad5f723fe7a3a27f899d"
 
 [[package]]
-name = "libm"
-version = "0.2.16"
+name = "linux-raw-sys"
+version = "0.12.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b6d2cec3eae94f9f509c767b45932f1ada8350c4bdb85af2fcab4a3c14807981"
+checksum = "32a66949e030da00e8c7d4434b251670a91556f4144941d37452769c25d58a53"
 
 [[package]]
 name = "log"
@@ -635,35 +556,6 @@ version = "0.4.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "5e5032e24019045c762d3c0f28f5b6b8bbf38563a65908389bf7978758920897"
 
-[[package]]
-name = "macroquad"
-version = "0.4.14"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d2befbae373456143ef55aa93a73594d080adfb111dc32ec96a1123a3e4ff4ae"
-dependencies = [
- "fontdue",
- "glam 0.27.0",
- "image",
- "macroquad_macro",
- "miniquad",
- "quad-rand",
-]
-
-[[package]]
-name = "macroquad_macro"
-version = "0.1.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "64b1d96218903768c1ce078b657c0d5965465c95a60d2682fd97443c9d2483dd"
-
-[[package]]
-name = "malloc_buf"
-version = "0.0.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "62bb907fe88d54d8d9ce32a3cceab4218ed2f6b7d35617cafe9adf84e43919cb"
-dependencies = [
- "libc",
-]
-
 [[package]]
 name = "matrixmultiply"
 version = "0.3.10"
@@ -680,28 +572,6 @@ version = "2.8.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "f8ca58f447f06ed17d5fc4043ce1b10dd205e060fb3ce5b979b8ed8e59ff3f79"
 
-[[package]]
-name = "miniquad"
-version = "0.4.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2fb3e758e46dbc45716a8a49ca9edc54b15bcca826277e80b1f690708f67f9e3"
-dependencies = [
- "libc",
- "ndk-sys",
- "objc-rs",
- "winapi",
-]
-
-[[package]]
-name = "miniz_oxide"
-version = "0.8.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1fa76a2c86f704bdb222d66965fb3d63269ce38518b83cb0575fca855ebb6316"
-dependencies = [
- "adler2",
- "simd-adler32",
-]
-
 [[package]]
 name = "nalgebra"
 version = "0.34.1"
@@ -745,12 +615,6 @@ dependencies = [
  "syn",
 ]
 
-[[package]]
-name = "ndk-sys"
-version = "0.2.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e1bcdd74c20ad5d95aacd60ef9ba40fdf77f767051040541df557b7a9b2a2121"
-
 [[package]]
 name = "num-bigint"
 version = "0.4.6"
@@ -776,17 +640,6 @@ version = "0.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "cf97ec579c3c42f953ef76dbf8d55ac91fb219dde70e49aa4a6b7d74e9919050"
 
-[[package]]
-name = "num-derive"
-version = "0.4.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ed3955f1a9c7c0c15e092f9c887db08b1fc683305fdf6eb6684f22555355e202"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
-]
-
 [[package]]
 name = "num-integer"
 version = "0.1.46"
@@ -814,16 +667,6 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841"
 dependencies = [
  "autocfg",
- "libm",
-]
-
-[[package]]
-name = "objc-rs"
-version = "0.2.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "64a1e7069a2525126bf12a9f1f7916835fafade384fb27cabf698e745e2a1eb8"
-dependencies = [
- "malloc_buf",
 ]
 
 [[package]]
@@ -838,69 +681,28 @@ version = "1.70.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "384b8ab6d37215f3c5301a95a4accb5d64aa607f1fcb26a11b5303878451b4fe"
 
-[[package]]
-name = "ordered-float"
-version = "5.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7f4779c6901a562440c3786d08192c6fbda7c1c2060edd10006b05ee35d10f2d"
-dependencies = [
- "num-traits",
-]
-
-[[package]]
-name = "parry3d"
-version = "0.25.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e99471b7b6870f7fe406d5611dd4b4c9b07aa3e5436b1d27e1515f9832bb0c6b"
-dependencies = [
- "approx",
- "arrayvec",
- "bitflags 2.11.0",
- "downcast-rs",
- "either",
- "ena",
- "foldhash 0.2.0",
- "glam 0.30.10",
- "hashbrown 0.16.1",
- "log",
- "nalgebra",
- "num-derive",
- "num-traits",
- "ordered-float",
- "rstar",
- "simba",
- "slab",
- "smallvec",
- "spade",
- "static_assertions",
- "thiserror",
-]
-
 [[package]]
 name = "paste"
 version = "1.0.15"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a"
 
-[[package]]
-name = "png"
-version = "0.17.16"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "82151a2fc869e011c153adc57cf2789ccb8d9906ce52c0b39a6b5697749d7526"
-dependencies = [
- "bitflags 1.3.2",
- "crc32fast",
- "fdeflate",
- "flate2",
- "miniz_oxide",
-]
-
 [[package]]
 name = "powerfmt"
 version = "0.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "439ee305def115ba05938db6eb1644ff94165c5ab5e9420d1c1bcedbba909391"
 
+[[package]]
+name = "prettyplease"
+version = "0.2.37"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "479ca8adacdd7ce8f1fb39ce9ecccbfe93a3f1344b3d0d97f20bc0196208f62b"
+dependencies = [
+ "proc-macro2",
+ "syn",
+]
+
 [[package]]
 name = "proc-macro2"
 version = "1.0.106"
@@ -910,31 +712,6 @@ dependencies = [
  "unicode-ident",
 ]
 
-[[package]]
-name = "profiling"
-version = "1.0.17"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3eb8486b569e12e2c32ad3e204dbaba5e4b5b216e9367044f25f1dba42341773"
-dependencies = [
- "profiling-procmacros",
-]
-
-[[package]]
-name = "profiling-procmacros"
-version = "1.0.17"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "52717f9a02b6965224f95ca2a81e2e0c5c43baacd28ca057577988930b6c3d5b"
-dependencies = [
- "quote",
- "syn",
-]
-
-[[package]]
-name = "quad-rand"
-version = "0.2.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5a651516ddc9168ebd67b24afd085a718be02f8858fe406591b013d101ce2f40"
-
 [[package]]
 name = "quote"
 version = "1.0.45"
@@ -945,30 +722,10 @@ dependencies = [
 ]
 
 [[package]]
-name = "rapier3d"
-version = "0.31.0"
+name = "r-efi"
+version = "6.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "68073fdc88f6b709002767ce8deffffb05ac06824bf9f98a23e270bcea64ba9f"
-dependencies = [
- "approx",
- "arrayvec",
- "bit-vec",
- "bitflags 2.11.0",
- "downcast-rs",
- "log",
- "nalgebra",
- "num-derive",
- "num-traits",
- "ordered-float",
- "parry3d",
- "profiling",
- "rustc-hash",
- "simba",
- "static_assertions",
- "thiserror",
- "vec_map",
- "wide",
-]
+checksum = "f8dcc9c7d52a811697d2151c701e0d08956f92b0e24136cf4cf27b57a6a0d9bf"
 
 [[package]]
 name = "rawpointer"
@@ -997,28 +754,18 @@ dependencies = [
 ]
 
 [[package]]
-name = "robust"
-version = "1.2.0"
+name = "rustix"
+version = "1.1.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4e27ee8bb91ca0adcf0ecb116293afa12d393f9c2b9b9cd54d33e8078fe19839"
-
-[[package]]
-name = "rstar"
-version = "0.12.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "421400d13ccfd26dfa5858199c30a5d76f9c54e0dba7575273025b43c5175dbb"
+checksum = "b6fe4565b9518b83ef4f91bb47ce29620ca828bd32cb7e408f0062e9930ba190"
 dependencies = [
- "heapless",
- "num-traits",
- "smallvec",
+ "bitflags",
+ "errno",
+ "libc",
+ "linux-raw-sys",
+ "windows-sys",
 ]
 
-[[package]]
-name = "rustc-hash"
-version = "2.1.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "357703d41365b4b27c590e3ed91eabb1b663f07c4c084095e60cbed4362dff0d"
-
 [[package]]
 name = "rustversion"
 version = "1.0.22"
@@ -1058,6 +805,12 @@ dependencies = [
  "serde_json",
 ]
 
+[[package]]
+name = "semver"
+version = "1.0.27"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d767eb0aabc880b29956c35734170f26ed551a859dbd361d140cdbeca61ab1e2"
+
 [[package]]
 name = "serde"
 version = "1.0.228"
@@ -1160,48 +913,6 @@ dependencies = [
  "wide",
 ]
 
-[[package]]
-name = "simd-adler32"
-version = "0.3.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e320a6c5ad31d271ad523dcf3ad13e2767ad8b1cb8f047f75a8aeaf8da139da2"
-
-[[package]]
-name = "slab"
-version = "0.4.12"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0c790de23124f9ab44544d7ac05d60440adc586479ce501c1d6d7da3cd8c9cf5"
-
-[[package]]
-name = "smallvec"
-version = "1.15.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "67b1b7a3b5fe4f1376887184045fcf45c69e92af734b7aaddc05fb777b6fbd03"
-
-[[package]]
-name = "spade"
-version = "2.15.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "fb313e1c8afee5b5647e00ee0fe6855e3d529eb863a0fdae1d60006c4d1e9990"
-dependencies = [
- "hashbrown 0.15.5",
- "num-traits",
- "robust",
- "smallvec",
-]
-
-[[package]]
-name = "stable_deref_trait"
-version = "1.2.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6ce2be8dc25455e1f91df71bfa12ad37d7af1092ae736f3a6cd0e37bc7810596"
-
-[[package]]
-name = "static_assertions"
-version = "1.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f"
-
 [[package]]
 name = "strsim"
 version = "0.11.1"
@@ -1220,23 +931,16 @@ dependencies = [
 ]
 
 [[package]]
-name = "thiserror"
-version = "2.0.18"
+name = "tempfile"
+version = "3.27.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4288b5bcbc7920c07a1149a35cf9590a2aa808e0bc1eafaade0b80947865fbc4"
+checksum = "32497e9a4c7b38532efcdebeef879707aa9f794296a4f0244f6f69e9bc8574bd"
 dependencies = [
- "thiserror-impl",
-]
-
-[[package]]
-name = "thiserror-impl"
-version = "2.0.18"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ebc4ee7f67670e9b64d05fa4253e753e016c6c95ff35b89b7941d6b856dec1d5"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
+ "fastrand",
+ "getrandom",
+ "once_cell",
+ "rustix",
+ "windows-sys",
 ]
 
 [[package]]
@@ -1309,12 +1013,6 @@ version = "1.0.6+spec-1.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "ab16f14aed21ee8bfd8ec22513f7287cd4a91aa92e44edfe2c17ddd004e92607"
 
-[[package]]
-name = "ttf-parser"
-version = "0.21.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2c591d83f69777866b9126b24c6dd9a18351f177e49d625920d19f989fd31cf8"
-
 [[package]]
 name = "typenum"
 version = "1.19.0"
@@ -1327,6 +1025,12 @@ version = "1.0.24"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75"
 
+[[package]]
+name = "unicode-xid"
+version = "0.2.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ebc1c04c71510c7f702b52b7c350734c9ff1295c464a03335b00bb84fc54f853"
+
 [[package]]
 name = "utf8parse"
 version = "0.2.2"
@@ -1334,10 +1038,22 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821"
 
 [[package]]
-name = "vec_map"
-version = "0.8.2"
+name = "wasip2"
+version = "1.0.2+wasi-0.2.9"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f1bddf1187be692e79c5ffeab891132dfb0f236ed36a43c7ed39f1165ee20191"
+checksum = "9517f9239f02c069db75e65f174b3da828fe5f5b945c4dd26bd25d89c03ebcf5"
+dependencies = [
+ "wit-bindgen",
+]
+
+[[package]]
+name = "wasip3"
+version = "0.4.0+wasi-0.3.0-rc-2026-01-06"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5428f8bf88ea5ddc08faddef2ac4a67e390b88186c703ce6dbd955e1c145aca5"
+dependencies = [
+ "wit-bindgen",
+]
 
 [[package]]
 name = "wasm-bindgen"
@@ -1384,6 +1100,40 @@ dependencies = [
  "unicode-ident",
 ]
 
+[[package]]
+name = "wasm-encoder"
+version = "0.244.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "990065f2fe63003fe337b932cfb5e3b80e0b4d0f5ff650e6985b1048f62c8319"
+dependencies = [
+ "leb128fmt",
+ "wasmparser",
+]
+
+[[package]]
+name = "wasm-metadata"
+version = "0.244.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bb0e353e6a2fbdc176932bbaab493762eb1255a7900fe0fea1a2f96c296cc909"
+dependencies = [
+ "anyhow",
+ "indexmap 2.13.0",
+ "wasm-encoder",
+ "wasmparser",
+]
+
+[[package]]
+name = "wasmparser"
+version = "0.244.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "47b807c72e1bac69382b3a6fb3dbe8ea4c0ed87ff5629b8685ae6b9a611028fe"
+dependencies = [
+ "bitflags",
+ "hashbrown 0.15.5",
+ "indexmap 2.13.0",
+ "semver",
+]
+
 [[package]]
 name = "wide"
 version = "0.7.33"
@@ -1394,28 +1144,6 @@ dependencies = [
  "safe_arch",
 ]
 
-[[package]]
-name = "winapi"
-version = "0.3.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419"
-dependencies = [
- "winapi-i686-pc-windows-gnu",
- "winapi-x86_64-pc-windows-gnu",
-]
-
-[[package]]
-name = "winapi-i686-pc-windows-gnu"
-version = "0.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6"
-
-[[package]]
-name = "winapi-x86_64-pc-windows-gnu"
-version = "0.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f"
-
 [[package]]
 name = "windows-core"
 version = "0.62.2"
@@ -1490,6 +1218,94 @@ version = "0.7.15"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "df79d97927682d2fd8adb29682d1140b343be4ac0f08fd68b7765d9c059d3945"
 
+[[package]]
+name = "wit-bindgen"
+version = "0.51.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d7249219f66ced02969388cf2bb044a09756a083d0fab1e566056b04d9fbcaa5"
+dependencies = [
+ "wit-bindgen-rust-macro",
+]
+
+[[package]]
+name = "wit-bindgen-core"
+version = "0.51.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ea61de684c3ea68cb082b7a88508a8b27fcc8b797d738bfc99a82facf1d752dc"
+dependencies = [
+ "anyhow",
+ "heck",
+ "wit-parser",
+]
+
+[[package]]
+name = "wit-bindgen-rust"
+version = "0.51.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b7c566e0f4b284dd6561c786d9cb0142da491f46a9fbed79ea69cdad5db17f21"
+dependencies = [
+ "anyhow",
+ "heck",
+ "indexmap 2.13.0",
+ "prettyplease",
+ "syn",
+ "wasm-metadata",
+ "wit-bindgen-core",
+ "wit-component",
+]
+
+[[package]]
+name = "wit-bindgen-rust-macro"
+version = "0.51.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0c0f9bfd77e6a48eccf51359e3ae77140a7f50b1e2ebfe62422d8afdaffab17a"
+dependencies = [
+ "anyhow",
+ "prettyplease",
+ "proc-macro2",
+ "quote",
+ "syn",
+ "wit-bindgen-core",
+ "wit-bindgen-rust",
+]
+
+[[package]]
+name = "wit-component"
+version = "0.244.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9d66ea20e9553b30172b5e831994e35fbde2d165325bec84fc43dbf6f4eb9cb2"
+dependencies = [
+ "anyhow",
+ "bitflags",
+ "indexmap 2.13.0",
+ "log",
+ "serde",
+ "serde_derive",
+ "serde_json",
+ "wasm-encoder",
+ "wasm-metadata",
+ "wasmparser",
+ "wit-parser",
+]
+
+[[package]]
+name = "wit-parser"
+version = "0.244.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ecc8ac4bc1dc3381b7f59c34f00b67e18f910c2c0f50015669dde7def656a736"
+dependencies = [
+ "anyhow",
+ "id-arena",
+ "indexmap 2.13.0",
+ "log",
+ "semver",
+ "serde",
+ "serde_derive",
+ "serde_json",
+ "unicode-xid",
+ "wasmparser",
+]
+
 [[package]]
 name = "zmij"
 version = "1.0.21"
diff --git a/Cargo.toml b/Cargo.toml
index 5837d25..e67ca1d 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,8 +1,16 @@
+
 [package]
 name = "drone_controller"
 version = "0.1.0"
 edition = "2024"
 
+[lib]
+crate-type = ["staticlib", "lib"]
+
+[build-dependencies]
+cbindgen = "0.29"
+
+
 [dependencies]
 
 nalgebra = { version = "0.34" }
@@ -14,8 +22,20 @@ serde_json = "1.0.149"
 
 clap = { version = "4", features = ["derive"] }
 
-[lib]
-crate-type = ["lib", "cdylib"]
+[features]
+std = []
 
-[build-dependencies]
-cbindgen = "0.29"
+[profile.dev]
+# Optimization level 1 is similar to "g" in gcc/clang, although for some reason
+# Cargo doesn't support that flag.
+#opt-level = 1
+opt-level = "s"
+
+[profile.release]
+debug = true
+opt-level = "z"
+#debug = true
+#lto = true
+#opt-level = "z"
+#codegen-units = 1
+#panic = "abort"
diff --git a/build.rs b/build.rs
index 3465469..b89a83c 100644
--- a/build.rs
+++ b/build.rs
@@ -1,11 +1,32 @@
-extern crate cbindgen;
-
-use std::env;
+use std::path::{Path, PathBuf};
+use std::{env, time};
 
 fn main() {
-    let crate_dir = env::var("CARGO_MANIFEST_DIR").unwrap();
+    if env::var("CARGO_BUILD_TARGET_DIR").is_ok() {
+        let cargo_dir = PathBuf::from(env::var("CARGO_MANIFEST_DIR").unwrap());
+        let target_dir = PathBuf::from(env::var("CARGO_BUILD_TARGET_DIR").unwrap());
 
-    let _ = cbindgen::generate(crate_dir)
-        .unwrap()
-        .write_to_file("bindings.h");
+        run_cbindgen(&cargo_dir, &target_dir);
+    }
+}
+
+fn run_cbindgen(cargo_dir: &Path, target_dir: &Path) {
+    let out = target_dir.join("drone_controller.h");
+
+    cbindgen::Builder::new()
+        .with_crate(cargo_dir)
+        .with_language(cbindgen::Language::Cxx)
+        .with_pragma_once(true)
+        .generate()
+        .expect("Unable to generate bindings")
+        .write_to_file(&out);
+
+    println!("cargo:rerun-if-changed={}", out.display());
+    println!(
+        "cargo:rerun-if-changed={}",
+        time::SystemTime::now()
+            .duration_since(time::UNIX_EPOCH)
+            .unwrap()
+            .as_secs()
+    );
 }
diff --git a/cbindgen.toml b/cbindgen.toml
index 94d7633..f3d2293 100644
--- a/cbindgen.toml
+++ b/cbindgen.toml
@@ -8,10 +8,25 @@
 language = "C++"
 
 
-include_guard = "my_bindings_h"
-autogen_warning = "/* Warning, this file is autogenerated by cbindgen. Don't modify this manually. */"
-include_version = true
+############## Options for Wrapping the Contents of the Header #################
 
+# header = "/* Text to put at the beginning of the generated file. Probably a license. */"
+# trailer = "/* Text to put at the end of the generated file */"
+# include_guard = "my_bindings_h"
+pragma_once = true
+# autogen_warning = "/* Warning, this file is autogenerated by cbindgen. Don't modify this manually. */"
+include_version = false
+namespace = "dcont"
+# namespaces = []
+using_namespaces = []
+sys_includes = []
+includes = []
+no_includes = false
+# cpp_compat = true
+after_includes = ""
+
+
+############################ Code Style Options ################################
 
 braces = "SameLine"
 line_length = 100
@@ -19,18 +34,104 @@ tab_width = 2
 documentation = true
 documentation_style = "auto"
 documentation_length = "full"
-line_endings = "LF"
+line_endings = "LF"           # also "CR", "CRLF", "Native"
 
-namespace = "dcont"
 
+############################# Codegen Options ##################################
 
 style = "both"
-sort_by = "Name"
+sort_by = "Name"       # default for `fn.sort_by` and `const.sort_by`
 usize_is_size_t = true
 
+
+[defines]
+# "target_os = freebsd" = "DEFINE_FREEBSD"
+# "feature = serde" = "DEFINE_SERDE"
+
+
 [export]
 include = []
 exclude = []
-# prefix = "DCONT_"
+# prefix = "CAPI_"
 item_types = []
 renaming_overrides_prefixing = false
+
+
+[export.rename]
+
+
+[export.body]
+
+
+[export.mangle]
+
+
+[fn]
+rename_args = "None"
+# must_use = "MUST_USE_FUNC"
+# deprecated = "DEPRECATED_FUNC"
+# deprecated_with_note = "DEPRECATED_FUNC_WITH_NOTE"
+# no_return = "NO_RETURN"
+# prefix = "START_FUNC"
+# postfix = "END_FUNC"
+args = "auto"
+sort_by = "Name"
+
+
+[struct]
+rename_fields = "None"
+# must_use = "MUST_USE_STRUCT"
+# deprecated = "DEPRECATED_STRUCT"
+# deprecated_with_note = "DEPRECATED_STRUCT_WITH_NOTE"
+rename_associated_constant = "None"
+derive_constructor = false
+derive_eq = false
+derive_neq = false
+derive_lt = false
+derive_lte = false
+derive_gt = false
+derive_gte = false
+
+
+[enum]
+rename_variants = "None"
+# must_use = "MUST_USE_ENUM"
+# deprecated = "DEPRECATED_ENUM"
+# deprecated_with_note = "DEPRECATED_ENUM_WITH_NOTE"
+add_sentinel = false
+prefix_with_name = false
+derive_helper_methods = false
+derive_const_casts = false
+derive_mut_casts = false
+# cast_assert_name = "ASSERT"
+derive_tagged_enum_destructor = false
+derive_tagged_enum_copy_constructor = false
+enum_class = true
+private_default_tagged_enum_constructor = false
+
+
+[const]
+allow_static_const = true
+allow_constexpr = false
+sort_by = "Name"
+
+
+[macro_expansion]
+bitflags = false
+
+
+############## Options for How Your Rust library Should Be Parsed ##############
+
+[parse]
+parse_deps = false
+# include = []
+exclude = []
+clean = false
+extra_bindings = []
+
+
+[parse.expand]
+crates = []
+all_features = false
+default_features = true
+features = []
diff --git a/placeholder.c b/placeholder.c
new file mode 100644
index 0000000..de0472a
--- /dev/null
+++ b/placeholder.c
@@ -0,0 +1,13 @@
+/* Hello World Example
+
+   This example code is in the Public Domain (or CC0 licensed, at your option.)
+
+   Unless required by applicable law or agreed to in writing, this
+   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
+   CONDITIONS OF ANY KIND, either express or implied.
+*/
+
+/* This is an empty source file to force the build of a CMake library that
+   can participate in the CMake dependency graph. This placeholder library
+   will depend on the actual library generated by external Rust build.
+*/
\ No newline at end of file
diff --git a/rust-toolchain.toml b/rust-toolchain.toml
new file mode 100644
index 0000000..a2f5ab5
--- /dev/null
+++ b/rust-toolchain.toml
@@ -0,0 +1,2 @@
+[toolchain]
+channel = "esp"
diff --git a/src/ccont.rs b/src/ccont.rs
new file mode 100644
index 0000000..07a7526
--- /dev/null
+++ b/src/ccont.rs
@@ -0,0 +1,96 @@
+use crate::*;
+use nalgebra as na;
+
+#[unsafe(no_mangle)]
+pub extern "C" fn create(c: config::ControllerConfig) -> *mut StackedController {
+    return Box::into_raw(Box::new(StackedController::new(c)));
+}
+
+
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn destroy(ptr: *mut StackedController) {
+    assert!(!ptr.is_null());
+    let _x = unsafe { Box::from_raw(ptr) };
+}
+
+#[repr(C)]
+pub struct MotorThrottles {
+    pub values: [f32; 4],
+}
+
+
+
+
+#[unsafe(no_mangle)]
+pub extern "C" fn get_throttles(ptr: *mut StackedController) -> MotorThrottles {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    return MotorThrottles {
+        values: controller.get_motor_throttles(),
+    };
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_cur_time(ptr: *mut StackedController, time: f32) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_time(time);
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_input(ptr: *mut StackedController, inp: Input) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_input(inp);
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_cur_rot(ptr: *mut StackedController, parts: Vec3C, scalar: f32) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    let quat: na::UnitQuaternion<f32> =
+        na::UnitQuaternion::from_quaternion(na::Quaternion::from_parts(scalar, parts.into()));
+    controller.set_rotation(quat);
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_cur_angvel(ptr: *mut StackedController, angvel: Vec3C) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_angular_velocity(angvel.into());
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_cur_linvel(ptr: *mut StackedController, vel: Vec3C) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_linvel(vel.into());
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_cur_pos(ptr: *mut StackedController, pos: Vec3C) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_pos(pos.into());
+}
diff --git a/src/cmath.rs b/src/cmath.rs
new file mode 100644
index 0000000..e8680a8
--- /dev/null
+++ b/src/cmath.rs
@@ -0,0 +1,82 @@
+use nalgebra as na;
+
+#[repr(C)]
+pub struct Vec3C {
+    pub x: f32,
+    pub y: f32,
+    pub z: f32,
+}
+
+#[repr(C)]
+pub struct QuatC {
+    pub i: f32,
+    pub j: f32,
+    pub k: f32,
+    pub w: f32,
+}
+
+impl Into<na::UnitQuaternion<f32>> for QuatC {
+    fn into(self) -> na::UnitQuaternion<f32> {
+        na::UnitQuaternion::from_quaternion(na::Quaternion::from_parts(
+            self.w,
+            na::vector![self.i, self.j, self.k],
+        ))
+    }
+}
+
+impl Into<na::Vector3<f32>> for Vec3C {
+    fn into(self) -> na::Vector3<f32> {
+        na::vector![self.x, self.y, self.z]
+    }
+}
+
+impl From<na::Vector3<f32>> for Vec3C {
+    fn from(value: na::Vector3<f32>) -> Self {
+        Vec3C { x: value.x, y: value.y, z: value.z }
+    }
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn apply_rot(vec_ptr: *const Vec3C, quat_ptr: *const QuatC) -> Vec3C {
+    if vec_ptr.is_null() || quat_ptr.is_null() {
+        return Vec3C { x: 0.0, y: 0.0, z: 0.0 };
+    }
+
+    unsafe {
+        let v_raw = &*vec_ptr;
+        let q_raw = &*quat_ptr;
+
+        let v = na::Vector3::new(v_raw.x, v_raw.y, v_raw.z);
+
+        let q = na::UnitQuaternion::from_quaternion(
+            na::Quaternion::new(q_raw.w , q_raw.i, q_raw.j, q_raw.k)
+        );
+
+        let rotated = q.transform_vector(&v);
+
+        Vec3C { x: rotated.x, y: rotated.y, z: rotated.z }
+    }
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn multiply_vec_by(vec_ptr: *mut Vec3C, k: f32) {
+    assert!(!vec_ptr.is_null());
+
+    unsafe {
+        (*vec_ptr).x *= k;
+        (*vec_ptr).y *= k;
+        (*vec_ptr).z *= k;
+    }
+}
+
+
+#[unsafe(no_mangle)]
+pub extern "C" fn add_to_vec(vec_ptr: *mut Vec3C, to_add_ptr: *const Vec3C){
+    assert!(!vec_ptr.is_null());
+    assert!(!to_add_ptr.is_null());
+    unsafe {
+        (*vec_ptr).x += (*to_add_ptr).x;
+        (*vec_ptr).y += (*to_add_ptr).y;
+        (*vec_ptr).z += (*to_add_ptr).z;
+    }
+}
diff --git a/src/lib.rs b/src/lib.rs
index 3ed0faf..ce245b3 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,5 +1,3 @@
-use nalgebra::{self as na, Quaternion, Unit};
-
 pub mod config;
 pub mod controller;
 pub mod input;
@@ -10,106 +8,7 @@ pub use controller::*;
 pub use input::*;
 pub use stacked::*;
 
-#[unsafe(no_mangle)]
-pub extern "C" fn create(c: config::ControllerConfig) -> *mut StackedController {
-    return Box::into_raw(Box::new(StackedController::new(c)));
-}
-
-
-#[unsafe(no_mangle)]
-pub unsafe extern "C" fn destroy(ptr: *mut ControllerConfig) {
-    assert!(!ptr.is_null());
-    let _x = unsafe { Box::from_raw(ptr) };
-}
-
-#[repr(C)]
-pub struct MotorThrottles {
-    pub values: [f32; 4],
-}
-
-#[repr(C)]
-pub struct Vec3C {
-    pub x: f32,
-    pub y: f32,
-    pub z: f32,
-}
-
-impl Into<na::Vector3<f32>> for Vec3C {
-    fn into(self) -> na::Vector3<f32> {
-        na::vector![self.x, self.y, self.z]
-    }
-}
-
-#[unsafe(no_mangle)]
-pub extern "C" fn get_throttles(ptr: *mut StackedController) -> MotorThrottles {
-    let controller = unsafe {
-        assert!(!ptr.is_null());
-        &mut *ptr
-    };
-
-    return MotorThrottles {
-        values: controller.get_motor_throttles(),
-    };
-}
-
-#[unsafe(no_mangle)]
-pub extern "C" fn set_cur_time(ptr: *mut StackedController, time: f32) {
-    let controller = unsafe {
-        assert!(!ptr.is_null());
-        &mut *ptr
-    };
-
-    controller.set_time(time);
-}
-
-#[unsafe(no_mangle)]
-pub extern "C" fn set_input(ptr: *mut StackedController, inp: Input) {
-    let controller = unsafe {
-        assert!(!ptr.is_null());
-        &mut *ptr
-    };
-
-    controller.set_input(inp);
-}
-
-#[unsafe(no_mangle)]
-pub extern "C" fn set_cur_rot(ptr: *mut StackedController, parts: Vec3C, scalar: f32) {
-    let controller = unsafe {
-        assert!(!ptr.is_null());
-        &mut *ptr
-    };
-
-    let quat: na::UnitQuaternion<f32> =
-        na::UnitQuaternion::from_quaternion(Quaternion::from_parts(scalar, parts.into()));
-    controller.set_rotation(quat);
-}
-
-#[unsafe(no_mangle)]
-pub extern "C" fn set_cur_angvel(ptr: *mut StackedController, angvel: Vec3C) {
-    let controller = unsafe {
-        assert!(!ptr.is_null());
-        &mut *ptr
-    };
-
-    controller.set_angular_velocity(angvel.into());
-}
-
-#[unsafe(no_mangle)]
-pub extern "C" fn set_cur_linvel(ptr: *mut StackedController, vel: Vec3C) {
-    let controller = unsafe {
-        assert!(!ptr.is_null());
-        &mut *ptr
-    };
-
-    controller.set_linvel(vel.into());
-}
-
-#[unsafe(no_mangle)]
-pub extern "C" fn set_cur_pos(ptr: *mut StackedController, pos: Vec3C) {
-    let controller = unsafe {
-        assert!(!ptr.is_null());
-        &mut *ptr
-    };
-
-    controller.set_pos(pos.into());
-}
+pub mod ccont;
+pub mod cmath;
+pub use ccont::*;
+pub use cmath::*;