use ndarray::prelude::*;

use super::Kernel;

#[derive(Clone)]
pub struct Matern32 {
    var: f64,
    l_scale: f64,
    lambda: f64,
}

impl Matern32 {
    pub fn new(var: f64, l_scale: f64) -> Self {
        Matern32 {
            var,
            l_scale,
            lambda: 3.0f64.sqrt() / l_scale,
        }
    }
}

impl Kernel for Matern32 {
    fn k_mat(&self, _ts1: &[f64], _ts2: Option<&[f64]>) -> ArrayD<f64> {
        unimplemented!();
    }

    fn k_diag(&self, ts: &[f64]) -> Array1<f64> {
        Array1::ones(ts.len()) * self.var
    }

    fn order(&self) -> usize {
        2
    }

    fn state_mean(&self, _t: f64) -> Array1<f64> {
        Array1::zeros(2)
    }

    fn state_cov(&self, _t: f64) -> Array2<f64> {
        let a = self.lambda;

        array![[1.0, 0.0], [0.0, a * a]] * self.var
    }

    fn measurement_vector(&self) -> Array1<f64> {
        array![1.0, 0.0]
    }

    fn feedback(&self) -> Array2<f64> {
        let a = self.lambda;

        array![[0.0, 1.0], [-a.powi(2), -2.0 * a]]
    }

    fn transition(&self, t0: f64, t1: f64) -> Array2<f64> {
        let d = t1 - t0;
        let a = self.lambda;

        let ba = array![[d * a + 1.0, d], [-d * a * a, 1.0 - d * a]];

        (-d * a).exp() * ba
    }

    fn noise_cov(&self, t0: f64, t1: f64) -> Array2<f64> {
        let d = t1 - t0;
        let a = self.lambda;
        let da = d * a;

        let c = (-2.0 * da).exp();

        let x11 = 1.0 - c * (2.0 * da * da + 2.0 * da + 1.0);
        let x12 = c * (2.0 * da * da * a);
        let x22 = a * a * (1.0 - c * (2.0 * da * da - 2.0 * da + 1.0));

        self.var * array![[x11, x12], [x12, x22]]
    }
}