/* -*- mia-c++  -*-
 *
 * This file is part of MIA - a toolbox for medical image analysis
 * Copyright (c) Leipzig, Madrid 1999-2017 Gert Wollny
 *
 * MIA is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with MIA; if not, see <http://www.gnu.org/licenses/>.
 *
 */


#include <mia/core/gsl_matrix_vector_ops.hh>

#ifndef BOOST_TEST_DYN_LINK
#define BOOST_TEST_DYN_LINK
#endif

#define BOOST_TEST_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <iostream>
#include <cmath>

using namespace gsl;
using std::sqrt;

BOOST_AUTO_TEST_CASE( test_vec_mult_marix )
{
       const double matrix_init[] = {
              1, 2, 3,
              4, 5, 6
       };
       const double vector_init[] = {
              2, 3
       };
       Matrix rhs(2, 3, matrix_init);
       Vector lhs(2, false);
       Vector result(3, true);
       std::copy(vector_init, vector_init + 2, lhs.begin());
       multiply_v_m(result, lhs, rhs);
       BOOST_CHECK_CLOSE(result[0], 14, 0.1);
       BOOST_CHECK_CLOSE(result[1], 19, 0.1);
       BOOST_CHECK_CLOSE(result[2], 24, 0.1);
}

BOOST_AUTO_TEST_CASE( test_mult_matrix_marix )
{
       const double lhs_init[] = {
              2, 3,
              1, 2,
              -1, -2,
              4, 5
       };
       const double rhs_init[] = {
              1, 2, 3,
              4, 5, 6
       };
       Matrix lhs(4, 2, lhs_init);
       Matrix rhs(2, 3, rhs_init);
       Matrix result(4, 3, true);
       multiply_m_m(result, lhs, rhs);
       BOOST_CHECK_CLOSE(result(0, 0), 14, 0.1);
       BOOST_CHECK_CLOSE(result(0, 1), 19, 0.1);
       BOOST_CHECK_CLOSE(result(0, 2), 24, 0.1);
       BOOST_CHECK_CLOSE(result(1, 0),  9, 0.1);
       BOOST_CHECK_CLOSE(result(1, 1), 12, 0.1);
       BOOST_CHECK_CLOSE(result(1, 2), 15, 0.1);
       BOOST_CHECK_CLOSE(result(2, 0), -9, 0.1);
       BOOST_CHECK_CLOSE(result(2, 1), -12, 0.1);
       BOOST_CHECK_CLOSE(result(2, 2), -15, 0.1);
       BOOST_CHECK_CLOSE(result(3, 0), 24, 0.1);
       BOOST_CHECK_CLOSE(result(3, 1), 33, 0.1);
       BOOST_CHECK_CLOSE(result(3, 2), 42, 0.1);
}


BOOST_AUTO_TEST_CASE( test_mult_matrix_matrixT )
{
       const double lhs_init[] = {
              2, 3,
              1, 2,
              -1, -2,
              4, 5
       };
       const double rhs_init[] = {
              1, 4,
              2, 5,
              3, 6
       };
       Matrix lhs(4, 2, lhs_init);
       Matrix rhs(3, 2, rhs_init);
       Matrix result(4, 3, true);
       multiply_m_mT(result, lhs, rhs);
       BOOST_CHECK_CLOSE(result(0, 0), 14, 0.1);
       BOOST_CHECK_CLOSE(result(0, 1), 19, 0.1);
       BOOST_CHECK_CLOSE(result(0, 2), 24, 0.1);
       BOOST_CHECK_CLOSE(result(1, 0),  9, 0.1);
       BOOST_CHECK_CLOSE(result(1, 1), 12, 0.1);
       BOOST_CHECK_CLOSE(result(1, 2), 15, 0.1);
       BOOST_CHECK_CLOSE(result(2, 0), -9, 0.1);
       BOOST_CHECK_CLOSE(result(2, 1), -12, 0.1);
       BOOST_CHECK_CLOSE(result(2, 2), -15, 0.1);
       BOOST_CHECK_CLOSE(result(3, 0), 24, 0.1);
       BOOST_CHECK_CLOSE(result(3, 1), 33, 0.1);
       BOOST_CHECK_CLOSE(result(3, 2), 42, 0.1);
}

BOOST_AUTO_TEST_CASE( test_mult_matrixT_martix )
{
       const double lhs_init[] = {
              2, 1, -1, 4,
              3, 2, -2, 5
       };
       const double rhs_init[] = {
              1, 2, 3,
              4, 5, 6
       };
       Matrix lhs(2, 4, lhs_init);
       Matrix rhs(2, 3, rhs_init);
       Matrix result(4, 3, true);
       multiply_mT_m(result, lhs, rhs);
       BOOST_CHECK_CLOSE(result(0, 0), 14, 0.1);
       BOOST_CHECK_CLOSE(result(0, 1), 19, 0.1);
       BOOST_CHECK_CLOSE(result(0, 2), 24, 0.1);
       BOOST_CHECK_CLOSE(result(1, 0),  9, 0.1);
       BOOST_CHECK_CLOSE(result(1, 1), 12, 0.1);
       BOOST_CHECK_CLOSE(result(1, 2), 15, 0.1);
       BOOST_CHECK_CLOSE(result(2, 0), -9, 0.1);
       BOOST_CHECK_CLOSE(result(2, 1), -12, 0.1);
       BOOST_CHECK_CLOSE(result(2, 2), -15, 0.1);
       BOOST_CHECK_CLOSE(result(3, 0), 24, 0.1);
       BOOST_CHECK_CLOSE(result(3, 1), 33, 0.1);
       BOOST_CHECK_CLOSE(result(3, 2), 42, 0.1);
}


BOOST_AUTO_TEST_CASE( test_mult_vec_marix )
{
       const double matrix_init[] = {
              1, 2,
              3, 4,
              5, 6
       };
       const double vector_init[] = {
              2, 3
       };
       Matrix lhs(3, 2, matrix_init);
       Vector rhs(2, false);
       Vector result(3, true);
       std::copy(vector_init, vector_init + 2, rhs.begin());
       multiply_m_v(result, lhs, rhs);
       BOOST_CHECK_CLOSE(result[0],  8, 0.1);
       BOOST_CHECK_CLOSE(result[1], 18, 0.1);
       BOOST_CHECK_CLOSE(result[2], 28, 0.1);
}

BOOST_AUTO_TEST_CASE( test_mult_vec_vec )
{
       const double vector1_init[] = {
              1, 2,
       };
       const double vector2_init[] = {
              2, 3
       };
       Vector lhs(2, false);
       Vector rhs(2, false);
       std::copy(vector1_init, vector1_init + 2, lhs.begin());
       std::copy(vector2_init, vector2_init + 2, rhs.begin());
       BOOST_CHECK_CLOSE(dot(lhs, rhs), 8, 0.1);
}


BOOST_AUTO_TEST_CASE( test_matrix_orthogonalize )
{
       const double U_init[] = {
              1.0, 0, 0, 0,
              0, 1.0 / sqrt(2.0), 1.0 / sqrt(2.0), 0,
              0, 1.0 / sqrt(2.0), -1.0 / sqrt(2.0), 0,
              0, 0, 0, 1.0
       };
       const double V_init[] = {
              1.0 / sqrt(2.0), 1.0 / sqrt(2.0), 0,
              0, 1.0 / sqrt(2.0), -1.0 / sqrt(2.0), 0,
              0, 0,  1.0
       };
       const double D_init[] = {
              2, 0, 0,
              0, 3, 0,
              0, 0, 4,
              0, 0, 0
       };
       Matrix U(4, 4, U_init);
       Matrix V(3, 3, V_init);
       Matrix D(4, 3, D_init);
       Matrix temp(4, 3, false);
       Matrix M(4, 3, false);
       multiply_m_m(temp, D, V);
       multiply_m_m(M, U, temp);
       matrix_orthogonalize(M);

       for (int c = 0; c < 3; ++c) {
              auto col_1 = gsl_matrix_column(M, c);
              // should be unity
              BOOST_CHECK_CLOSE(dot(&col_1.vector, &col_1.vector), 1.0, 0.1);

              for (int c2 = 0; c2 < c; ++c2) {
                     auto col_2 = gsl_matrix_column(M, c2);
                     BOOST_CHECK_SMALL(dot(&col_1.vector, &col_2.vector), 1e-10);
              }
       }
}
