1use crate::solvers::SolversError as ExecutionError;
7use crate::solvers::{SolverConfig, SolverResult, SolverState};
8
9pub mod metaheuristics;
13
14#[repr(C)]
16pub struct NelderMeadSimplex {
17 pub vertices: [[f64; 4]; 5], pub values: [f64; 5],
21 pub iteration: u32,
23 pub best_point: [f64; 4],
25 pub best_value: f64,
26 pub config: SolverConfig,
28 pub solver_state: SolverState,
30}
31
32#[repr(C)]
34pub struct BoundedNewtonRaphson {
35 pub current_guess: f64,
37 pub previous_guess: f64,
39 pub current_value: f64,
41 pub current_derivative: f64,
43 pub lower_bound: f64,
45 pub upper_bound: f64,
46 pub config: SolverConfig,
48 pub solver_state: SolverState,
50}
51
52#[repr(C)]
54pub struct LevenbergMarquardtStack {
55 pub parameters: [f64; 4],
57 pub delta_parameters: [f64; 4],
59 pub jacobian: [[f64; 10]; 4],
61 pub residuals: [f64; 10],
63 pub lambda: f64,
65 pub chi_squared: f64,
67 pub config: SolverConfig,
69 pub solver_state: SolverState,
71}
72
73#[repr(C)]
75#[derive(Clone, Copy)]
76pub struct OptimizationState {
77 pub iteration: u32,
79 pub objective_value: f64,
81 pub converged: bool,
83 pub simplex_size: f64,
85}
86
87#[repr(C)]
89#[derive(Clone, Copy)]
90pub struct RootFindingState {
91 pub iteration: u32,
93 pub function_value: f64,
95 pub converged: bool,
97 pub step_size: f64,
99}
100
101#[repr(C)]
103#[derive(Clone, Copy)]
104pub struct CurveFitState {
105 pub iteration: u32,
107 pub chi_squared: f64,
109 pub converged: bool,
111 pub lambda: f64,
113}
114
115pub trait ObjectiveFunction {
117 fn evaluate(&self, params: &[f64; 4]) -> f64;
119
120 fn in_bounds(&self, _params: &[f64; 4]) -> bool {
122 true }
124}
125
126pub trait RootFunction {
128 fn evaluate(&self, x: f64) -> f64;
130
131 fn derivative(&self, x: f64) -> f64;
133}
134
135pub trait CurveFitFunction {
137 fn evaluate(&self, x: f64, params: &[f64; 4]) -> f64;
139
140 fn jacobian(&self, x: f64, params: &[f64; 4]) -> [f64; 4];
142}
143
144impl NelderMeadSimplex {
145 pub fn new(initial_point: [f64; 4], config: SolverConfig) -> Self {
147 Self {
148 vertices: Self::initialize_simplex(initial_point),
149 values: [0.0; 5],
150 iteration: 0,
151 best_point: initial_point,
152 best_value: f64::MAX,
153 config,
154 solver_state: SolverState::default(),
155 }
156 }
157
158 const fn initialize_simplex(initial_point: [f64; 4]) -> [[f64; 4]; 5] {
160 let mut simplex = [[0.0; 4]; 5];
161
162 simplex[0] = initial_point;
164
165 let mut i = 1;
167 while i < 5 {
168 let mut vertex = initial_point;
169 vertex[i - 1] += 0.1; simplex[i] = vertex;
171 i += 1;
172 }
173
174 simplex
175 }
176
177 pub fn optimize<F>(&mut self, f: &F) -> SolverResult<OptimizationState>
179 where
180 F: ObjectiveFunction,
181 {
182 for i in 0..5 {
184 self.values[i] = f.evaluate(&self.vertices[i]);
185 }
186
187 self.iteration = 0;
188 self.solver_state.converged = false;
189
190 while self.iteration < self.config.max_iterations {
191 self.sort_vertices();
193
194 self.best_point = self.vertices[0];
196 self.best_value = self.values[0];
197
198 if self.check_convergence() {
200 self.solver_state.converged = true;
201 break;
202 }
203
204 self.nelder_mead_step(f)?;
206
207 self.iteration += 1;
208 }
209
210 Ok(OptimizationState {
211 iteration: self.iteration,
212 objective_value: self.best_value,
213 converged: self.solver_state.converged,
214 simplex_size: self.calculate_simplex_size(),
215 })
216 }
217
218 fn sort_vertices(&mut self) {
220 for i in 0..4 {
222 for j in 0..4 - i {
223 if self.values[j] > self.values[j + 1] {
224 let temp_vertex = self.vertices[j];
226 self.vertices[j] = self.vertices[j + 1];
227 self.vertices[j + 1] = temp_vertex;
228
229 let temp_value = self.values[j];
231 self.values[j] = self.values[j + 1];
232 self.values[j + 1] = temp_value;
233 }
234 }
235 }
236 }
237
238 fn check_convergence(&self) -> bool {
240 let simplex_size = self.calculate_simplex_size();
241 simplex_size < self.config.tolerance
242 }
243
244 fn calculate_simplex_size(&self) -> f64 {
246 let mut size: f64 = 0.0;
247
248 for i in 1..5 {
249 let mut distance = 0.0;
250 for j in 0..4 {
251 distance += (self.vertices[i][j] - self.vertices[0][j]).powi(2);
252 }
253 size = size.max(distance.sqrt());
254 }
255
256 size
257 }
258
259 fn nelder_mead_step<F>(&mut self, f: &F) -> SolverResult<()>
261 where
262 F: ObjectiveFunction,
263 {
264 let centroid = self.calculate_centroid();
266
267 let reflected = self.reflect(¢roid);
269 let reflected_value = f.evaluate(&reflected);
270
271 if reflected_value < self.values[0] {
272 let expanded = self.expand(¢roid, &reflected);
274 let expanded_value = f.evaluate(&expanded);
275
276 if expanded_value < reflected_value {
277 self.vertices[4] = expanded;
278 self.values[4] = expanded_value;
279 } else {
280 self.vertices[4] = reflected;
281 self.values[4] = reflected_value;
282 }
283 } else if reflected_value < self.values[3] {
284 self.vertices[4] = reflected;
286 self.values[4] = reflected_value;
287 } else {
288 if reflected_value < self.values[4] {
290 let contracted = self.contract(¢roid, &reflected);
291 let contracted_value = f.evaluate(&contracted);
292
293 if contracted_value < reflected_value {
294 self.vertices[4] = contracted;
295 self.values[4] = contracted_value;
296 } else {
297 self.shrink(f)?;
298 }
299 } else {
300 let contracted = self.contract(¢roid, &self.vertices[4]);
301 let contracted_value = f.evaluate(&contracted);
302
303 if contracted_value < self.values[4] {
304 self.vertices[4] = contracted;
305 self.values[4] = contracted_value;
306 } else {
307 self.shrink(f)?;
308 }
309 }
310 }
311
312 Ok(())
313 }
314
315 fn calculate_centroid(&self) -> [f64; 4] {
317 let mut centroid = [0.0; 4];
318
319 for i in 0..4 {
320 for j in 0..4 {
321 centroid[j] += self.vertices[i][j];
322 }
323 }
324
325 for j in 0..4 {
326 centroid[j] /= 4.0;
327 }
328
329 centroid
330 }
331
332 fn reflect(&self, centroid: &[f64; 4]) -> [f64; 4] {
334 let mut reflected = [0.0; 4];
335 let alpha = 1.0; for i in 0..4 {
338 reflected[i] = centroid[i] + alpha * (centroid[i] - self.vertices[4][i]);
339 }
340
341 reflected
342 }
343
344 fn expand(&self, centroid: &[f64; 4], reflected: &[f64; 4]) -> [f64; 4] {
346 let mut expanded = [0.0; 4];
347 let gamma = 2.0; for i in 0..4 {
350 expanded[i] = centroid[i] + gamma * (reflected[i] - centroid[i]);
351 }
352
353 expanded
354 }
355
356 fn contract(&self, centroid: &[f64; 4], worst: &[f64; 4]) -> [f64; 4] {
358 let mut contracted = [0.0; 4];
359 let rho = 0.5; for i in 0..4 {
362 contracted[i] = centroid[i] + rho * (worst[i] - centroid[i]);
363 }
364
365 contracted
366 }
367
368 fn shrink<F>(&mut self, f: &F) -> SolverResult<()>
370 where
371 F: ObjectiveFunction,
372 {
373 let sigma = 0.5; for i in 1..5 {
376 for j in 0..4 {
377 self.vertices[i][j] =
378 self.vertices[0][j] + sigma * (self.vertices[i][j] - self.vertices[0][j]);
379 }
380 self.values[i] = f.evaluate(&self.vertices[i]);
381 }
382
383 Ok(())
384 }
385
386 pub fn get_best_solution(&self) -> ([f64; 4], f64) {
388 (self.best_point, self.best_value)
389 }
390}
391
392impl BoundedNewtonRaphson {
393 pub fn new(
395 initial_guess: f64,
396 lower_bound: f64,
397 upper_bound: f64,
398 config: SolverConfig,
399 ) -> Self {
400 Self {
401 current_guess: initial_guess,
402 previous_guess: initial_guess,
403 current_value: 0.0,
404 current_derivative: 0.0,
405 lower_bound,
406 upper_bound,
407 config,
408 solver_state: SolverState::default(),
409 }
410 }
411
412 pub fn find_root<F>(&mut self, f: &F) -> SolverResult<RootFindingState>
414 where
415 F: RootFunction,
416 {
417 self.solver_state.iteration = 0;
418 self.solver_state.converged = false;
419
420 while self.solver_state.iteration < self.config.max_iterations {
421 self.current_value = f.evaluate(self.current_guess);
423 self.current_derivative = f.derivative(self.current_guess);
424
425 if self.current_value.abs() < self.config.tolerance {
427 self.solver_state.converged = true;
428 break;
429 }
430
431 if self.current_derivative.abs() < 1e-10 {
433 return Err(ExecutionError::ConvergenceFailed);
434 }
435
436 let new_guess = self.current_guess - self.current_value / self.current_derivative;
438
439 let bounded_guess = new_guess.clamp(self.lower_bound, self.upper_bound);
441
442 if (bounded_guess - self.current_guess).abs() < self.config.tolerance {
444 self.solver_state.converged = true;
445 break;
446 }
447
448 self.previous_guess = self.current_guess;
449 self.current_guess = bounded_guess;
450 self.solver_state.iteration += 1;
451 }
452
453 Ok(RootFindingState {
454 iteration: self.solver_state.iteration,
455 function_value: self.current_value,
456 converged: self.solver_state.converged,
457 step_size: (self.current_guess - self.previous_guess).abs(),
458 })
459 }
460
461 pub fn get_root(&self) -> f64 {
463 self.current_guess
464 }
465}
466
467impl LevenbergMarquardtStack {
468 pub fn new(initial_parameters: [f64; 4], config: SolverConfig) -> Self {
470 Self {
471 parameters: initial_parameters,
472 delta_parameters: [0.0; 4],
473 jacobian: [[0.0; 10]; 4],
474 residuals: [0.0; 10],
475 lambda: 1e-3,
476 chi_squared: f64::MAX,
477 config,
478 solver_state: SolverState::default(),
479 }
480 }
481
482 pub fn fit_curve<F>(
484 &mut self,
485 f: &F,
486 x_data: &[f64; 10],
487 y_data: &[f64; 10],
488 ) -> SolverResult<CurveFitState>
489 where
490 F: CurveFitFunction,
491 {
492 self.solver_state.iteration = 0;
493 self.solver_state.converged = false;
494
495 self.evaluate_residuals(f, x_data, y_data)?;
497 self.chi_squared = self.calculate_chi_squared();
498
499 while self.solver_state.iteration < self.config.max_iterations {
500 self.calculate_jacobian(f, x_data)?;
502
503 self.solve_parameter_update()?;
505
506 let old_chi_squared = self.chi_squared;
508 let old_parameters = self.parameters;
509
510 for i in 0..4 {
512 self.parameters[i] += self.delta_parameters[i];
513 }
514
515 self.evaluate_residuals(f, x_data, y_data)?;
517 self.chi_squared = self.calculate_chi_squared();
518
519 if self.chi_squared < old_chi_squared {
521 self.lambda *= 0.1;
523 } else {
524 self.lambda *= 10.0;
526 self.parameters = old_parameters;
527 self.chi_squared = old_chi_squared;
528 }
529
530 if self.check_convergence(old_chi_squared) {
532 self.solver_state.converged = true;
533 break;
534 }
535
536 self.solver_state.iteration += 1;
537 }
538
539 Ok(CurveFitState {
540 iteration: self.solver_state.iteration,
541 chi_squared: self.chi_squared,
542 converged: self.solver_state.converged,
543 lambda: self.lambda,
544 })
545 }
546
547 fn evaluate_residuals<F>(
549 &mut self,
550 f: &F,
551 x_data: &[f64; 10],
552 y_data: &[f64; 10],
553 ) -> SolverResult<()>
554 where
555 F: CurveFitFunction,
556 {
557 for i in 0..10 {
558 let model_value = f.evaluate(x_data[i], &self.parameters);
559 self.residuals[i] = y_data[i] - model_value;
560 }
561
562 Ok(())
563 }
564
565 fn calculate_chi_squared(&self) -> f64 {
567 let mut chi_sq = 0.0;
568
569 for i in 0..10 {
570 chi_sq += self.residuals[i] * self.residuals[i];
571 }
572
573 chi_sq
574 }
575
576 fn calculate_jacobian<F>(&mut self, f: &F, x_data: &[f64; 10]) -> SolverResult<()>
578 where
579 F: CurveFitFunction,
580 {
581 for i in 0..10 {
582 let jacobian_row = f.jacobian(x_data[i], &self.parameters);
583 for j in 0..4 {
584 self.jacobian[j][i] = jacobian_row[j];
585 }
586 }
587
588 Ok(())
589 }
590
591 fn solve_parameter_update(&mut self) -> SolverResult<()> {
593 let mut jtj = [[0.0; 4]; 4];
595 for i in 0..4 {
596 for j in 0..4 {
597 let mut sum = 0.0;
598 for k in 0..10 {
599 sum += self.jacobian[i][k] * self.jacobian[j][k];
600 }
601 jtj[i][j] = sum;
602 }
603 jtj[i][i] += self.lambda;
605 }
606
607 let mut jtr = [0.0; 4];
609 for i in 0..4 {
610 let mut sum = 0.0;
611 for k in 0..10 {
612 sum += self.jacobian[i][k] * self.residuals[k];
613 }
614 jtr[i] = sum;
615 }
616
617 self.solve_4x4_system(&jtj, &jtr)
619 }
620
621 fn solve_4x4_system(&mut self, matrix: &[[f64; 4]; 4], rhs: &[f64; 4]) -> SolverResult<()> {
623 let mut a = *matrix;
625 let mut b = *rhs;
626 let mut pivot = [0; 4];
627
628 for i in 0..4 {
630 let mut max_row = i;
632 let mut max_val = a[i][i].abs();
633
634 for j in i + 1..4 {
635 if a[j][i].abs() > max_val {
636 max_val = a[j][i].abs();
637 max_row = j;
638 }
639 }
640
641 if max_val < 1e-10 {
642 return Err(ExecutionError::SingularMatrix);
643 }
644
645 if max_row != i {
647 for k in 0..4 {
648 let temp = a[i][k];
649 a[i][k] = a[max_row][k];
650 a[max_row][k] = temp;
651 }
652 let temp = b[i];
653 b[i] = b[max_row];
654 b[max_row] = temp;
655 }
656
657 pivot[i] = max_row;
658
659 for j in i + 1..4 {
661 let factor = a[j][i] / a[i][i];
662 a[j][i] = factor;
663
664 for k in i + 1..4 {
665 a[j][k] -= factor * a[i][k];
666 }
667 b[j] -= factor * b[i];
668 }
669 }
670
671 for i in (0..4).rev() {
673 let mut sum = b[i];
674 for j in i + 1..4 {
675 sum -= a[i][j] * self.delta_parameters[j];
676 }
677 self.delta_parameters[i] = sum / a[i][i];
678 }
679
680 Ok(())
681 }
682
683 fn check_convergence(&self, old_chi_squared: f64) -> bool {
685 let relative_change = (old_chi_squared - self.chi_squared).abs() / old_chi_squared;
686 relative_change < self.config.tolerance
687 }
688
689 pub fn get_parameters(&self) -> [f64; 4] {
691 self.parameters
692 }
693}
694
695impl Default for OptimizationState {
696 fn default() -> Self {
697 Self {
698 iteration: 0,
699 objective_value: f64::MAX,
700 converged: false,
701 simplex_size: f64::MAX,
702 }
703 }
704}
705
706impl Default for RootFindingState {
707 fn default() -> Self {
708 Self {
709 iteration: 0,
710 function_value: f64::MAX,
711 converged: false,
712 step_size: f64::MAX,
713 }
714 }
715}
716
717impl Default for CurveFitState {
718 fn default() -> Self {
719 Self {
720 iteration: 0,
721 chi_squared: f64::MAX,
722 converged: false,
723 lambda: 1e-3,
724 }
725 }
726}
727
728impl Default for NelderMeadSimplex {
729 fn default() -> Self {
730 Self::new([0.0; 4], SolverConfig::default())
731 }
732}
733
734impl Default for BoundedNewtonRaphson {
735 fn default() -> Self {
736 Self::new(0.0, -1e6, 1e6, SolverConfig::default())
737 }
738}
739
740impl Default for LevenbergMarquardtStack {
741 fn default() -> Self {
742 Self::new([0.0; 4], SolverConfig::default())
743 }
744}
745
746#[cfg(test)]
747mod tests {
748 use super::*;
749
750 struct QuadraticFunction;
752
753 impl ObjectiveFunction for QuadraticFunction {
754 fn evaluate(&self, params: &[f64; 4]) -> f64 {
755 (params[0] - 1.0).powi(2)
756 + (params[1] - 2.0).powi(2)
757 + (params[2] - 3.0).powi(2)
758 + (params[3] - 4.0).powi(2)
759 }
760 }
761
762 #[test]
763 fn test_nelder_mead_simplex() {
764 let mut optimizer = NelderMeadSimplex::new([0.0; 4], SolverConfig::default());
765 let func = QuadraticFunction;
766
767 let result = optimizer.optimize(&func);
768 assert!(result.is_ok());
769
770 let state = result.unwrap();
771 assert!(state.converged);
772
773 let (params, value) = optimizer.get_best_solution();
774 assert!((params[0] - 1.0).abs() < 0.1);
775 assert!((params[1] - 2.0).abs() < 0.1);
776 assert!((params[2] - 3.0).abs() < 0.1);
777 assert!((params[3] - 4.0).abs() < 0.1);
778 assert!(value < 0.1);
779 }
780
781 struct CubicFunction;
783
784 impl RootFunction for CubicFunction {
785 fn evaluate(&self, x: f64) -> f64 {
786 x.powi(3) - 2.0 * x - 5.0
787 }
788
789 fn derivative(&self, x: f64) -> f64 {
790 3.0 * x * x - 2.0
791 }
792 }
793
794 #[test]
795 fn test_bounded_newton_raphson() {
796 let mut solver = BoundedNewtonRaphson::new(2.0, -10.0, 10.0, SolverConfig::default());
797 let func = CubicFunction;
798
799 let result = solver.find_root(&func);
800 assert!(result.is_ok());
801
802 let state = result.unwrap();
803 assert!(state.converged);
804
805 let root = solver.get_root();
806 assert!((root - 2.094).abs() < 0.01); }
808
809 struct PolynomialFit;
811
812 impl CurveFitFunction for PolynomialFit {
813 fn evaluate(&self, x: f64, params: &[f64; 4]) -> f64 {
814 params[0] + params[1] * x + params[2] * x * x + params[3] * x * x * x
815 }
816
817 fn jacobian(&self, x: f64, _params: &[f64; 4]) -> [f64; 4] {
818 [1.0, x, x * x, x * x * x]
819 }
820 }
821
822 #[test]
823 fn test_levenberg_marquardt_stack() {
824 let mut optimizer =
825 LevenbergMarquardtStack::new([1.0, 1.0, 1.0, 1.0], SolverConfig::default());
826
827 let mut x_data = [0.0; 10];
829 let mut y_data = [0.0; 10];
830
831 for i in 0..10 {
832 x_data[i] = i as f64;
833 y_data[i] = 2.0
834 + 3.0 * x_data[i]
835 + 0.5 * x_data[i] * x_data[i]
836 + 0.1 * x_data[i] * x_data[i] * x_data[i];
837 }
838
839 let func = PolynomialFit;
840 let result = optimizer.fit_curve(&func, &x_data, &y_data);
841 assert!(result.is_ok());
842
843 let state = result.unwrap();
844 assert!(state.converged);
845
846 let params = optimizer.get_parameters();
847 assert!((params[0] - 2.0).abs() < 0.1);
848 assert!((params[1] - 3.0).abs() < 0.1);
849 }
850
851 #[test]
852 fn test_zero_allocation_guarantee() {
853 }
857}