From 8016a8c7f8785960bfd1f43dcf6e9a12f9254712 Mon Sep 17 00:00:00 2001
From: TQ Hirsch <thequux@upstandinghackers.com>
Date: Mon, 25 Apr 2022 00:01:17 +0200
Subject: [PATCH] Added missing math extensions

---
 firmware/jerk_control/src/math_ext.rs | 78 +++++++++++++++++++++++++++
 1 file changed, 78 insertions(+)
 create mode 100644 firmware/jerk_control/src/math_ext.rs

diff --git a/firmware/jerk_control/src/math_ext.rs b/firmware/jerk_control/src/math_ext.rs
new file mode 100644
index 0000000..b7d7924
--- /dev/null
+++ b/firmware/jerk_control/src/math_ext.rs
@@ -0,0 +1,78 @@
+
+
+pub trait Roots {
+    fn sqrt(self) -> Self;
+    fn cbrt(self) -> Self;
+}
+
+#[cfg(ignore)]
+impl Roots for u64 {
+    fn sqrt(self) -> Self {
+	let mut estimate = self / 2;
+	for _ in 0..6 {
+	    estimate = (self / estimate + estimate) / 2;
+	}
+	estimate
+    }
+
+    fn cbrt(self) -> Self {
+    }
+}
+
+#[cfg(ignore)]
+impl Roots for u32 {
+   fn sqrt(self) -> Self {
+	let mut estimate = self / 2;
+	for _ in 0..5 {
+	    estimate = (self / estimate + estimate) / 2;
+	}
+	estimate
+    }
+
+    fn cbrt(self) -> Self {
+    }
+}
+
+
+fn sqrt_approx(value: f32) -> f32 {
+    f32::from_bits((value.to_bits() + 0x3f80_0000) >> 1)
+}
+
+pub fn cbrt_approx(value: f32) -> f32 {
+    let value = value.to_bits();
+
+	const EXP_BIAS: i32 = 0x3f80_0000;
+
+	f32::from_bits((value & 0x8000_0000) |
+					   (((value as i32 & 0x7fff_ffff) - EXP_BIAS) / 3 + EXP_BIAS) as u32)
+}
+
+impl Roots for f32 {
+    fn sqrt(self) -> Self {
+	let mut estimate = sqrt_approx(self);
+	for _ in 0..5 {
+	    estimate = (self / estimate + estimate) / 2.;
+	}
+	estimate
+    }
+
+    fn cbrt(self) -> Self {
+	let mut estimate = cbrt_approx(self); // TODO: improve this with bit twiddling
+
+	// Use halley's method to refine the approximation.
+	for _ in 0..3 {
+	    let e3 = estimate * estimate * estimate;
+	    estimate = estimate * (e3 + self + self) / ( e3 + e3 + self);
+	}
+	
+	estimate
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use super::Roots;
+
+
+    
+}