optimization_engine 0.12.0

use super::super::*;
use super::*;
use crate::constraints;
use crate::core::fbs::fbs_engine::FBSEngine;
use std::num::NonZeroUsize;

const N_DIM: usize = 2;

#[cfg(test)]
use crate::mocks;

fn solve_with_optimizer_trait(
    optimizer: &mut impl crate::core::Optimizer<f32>,
    u: &mut [f32],
) -> Result<crate::core::SolverStatus<f32>, crate::SolverError> {
    optimizer.solve(u)
}

#[test]
fn t_solve_fbs_hard() {
    let bounds = constraints::NoConstraints::new();

    let problem = Problem::new(
        &bounds,
        mocks::hard_quadratic_gradient,
        mocks::hard_quadratic_cost,
    );
    let gamma = 0.0005;
    let tolerance = 1e-6;

    let mut fbs_cache = FBSCache::new(NonZeroUsize::new(3).unwrap(), gamma, tolerance);
    let mut u = [-12., -160., 55.];
    let mut optimizer = FBSOptimizer::new(problem, &mut fbs_cache).with_max_iter(100_000);
    let status = optimizer.solve(&mut u).unwrap();

    println!("|fpr| = {}", status.norm_fpr());
    println!("solution = {:?}", u);
    assert!(status.has_converged());
    assert!(status.norm_fpr() < tolerance);
}

#[test]
fn t_solve_fbs_hard_failure_nan() {
    let bounds = constraints::NoConstraints::new();

    let problem = Problem::new(
        &bounds,
        mocks::hard_quadratic_gradient,
        mocks::hard_quadratic_cost,
    );
    let gamma = 0.005;
    let tolerance = 1e-6;

    let mut fbs_cache = FBSCache::new(NonZeroUsize::new(3).unwrap(), gamma, tolerance);
    let mut u = [-12., -160., 55.];
    let mut optimizer = FBSOptimizer::new(problem, &mut fbs_cache).with_max_iter(10000);
    let status = optimizer.solve(&mut u);
    assert!(matches!(status, Err(SolverError::NotFiniteComputation(_))));
}

#[test]
fn t_fbs_step_no_constraints() {
    let no_constraints = constraints::NoConstraints::new();
    let problem = Problem::new(&no_constraints, mocks::my_gradient, mocks::my_cost);
    let gamma = 0.1;
    let tolerance = 1e-6;

    let mut fbs_cache = FBSCache::new(NonZeroUsize::new(N_DIM).unwrap(), gamma, tolerance);
    {
        let mut fbs_engine = FBSEngine::new(problem, &mut fbs_cache);
        let mut u = [1.0, 3.0];

        assert!(fbs_engine.step(&mut u).unwrap());
        unit_test_utils::assert_nearly_equal_array(&[0.5, 2.4], &u, 1e-10, 1e-14, "u");
    }
    unit_test_utils::assert_nearly_equal_array(
        &[1., 3.],
        &fbs_cache.work_u_previous,
        1e-10,
        1e-14,
        "fbs_cache.work_u_previous",
    );
}

#[test]
fn t_fbs_step_ball_constraints() {
    let no_constraints = constraints::Ball2::new(None, 0.1);
    let problem = Problem::new(&no_constraints, mocks::my_gradient, mocks::my_cost);
    let gamma = 0.1;
    let tolerance = 1e-6;

    let mut fbs_cache = FBSCache::new(NonZeroUsize::new(N_DIM).unwrap(), gamma, tolerance);
    let mut fbs_engine = FBSEngine::new(problem, &mut fbs_cache);

    let mut u = [1.0, 3.0];

    assert!(fbs_engine.step(&mut u).unwrap());
    unit_test_utils::assert_nearly_equal_array(
        &[0.020_395_425_411_200, 0.097_898_041_973_761],
        &u,
        1e-8,
        1e-14,
        "u",
    );
}

#[test]
fn t_solve_fbs() {
    let radius = 0.2;
    let box_constraints = constraints::Ball2::new(None, radius);
    let problem = Problem::new(&box_constraints, mocks::my_gradient, mocks::my_cost);
    let gamma = 0.1;
    let tolerance = 1e-6;

    let mut fbs_cache = FBSCache::new(NonZeroUsize::new(N_DIM).unwrap(), gamma, tolerance);
    let mut u = [0.0; N_DIM];
    let mut optimizer = FBSOptimizer::new(problem, &mut fbs_cache);

    let status = optimizer.solve(&mut u).unwrap();

    assert!(status.has_converged());
    assert!(status.norm_fpr() < tolerance);

    unit_test_utils::assert_nearly_equal_array(&mocks::solution_a(), &u, 1e-4, 1e-5, "u");
}

#[test]
fn t_solve_fbs_many_times() {
    // Algorithm configuration
    let gamma = 0.1;
    let tolerance = 1e-6;

    // The cache is constructed ONCE. This step allocates memory.
    let mut fbs_cache = FBSCache::new(NonZeroUsize::new(N_DIM).unwrap(), gamma, tolerance);

    let mut u = [0.0; 2];

    for _i in 1..10 {
        // Every time NMPC is executed, the constraints may change
        let box_constraints = constraints::Ball2::new(None, 0.2);

        // The problem is surely update at every execution of NMPC
        let problem = Problem::new(&box_constraints, mocks::my_gradient, mocks::my_cost);

        // Here comes the new initial condition
        u[0] = 2.0 * _i as f64;
        u[1] = -_i as f64;

        // Create a new optimizer...
        let mut optimizer = FBSOptimizer::new(problem, &mut fbs_cache);

        let status = optimizer.solve(&mut u).unwrap();

        assert!(status.norm_fpr() < tolerance);
    }
}

#[test]
fn t_fbs_step_no_constraints_f32() {
    let no_constraints = constraints::NoConstraints::new();
    let problem = Problem::new(
        &no_constraints,
        |u: &[f32], grad: &mut [f32]| -> FunctionCallResult {
            grad[0] = u[0] + u[1] + 1.0;
            grad[1] = u[0] + 2.0 * u[1] - 1.0;
            Ok(())
        },
        |u: &[f32], cost: &mut f32| -> FunctionCallResult {
            *cost = u[0] * u[0] + 2.0 * u[1] * u[1] + u[0] - u[1] + 3.0;
            Ok(())
        },
    );
    let gamma = 0.1_f32;
    let tolerance = 1e-6_f32;

    let mut fbs_cache = FBSCache::<f32>::new(NonZeroUsize::new(N_DIM).unwrap(), gamma, tolerance);
    let mut fbs_engine = FBSEngine::new(problem, &mut fbs_cache);
    let mut u = [1.0_f32, 3.0_f32];

    assert!(fbs_engine.step(&mut u).unwrap());
    assert!((u[0] - 0.5_f32).abs() < 1e-6);
    assert!((u[1] - 2.4_f32).abs() < 1e-6);
}

#[test]
fn t_solve_fbs_f32() {
    let radius = 0.2_f32;
    let box_constraints = constraints::Ball2::new(None, radius);
    let problem = Problem::new(
        &box_constraints,
        |u: &[f32], grad: &mut [f32]| -> FunctionCallResult {
            grad[0] = u[0] + u[1] + 1.0;
            grad[1] = u[0] + 2.0 * u[1] - 1.0;
            Ok(())
        },
        |u: &[f32], cost: &mut f32| -> FunctionCallResult {
            *cost = u[0] * u[0] + 2.0 * u[1] * u[1] + u[0] - u[1] + 3.0;
            Ok(())
        },
    );
    let gamma = 0.1_f32;
    let tolerance = 1e-6_f32;

    let mut fbs_cache = FBSCache::<f32>::new(NonZeroUsize::new(N_DIM).unwrap(), gamma, tolerance);
    let mut u = [0.0_f32; N_DIM];
    let mut optimizer = FBSOptimizer::new(problem, &mut fbs_cache);

    let status = optimizer.solve(&mut u).unwrap();

    assert!(status.has_converged());
    let expected = crate::mocks::solution_a::<f32>();
    assert!(status.norm_fpr() < tolerance);
    assert!((u[0] - expected[0]).abs() < 1e-4);
    assert!((u[1] - expected[1]).abs() < 1e-4);
}

#[test]
fn t_solve_fbs_f32_via_optimizer_trait() {
    let radius = 0.2_f32;
    let box_constraints = constraints::Ball2::new(None, radius);
    let problem = Problem::new(
        &box_constraints,
        |u: &[f32], grad: &mut [f32]| -> FunctionCallResult {
            grad[0] = u[0] + u[1] + 1.0;
            grad[1] = u[0] + 2.0 * u[1] - 1.0;
            Ok(())
        },
        |u: &[f32], cost: &mut f32| -> FunctionCallResult {
            *cost = u[0] * u[0] + 2.0 * u[1] * u[1] + u[0] - u[1] + 3.0;
            Ok(())
        },
    );
    let gamma = 0.1_f32;
    let tolerance = 1e-6_f32;

    let mut fbs_cache = FBSCache::<f32>::new(NonZeroUsize::new(N_DIM).unwrap(), gamma, tolerance);
    let mut u = [0.0_f32; N_DIM];
    let mut optimizer = FBSOptimizer::new(problem, &mut fbs_cache);

    let status = solve_with_optimizer_trait(&mut optimizer, &mut u).unwrap();

    assert!(status.has_converged());
    assert!(status.norm_fpr() < tolerance);
}