/** This is free and unencumbered software released into the public domain.
The authors of ISIS do not claim copyright on the contents of this file.
For more details about the LICENSE terms and the AUTHORS, you will
find files of those names at the top level of this repository. **/

/* SPDX-License-Identifier: CC0-1.0 */

// std library
#include <cstdlib>

// boost library
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

// Qt Library
#include <QDebug>

// Isis library
#include "Angle.h"
#include "Constants.h"
#include "IException.h"
#include "LinearAlgebra.h"
#include "Preference.h"

using namespace Isis;
using namespace std;

namespace Isis {
  /**
   * Test child class created to test constructor/destructor.
   */
  class TestLinearAlgebra : public LinearAlgebra {
    public:
      TestLinearAlgebra() : LinearAlgebra() {
        qDebug() << "***************************************************";
        qDebug() << "Test Protected Constructor...";
        qDebug() << "***************************************************";
      }
      ~TestLinearAlgebra() {
      }
  };
};

/**
 * Unit Test for LinearAlgebra class.
 *
 * @author 2013-06-01 Jeannie Backer
 *
 * @internal
 *   @history 2013-06-01 Jeannie Backer - Original version.
 */
int main(int argc, char *argv[]) {
  Isis::Preference::Preferences(true);

  try {
    LinearAlgebra::Matrix emptyMatrix(0, 0);
    qDebug() << "UnitTest for LinearAlgebra" << endl;

    qDebug() << "";
    TestLinearAlgebra constructorTest;
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test identity() method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Matrix id3x3 = LinearAlgebra::identity(3);
    qDebug() << id3x3;
    qDebug() << "";
    try { // error message
      LinearAlgebra::identity(-1);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test zeroMatrix() method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Matrix z3x3 = LinearAlgebra::zeroMatrix(3, 3);
    qDebug() << z3x3;
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test 3D vector setters...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector i = LinearAlgebra::vector(1.0, 0.0, 0.0);
    qDebug() << "i = vector(1,0,0) = " << i;
    LinearAlgebra::Vector j = LinearAlgebra::vector(0.0, 1.0, 0.0);
    qDebug() << "j = vector(0,1,0) = " << j;
    LinearAlgebra::Vector v456(3);
    LinearAlgebra::setVec3(&v456, 4.0, 5.0, 6.0);
    qDebug() << "setVec3(v, 4, 5, 6) = " << v456;
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test 4D vector setters...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector v1234 = LinearAlgebra::vector(1.0, 2.0, 3.0, 4.0);
    qDebug() << "vector(1, 2, 3, 4) = " << v1234;
    LinearAlgebra::setVec4(&v1234, 5.0, 6.0, 7.0, 8.0);
    qDebug() << "setVec4(v, 5, 6, 7, 8) = " << v1234;
    qDebug() << "";

    qDebug() << "";
    qDebug() << "**************************************************************************";
    qDebug() << "Test subVector(vector, startIndex, size) method...";
    qDebug() << "**************************************************************************";
    LinearAlgebra::Vector subVector = LinearAlgebra::subVector(v1234, 1, 2);
    qDebug() << "subVector(v5678, 1, 2) = " << subVector;
    qDebug() << "";

    // create more matrices to be used later...
    qDebug() << "";
    qDebug() << "*****************************************************************************";
    qDebug() << "Test setRow(), row(), setColumn() and column() methods...";
    qDebug() << "*****************************************************************************";
    // non-sqare empty matrices
    LinearAlgebra::Matrix m2x3(2, 3);
    LinearAlgebra::Matrix m3x2(3, 2);
    LinearAlgebra::setRow(m2x3, i, 0);
    LinearAlgebra::setRow(m2x3, j, 1);
    qDebug() << m2x3;
    qDebug() << "column 2 (index 1) = " << LinearAlgebra::column(m2x3, 1);
    qDebug() << "";

    LinearAlgebra::setColumn(m3x2, -i, 0);
    LinearAlgebra::setColumn(m3x2, -j, 1);
    qDebug() << m3x2;
    qDebug() << "row 2 (index 1) = " << LinearAlgebra::row(m3x2, 1);
    qDebug() << "";

    //2x2 rotation
    LinearAlgebra::Matrix rot2x2 = LinearAlgebra::identity(2);
    LinearAlgebra::Vector col1 = LinearAlgebra::column(rot2x2, 0);
    LinearAlgebra::Vector col2 = LinearAlgebra::column(rot2x2, 1);
    LinearAlgebra::setColumn(rot2x2, -col2, 0);
    LinearAlgebra::setColumn(rot2x2,  col1, 1);
    //3x3 rotation kij
    LinearAlgebra::Matrix rot3x3 = LinearAlgebra::identity(3);
    LinearAlgebra::setColumn(rot3x3, LinearAlgebra::column(id3x3, 2), 0);
    LinearAlgebra::setColumn(rot3x3, i, 1);
    LinearAlgebra::setColumn(rot3x3, j, 2);
    // a 2x2 matrix of ones
    LinearAlgebra::Matrix ones = LinearAlgebra::identity(2);
    ones(0, 1) = 1.0; ones(1, 0) = 1.0;
    // create a matrix that has unitized column vecotors but determinant less than 1
    LinearAlgebra::Matrix notRot = ones / sqrt(2);
    // create a matrix that has unitized column vecotors but determinant greater than 1
    // not mathematically possible???

    try { // setRow index out of bounds
      LinearAlgebra::setRow(m2x3, j, 2);
    }
    catch (IException &e) {
      e.print();
    }
    try { // setColumn index out of bounds
      LinearAlgebra::setColumn(m2x3, j, 3);
    }
    catch (IException &e) {
      e.print();
    }
    try { // row index out of bounds
      LinearAlgebra::row(m2x3, 2);
    }
    catch (IException &e) {
      e.print();
    }
    try { // column index out of bounds
      LinearAlgebra::column(m2x3, 3);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test isIdentity() methods...";
    qDebug() << "***************************************************";
    qDebug() << "Is emptyMatrix identity?" << LinearAlgebra::isIdentity(emptyMatrix);
    qDebug() << "Is 3x3 identity matrix identity?" << LinearAlgebra::isIdentity(id3x3);
    qDebug() << "Is 3x3 2*identity matrix identity?" << LinearAlgebra::isIdentity(id3x3 * 2.0);
    qDebug() << "Is 2x2 ones matrix identity?" << LinearAlgebra::isIdentity(ones);
    qDebug() << "Is 2x3 matrix identity?" << LinearAlgebra::isIdentity(m2x3);
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test isOrthogonal() method...";
    qDebug() << "***************************************************";
    qDebug() << "Is emptyMatrix orthogonal?" << LinearAlgebra::isOrthogonal(emptyMatrix);
    qDebug() << "Is 3x3 identity matrix orthogonal?" << LinearAlgebra::isOrthogonal(id3x3);
    qDebug() << "Is [0 -1; 1 0] orthogonal?" << LinearAlgebra::isOrthogonal(rot2x2);
    qDebug() << "Is 1/sqrt2 * [1 1; 1 1] orthogonal?" << LinearAlgebra::isOrthogonal(notRot);
    qDebug() << "Is 3x3 zero matrix orthogonal?" << LinearAlgebra::isOrthogonal(z3x3);
    qDebug() << "Is 2x3 matrix orthogonal?" << LinearAlgebra::isOrthogonal(m2x3);
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test isRotationMatrix() method...";
    qDebug() << "***************************************************";
    qDebug() << "Is 3x3 identity matrix rotation matrix?" << LinearAlgebra::isRotationMatrix(id3x3);
    qDebug() << "Is 2x2 identity matrix rotation matrix?" << LinearAlgebra::isRotationMatrix(LinearAlgebra::identity(2));
    qDebug() << "Is [0 -1; 1 0] rotation?" << LinearAlgebra::isRotationMatrix(rot2x2);
    qDebug() << "Is 1/sqrt2 * [1 1; 1 1] rotation?" << LinearAlgebra::isRotationMatrix(notRot);
    qDebug() << "Is 3x3 2*identity matrix rotation matrix?" << LinearAlgebra::isRotationMatrix(id3x3 * 2.0);
    qDebug() << "Is 3x3 zero matrix rotation matrix?" << LinearAlgebra::isRotationMatrix(z3x3);
    qDebug() << "Is 2x3 matrix rotation matrix?" << LinearAlgebra::isRotationMatrix(m2x3);
    try { // method works for 2x2 or 3x3 only
      LinearAlgebra::isRotationMatrix(LinearAlgebra::identity(4));
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test isZero(matrix) method...";
    qDebug() << "***************************************************";
    qDebug() << "Is emptyMatrix zero?" << LinearAlgebra::isZero(emptyMatrix);
    qDebug() << "Is 3x3 zero matrix zero?" << LinearAlgebra::isZero(z3x3);
    qDebug() << "Is 3x3 identity matrix zero?" << LinearAlgebra::isZero(id3x3);
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test determinant() method...";
    qDebug() << "***************************************************";
    qDebug() << "Determinant of 3x3 2*identity = " << LinearAlgebra::determinant(id3x3 * 2.0);
    qDebug() << "Determinant of 2x2 2*identity = " << LinearAlgebra::determinant(LinearAlgebra::identity(2) * 2.0);
    try { // method works for 2x2 or 3x3 only
      LinearAlgebra::determinant(LinearAlgebra::identity(4) * 2.0);
    }
    catch (IException &e) {
      e.print();
    }
    try {
      LinearAlgebra::determinant(m2x3);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test transpose() method...";
    qDebug() << "***************************************************";
    qDebug() << "Input matrix";
    id3x3(0, 1) = 2;
    qDebug() << id3x3;
    qDebug() << "";
    LinearAlgebra::Matrix mT = LinearAlgebra::transpose(id3x3);
    qDebug() << "transpose matrix";
    qDebug() << mT;
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test inverse() method...";
    qDebug() << "***************************************************";
    qDebug() << "The inverse of the rotation matrix is the transpose. ";
    qDebug() << "rotation matrix = ";
    qDebug() << rot2x2;
    qDebug() << "inverse = ";
    LinearAlgebra::Matrix invRot2x2 = LinearAlgebra::inverse(rot2x2);
    qDebug() << invRot2x2;
    qDebug() << "other matrix = ";
    qDebug() << id3x3;
    qDebug() << "inverse = ";
    LinearAlgebra::Matrix inv = LinearAlgebra::inverse(id3x3);
    qDebug() << inv;
    try {
      // determinant zero
      LinearAlgebra::inverse(notRot);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    // vector with no elements
    LinearAlgebra::Vector emptyVector;
    // zero vector with 3 elements
    LinearAlgebra::Vector z3 = LinearAlgebra::zeroVector(3);
    // vector [0 1 2]
    LinearAlgebra::Vector v012(3);
    for (unsigned int i = 0; i < 3; i++) {
      v012(i) = i;
    }

    LinearAlgebra::Vector w345(3);
    for (unsigned int i = 0; i < 3; i++) {
      w345(i) = i + 3;
    }

    qDebug() << "";
    qDebug() << "v = " << v012;
    qDebug() << "w = " << w345;
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test isZero() method...";
    qDebug() << "***************************************************";
    qDebug() << "Is emptyVector a zero vector? " << LinearAlgebra::isZero(emptyVector);
    qDebug() << "Is zero a zero vector? " << LinearAlgebra::isZero(z3);
    qDebug() << "Is v a zero vector? " << LinearAlgebra::isZero(v012);
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test isEmpty() method...";
    qDebug() << "***************************************************";
    qDebug() << "Is emptyVector an empty vector? " << LinearAlgebra::isEmpty(emptyVector);
    qDebug() << "Is zero an empty vector? " << LinearAlgebra::isEmpty(z3);
    qDebug() << "Is v an empty vector? " << LinearAlgebra::isEmpty(v012);
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test isUnit() method...";
    qDebug() << "***************************************************";
    qDebug() << "Is emptyVector a unit vector? " << LinearAlgebra::isUnit(emptyVector);
    qDebug() << "Is zero a unit vector? " << LinearAlgebra::isUnit(z3);
    qDebug() << "Is v a unit vector? " << LinearAlgebra::isUnit(v012);
    qDebug() << "Is i a unit vector? " << LinearAlgebra::isUnit(i);
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test magnitude() method...";
    qDebug() << "***************************************************";
    qDebug() << "Magnitude of zero = " << LinearAlgebra::magnitude(z3);
    qDebug() << "Magnitude of v = " << LinearAlgebra::magnitude(v012);
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test absoluteMaximum() method...";
    qDebug() << "***************************************************";
    qDebug() << "Max Norm of v = " << LinearAlgebra::absoluteMaximum(v012);
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test normalize() method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector u012 = LinearAlgebra::normalize(v012);
    qDebug() << "u = " << u012;
    try {// cannot normalize zero vector
      LinearAlgebra::normalize(z3);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test multiply(matrix1, matrix2) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Matrix matrixMult = LinearAlgebra::multiply(mT, id3x3);
    qDebug() << "MT * M = ";
    qDebug() << matrixMult;
    qDebug() << "";
    try {// left columns must match right rows
      LinearAlgebra::multiply(id3x3, m2x3);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test multiply(matrix, vector) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector matrixVectorMult = LinearAlgebra::multiply(id3x3, v012);
    cout << "M * v = " << matrixVectorMult << endl; // test cout with boost vector
    try { // matrix columns must equal vector size
      LinearAlgebra::multiply(LinearAlgebra::transpose(m2x3), v012);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test multiply(scalar, vector) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector scalarVectorMult = LinearAlgebra::multiply(2.0, v012);
    qDebug() << "2.0 * v = " << scalarVectorMult;
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test multiply(scalar, matrix) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Matrix scalarMatrixMult = LinearAlgebra::multiply(2.0, id3x3);
    qDebug() << "2.0 * M = ";
    qDebug() << scalarMatrixMult;
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test add(vector1, vector2) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector vectorAdd = LinearAlgebra::add(u012, v012);
    qDebug() << "u + v = " << vectorAdd;
    try { // sizes must match
      LinearAlgebra::add(emptyVector, v012);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test subtract(vector1, vector2) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector vectorSubtract = LinearAlgebra::subtract(u012, v012);
    qDebug() << "u - v = " << vectorSubtract;
    try { // sizes must match
      LinearAlgebra::subtract(emptyVector, v012);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test crossProduct(vector1, vector2) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector vectorCross = LinearAlgebra::crossProduct(u012, v012);
    qDebug() << "(parallel vectors) u x v = " << vectorCross;
    vectorCross = LinearAlgebra::crossProduct(i, j);
    qDebug() << "(basis vectors) i x j = k = " << vectorCross;
    vectorCross = LinearAlgebra::crossProduct(i, v012);
    qDebug() << "i x v = " << vectorCross;
    vectorCross = LinearAlgebra::crossProduct(v012, i);
    qDebug() << "v x i = " << vectorCross;
    try { // vector1 is not size 3
      LinearAlgebra::crossProduct(emptyVector, v012);
    }
    catch (IException &e) {
      e.print();
    }
    try { // vector2 is not size 3
      LinearAlgebra::crossProduct(v012, emptyVector);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test normalizedCrossProduct(vector1, vector2) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector normVectorCross = LinearAlgebra::normalizedCrossProduct(i, v012);
    qDebug() << "i x v = " << normVectorCross;
    normVectorCross = LinearAlgebra::normalizedCrossProduct(i, u012);
    qDebug() << "i x u = " << normVectorCross;
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test outerProduct(vector1, vector2) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Matrix vectorOuter = LinearAlgebra::outerProduct(u012, v012);
    qDebug() << vectorOuter;
    try { // vectors must be same size
      LinearAlgebra::outerProduct(v012, emptyVector);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test dotProduct(vector1, vector2) method...";
    qDebug() << "***************************************************";
    qDebug() << "u . v = " << LinearAlgebra::dotProduct(u012, v012);
    try { // vectors must be same size
      LinearAlgebra::dotProduct(v012, emptyVector);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test innerProduct(vector1, vector2) method...";
    qDebug() << "***************************************************";
    qDebug() << "<u,v> = " << LinearAlgebra::innerProduct(u012, v012);
    try { // vectors must be same size
      LinearAlgebra::innerProduct(v012, emptyVector);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test project(vector1, vector2) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector vectorProj = LinearAlgebra::project(u012, v012);
    qDebug() << "Proj(u, v) = " << vectorProj;
    LinearAlgebra::Vector vectorProjZ = LinearAlgebra::project(u012, z3);
    qDebug() << "Proj(u, zero) is zero? " << LinearAlgebra::isZero(vectorProjZ);
    try { // vectors must be same size
      LinearAlgebra::project(emptyVector, v012);
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test rotate(vector1, vector2, theta) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector vectorRotate = LinearAlgebra::rotate(u012, v012, Angle(HALFPI, Angle::Radians));
    qDebug() << "rotate(u,v,PI/2) = " << vectorRotate;
    LinearAlgebra::Vector vectorRotateZ = LinearAlgebra::rotate(u012, z3, Angle(HALFPI, Angle::Radians));
    qDebug() << "rotate(u,zero,PI/2) = " << vectorRotateZ;
    try { // vector size must be 3
      LinearAlgebra::rotate(emptyVector, v012, Angle(HALFPI, Angle::Radians));
    }
    catch (IException &e) {
      e.print();
    }
    try { // axis size must be 3
      LinearAlgebra::rotate(v012, emptyVector, Angle(HALFPI, Angle::Radians));
    }
    catch (IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test perpendicular(vector1, vector2) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector perpendicularVector = LinearAlgebra::perpendicular(z3, v012);
    qDebug() << "perpendicular(zero, [0 1 2]) = " << perpendicularVector;
    perpendicularVector = LinearAlgebra::perpendicular(v012, z3);
    qDebug() << "perpendicular([0 1 2], zero) = " << perpendicularVector;
    perpendicularVector = LinearAlgebra::perpendicular(v012, u012);
    qDebug() << "perpendicular([0 1 2], c*[0 1 2]) = " << perpendicularVector;
    perpendicularVector = LinearAlgebra::perpendicular(v012, w345);
    qDebug() << "perpendicular([0 1 2], [3 4 5]) = " << perpendicularVector;
    qDebug() << "verify: [0 1 2] - perp = " << v012 - perpendicularVector;
    qDebug() << "        is parallel to [3 4 5] since 0.28*[3 4 5 ] = " << (0.28 * w345);
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test toMatrix(quaternion) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector idQ(4);
    idQ(0) = 1;    idQ(1) = 0;    idQ(2) = 0;    idQ(3) = 0;
    LinearAlgebra::Vector q1(4);
    q1(0) = 0;    q1(1) = 1;    q1(2) = 0;    q1(3) = 0;
    LinearAlgebra::Vector q2(4);
    q2(0) = 0;    q2(1) = 0;    q2(2) = 1;    q2(3) = 0;
    LinearAlgebra::Vector q3(4);
    q3(0) = 0;    q3(1) = 0;    q3(2) = 0;    q3(3) = 1;
    LinearAlgebra::Vector q4(4);
    q4(0) = 0.91219099598223; q4(1) = -0.22019944193656;
    q4(2) = -0.2554485981203; q4(3) = -0.23273548577332;
    LinearAlgebra::Vector q5(4);
    q5(0) = 1.0; q5(1) = 1.0; q5(2) = 1.0; q5(3) = 1.0;
    LinearAlgebra::Matrix z2m = LinearAlgebra::toMatrix(LinearAlgebra::zeroVector(4));
    qDebug() << "From the zero quaternion we get identity...";
    // because rotation about origin is just the origin???
    qDebug() << "matrix = ";
    qDebug() << z2m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix id2m = LinearAlgebra::toMatrix(idQ);
    qDebug() << "From the identity quaternion we get identity...";
    qDebug() << "matrix = ";
    qDebug() << id2m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix q12m = LinearAlgebra::toMatrix(q1);
    qDebug() << "From quaternion 1 = " << q1;
    qDebug() << "matrix 1 = ";
    qDebug() << q12m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix q22m = LinearAlgebra::toMatrix(q2);
    qDebug() << "From quaternion 2 = " << q2;
    qDebug() << "matrix 2 = ";
    qDebug() << q22m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix q32m = LinearAlgebra::toMatrix(q3);
    qDebug() << "From quaternion 3 = " << q3;
    qDebug() << "matrix 3 = ";
    qDebug() << q32m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix q42m = LinearAlgebra::toMatrix(q4);
    qDebug() << "From quaternion 4 = " << q4;
    qDebug() << "matrix 4 = ";
    qDebug() << q42m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix q52m = LinearAlgebra::toMatrix(q5);
    qDebug() << "From quaternion 5 (not unit quaternion) = " << q5;
    qDebug() << "matrix 5 = ";
    qDebug() << q52m;
    qDebug() << "--------------------------";
    try { // quaternion must be size 4
      LinearAlgebra::toMatrix(z3);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test toQuaternion(matrix) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::Vector id2q = LinearAlgebra::toQuaternion(LinearAlgebra::identity(3));
    qDebug() << "From the identity matrix we get... ";
    qDebug() << "quaternion = " << id2q;
    qDebug() << "";
    qDebug() << "--------------------------";
    LinearAlgebra::Vector q12m2q = LinearAlgebra::toQuaternion(q12m);
    qDebug() << "From matrix 1 = ";
    qDebug() << q12m;
    qDebug() << "quaternion 1 = " << q12m2q;
    qDebug() << "";
    qDebug() << "--------------------------";
    LinearAlgebra::Vector q22m2q = LinearAlgebra::toQuaternion(q22m);
    qDebug() << "From matrix 2 = ";
    qDebug() << q22m;
    qDebug() << "quaternion 2 = " << q22m2q;
    qDebug() << "";
    qDebug() << "--------------------------";
    LinearAlgebra::Vector q32m2q = LinearAlgebra::toQuaternion(q32m);
    qDebug() << "From matrix 3 = ";
    qDebug() << q32m;
    qDebug() << "quaternion 3 = " << q32m2q;
    qDebug() << "";
    qDebug() << "--------------------------";
    LinearAlgebra::Vector q42m2q = LinearAlgebra::toQuaternion(q42m);
    qDebug() << "From matrix 4 = ";
    qDebug() << q42m;
    qDebug() << "quaternion 4 = " << q42m2q;
    qDebug() << "";
    qDebug() << "--------------------------";
    LinearAlgebra::Vector q52m2q = LinearAlgebra::toQuaternion(q52m);
    qDebug() << "From matrix 5 = ";
    qDebug() << q52m;
    qDebug() << "quaternion 5 = " << q52m2q;
    qDebug() << "";
    qDebug() << "--------------------------";
    try { // matrix must be 3x3
      LinearAlgebra::toQuaternion(m2x3);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try { // matrix must be 3x3
      LinearAlgebra::toQuaternion(m3x2);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try { // matrix must be rotation
      LinearAlgebra::toQuaternion(id3x3 * 2.0);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test toAxisAngle(matrix) method...";
    qDebug() << "***************************************************";
    LinearAlgebra::AxisAngle id2aa = LinearAlgebra::toAxisAngle(LinearAlgebra::identity(3));
    qDebug() << "From the identity matrix we get...";
    qDebug() << "axis = " << id2aa.first;
    qDebug() << "angle = " << id2aa.second.radians();
    qDebug() << "";
    qDebug() << "--------------------------";
    LinearAlgebra::AxisAngle q12m2aa = LinearAlgebra::toAxisAngle(q12m);
    qDebug() << "From matrix 1 = ";
    qDebug() << q12m;
    qDebug() << "axis 1 = " << q12m2aa.first;
    qDebug() << "angle 1 = " << q12m2aa.second.radians();
    qDebug() << "";
    qDebug() << "--------------------------";
    LinearAlgebra::AxisAngle q22m2aa = LinearAlgebra::toAxisAngle(q22m);
    qDebug() << "From matrix 2 = ";
    qDebug() << q22m;
    qDebug() << "axis 2 = " << q22m2aa.first;
    qDebug() << "angle 2 = " << q22m2aa.second.radians();
    qDebug() << "";
    qDebug() << "--------------------------";
    LinearAlgebra::AxisAngle q32m2aa = LinearAlgebra::toAxisAngle(q32m);
    qDebug() << "From matrix 3 = ";
    qDebug() << q32m;
    qDebug() << "axis 3 = " << q32m2aa.first;
    qDebug() << "angle 3 = " << q32m2aa.second.radians();
    qDebug() << "";
    qDebug() << "--------------------------";
    LinearAlgebra::AxisAngle q42m2aa = LinearAlgebra::toAxisAngle(q42m);
    qDebug() << "From matrix 4 = ";
    qDebug() << q42m;
    qDebug() << "axis 4 = " << q42m2aa.first;
    qDebug() << "angle 4 = " << q42m2aa.second.radians();
    qDebug() << "";
    qDebug() << "--------------------------";
    LinearAlgebra::AxisAngle q52m2aa = LinearAlgebra::toAxisAngle(q52m);
    qDebug() << "From matrix 5 = ";
    qDebug() << q52m;
    qDebug() << "axis 5 = " << q52m2aa.first;
    qDebug() << "angle 5 = " << q52m2aa.second.radians();
    qDebug() << "";
    qDebug() << "--------------------------";
    try { // matrix must be 3x3
      LinearAlgebra::toAxisAngle(m2x3);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try {  // matrix must be 3x3
      LinearAlgebra::toAxisAngle(m3x2);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try { // must be rotation matrix
      LinearAlgebra::toAxisAngle(id3x3 * 2.0);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test toMatrix(AxisAngle) and toMatrix(axis, angle) methods...";
    qDebug() << "***************************************************";
    LinearAlgebra::Matrix id2aa2m = LinearAlgebra::toMatrix(id2aa);
    qDebug() << "From the identity axis/angle we get...";
    qDebug() << "axis = " << id2aa.first;
    qDebug() << "angle = " << id2aa.second.radians();
    qDebug() << "matrix = ";
    qDebug() << id2aa2m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix q12m2aa2m = LinearAlgebra::toMatrix(q12m2aa);
    qDebug() << "From axis = " << q12m2aa.first;
    qDebug() << "     angle = " << q12m2aa.second.radians();
    qDebug() << "matrix = ";
    qDebug() << q12m2aa2m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix q22m2aa2m = LinearAlgebra::toMatrix(q22m2aa);
    qDebug() << "From axis = " << q22m2aa.first;
    qDebug() << "     angle = " << q22m2aa.second.radians();
    qDebug() << "matrix = ";
    qDebug() << q22m2aa2m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix q32m2aa2m = LinearAlgebra::toMatrix(q32m2aa);
    qDebug() << "From axis = " << q32m2aa.first;
    qDebug() << "     angle = " << q32m2aa.second.radians();
    qDebug() << "matrix = ";
    qDebug() << q32m2aa2m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix q42m2aa2m = LinearAlgebra::toMatrix(q42m2aa);
    qDebug() << "From axis = " << q42m2aa.first;
    qDebug() << "     angle = " << q42m2aa.second.radians();
    qDebug() << "matrix = ";
    qDebug() << q42m2aa2m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix q52m2aa2m = LinearAlgebra::toMatrix(q52m2aa);
    qDebug() << "From axis = " << q52m2aa.first;
    qDebug() << "     angle = " << q52m2aa.second.radians();
    qDebug() << "matrix = ";
    qDebug() << q52m2aa2m;
    qDebug() << "--------------------------";
    try { // vector must be size 3
      LinearAlgebra::toMatrix(emptyVector, Angle(0.0, Angle::Radians));
    }
    catch(Isis::IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test toEulerAngles(matrix, axes) method...";
    qDebug() << "***************************************************";
    QList<int> axes323;
    axes323 << 3 << 2 << 3;
    QList<LinearAlgebra::EulerAngle> id2ea =
                                     LinearAlgebra::toEulerAngles(LinearAlgebra::identity(3), axes323);
    qDebug() << "From axes 323 and the identity matrix we get...";
    qDebug() << "Euler Angle 1 (angle/axis number) = " << id2ea[0].first.radians() << id2ea[0].second;
    qDebug() << "Euler Angle 2 (angle/axis number) = " << id2ea[1].first.radians() << id2ea[1].second;
    qDebug() << "Euler Angle 3 (angle/axis number) = " << id2ea[2].first.radians() << id2ea[2].second;
    qDebug() << "";
    qDebug() << "--------------------------";
    QList<int> axes313;
    axes313 << 3 << 1 << 3;
    QList<LinearAlgebra::EulerAngle> q12m2ea = LinearAlgebra::toEulerAngles(q12m, axes313);
    qDebug() << "From Axes: " << axes313;
    qDebug() << "     matrix 1 = ";
    qDebug() << q12m;
    qDebug() << "Euler Angle 1 (angle/axis number) = " << q12m2ea[0].first.radians() << q12m2ea[0].second;
    qDebug() << "Euler Angle 2 (angle/axis number) = " << q12m2ea[1].first.radians() << q12m2ea[1].second;
    qDebug() << "Euler Angle 3 (angle/axis number) = " << q12m2ea[2].first.radians() << q12m2ea[2].second;
    qDebug() << "";
    qDebug() << "--------------------------";
    QList<int> axes213;
    axes213 << 2 << 1 << 3;
    QList<LinearAlgebra::EulerAngle> q22m2ea = LinearAlgebra::toEulerAngles(q22m, axes213);
    qDebug() << "From Axes: " << axes213;
    qDebug() << "     matrix 2 = ";
    qDebug() << q22m;
    qDebug() << "Euler Angle 1 (angle/axis number) = " << q22m2ea[0].first.radians() << q22m2ea[0].second;
    qDebug() << "Euler Angle 2 (angle/axis number) = " << q22m2ea[1].first.radians() << q22m2ea[1].second;
    qDebug() << "Euler Angle 3 (angle/axis number) = " << q22m2ea[2].first.radians() << q22m2ea[2].second;
    qDebug() << "";
    qDebug() << "--------------------------";
    QList<int> axes123;
    axes123 << 1 << 2 << 3;
    QList<LinearAlgebra::EulerAngle> q32m2ea = LinearAlgebra::toEulerAngles(q32m, axes123);
    qDebug() << "From Axes: " << axes123;
    qDebug() << "     matrix 3 = ";
    qDebug() << q32m;
    qDebug() << "Euler Angle 1 (angle/axis number) = " << q32m2ea[0].first.radians() << q32m2ea[0].second;
    qDebug() << "Euler Angle 2 (angle/axis number) = " << q32m2ea[1].first.radians() << q32m2ea[1].second;
    qDebug() << "Euler Angle 3 (angle/axis number) = " << q32m2ea[2].first.radians() << q32m2ea[2].second;
    qDebug() << "";
    qDebug() << "--------------------------";
    QList<LinearAlgebra::EulerAngle> rot123 = LinearAlgebra::toEulerAngles(rot3x3, axes123);
    qDebug() << "From Axes: " << axes123;
    qDebug() << "     matrix = ";
    qDebug() << rot3x3;
    qDebug() << "Euler Angle 1 (angle/axis number) = " << rot123[0].first.radians() << rot123[0].second;
    qDebug() << "Euler Angle 2 (angle/axis number) = " << rot123[1].first.radians() << rot123[1].second;
    qDebug() << "Euler Angle 3 (angle/axis number) = " << rot123[2].first.radians() << rot123[2].second;
    qDebug() << "";
    qDebug() << "--------------------------";
    QList<LinearAlgebra::EulerAngle> rot313 = LinearAlgebra::toEulerAngles(rot3x3, axes313);
    qDebug() << "From Axes: " << axes313;
    qDebug() << "     matrix = ";
    qDebug() << rot3x3;
    qDebug() << "Euler Angle 1 (angle/axis number) = " << rot313[0].first.radians() << rot313[0].second;
    qDebug() << "Euler Angle 2 (angle/axis number) = " << rot313[1].first.radians() << rot313[1].second;
    qDebug() << "Euler Angle 3 (angle/axis number) = " << rot313[2].first.radians() << rot313[2].second;
    qDebug() << "";
    qDebug() << "--------------------------";
    try { // matrix must be 3x3
      LinearAlgebra::toEulerAngles(m2x3, axes313);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try { // matrix must be 3x3
      LinearAlgebra::toEulerAngles(m3x2, axes313);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try {
      // 3x3 but not rotation
      LinearAlgebra::toEulerAngles(z3x3, axes313);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try {
      // axes 3 1 0
      axes313[2] = 0;
      LinearAlgebra::toEulerAngles(q12m, axes313);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try {
      // axes 3 -1 0
      axes313[1] = -1;
      LinearAlgebra::toEulerAngles(q12m, axes313);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try {
      // axes 4 -1 0
      axes313[0] = 4;
      LinearAlgebra::toEulerAngles(q12m, axes313);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try {
      // axis2 = axis1
      axes313[0] = 1;
      axes313[1] = 1;
      axes313[2] = 1;
      LinearAlgebra::toEulerAngles(q12m, axes313);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try {
      // axis2 = axis3
      axes313[0] = 2;
      axes313[1] = 1;
      axes313[2] = 1;
      LinearAlgebra::toEulerAngles(q12m, axes313);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try {
      // axis != 3
      axes313.pop_back();
      LinearAlgebra::toEulerAngles(q12m, axes313);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    qDebug() << "";

    qDebug() << "";
    qDebug() << "***************************************************";
    qDebug() << "Test toMatrix(EulerAngles) methods...";
    qDebug() << "***************************************************";
    LinearAlgebra::Matrix q12m2ea2m = LinearAlgebra::toMatrix(q12m2ea);
    qDebug() << "From Euler Angle 1 (angle/axis number) = " << q12m2ea[0].first.radians() << q12m2ea[0].second;
    qDebug() << "     Euler Angle 2 (angle/axis number) = " << q12m2ea[1].first.radians() << q12m2ea[1].second;
    qDebug() << "     Euler Angle 3 (angle/axis number) = " << q12m2ea[2].first.radians() << q12m2ea[2].second;
    qDebug() << "matrix = ";
    qDebug() << q12m2ea2m;
    qDebug() << "--------------------------";
    LinearAlgebra::Matrix q12m2ea2mB = LinearAlgebra::toMatrix(q12m2ea[0], q12m2ea[1], q12m2ea[2]);
    qDebug() << "From Euler Angle 1 (angle/axis number) = " << q12m2ea[0].first.radians() << q12m2ea[0].second;
    qDebug() << "     Euler Angle 2 (angle/axis number) = " << q12m2ea[1].first.radians() << q12m2ea[1].second;
    qDebug() << "     Euler Angle 3 (angle/axis number) = " << q12m2ea[2].first.radians() << q12m2ea[2].second;
    qDebug() << "matrix = ";
    qDebug() << q12m2ea2mB;
    qDebug() << "--------------------------";
    try { // invalid axis (must be 1, 2, or 3)
      LinearAlgebra::toMatrix(qMakePair(Angle(0.0, Angle::Radians), 4),
                              qMakePair(Angle(0.0, Angle::Radians), 1),
                              qMakePair(Angle(0.0, Angle::Radians), 1));
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try { // invalid axis (must be 1, 2, or 3)
      LinearAlgebra::toMatrix(qMakePair(Angle(0.0, Angle::Radians), 1),
                              qMakePair(Angle(0.0, Angle::Radians), 0),
                              qMakePair(Angle(0.0, Angle::Radians), 1));
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try { // invalid axis (must be 1, 2, or 3)
      LinearAlgebra::toMatrix(qMakePair(Angle(0.0, Angle::Radians), 2),
                              qMakePair(Angle(0.0, Angle::Radians), 1),
                              qMakePair(Angle(0.0, Angle::Radians), -1));
    }
    catch(Isis::IException &e) {
      e.print();
    }
    try { // list must be size 3
      q12m2ea.pop_back();
      LinearAlgebra::toMatrix(q12m2ea);
    }
    catch(Isis::IException &e) {
      e.print();
    }
    qDebug() << "";
 }
  catch(Isis::IException &e) {
    e.print();
  }
}


#include <bits/types/FILE.h>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <string>
#include <tgmath.h>
#include "image_ppm.h"
#include <filesystem>
// #include <Eigen/Dense>

using namespace std;
// using namespace Eigen;

// typedef Matrix<u_char, Dynamic, Dynamic> MatrixImg;
// typedef Matrix<double, Dynamic, Dynamic> TempMatrixImg;
// typedef Vector<u_char,Dynamic> ImgLine;
// typedef Vector<double,Dynamic> TempImgLine;
//typedef Matrix<ImgLine,Dynamic,Dynamic> FlattenedImages;

unsigned char max(u_char a, u_char b){
    if (a<b) return b;
    else return a;
}

unsigned char min(u_char a, u_char b){
    if (a>b) return b;
    else return a;
}

double max(double a, double b){
    if (a<b) return b;
    else return a;
}

double min(double a, double b){
    if (a>b) return b;
    else return a;
}

int max(int a, int b){
    if (a<b) return b;
    else return a;
}

int min(int a, int b){
    if (a>b) return b;
    else return a;
}

// auto max(auto a, auto b){
//     if (a<b) return b;
//     else return a;
// }

// auto min(auto a, auto b){
//     if (a>b) return b;
//     else return a;
// }

// vector<double> projectOnEigenSpace(vector<TempImgLine> eigenfaces, TempImgLine imToProj,int K){
//     vector<double> res = vector<double>();
//     for (int i=0;i<K;i++){
//         res.push_back(eigenfaces[i].dot(imToProj));
//     }
//     return res;
// }

// ImgLine octToVec(OCTET* im, int nH, int nW){
//     ImgLine res(nH*nW);
//     for (int i=0; i<nH*nW;i++){
//         res(i)=im[i];
//     }
//     return res;
// }

// double eigenProjsDistance(vector<double> proj1, vector<double> proj2){

//     double res =0.0;
//     for (int i=0; i<min(proj1.size(),proj2.size());i++){
//         res+=(proj1[i]-proj2[i])*(proj1[i]-proj2[i]);
//     }
//     return res;
// }

unsigned char LBPvalue(OCTET* image, int x, int y, int nW, int nH){
    int count =0;
    int* binaryNumber = new int[8];

    for(int i =-1;i<2;i++){
        for (int j=-1;j<2;j++){
            if (i!=0 && j!=0){
                if (image[(x+i)*nW+j+y]<image[x*nW+y]){
                    binaryNumber[count]=0;
                }
                if (image[(x+i)*nW+j+y]>=image[x*nW+y]){
                    binaryNumber[count]=1;
                }
                count++;
            }
        }
    }
    unsigned char res = (unsigned char)(1*binaryNumber[7] + 2*binaryNumber[6] + 4*binaryNumber[5] + 8*binaryNumber[4] + 16*binaryNumber[3] + 32*binaryNumber[2] + 64*binaryNumber[1] + 128*binaryNumber[0]);
    return (res);
}

double minutie(int* hist, int size,int nH, int nW, int gridX, int gridY){
    double res =0.0;
    for (int i =0;i<size;i++){
        res+=i*(double)hist[i]/((double)(nW*nH)/(double)(gridY*gridX));//(double)size;
    }
    return res;
}

double divergenceLK(int* hist1, int* hist2, int size){
    double res = 0.0;
    for (int k=0;k<size;k++){
        res+=(hist1[k]+1.0)*log((hist1[k]+1.0)/(hist2[k]+1.0));
    }
    return res;
}

int main(int argc, char* argv[]){

    //DB file :
    ofstream outFile;
    outFile.open ("DBLBP.txt");

    vector<vector<int*>> histos = vector<vector<int*>>();

    int nH, nW;
    int gridX=5,gridY=5;

    if (argc<2){
        // cout<<"usage : dir0 ... dirn\n"<<endl;
    }
    vector<string> directories;
    for (int i=0; i<argc-1;i++){
        string s = string(argv[i+1]);
        int found = s.find_last_of('/');

        directories.push_back(s.substr(found+1));
    }

    //vector<vector<vector<double>>> registre = vector<vector<vector<double>>>();

    int countDirs =0;
    for (int i=0; i<argc-1;i++){
        int countFile=0;
        outFile<<"!"<<directories[i]<<endl;
        histos.push_back(vector<int*>());
        for (auto& file : std::filesystem::directory_iterator(argv[i+1])){
            histos[i].push_back((int*)calloc(gridX*gridY*256,sizeof(int)));
            //registre.push_back(vector<vector<double>>());
            OCTET* img;
            lire_nb_lignes_colonnes_image_pgm(file.path().c_str(),&nH,&nW);
            allocation_tableau(img,OCTET, nH*nW);
            lire_image_pgm(file.path().c_str(),img,nH*nW);

            OCTET* LBP;
            allocation_tableau(LBP,OCTET, nH*nW);
            for (int i=1;i<nH-1;i++){
                for (int j =1;j<nW-1;j++){
                    LBP[i*nW+j]=LBPvalue(img, i, j, nW, nH);
                }
            }

            int compteur=0;
            for (int x = 0 ; x<gridX;x++){
                for (int y = 0;y<gridY;y++){
                    int *tempHisto = (int*)calloc(256,sizeof(int));
                    for (int i=y*nH/gridY;i<(y+1)*nH/gridY;i++){
                        for (int j=x*nW/gridX;j<(x+1)*nW/gridX;j++){
                            tempHisto[(int)LBP[i*nW+j]]++;
                        }
                    }
                    for (int k = compteur*256;k<(compteur+1)*256;k++){
                        histos[i][countFile][k]=tempHisto[k%255];
                    }
                    compteur++;
                    delete[] tempHisto;
                }
            }

            //registre[countDirs].push_back(projectOnEigenSpace(eigenfaces,imgLine,K));
            //vector<double> testPrint = projectOnEigenSpace(eigenfaces,imgLine,K);
            for (int d=0; d<256*gridX*gridY;d++){outFile<<histos[i][countFile][d]<<" ";}
            outFile<<endl;

            countFile++;
        }
        outFile<<endl;
        countDirs++;
    }

    outFile.close();
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/fold_right.hpp>
#include <boost/hana/if.hpp>
#include <boost/hana/less.hpp>
#include <boost/hana/prepend.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;
using namespace hana::literals;

int main() {
    constexpr auto numbers = hana::tuple_c<int, 5, -1, 0, -7, -2, 0, -5, 4>;
    constexpr auto negatives = hana::tuple_c<int, -1, -7, -2, -5>;

    BOOST_HANA_CONSTEXPR_LAMBDA auto keep_negatives = [](auto n, auto acc) {
        return hana::if_(n < 0_c, hana::prepend(acc, n), acc);
    };

    BOOST_HANA_CONSTANT_CHECK(hana::fold_right(numbers, hana::tuple_c<int>, keep_negatives) == negatives);
}


//
//  Copyright (c) 2021 Kannav Mehta
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef LA_MATRIX_EXPRESSION_HPP
#define LA_MATRIX_EXPRESSION_HPP

#include <boost/numeric/ublas/matrix/matrix/traits/matrix_traits.hpp>
#include <type_traits>

namespace boost::numeric::ublas::experimental::detail {

/**
 * @brief Template class to store both unary and binary matrix expressions
 *
 * @tparam callable The function object for the respective expression
 * @tparam operands The one or two operands for the expression
 */
template<class callable, class... operands>
class matrix_expression {
    using expr_t = callable;
public:
    explicit matrix_expression(callable f, operands const &... args) : f(f), args(args...) {}

private:
    callable f;
    std::tuple<operands const &...> args;
};

template<class L, class R>
auto operator*(const L &l, const R &r) {
    return matrix_expression{[](const L &l, const R &r) {
        return l * r;
    }, l, r};
}

}
#endif //LA_MATRIX_EXPRESSION_HPP


//! \file examples/Minkowski_sum_2/exact_offset.cpp
// Computing the exact offset of a polygon.

#include <iostream>

#include <CGAL/config.h>

#ifndef CGAL_USE_CORE
int main()
{
  std::cout << "Sorry, this example needs CORE ..." << std::endl;
  return 0;
}
#else

#include <fstream>
#include <boost/timer.hpp>

#include <CGAL/Gps_traits_2.h>
#include <CGAL/offset_polygon_2.h>

#include "arr_conics.h"

typedef CGAL::Polygon_2<Rat_kernel>             Polygon_2;
typedef CGAL::Gps_traits_2<Traits>              Gps_traits;
typedef Gps_traits::Polygon_with_holes_2        Offset_polygon_with_holes_2;

int main(int argc, char* argv[])
{
  // Open the input file and read the input polygon.
  const char* filename = (argc > 1) ? argv[1] : "spiked.dat";
  std::ifstream in_file(filename);
  if (! in_file.is_open()) {
    std::cerr << "Failed to open the input file." << std::endl;
    return -1;
  }
  Polygon_2  P;
  in_file >> P;
  in_file.close();
  std::cout << "Read an input polygon with " << P.size() << " vertices."
            << std::endl;

  // Compute the offset polygon.
  Traits traits;
  boost::timer timer;
  Offset_polygon_with_holes_2 offset = CGAL::offset_polygon_2(P, 5, traits);
  double secs = timer.elapsed();

  std::cout << "The offset polygon has " << offset.outer_boundary().size()
            << " vertices, " << offset.number_of_holes() << " holes."
            << std::endl;
  std::cout << "Offset computation took " << secs << " seconds." << std::endl;
  return 0;
}

#endif


//
//  ImplicitSurface.hpp
//  CSE_168
//
//  Created by Clayton Anderson on 4/11/17.
//  Copyright © 2017 Clayton Anderson. All rights reserved.
//

#ifndef ImplicitSurface_hpp
#define ImplicitSurface_hpp

#include "Object.h"
#include <glm/mat4x4.hpp>
#include <iostream>
#include <glm/gtc/matrix_transform.hpp>
//#include <boost/math/tools/roots.hpp>
#include <vector>
#include <algorithm>
#include <glm/gtc/random.hpp>
#define SHOWVEC(v) std::cout << #v << ": " << v.x << " " << v.y << " " << v.z << std::endl

using namespace glm;
//using namespace boost::math::tools;

class ImplicitSurface:public Object
{
public:
    ImplicitSurface() {}
    ImplicitSurface( float (*f)(float, float ,float) ) {fun = f;}
    bool Intersect(const Ray &ray, Intersection &hit);

    void SetF(float (*f)(float, float ,float) ){fun = f;}
    void SetGrad(std::tuple<float, float, float> (*f)(float, float, float)) {grad = f;}
    float EvaluateF(float x, float y, float z) {return fun(x,y,z); }
    std::tuple<float,float,float> EvaluateGrad(float x, float y, float z) {return grad(x,y,z); }
    std::vector<float> freqs;
    void SetFandG();

private:
    float (*fun)(float, float, float);

    std::tuple<float, float, float> (*grad)(float, float, float);
};

#endif /* ImplicitSurface_hpp */


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "common.h"
#include <Eigen/Eigenvalues>

// computes an LU factorization of a general M-by-N matrix A using partial pivoting with row interchanges
EIGEN_LAPACK_FUNC(syev,(char *jobz, char *uplo, int* n, Scalar* a, int *lda, Scalar* w, Scalar* /*work*/, int* lwork, int *info))
{
  // TODO exploit the work buffer
  bool query_size = *lwork==-1;

  *info = 0;
        if(*jobz!='N' && *jobz!='V')                    *info = -1;
  else  if(UPLO(*uplo)==INVALID)                        *info = -2;
  else  if(*n<0)                                        *info = -3;
  else  if(*lda<std::max(1,*n))                         *info = -5;
  else  if((!query_size) && *lwork<std::max(1,3**n-1))  *info = -8;

//   if(*info==0)
//   {
//     int nb = ILAENV( 1, 'SSYTRD', UPLO, N, -1, -1, -1 )
//          LWKOPT = MAX( 1, ( NB+2 )*N )
//          WORK( 1 ) = LWKOPT
// *
//          IF( LWORK.LT.MAX( 1, 3*N-1 ) .AND. .NOT.LQUERY )
//      $      INFO = -8
//       END IF
// *
//       IF( INFO.NE.0 ) THEN
//          CALL XERBLA( 'SSYEV ', -INFO )
//          RETURN
//       ELSE IF( LQUERY ) THEN
//          RETURN
//       END IF

  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"SYEV ", &e, 6);
  }

  if(query_size)
  {
    *lwork = 0;
    return 0;
  }

  if(*n==0)
    return 0;

  PlainMatrixType mat(*n,*n);
  if(UPLO(*uplo)==UP) mat = matrix(a,*n,*n,*lda).adjoint();
  else                mat = matrix(a,*n,*n,*lda);

  bool computeVectors = *jobz=='V' || *jobz=='v';
  SelfAdjointEigenSolver<PlainMatrixType> eig(mat,computeVectors?ComputeEigenvectors:EigenvaluesOnly);

  if(eig.info()==NoConvergence)
  {
    make_vector(w,*n).setZero();
    if(computeVectors)
      matrix(a,*n,*n,*lda).setIdentity();
    //*info = 1;
    return 0;
  }

  make_vector(w,*n) = eig.eigenvalues();
  if(computeVectors)
    matrix(a,*n,*n,*lda) = eig.eigenvectors();

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_OPERATOR_FUNCTIONS_COMPLEX_GENERIC_MULTIPLIES_HPP_INCLUDED
#define NT2_OPERATOR_FUNCTIONS_COMPLEX_GENERIC_MULTIPLIES_HPP_INCLUDED

#include <nt2/operator/functions/multiplies.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/functions/pure.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/include/functions/simd/any.hpp>
#include <nt2/include/functions/fma.hpp>
#include <nt2/include/functions/simd/all.hpp>
#include <nt2/include/functions/simd/is_nez.hpp>
#include <nt2/include/functions/simd/is_invalid.hpp>
#include <nt2/include/functions/logical_or.hpp>
#include <nt2/include/functions/logical_and.hpp>
#include <nt2/include/functions/logical_andnot.hpp>
#include <nt2/include/functions/if_else.hpp>
#include <nt2/include/functions/if_zero_else.hpp>
#include <nt2/include/functions/if_else_zero.hpp>
#include <nt2/include/functions/is_finite.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/is_real.hpp>
#include <nt2/include/functions/is_imag.hpp>
#include <nt2/include/functions/simd/mul_i.hpp>
#include <nt2/include/functions/bitwise_cast.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/sdk/complex/hierarchy.hpp>
#include <nt2/sdk/complex/meta/as_real.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <boost/simd/sdk/config.hpp>

namespace boost { namespace simd { namespace ext
{
  // complex/complex
  BOOST_DISPATCH_IMPLEMENT  ( multiplies_, tag::cpu_, (A0)
                            , ((generic_< complex_< arithmetic_<A0> > >))
                              ((generic_< complex_< arithmetic_<A0> > >))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename nt2::meta::as_real<A0>::type r_type;
      const r_type a = nt2::real(a0);
      const r_type b = nt2::imag(a0);
      const r_type c = nt2::real(a1);
      const r_type d = nt2::imag(a1);
      result_type r(fma(a,c,-b*d), fma(a,d,b*c));
#ifndef BOOST_SIMD_NO_INVALIDS
      typedef typename meta::as_logical<r_type>::type l_type;
      l_type test = is_finite(r);
      if (nt2::all(test)) return r;
      l_type cur  = nt2::logical_andnot(is_real(a0), test);
      if (nt2::any(cur))
      {
        r = if_else(cur, nt2::multiplies(a, a1), r);
        test = logical_or(test, cur);
        if (nt2::all(test)) return r;
      }
      cur = nt2::logical_andnot(is_imag(a0), test);
      if (nt2::any(cur))
      {
        r = if_else(cur, nt2::mul_i(nt2::multiplies(b, a1)), r);
        test = logical_or(test, cur);
        if (nt2::all(test)) return r;
      }
      cur = nt2::logical_andnot(is_real(a1), test);
      if (nt2::any(cur))
      {
        r = if_else(cur, nt2::multiplies(c, a0), r);
        test = logical_or(test, cur);
        if (nt2::all(test)) return r;
      }
      cur = nt2::logical_andnot(is_imag(a1), test);
      if (nt2::any(cur))
      {
        r = if_else(cur, nt2::mul_i(nt2::multiplies(d, a0)), r);
        test = logical_or(test, cur);
        if (nt2::all(test)) return r;
      }
 #endif
      return r;
    }
  };

  // complex/real
  BOOST_DISPATCH_IMPLEMENT  ( multiplies_, tag::cpu_, (A0)(A1)
                            , ((generic_< arithmetic_<A0> >))
                              ((generic_< complex_< arithmetic_<A1> > >))
                            )
  {
    typedef A1 result_type;
    typedef A0 r_type;
    NT2_FUNCTOR_CALL(2)
    {
      r_type r = a0*nt2::real(a1);
      r_type i = a0*nt2::imag(a1);
#ifndef BOOST_SIMD_NO_INVALIDS
      typename meta::as_logical<A1>::type is_real_a1 = is_real(a1);
      r = if_zero_else(logical_andnot(is_imag(a1), is_real_a1), r);
      i = if_zero_else(is_real_a1, i);
#endif
      return result_type(r, i);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( multiplies_, tag::cpu_, (A0)(A1)
                            , ((generic_< complex_< arithmetic_<A0> > >))
                              ((generic_< arithmetic_<A1> >))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      return nt2::multiplies(a1, a0);
    }
  };
  BOOST_DISPATCH_IMPLEMENT  ( multiplies_, tag::cpu_, (A0)(A1)
                            , ((generic_< dry_ < arithmetic_<A0> > >))
                              ((generic_< complex_< arithmetic_<A1> > >))
                            )
  {
    typedef A1 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      return nt2::multiplies(nt2::real(a0), a1);
    }
  };
  BOOST_DISPATCH_IMPLEMENT  ( multiplies_, tag::cpu_, (A0)(A1)
                            , ((generic_< complex_< arithmetic_<A0> > >))
                              ((generic_< dry_ < arithmetic_<A1> > >))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      return nt2::multiplies(a0, nt2::real(a1));
    }
  };

  // dry/dry
  BOOST_DISPATCH_IMPLEMENT  ( multiplies_, tag::cpu_, (A0)(A1)
                            , ((generic_< dry_< arithmetic_<A0> > >))
                              ((generic_< dry_< arithmetic_<A1> > >))
                            )
  {
    typedef typename nt2::meta::as_dry<A0>::type result_type;
    NT2_FUNCTOR_CALL(2)
    {
      return bitwise_cast<result_type>(nt2::real(a0)*nt2::real(a1));
    }
  };

} } }

#endif


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120323    K. Kumar          Created based off of unit test for RKF78 integrator; test data
 *                                  taken from (The Mathworks, 2012).
 *      120404    K. Kumar          Updated MATLAB unit test by adding discrete-event data file.
 *      130204    K. Kumar          Updated tolerance for discrete-event case to prevent failure
 *                                  of test under Windows.
 *      130916    K. Kumar          Updated error tolerances for MuPAD-based tests.
 *
 *    References
 *      Montenbruck, O., Gill, E. Satellite Orbits: Models, Methods, Applications, Springer, 2005.
 *      The Mathworks, Inc. DOPRI78, Symbolic Math Toolbox, 2012.
 *
 *    Notes
 *      All the test for this integrator are based on the data generated using the Symbolic Math
 *      Toolbox (MathWorks, 2012). Ideally, another source of data should be used to complete the
 *      testing.
 *
 *      The single step and full integration error tolerances were picked to be as small as
 *      possible, without causing the tests to fail. These values are not deemed to indicate any
 *      bugs in the code; however, it is important to take these discrepancies into account when
 *      using this numerical integrator.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaVariableStepSizeIntegrator.h"
#include "Tudat/InputOutput/matrixTextFileReader.h"
#include "Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/reinitializableNumericalIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTests.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h"

#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"

#include <limits>
#include <string>

#include <Eigen/Core>

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_runge_kutta_87_dormand_and_prince_integrator )

using linear_algebra::flipMatrixRows;

using numerical_integrators::NumericalIntegratorXdPointer;
using numerical_integrators::ReinitializableNumericalIntegratorXdPointer;
using numerical_integrators::RungeKuttaVariableStepSizeIntegratorXd;
using numerical_integrators::RungeKuttaCoefficients;

using numerical_integrator_test_functions::computeNonAutonomousModelStateDerivative;

//! Test Runge-Kutta 87 Dormand-Prince integrator using benchmark data from (The MathWorks, 2012).
BOOST_AUTO_TEST_CASE( testRungeKutta87DormandAndPrinceIntegratorUsingMatlabData )
{
    using namespace numerical_integrator_tests;

    // Read in benchmark data (generated using Symbolic Math Toolbox in Matlab
    // (The MathWorks, 2012)). This data is generated using the DOPRI78 numerical integrator.
    const std::string pathToForwardIntegrationOutputFile = input_output::getTudatRootPath( )
            + "/Mathematics/NumericalIntegrators/UnitTests"
            + "/matlabOutputRungeKutta87DormandPrinceForward.txt";
    const std::string pathToDiscreteEventIntegrationOutputFile = input_output::getTudatRootPath( )
            + "/Mathematics/NumericalIntegrators/UnitTests"
            + "/matlabOutputRungeKutta87DormandPrinceDiscreteEvent.txt";

    // Store benchmark data in matrix.
    const Eigen::MatrixXd matlabForwardIntegrationData =
            input_output::readMatrixFromFile( pathToForwardIntegrationOutputFile, "," );
    Eigen::MatrixXd matlabBackwardIntegrationData = matlabForwardIntegrationData;
    flipMatrixRows( matlabBackwardIntegrationData );
    const Eigen::MatrixXd matlabDiscreteEventIntegrationData =
            input_output::readMatrixFromFile( pathToDiscreteEventIntegrationOutputFile, "," );

    // Set integrator parameters.

    // All of the following parameters are set such that the input data is fully accepted by the
    // integrator, to determine the steps to be taken.
    const double zeroMinimumStepSize = std::numeric_limits< double >::epsilon( );
    const double infiniteMaximumStepSize = std::numeric_limits< double >::infinity( );
    const double infiniteRelativeErrorTolerance = std::numeric_limits< double >::infinity( );
    const double infiniteAbsoluteErrorTolerance = std::numeric_limits< double >::infinity( );

    // The following parameters set how the error control mechanism should work.
    const double relativeErrorTolerance = 1.0e-15;
    const double absoluteErrorTolerance = 1.0e-15;

    // Case 1: Execute integrateTo() to integrate one step forward in time.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = boost::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKutta87DormandPrince ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        executeOneIntegrateToStep( matlabForwardIntegrationData, 1.0e-15, integrator );
    }

    // Case 2: Execute performIntegrationStep() to perform multiple integration steps until final
    //         time.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = boost::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKutta87DormandPrince ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTime( matlabForwardIntegrationData,
                                               1.0e-15, 1.0e-15, integrator );
    }

    // Case 3: Execute performIntegrationStep() to perform multiple integration steps until initial
    //         time (backwards).
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = boost::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKutta87DormandPrince ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabBackwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabBackwardIntegrationData( FIRST_ROW,
                                                        STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTime( matlabBackwardIntegrationData,
                                               1.0e-15, 1.0e-14, integrator );
    }

    // Case 4: Execute integrateTo() to integrate to specified time in one step.
    {
        // Note that this test has a strange issue that the if the absolute error tolerance is set
        // to 1.0e-15, the last step that the integrateTo() function takes does not result in the
        // expected final time of 1.0. As a temporary solution, the absolute error tolerance has
        // been multiplied by 10.0, which seems to solve the problem. This error indicated a
        // possible problem with the implementation of the integrateTo() function, which needs to
        // be investigated in future.

        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = boost::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKutta87DormandPrince ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    relativeErrorTolerance,
                    absoluteErrorTolerance * 10.0 );

        executeIntegrateToToSpecifiedTime( matlabForwardIntegrationData, 1.0e-15, integrator,
                                           matlabForwardIntegrationData(
                                               matlabForwardIntegrationData.rows( ) - 1,
                                               TIME_COLUMN_INDEX ) );
    }

    // Case 5: Execute performIntegrationstep() to integrate to specified time in multiple steps,
    //         including discrete events.
    {
        // Declare integrator with all necessary settings.
        ReinitializableNumericalIntegratorXdPointer integrator
                = boost::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKutta87DormandPrince ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTimeWithEvents( matlabDiscreteEventIntegrationData,
                                                         1.0e-15, 1.0e-12, integrator );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

**/
//==================================================================================================
#ifndef BOOST_SIMD_PREDICATES_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-predicates Predicates functions

      These functions provide scalar and SIMD algorithms for testing properties of values.
      Tests such as checks for validity, infinity, sign and others are provided. As for @ref
      group-boolean, these functions handle and return typed logical types.

      - These predicates can be used through an operator

        | name                  |  op  | name               |  op   |
        |:---------------------:|:----:|:------------------:|:-----:|
        | @ref is_equal         | ==   | @ref is_less_equal |  <=   |
        | @ref is_greater_equal | >=   | @ref is_less       |  <    |
        | @ref is_greater       | >    | @ref is_not_equal  |  !=   |

      - The following ones have to be called directly.

        Many of these are restriction to special values of some of the preceding ones.
        Their use often provides speed gain in SIMD mode.

        | name                            | name                              | name                  |
        |:-------------------------------:|:---------------------------------:|:---------------------:|
        | @ref is_denormal                | @ref is_lez                       | @ref is_not_imag      |
        | @ref is_equal_with_equal_nans   | @ref is_ltz                       | @ref is_not_infinite  |
        | @ref is_eqz                     | @ref is_nan                       | @ref is_not_less_equal|
        | @ref is_even                    | @ref is_negative                  | @ref is_not_less      |
        | @ref is_finite                  | @ref is_nez                       | @ref is_not_nan       |
        | @ref is_flint                   | @ref is_ngez                      | @ref is_not_real      |
        | @ref is_gez                     | @ref is_ngtz                      | @ref is_odd           |
        | @ref is_gtz                     | @ref is_nlez                      | @ref is_ord           |
        | @ref is_imag                    | @ref is_nltz                      | @ref is_positive      |
        | @ref is_included_c              | @ref is_normal                    | @ref is_real          |
        | @ref is_included                | @ref is_not_denormal              | @ref is_simd_logical  |
        | @ref is_inf                     | @ref is_not_equal_with_equal_nans | @ref is_unord         |
        | @ref is_invalid                 | @ref is_not_greater               |                       |
        | @ref is_lessgreater             | @ref is_not_greater_equal         |                       |

  **/

} }

#include <boost/simd/function/is_denormal.hpp>
#include <boost/simd/function/is_equal_with_equal_nans.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_even.hpp>
#include <boost/simd/function/is_finite.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/is_greater_equal.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <boost/simd/function/is_imag.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/is_invalid.hpp>
#include <boost/simd/function/is_less_equal.hpp>
#include <boost/simd/function/is_lessgreater.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/is_lez.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/is_ngez.hpp>
#include <boost/simd/function/is_ngtz.hpp>
#include <boost/simd/function/is_nlez.hpp>
#include <boost/simd/function/is_nltz.hpp>
#include <boost/simd/function/is_normal.hpp>
#include <boost/simd/function/is_not_denormal.hpp>
#include <boost/simd/function/is_not_equal_with_equal_nans.hpp>
#include <boost/simd/function/is_not_greater_equal.hpp>
#include <boost/simd/function/is_not_greater.hpp>
#include <boost/simd/function/is_not_imag.hpp>
#include <boost/simd/function/is_not_infinite.hpp>
#include <boost/simd/function/is_not_less_equal.hpp>
#include <boost/simd/function/is_not_less.hpp>
#include <boost/simd/function/is_not_nan.hpp>
#include <boost/simd/function/is_not_real.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/is_ord.hpp>
#include <boost/simd/function/is_positive.hpp>
#include <boost/simd/function/is_real.hpp>
#include <boost/simd/function/is_unord.hpp>
#include <boost/simd/function/majority.hpp>

#endif


// Copyright 2017-2020 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/random/random_device.hpp>

#if defined(_MSC_VER)
__declspec(dllexport)
#endif
unsigned get_random_number()
{
    boost::random::random_device dev;
    return dev();
}


//  See http://www.boost.org/libs/test for the library home page.

// Boost.Test

// each test module could contain no more then one 'main' file with init function defined
// alternatively you could define init function yourself
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include "gl/geometry_types.hpp"
#include "gl/Camera.hpp"

using namespace kinski;
//____________________________________________________________________________//

// plane test
BOOST_AUTO_TEST_CASE( test_Plane )
{
    glm::vec3 v(10, 20, 30);
    gl::Plane p;

    // test contructors

    // foot, normal
    p = gl::Plane(glm::vec3(99, 0, -69), glm::vec3(0, 1, 0));
    BOOST_CHECK(p.distance(v) == 20);

    // coefficients
    p = gl::Plane(0, 1, 0, 0);
    BOOST_CHECK(p.distance(v) == 20);

    // 3 vertices
    p = gl::Plane(glm::vec3(10, 0, 30), glm::vec3(99, 0, -222), glm::vec3(69, 0, -69));
    BOOST_CHECK(p.distance(v) == 20);

    for(int i = 0; i < 5000; i++)
    {
        // generate random plane
        glm::vec3 v1 = v + glm::sphericalRand(50.f);
        p = gl::Plane(v, v1 - v);
        BOOST_CHECK_CLOSE(p.distance(v1), 50.f, 0.0001f);

        // transform the plane
        p.transform(glm::translate(glm::mat4(1), glm::normalize(v1 - v) * 20.f));
        BOOST_CHECK_CLOSE(p.distance(v1), 30.f, 0.0001f);
    }

}

BOOST_AUTO_TEST_CASE( test_scale )
{
    auto test_object = gl::Object3D::create();
    test_object->transform() = glm::rotate(test_object->transform(), 47.f, glm::vec3(1, 1, 1));
    test_object->transform() = glm::translate(test_object->transform(), glm::vec3(100, -1221, 1));
    test_object->transform() = glm::scale(test_object->transform(), glm::vec3(2, 3, 4));

    glm::vec3 scale = test_object->scale();
    BOOST_CHECK_CLOSE(scale.x, 2.f, 0.0001f);
    BOOST_CHECK_CLOSE(scale.y, 3.f, 0.0001f);
    BOOST_CHECK_CLOSE(scale.z, 4.f, 0.0001f);

    test_object->set_scale(glm::vec3(5));
    scale = test_object->scale();
    BOOST_CHECK_CLOSE(scale.x, 5.f, 0.0001f);
    BOOST_CHECK_CLOSE(scale.y, 5.f, 0.0001f);
    BOOST_CHECK_CLOSE(scale.z, 5.f, 0.0001f);
}

//____________________________________________________________________________//

// EOF


/*
	This file is part of cpp-ethereum.

	cpp-ethereum 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.

	cpp-ethereum 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 cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file difficulty.cpp
 * @author Christoph Jentzsch <cj@ethdev.com>
 * @date 2015
 * difficulty calculation tests.
 */

#include <boost/test/unit_test.hpp>
#include <test/libtesteth/TestHelper.h>
#include <test/fuzzTesting/fuzzHelper.h>
#include <libethashseal/Ethash.h>
#include <libethashseal/GenesisInfo.h>
#include <cpp-ethereum/libethereum/ChainParams.h>
using namespace std;
using namespace dev;
using namespace dev::eth;

namespace js = json_spirit;
std::string const c_testDifficulty = R"(
 "DifficultyTest[N]" : {
		"parentTimestamp" : "[PSTAMP]",
		"parentDifficulty" : "[PDIFF]",
		"currentTimestamp" : "[СSTAMP]",
		"currentBlockNumber" : "[CNUM]",
		"currentDifficulty" : "[CDIFF]"
	},
)";

void checkCalculatedDifficulty(BlockHeader const& _bi, BlockHeader const& _parent, Network _n, ChainOperationParams const& _p, string const& _testName = "")
{
	u256 difficulty = _bi.difficulty();
	u256 frontierDiff = _p.u256Param("homsteadForkBlock");

	//The ultimate formula (Homestead)
	if (_bi.number() > frontierDiff)
	{
		u256 minimumDifficulty = _p.u256Param("minimumDifficulty");
		bigint block_diff = _parent.difficulty();

		bigint a = (_parent.difficulty() / 2048);
		int b = 1 - int(_bi.timestamp() - _parent.timestamp()) / 10;
		bigint c = (_bi.number() / 100000) - 2;

		block_diff += a * max<int>(b, -99);
		block_diff += u256(1) << (unsigned)c;
		block_diff = max<bigint>(minimumDifficulty, block_diff);

		BOOST_CHECK_MESSAGE(difficulty == block_diff, "Homestead Check Calculated diff = " << difficulty << " expected diff = " << block_diff << _testName);
		return;
	}

	u256 durationLimit;
	u256 minimumDifficulty;
	u256 difficultyBoundDivisor;
	switch(_n)
	{
	case Network::MainNetwork:
	case Network::FrontierTest:
	case Network::HomesteadTest:
	case Network::Ropsten:
	case Network::TransitionnetTest:
		durationLimit = 13;
		minimumDifficulty = 131072;
		difficultyBoundDivisor = 2048;
	break;
	default:
		cerr << "testing undefined network difficulty";
		durationLimit = _p.u256Param("durationLimit");
		minimumDifficulty = _p.u256Param("minimumDifficulty");
		difficultyBoundDivisor = _p.u256Param("difficultyBoundDivisor");
		break;
	}

	//Frontier Era
	bigint block_diff = _parent.difficulty();

	bigint a = (_parent.difficulty() / difficultyBoundDivisor);
	bigint b = ((_bi.timestamp() - _parent.timestamp()) < durationLimit) ?  1 : -1;
	bigint c = (_bi.number() / 100000) - 2;

	block_diff += a * b;
	block_diff += u256(1) << (unsigned)c;
	block_diff = max<bigint>(minimumDifficulty, block_diff);

	BOOST_CHECK_MESSAGE(difficulty == block_diff, "Check Calculated diff = " << difficulty << " expected diff = " << block_diff << _testName);
	return;
}

void fillDifficulty(string const& _testFileFullName, Ethash& _sealEngine)
{
	int testN = 0;
	ostringstream finalTest;
	finalTest << "{" << std::endl;
	dev::test::TestOutputHelper::initTest(900);

	for (int stampDelta = 0; stampDelta < 45; stampDelta+=2)
	{
		for (u256 blockNumber = 1; blockNumber < 1500000; blockNumber += 25000)
		{
			testN++;
			json_spirit::mObject m;
			string testName = "DifficultyTest"+toString(testN);
			if (!dev::test::TestOutputHelper::passTest(m, testName))
				continue;

			u256 pStamp = dev::test::RandomCode::randomUniInt();
			u256 pDiff = dev::test::RandomCode::randomUniInt();
			u256 cStamp = pStamp + stampDelta;
			u256 cNum = blockNumber;

			BlockHeader parent;
			parent.setTimestamp(pStamp);
			parent.setDifficulty(pDiff);
			parent.setNumber(cNum - 1);

			BlockHeader current;
			current.setTimestamp(cStamp);
			current.setNumber(cNum);

			string tmptest = c_testDifficulty;
			std::map<string, string> replaceMap;
			replaceMap["[N]"] = toString(testN);
			replaceMap["[PDIFF]"] = toCompactHex(pDiff, HexPrefix::Add);
			replaceMap["[PSTAMP]"] = toCompactHex(pStamp, HexPrefix::Add);
			replaceMap["[СSTAMP]"] = toCompactHex(cStamp, HexPrefix::Add);
			replaceMap["[CNUM]"] = toCompactHex(cNum, HexPrefix::Add);
			replaceMap["[CDIFF]"] = toCompactHex(_sealEngine.calculateDifficulty(current, parent), HexPrefix::Add);

			dev::test::RandomCode::parseTestWithTypes(tmptest, replaceMap);
			finalTest << tmptest;
		}
	}

	finalTest << std::endl << "}";
	string testFile = finalTest.str();
	testFile = testFile.replace(testFile.find_last_of(","), 1, "");
	writeFile(_testFileFullName, asBytes(testFile));
}

void testDifficulty(string const& _testFileFullName, Ethash& _sealEngine, Network _n)
{
	//Test File
	js::mValue v;
	string s = contentsString(_testFileFullName);
	BOOST_REQUIRE_MESSAGE(s.length() > 0, "Contents of '" << _testFileFullName << "' is empty. Have you cloned the 'tests' repo branch develop?");
	js::read_string(s, v);
	dev::test::TestOutputHelper::initTest(v);

	for (auto& i: v.get_obj())
	{
		js::mObject o = i.second.get_obj();
		string testname = i.first;
		if (!dev::test::TestOutputHelper::passTest(o, testname))
			continue;

		BlockHeader parent;
		parent.setTimestamp(test::toInt(o["parentTimestamp"]));
		parent.setDifficulty(test::toInt(o["parentDifficulty"]));
		parent.setNumber(test::toInt(o["currentBlockNumber"]) - 1);

		BlockHeader current;
		current.setTimestamp(test::toInt(o["currentTimestamp"]));
		current.setNumber(test::toInt(o["currentBlockNumber"]));

		u256 difficulty = _sealEngine.calculateDifficulty(current, parent);
		current.setDifficulty(difficulty);
		BOOST_CHECK_EQUAL(difficulty, test::toInt(o["currentDifficulty"]));

		//Manual formula test
		checkCalculatedDifficulty(current, parent, _n, _sealEngine.chainParams(), "(" + i.first + ")");
	}
	dev::test::TestOutputHelper::finishTest();
}

BOOST_AUTO_TEST_SUITE(DifficultyTests)

BOOST_AUTO_TEST_CASE(difficultyTestsFrontier)
{
	string testFileFullName = test::getTestPath();
	testFileFullName += "/BasicTests/difficultyFrontier.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::FrontierTest)));

	if (dev::test::Options::get().filltests)
		fillDifficulty(testFileFullName, sealEngine);

	testDifficulty(testFileFullName, sealEngine, Network::FrontierTest);
}

BOOST_AUTO_TEST_CASE(difficultyTestsRopsten)
{
	string testFileFullName = test::getTestPath();
	testFileFullName += "/BasicTests/difficultyRopsten.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::Ropsten)));

	if (dev::test::Options::get().filltests)
		fillDifficulty(testFileFullName, sealEngine);

	testDifficulty(testFileFullName, sealEngine, Network::Ropsten);
}

BOOST_AUTO_TEST_CASE(difficultyTestsHomestead)
{
	string testFileFullName = test::getTestPath();
	testFileFullName += "/BasicTests/difficultyHomestead.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::HomesteadTest)));

	if (dev::test::Options::get().filltests)
		fillDifficulty(testFileFullName, sealEngine);

	testDifficulty(testFileFullName, sealEngine, Network::HomesteadTest);
}

BOOST_AUTO_TEST_CASE(difficultyTestsMainNetwork)
{
	string testFileFullName = test::getTestPath();
	testFileFullName += "/BasicTests/difficultyMainNetwork.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::MainNetwork)));

	if (dev::test::Options::get().filltests)
		fillDifficulty(testFileFullName, sealEngine);

	testDifficulty(testFileFullName, sealEngine, Network::MainNetwork);
}

BOOST_AUTO_TEST_CASE(difficultyTestsCustomMainNetwork)
{
	string testFileFullName = test::getTestPath();
	testFileFullName += "/BasicTests/difficultyCustomMainNetwork.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::MainNetwork)));

	if (dev::test::Options::get().filltests)
	{
		u256 homsteadBlockNumber = 1000000;
		std::vector<u256> blockNumberVector = {homsteadBlockNumber - 100000, homsteadBlockNumber, homsteadBlockNumber + 100000};
		std::vector<u256> parentDifficultyVector = {1000, 2048, 4000, 1000000};
		std::vector<int> timestampDeltaVector = {0, 1, 8, 10, 13, 20, 100, 800, 1000, 1500};

		int testN = 0;
		ostringstream finalTest;
		finalTest << "{" << std::endl;

		for (size_t bN = 0; bN < blockNumberVector.size(); bN++)
			for (size_t pdN = 0; pdN < parentDifficultyVector.size(); pdN++)
				for (size_t tsN = 0; tsN < timestampDeltaVector.size(); tsN++)
				{
					testN++;
					int stampDelta = timestampDeltaVector.at(tsN);
					u256 blockNumber = blockNumberVector.at(bN);
					u256 pDiff = parentDifficultyVector.at(pdN);

					u256 pStamp = dev::test::RandomCode::randomUniInt();
					u256 cStamp = pStamp + stampDelta;
					u256 cNum = blockNumber;

					BlockHeader parent;
					parent.setTimestamp(pStamp);
					parent.setDifficulty(pDiff);
					parent.setNumber(cNum - 1);

					BlockHeader current;
					current.setTimestamp(cStamp);
					current.setNumber(cNum);

					string tmptest = c_testDifficulty;
					std::map<string, string> replaceMap;
					replaceMap["[N]"] = toString(testN);
					replaceMap["[PDIFF]"] = toCompactHex(pDiff, HexPrefix::Add);
					replaceMap["[PSTAMP]"] = toCompactHex(pStamp, HexPrefix::Add);
					replaceMap["[СSTAMP]"] = toCompactHex(cStamp, HexPrefix::Add);
					replaceMap["[CNUM]"] = toCompactHex(cNum, HexPrefix::Add);
					replaceMap["[CDIFF]"] = toCompactHex(sealEngine.calculateDifficulty(current, parent), HexPrefix::Add);

					dev::test::RandomCode::parseTestWithTypes(tmptest, replaceMap);
					finalTest << tmptest;
				}

		finalTest << std::endl << "}";
		string testFile = finalTest.str();
		testFile = testFile.replace(testFile.find_last_of(","), 1, "");
		writeFile(testFileFullName, asBytes(testFile));
	}

	testDifficulty(testFileFullName, sealEngine, Network::MainNetwork);
}

BOOST_AUTO_TEST_CASE(basicDifficultyTest)
{
	string testPath = test::getTestPath();
	testPath += "/BasicTests/difficulty.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::MainNetwork)));

	testDifficulty(testPath, sealEngine, Network::MainNetwork);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <type_traits>
#include <unordered_map>
#include <boost/assert.hpp>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/lac/trilinos_sparse_matrix.h>

#include <app/app.hpp>
#include <matrix/assembly/velocity_var_form.hpp>
#include <matrix/dofs/dofindex_sets.hpp>
#include <matrix/sparsity_pattern/sparsity_pattern_base.hpp>
#include "aux/tensor_helpers.hpp"
#include "matrix/sparsity_pattern/sparsity_pattern.hpp"
#include "vsparsity.hpp"

namespace boltzmann {

template <typename METHOD, typename APP>
class SystemMatrixHandler
{
 public:
  constexpr const static int dim = 2;

 public:
  typedef dealii::TrilinosWrappers::SparseMatrix matrix_t;
  typedef dealii::TrilinosWrappers::MPI::Vector vector_t;
  typedef dealii::TrilinosWrappers::SparsityPattern sparsity_pattern_t;
  typedef dealii::DoFHandler<dim> dof_handler_t;

 private:
  typedef typename METHOD::spectral_basis_t spectral_basis_t;

 public:
  template <typename INDEXER>
  SystemMatrixHandler(const dof_handler_t& dof_handler,
                      const spectral_basis_t& spectral_basis,
                      const INDEXER& indexer,
                      const DoFIndexSetsBase& dof_map,
                      const double dt);

  const matrix_t& get_lhs() const;
  const matrix_t& get_rhs() const;

 private:
  void finalize();

 protected:
  typedef typename METHOD::template var_form_t<dim, APP> var_form_t;

 protected:
  matrix_t LHS_matrix;
  matrix_t RHS_matrix;
  bool finalized = false;

  VelocityVarForm<2> ventries_;
  //@{
  /// Velocity sparsity pattern for matrices appearing on the left, right
  dealii::SparsityPattern vsparsity_lhs_;
  dealii::SparsityPattern vsparsity_rhs_;
  //@}

  //@{
  // global sparsity patterns
  boltzmann::SparsityPattern sparsity_lhs_;
  boltzmann::SparsityPattern sparsity_rhs_;
  //@}
};

// ----------------------------------------------------------------------
template <typename METHOD, typename APP>
template <typename INDEXER>
SystemMatrixHandler<METHOD, APP>::SystemMatrixHandler(const dof_handler_t& dof_handler,
                                                      const spectral_basis_t& spectral_basis,
                                                      const INDEXER& indexer,
                                                      const DoFIndexSetsBase& dof_map,
                                                      const double dt)
{
  ventries_.init(spectral_basis);
  vsparsity<METHOD::lsq_type>::make(
      vsparsity_lhs_, vsparsity_rhs_, spectral_basis.size(), ventries_);

  sparsity_lhs_.init(spectral_basis, dof_handler, vsparsity_lhs_, indexer, dof_map, false);
  LHS_matrix.reinit(sparsity_lhs_.get_sparsity());

  sparsity_rhs_.init(spectral_basis, dof_handler, vsparsity_rhs_, indexer, dof_map, false);
  RHS_matrix.reinit(sparsity_rhs_.get_sparsity());

  const int dofs_per_cell = dof_handler.get_fe().dofs_per_cell;
  var_form_t var_form_id(dof_handler.get_fe());
  var_form_t var_form_transport(dof_handler.get_fe());
  var_form_t var_form_rhs(dof_handler.get_fe());

  const unsigned int mpi_this_process = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);
  const unsigned int N = spectral_basis.n_dofs();

  const double dt2 = dt * dt;

  typedef typename matrix_t::size_type size_type;

  for (auto cell = dof_handler.begin_active(); cell != dof_handler.end(); ++cell) {
    if (cell->subdomain_id() == mpi_this_process) {
      std::vector<size_type> local_dof_indices(dofs_per_cell);
      cell->get_dof_indices(local_dof_indices);
      var_form_id.calc_raw_identity(cell);
      var_form_transport.calc_transport_cell(cell);
      var_form_rhs.calc_identity(cell);

      const auto& IDx_loc = var_form_id.S0();
      const auto& T2x_loc = var_form_transport.T2();
      const auto& T1x_loc = var_form_rhs.S1();

      const auto& VS0 = ventries_.s0();
      const auto& VT1 = ventries_.t1();
      const auto& VT2 = ventries_.t2();

      // iterate over velocity test functions
      for (unsigned int j1 = 0; j1 < N; ++j1) {
        typedef std::unordered_map<unsigned int, unsigned int> map_t;
        // prepare column indices for velocity sparsity pattern
        auto prepare_cidx = [](
            map_t& J2Idx, const dealii::SparsityPattern& vsparsity, unsigned int j1) {
          // non zero velocity entries on row j1
          const unsigned int nentries = vsparsity.end(j1) - vsparsity.begin(j1);
          std::vector<unsigned int> J2(nentries);
          auto it = vsparsity.begin(j1);
          for (unsigned int i = 0; i < nentries; ++i) J2[i] = it++->column();
          // map column indices to array
          unsigned int counter = 0;
          std::for_each(J2.begin(), J2.end(), [&](unsigned int v) { J2Idx[v] = counter++; });
        };

        // std::unordered_map j2 (col index) -> local accumulator
        map_t mJ2_lhs, mJ2_rhs;
        prepare_cidx(mJ2_lhs, vsparsity_lhs_, j1);
        prepare_cidx(mJ2_rhs, vsparsity_rhs_, j1);

        auto get_idx = [](const map_t& m, unsigned int j2) {
          auto it = m.find(j2);
          assert(it != m.end());
          return it->second;
        };

        // global column indices
        std::vector<size_type> gcol_idx_lhs(mJ2_lhs.size());
        std::vector<size_type> gcol_idx_rhs(mJ2_rhs.size());
        // local accumulators
        std::vector<double> entriesLHS(mJ2_lhs.size());
        std::vector<double> entriesRHS(mJ2_rhs.size());

        // TODO: factor this out, it depends on the method!
        for (int i1_loc = 0; i1_loc < dofs_per_cell; ++i1_loc) {
          for (int i2_loc = 0; i2_loc < dofs_per_cell; ++i2_loc) {
            std::fill(entriesLHS.begin(), entriesLHS.end(), 0.0);
            std::fill(entriesRHS.begin(), entriesRHS.end(), 0.0);

            for (auto it_s0 = VS0.row_begin(j1); it_s0 < VS0.row_end(j1); ++it_s0) {
              const double v = dealii::scalar_product(IDx_loc[i1_loc][i2_loc], it_s0->val);
              unsigned int ilhs = get_idx(mJ2_lhs, it_s0->col);
              unsigned int irhs = get_idx(mJ2_rhs, it_s0->col);
              entriesLHS[ilhs] += v;
              entriesRHS[irhs] += v;
            }

            for (auto it_t1 = VT1.row_begin(j1); it_t1 < VT1.row_end(j1); ++it_t1) {
              unsigned int irhs = get_idx(mJ2_rhs, it_t1->col);
              entriesRHS[irhs] += dt * dealii::scalar_product(T1x_loc[i1_loc][i2_loc], it_t1->val);
            }

            for (auto it_t2 = VT2.row_begin(j1); it_t2 < VT2.row_end(j1); ++it_t2) {
              unsigned int ilhs = get_idx(mJ2_lhs, it_t2->col);
              entriesLHS[ilhs] += dt2 * dealii::scalar_product(T2x_loc[i1_loc][i2_loc], it_t2->val);
            }

            // prepare indices: global indices
            unsigned int i1 = local_dof_indices[i1_loc];
            unsigned int i2 = local_dof_indices[i2_loc];

            // this could be further improved...
            {
              unsigned int counter = 0;
              for (auto it = vsparsity_lhs_.begin(j1); it != vsparsity_lhs_.end(j1); ++it)
                gcol_idx_lhs[counter++] = indexer.to_global(i2, it->column());
#ifdef DEBUG
              size_type cmax = *std::max_element(gcol_idx_lhs.begin(), gcol_idx_lhs.end());
              BOOST_ASSERT(cmax < LHS_matrix.m());
#endif
            }
            {
              unsigned int counter = 0;
              for (auto it = vsparsity_rhs_.begin(j1); it != vsparsity_rhs_.end(j1); ++it)
                gcol_idx_rhs[counter++] = indexer.to_global(i2, it->column());
#ifdef DEBUG
              size_type cmax = *std::max_element(gcol_idx_rhs.begin(), gcol_idx_rhs.end());
              BOOST_ASSERT(cmax < RHS_matrix.m());
#endif
            }
            // global row index
            unsigned int gi = indexer.to_global(i1, j1);
            BOOST_ASSERT(gi < LHS_matrix.n());
            BOOST_ASSERT(gi < RHS_matrix.n());
            // TrilinosWrappers::SparseMatrix::add(row,n_cols, *col_indices, *values,
            // elide_zero_values col_indices_are_sorted)
            LHS_matrix.add(gi, mJ2_lhs.size(), gcol_idx_lhs.data(), entriesLHS.data(), true, true);
            RHS_matrix.add(gi, mJ2_rhs.size(), gcol_idx_rhs.data(), entriesRHS.data(), true, true);
          }
        }
      }
    }
  }
  finalize();
}

// ----------------------------------------------------------------------
template <typename METHOD, typename APP>
void
SystemMatrixHandler<METHOD, APP>::finalize()
{
  LHS_matrix.compress(dealii::VectorOperation::add);
  RHS_matrix.compress(dealii::VectorOperation::add);
  finalized = true;
}

// ----------------------------------------------------------------------
template <typename METHOD, typename APP>
const typename SystemMatrixHandler<METHOD, APP>::matrix_t&
SystemMatrixHandler<METHOD, APP>::get_lhs() const
{
  if (!finalized) throw std::runtime_error("not finalized!");
  return LHS_matrix;
}

// ----------------------------------------------------------------------
template <typename METHOD, typename APP>
const typename SystemMatrixHandler<METHOD, APP>::matrix_t&
SystemMatrixHandler<METHOD, APP>::get_rhs() const
{
  if (!finalized) throw std::runtime_error("not finalized!");
  return RHS_matrix;
}

}  // end namespace boltzmann


// BLAS level 3
// TNT arrays

#include <iostream>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/traits/tnt.hpp>
#include "utils.h"
#include "tnt_utils.h"

namespace blas = boost::numeric::bindings::blas;

using std::cout;
using std::endl;

#ifndef F_FORTRAN
typedef TNT::Array2D<double> m_t;
#else
typedef TNT::Fortran_Array2D<double> m_t;
#endif

int main() {

  cout << endl;

  m_t a (2, 2);
  init_m (a, kpp (1));
  print_m (a, "A");
  cout << endl;

  m_t b (2, 3);
  init_m (b, cls1());
  print_m (b, "B");
  cout << endl;

  m_t c (2, 3);
  init_m<const_val<double> > (c);
  print_m (c, "C");
  cout << endl;

  blas::gemm (a, b, c);
  print_m (c, "A B");
  cout << endl;

  blas::gemm (CblasNoTrans, CblasNoTrans, 1.0, a, b, 0.0, c);
  print_m (c, "A B");
  cout << endl;

  blas::gemm (1.0, a, b, 0.0, c);
  print_m (c, "A B");
  cout << endl;

  init_m (c, const_val<double> (1.));
  print_m (c, "C");
  cout << endl;

  blas::gemm (1.0, a, b, 0.05, c);
  print_m (c, "0.05 C + A B");
  cout << endl;

  m_t d (3, 2);

  blas::gemm (CblasTrans, CblasTrans, 1.0, b, a, 0.0, d);
  print_m (d, "B^T A^T");
  cout << endl;

  blas::gemm (CblasTrans, CblasNoTrans, 1.0, a, b, 0.0, c);
  print_m (c, "A^T B");
  cout << endl;

  blas::gemm (CblasTrans, CblasNoTrans, 1.0, b, a, 0.0, d);
  print_m (d, "B^T A");

  cout << endl;

}