//! # CasADi Rust interface
//!
//! This is a Rust interface to C functions generated by Rust
//!
//! This is a `no-std` library (however, mind that the CasADi-generated code
//! requires `libm` to call functions such as `sqrt`, `sin`, etc...)

#![no_std]

extern crate libc;
use libc::{c_double, c_int};

extern "C" {

    fn icasadi_num_static_parameters() -> c_int;

    fn icasadi_num_decision_variables() -> c_int;

    fn icasadi_cost_(
        u: *const c_double,
        casadi_static_params: *const c_double,
        cost_value: *mut c_double,
    ) -> c_int;

    fn icasadi_grad_(
        u: *const c_double,
        casadi_static_params: *const c_double,
        cost_jacobian: *mut c_double,
    ) -> c_int;
}

///
/// Number of static parameters
///
pub fn num_static_parameters() -> usize {
    unsafe { icasadi_num_static_parameters() as usize }
}

///
/// Number of decision variables
///
pub fn num_decision_variables() -> usize {
    unsafe { icasadi_num_decision_variables() as usize }
}

///
/// Consume the cost function written in C
///
/// # Example
/// ```
/// fn tst_call_casadi_cost() {
///     let u = [1.0, 2.0, 3.0, -5.0, 1.0, 10.0, 14.0, 17.0, 3.0, 5.0];
///     let p = [1.0, -1.0];
///     let mut cost_value = 0.0;
///     icasadi::icasadi_cost(&u, &p, &mut cost_value);
/// }
/// ```   
///
/// # Panics
/// This method does not panic (on purpose). However, users need to be
/// careful when providing the arguments `u` and `casadi_static_params`
/// as they must be arrays of appropriate size.
///
/// As a safety measure, you may check whether
///
/// - `u.len() >= icasadi::num_decision_variables()`
/// - `casadi_static_params.len() >= icasadi::num_static_parameters()`
///
pub fn icasadi_cost(u: &[f64], casadi_static_params: &[f64], cost_value: &mut f64) -> c_int {
    unsafe { icasadi_cost_(u.as_ptr(), casadi_static_params.as_ptr(), cost_value) }
}

///
/// Consume the Jacobian function written in C
///
/// # Example
/// ```
/// fn tst_call_casadi_cost() {
///     let u = [1.0, 2.0, 3.0, -5.0, 1.0, 10.0, 14.0, 17.0, 3.0, 5.0];
///     let p = [1.0, -1.0];
///     let mut jac = [0.0; 10];
///     icasadi::icasadi_grad(&u, &p, &mut jac);
/// }
/// ```
///
/// # Panics
/// This method does not panic (on purpose). However, users need to be
/// careful when providing the arguments `u` and `casadi_static_params`
/// as they must be arrays of appropriate size.
///
/// As a safety measure, you may check whether
///
/// - `u.len() >= icasadi::num_decision_variables()`
/// - `casadi_static_params.len() >= icasadi::num_static_parameters()`
/// - `cost_jacobian.len() >= icasadi::num_decision_variables()`
///
pub fn icasadi_grad(u: &[f64], casadi_static_params: &[f64], cost_jacobian: &mut [f64]) -> c_int {
    unsafe {
        icasadi_grad_(
            u.as_ptr(),
            casadi_static_params.as_ptr(),
            cost_jacobian.as_mut_ptr(),
        )
    }
}

#[inline(always)]
fn norm2_squared(a: &[f64]) -> f64 {
    a.iter().map(|&x: &f64| -> f64 { x * x }).sum::<f64>()
}

///
/// Estimates a local squared Lipschitz constant for the gradient of the cost
/// 
/// Input arguments:
///
/// - `u` point where the squared Lipschitz constant is estimated
/// - `p` parameter
/// - `jac` a slice which is updated with the Jacobian matrix of 
///    the cost function at `u`; the provided value of `jac` is 
///    not used
/// - `workspace` externally allocated workspace memory
///
/// Output arguments:
///
/// - local squared Lipschitz constant (at point `(u, p)`)
///
/// # Method
///
/// This function computes a numerical approximation of the norm of the directional
/// derivative of `gradf` along a direction `h = max {delta, epsilon*u}`, where `delta`
/// and `epsilon` are small numbers.
///
/// # Example
///
/// ```
/// let mut u: [f64; 10] = [1.0, 2.0, 3.0, -5.0, 1.0, 10.0, 14.0, 17.0, 3.0, 5.0];
/// let p = [1.0, -1.0];
/// let mut cost_value = 0.0;
/// let mut jac = vec![0.0; icasadi::num_decision_variables()];
/// let lip = icasadi::estimate_local_lipschitz_squared(&mut u, &p, &mut jac, &mut [0.0_f64; 10]);
/// ```
///
/// # Panics
/// No rust-side panics, unless the C function which is called via this interface
/// fails.
///
/// # Why squared?
/// We are trying to keep most crated under `#![no_std]` and the square root is not
/// supported in `no_std` mode.
pub fn estimate_local_lipschitz_squared(
    u: &mut [f64],
    p: &[f64],
    jac: &mut [f64],
    workspace: &mut [f64],
) -> f64 {
    icasadi_grad(u, p, jac);

    let epsilon_lip = 1e-10;
    let delta_lip = 1e-10;

    // Use `workspace` to store h
    workspace.iter_mut().zip(u.iter()).for_each(|(out, &s)| {
        *out = if epsilon_lip * s > delta_lip {
            epsilon_lip * s
        } else {
            delta_lip
        }
    });
    let norm_h = norm2_squared(&workspace);

    // store u + h into u
    u.iter_mut()
        .zip(workspace.iter())
        .for_each(|(out, a)| *out = *out + *a);

    icasadi_grad(u, p, workspace);

    // compute the norm of workspace - jac
    workspace
        .iter_mut()
        .zip(jac.iter())
        .for_each(|(out, a)| *out = *out - *a);

    let norm_workspace = norm2_squared(&workspace);
    norm_workspace / norm_h
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn tst_num_static() {
        let np = num_static_parameters();
        assert_eq!(np, 2);
    }

    #[test]
    fn tst_num_decision_var() {
        let np = num_decision_variables();
        assert_eq!(np, 10);
    }

    #[test]
    fn tst_call_casadi_cost() {
        let u = [1.0, 2.0, 3.0, -5.0, 1.0, 10.0, 14.0, 17.0, 3.0, 5.0];
        let p = [1.0, -1.0];
        let mut cost_value = 0.0;
        icasadi_cost(&u, &p, &mut cost_value);
        assert!((68.9259 - cost_value).abs() < 1e-4);
    }

    #[test]
    fn tst_call_casadi_grad() {
        let u = [1.0, 2.0, 3.0, -5.0, 1.0, 10.0, 14.0, 17.0, 3.0, 5.0];
        let p = [1.0, -1.0];
        let mut jac = [0.0; 10];
        icasadi_grad(&u, &p, &mut jac);
        assert!((jac[0] - 0.5270086046800542).abs() < 1e-6);
        assert!((jac[5] + 6.9744770099018885).abs() < 1e-6);
    }
}