#include <iostream>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include<vector>
#include <complex.h>
#include <algorithm>
#include "IIR_Butterworth.h"

//#define ARMA_DONT_USE_CXX11
#define ARMA_DONT_USE_CXX11_MUTEX
#include <armadillo>

#define PI 3.141592653589793

//Global variables
double fs = 2;
double u_f1;
double u_f2;
double Wn;
double Bw;

std::vector<std::complex<double>> ptemp;
std::complex<double> complex_real(1.0, 0.0);
std::complex<double> complex_imag(0.0, 1.0);
std::vector<std::complex<double>> p;

int temp_dim_arr_matr;
std::vector<std::vector<std::complex<double>> > a;
std::vector<std::complex<double>> b;
std::vector<std::complex<double>> c;

double d[1];
double t[1];

arma::cx_mat a_arma;
arma::cx_mat b_arma;
arma::cx_mat c_arma;
arma::cx_mat d_arma;

arma::cx_mat t1_arma;
arma::cx_mat t2_arma;
arma::cx_mat ad_arma;
arma::cx_mat bd_arma;
arma::cx_mat cd_arma;
arma::cx_mat dd_arma;

std::vector<double> num_filt;   //Vector where to temporarily save the numerator
std::vector<double> den_filt;   // Vector where to temporarily save the denumerator
std::vector<std::vector<double> > save_filt_coeff;  //Matrix where to save the numerator and denominator. First row is the numerator; second row is the denominator

using namespace IIR_B_F;

//Step 1: get analog, pre - warped frequencies
void IIR_Butterworth::freq_pre_wrapped(int type_filt, double Wnf_1, double Wnf_2)
{

    Bw = 0;

    switch (type_filt)
    {

        //Band-pass
    case 0:

        u_f1 = 2 * fs * tan(PI * Wnf_1 / fs);
        u_f2 = 2 * fs * tan(PI * Wnf_2 / fs);

        break;

        //Band-stop
    case 1:

        u_f1 = 2 * fs * tan(PI * Wnf_1 / fs);
        u_f2 = 2 * fs * tan(PI * Wnf_2 / fs);

        break;

        //High-pass
    case 2:

        u_f1 = 2 * fs * tan(PI * Wnf_1 / fs);

        break;

        //Low-pass
    case 3:

        u_f2 = 2 * fs * tan(PI * Wnf_2 / fs);

        break;

    }
}

//Step 2: convert to low-pass prototype estimate
void IIR_Butterworth::Wn_f1_Wn_f2(int type_filt, double u_f1, double u_f2)
{

    switch (type_filt)
    {

        //Band-pass
    case 0:
        Bw = u_f2 - u_f1;
        Wn = sqrt(u_f1 * u_f2);

        break;

        //Band-stop
    case 1:

        Bw = u_f2 - u_f1;
        Wn = sqrt(u_f1 * u_f2);

        break;

        //Low-pass
    case 2:

        Wn = u_f1;

        break;

        //High-pass
    case 3:

        Wn = u_f2;

        break;

    }
}

//Step 3: Get N - th order Butterworth analog lowpass prototype
void IIR_Butterworth::buttap(int order_filt)
{
    double order_filt_exp = (double)order_filt;
    int temp_length_vec = 0;
    int kkk = 1;
    do {

        temp_length_vec++;
        kkk += 2;

    } while (kkk <= order_filt - 1);

    //Initialize the vector "ptemp"
    for (int i = 0; i < temp_length_vec; i++)
    {

        ptemp.push_back(0);

    }

    int track_cell = 0;
    for (double kk = 0; kk < (double)order_filt - 1; kk += 2)
    {

        ptemp.at(track_cell) = exp(complex_imag * ((PI * (kk + 1)) / (2 * order_filt_exp) + PI / 2));

        track_cell++;

    }

    //Initialize the vector "p"
    for (int i = 0; i < order_filt; i++)

    {

        p.push_back(0);

    }

    double temp_rem = remainder(order_filt, 2);

    if (temp_rem != 0)
    {

        for (int kk = 0; kk < order_filt; kk++)
        {

            if (kk < order_filt - 1)
            {

                p.at(kk) = complex_imag;

            }

            else
            {

                p.at(kk) = -complex_real;

            }

        }

    }

    else
    {

        for (int kk = 0; kk < order_filt; kk++)
        {

            p.at(kk) = complex_imag;

        }

    }

    if (order_filt > 1)
    {
        track_cell = 0;
        for (int kk = 0; kk < temp_length_vec * 2; kk += 2)
        {

            p.at(kk) = ptemp.at(track_cell);
            p.at(kk + 1) = conj(ptemp.at(track_cell));

            track_cell++;

        }
    }

}

//Step 4: Transform to state-space
//Intermidiate step: calculate the coefficients of the polynomial (based on Matlab code)
std::vector<std::complex<double>> IIR_Butterworth::poly(std::vector<std::complex<double>> temp_array_poly, int col_poly)
{
    std::vector<std::complex<double>> coeff_pol_f(col_poly + 1);
    coeff_pol_f.at(0) = 1;

    for (int ll = 0; ll < col_poly; ll++)
    {

        int yy = 0;

        do
        {

            coeff_pol_f.at(ll + 1 - yy) = coeff_pol_f.at(ll + 1 - yy) - temp_array_poly.at(ll) * coeff_pol_f.at(ll - yy);
            yy++;

        } while (yy <= ll);

    }

    return coeff_pol_f;

}

//Calculate the factorial of the given number
int IIR_Butterworth::factorial(int i)
{

    if (i <= 1)
    {

        return 1;

    }

    return i * factorial(i - 1);

}

//Method to calculate all the possible combinations. Code taken and slightly modified from:
//http://rosettacode.org/wiki/Combinations#C.2B.2B
std::vector<std::vector<int> > IIR_Butterworth::combination_method(int N, int K, int comb_n)
{

    int rows_f = 0;

    std::vector<std::vector<int> > matrix_comb_f;

        std::vector<int> temp_v;

        for (int ff = 0; ff < K; ff++)
        {

            temp_v.push_back(0);

        }

        for (int hh = 0; hh < comb_n; hh++)
        {

            matrix_comb_f.push_back(temp_v);

        }

    std::string bitmask(K, 1); // K leading 1's
    bitmask.resize(N, 0); // N-K trailing 0's

    int col_f;
    do {

        col_f = 0;

        for (int i = 0; i < N; ++i) // [0..N-1] integers
        {

            if (bitmask[i])

            {

                matrix_comb_f[rows_f][col_f] = i;

                col_f++;

            }
        }

        rows_f++;

    } while (std::prev_permutation(bitmask.begin(), bitmask.end()));

    return matrix_comb_f;

}

//Intermidiate step: calculate the coefficients of the polynomial (Bernard Brooks' paper (2016))
std::vector<std::complex<double>> IIR_Butterworth::char_poly(arma::cx_mat temp_matr_poly, int row_col)
{

    std::vector<std::complex<double>> coeff_pol_ff(row_col + 1);
    arma::cx_mat temp_val = arma::zeros <arma::cx_mat>(1,1);
    double num_det;
    arma::cx_mat temp_matr = arma::zeros <arma::cx_mat>(row_col, row_col);

    std::vector<std::vector<int> > matrix_comb;

    for (int kk = 0; kk < row_col + 1; kk++)

    {

        if (kk == 0)
        {

            coeff_pol_ff[row_col - kk] = pow(-1, row_col) * arma::det(temp_matr_poly);

        }

        else
        {

            temp_val(0, 0) = 0;

            num_det = (double)factorial(row_col) / (double)(factorial(row_col - kk) * (double)factorial(kk));  //Calculate the number of combinations

        //Catching the situation where the denominator is zero. Assign the default value of 10^10 to the denominator
            if (isnan(num_det))
            {

                for (int ff = 0; ff < row_col + 1; ff++)
                {

                    coeff_pol_ff[ff] = pow(10, 10);

                }

                break;

            }

            if (num_det < 1)    //Handling the situation where numerical overflow generates a negative or zero value for the num_det because of the instability of the filter
            {

                for (int ff = 0; ff < row_col + 1; ff++)
                {

                    coeff_pol_ff[ff] = pow(10, 10);

                }

                break;

            }

            std::vector<int> temp_v;

            for (int ff = 0; ff < num_det; ff++)
            {

                temp_v.push_back(0);

            }

            for (int hh = 0; hh < num_det; hh++)
            {

                matrix_comb.push_back(temp_v);

            }

            if (num_det - (int)num_det == 0)
            {
                // Generate the combinations
                matrix_comb = combination_method(row_col, kk, (int)num_det);

                for (int mm = 0; mm < num_det; mm++)

                {

                    temp_matr = temp_matr_poly;

                    for (int pp = 0; pp < row_col; pp++)
                    {

                        temp_matr(matrix_comb[mm][0], pp) = 0;
                        temp_matr(pp, matrix_comb[mm][0]) = 0;
                        temp_matr(matrix_comb[mm][0], matrix_comb[mm][0]) = -1;

                    }

                    for (int nn = 1; nn < kk; nn++)
                    {

                        for (int pp = 0; pp < row_col; pp++)
                        {

                            temp_matr(matrix_comb[mm][nn], pp) = 0;
                            temp_matr(pp, matrix_comb[mm][nn]) = 0;
                            temp_matr(matrix_comb[mm][nn], matrix_comb[mm][nn]) = -1;

                        }

                    }

                    temp_val(0, 0) += det(temp_matr);

                }

                coeff_pol_ff[row_col - kk] = pow(-1, row_col) * temp_val(0, 0);

            }

            else
            {

                for (int ff = 0; ff < row_col + 1; ff++)
                {

                    coeff_pol_ff[ff] = pow(10, 10);

                }

                break;

            }

        }

    }

    return coeff_pol_ff;

}

//Step 4: Transform to state-space
void IIR_Butterworth::zp2ss(int order_filt)
{

    //Order the pairs of complex conjugate. The pairs with the smallest real part come first. Within pairs, the ones with the negative imaginary part comes first
    std::complex<double> temp_max;

    //Using the selection sort algorithm to order based on the real part
    double order_filt_d = (double)order_filt;
    double temp_rem;
    if (remainder(order_filt, 2) == 0)
    {

        temp_rem = order_filt_d;

    }

    else
    {

        temp_rem = order_filt_d - 1;

    }

    int min_real;
    for (int kk = 0; kk < temp_rem - 1; kk++)
    {
        min_real = kk;

        for (int jj = kk + 1; jj < temp_rem; jj++)
        {
            if (real(p[jj]) < real(p[min_real]))
            {

                min_real = jj;

                temp_max = p[kk];
                p[kk] = p[min_real];
                p[min_real] = temp_max;

            }
        }
    }

    //Using the selection sort algorithm to order the values based on the imaginary part
    for (int kk = 0; kk < temp_rem - 1; kk += 2)
    {
        min_real = kk;

        if (imag(p[kk]) > imag(p[kk + 1]))
        {

            min_real = kk + 1;

            temp_max = p[kk];
            p[kk] = p[min_real];
            p[min_real] = temp_max;

        }
    }

    // Initialize state - space matrices for running series
    d[0] = 1;

    int track_index = 1;

    // Take care of any left over unmatched pole pairs.
        // H(s) = 1 / (s ^ 2 + den(2)s + den(3))
    std::vector<std::complex<double>> temp_poly_p;

    double wn = 1;

    if (order_filt > 1)
    {

        double b1[2] = { 1,0 };
        double c1[2] = { 0,1 };
        double d1 = 0;

        temp_rem = remainder(order_filt, 2);
        int order_filt_temp;
        int dim_matr;
        std::vector<std::vector<std::complex<double>> > temp_matrix_a;    //Temporary matrix where to save the coefficients at each interaction
        int coeff_numb = 3;

        if (temp_rem == 0)
        {

            order_filt_temp = order_filt;
            dim_matr = order_filt_temp / 2;

                std::vector<std::complex<double>> temp_v;

                for (int ll = 0; ll < coeff_numb; ll++)
                {

                    temp_v.push_back(0);

                }

                for (int kk = 0; kk < dim_matr; kk++)
                {

                    temp_matrix_a.push_back(temp_v);

                }

        }

        else
        {

            order_filt_temp = order_filt - 1;
            dim_matr = order_filt_temp / 2;

                std::vector<std::complex<double>> temp_v;

                for (int ll = 0; ll < coeff_numb; ll++)
                {

                    temp_v.push_back(0);

                }

                for (int kk = 0; kk < dim_matr; kk++)
                {

                    temp_matrix_a.push_back(temp_v);

                }

        }

        int track_cycles = 0;
        int temp_val_pos_check;
        int dim_poly_p = 2;

        for (int i = 0; i < dim_poly_p; i++)
        {

            temp_poly_p.push_back(0);

        }

        temp_dim_arr_matr = dim_poly_p + (order_filt % 2);

        while (track_index < order_filt_temp)
        {

            for (int rr = track_index - 1; rr < track_index + 1; rr++)
            {

                temp_poly_p[rr - track_index + 1] = p[rr];

            }

            std::vector<std::complex<double>> coeff_pol(dim_poly_p + 1);

            coeff_pol = poly(temp_poly_p, dim_poly_p);

            for (int qq = 0; qq < coeff_numb; qq++)
            {

                temp_matrix_a[track_cycles][qq] = -real(coeff_pol[qq]);

            }

            //Update the state-space arrays/matrix
            track_cycles += 1;

                std::vector<std::complex<double>> temp_v;

                for (int ll = 0; ll < temp_dim_arr_matr; ll++)
                {

                    temp_v.push_back(0);

                }

                for (int kk = 0; kk < temp_dim_arr_matr; kk++)
                {

                    a.push_back(temp_v);

                }

            int track_index_coeff = 0;

            if (remainder(order_filt, 2) == 0)
            {
                ///////////////////////////////////////////////////////////////////////////////////////////////
                //Even number of poles
                for (int kk = 0; kk < temp_dim_arr_matr; kk++)
                {

                    for (int gg = 0; gg < temp_dim_arr_matr; gg++)

                    {

                        temp_val_pos_check = kk - gg;

                        switch (temp_val_pos_check)
                        {
                        case 1:

                            a[kk][gg] = 1;

                            break;

                        case 0:

                            if ((remainder(kk + 1, 2) != 0) || (kk == 0))
                            {

                                a[kk][gg] = temp_matrix_a[track_index_coeff][1];

                            }

                            else
                            {

                                a[kk][gg] = 0;

                            }

                            break;

                        case -1:

                            if ((remainder(kk + 1, 2) != 0) || (kk == 0))
                            {

                                a[kk][gg] = temp_matrix_a[track_index_coeff][2];
                                track_index_coeff++;

                            }

                            else
                            {

                                a[kk][gg] = 0;

                            }
                            break;

                        default:

                            a[kk][gg] = 0;

                            break;

                        }

                    }

                }
                ///////////////////////////////////////////////////////////////////////////////////////////////
            }

            else
            {
                ///////////////////////////////////////////////////////////////////////////////////////////////
                //Odd number of poles
                for (int kk = 0; kk < temp_dim_arr_matr; kk++)
                {

                    for (int gg = 0; gg < temp_dim_arr_matr; gg++)

                    {

                        temp_val_pos_check = kk - gg;

                        switch (temp_val_pos_check)
                        {
                        case 1:

                            a[kk][gg] = 1;

                            break;

                        case 0:

                            if (kk == 0)
                            {

                                a[kk][gg] = -1;

                            }

                            else
                            {

                                if ((remainder(kk + 1, 2) == 0))
                                {

                                    a[kk][gg] = temp_matrix_a[track_index_coeff][1];

                                }

                                else
                                {

                                    a[kk][gg] = 0;

                                }
                            }

                            break;

                        case -1:

                            if ((remainder(kk + 1, 2) == 0))
                            {

                                a[kk][gg] = temp_matrix_a[track_index_coeff][2];
                                track_index_coeff++;

                            }

                            else
                            {

                                a[kk][gg] = 0;

                            }
                            break;

                        default:

                            a[kk][gg] = 0;

                            break;

                        }

                    }

                }

            }
             ///////////////////////////////////////////////////////////////////////////////////////////////

             //Initialize the vectors "b" and "c"
            for (int i = 0; i < temp_dim_arr_matr; i++)
            {

                b.push_back(0);
                c.push_back(0);

            }

            for (int kk = 0; kk < temp_dim_arr_matr; kk++)
            {

                if (kk == 0)
                {

                    b[kk] = 1;

                }

                else
                {

                    b[kk] = 0;

                }

            }

            for (int kk = 0; kk < temp_dim_arr_matr; kk++)
            {

                if (kk == temp_dim_arr_matr - 1)
                {

                    c[kk] = 1;

                }

                else
                {

                    c[kk] = 0;

                }

            }

            track_index += 2;

            if (track_index < order_filt_temp)
            {

                //Clean up the matrix "a" and the arrays "b" and "c", so they can be re-initialized
                a.erase(a.begin(), a.begin() + temp_dim_arr_matr);
                b.erase(b.begin(), b.begin() + temp_dim_arr_matr);
                c.erase(c.begin(), c.begin() + temp_dim_arr_matr);

            }

            dim_matr += 2;
            temp_dim_arr_matr += 2;

        }

    }

    else
    {

        std::vector<std::complex<double>> temp_v;
        temp_v.push_back(0);
        a.push_back(temp_v);
        a[0][0] = p[0];

        b.push_back(1);
        c.push_back(1);

    }

    d[0] = 0;

}

//Extract the coefficients of the band pass filter
std::vector<std::vector<double> > IIR_Butterworth::lp2bp(double W_f1, double W_f2, int order_filt)
{

    //Check that the low cut-off frequency is higher than the low cut-off frequency
    if (W_f2 <= W_f1)
    {

        throw new std::exception("The low cut-off frequency needs to be higher than the low cut-off frequency");

    }

    //Check that the normalized frequencies are within the correct range of values
    if ((W_f1 <= 0) || (W_f1 >= 1) || (W_f2 <= 0) || (W_f2 >= 1))
    {

        throw new std::exception("Cut-off frequencies must be in the (0,1) range");

    }

    //Check that the order of the filter is > 0
    if (order_filt <= 0)
    {

        throw new std::exception("The order of the filter must be > 0");

    }

    //Clean up the global variables for a new analysis
    if (save_filt_coeff.size() > 0)
    {

        save_filt_coeff.erase(save_filt_coeff.begin(), save_filt_coeff.begin() + save_filt_coeff.size());
        ptemp.erase(ptemp.begin(), ptemp.begin() + ptemp.size());
        p.erase(p.begin(), p.begin() + p.size());

        a.erase(a.begin(), a.begin() + a.size());
        b.erase(b.begin(), b.begin() + b.size());
        c.erase(c.begin(), c.begin() + c.size());

        num_filt.erase(num_filt.begin(), num_filt.begin() + num_filt.size());
        den_filt.erase(den_filt.begin(), den_filt.begin() + den_filt.size());

    }

        std::vector<double> temp_v;

        for (int ff = 0; ff < 2*order_filt + 1; ff++)
        {

            temp_v.push_back(0);

        }

        for (int hh = 0; hh < 2; hh++)
        {

            save_filt_coeff.push_back(temp_v);

        }

    int type_filt = 0;

    //Step 1: get analog, pre - warped frequencies
    freq_pre_wrapped(type_filt, W_f1, W_f2);

    //Step 2: convert to low-pass prototype estimate
    Wn_f1_Wn_f2(type_filt, u_f1, u_f2);

    //Step 3: Get N - th order Butterworth analog lowpass prototype
    buttap(order_filt);

    //Step 4: Transform to state-space
    zp2ss(order_filt);

    if (order_filt > 1)
    {

        temp_dim_arr_matr -= 2;

    }

    else
    {

        temp_dim_arr_matr = order_filt;

    }

    //Copy the values of the matrix/arrays "arma" matrix/array in order to compute the pseudo-inverse of the matrix and other matrix operations
    a_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 2 * temp_dim_arr_matr);
    arma::cx_mat a_arma_p_eye = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    a_arma_p_eye = a_arma_p_eye.eye();
    arma::cx_mat a_arma_n_eye = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    a_arma_n_eye = -a_arma_p_eye.eye();
    b_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 1);
    c_arma = arma::zeros <arma::cx_mat>(1, 2 * temp_dim_arr_matr);
    d_arma = arma::zeros <arma::cx_mat>(1, 1);

    double q = Wn / Bw;
    for (int kk = 0; kk < 2 * temp_dim_arr_matr; kk++)
    {
        if (kk < temp_dim_arr_matr)
        {

            b_arma(kk, 0) = b[kk] * Wn / q;
            c_arma(0, kk) = c[kk];

        }

        for (int ll = 0; ll < 2 * temp_dim_arr_matr; ll++)
        {

            if (kk < temp_dim_arr_matr)
            {

                if (ll < temp_dim_arr_matr)

                {

                    a_arma(kk, ll) = Wn * a[kk][ll] / q;

                }

                else
                {

                    a_arma(kk, ll) = Wn * a_arma_p_eye(kk, ll - temp_dim_arr_matr);

                }
            }

            else
            {
                if (ll < temp_dim_arr_matr)
                {

                    a_arma(kk, ll) = Wn * a_arma_n_eye(kk - temp_dim_arr_matr, ll);

                }
            }

        }

    }

    d_arma = d_arma;

    //Step 5: Use Bilinear transformation to find discrete equivalent
    bilinear(a_arma, b_arma, c_arma, d_arma, fs, type_filt);

    //Step 6: Transform to zero-pole-gain and polynomial forms
    zero_pole_gain(ad_arma, type_filt, order_filt, Wn, Bw);

    return save_filt_coeff;

}

//Extract the coefficients of the band stop filter
std::vector<std::vector<double> > IIR_Butterworth::lp2bs(double W_f1, double W_f2, int order_filt)
{

    //Check that the low cut-off frequency is higher than the low cut-off frequency
    if (W_f2 <= W_f1)
    {

        throw new std::exception("The low cut-off frequency needs to be higher than the low cut-off frequency");

    }

    //Check that the normalized frequencies are within the correct range of values
    if ((W_f1 <= 0) || (W_f1 >= 1) || (W_f2 <= 0) || (W_f2 >= 1))
    {

        throw new std::exception("Cut-off frequencies must be in the (0,1) range");

    }

    //Check that the order of the filter is > 0
    if (order_filt <= 0)
    {

        throw new std::exception("The order of the filter must be > 0");

    }

    //Clean up the global variables for a new analysis
    if (save_filt_coeff.size() > 0)
    {

        save_filt_coeff.erase(save_filt_coeff.begin(), save_filt_coeff.begin() + save_filt_coeff.size());
        ptemp.erase(ptemp.begin(), ptemp.begin() + ptemp.size());
        p.erase(p.begin(), p.begin() + p.size());

        a.erase(a.begin(), a.begin() + a.size());
        b.erase(b.begin(), b.begin() + b.size());
        c.erase(c.begin(), c.begin() + c.size());

        num_filt.erase(num_filt.begin(), num_filt.begin() + num_filt.size());
        den_filt.erase(den_filt.begin(), den_filt.begin() + den_filt.size());

    }

        std::vector<double> temp_v;

        for (int ff = 0; ff < 2 * order_filt + 1; ff++)
        {

            temp_v.push_back(0);

        }

        for (int hh = 0; hh < 2; hh++)
        {

            save_filt_coeff.push_back(temp_v);

        }

    int type_filt = 1;

    //Step 1: get analog, pre - warped frequencies
    freq_pre_wrapped(type_filt, W_f1, W_f2);

    //Step 2: convert to low-pass prototype estimate
    Wn_f1_Wn_f2(type_filt, u_f1, u_f2);

    //Step 3: Get N - th order Butterworth analog lowpass prototype
    buttap(order_filt);

    //Step 4: Transform to state-space
    zp2ss(order_filt);

    if (order_filt > 1)
    {

        temp_dim_arr_matr -= 2;

    }

    else
    {

        temp_dim_arr_matr = order_filt;

    }

    //Copy the values of the matrix/arrays "arma" matrix/array in order to compute the pseudo-inverse of the matrix and other matrix operations
    a_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 2 * temp_dim_arr_matr);
    arma::cx_mat a_arma_p_eye = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    a_arma_p_eye = a_arma_p_eye.eye();
    arma::cx_mat a_arma_n_eye = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    a_arma_n_eye = -a_arma_p_eye.eye();
    b_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 1);
    c_arma = arma::zeros <arma::cx_mat>(1, 2 * temp_dim_arr_matr);
    d_arma = arma::zeros <arma::cx_mat>(1, 1);

    arma::cx_mat a_arma_pinv = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    arma::cx_mat b_arma_temp = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, 1);
    arma::cx_mat c_arma_temp = arma::zeros <arma::cx_mat>(1, temp_dim_arr_matr);

    arma::cx_mat a_b_arma_temp = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, 1);
    arma::cx_mat c_a_arma_temp = arma::zeros <arma::cx_mat>(1, temp_dim_arr_matr);

    for (int kk = 0; kk < temp_dim_arr_matr; kk++)
    {
        b_arma_temp(kk, 0) = b[kk];
        c_arma_temp(0, kk) = c[kk];

        for (int ll = 0; ll < temp_dim_arr_matr; ll++)
        {

            a_arma_pinv(kk, ll) = a[kk][ll];

        }

    }

    double q = Wn / Bw;

    try
    {

        a_arma_pinv = (Wn / q) * arma::pinv(a_arma_pinv);

    }

    catch (std::runtime_error)
    {

        for (int kk = 0; kk < 2; kk++)
        {

            for (int hh = 0; hh < 2 * order_filt + 1; hh++)
            {

                save_filt_coeff[kk][hh] = pow(10, 10);

            }

        }

        return save_filt_coeff;

    }

    a_b_arma_temp = (Wn) * (a_arma_pinv * b_arma_temp) / q;
    c_a_arma_temp = c_arma_temp * a_arma_pinv;

    for (int kk = 0; kk < 2 * temp_dim_arr_matr; kk++)
    {
        if (kk < temp_dim_arr_matr)
        {

            b_arma(kk, 0) = -a_b_arma_temp(kk, 0);
            c_arma(0, kk) = c_a_arma_temp(0, kk);

        }

        for (int ll = 0; ll < 2 * temp_dim_arr_matr; ll++)
        {

            if (kk < temp_dim_arr_matr)
            {

                if (ll < temp_dim_arr_matr)

                {

                    a_arma(kk, ll) = a_arma_pinv(kk, ll);

                }

                else
                {

                    a_arma(kk, ll) = Wn * a_arma_p_eye(kk, ll - temp_dim_arr_matr);

                }
            }

            else
            {
                if (ll < temp_dim_arr_matr)
                {

                    a_arma(kk, ll) = Wn * a_arma_n_eye(kk - temp_dim_arr_matr, ll);

                }
            }

        }

    }

    d_arma = d[0] + c_a_arma_temp * b_arma_temp;

    //Step 5: Use Bilinear transformation to find discrete equivalent
    bilinear(a_arma, b_arma, c_arma, d_arma, fs, type_filt);

    //Step 6: Transform to zero-pole-gain and polynomial forms
    zero_pole_gain(ad_arma, type_filt, order_filt, Wn, Bw);

    return save_filt_coeff;

}

//Extract the coefficients of the high pass filter
std::vector<std::vector<double> > IIR_Butterworth::lp2hp(double W_f2, int order_filt)
{

    //Check that the normalized frequencies are within the correct range of values
    if ((W_f2 <= 0) || (W_f2 >= 1))
    {

        throw new std::exception("Cut-off frequencies must be in the (0,1) range");

    }

    //Check that the order of the filter is > 0
    if (order_filt <= 0)
    {

        throw new std::exception("The order of the filter must be > 0");

    }

    //Clean up the global variables for a new analysis
    if (save_filt_coeff.size() > 0)
    {

        save_filt_coeff.erase(save_filt_coeff.begin(), save_filt_coeff.begin() + save_filt_coeff.size());
        ptemp.erase(ptemp.begin(), ptemp.begin() + ptemp.size());
        p.erase(p.begin(), p.begin() + p.size());

        a.erase(a.begin(), a.begin() + a.size());
        b.erase(b.begin(), b.begin() + b.size());
        c.erase(c.begin(), c.begin() + c.size());

        num_filt.erase(num_filt.begin(), num_filt.begin() + num_filt.size());
        den_filt.erase(den_filt.begin(), den_filt.begin() + den_filt.size());

    }

    std::vector<double> temp_v;

    for (int ff = 0; ff < 2 * order_filt; ff++)
    {

        temp_v.push_back(0);

    }

    for (int hh = 0; hh < 2; hh++)
    {

        save_filt_coeff.push_back(temp_v);

    }

    int type_filt = 2;

    //Step 1: get analog, pre - warped frequencies
    freq_pre_wrapped(type_filt, W_f2, 0);

    //Step 2: convert to low-pass prototype estimate
    Wn_f1_Wn_f2(type_filt, u_f1, u_f2);

    //Step 3: Get N - th order Butterworth analog lowpass prototype
    buttap(order_filt);

    //Step 4: Transform to state-space
    zp2ss(order_filt);

    if (order_filt > 1)
    {

        temp_dim_arr_matr -= 2;

    }

    else
    {

        temp_dim_arr_matr = order_filt;

    }

    //Copy the values of the matrix/arrays "arma" matrix/array in order to compute the pseudo-inverse of the matrix and other matrix operations
    a_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    b_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, 1);
    c_arma = arma::zeros <arma::cx_mat>(1, temp_dim_arr_matr);
    d_arma = arma::zeros <arma::cx_mat>(1, 1);

    for (int kk = 0; kk < temp_dim_arr_matr; kk++)
    {
        b_arma(kk, 0) = b[kk];
        c_arma(0, kk) = c[kk];

        for (int ll = 0; ll < temp_dim_arr_matr; ll++)
        {

            a_arma(kk, ll) = a[kk][ll];

        }

    }

    try
    {

        d_arma = d_arma - (c_arma * arma::pinv(a_arma)) * b_arma;

    }

    catch (std::runtime_error)
    {

        for (int kk = 0; kk < 2; kk++)
        {

            for (int hh = 0; hh < order_filt + 1; hh++)
            {

                save_filt_coeff[kk][hh] = pow(10, 10);

            }

        }

        return save_filt_coeff;

    }

    c_arma = c_arma * arma::pinv(a_arma);
    b_arma = -Wn * arma::pinv(a_arma) * b_arma;
    a_arma = Wn * arma::pinv(a_arma);

    //Step 5: Use Bilinear transformation to find discrete equivalent
    bilinear(a_arma, b_arma, c_arma, d_arma, fs, type_filt);

    //Step 6: Transform to zero-pole-gain and polynomial forms
    zero_pole_gain(ad_arma, type_filt, order_filt, Wn, Bw);

    return save_filt_coeff;

}

//Extract the coefficients of the low pass filter
std::vector<std::vector<double> > IIR_Butterworth::lp2lp(double W_f1, int order_filt)
{
    //Check that the normalized frequencies are within the correct range of values
    if ((W_f1 <= 0) || (W_f1 >= 1))
    {

        throw new std::exception("Cut-off frequencies must be in the (0,1) range");

    }

    //Check that the order of the filter is > 0
    if (order_filt <= 0)
    {

        throw new std::exception("The order of the filter must be > 0");

    }

    //Clean up the global variables for a new analysis
    if (save_filt_coeff.size() > 0)
    {

        save_filt_coeff.erase(save_filt_coeff.begin(), save_filt_coeff.begin() + save_filt_coeff.size());
        ptemp.erase(ptemp.begin(), ptemp.begin() + ptemp.size());
        p.erase(p.begin(), p.begin() + p.size());

        a.erase(a.begin(), a.begin() + a.size());
        b.erase(b.begin(), b.begin() + b.size());
        c.erase(c.begin(), c.begin() + c.size());

        num_filt.erase(num_filt.begin(), num_filt.begin() + num_filt.size());
        den_filt.erase(den_filt.begin(), den_filt.begin() + den_filt.size());

    }

        std::vector<double> temp_v;

        for (int ff = 0; ff < 2 * order_filt; ff++)
        {

            temp_v.push_back(0);

        }

        for (int hh = 0; hh < 2; hh++)
        {

            save_filt_coeff.push_back(temp_v);

        }

    int type_filt = 3;

    //Step 1: get analog, pre - warped frequencies
    freq_pre_wrapped(type_filt, 0, W_f1);

    //Step 2: convert to low-pass prototype estimate
    Wn_f1_Wn_f2(type_filt, u_f1, u_f2);

    //Step 3: Get N - th order Butterworth analog lowpass prototype
    buttap(order_filt);

    //Step 4: Transform to state-space
    zp2ss(order_filt);

    if (order_filt > 1)
    {

        temp_dim_arr_matr -= 2;

    }

    else
    {

        temp_dim_arr_matr = order_filt;

    }

    //Copy the values of the matrix/arrays "arma" matrix/array in order to compute the pseudo-inverse of the matrix and other matrix operations
    a_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    b_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, 1);
    c_arma = arma::zeros <arma::cx_mat>(1, temp_dim_arr_matr);
    d_arma = arma::zeros <arma::cx_mat>(1, 1);

    for (int kk = 0; kk < temp_dim_arr_matr; kk++)
    {
        b_arma(kk, 0) = b[kk];
        c_arma(0, kk) = c[kk];

        for (int ll = 0; ll < temp_dim_arr_matr; ll++)
        {

            a_arma(kk, ll) = a[kk][ll];

        }

    }

    d_arma = d_arma;
    c_arma = c_arma;
    b_arma = Wn * b_arma;
    a_arma = Wn * a_arma;

    //Step 5: Use Bilinear transformation to find discrete equivalent
    bilinear(a_arma, b_arma, c_arma, d_arma, fs, type_filt);

    //Step 6: Transform to zero-pole-gain and polynomial forms
    zero_pole_gain(ad_arma, type_filt, order_filt, Wn, Bw);

    return save_filt_coeff;

}

//Step 5: Use Bilinear transformation to find discrete equivalent
void IIR_Butterworth::bilinear(arma::cx_mat a_arma_f, arma::cx_mat b_arma_f, arma::cx_mat c_arma_f, arma::cx_mat d_arma_f, double fs_f, int type_filt_f)
{

    double t_arma;
    double r_arma;

    if (type_filt_f > 1)
    {
        t1_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
        t2_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
        ad_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
        bd_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, 1);
        cd_arma = arma::zeros <arma::cx_mat>(1, temp_dim_arr_matr);
        dd_arma = arma::zeros <arma::cx_mat>(1, 1);
    }

    else
    {

        t1_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 2 * temp_dim_arr_matr);
        t2_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 2 * temp_dim_arr_matr);
        ad_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 2 * temp_dim_arr_matr);
        bd_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 1);
        cd_arma = arma::zeros <arma::cx_mat>(1, 2 * temp_dim_arr_matr);
        dd_arma = arma::zeros <arma::cx_mat>(1, 1);

    }

    try
    {
        t_arma = (1 / fs_f);
        r_arma = sqrt(t_arma);
        t1_arma = t1_arma.eye() + a_arma_f * t_arma * 0.5; //t1_arma.eye()
        t2_arma = t2_arma.eye() - a_arma_f * t_arma * 0.5;
        ad_arma = t1_arma * arma::pinv(t2_arma);
        bd_arma = (t_arma / r_arma) * arma::solve(t2_arma, b_arma_f);
        cd_arma = (r_arma * c_arma_f) * arma::pinv(t2_arma);
        dd_arma = (c_arma_f * arma::pinv(t2_arma)) * b_arma_f * (t_arma / 2) + d_arma_f;
    }

    catch (std::runtime_error)
    {

    }
}

//Step 6: Transform to zero-pole-gain and polynomial forms
void IIR_Butterworth::zero_pole_gain(arma::cx_mat a_arma_f, int type_filt_f, int order_filt_f, double Wn_f_f, double Bw_f)
{

    int dim_array;

    if (type_filt_f > 1)
    {

        //Initialize the vectors "num_filt" and "den_filt"
        for (int i = 0; i < order_filt_f + 1; i++)
        {

            num_filt.push_back(0);
            den_filt.push_back(0);

        }

        dim_array = temp_dim_arr_matr;

    }

    else
    {

        //Initialize the vectors "num_filt" and "den_filt"
        for (int i = 0; i < 2 * order_filt_f + 1; i++)
        {

            num_filt.push_back(0);
            den_filt.push_back(0);

        }

        dim_array = 2 * temp_dim_arr_matr;

    }

    //Extract the coefficients of the denumerator
    std::vector<std::complex<double>> coeff_pol(temp_dim_arr_matr + 1);

    if (type_filt_f > 1)

    {

        coeff_pol = char_poly(a_arma_f, temp_dim_arr_matr);

    }

    else
    {

        coeff_pol = char_poly(a_arma_f, 2 * temp_dim_arr_matr);

    }

    for (int qq = 0; qq < dim_array + 1; qq++)
    {

        den_filt[qq] = real(coeff_pol[qq]);
        save_filt_coeff[1][qq] = den_filt[qq];

    }

    //Extract the coefficients of the denominator
    double w;
    Wn = 2 * std::atan2(Wn, 4);
    std::vector<std::complex<double>> r;

    switch (type_filt_f)
    {

    case 0: // band-pass

        for (int i = 0; i <= dim_array; i++)
        {

            r.push_back(0);

        }

        for (int kk = 0; kk < dim_array; kk++)
        {

            if (kk < temp_dim_arr_matr)
            {

                r[kk] = 1;

            }

            else
            {

                r[kk] = -1;

            }

        }

        w = Wn;

        break;

    case 1: // band-stop

        for (int i = 0; i <= dim_array; i++)
        {

            r.push_back(0);

        }

        for (int kk = 0; kk < dim_array; kk++)
        {

            r[kk] = exp(complex_imag * Wn * pow(-1, kk));

        }
        w = 0;

        break;

    case 2: //high-pass

        for (int i = 0; i <= dim_array; i++)
        {

            r.push_back(0);

        }

        for (int kk = 0; kk < dim_array; kk++)
        {

            r[kk] = 1;

        }

        w = PI;
        break;

    case 3: // low-pass

        for (int i = 0; i <= dim_array; i++)
        {

            r.push_back(0);

        }

        for (int kk = 0; kk < dim_array; kk++)
        {

            r[kk] = -1;

        }
        w = 0;
        break;

    default:
        for (int i = 0; i <= dim_array; i++)
        {

            r.push_back(0);

        }

    }

    std::vector<std::complex<double>> coeff_pol_num(dim_array + 1);

    coeff_pol_num = poly(r, dim_array);

    std::vector<std::complex<double>> kern(dim_array + 1);

    for (int kk = 0; kk < dim_array + 1; kk++)
    {

        kern[kk] = exp(-complex_imag * w * double(kk));

    }

    std::complex<double> temp_sum_I;
    std::complex<double> temp_sum_II;

     for (int kk = 0; kk < dim_array + 1; kk++)
    {

        temp_sum_I = 0.0;
        temp_sum_II = 0.0;

        for (int hh = 0; hh < dim_array + 1; hh++)
        {

            temp_sum_I += kern[hh] * den_filt[hh];
            temp_sum_II += kern[hh] * coeff_pol_num[hh];

        }

        num_filt[kk] = real(coeff_pol_num[kk] * temp_sum_I / temp_sum_II);
        save_filt_coeff[0][kk] = num_filt[kk];

    }

}
    //Check the stability of the filter
    bool IIR_Butterworth::check_stability_iir(std::vector<std::vector<double> > coeff_filt)
    {
        bool stability_flag = true;

        //Calculate the roots
        arma::mat roots_den_matrix = arma::zeros(coeff_filt[1].size() - 1, coeff_filt[1].size() - 1);
        for (int kk = 0; kk < coeff_filt[1].size() - 2; kk++)
        {

            roots_den_matrix(kk + 1, kk) = 1;

        }

         for (int kk = 0; kk < coeff_filt[1].size() - 1; kk++)
        {

            roots_den_matrix(0, kk) = -coeff_filt[1][kk + 1];

        }

        std::vector<double> magnitude_roots_den (coeff_filt[1].size() - 1);
        arma::cx_vec roots_den;
        arma::cx_mat eigvec;

        arma::eig_gen(roots_den,eigvec, roots_den_matrix);

        for (int kk = 0; kk < coeff_filt[1].size() - 1; kk++)
        {

            magnitude_roots_den[kk] = abs(roots_den[kk]);

            if (magnitude_roots_den[kk] >= 1)
            {

                stability_flag = false;
                break;
            }

        }

        return stability_flag;

    }

//Filter the data by using the Direct-Form II Transpose, as explained in the Matlab documentation
std::vector<double> IIR_Butterworth::Filter_Data(std::vector<std::vector<double> > coeff_filt, std::vector<double> pre_filt_signal)
{

        std::vector<double> filt_signal(pre_filt_signal.size(), 0.0);

        std::vector<std::vector<double>> w_val;
        std::vector<double> temp_v;

        for (int ff = 0; ff < pre_filt_signal.size(); ff++)
        {

                temp_v.push_back(0);

        }

        for (int hh = 0; hh < coeff_filt[0].size(); hh++)
        {

                w_val.push_back(temp_v);

        }

        //Convolution product to filter the data
        for (int kk = 0; kk < pre_filt_signal.size(); kk++)
        {

                if (kk == 0)
                {

                        filt_signal[kk] = pre_filt_signal[kk] * coeff_filt[0][0];

                        for (int ww = 1; ww < coeff_filt[0].size(); ww++)
                        {

                                w_val[ww - 1][kk] = pre_filt_signal[kk] * coeff_filt[0][ww] - filt_signal[kk] * coeff_filt[1][ww];

        }

                }

                else
                {

                        filt_signal[kk] = pre_filt_signal[kk] * coeff_filt[0][0] + w_val[0][kk - 1];

                                for (int ww = 1; ww < coeff_filt[0].size(); ww++)
                                {

                                        w_val[ww - 1][kk] = pre_filt_signal[kk] * coeff_filt[0][ww] + w_val[ww][kk - 1] - filt_signal[kk] * coeff_filt[1][ww];

                                        if (ww == coeff_filt[0].size() - 1)
                                        {

                                                 w_val[ww - 1][kk] = pre_filt_signal[kk] * coeff_filt[0][ww] - filt_signal[kk] * coeff_filt[1][ww];

                                        }

                                }

                }

        }

        return filt_signal;

}


#include <iostream>
#include <armadillo>

using namespace arma;

int
main()
  {
  std::cout << "*** smoke test start" << std::endl;

  uword N = 5;

  mat A = reshape(regspace(1, N*N), N, N);

  A.diag() += randu<vec>(N);

  mat B;

  bool status = expmat(B,A);

  A.print("A:");
  B.print("B:");

  std::cout << ((status) ? "*** smoke test okay" : "*** smoke test failed") << std::endl;

  return (status) ? 0 : -1;
  }


#include "catch.hpp"

#include "libirc/mathtools.h"

#include <cmath> // nextafter

#ifdef HAVE_ARMA
#include <armadillo>
using vec3 = arma::vec3;
using vec = arma::vec;
using mat = arma::mat;
template<typename T>
using Mat = arma::Mat<T>;
#elif HAVE_EIGEN3
#include <Eigen/Dense>
using vec3 = Eigen::Vector3d;
using vec = Eigen::VectorXd;
using mat = Eigen::MatrixXd;
template<typename T>
using Mat = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
#else
#error
#endif

using namespace irc;

using tools::constants::pi;
using namespace tools::math;

TEST_CASE("pirange_rad produces angles in the interval (-pi,pi]") {

  const auto inclusive_angle = pi;
  CAPTURE(inclusive_angle);

  CHECK(pirange_rad(-pi) == Approx(inclusive_angle));
  CHECK(pirange_rad(0) == 0);
  CHECK(pirange_rad(pi) == Approx(inclusive_angle));
  CHECK(std::nextafter(pi, 0) < pi);
  CHECK(pirange_rad(std::nextafter(pi, 0)) == Approx(pi));
  CHECK(pirange_rad(std::nextafter(pi, 2 * pi)) == Approx(-pi));
  CHECK(pirange_rad(std::nextafter(-pi, -2 * pi)) == Approx(pi));
  CHECK(pirange_rad(std::nextafter(-pi, 0)) == Approx(-pi));

  CHECK(pirange_rad(2 * pi) == Approx(0));
  CHECK(pirange_rad(4 * pi) == Approx(0));
  CHECK(pirange_rad(6 * pi) == Approx(0));
  CHECK(pirange_rad(8 * pi) == Approx(0));
  CHECK(pirange_rad(1 * pi) == Approx(inclusive_angle));
  CHECK(pirange_rad(3 * pi) == Approx(inclusive_angle));
  CHECK(pirange_rad(5 * pi) == Approx(inclusive_angle));
  CHECK(pirange_rad(-1 * pi) == Approx(inclusive_angle));
  CHECK(pirange_rad(-3 * pi) == Approx(inclusive_angle));
  CHECK(pirange_rad(-5 * pi) == Approx(inclusive_angle));

  CHECK(pirange_rad(pi / 2.) == Approx(pi / 2.));
  CHECK(pirange_rad(-pi / 2.) == Approx(-pi / 2.));
  CHECK(pirange_rad(pi / 2. + pi) == Approx(-pi / 2.));
  CHECK(pirange_rad(-pi / 2. - pi) == Approx(pi / 2.));
}

TEST_CASE("pirange_deg produces angles in the interval (-180,180]") {

  const auto inclusive_angle = 180.0;
  CAPTURE(inclusive_angle);

  CHECK(pirange_deg(-180) == Approx(inclusive_angle));
  CHECK(pirange_deg(0) == 0);
  CHECK(pirange_deg(180) == Approx(inclusive_angle));
  CHECK(pirange_deg(std::nextafter(180, 0)) == Approx(180));
  CHECK(pirange_deg(std::nextafter(180, 2 * 180)) == Approx(-180));
  CHECK(pirange_deg(std::nextafter(-180, -2 * 180)) == Approx(180));
  CHECK(pirange_deg(std::nextafter(-180, 0)) == Approx(-180));

  CHECK(pirange_deg(2 * 180) == Approx(0));
  CHECK(pirange_deg(4 * 180) == Approx(0));
  CHECK(pirange_deg(6 * 180) == Approx(0));
  CHECK(pirange_deg(8 * 180) == Approx(0));
  CHECK(pirange_deg(1 * 180) == Approx(inclusive_angle));
  CHECK(pirange_deg(3 * 180) == Approx(inclusive_angle));
  CHECK(pirange_deg(5 * 180) == Approx(inclusive_angle));
  CHECK(pirange_deg(-1 * 180) == Approx(inclusive_angle));
  CHECK(pirange_deg(-3 * 180) == Approx(inclusive_angle));
  CHECK(pirange_deg(-5 * 180) == Approx(inclusive_angle));

  CHECK(pirange_deg(90) == Approx(90));
  CHECK(pirange_deg(-90) == Approx(-90));
  CHECK(pirange_deg(270) == Approx(-90));
  CHECK(pirange_deg(-270) == Approx(90));
}

TEST_CASE("collinear vectors") {

  vec3 v1 = {1., 0., 0.};
  vec3 v2 = {2., 0., 0.};
  vec3 v3 = {-0.5, 0., 0.};
  vec3 v4 = {1., 1., 1.};

  CHECK(collinear(v1, v2));
  CHECK(collinear(v1, v3));
  CHECK(collinear(v2, v3));

  CHECK(!collinear(v1, v4));
  CHECK(!collinear(v2, v4));
  CHECK(!collinear(v3, v4));
}


// Copyright (C) 2006  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/matrix.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../stl_checked.h"
#include "../array.h"
#include "../rand.h"

#include "tester.h"
#include <dlib/memory_manager_stateless.h>
#include <dlib/array2d.h>

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    dlib::rand rnd;

    logger dlog("test.matrix");

    template <typename type>
    const matrix<type> rand_sp_banded(long n, long bw)
    {
        matrix<type> m = 10 * identity_matrix<type>(n);
        for (long row = 0; row < m.nr(); ++row)
        {
            for (long col = row; col < min(m.nc(), row + bw); ++col)
            {
                type r = rnd.get_random_double();
                m(row,col) += r;
                m(col,row) += r;
            }
        }

        return m;
    }

    void matrix_test (
    )
    /*!
        ensures
            - runs tests on the matrix stuff compliance with the specs
    !*/
    {
        typedef memory_manager_stateless<char>::kernel_2_2a MM;
        print_spinner();

        {
            matrix<complex<double>,2,2,MM> m;
            set_all_elements(m,complex<double>(1,2));
            DLIB_TEST((conj(m) == uniform_matrix<complex<double>,2,2>(conj(m(0,0)))));
            DLIB_TEST((real(m) == uniform_matrix<double,2,2>(1)));
            DLIB_TEST((imag(m) == uniform_matrix<double,2,2>(2)));
            DLIB_TEST_MSG((sum(abs(norm(m) - uniform_matrix<double,2,2>(5))) < 1e-10 ),norm(m));

        }

        {
            matrix<double,5,5,MM,column_major_layout> m(5,5);

            for (long r = 0; r < m.nr(); ++r)
            {
                for (long c = 0; c < m.nc(); ++c)
                {
                    m(r,c) = r*c;
                }
            }

            m = cos(exp(m));

            matrix<double> mi = pinv(m );
            DLIB_TEST(mi.nr() == m.nc());
            DLIB_TEST(mi.nc() == m.nr());
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,5>())));
            DLIB_TEST((equal(round_zeros(m*mi,0.000001) , identity_matrix<double,5>())));
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix(m))));
            DLIB_TEST((equal(round_zeros(m*mi,0.000001) , identity_matrix(m))));

            mi = pinv(m,1e-12);
            DLIB_TEST(mi.nr() == m.nc());
            DLIB_TEST(mi.nc() == m.nr());
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,5>())));
            DLIB_TEST((equal(round_zeros(m*mi,0.000001) , identity_matrix<double,5>())));
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix(m))));
            DLIB_TEST((equal(round_zeros(m*mi,0.000001) , identity_matrix(m))));

            m = diagm(diag(m));
            mi = pinv(diagm(diag(m)),1e-12);
            DLIB_TEST(mi.nr() == m.nc());
            DLIB_TEST(mi.nc() == m.nr());
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,5>())));
            DLIB_TEST((equal(round_zeros(m*mi,0.000001) , identity_matrix<double,5>())));
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix(m))));
            DLIB_TEST((equal(round_zeros(m*mi,0.000001) , identity_matrix(m))));

            mi = pinv(m,0);
            DLIB_TEST(mi.nr() == m.nc());
            DLIB_TEST(mi.nc() == m.nr());
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,5>())));
            DLIB_TEST((equal(round_zeros(m*mi,0.000001) , identity_matrix<double,5>())));
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix(m))));
            DLIB_TEST((equal(round_zeros(m*mi,0.000001) , identity_matrix(m))));

            m = diagm(diag(m));
            mi = pinv(diagm(diag(m)),0);
            DLIB_TEST(mi.nr() == m.nc());
            DLIB_TEST(mi.nc() == m.nr());
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,5>())));
            DLIB_TEST((equal(round_zeros(m*mi,0.000001) , identity_matrix<double,5>())));
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix(m))));
            DLIB_TEST((equal(round_zeros(m*mi,0.000001) , identity_matrix(m))));
        }
        {
            matrix<double,5,0,MM> m(5,5);

            for (long r = 0; r < m.nr(); ++r)
            {
                for (long c = 0; c < m.nc(); ++c)
                {
                    m(r,c) = r*c;
                }
            }

            m = cos(exp(m));

            matrix<double> mi = pinv(m );
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,5>())));
        }

        {
            matrix<double,0,5,MM> m(5,5);

            for (long r = 0; r < m.nr(); ++r)
            {
                for (long c = 0; c < m.nc(); ++c)
                {
                    m(r,c) = r*c;
                }
            }

            m = cos(exp(m));

            matrix<double> mi = pinv(m );
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,5>())));
        }

        {
            matrix<double> m(5,5);

            for (long r = 0; r < m.nr(); ++r)
            {
                for (long c = 0; c < m.nc(); ++c)
                {
                    m(r,c) = r*c;
                }
            }

            m = cos(exp(m));

            matrix<double> mi = pinv(m );
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,5>())));
        }

        {
            matrix<double,5,2,MM,column_major_layout> m;

            for (long r = 0; r < m.nr(); ++r)
            {
                for (long c = 0; c < m.nc(); ++c)
                {
                    m(r,c) = r*c;
                }
            }

            m = cos(exp(m));

            matrix<double> mi = pinv(m );
            DLIB_TEST(mi.nr() == m.nc());
            DLIB_TEST(mi.nc() == m.nr());
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,2>())));
        }

        {
            matrix<double> m(5,2);

            for (long r = 0; r < m.nr(); ++r)
            {
                for (long c = 0; c < m.nc(); ++c)
                {
                    m(r,c) = r*c;
                }
            }

            m = cos(exp(m));

            matrix<double> mi = pinv(m );
            DLIB_TEST(mi.nr() == m.nc());
            DLIB_TEST(mi.nc() == m.nr());
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,2>())));
        }

        {
            matrix<double,5,2,MM,column_major_layout> m;

            for (long r = 0; r < m.nr(); ++r)
            {
                for (long c = 0; c < m.nc(); ++c)
                {
                    m(r,c) = r*c;
                }
            }

            m = cos(exp(m));

            matrix<double> mi = trans(pinv(trans(m) ));
            DLIB_TEST(mi.nr() == m.nc());
            DLIB_TEST(mi.nc() == m.nr());
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,2>())));
        }

        {
            matrix<double> m(5,2);

            for (long r = 0; r < m.nr(); ++r)
            {
                for (long c = 0; c < m.nc(); ++c)
                {
                    m(r,c) = r*c;
                }
            }

            m = cos(exp(m));

            matrix<double> mi = trans(pinv(trans(m) ));
            DLIB_TEST(mi.nr() == m.nc());
            DLIB_TEST(mi.nc() == m.nr());
            DLIB_TEST((equal(round_zeros(mi*m,0.000001) , identity_matrix<double,2>())));
        }

        {
            matrix<long> a1(5,1);
            matrix<long,0,0,MM,column_major_layout> a2(1,5);
            matrix<long,5,1> b1(5,1);
            matrix<long,1,5> b2(1,5);
            matrix<long,0,1> c1(5,1);
            matrix<long,1,0> c2(1,5);
            matrix<long,0,1,MM,column_major_layout> d1(5,1);
            matrix<long,1,0,MM> d2(1,5);

            for (long i = 0; i < 5; ++i)
            {
                a1(i) = i;
                a2(i) = i;
                b1(i) = i;
                b2(i) = i;
                c1(i) = i;
                c2(i) = i;
                d1(i) = i;
                d2(i) = i;
            }

            DLIB_TEST(a1 == trans(a2));
            DLIB_TEST(a1 == trans(b2));
            DLIB_TEST(a1 == trans(c2));
            DLIB_TEST(a1 == trans(d2));

            DLIB_TEST(a1 == b1);
            DLIB_TEST(a1 == c1);
            DLIB_TEST(a1 == d1);

            DLIB_TEST(trans(a1) == c2);
            DLIB_TEST(trans(b1) == c2);
            DLIB_TEST(trans(c1) == c2);
            DLIB_TEST(trans(d1) == c2);

            DLIB_TEST(sum(a1) == 10);
            DLIB_TEST(sum(a2) == 10);
            DLIB_TEST(sum(b1) == 10);
            DLIB_TEST(sum(b2) == 10);
            DLIB_TEST(sum(c1) == 10);
            DLIB_TEST(sum(c2) == 10);
            DLIB_TEST(sum(d1) == 10);
            DLIB_TEST(sum(d2) == 10);

            const matrix<long> orig1 = a1;
            const matrix<long> orig2 = a2;

            ostringstream sout;
            serialize(a1,sout);
            serialize(a2,sout);
            serialize(b1,sout);
            serialize(b2,sout);
            serialize(c1,sout);
            serialize(c2,sout);
            serialize(d1,sout);
            serialize(d2,sout);

            DLIB_TEST(a1 == orig1);
            DLIB_TEST(a2 == orig2);
            DLIB_TEST(b1 == orig1);
            DLIB_TEST(b2 == orig2);
            DLIB_TEST(c1 == orig1);
            DLIB_TEST(c2 == orig2);
            DLIB_TEST(d1 == orig1);
            DLIB_TEST(d2 == orig2);

            set_all_elements(a1,99);
            set_all_elements(a2,99);
            set_all_elements(b1,99);
            set_all_elements(b2,99);
            set_all_elements(c1,99);
            set_all_elements(c2,99);
            set_all_elements(d1,99);
            set_all_elements(d2,99);

            DLIB_TEST(a1 != orig1);
            DLIB_TEST(a2 != orig2);
            DLIB_TEST(b1 != orig1);
            DLIB_TEST(b2 != orig2);
            DLIB_TEST(c1 != orig1);
            DLIB_TEST(c2 != orig2);
            DLIB_TEST(d1 != orig1);
            DLIB_TEST(d2 != orig2);

            istringstream sin(sout.str());

            deserialize(a1,sin);
            deserialize(a2,sin);
            deserialize(b1,sin);
            deserialize(b2,sin);
            deserialize(c1,sin);
            deserialize(c2,sin);
            deserialize(d1,sin);
            deserialize(d2,sin);

            DLIB_TEST(a1 == orig1);
            DLIB_TEST(a2 == orig2);
            DLIB_TEST(b1 == orig1);
            DLIB_TEST(b2 == orig2);
            DLIB_TEST(c1 == orig1);
            DLIB_TEST(c2 == orig2);
            DLIB_TEST(d1 == orig1);
            DLIB_TEST(d2 == orig2);

        }

        {
            matrix<double,1,0> a(5);
            matrix<double,0,1> b(5);
            matrix<double,1,5> c(5);
            matrix<double,5,1> d(5);
            DLIB_TEST(a.nr() == 1);
            DLIB_TEST(a.nc() == 5);
            DLIB_TEST(c.nr() == 1);
            DLIB_TEST(c.nc() == 5);

            DLIB_TEST(b.nc() == 1);
            DLIB_TEST(b.nr() == 5);
            DLIB_TEST(d.nc() == 1);
            DLIB_TEST(d.nr() == 5);
        }

        {
            matrix<double,1,0> a;
            matrix<double,0,1> b;
            matrix<double,1,5> c;
            matrix<double,5,1> d;

            a.set_size(5);
            b.set_size(5);
            c.set_size(5);
            d.set_size(5);

            DLIB_TEST(a.nr() == 1);
            DLIB_TEST(a.nc() == 5);
            DLIB_TEST(c.nr() == 1);
            DLIB_TEST(c.nc() == 5);

            DLIB_TEST(b.nc() == 1);
            DLIB_TEST(b.nr() == 5);
            DLIB_TEST(d.nc() == 1);
            DLIB_TEST(d.nr() == 5);
        }

        {
            matrix<double> a(1,5);
            matrix<double> b(5,1);

            set_all_elements(a,1);
            set_all_elements(b,1);

            a = a*b;

            DLIB_TEST(a(0) == 5);
        }

        {
            matrix<double,0,0,MM,column_major_layout> a(6,7);

            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = r*a.nc() + c;
                }
            }

            DLIB_TEST(rowm(a,1).nr() == 1);
            DLIB_TEST(rowm(a,1).nc() == a.nc());
            DLIB_TEST(colm(a,1).nr() == a.nr());
            DLIB_TEST(colm(a,1).nc() == 1);

            for (long c = 0; c < a.nc(); ++c)
            {
                DLIB_TEST( rowm(a,1)(c) == 1*a.nc() + c);
            }

            for (long r = 0; r < a.nr(); ++r)
            {
                DLIB_TEST( colm(a,1)(r) == r*a.nc() + 1);
            }

            rectangle rect(2, 1, 3+2-1, 2+1-1);
            DLIB_TEST(get_rect(a).contains(get_rect(a)));
            DLIB_TEST(get_rect(a).contains(rect));
            for (long r = 0; r < 2; ++r)
            {
                for (long c = 0; c < 3; ++c)
                {
                    DLIB_TEST(subm(a,1,2,2,3)(r,c) == (r+1)*a.nc() + c+2);
                    DLIB_TEST(subm(a,1,2,2,3) == subm(a,rect));
                    DLIB_TEST(subm_clipped(a,1,2,2,3) == subm(a,rect));
                    DLIB_TEST(subm_clipped(a,1,2,2,3) == subm_clipped(a,rect));
                }
            }

            DLIB_TEST(subm(a,rectangle()).nr() == 0);
            DLIB_TEST(subm(a,rectangle()).nc() == 0);

        }

        {
            array2d<double> a;
            a.set_size(6,7);

            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a[r][c] = r*a.nc() + c;
                }
            }

            DLIB_TEST(rowm(mat(a),1).nr() == 1);
            DLIB_TEST(rowm(mat(a),1).nc() == a.nc());
            DLIB_TEST(colm(mat(a),1).nr() == a.nr());
            DLIB_TEST(colm(mat(a),1).nc() == 1);

            for (long c = 0; c < a.nc(); ++c)
            {
                DLIB_TEST( rowm(mat(a),1)(c) == 1*a.nc() + c);
            }

            for (long r = 0; r < a.nr(); ++r)
            {
                DLIB_TEST( colm(mat(a),1)(r) == r*a.nc() + 1);
            }

            for (long r = 0; r < 2; ++r)
            {
                for (long c = 0; c < 3; ++c)
                {
                    DLIB_TEST(subm(mat(a),1,2,2,3)(r,c) == (r+1)*a.nc() + c+2);
                }
            }

        }

        {
            array2d<double> m;
            m.set_size(5,5);

            for (long r = 0; r < m.nr(); ++r)
            {
                for (long c = 0; c < m.nc(); ++c)
                {
                    m[r][c] = r*c;
                }
            }

            matrix<double> mi = pinv(cos(exp(mat(m))) );
            DLIB_TEST(mi.nr() == m.nc());
            DLIB_TEST(mi.nc() == m.nr());
            DLIB_TEST((equal(round_zeros(mi*cos(exp(mat(m))),0.000001) , identity_matrix<double,5>())));
            DLIB_TEST((equal(round_zeros(cos(exp(mat(m)))*mi,0.000001) , identity_matrix<double,5>())));
        }

        {
            matrix<long,5,5,MM,column_major_layout> m1, res;
            matrix<long,2,2> m2;

            set_all_elements(m1,0);

            long res_vals[] = {
                9, 9, 9, 9, 9,
                0, 1, 1, 0, 0,
                0, 1, 1, 0, 2,
                0, 0, 2, 2, 2,
                0, 0, 2, 2, 0
            };

            res = res_vals;

            set_all_elements(m2, 1);
            set_subm(m1, range(1,2), range(1,2)) = subm(m2,0,0,2,2);
            set_all_elements(m2, 2);
            set_subm(m1, 3,2,2,2) = m2;

            set_colm(m1,4) = trans(rowm(m1,4));
            set_rowm(m1,0) = 9;

            DLIB_TEST_MSG(m1 == res, "m1: \n" << m1 << "\nres: \n" << res);

            set_subm(m1,0,0,5,5) = m1*m1;
            DLIB_TEST_MSG(m1 == res*res, "m1: \n" << m1 << "\nres*res: \n" << res*res);

            m1 = res;
            set_subm(m1,1,1,2,2) = subm(m1,0,0,2,2);

            long res_vals2[] = {
                9, 9, 9, 9, 9,
                0, 9, 9, 0, 0,
                0, 0, 1, 0, 2,
                0, 0, 2, 2, 2,
                0, 0, 2, 2, 0
            };

            res = res_vals2;
            DLIB_TEST_MSG(m1 == res, "m1: \n" << m1 << "\nres: \n" << res);

        }

        {
            matrix<long,5,5> m1, res;
            matrix<long,2,2> m2;

            set_all_elements(m1,0);

            long res_vals[] = {
                9, 9, 9, 9, 9,
                0, 1, 1, 0, 0,
                0, 1, 1, 0, 2,
                0, 0, 2, 2, 2,
                0, 0, 2, 2, 0
            };

            res = res_vals;

            set_all_elements(m2, 1);
            set_subm(m1, rectangle(1,1,2,2)) = subm(m2,0,0,2,2);
            set_all_elements(m2, 2);
            set_subm(m1, 3,2,2,2) = m2;

            set_colm(m1,4) = trans(rowm(m1,4));
            set_rowm(m1,0) = 9;

            DLIB_TEST_MSG(m1 == res, "m1: \n" << m1 << "\nres: \n" << res);

            set_subm(m1,0,0,5,5) = m1*m1;
            DLIB_TEST_MSG(m1 == res*res, "m1: \n" << m1 << "\nres*res: \n" << res*res);

            m1 = res;
            set_subm(m1,1,1,2,2) = subm(m1,0,0,2,2);

            long res_vals2[] = {
                9, 9, 9, 9, 9,
                0, 9, 9, 0, 0,
                0, 0, 1, 0, 2,
                0, 0, 2, 2, 2,
                0, 0, 2, 2, 0
            };

            res = res_vals2;
            DLIB_TEST_MSG(m1 == res, "m1: \n" << m1 << "\nres: \n" << res);

        }

        {
            matrix<long,5,5> m1, res;
            matrix<long,2,2> m2;

            set_all_elements(m1,0);

            long res_vals[] = {
                9, 0, 3, 3, 0,
                9, 2, 2, 2, 0,
                9, 2, 2, 2, 0,
                4, 4, 4, 4, 4,
                9, 0, 3, 3, 0
            };
            long res_vals_c3[] = {
                9, 0, 3, 0,
                9, 2, 2, 0,
                9, 2, 2, 0,
                4, 4, 4, 4,
                9, 0, 3, 0
            };
            long res_vals_r2[] = {
                9, 0, 3, 3, 0,
                9, 2, 2, 2, 0,
                4, 4, 4, 4, 4,
                9, 0, 3, 3, 0
            };

            matrix<long> temp;

            res = res_vals;

            temp = matrix<long,4,5>(res_vals_r2);
            DLIB_TEST(remove_row<2>(res) == temp);
            DLIB_TEST(remove_row<2>(res)(3,3) == 3);
            DLIB_TEST(remove_row<2>(res).nr() == 4);
            DLIB_TEST(remove_row<2>(res).nc() == 5);
            DLIB_TEST(remove_row(res,2) == temp);
            DLIB_TEST(remove_row(res,2)(3,3) == 3);
            DLIB_TEST(remove_row(res,2).nr() == 4);
            DLIB_TEST(remove_row(res,2).nc() == 5);

            temp = matrix<long,5,5>(res_vals);
            temp = remove_row(res,2);
            DLIB_TEST((temp == matrix<long,4,5>(res_vals_r2)));
            temp = matrix<long,5,5>(res_vals);
            temp = remove_col(res,3);
            DLIB_TEST((temp == matrix<long,5,4>(res_vals_c3)));

            matrix<long,3,1> vect;
            set_all_elements(vect,1);
            temp = identity_matrix<long>(3);
            temp = temp*vect;
            DLIB_TEST(temp == vect);

            temp = matrix<long,5,4>(res_vals_c3);
            DLIB_TEST(remove_col(res,3) == temp);
            DLIB_TEST(remove_col(res,3)(2,3) == 0);
            DLIB_TEST(remove_col(res,3).nr() == 5);
            DLIB_TEST(remove_col(res,3).nc() == 4);

            set_all_elements(m2, 1);
            set_subm(m1, rectangle(1,1,3,2)) = 2;
            set_all_elements(m2, 2);
            set_subm(m1, 3,2,2,2) = 3;

            set_colm(m1,0) = 9;
            set_rowm(m1,0) = rowm(m1,4);
            set_rowm(m1,3) = 4;

            DLIB_TEST_MSG(m1 == res, "m1: \n" << m1 << "\nres: \n" << res);

        }

    }

    void matrix_test2()
    {
        print_spinner();

        {

            const double stuff[] = {
                1, 2, 3,
                6, 3, 3,
                7, 3, 9};

            matrix<double,3,3> m(stuff);

            // make m be symmetric
            m = m*trans(m);

            matrix<double> L = chol(m);
            DLIB_TEST(equal(L*trans(L), m));

            DLIB_TEST_MSG(equal(inv(m), inv_upper_triangular(trans(L))*inv_lower_triangular((L))), "");
            DLIB_TEST(equal(round_zeros(inv_upper_triangular(trans(L))*trans(L),1e-10), identity_matrix<double>(3), 1e-10));
            DLIB_TEST(equal(round_zeros(inv_lower_triangular((L))*(L),1e-10) ,identity_matrix<double>(3),1e-10));

        }

        {

            const double stuff[] = {
                1, 2, 3, 6, 3, 4,
                6, 3, 3, 1, 2, 3,
                7, 3, 9, 54.3, 5, 3,
                -6, 3, -3, 1, 2, 3,
                1, 2, 3, 5, -3, 3,
                7, 3, -9, 54.3, 5, 3
                };

            matrix<double,6,6> m(stuff);

            // make m be symmetric
            m = m*trans(m);

            matrix<double> L = chol(m);
            DLIB_TEST_MSG(equal(L*trans(L), m, 1e-10), L*trans(L)-m);

            DLIB_TEST_MSG(equal(inv(m), inv_upper_triangular(trans(L))*inv_lower_triangular((L))), "");
            DLIB_TEST_MSG(equal(inv(m), trans(inv_lower_triangular(L))*inv_lower_triangular((L))), "");
            DLIB_TEST_MSG(equal(inv(m), trans(inv_lower_triangular(L))*trans(inv_upper_triangular(trans(L)))), "");
            DLIB_TEST_MSG(equal(round_zeros(inv_upper_triangular(trans(L))*trans(L),1e-10) , identity_matrix<double>(6), 1e-10),
                         round_zeros(inv_upper_triangular(trans(L))*trans(L),1e-10));
            DLIB_TEST_MSG(equal(round_zeros(inv_lower_triangular((L))*(L),1e-10) ,identity_matrix<double>(6), 1e-10),
                         round_zeros(inv_lower_triangular((L))*(L),1e-10));

        }

        {
            // Test band chol
            matrix<double> m = rand_sp_banded<double>(10, 3);

            matrix<double> L = chol(m);
            DLIB_TEST_MSG(equal(L*trans(L), m, 1e-10), L*trans(L)-m);
            DLIB_TEST_MSG(equal(inv(m), inv_upper_triangular(trans(L))*inv_lower_triangular((L))), "");
            DLIB_TEST_MSG(equal(inv(m), trans(inv_lower_triangular(L))*inv_lower_triangular((L))), "");
            DLIB_TEST_MSG(equal(inv(m), trans(inv_lower_triangular(L))*trans(inv_upper_triangular(trans(L)))), "");
        }

        {
            // Test band chol in column major layout
            matrix<double,10,10,default_memory_manager,column_major_layout> m(rand_sp_banded<double>(10, 3));

            matrix<double> L = chol(m);
            DLIB_TEST_MSG(equal(L*trans(L), m, 1e-10), L*trans(L)-m);
            DLIB_TEST_MSG(equal(inv(m), inv_upper_triangular(trans(L))*inv_lower_triangular((L))), "");
            DLIB_TEST_MSG(equal(inv(m), trans(inv_lower_triangular(L))*inv_lower_triangular((L))), "");
            DLIB_TEST_MSG(equal(inv(m), trans(inv_lower_triangular(L))*trans(inv_upper_triangular(trans(L)))), "");
        }

        {
            matrix<int> m(3,4), m2;
            m = 1,2,3,4,
                4,5,6,6,
                6,1,8,0;
            m2 = m;
            DLIB_TEST(round(m) == m2);
            DLIB_TEST(round_zeros(m) == m2);

            m2 = 0,2,3,4,
                 4,5,6,6,
                 6,0,8,0;

            DLIB_TEST(round_zeros(m,2) == m2);
        }

        {

            matrix<double,6,6> m(identity_matrix<double>(6)*4.5);

            matrix<double> L = chol(m);
            DLIB_TEST_MSG(equal(L*trans(L), m, 1e-10), L*trans(L)-m);

            DLIB_TEST_MSG(equal(inv(m), inv_upper_triangular(trans(L))*inv_lower_triangular((L))), "");
            DLIB_TEST_MSG(equal(round_zeros(inv_upper_triangular(trans(L))*trans(L),1e-10) , identity_matrix<double>(6), 1e-10),
                         round_zeros(inv_upper_triangular(trans(L))*trans(L),1e-10));
            DLIB_TEST_MSG(equal(round_zeros(inv_lower_triangular((L))*(L),1e-10) ,identity_matrix<double>(6), 1e-10),
                         round_zeros(inv_lower_triangular((L))*(L),1e-10));

        }

        {

            matrix<double,6,6> m(identity_matrix<double>(6)*4.5);
            m(1,4) = 2;

            DLIB_TEST_MSG(dlib::equal(inv_upper_triangular(m), inv(m),1e-10), inv_upper_triangular(m)-inv(m));
            DLIB_TEST_MSG(dlib::equal(inv_lower_triangular(trans(m)), inv(trans(m)),1e-10), inv_lower_triangular(trans(m))-inv(trans(m)));

        }

        {
            matrix<double> a;
            matrix<float> b;
            matrix<int> i;
            a.set_size(1000,10);
            b.set_size(1000,10);
            i.set_size(1000,10);
            dlib::rand rnd;
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = rnd.get_random_double();
                    b(r,c) = rnd.get_random_float();
                    i(r,c) = r+c*r;
                }
            }

            // make sure the multiply optimizations aren't messing things up
            DLIB_TEST(trans(i)*i == tmp(trans(i)*i));
            DLIB_TEST_MSG(equal(trans(a)*a , tmp(trans(a)*a), 1e-11),max(abs(trans(a)*a - tmp(trans(a)*a))));
            DLIB_TEST_MSG(equal(trans(b)*b , tmp(trans(b)*b), 1e-3f),max(abs(trans(b)*b - tmp(trans(b)*b))));
        }

        {
            matrix<int,4> i(4,1);
            i(0) = 1;
            i(1) = 2;
            i(2) = 3;
            i(3) = 4;
            matrix<int,4,4> m;
            set_all_elements(m,0);
            m(0,0) = 1;
            m(1,1) = 2;
            m(2,2) = 3;
            m(3,3) = 4;

            DLIB_TEST(diagm(i) == m);
        }

        {
            matrix<int,1,4> i;
            i(0) = 1;
            i(1) = 2;
            i(2) = 3;
            i(3) = 4;
            matrix<int,4,4> m;
            set_all_elements(m,0);
            m(0,0) = 1;
            m(1,1) = 2;
            m(2,2) = 3;
            m(3,3) = 4;

            DLIB_TEST(diagm(i) == m);
        }

        {
            matrix<int> i(4,1);
            i(0) = 1;
            i(1) = 2;
            i(2) = 3;
            i(3) = 4;
            matrix<int> m(4,4);
            set_all_elements(m,0);
            m(0,0) = 1;
            m(1,1) = 2;
            m(2,2) = 3;
            m(3,3) = 4;

            DLIB_TEST(diagm(i) == m);
        }

        {
            matrix<int> i(1,4);
            i(0) = 1;
            i(1) = 2;
            i(2) = 3;
            i(3) = 4;
            matrix<int> m(4,4);
            set_all_elements(m,0);
            m(0,0) = 1;
            m(1,1) = 2;
            m(2,2) = 3;
            m(3,3) = 4;

            DLIB_TEST(diagm(i) == m);
        }

        {
            DLIB_TEST(range(0,5).nc() == 6);
            DLIB_TEST(range(1,5).nc() == 5);
            DLIB_TEST(range(0,5).nr() == 1);
            DLIB_TEST(range(1,5).nr() == 1);
            DLIB_TEST(trans(range(0,5)).nr() == 6);
            DLIB_TEST(trans(range(1,5)).nr() == 5);
            DLIB_TEST(trans(range(0,5)).nc() == 1);
            DLIB_TEST(trans(range(1,5)).nc() == 1);

            DLIB_TEST(range(0,2,5).nc() == 3);
            DLIB_TEST(range(1,2,5).nc() == 3);
            DLIB_TEST(range(0,2,5).nr() == 1);
            DLIB_TEST(range(1,2,5).nr() == 1);
            DLIB_TEST(trans(range(0,2,5)).nr() == 3);
            DLIB_TEST(trans(range(1,2,5)).nr() == 3);
            DLIB_TEST(trans(range(0,2,5)).nc() == 1);
            DLIB_TEST(trans(range(1,2,5)).nc() == 1);

            DLIB_TEST(range(0,3,6).nc() == 3);
            DLIB_TEST(range(1,3,5).nc() == 2);
            DLIB_TEST(range(0,3,5).nr() == 1);
            DLIB_TEST(range(1,3,5).nr() == 1);
            DLIB_TEST(trans(range(0,3,6)).nr() == 3);
            DLIB_TEST(trans(range(1,3,5)).nr() == 2);
            DLIB_TEST(trans(range(0,3,5)).nc() == 1);
            DLIB_TEST(trans(range(1,3,5)).nc() == 1);

            DLIB_TEST(range(1,9,5).nc() == 1);
            DLIB_TEST(range(1,9,5).nr() == 1);

            DLIB_TEST(range(0,0).nc() == 1);
            DLIB_TEST(range(0,0).nr() == 1);

            DLIB_TEST(range(1,1)(0) == 1);

            DLIB_TEST(range(0,5)(0) == 0 && range(0,5)(1) == 1 && range(0,5)(5) == 5);
            DLIB_TEST(range(1,2,5)(0) == 1 && range(1,2,5)(1) == 3 && range(1,2,5)(2) == 5);
            DLIB_TEST((range<0,5>()(0) == 0 && range<0,5>()(1) == 1 && range<0,5>()(5) == 5));
            DLIB_TEST((range<1,2,5>()(0) == 1 && range<1,2,5>()(1) == 3 && range<1,2,5>()(2) == 5));

            DLIB_TEST((range<0,5>().nc() == 6));
            DLIB_TEST((range<1,5>().nc() == 5));
            DLIB_TEST((range<0,5>().nr() == 1));
            DLIB_TEST((range<1,5>().nr() == 1));
            DLIB_TEST((trans(range<0,5>()).nr() == 6));
            DLIB_TEST((trans(range<1,5>()).nr() == 5));
            DLIB_TEST((trans(range<0,5>()).nc() == 1));
            DLIB_TEST((trans(range<1,5>()).nc() == 1));

            DLIB_TEST((range<0,2,5>().nc() == 3));
            DLIB_TEST((range<1,2,5>().nc() == 3));
            DLIB_TEST((range<0,2,5>().nr() == 1));
            DLIB_TEST((range<1,2,5>().nr() == 1));
            DLIB_TEST((trans(range<0,2,5>()).nr() == 3));
            DLIB_TEST((trans(range<1,2,5>()).nr() == 3));
            DLIB_TEST((trans(range<0,2,5>()).nc() == 1));
            DLIB_TEST((trans(range<1,2,5>()).nc() == 1));

            DLIB_TEST((range<0,3,6>().nc() == 3));
            DLIB_TEST((range<1,3,5>().nc() == 2));
            DLIB_TEST((range<0,3,5>().nr() == 1));
            DLIB_TEST((range<1,3,5>().nr() == 1));
            DLIB_TEST((trans(range<0,3,6>()).nr() == 3));
            DLIB_TEST((trans(range<1,3,5>()).nr() == 2));
            DLIB_TEST((trans(range<0,3,5>()).nc() == 1));
            DLIB_TEST((trans(range<1,3,5>()).nc() == 1));
        }

        {
            DLIB_TEST(range(5,0).nc() == 6);
            DLIB_TEST(range(5,1).nc() == 5);
            DLIB_TEST(range(5,0).nr() == 1);
            DLIB_TEST(range(5,1).nr() == 1);
            DLIB_TEST(trans(range(5,0)).nr() == 6);
            DLIB_TEST(trans(range(5,1)).nr() == 5);
            DLIB_TEST(trans(range(5,0)).nc() == 1);
            DLIB_TEST(trans(range(5,1)).nc() == 1);

            DLIB_TEST(range(5,2,0).nc() == 3);
            DLIB_TEST(range(5,2,1).nc() == 3);
            DLIB_TEST(range(5,2,0).nr() == 1);
            DLIB_TEST(range(5,2,1).nr() == 1);
            DLIB_TEST(trans(range(5,2,0)).nr() == 3);
            DLIB_TEST(trans(range(5,2,1)).nr() == 3);
            DLIB_TEST(trans(range(5,2,0)).nc() == 1);
            DLIB_TEST(trans(range(5,2,1)).nc() == 1);

            DLIB_TEST(range(6,3,0).nc() == 3);
            DLIB_TEST(range(5,3,1).nc() == 2);
            DLIB_TEST(range(5,3,0).nr() == 1);
            DLIB_TEST(range(5,3,1).nr() == 1);
            DLIB_TEST(trans(range(6,3,0)).nr() == 3);
            DLIB_TEST(trans(range(5,3,1)).nr() == 2);
            DLIB_TEST(trans(range(5,3,0)).nc() == 1);
            DLIB_TEST(trans(range(5,3,1)).nc() == 1);

            DLIB_TEST(range(5,9,1).nc() == 1);
            DLIB_TEST(range(5,9,1).nr() == 1);

            DLIB_TEST(range(0,0).nc() == 1);
            DLIB_TEST(range(0,0).nr() == 1);

            DLIB_TEST(range(1,1)(0) == 1);

            DLIB_TEST(range(5,0)(0) == 5 && range(5,0)(1) == 4 && range(5,0)(5) == 0);
            DLIB_TEST(range(5,2,1)(0) == 5 && range(5,2,1)(1) == 3 && range(5,2,1)(2) == 1);
            DLIB_TEST((range<5,0>()(0) == 5 && range<5,0>()(1) == 4 && range<5,0>()(5) == 0));
            DLIB_TEST((range<5,2,1>()(0) == 5 && range<5,2,1>()(1) == 3 && range<5,2,1>()(2) == 1));

            DLIB_TEST((range<5,0>().nc() == 6));
            DLIB_TEST((range<5,1>().nc() == 5));
            DLIB_TEST((range<5,0>().nr() == 1));
            DLIB_TEST((range<5,1>().nr() == 1));
            DLIB_TEST((trans(range<5,0>()).nr() == 6));
            DLIB_TEST((trans(range<5,1>()).nr() == 5));
            DLIB_TEST((trans(range<5,0>()).nc() == 1));
            DLIB_TEST((trans(range<5,1>()).nc() == 1));

            DLIB_TEST((range<5,2,0>().nc() == 3));
            DLIB_TEST((range<5,2,1>().nc() == 3));
            DLIB_TEST((range<5,2,0>().nr() == 1));
            DLIB_TEST((range<5,2,1>().nr() == 1));
            DLIB_TEST((trans(range<5,2,0>()).nr() == 3));
            DLIB_TEST((trans(range<5,2,1>()).nr() == 3));
            DLIB_TEST((trans(range<5,2,0>()).nc() == 1));
            DLIB_TEST((trans(range<5,2,1>()).nc() == 1));

            DLIB_TEST((range<6,3,0>().nc() == 3));
            DLIB_TEST((range<5,3,1>().nc() == 2));
            DLIB_TEST((range<5,3,0>().nr() == 1));
            DLIB_TEST((range<5,3,1>().nr() == 1));
            DLIB_TEST((trans(range<6,3,0>()).nr() == 3));
            DLIB_TEST((trans(range<5,3,1>()).nr() == 2));
            DLIB_TEST((trans(range<5,3,0>()).nc() == 1));
            DLIB_TEST((trans(range<5,3,1>()).nc() == 1));
        }

        {
            matrix<double> m(4,3);
            for (long r = 0; r < m.nr(); ++r)
            {
                for (long c = 0; c < m.nc(); ++c)
                {
                    m(r,c) = r*c;
                }
            }

            DLIB_TEST(subm(m,range(0,3),range(0,0)) == colm(m,0));
            DLIB_TEST(subm(m,range(0,3),range(1,1)) == colm(m,1));
            DLIB_TEST(subm(m,range(0,3),range(2,2)) == colm(m,2));

            DLIB_TEST(subm(m,range(0,0),range(0,2)) == rowm(m,0));
            DLIB_TEST(subm(m,range(1,1),range(0,2)) == rowm(m,1));
            DLIB_TEST(subm(m,range(2,2),range(0,2)) == rowm(m,2));
            DLIB_TEST(subm(m,range(3,3),range(0,2)) == rowm(m,3));
            DLIB_TEST(rowm(m,matrix<long>()).size()==0);
            DLIB_TEST(colm(m,matrix<long>()).size()==0);

            DLIB_TEST(subm(m,0,0,2,2) == subm(m,range(0,1),range(0,1)));
            DLIB_TEST(subm(m,1,1,2,2) == subm(m,range(1,2),range(1,2)));

            matrix<double,2,2> m2 = subm(m,range(0,2,2),range(0,2,2));

            DLIB_TEST(m2(0,0) == m(0,0));
            DLIB_TEST(m2(0,1) == m(0,2));
            DLIB_TEST(m2(1,0) == m(2,0));
            DLIB_TEST(m2(1,1) == m(2,2));

        }
        {
            matrix<double,4,3> m(4,3);
            for (long r = 0; r < m.nr(); ++r)
            {
                for (long c = 0; c < m.nc(); ++c)
                {
                    m(r,c) = r*c;
                }
            }

            DLIB_TEST(subm(m,range<0,3>(),range<0,0>()) == colm(m,0));
            DLIB_TEST(subm(m,range<0,3>(),range<1,1>()) == colm(m,1));
            DLIB_TEST(subm(m,range<0,3>(),range<2,2>()) == colm(m,2));

            DLIB_TEST(subm(m,range<0,0>(),range<0,2>()) == rowm(m,0));
            DLIB_TEST(subm(m,range<1,1>(),range<0,2>()) == rowm(m,1));
            DLIB_TEST(subm(m,range<2,2>(),range<0,2>()) == rowm(m,2));
            DLIB_TEST(subm(m,range<3,3>(),range<0,2>()) == rowm(m,3));

            DLIB_TEST(subm(m,0,0,2,2) == subm(m,range<0,1>(),range<0,1>()));
            DLIB_TEST(subm(m,1,1,2,2) == subm(m,range<1,2>(),range<1,2>()));

            matrix<double,2,2> m2 = subm(m,range<0,2,2>(),range<0,2,2>());

            DLIB_TEST(m2(0,0) == m(0,0));
            DLIB_TEST(m2(0,1) == m(0,2));
            DLIB_TEST(m2(1,0) == m(2,0));
            DLIB_TEST(m2(1,1) == m(2,2));

        }

        {
            matrix<double> a = randm(3,4);
            matrix<double> b = randm(3,4);

            matrix<double> m1, m2;

            m1 = max_pointwise(a,b);
            m2 = min_pointwise(a,b);
            DLIB_TEST(m1.nr() == a.nr());
            DLIB_TEST(m1.nc() == a.nc());
            DLIB_TEST(m2.nr() == a.nr());
            DLIB_TEST(m2.nc() == a.nc());
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    DLIB_TEST_MSG(m1(r,c) == std::max(a(r,c), b(r,c)), m1(r,c)  << " : " << a(r,c) << " " << b(r,c));
                    DLIB_TEST(m2(r,c) == std::min(a(r,c), b(r,c)));
                }
            }
        }

        {
            matrix<double,4,5> m;
            set_subm(m, range(0,3), range(0,4)) = 4;
            DLIB_TEST(min(m) == max(m) && min(m) == 4);

            set_subm(m,range(1,1),range(0,4)) = 7;
            DLIB_TEST((rowm(m,0) == uniform_matrix<double>(1,5, 4)));
            DLIB_TEST((rowm(m,1) == uniform_matrix<double>(1,5, 7)));
            DLIB_TEST((rowm(m,2) == uniform_matrix<double>(1,5, 4)));
            DLIB_TEST((rowm(m,3) == uniform_matrix<double>(1,5, 4)));

            set_subm(m, range(0,2,3), range(0,2,4)) = trans(subm(m,0,0,3,2));

            DLIB_TEST(m(0,2) == 7);
            DLIB_TEST(m(2,2) == 7);

            DLIB_TEST(sum(m) == 7*5+ 7+7 +  4*(4*5 - 7));

        }

        {
            matrix<double> mat(4,5);
            DLIB_TEST((uniform_matrix<double>(4,5,1) == ones_matrix<double>(4,5)));
            DLIB_TEST((uniform_matrix<double>(4,5,1) == ones_matrix(mat)));
            DLIB_TEST((uniform_matrix<double>(4,5,0) == zeros_matrix<double>(4,5)));
            DLIB_TEST((uniform_matrix<double>(4,5,0) == zeros_matrix(mat)));
            DLIB_TEST((uniform_matrix<float>(4,5,1) == ones_matrix<float>(4,5)));
            DLIB_TEST((uniform_matrix<float>(4,5,0) == zeros_matrix<float>(4,5)));
            DLIB_TEST((uniform_matrix<complex<double> >(4,5,1) == ones_matrix<complex<double> >(4,5)));
            DLIB_TEST((uniform_matrix<complex<double> >(4,5,0) == zeros_matrix<complex<double> >(4,5)));
            DLIB_TEST((uniform_matrix<complex<float> >(4,5,1) == ones_matrix<complex<float> >(4,5)));
            DLIB_TEST((uniform_matrix<complex<float> >(4,5,0) == zeros_matrix<complex<float> >(4,5)));
            DLIB_TEST((complex_matrix(ones_matrix<double>(3,3), zeros_matrix<double>(3,3)) == complex_matrix(ones_matrix<double>(3,3))));
            DLIB_TEST((pointwise_multiply(complex_matrix(ones_matrix<double>(3,3)), ones_matrix<double>(3,3)*2) ==
                       complex_matrix(2*ones_matrix<double>(3,3))));
            DLIB_TEST((pointwise_divide(complex_matrix(ones_matrix<double>(3,3)), ones_matrix<double>(3,3)) ==
                       complex_matrix(ones_matrix<double>(3,3))));
            DLIB_TEST((pointwise_divide(complex_matrix(zeros_matrix<double>(3,3)), ones_matrix<double>(3,3)) ==
                       complex_matrix(zeros_matrix<double>(3,3))));
        }

        {
            DLIB_TEST(( uniform_matrix<double>(303,303, 3)*identity_matrix<double>(303) == uniform_matrix<double,303,303>(3) ) );
            DLIB_TEST(( uniform_matrix<double,303,303>(3)*identity_matrix<double,303>() == uniform_matrix<double,303,303>(3) ));
        }

        {
            matrix<double> m(2,3);
            m = 1,2,3,
                5,6,7;

            DLIB_TEST_MSG(m(0,0) == 1 && m(0,1) == 2 && m(0,2) == 3 &&
                         m(1,0) == 5 && m(1,1) == 6 && m(1,2) == 7,"");

            m = 4;
            DLIB_TEST((m == uniform_matrix<double,2,3>(4)));

            matrix<double,2,3> m2;
            m2 = 1,2,3,
                 5,6,7;
            DLIB_TEST_MSG(m2(0,0) == 1 && m2(0,1) == 2 && m2(0,2) == 3 &&
                         m2(1,0) == 5 && m2(1,1) == 6 && m2(1,2) == 7,"");

            matrix<double,2,1> m3;
            m3 = 1,
                 5;
            DLIB_TEST(m3(0) == 1 && m3(1) == 5 );

            matrix<double,1,2> m4;
            m4 = 1, 5;
            DLIB_TEST(m3(0) == 1 && m3(1) == 5 );
        }

        {
            matrix<double> m(4,1);
            m = 3, 1, 5, 2;
            DLIB_TEST(index_of_min(m) == 1);
            DLIB_TEST(index_of_max(m) == 2);
            DLIB_TEST(index_of_min(trans(m)) == 1);
            DLIB_TEST(index_of_max(trans(m)) == 2);
        }

        {
            matrix<double> m1(1,5), m2;

            m1 = 3.0000,  3.7500,  4.5000,  5.2500,  6.0000;
            m2 = linspace(3, 6, 5);

            DLIB_TEST(equal(m1, m2));

            m1 = pow(10, m1);
            m2 = logspace(3, 6, 5);

            DLIB_TEST(equal(m1, m2));
        }

        {
            matrix<long> m = cartesian_product(range(1,3), range(0,1));

            matrix<long,2,1> c0, c1, c2, c3, c4, c5;
            c0 = 1, 0;
            c1 = 1, 1;
            c2 = 2, 0;
            c3 = 2, 1;
            c4 = 3, 0;
            c5 = 3, 1;

            DLIB_TEST_MSG(colm(m,0) == c0, colm(m,0) << "\n\n" << c0);
            DLIB_TEST(colm(m,1) == c1);
            DLIB_TEST(colm(m,2) == c2);
            DLIB_TEST(colm(m,3) == c3);
            DLIB_TEST(colm(m,4) == c4);
            DLIB_TEST(colm(m,5) == c5);
        }

        {
            matrix<double> m(2,2), mr(2,2), mr_max(2,2);

            m = 1, 2,
                0, 4;

            mr = 1, 1.0/2.0,
                0,  1.0/4.0;

            mr_max = 1, 1.0/2.0,
                     std::numeric_limits<double>::max(),  1.0/4.0;

            DLIB_TEST(equal(reciprocal(m), mr));
            DLIB_TEST(equal(reciprocal_max(m), mr_max));

        }

        {
            matrix<double> m1, m2;
            m1.set_size(3,1);
            m2.set_size(1,3);

            m1 = 1,2,3;
            m2 = 4,5,6;
            DLIB_TEST(dot(m1, m2)               == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(m1, trans(m2))        == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(trans(m1), m2)        == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(trans(m1), trans(m2)) == 1*4 + 2*5 + 3*6);
        }

        {
            matrix<double,3,1> m1, m2;
            m1.set_size(3,1);
            m2.set_size(3,1);

            m1 = 1,2,3;
            m2 = 4,5,6;
            DLIB_TEST(dot(m1, m2)               == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(m1, trans(m2))        == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(trans(m1), m2)        == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(trans(m1), trans(m2)) == 1*4 + 2*5 + 3*6);
        }
        {
            matrix<double,1,3> m1, m2;
            m1.set_size(1,3);
            m2.set_size(1,3);

            m1 = 1,2,3;
            m2 = 4,5,6;
            DLIB_TEST(dot(m1, m2)               == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(m1, trans(m2))        == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(trans(m1), m2)        == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(trans(m1), trans(m2)) == 1*4 + 2*5 + 3*6);
        }
        {
            matrix<double,1,3> m1;
            matrix<double> m2;
            m1.set_size(1,3);
            m2.set_size(3,1);

            m1 = 1,2,3;
            m2 = 4,5,6;
            DLIB_TEST(dot(m1, m2)               == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(m1, trans(m2))        == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(trans(m1), m2)        == 1*4 + 2*5 + 3*6);
            DLIB_TEST(dot(trans(m1), trans(m2)) == 1*4 + 2*5 + 3*6);
        }

        {
            matrix<double> m1(3,3), m2(3,3);

            m1 = 1;
            m2 = 1;
            m1 = m1*subm(m2,0,0,3,3);
            DLIB_TEST(is_finite(m1));
        }
        {
            matrix<double,3,1> m1;
            matrix<double> m2(3,3);

            m1 = 1;
            m2 = 1;
            m1 = subm(m2,0,0,3,3)*m1;
        }

        {
            matrix<int> m(2,1);

            m = 3,3;
            m /= m(0);

            DLIB_TEST(m(0) == 1);
            DLIB_TEST(m(1) == 1);
        }
        {
            matrix<int> m(2,1);

            m = 3,3;
            m *= m(0);

            DLIB_TEST(m(0) == 9);
            DLIB_TEST(m(1) == 9);
        }
        {
            matrix<int> m(2,1);

            m = 3,3;
            m -= m(0);

            DLIB_TEST(m(0) == 0);
            DLIB_TEST(m(1) == 0);
        }
        {
            matrix<int> m(2,1);

            m = 3,3;
            m += m(0);

            DLIB_TEST(m(0) == 6);
            DLIB_TEST(m(1) == 6);
            DLIB_TEST(is_finite(m));
        }

        {
            matrix<double> m(3,3);
            m = 3;
            m(1,1) = std::numeric_limits<double>::infinity();
            DLIB_TEST(is_finite(m) == false);
            m(1,1) = -std::numeric_limits<double>::infinity();
            DLIB_TEST(is_finite(m) == false);
            m(1,1) = 2;
            DLIB_TEST(is_finite(m));
        }

        {
            matrix<int> m(4,1), mm, mmm;

            mmm = mm = (m = 1,2,3,4);
            DLIB_TEST(m(0) == 1);
            DLIB_TEST(m(1) == 2);
            DLIB_TEST(m(2) == 3);
            DLIB_TEST(m(3) == 4);
            DLIB_TEST(mm == m);
            DLIB_TEST(mmm == m);
            DLIB_TEST(mm(0) == 1);
            DLIB_TEST(mm(1) == 2);
            DLIB_TEST(mm(2) == 3);
            DLIB_TEST(mm(3) == 4);
        }

        {
            const long n = 5;
            matrix<double> m1, m2, m3, truth;
            m1 = randm(n,n);
            m2 = randm(n,n);

            rectangle rect1(1,1,3,3);
            rectangle rect2(2,1,4,3);

            truth = subm(m1,rect1)*subm(m2,rect2);
            m3 = mat(&m1(0,0)+6, 3,3, m1.nc()) * mat(&m2(0,0)+7, 3,3, m2.nc());

            DLIB_TEST(max(abs(truth-m3)) < 1e-13);
        }

        {
            const long n = 5;
            matrix<double> m1, m2, m3, truth;
            m1 = randm(n,n);
            m2 = randm(n,n);
            m3 = randm(n,n);

            truth = m1*m2;
            m3 = mat(&m1(0,0),n,n)*mat(&m2(0,0),n,n);
            DLIB_TEST(max(abs(truth-m3)) < 1e-13);
            m3 = 0;
            set_ptrm(&m3(0,0),n,n) = mat(&m1(0,0),n,n)*mat(&m2(0,0),n,n);
            DLIB_TEST(max(abs(truth-m3)) < 1e-13);
            set_ptrm(&m3(0,0),n,n) = m1*m2;
            DLIB_TEST(max(abs(truth-m3)) < 1e-13);

            // now make sure it deals with aliasing correctly.
            truth = m1*m2;
            m1 = mat(&m1(0,0),n,n)*mat(&m2(0,0),n,n);
            DLIB_TEST(max(abs(truth-m1)) < 1e-13);

            m1 = randm(n,n);
            truth = m1*m2;
            set_ptrm(&m1(0,0),n,n) = mat(&m1(0,0),n,n)*mat(&m2(0,0),n,n);
            DLIB_TEST(max(abs(truth-m1)) < 1e-13);

            m1 = randm(n,n);
            truth = m1*m2;
            set_ptrm(&m1(0,0),n,n) = m1*m2;
            DLIB_TEST(max(abs(truth-m1)) < 1e-13);

            m1 = randm(n,n);
            truth = m1+m1*m2;
            set_ptrm(&m1(0,0),n,n) += m1*m2;
            DLIB_TEST(max(abs(truth-m1)) < 1e-13);

            m1 = randm(n,n);
            truth = m1-m1*m2;
            set_ptrm(&m1(0,0),n,n) -= m1*m2;
            DLIB_TEST(max(abs(truth-m1)) < 1e-13);

        }

        {
            matrix<double,3,3,default_memory_manager,column_major_layout> a(3,3);
            matrix<double,3,3,default_memory_manager,column_major_layout> m = randm(3,3);
            matrix<double,3,1,default_memory_manager,column_major_layout> b = randm(3,1);

            a = 0;
            set_colm(a,0) = m*b;
            DLIB_TEST(colm(a,0) == m*b);
            a = 0;
            set_rowm(a,0) = trans(m*b);
            DLIB_TEST(rowm(a,0) == trans(m*b));
            DLIB_TEST(rowm(a,0) != m*b);
        }
        {
            matrix<double,0,0,default_memory_manager,column_major_layout> a(3,3);
            matrix<double,0,0,default_memory_manager,column_major_layout> m = randm(3,3);
            matrix<double,0,0,default_memory_manager,column_major_layout> b = randm(3,1);

            a = 0;
            set_colm(a,0) = m*b;
            DLIB_TEST(equal(colm(a,0) , m*b));
            a = 0;
            set_rowm(a,0) = trans(m*b);
            DLIB_TEST(equal(rowm(a,0) , trans(m*b)));
            DLIB_TEST(!equal(rowm(a,0) , m*b));
        }
        {
            matrix<double> a(3,3);
            matrix<double> m = randm(3,3);
            matrix<double> b = randm(3,1);

            a = 0;
            set_colm(a,0) = m*b;
            DLIB_TEST(equal(colm(a,0) , m*b));
            a = 0;
            set_rowm(a,0) = trans(m*b);
            DLIB_TEST(equal(rowm(a,0) , trans(m*b)));
            DLIB_TEST(!equal(rowm(a,0) , m*b));
        }
    }

    class matrix_tester : public tester
    {
    public:
        matrix_tester (
        ) :
            tester ("test_matrix",
                    "Runs tests on the matrix component.")
        {}

        void perform_test (
        )
        {
            matrix_test();
            matrix_test2();
        }
    } a;

}


#ifndef MLT_UTILS_OPTIMIZERS_STOCHASTIC_GRADIENT_DESCENT_TRAINER_HPP
#define MLT_UTILS_OPTIMIZERS_STOCHASTIC_GRADIENT_DESCENT_TRAINER_HPP

#include <algorithm>
#include <random>
#include <tuple>
#include <vector>

#include <Eigen/Core>

#include "../../defs.hpp"
#include "../eigen.hpp"
#include "gradient_descent_updates.hpp"

namespace mlt {
namespace utils {
namespace optimizers {
	using namespace eigen;

    // Implementation of Stochastic Gradient Descent
    // Parameters:
    // - size_t batch_size: size of each batch when using mini-batches (set to 0 if using full batch gradient descent)
    // - size_t epochs: number of epochs to run the descent steps through the training data
    // - double learning_rate: initial learning rate
    // - double learning_rate_decay: learning_rate is multiplied by this after each epoch
    // - UpdateMethod update_method: object implementing the update strategy to use
	template <class UpdateMethod = RMSPropGradientDescentUpdate>
    class StochasticGradientDescent {
    public:
		StochasticGradientDescent(size_t batch_size = 1, size_t epochs = 1000, double learning_rate = 0.001, double learning_rate_decay = 0.99,
			const UpdateMethod& update_method = UpdateMethod()) : _batch_size(batch_size), _epochs(epochs), _learning_rate(learning_rate),
			_current_learning_rate(learning_rate), _learning_rate_decay(learning_rate_decay), _update_method(update_method) {}

        template <class Model, class Target>
		auto operator()(const Model& model, Features input, Target target, MatrixXdRef init, bool cold_start) {
			assert(input.cols() == target.cols());

            if (cold_start) {
                _current_learning_rate = _learning_rate;
				_update_method.restart();
            }

            auto iters_per_epoch = _batch_size > 0 && _batch_size <= input.cols() ? input.cols() / _batch_size : 1;

			MatrixXd params = init;

			random_device rd;
            default_random_engine generator(rd());

            for (auto epoch = 0; epoch < _epochs; epoch++) {
                for (auto iter = 0; iter < iters_per_epoch; iter++) {
					if (_batch_size > 0) {
						auto subset = tied_random_cols_subset(input, target, _batch_size, generator);
						auto input_batch = get<0>(subset);
						auto target_batch = get<1>(subset);

						params += _update_method.step(_current_learning_rate, model.gradient(params, input_batch, target_batch));
					} else {
						params += _update_method.step(_current_learning_rate, model.gradient(params, input, target));
					}
                }

#ifdef MLT_VERBOSE
				if (epoch % 10 == 0) {
					auto loss = model.loss(params, input, target);
					MLT_LOG_LINE("[SGD]: Epoch " << epoch + 1 << "/" << _epochs << " Loss " << loss);
				}
#endif

				_current_learning_rate *= _learning_rate_decay;
            }

			return params;
        }

	protected:
		size_t _batch_size;
		size_t _epochs;
		double _learning_rate;
		double _current_learning_rate;
		double _learning_rate_decay;
		UpdateMethod _update_method;
    };
}
}
}
#endif