From dc57def4ca935dfb883ddf7e44c4049c07d8d6c6 Mon Sep 17 00:00:00 2001
From: Dar Dahlen <dardahlen@gmail.com>
Date: Sun, 22 Sep 2024 11:04:49 -0700
Subject: [PATCH] Add root finding using Halley's method

---
 src/kete_core/src/fitting/halley.rs | 90 +++++++++++++++++++++++++++++
 src/kete_core/src/fitting/mod.rs    |  2 +
 src/kete_core/src/fitting/newton.rs |  5 +-
 3 files changed, 93 insertions(+), 4 deletions(-)
 create mode 100644 src/kete_core/src/fitting/halley.rs

diff --git a/src/kete_core/src/fitting/halley.rs b/src/kete_core/src/fitting/halley.rs
new file mode 100644
index 0000000..b981fee
--- /dev/null
+++ b/src/kete_core/src/fitting/halley.rs
@@ -0,0 +1,90 @@
+//! # Halley's method
+//!
+//! Third order root finding algorithm.
+//! This is the next order method of newton-raphson.
+
+use crate::{errors::Error, prelude::NeosResult};
+
+/// Solve root using Halley's method.
+///
+/// This accepts a three functions, the first being a single input function for which
+/// the root is desired. The second function being the derivative of the first with
+/// respect to the input variable. The third is the third derivative.
+///
+/// ```
+///     use kete_core::fitting::halley;
+///     let f = |x| { 1.0 * x * x - 1.0 };
+///     let d = |x| { 2.0 * x };
+///     let dd = |_| { 2.0};
+///     let root = halley(f, d, dd, 0.0, 1e-10).unwrap();
+///     assert!((root - 1.0).abs() < 1e-12);
+/// ```
+///
+#[inline(always)]
+pub fn halley<Func, Der, SecDer>(
+    func: Func,
+    der: Der,
+    sec_der: SecDer,
+    start: f64,
+    atol: f64,
+) -> NeosResult<f64>
+where
+    Func: Fn(f64) -> f64,
+    Der: Fn(f64) -> f64,
+    SecDer: Fn(f64) -> f64,
+{
+    let mut x = start;
+
+    // if the starting position has derivative of 0, nudge it a bit.
+    if der(x).abs() < 1e-12 {
+        x += 0.1;
+    }
+
+    let mut f_eval: f64;
+    let mut d_eval: f64;
+    let mut dd_eval: f64;
+    let mut step: f64;
+    for _ in 0..100 {
+        f_eval = func(x);
+        if f_eval.abs() < atol {
+            return Ok(x);
+        }
+        d_eval = der(x);
+
+        // Derivative is 0, cannot solve
+        if d_eval.abs() < 1e-12 {
+            Err(Error::Convergence(
+                "Halley's root finding failed to converge due to zero derivative.".into(),
+            ))?;
+        }
+
+        dd_eval = sec_der(x);
+
+        if !dd_eval.is_finite() || !d_eval.is_finite() || !f_eval.is_finite() {
+            Err(Error::Convergence(
+                "Halley root finding failed to converge due to non-finite evaluations".into(),
+            ))?;
+        }
+        step = f_eval / d_eval;
+        step = step / (1.0 - step * dd_eval / (2.0 * d_eval));
+
+        x -= step;
+    }
+    Err(Error::Convergence(
+        "Halley's root finding hit iteration limit without converging.".into(),
+    ))?
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::fitting::halley;
+
+    #[test]
+    fn test_haley() {
+        let f = |x| 1.0 * x * x - 1.0;
+        let d = |x| 2.0 * x;
+        let dd = |_| 2.0;
+        let root = halley(f, d, dd, 0.0, 1e-10).unwrap();
+        assert!((root - 1.0).abs() < 1e-12);
+    }
+}
diff --git a/src/kete_core/src/fitting/mod.rs b/src/kete_core/src/fitting/mod.rs
index 1bea427..81fc10c 100644
--- a/src/kete_core/src/fitting/mod.rs
+++ b/src/kete_core/src/fitting/mod.rs
@@ -1,7 +1,9 @@
 //! # Fitting
 //! Fitting tools, including root finding.
+mod halley;
 mod newton;
 mod reduced_chi2;
 
+pub use halley::halley;
 pub use newton::newton_raphson;
 pub use reduced_chi2::{fit_reduced_chi2, reduced_chi2, reduced_chi2_der};
diff --git a/src/kete_core/src/fitting/newton.rs b/src/kete_core/src/fitting/newton.rs
index dac91e3..74dc1bb 100644
--- a/src/kete_core/src/fitting/newton.rs
+++ b/src/kete_core/src/fitting/newton.rs
@@ -24,7 +24,7 @@ where
 
     // if the starting position has derivative of 0, nudge it a bit.
     if der(x).abs() < 1e-12 {
-        x += 1.0;
+        x += 0.1;
     }
 
     let mut f_eval: f64;
@@ -75,8 +75,5 @@ mod tests {
 
         let root = newton_raphson(f, d, 0.0, 1e-10).unwrap();
         assert!((root - 1.0).abs() < 1e-12);
-
-        let root = newton_raphson(f, d, 1.0, 1e-10).unwrap();
-        assert!((root - 1.0).abs() < 1e-12);
     }
 }