/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2003 Ferdinando Ametrano
 Copyright (C) 2007, 2008 Klaus Spanderen
 Copyright (C) 2007 Neil Firth
 Copyright (C) 2016 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

#include "matrices.hpp"
#include "utilities.hpp"
#include <ql/experimental/math/moorepenroseinverse.hpp>
#include <ql/math/matrix.hpp>
#include <ql/math/matrixutilities/basisincompleteordered.hpp>
#include <ql/math/matrixutilities/bicgstab.hpp>
#include <ql/math/matrixutilities/choleskydecomposition.hpp>
#include <ql/math/matrixutilities/gmres.hpp>
#include <ql/math/matrixutilities/pseudosqrt.hpp>
#include <ql/math/matrixutilities/qrdecomposition.hpp>
#include <ql/math/matrixutilities/svd.hpp>
#include <ql/math/matrixutilities/symmetricschurdecomposition.hpp>
#include <ql/math/matrixutilities/sparsematrix.hpp>
#include <ql/math/randomnumbers/mt19937uniformrng.hpp>
#include <cmath>
#include <utility>
#include <numeric>

using namespace QuantLib;
using namespace boost::unit_test_framework;

using std::fabs;

namespace matrices_test {

    Size N;
    Matrix M1, M2, M3, M4, M5, M6, M7, I;

    Real norm(const Array& v) {
        return std::sqrt(DotProduct(v,v));
    }

    Real norm(const Matrix& m) {
        Real sum = 0.0;
        for (Size i=0; i<m.rows(); i++)
            for (Size j=0; j<m.columns(); j++)
                sum += m[i][j]*m[i][j];
        return std::sqrt(sum);
    }

    void setup() {

        N = 3;
        M1 = M2 = I = Matrix(N,N);
        M3 = Matrix(3,4);
        M4 = Matrix(4,3);
        M5 = Matrix(4, 4, 0.0);
        M6 = Matrix(4, 4, 0.0);

        M1[0][0] = 1.0;  M1[0][1] = 0.9;  M1[0][2] = 0.7;
        M1[1][0] = 0.9;  M1[1][1] = 1.0;  M1[1][2] = 0.4;
        M1[2][0] = 0.7;  M1[2][1] = 0.4;  M1[2][2] = 1.0;

        M2[0][0] = 1.0;  M2[0][1] = 0.9;  M2[0][2] = 0.7;
        M2[1][0] = 0.9;  M2[1][1] = 1.0;  M2[1][2] = 0.3;
        M2[2][0] = 0.7;  M2[2][1] = 0.3;  M2[2][2] = 1.0;

        I[0][0] = 1.0;  I[0][1] = 0.0;  I[0][2] = 0.0;
        I[1][0] = 0.0;  I[1][1] = 1.0;  I[1][2] = 0.0;
        I[2][0] = 0.0;  I[2][1] = 0.0;  I[2][2] = 1.0;

        M3[0][0] = 1; M3[0][1] = 2; M3[0][2] = 3; M3[0][3] = 4;
        M3[1][0] = 2; M3[1][1] = 0; M3[1][2] = 2; M3[1][3] = 1;
        M3[2][0] = 0; M3[2][1] = 1; M3[2][2] = 0; M3[2][3] = 0;

        M4[0][0] = 1;  M4[0][1] = 2;  M4[0][2] = 400;
        M4[1][0] = 2;  M4[1][1] = 0;  M4[1][2] = 1;
        M4[2][0] = 30; M4[2][1] = 2;  M4[2][2] = 0;
        M4[3][0] = 2;  M4[3][1] = 0;  M4[3][2] = 1.05;

        // from Higham - nearest correlation matrix
        M5[0][0] = 2;   M5[0][1] = -1;  M5[0][2] = 0.0; M5[0][3] = 0.0;
        M5[1][0] = M5[0][1];  M5[1][1] = 2;   M5[1][2] = -1;  M5[1][3] = 0.0;
        M5[2][0] = M5[0][2]; M5[2][1] = M5[1][2];  M5[2][2] = 2;   M5[2][3] = -1;
        M5[3][0] = M5[0][3]; M5[3][1] = M5[1][3]; M5[3][2] = M5[2][3];  M5[3][3] = 2;

        // from Higham - nearest correlation matrix to M5
        M6[0][0] = 1;        M6[0][1] = -0.8084124981;  M6[0][2] = 0.1915875019;   M6[0][3] = 0.106775049;
        M6[1][0] = M6[0][1]; M6[1][1] = 1;        M6[1][2] = -0.6562326948;  M6[1][3] = M6[0][2];
        M6[2][0] = M6[0][2]; M6[2][1] = M6[1][2]; M6[2][2] = 1;        M6[2][3] = M6[0][1];
        M6[3][0] = M6[0][3]; M6[3][1] = M6[1][3]; M6[3][2] = M6[2][3]; M6[3][3] = 1;

        M7 = M1;
        M7[0][1] = 0.3; M7[0][2] = 0.2; M7[2][1] = 1.2;
    }

}


void MatricesTest::testEigenvectors() {

    BOOST_TEST_MESSAGE("Testing eigenvalues and eigenvectors calculation...");

    using namespace matrices_test;

    setup();

    Matrix testMatrices[] = { M1, M2 };

    for (auto& M : testMatrices) {

        SymmetricSchurDecomposition dec(M);
        Array eigenValues = dec.eigenvalues();
        Matrix eigenVectors = dec.eigenvectors();
        Real minHolder = QL_MAX_REAL;

        for (Size i=0; i<N; i++) {
            Array v(N);
            for (Size j=0; j<N; j++)
                v[j] = eigenVectors[j][i];
            // check definition
            Array a = M*v;
            Array b = eigenValues[i]*v;
            if (norm(a-b) > 1.0e-15)
                BOOST_FAIL("Eigenvector definition not satisfied");
            // check decreasing ordering
            if (eigenValues[i] >= minHolder) {
                BOOST_FAIL("Eigenvalues not ordered: " << eigenValues);
            } else
                minHolder = eigenValues[i];
        }

        // check normalization
        Matrix m = eigenVectors * transpose(eigenVectors);
        if (norm(m-I) > 1.0e-15)
            BOOST_FAIL("Eigenvector not normalized");
    }
}

void MatricesTest::testSqrt() {

    BOOST_TEST_MESSAGE("Testing matricial square root...");

    using namespace matrices_test;

    setup();

    Matrix m = pseudoSqrt(M1, SalvagingAlgorithm::None);
    Matrix temp = m*transpose(m);
    Real error = norm(temp - M1);
    Real tolerance = 1.0e-12;
    if (error>tolerance) {
        BOOST_FAIL("Matrix square root calculation failed\n"
                   << "original matrix:\n" << M1
                   << "pseudoSqrt:\n" << m
                   << "pseudoSqrt*pseudoSqrt:\n" << temp
                   << "\nerror:     " << error
                   << "\ntolerance: " << tolerance);
    }
}

void MatricesTest::testHighamSqrt() {
    BOOST_TEST_MESSAGE("Testing Higham matricial square root...");

    using namespace matrices_test;

    setup();

    Matrix tempSqrt = pseudoSqrt(M5, SalvagingAlgorithm::Higham);
    Matrix ansSqrt = pseudoSqrt(M6, SalvagingAlgorithm::None);
    Real error = norm(ansSqrt - tempSqrt);
    Real tolerance = 1.0e-4;
    if (error>tolerance) {
        BOOST_FAIL("Higham matrix correction failed\n"
                   << "original matrix:\n" << M5
                   << "pseudoSqrt:\n" << tempSqrt
                   << "should be:\n" << ansSqrt
                   << "\nerror:     " << error
                   << "\ntolerance: " << tolerance);
    }
}

void MatricesTest::testSVD() {

    BOOST_TEST_MESSAGE("Testing singular value decomposition...");

    using namespace matrices_test;

    setup();

    Real tol = 1.0e-12;
    Matrix testMatrices[] = { M1, M2, M3, M4 };

    for (auto& A : testMatrices) {
        // m >= n required (rows >= columns)
        SVD svd(A);
        // U is m x n
        const Matrix& U = svd.U();
        // s is n long
        Array s = svd.singularValues();
        // S is n x n
        Matrix S = svd.S();
        // V is n x n
        const Matrix& V = svd.V();

        for (Size i=0; i < S.rows(); i++) {
            if (S[i][i] != s[i])
                BOOST_FAIL("S not consistent with s");
        }

        // tests
        Matrix U_Utranspose = transpose(U)*U;
        if (norm(U_Utranspose-I) > tol)
            BOOST_FAIL("U not orthogonal (norm of U^T*U-I = "
                       << norm(U_Utranspose-I) << ")");

        Matrix V_Vtranspose = transpose(V)*V;
        if (norm(V_Vtranspose-I) > tol)
            BOOST_FAIL("V not orthogonal (norm of V^T*V-I = "
                       << norm(V_Vtranspose-I) << ")");

        Matrix A_reconstructed = U * S * transpose(V);
        if (norm(A_reconstructed-A) > tol)
            BOOST_FAIL("Product does not recover A: (norm of U*S*V^T-A = "
                       << norm(A_reconstructed-A) << ")");
    }
}

void MatricesTest::testQRDecomposition() {

    BOOST_TEST_MESSAGE("Testing QR decomposition...");

    using namespace matrices_test;

    setup();

    Real tol = 1.0e-12;
    Matrix testMatrices[] = { M1, M2, I,
                              M3, transpose(M3), M4, transpose(M4), M5 };

    for (const auto& A : testMatrices) {
        Matrix Q, R;
        bool pivot = true;
        const std::vector<Size> ipvt = qrDecomposition(A, Q, R, pivot);

        Matrix P(A.columns(), A.columns(), 0.0);

        // reverse column pivoting
        for (Size i=0; i < P.columns(); ++i) {
            P[ipvt[i]][i] = 1.0;
        }

        if (norm(Q*R - A*P) > tol)
            BOOST_FAIL("Q*R does not match matrix A*P (norm = "
                       << norm(Q*R-A*P) << ")");

        pivot = false;
        qrDecomposition(A, Q, R, pivot);

        if (norm(Q*R - A) > tol)
            BOOST_FAIL("Q*R does not match matrix A (norm = "
                       << norm(Q*R-A) << ")");
    }
}

void MatricesTest::testQRSolve() {

    BOOST_TEST_MESSAGE("Testing QR solve...");

    using namespace matrices_test;

    setup();

    Real tol = 1.0e-12;
    MersenneTwisterUniformRng rng(1234);
    Matrix bigM(50, 100, 0.0);
    for (Size i=0; i < std::min(bigM.rows(), bigM.columns()); ++i) {
        bigM[i][i] = i+1.0;
    }

    Matrix randM(50, 200);
    for (Size i=0; i < randM.rows(); ++i)
        for (Size j=0; j < randM.columns(); ++j)
            randM[i][j] = rng.next().value;

    Matrix testMatrices[] = {M1, M2, M3, transpose(M3),
                              M4, transpose(M4), M5, I, M7,
                              bigM, transpose(bigM),
                              randM, transpose(randM) };

    for (const auto& A : testMatrices) {
        Array b(A.rows());

        for (Size k=0; k < 10; ++k) {
            for (Real& iter : b) {
                iter = rng.next().value;
            }
            const Array x = qrSolve(A, b, true);

            if (A.columns() >= A.rows()) {
                if (norm(A*x - b) > tol)
                    BOOST_FAIL("A*x does not match vector b (norm = "
                               << norm(A*x - b) << ")");
            }
            else {
                // use the SVD to calculate the reference values
                const Size n = A.columns();
                Array xr(n, 0.0);

                SVD svd(A);
                const Matrix& V = svd.V();
                const Matrix& U = svd.U();
                const Array&  w = svd.singularValues();
                const Real threshold = n*QL_EPSILON;

                for (Size i=0; i<n; ++i) {
                    if (w[i] > threshold) {
                        const Real u = std::inner_product(U.column_begin(i),
                                                          U.column_end(i),
                                                          b.begin(), Real(0.0))/w[i];

                        for (Size j=0; j<n; ++j) {
                            xr[j]  +=u*V[j][i];
                        }
                    }
                }

                if (norm(xr-x) > tol) {
                    BOOST_FAIL("least square solution does not match (norm = "
                               << norm(x - xr) << ")");

                }
            }
        }
    }
}

void MatricesTest::testInverse() {

    BOOST_TEST_MESSAGE("Testing LU inverse calculation...");

    using namespace matrices_test;

    setup();

    Real tol = 1.0e-12;
    Matrix testMatrices[] = { M1, M2, I, M5 };

    for (const auto& A : testMatrices) {
        const Matrix invA = inverse(A);

        const Matrix I1 = invA*A;
        const Matrix I2 = A*invA;

        Matrix identity(A.rows(), A.rows(), 0.0);
        for (Size i=0; i < A.rows(); ++i) identity[i][i] = 1.0;

        if (norm(I1 - identity) > tol)
            BOOST_FAIL("inverse(A)*A does not recover unit matrix (norm = "
                       << norm(I1-identity) << ")");

        if (norm(I2 - identity) > tol)
            BOOST_FAIL("A*inverse(A) does not recover unit matrix (norm = "
                       << norm(I1-identity) << ")");
    }
}

void MatricesTest::testDeterminant() {

    BOOST_TEST_MESSAGE("Testing LU determinant calculation...");

    using namespace matrices_test;

    setup();
    Real tol = 1e-10;

    Matrix testMatrices[] = {M1, M2, M5, M6, I};
    // expected results calculated with octave
    Real expected[] = { 0.044, -0.012, 5.0, 5.7621e-11, 1.0};

    for (Size j=0; j<LENGTH(testMatrices); ++j) {
        const Real calculated = determinant(testMatrices[j]);
        if (std::fabs(expected[j] - calculated) > tol)
            BOOST_FAIL("determinant calculation failed "
                       << "\n matrix     :\n" << testMatrices[j]
                       << "\n calculated : " << calculated
                       << "\n expected   : " << expected[j]);
    }

    MersenneTwisterUniformRng rng(1234);
    for (Size j=0; j<100; ++j) {
        Matrix m(3, 3, 0.0);
        for (Real& iter : m)
            iter = rng.next().value;

        if ((j % 3) == 0U) {
            // every third matrix is a singular matrix
            Size row = Size(3*rng.next().value);
            std::fill(m.row_begin(row), m.row_end(row), 0.0);
        }

        Real a=m[0][0];
        Real b=m[0][1];
        Real c=m[0][2];
        Real d=m[1][0];
        Real e=m[1][1];
        Real f=m[1][2];
        Real g=m[2][0];
        Real h=m[2][1];
        Real i=m[2][2];

        const Real expected = a*e*i+b*f*g+c*d*h-(g*e*c+h*f*a+i*d*b);
        const Real calculated = determinant(m);

        if (std::fabs(expected-calculated) > tol)
            BOOST_FAIL("determinant calculation failed "
                       << "\n matrix     :\n" << m
                       << "\n calculated : " << calculated
                       << "\n expected   : " << expected);
    }
}

void MatricesTest::testOrthogonalProjection() {
    BOOST_TEST_MESSAGE("Testing orthogonal projections...");

    Size dimension = 1000;
    Size numberVectors = 50;
    Real multiplier = 100;
    Real tolerance = 1e-6;
    unsigned long seed = 1;

    Real errorAcceptable = 1E-11;

    Matrix test(numberVectors,dimension);

    MersenneTwisterUniformRng rng(seed);

    for (Size i=0; i < numberVectors; ++i)
        for (Size j=0; j < dimension; ++j)
            test[i][j] = rng.next().value;

    OrthogonalProjections projector(test,
                                    multiplier,
                                    tolerance  );

    Size numberFailures =0;
    Size failuresTwo=0;

    for (Size i=0; i < numberVectors; ++i)
    {
        // check output vector i is orthogonal to all other vectors

        if (projector.validVectors()[i])
        {
            for (Size j=0; j < numberVectors; ++j)
                  if (projector.validVectors()[j] && i != j)
                  {
                      Real dotProduct=0.0;
                      for (Size k=0; k < dimension; ++k)
                          dotProduct += test[j][k]*projector.GetVector(i)[k];

                      if (fabs(dotProduct) > errorAcceptable)
                          ++numberFailures;

                  }

           Real innerProductWithOriginal =0.0;
           Real normSq =0.0;

           for (Size j=0; j < dimension; ++j)
           {
                innerProductWithOriginal +=   projector.GetVector(i)[j]*test[i][j];
                normSq += test[i][j]*test[i][j];
           }

           if (fabs(innerProductWithOriginal-normSq) > errorAcceptable)
               ++failuresTwo;

        }

    }

    if (numberFailures > 0 || failuresTwo >0)
        BOOST_FAIL("OrthogonalProjections test failed with " << numberFailures << " failures  of orthogonality and "
                    << failuresTwo << " failures of projection size.");

}

void MatricesTest::testCholeskyDecomposition() {

    BOOST_TEST_MESSAGE("Testing Cholesky Decomposition...");

    // This test case fails prior to release 1.8

    // The eigenvalues of this matrix are
    // 0.0438523; 0.0187376; 0.000245617; 0.000127656; 8.35899e-05; 6.14215e-05;
    // 1.94241e-05; 1.14417e-06; 9.79481e-18; 1.31141e-18; 5.81155e-19

    Real tmp[11][11] = {
        {6.4e-05, 5.28e-05, 2.28e-05, 0.00032, 0.00036, 6.4e-05,
         6.3968010664e-06, 7.2e-05, 7.19460269899e-06, 1.2e-05,
         1.19970004999e-06},
        {5.28e-05, 0.000121, 1.045e-05, 0.00044, 0.000165, 2.2e-05,
         2.19890036657e-06, 1.65e-05, 1.64876311852e-06, 1.1e-05,
         1.09972504583e-06},
        {2.28e-05, 1.045e-05, 9.025e-05, 0, 0.0001425, 9.5e-06,
         9.49525158294e-07, 2.85e-05, 2.84786356835e-06, 4.75e-06,
         4.74881269789e-07},
        {0.00032, 0.00044, 0, 0.04, 0.009, 0.0008, 7.996001333e-05, 0.0006,
         5.99550224916e-05, 0.0001, 9.99750041661e-06},
        {0.00036, 0.000165, 0.0001425, 0.009, 0.0225, 0.0003, 2.99850049987e-05,
         0.001125, 0.000112415667172, 0.000225, 2.24943759374e-05},
        {6.4e-05, 2.2e-05, 9.5e-06, 0.0008, 0.0003, 0.0001, 9.99500166625e-06,
         7.5e-05, 7.49437781145e-06, 2e-05, 1.99950008332e-06},
        {6.3968010664e-06, 2.19890036657e-06, 9.49525158294e-07,
         7.996001333e-05, 2.99850049987e-05, 9.99500166625e-06,
         9.99000583083e-07, 7.49625124969e-06, 7.49063187129e-07,
         1.99900033325e-06, 1.99850066645e-07},
        {7.2e-05, 1.65e-05, 2.85e-05, 0.0006, 0.001125, 7.5e-05,
         7.49625124969e-06, 0.000225, 2.24831334343e-05, 1.5e-05,
         1.49962506249e-06},
        {7.19460269899e-06, 1.64876311852e-06, 2.84786356835e-06,
         5.99550224916e-05, 0.000112415667172, 7.49437781145e-06,
         7.49063187129e-07, 2.24831334343e-05, 2.24662795123e-06,
         1.49887556229e-06, 1.49850090584e-07},
        {1.2e-05, 1.1e-05, 4.75e-06, 0.0001, 0.000225, 2e-05, 1.99900033325e-06,
         1.5e-05, 1.49887556229e-06, 2.5e-05, 2.49937510415e-06},
        {1.19970004999e-06, 1.09972504583e-06, 4.74881269789e-07,
         9.99750041661e-06, 2.24943759374e-05, 1.99950008332e-06,
         1.99850066645e-07, 1.49962506249e-06, 1.49850090584e-07,
         2.49937510415e-06, 2.49875036451e-07}};

    Matrix m(11,11);
    for(Size i=0;i<11;++i) {
        for(Size j=0;j<11;++j) {
            m[i][j] = tmp[i][j];
        }
    }

    Matrix c = CholeskyDecomposition(m,true);
    Matrix m2 = c * transpose(c);

    Real tol = 1.0E-12;
    for(Size i=0;i<11;++i) {
        for(Size j=0;j<11;++j) {
            if(std::isnan(m2[i][j])) {
                BOOST_FAIL("Faield to verify Cholesky decomposition at (i,j)=("
                           << i << "," << j << "), replicated value is nan");
            }
            // this does not detect nan values
            if(std::abs(m[i][j]-m2[i][j]) > tol) {
                BOOST_FAIL("Failed to verify Cholesky decomposition at (i,j)=("
                           << i << "," << j << "), original value is "
                           << m[i][j] << ", replicated value is " << m2[i][j]);
            }
        }
    }
}

void MatricesTest::testMoorePenroseInverse() {

    BOOST_TEST_MESSAGE("Testing Moore-Penrose inverse...");

    // this is taken from
    // http://de.mathworks.com/help/matlab/ref/pinv.html
    Real tmp[8][6] = {{64, 2, 3, 61, 60, 6},    {9, 55, 54, 12, 13, 51},
                      {17, 47, 46, 20, 21, 43}, {40, 26, 27, 37, 36, 30},
                      {32, 34, 35, 29, 28, 38}, {41, 23, 22, 44, 45, 19},
                      {49, 15, 14, 52, 53, 11}, {8, 58, 59, 5, 4, 62}};
    Matrix A(8, 6);
    for (Size i = 0; i < 8; ++i) {
        for (Size j = 0; j < 6; ++j) {
            A(i, j) = tmp[i][j];
        }
    }

    Matrix P = moorePenroseInverse(A);
    Array b(8, 260.0);
    Array x = P*b;

    Real cached[6] = {1.153846153846152, 1.461538461538463, 1.384615384615384,
                      1.384615384615385, 1.461538461538462, 1.153846153846152};
    constexpr double tol = 500.0 * QL_EPSILON;

    for (Size i = 0; i < 6; ++i) {
        if (std::abs(x[i] - cached[i]) > tol) {
            BOOST_FAIL("Failed to verify minimal norm solution obtained from "
                       "Moore-Penrose-Inverse against cached results, component "
                       << i << " is " << x[i] << ", expected " << cached[i]
                       << ", difference " << x[i] - cached[i] << ", tolerance "
                       << tol);
        }
    }

    Array y = A*x;
    constexpr double tol2 = 2000.0 * QL_EPSILON;
    for (Size i = 0; i < 6; ++i) {
        if (std::abs(y[i] - 260.0) > tol2) {
            BOOST_FAIL(
                "Failed to verify minimal norm solution obtained from "
                "Moore-Penrose-Inverse when back-substituting, rhs component "
                << i << " is " << y[i] << ", expected 260.0, difference "
                << y[i] - 260.0 << ", tolerance " << tol2);
        }
    }

}


namespace matrices_test {
    class MatrixMult {
      public:
        explicit MatrixMult(Matrix m) : m_(std::move(m)) {}
        Array operator()(const Array& x) const {
            return m_ * x;
        }

      private:
        const Matrix m_;
    };

    Real norm2(const Array& x) {
        return std::sqrt(DotProduct(x,x));
    }
}

void MatricesTest::testIterativeSolvers() {
    BOOST_TEST_MESSAGE("Testing iterative solvers...");

    using namespace matrices_test;

    setup();

    Array b(3);
    b[0] = 1.0; b[1] = 0.5; b[2] = 3.0;

    constexpr double relTol = 1e4 * QL_EPSILON;

    const Array x = BiCGstab(MatrixMult(M1), 3, relTol).solve(b).x;
    if (norm2(M1*x-b)/norm2(b) > relTol) {
        BOOST_FAIL("Failed to calculate inverse using BiCGstab"
                << "\n  rel error     : " << norm2(M1*x-b)/norm2(b)
                << "\n  rel tolerance : " << relTol);
    }

    const GMRESResult u = GMRES(MatrixMult(M1), 3, relTol).solve(b, b);
    if (norm2(M1*u.x-b)/norm2(b) > relTol) {
        BOOST_FAIL("Failed to calculate inverse using gmres"
                << "\n  rel error     : " << norm2(M1*u.x-b)/norm2(b)
                << "\n  rel tolerance : " << relTol);
    }
    const Array errors = Array(u.errors.begin(), u.errors.end());
    for (Real error : errors) {
        const Array x = GMRES(MatrixMult(M1), 10, 1.01 * error).solve(b, b).x;

        const Real calculated = norm2(M1*x-b)/norm2(b);
        const Real expected = error;

        if (std::fabs(calculated - expected) > relTol) {
            BOOST_FAIL("Failed to calculate solution error"
                    << "\n  calculated error: " << calculated
                    << "\n  expected error  : " << expected);
        }
    }

    const Array v = GMRES(MatrixMult(M1), 1, relTol,
        MatrixMult(inverse(M1))).solve(b, b).x;

    if (norm2(M1*v-b)/norm2(b) > relTol) {
        BOOST_FAIL("Failed to calculate inverse using gmres "
                   "with exact preconditioning"
                << "\n  rel error     : " << norm2(M1*v-b)/norm2(b)
                << "\n  rel tolerance : " << relTol);
    }

    const Array w = GMRES(MatrixMult(M1), 3, relTol,
        MatrixMult(M1)).solve(b, b).x;
    if (norm2(M1*w-b)/norm2(b) > relTol) {
        BOOST_FAIL("Failed to calculate inverse using gmres "
                   "with nonsense preconditioning"
                << "\n  rel error     : " << norm2(M1*w-b)/norm2(b)
                << "\n  rel tolerance : " << relTol);
    }
}

void MatricesTest::testInitializers() {
    BOOST_TEST_MESSAGE("Testing matrix initializers...");

    Matrix m1 = {};
    BOOST_REQUIRE(m1.rows() == 0);
    BOOST_REQUIRE(m1.columns() == 0);

    Matrix m2 = {
        {1.0, 2.0, 3.0},
        {4.0, 5.0, 6.0}
    };
    BOOST_REQUIRE(m2.rows() == 2);
    BOOST_REQUIRE(m2.columns() == 3);
    BOOST_CHECK_EQUAL(m2(0, 0), 1.0);
    BOOST_CHECK_EQUAL(m2(0, 1), 2.0);
    BOOST_CHECK_EQUAL(m2(0, 2), 3.0);
    BOOST_CHECK_EQUAL(m2(1, 0), 4.0);
    BOOST_CHECK_EQUAL(m2(1, 1), 5.0);
    BOOST_CHECK_EQUAL(m2(1, 2), 6.0);
}


namespace {

    typedef std::pair< std::pair< std::vector<Size>, std::vector<Size> >,
                   std::vector<Real> > coordinate_tuple;

    coordinate_tuple sparseMatrixToCoordinateTuple(const SparseMatrix& m) {
        std::vector<Size> row_idx, col_idx;
        std::vector<Real> data;
        for (auto iter1 = m.begin1(); iter1 != m.end1(); ++iter1)
            for (auto iter2 = iter1.begin(); iter2 != iter1.end(); ++iter2) {
                row_idx.push_back(iter1.index1());
                col_idx.push_back(iter2.index2());
                data.push_back(*iter2);
            }

        return std::make_pair(std::make_pair(row_idx, col_idx), data);
    }

}

void MatricesTest::testSparseMatrixMemory() {

    BOOST_TEST_MESSAGE("Testing sparse matrix memory layout...");

    SparseMatrix m(8, 4);
    BOOST_CHECK_EQUAL(m.filled1(), 1);
    BOOST_CHECK_EQUAL(m.size1(), 8);
    BOOST_CHECK_EQUAL(m.size2(), 4);
    BOOST_CHECK_EQUAL(std::distance(m.begin1(), m.end1()), m.size1());

    auto coords = sparseMatrixToCoordinateTuple(m);
    BOOST_CHECK_EQUAL(coords.first.first.size(), 0);

    m(3, 1) = 42;
    coords = sparseMatrixToCoordinateTuple(m);
    BOOST_CHECK_EQUAL(std::distance(m.begin1(), m.end1()), m.size1());
    BOOST_CHECK_EQUAL(coords.first.first.size(), 1);
    BOOST_CHECK_EQUAL(coords.first.first[0], 3);
    BOOST_CHECK_EQUAL(coords.first.second[0], 1);
    BOOST_CHECK_EQUAL(coords.second[0], 42);

    m(1, 2) = 6;
    coords = sparseMatrixToCoordinateTuple(m);
    BOOST_CHECK_EQUAL(coords.first.first.size(), 2);
    BOOST_CHECK_EQUAL(coords.first.first[0], 1);
    BOOST_CHECK_EQUAL(coords.first.second[0], 2);
    BOOST_CHECK_EQUAL(coords.second[0], 6);

    Array x{1, 2, 3, 4};
    Array y = prod(m, x);
    BOOST_CHECK_EQUAL(y, Array({0, 18, 0, 84}));

    m(3, 2) = 43;
    coords = sparseMatrixToCoordinateTuple(m);
    BOOST_CHECK_EQUAL(coords.first.first.size(), 3);
    BOOST_CHECK_EQUAL(coords.first.first[2], 3);
    BOOST_CHECK_EQUAL(coords.first.second[2], 2);
    BOOST_CHECK_EQUAL(coords.second[2], 43);

    m(7, 3) = 44;
    coords = sparseMatrixToCoordinateTuple(m);
    BOOST_CHECK_EQUAL(coords.first.first.size(), 4);
    BOOST_CHECK_EQUAL(coords.first.first[3], 7);
    BOOST_CHECK_EQUAL(coords.first.second[3], 3);
    BOOST_CHECK_EQUAL(coords.second[3], 44);

    Size entries(0);
    for (auto iter1 = m.begin1(); iter1 != m.end1(); ++iter1)
        entries+=std::distance(iter1.begin(), iter1.end());

    BOOST_CHECK_EQUAL(entries, 4);

}

test_suite* MatricesTest::suite() {
    auto* suite = BOOST_TEST_SUITE("Matrix tests");

    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testOrthogonalProjection));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testEigenvectors));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testSqrt));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testSVD));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testHighamSqrt));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testQRDecomposition));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testQRSolve));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testInverse));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testDeterminant));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testSparseMatrixMemory));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testCholeskyDecomposition));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testMoorePenroseInverse));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testIterativeSolvers));
    suite->add(QUANTLIB_TEST_CASE(&MatricesTest::testInitializers));
    return suite;
}

