/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2020 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#include "SiconosConfig.h"

#include "SiconosAlgebraTypeDef.hpp"
#include "SimpleMatrixFriends.hpp"
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include "BlockMatrixTest.hpp"
#include "SimpleMatrix.hpp"
#include "SiconosVector.hpp"

#define CPPUNIT_ASSERT_NOT_EQUAL(message, alpha, omega)      \
            if ((alpha) == (omega)) CPPUNIT_FAIL(message);

// on place cette classe de test dans le registry
CPPUNIT_TEST_SUITE_REGISTRATION(BlockMatrixTest);

void BlockMatrixTest::setUp()
{
  tol = 1e-12;

  B.reset(new SimpleMatrix(2, 2, 1));
  C.reset(new SimpleMatrix(2, 4, 2));
  D.reset(new SimpleMatrix(2, 1, 3));
  E.reset(new SimpleMatrix(3, 2, 4));
  F.reset(new SimpleMatrix(3, 4, 5));
  G.reset(new SimpleMatrix(3, 1, 6));

  m.resize(6);
  m[0] = B ;
  m[1] = C ;
  m[2] = D ;
  m[3] = E ;
  m[4] = F ;
  m[5] = G ;

  tRow.resize(2);
  tRow[0] = 2;
  tRow[1] = 5;
  tCol.resize(3);
  tCol[0] = 2;
  tCol[1] = 6;
  tCol[2] = 7;

  mapRef.reset(new BlocksMat(2, 1));
  (*mapRef)(0, 0) = B;
  (*mapRef)(1, 0) = E;

}

void BlockMatrixTest::tearDown()
{
  m.clear();
}

//______________________________________________________________________________

void BlockMatrixTest::testConstructor0() // constructor with a vector of SP::SiconosMatrix
{
  std::cout << "====================================" <<std::endl;
  std::cout << "=== Block Matrix tests start ...=== " <<std::endl;
  std::cout << "====================================" <<std::endl;
  std::cout << "--> Test: constructor 0." <<std::endl;
  SP::SiconosMatrix test(new BlockMatrix(m, 2, 3));
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ", test->isBlock() == true, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ", test->size(0) == 5, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ", test->size(1) == 7, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ", test->block(0, 0) == B, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ", test->block(0, 1) == C, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ", test->block(0, 2) == D, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ", test->block(1, 0) == E, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ", test->block(1, 1) == F, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ", test->block(1, 2) == G, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ", *test->tabRow() == tRow, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ", *test->tabCol() == tCol, true);
  std::cout << "--> Constructor 0 test ended with success." <<std::endl;
}

void BlockMatrixTest::testConstructor1() // Copy constructor, from a BlockMatrix
{
  std::cout << "--> Test: constructor 1." <<std::endl;
  SP::BlockMatrix ref(new BlockMatrix(m, 2, 3));
  SP::SiconosMatrix test(new BlockMatrix(*ref));
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor1 : ", test->isBlock() == true, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor1 : ", test->size(0) == 5, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor1 : ", test->size(1) == 7, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor1 : ", *(test->block(0, 0)) == *B, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor1 : ", *(test->block(0, 1)) == *C, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor1 : ", *(test->block(0, 2)) == *D, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor1 : ", *(test->block(1, 0)) == *E, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor1 : ", *(test->block(1, 1)) == *F, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor1 : ", *(test->block(1, 2)) == *G, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor1 : ", *test->tabRow() == tRow, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor1 : ", *test->tabCol() == tCol, true);
  std::cout << "--> Constructor 1(copy) test ended with success." <<std::endl;
}

void BlockMatrixTest::testConstructor2() // Copy constructor, from a SiconosMatrix(Block)
{
  std::cout << "--> Test: constructor 2." <<std::endl;
  SP::SiconosMatrix ref(new BlockMatrix(m, 2, 3));
  SP::SiconosMatrix test(new BlockMatrix(*ref));
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor2 : ", test->isBlock() == true, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor2 : ", test->size(0) == 5, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor2 : ", test->size(1) == 7, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor2 : ", *(test->block(0, 0)) == *B, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor2 : ", *(test->block(0, 1)) == *C, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor2 : ", *(test->block(0, 2)) == *D, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor2 : ", *(test->block(1, 0)) == *E, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor2 : ", *(test->block(1, 1)) == *F, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor2 : ", *(test->block(1, 2)) == *G, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor2 : ", *test->tabRow() == tRow, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor2 : ", *test->tabCol() == tCol, true);

  std::cout << "--> Constructor 2(copy) test ended with success." <<std::endl;
}
void BlockMatrixTest::testConstructor3() // Copy constructor, from a SiconosMatrix(Simple)
{
  std::cout << "--> Test: constructor 3." <<std::endl;
  SP::SiconosMatrix ref(new SimpleMatrix(5, 7, 2.3));
  SP::SiconosMatrix test(new BlockMatrix(*ref));

  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", test->isBlock() == true, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", test->size(0) == 5, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", test->size(1) == 7, true);
  for(unsigned int i = 0; i < 5; ++i)
    for(unsigned int j = 0; j < 7; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", fabs((*test)(i, j) - 2.3) < tol, true);
  SPC::Index tab = test->tabRow();
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", tab->size() == 1, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", (*tab)[0] == 5, true);

  tab = test->tabCol();
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", tab->size() == 1, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", (*tab)[0] == 7, true);

  std::cout << "--> Constructor 3(copy) test ended with success." <<std::endl;
}

void BlockMatrixTest::testConstructor4() // Constructor from 4 SP::SiconosMatrix
{
  std::cout << "--> Test: constructor 4." <<std::endl;
  SP::SiconosMatrix test(new BlockMatrix(B, C, E, F));
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor4 : ", test->isBlock() == true, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor4 : ", test->size(0) == 5, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor4 : ", test->size(1) == 6, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor4 : ", test->block(0, 0) == B, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor4 : ", test->block(0, 1) == C, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor4 : ", test->block(1, 0) == E, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor4 : ", test->block(1, 1) == F, true);
  SPC::Index tab = test->tabRow();
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", tab->size() == 2, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", (*tab)[0] == 2, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", (*tab)[1] == 5, true);
  tab = test->tabCol();
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", tab->size() == 2, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", (*tab)[0] == 2, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor3 : ", (*tab)[1] == 6, true);
  std::cout << "--> Constructor 4 test ended with success." <<std::endl;
}

// void BlockMatrixTest::testResize()
// {
//   std::cout << "--> Test: resize." <<std::endl;
//   SP::SiconosMatrix test(new BlockMatrix(m,2,3);
//   test->resize(3,4);
//   (*test)(2,0) = B;
//   (*test)(2,1) = C;
//   (*test)(2,2) = D;
//   (*test)(2,3) = C;
//   (*test)(0,3) = C;
//   (*test)(1,3) = F;

//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->isBlock() == true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->size(0) == 7, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->size(1) == 11, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(0,0) == B, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(0,1) == C, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(0,2) == D, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(0,3) == C, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(1,0) == E, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(1,1) == F, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(1,2) == G, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(1,3) == F, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(2,0) == B, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(2,1) == C, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(2,2) == D, true);
//   CPPUNIT_ASSERT_EQUAL_MESSAGE("testConstructor0 : ",test->block(2,3) == C, true);

//   std::cout << "--> resize test ended with success." <<std::endl;
// }

void BlockMatrixTest::testNormInf()
{
  std::cout << "--> Test: normInf." <<std::endl;
  SP::SiconosMatrix test(new BlockMatrix(m, 2, 3));
  test->zero();
  double n = 12;
  (*test)(4, 3) = n;
  (*test)(2, 1) = n - 3;
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testNormInf: ", test->normInf() == n, true);
  std::cout << "--> normInf test ended with success." <<std::endl;
}

void BlockMatrixTest::testZero()
{
  std::cout << "--> Test: zero." <<std::endl;
  SP::SiconosMatrix A(new SimpleMatrix(2, 2));
  A->eye();
  SP::SiconosMatrix H(new SimpleMatrix(2, 4));
  H->eye();
  SP::SiconosMatrix I(new SimpleMatrix(5, 2));
  I->eye();
  SP::SiconosMatrix J(new SimpleMatrix(5, 4));
  J->eye();

  std::vector<SP::SiconosMatrix> v(4);
  v[0] = A ;
  v[1] = H ;
  v[2] = I ;
  v[3] = J ;
  SP::SiconosMatrix test(new BlockMatrix(v, 2, 2));
  test->zero();
  unsigned int n1 = test->size(0);
  unsigned int n2 = test->size(1);
  for(unsigned int i = 0; i < n1; ++i)
    for(unsigned int j = 0; j < n2; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testZero : ", (*test)(i, j) == 0, true);
  for(unsigned int i = 0; i < 2; ++i)
  {
    for(unsigned int j = 0; j < 2; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testZero : ", (*A)(i, j) == 0, true);
    for(unsigned int j = 0; j < 4; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testZero : ", (*H)(i, j) == 0, true);
  }
  for(unsigned int i = 0; i < 5; ++i)
  {
    for(unsigned int j = 0; j < 2; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testZero : ", (*I)(i, j) == 0, true);
    for(unsigned int j = 0; j < 4; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testZero : ", (*J)(i, j) == 0, true);
  }

  std::cout << "--> zero test ended with success." <<std::endl;
}

void BlockMatrixTest::testEye()
{
  std::cout << "--> Test: eye." <<std::endl;
  SP::SiconosMatrix A(new SimpleMatrix(2, 2));
  SP::SiconosMatrix H(new SimpleMatrix(2, 4));
  SP::SiconosMatrix I(new SimpleMatrix(5, 2));
  SP::SiconosMatrix J(new SimpleMatrix(5, 4));

  std::vector<SP::SiconosMatrix> v(4);
  v[0] = A ;
  v[1] = H ;
  v[2] = I ;
  v[3] = J ;
  SP::SiconosMatrix test(new BlockMatrix(v, 2, 2));
  test->eye();
  unsigned int n1 = test->size(0);
  unsigned int n2 = test->size(1);
  for(unsigned int i = 0; i < n1; ++i)
    for(unsigned int j = 0; j < n2; ++j)
      if(i == j)
        CPPUNIT_ASSERT_EQUAL_MESSAGE("testEye : ", (*test)(i, j) == 1, true);
      else
        CPPUNIT_ASSERT_EQUAL_MESSAGE("testEye : ", (*test)(i, j) == 0, true);

  for(unsigned int i = 0; i < 2; ++i)
  {
    for(unsigned int j = 0; j < 2; ++j)
    {
      if(i == j)
        CPPUNIT_ASSERT_EQUAL_MESSAGE("testEye : ", (*A)(i, j) == 1, true);
      else
        CPPUNIT_ASSERT_EQUAL_MESSAGE("testEye : ", (*A)(i, j) == 0, true);
    }
    for(unsigned int j = 0; j < 4; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testEye : ", (*H)(i, j) == 0, true);
  }
  for(unsigned int i = 0; i < 5; ++i)
  {
    for(unsigned int j = 0; j < 2; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testEye : ", (*I)(i, j) == 0, true);
    for(unsigned int j = 0; j < 4; ++j)
    {
      if(i == j)
        CPPUNIT_ASSERT_EQUAL_MESSAGE("testEye : ", (*J)(i, j) == 1, true);
      else
        CPPUNIT_ASSERT_EQUAL_MESSAGE("testEye : ", (*J)(i, j) == 0, true);
    }
  }

  std::cout << "--> eye test ended with success." <<std::endl;
}
// Add tests with getDense ...

void BlockMatrixTest::testGetSetRowCol()
{
  std::cout << "--> Test: get, set Row and Col." <<std::endl;
  SP::SiconosVector tmp(new SiconosVector(6));
  SP::SiconosVector tmp1(new SiconosVector(6));
  (*tmp1)(0) = 1;
  (*tmp1)(2) = 2;
  SP::SiconosMatrix A(new SimpleMatrix(2, 2));
  SP::SiconosMatrix H(new SimpleMatrix(2, 4));
  SP::SiconosMatrix I(new SimpleMatrix(5, 2));
  SP::SiconosMatrix J(new SimpleMatrix(5, 4));
  std::vector<SP::SiconosMatrix> v(4);
  v[0] = A ;
  v[1] = H ;
  v[2] = I ;
  v[3] = J ;
  SP::SiconosMatrix test(new BlockMatrix(v, 2, 2));

  test->setRow(1, *tmp1);
  test->getRow(1, *tmp);

  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGetSetRowCol : ", *tmp == *tmp1, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGetSetRowCol : ", (*A)(1, 0) == 1, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGetSetRowCol : ", (*H)(1, 0) == 2, true);

  SP::SiconosVector tmp2(new SiconosVector(7));
  SP::SiconosVector tmp3(new SiconosVector(7));
  (*tmp3)(0) = 1;
  (*tmp3)(2) = 2;
  test->setCol(1, *tmp3);
  test->getCol(1, *tmp2);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGetSetRowCol : ", *tmp2 == *tmp3, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGetSetRowCol : ", (*A)(0, 1) == 1, true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGetSetRowCol : ", (*I)(0, 1) == 2, true);

  std::cout << "--> get, set Row and Col tests ended with success." <<std::endl;
}

void BlockMatrixTest::testAssignment()
{
  std::cout << "--> Test: assignment." <<std::endl;
  SP::SiconosMatrix Btmp(new SimpleMatrix(2, 2));
  SP::SiconosMatrix Ctmp(new SimpleMatrix(2, 5));
  SP::SiconosMatrix Dtmp(new SimpleMatrix(3, 2));
  SP::SiconosMatrix Etmp(new SimpleMatrix(3, 5));

  SP::SiconosMatrix test(new BlockMatrix(Btmp, Ctmp, Dtmp, Etmp));
  // Block = Siconos(Simple)
  unsigned int size0 = test->size(0), size1 = test->size(1);
  SP::SiconosMatrix ref(new SimpleMatrix(size0, size1));
  for(unsigned int i = 0; i < size0 ; ++i)
    for(unsigned int j = 0; j < size1; ++j)
      (*ref)(i, j) = i + j;
  *test = *ref;
  for(unsigned int i = 0; i < size0 ; ++i)
    for(unsigned int j = 0; j < size1; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testAssignment: ", fabs((*test)(i, j) - (*ref)(i, j)) < tol, true);

  // Block = Siconos(Block)
  test->zero();

  ref.reset(new BlockMatrix(m, 2, 3));
  *test = *ref;
  for(unsigned int i = 0; i < size0 ; ++i)
    for(unsigned int j = 0; j < size1; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testAssignment: ", fabs((*test)(i, j) - (*ref)(i, j)) < tol, true);

  // Block = Block
  test->zero();
  SP::BlockMatrix ref2(new BlockMatrix(m, 2, 3));
  *test = *ref2;
  for(unsigned int i = 0; i < size0 ; ++i)
    for(unsigned int j = 0; j < size1; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testAssignment: ", fabs((*test)(i, j) - (*ref2)(i, j)) < tol, true);
  std::cout << "-->  test assignment ended with success." <<std::endl;
}

void BlockMatrixTest::testOperators1()
{
  std::cout << "--> Test: operators1." <<std::endl;
  double tol = 1e-10;
  SP::SiconosMatrix Ab(new BlockMatrix(m, 2, 3));
  SP::SiconosMatrix Cb(new BlockMatrix(*Ab));
  SP::SiconosMatrix A(new SimpleMatrix(5, 7));

  for(unsigned int i = 0; i < 5 ; ++i)
    for(unsigned int j = 0; j < 7; ++j)
      (*A)(i, j) = i + j;

  double a = 2.3;
  int a1 = 2;

  // Block *= scal or /= scal
  *Cb *= a;
  for(unsigned int i = 0; i < Cb->size(0); ++i)
    for(unsigned int j = 0 ; j < Cb->size(1); ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testOperators1: ", fabs((*Cb)(i, j) - a * (*Ab)(i, j)) < tol, true);

  *Cb *= a1;
  for(unsigned int i = 0; i < Cb->size(0); ++i)
    for(unsigned int j = 0 ; j < Cb->size(1); ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testOperators1: ", fabs((*Cb)(i, j) - a1 * a * (*Ab)(i, j)) < tol, true);

  *Cb /= a;
  for(unsigned int i = 0; i < Cb->size(0); ++i)
    for(unsigned int j = 0 ; j < Cb->size(1); ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testOperators1: ", fabs((*Cb)(i, j) - a1 * (*Ab)(i, j)) < tol, true);
  *Cb /= a1;
  for(unsigned int i = 0; i < Cb->size(0); ++i)
    for(unsigned int j = 0 ; j < Cb->size(1); ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testOperators1: ", fabs((*Cb)(i, j) - (*Ab)(i, j)) < tol, true);

  // Block +=  Simple
  *Cb += *A;
  for(unsigned int i = 0; i < Cb->size(0); ++i)
    for(unsigned int j = 0 ; j < Cb->size(1); ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testOperators1: ", fabs((*Cb)(i, j) - (*Ab)(i, j) - (*A)(i, j)) < tol, true);

  // Block -=  Block
  *Cb -= *Ab;
  for(unsigned int i = 0; i < Cb->size(0); ++i)
    for(unsigned int j = 0 ; j < Cb->size(1); ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testOperators1: ", fabs((*Cb)(i, j) - (*A)(i, j)) < tol, true);

  // Block += Block
  *Cb += *Ab;
  for(unsigned int i = 0; i < Cb->size(0); ++i)
    for(unsigned int j = 0 ; j < Cb->size(1); ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testOperators1: ", fabs((*Cb)(i, j) - (*Ab)(i, j) - (*A)(i, j)) < tol, true);

  // Block -= Simple
  *Cb -= *A;
  for(unsigned int i = 0; i < Cb->size(0); ++i)
    for(unsigned int j = 0 ; j < Cb->size(1); ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testOperators1: ", fabs((*Cb)(i, j) - (*Ab)(i, j)) < tol, true);

  std::cout << "-->  test operators1 ended with success." <<std::endl;
}

void BlockMatrixTest::End()
{
  std::cout << "======================================" <<std::endl;
  std::cout << " ===== End of BlockMatrix Tests ===== " <<std::endl;
  std::cout << "======================================" <<std::endl;
}


// Copyright 2018 Dr. Matthias Hölzl
// Licensed under the MIT License
//

#include <boost/units/systems/si.hpp>
// #include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/cgs.hpp>
// #include <boost/units/systems/cgs/io.hpp>
#include <boost/units/operators.hpp>
#include "gmock/gmock.h"

using namespace boost::units;
using namespace boost::units::si;
using boost::units::cgs::centimeters;

using ::testing::Eq;

TEST(LengthTest1, Unitless)
{
    EXPECT_THAT(1, Eq(1));
}

TEST(LengthTest1, MetersOnly)
{
    EXPECT_THAT(1.0 * meter, Eq(1.0 * meter));
    EXPECT_THAT(1.0 * meter, Eq(2.0 * meter)) << "1 m = 2 m"; // << 2.0 * meter;
}

TEST(LengthTest1, MetersAndCentimeters)
{
    auto one_meter{ 1.0 * meter};
    auto one_hundred_centimeters{ 100.0 * centimeters };
    // EXPECT_THAT(one_meter, Eq(one_hundred_centimeters));
    EXPECT_THAT(one_meter, Eq(static_cast<decltype(one_meter)>(one_hundred_centimeters)));
}


//
// Created by philipp on 1/10/20.
//
#include <iostream>
#include <Eigen/Core>
#include <memory>

#include "funnels/distances.hh"
#include "funnels/dynamics.hh"
#include "helpers/trajectory.hh"
#include "helpers/bounding_box.hh"
#include "funnels/funnels.hh"
#include "funnels/funnel_sys.hh"
#include "aut/aut_collision.hh"
#include "aut/aut_survival.hh"
#include "aut/ta.hh"

//#include "heuristics/ex1_heu.hh"
#include "heuristics/ex1_fixed.hh"

using dist_t = partial_so2_dist_t;

using dyn_t = dynamics::kinematic_2d_sys_t;
using traj_t = trajectories::timed_way_points_t<dyn_t>;
using lyap_t = lyapunov::fixed_ellipsoidal_lyap_t<traj_t, dist_t>;
using cvx_hull_t = helper_geom::aligned_bounding_box_t<lyap_t>;
using fun_t = funnels::funnel_t<cvx_hull_t>;

using fun_ptr_t = fun_t::fun_ptr_t;
using fun_sys_t = funnel_sys_t<fun_t>;
using fun_sys_ptr_t = std::shared_ptr<fun_sys_t>;

using matrix_t = fun_t::matrix_t;
using vector_x_t = fun_t::vector_x_t;
using vector_u_t = fun_t::vector_u_t;
using vector_t_t = fun_t::vector_t_t;
using matrix_ptr_t = fun_t::matrix_ptr_t;
using vector_x_ptr_t = fun_t::vector_x_ptr_t;
using vector_u_ptr_t = fun_t::vector_u_ptr_t;
using vector_t_ptr_t = fun_t::vector_t_ptr_t;

using p_mat_t = lyap_t::s_mat_t;

const size_t n_verif=100;
const double_t t_step=200;

// Predefinitions
dist_t my_dist;

//Funnel shapes for obstacle and system
p_mat_t get_P_sys(){
  return p_mat_t::Identity();
}
p_mat_t get_P_obs(){
  p_mat_t P_obs = p_mat_t::Zero();
  P_obs(0,0) = 1.; P_obs(1,1) = 1.;
  P_obs(2,2) = 1.e-10; P_obs(3,3) = 1.e-10;
  return P_obs;
}

fun_ptr_t get_obstacle_box_fun(){
  // Creates an obstacle that moves along a square box of side length 0.5
  // The obstacle is a sphere of radius 0.5 in x-y-plane

  // Velocity of the obstacle is 0.25
  const double vel = 0.25;
  const double corner = 0.5;

  const double gamma_obs = 0.; // Obstacle does not contract
  const process_t &my_proc_obs = utils_ext::process_map.create_and_get
      ("obs_col");
  // Create the funnel
  fun_ptr_t my_obs_fun = make_shared<fun_t>(4 * n_verif, "obs_fun", my_proc_obs,
                                            4.*get_P_obs(), gamma_obs, my_dist);
  //Fill it
  vector_t_ptr_t t_ptr = std::make_shared<vector_t_t>(4 * n_verif);
  vector_u_ptr_t u_ptr = std::make_shared<vector_u_t>(my_obs_fun->dimu);
  matrix_ptr_t x_ptr = std::make_shared<matrix_t>(my_obs_fun->dimx, 4 * n_verif);

  // set time
  t_ptr->setLinSpaced(0., 4.*corner/vel);
  // dummy for control
  u_ptr->fill(0);
  // Obstacle will start off upper left corner and go clock-wise
  //left upper -> right upper
  x_ptr->block(0, 0, 1, n_verif) =
      Eigen::VectorXd::LinSpaced(n_verif, -corner, corner).transpose();
  x_ptr->block(1, 0, 1, n_verif).fill(corner);
  x_ptr->block(2, 0, 1, n_verif).fill(vel);
  x_ptr->block(3, 0, 1, n_verif).fill(0);
  //right upper -> right lower
  x_ptr->block(0, n_verif, 1, n_verif).fill(corner);
  x_ptr->block(1, n_verif, 1, n_verif) =
      Eigen::VectorXd::LinSpaced(n_verif, corner, -corner).transpose();
  x_ptr->block(2, n_verif, 1, n_verif).fill(0);
  x_ptr->block(3, n_verif, 1, n_verif).fill(-vel);
  // right lower -> left lower
  x_ptr->block(0, 2 * n_verif, 1, n_verif) =
      Eigen::VectorXd::LinSpaced(n_verif, corner, -corner).transpose();
  x_ptr->block(1, 2 * n_verif, 1, n_verif).fill(-corner);
  x_ptr->block(2, 2 * n_verif, 1, n_verif).fill(-vel);
  x_ptr->block(3, 2 * n_verif, 1, n_verif).fill(0);
  //left lower -> left upper
  x_ptr->block(0, 3 * n_verif, 1, n_verif).fill(-corner);
  x_ptr->block(1, 3 * n_verif, 1, n_verif) =
      Eigen::VectorXd::LinSpaced(n_verif, -corner, corner).transpose();
  x_ptr->block(2, 3 * n_verif, 1, n_verif).fill(0);
  x_ptr->block(3, 3 * n_verif, 1, n_verif).fill(vel);

  // Set in funnel
  my_obs_fun->set_traj(t_ptr, u_ptr, x_ptr);

  return my_obs_fun;
}

fun_ptr_t get_init_fun(){

  // Initial funnel system starts off without velocity at
  // position [0.75,1.]in a very small funnel
  //
  // Create the funnel
  fun_ptr_t my_init_fun = make_shared<fun_t>(3, "fun_init",
      utils_ext::process_map["proc_fun_sys_0"], (100.*100.)*get_P_sys(),
      0.0, my_dist);
  //Fill it
  vector_u_t u = vector_u_t::Zero();
  vector_x_t x0 = vector_x_t::Zero();
  x0(0) = 0.45;
  x0(1)= 0.45;
  my_init_fun->compute(x0, 0., 0.1, u);
  my_init_fun->loc().set_initial(true);
  my_init_fun->start_family(my_init_fun);
  return my_init_fun;
}

int main() {

  // Playground is between -1 and 1 in x and y dimension
  // Obstacle moves along a scaled square box of side length 0.5
  // obstacle is a sphere of size 0.5

  std::cout << "funnel sys" << std::endl;

  utils_ext::clear_all_maps();
  // global clock
  funnels::clock_ta_t glob_clk = utils_ext::clock_map.create_and_get("c_g");
  // funnel controller clock
  funnels::clock_ta_t ctrl_clk = utils_ext::clock_map.create_and_get("c_t");
  // funnel local clock
  funnels::clock_ta_t lcl_clk = utils_ext::clock_map.create_and_get("c_h");
  // obstacle movement clock
  funnels::clock_ta_t obs_clk = utils_ext::clock_map.create_and_get("c_o");

  // process for funnel sys
  funnels::process_t &fun_sys_proc = utils_ext::process_map.create_and_get("proc_fun_sys_0");

  // Dummy event for switching between funnels / converging to funnels
  event_t &dummy = utils_ext::event_map.create_and_get("no_action");
  event_t &dummy2 = utils_ext::event_map.create_and_get("init");

  // Define the quadratic term of the Lyapunov function,
  // Here we use two different ones:
  // One for the dynamical system corresponding to a sphere in
  // in the state-space
  // One for the obstacle, corresponding to a cylinder in the state-space
  // (The obstacle blocks a position for all velocities)
  // Note approximation necessary

  // Create the funnel system (funnels will be added later
  fun_sys_ptr_t my_fun_sys = std::make_shared<fun_sys_t>(ctrl_clk, lcl_clk,
      fun_sys_proc);

  // Create the obstacle automaton
  aut_col::aut_col<fun_t> my_aut_col(get_obstacle_box_fun(), obs_clk);

  // Create the survival automaton
  // longer then the cycle time of the obstacle
  aut_surv::aut_surv my_timer("surv", glob_clk, 1.5*4*2., false);

  // Get initial funnel/position
  my_fun_sys->add_funnel(get_init_fun(), false, true);

  // Test expanding a funnel
  add_new_funnels(my_fun_sys->all_funnels()[0], 0.01, *my_fun_sys);
  for (auto ff : my_fun_sys->all_funnels()){
    std::cout << ff->loc().name() << " " << ff->t()(ff->size()-1) << std::endl;
  }

  // Add it all together to obtain a TA
  ta::ta_t my_ta("system:funnel_ex_1\n\n");

  my_timer.register_self(my_ta);
  my_aut_col.register_self(my_ta);
  my_fun_sys->register_self(my_ta);

  my_fun_sys->clear_all();
  // Compute transitions and collisions
  // COLLISSIONS FIRST!!
  my_aut_col.compute_collisions(my_fun_sys, t_step);
  my_fun_sys->generate_transitions(t_step, true, false);

  // Auxilliary declarations
  // todo not very beautiful
  std::function<std::string()> aux_event =
      [](){return utils_ext::event_map["no_action"].declare()+
              "\n" + utils_ext::event_map["init"].declare();};

  my_ta._fun_event.push_back(aux_event);

  // Timer update
  my_timer.set_t_step(t_step);

  std::cerr << my_ta.declare() << std::endl;

  return 0;
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file interpolateddiscountcurve.hpp
    \brief interpolated discount term structure
    \ingroup termstructures
*/

#ifndef quantext_interpolated_discount_curve_hpp
#define quantext_interpolated_discount_curve_hpp

#include <boost/make_shared.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <qle/quotes/logquote.hpp>

namespace QuantExt {
using namespace QuantLib;

//! InterpolatedDiscountCurve based on loglinear interpolation of DiscountFactors
/*! InterpolatedDiscountCurve based on loglinear interpolation of DiscountFactors,
    flat fwd extrapolation is always enabled, the term structure has always a
    floating reference date

        \ingroup termstructures
    */
class InterpolatedDiscountCurve : public YieldTermStructure {
public:
    enum class Interpolation { logLinear, linearZero };
    enum class Extrapolation { flatFwd, flatZero };
    //! \name Constructors
    //@{
    //! default constructor
    InterpolatedDiscountCurve(const std::vector<Time>& times, const std::vector<Handle<Quote>>& quotes,
                              const Natural settlementDays, const Calendar& cal, const DayCounter& dc,
                              const Interpolation interpolation = Interpolation::logLinear,
                              const Extrapolation extrapolation = Extrapolation::flatFwd)
        : YieldTermStructure(settlementDays, cal, dc), times_(times), interpolation_(interpolation),
          extrapolation_(extrapolation) {
        initalise(quotes);
    }

    //! constructor that takes a vector of dates
    InterpolatedDiscountCurve(const std::vector<Date>& dates, const std::vector<Handle<Quote>>& quotes,
                              const Natural settlementDays, const Calendar& cal, const DayCounter& dc,
                              const Interpolation interpolation = Interpolation::logLinear,
                              const Extrapolation extrapolation = Extrapolation::flatFwd)
        : YieldTermStructure(settlementDays, cal, dc), times_(dates.size()), interpolation_(interpolation),
          extrapolation_(extrapolation) {
        for (Size i = 0; i < dates.size(); ++i)
            times_[i] = timeFromReference(dates[i]);
        initalise(quotes);
    }
    //@}

private:
    void initalise(const std::vector<Handle<Quote>>& quotes) {
        QL_REQUIRE(times_.size() > 1, "at least two times required");
        QL_REQUIRE(times_[0] == 0.0, "First time must be 0, got " << times_[0]); // or date=asof
        QL_REQUIRE(times_.size() == quotes.size(), "size of time and quote vectors do not match");
        for (Size i = 0; i < quotes.size(); ++i) {
            quotes_.push_back(boost::make_shared<LogQuote>(quotes[i]));
        }
        for (Size i = 0; i < times_.size() - 1; ++i)
            timeDiffs_.push_back(times_[i + 1] - times_[i]);
    }

    //! \name TermStructure interface
    //@{
    Date maxDate() const { return Date::maxDate(); } // flat fwd extrapolation
    //@}

protected:
    DiscountFactor discountImpl(Time t) const {
        if (t > this->times_.back() && extrapolation_ == Extrapolation::flatZero) {
            Real tMax = this->times_.back();
            Real dMax = std::exp(quotes_.back()->value());
            return std::pow(dMax, t / tMax);
        }
        std::vector<Time>::const_iterator it = std::upper_bound(times_.begin(), times_.end(), t);
        Size i = std::min<Size>(it - times_.begin(), times_.size() - 1);
        Real weight = (times_[i] - t) / timeDiffs_[i - 1];
        if (interpolation_ == Interpolation::logLinear || t > this->times_.back()) {
            // this handles flat fwd extrapolation (t > times.back()) as well
            Real value = (1.0 - weight) * quotes_[i]->value() + weight * quotes_[i - 1]->value();
            return ::exp(value);
        } else {
            Real value =
                (1.0 - weight) * quotes_[i]->value() / times_[i] + weight * quotes_[i - 1]->value() / times_[i - 1];
            return ::exp(t * value);
        }
    }

private:
    std::vector<Time> times_;
    std::vector<Time> timeDiffs_;
    std::vector<boost::shared_ptr<Quote>> quotes_;
    Interpolation interpolation_;
    Extrapolation extrapolation_;
};

} // namespace QuantExt

#endif


#include <boost/random/mersenne_twister.hpp>


#include <boost/multiprecision/gmp.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/detail/lightweight_test.hpp>
#include <kanooth/numbers/boost_integer.hpp>
//#include <boost/multiprecision/cpp_int.hpp>
#include <kanooth/numbers/generate_random_bits_number.hpp>
#include <ctime>

template <class Number>
struct tester
{
    typedef Number test_type;
    typedef boost::multiprecision::mpz_int ref_type;

    boost::random::mt19937 engine;
    unsigned last_error_count;

    ref_type a, b, c, d;
    int si;
    unsigned ui;
    test_type a1, b1, c1, d1;

    void t1()
    {
        BOOST_TEST_EQ(a.str(), a1.str());
        BOOST_TEST_EQ(b.str(), b1.str());
        BOOST_TEST_EQ(c.str(), c1.str());
        BOOST_TEST_EQ(d.str(), d1.str());
        BOOST_TEST_EQ(ref_type(a+b).str(), test_type(a1 + b1).str());
        BOOST_TEST_EQ((ref_type(a)+=b).str(), (test_type(a1) += b1).str());
        BOOST_TEST_EQ((ref_type(b)+=a).str(), (test_type(b1) += a1).str());
        BOOST_TEST_EQ(ref_type(a-b).str(), test_type(a1 - b1).str());
        BOOST_TEST_EQ((ref_type(a)-=b).str(), (test_type(a1) -= b1).str());
        BOOST_TEST_EQ(ref_type(ref_type(-a)+b).str(), test_type(test_type(-a1) + b1).str());
        BOOST_TEST_EQ(ref_type(ref_type(-a)-b).str(), test_type(test_type(-a1) - b1).str());
        BOOST_TEST_EQ(ref_type(c * d).str(), test_type(c1 * d1).str());
        BOOST_TEST_EQ((ref_type(c)*=d).str(), (test_type(c1) *= d1).str());
        BOOST_TEST_EQ((ref_type(d)*=c).str(), (test_type(d1) *= c1).str());
        BOOST_TEST_EQ(ref_type(c * -d).str(), test_type(c1 * -d1).str());
        BOOST_TEST_EQ(ref_type(-c * d).str(), test_type(-c1 * d1).str());
        BOOST_TEST_EQ((ref_type(c)*=-d).str(), (test_type(c1) *= -d1).str());
        BOOST_TEST_EQ((ref_type(-d)*=c).str(), (test_type(-d1) *= c1).str());
        BOOST_TEST_EQ(ref_type(b * c).str(), test_type(b1 * c1).str());
        BOOST_TEST_EQ(ref_type(a / b).str(), test_type(a1 / b1).str());
        BOOST_TEST_EQ((ref_type(a)/=b).str(), (test_type(a1) /= b1).str());
        BOOST_TEST_EQ(ref_type(a / -b).str(), test_type(a1 / -b1).str());
        BOOST_TEST_EQ(ref_type(-a / b).str(), test_type(-a1 / b1).str());
        BOOST_TEST_EQ((ref_type(a)/=-b).str(), (test_type(a1) /= -b1).str());
        BOOST_TEST_EQ((ref_type(-a)/=b).str(), (test_type(-a1) /= b1).str());
        BOOST_TEST_EQ(ref_type(a / d).str(), test_type(a1 / d1).str());
        BOOST_TEST_EQ(ref_type(a % b).str(), test_type(a1 % b1).str());
        BOOST_TEST_EQ((ref_type(a)%=b).str(), (test_type(a1) %= b1).str());
        BOOST_TEST_EQ(ref_type(a % -b).str(), test_type(a1 % -b1).str());
        BOOST_TEST_EQ((ref_type(a)%=-b).str(), (test_type(a1) %= -b1).str());
        BOOST_TEST_EQ(ref_type(-a % b).str(), test_type(-a1 % b1).str());
        BOOST_TEST_EQ((ref_type(-a)%=b).str(), (test_type(-a1) %= b1).str());
        BOOST_TEST_EQ(ref_type(a % d).str(), test_type(a1 % d1).str());
        BOOST_TEST_EQ((ref_type(a)%=d).str(), (test_type(a1) %= d1).str());
    }

    void t2()
    {
        // bitwise ops:
        BOOST_TEST_EQ(ref_type(a|b).str(), test_type(a1 | b1).str());
        BOOST_TEST_EQ((ref_type(a)|=b).str(), (test_type(a1) |= b1).str());
        BOOST_TEST_EQ(ref_type(-a|b).str(), test_type(-a1 | b1).str());
        BOOST_TEST_EQ((ref_type(-a)|=b).str(), (test_type(-a1) |= b1).str());
        BOOST_TEST_EQ(ref_type(a|-b).str(), test_type(a1 | -b1).str());
        BOOST_TEST_EQ((ref_type(a)|=-b).str(), (test_type(a1) |= -b1).str());
        BOOST_TEST_EQ(ref_type(-a|-b).str(), test_type(-a1 | -b1).str());
        BOOST_TEST_EQ((ref_type(-a)|=-b).str(), (test_type(-a1) |= -b1).str());
        BOOST_TEST_EQ(ref_type(a&b).str(), test_type(a1 & b1).str());
        BOOST_TEST_EQ((ref_type(a)&=b).str(), (test_type(a1) &= b1).str());
        BOOST_TEST_EQ(ref_type(-a&b).str(), test_type(-a1 & b1).str());
        BOOST_TEST_EQ((ref_type(-a)&=b).str(), (test_type(-a1) &= b1).str());
        BOOST_TEST_EQ(ref_type(a&-b).str(), test_type(a1 & -b1).str());
        BOOST_TEST_EQ((ref_type(a)&=-b).str(), (test_type(a1) &= -b1).str());
        BOOST_TEST_EQ(ref_type(-a&-b).str(), test_type(-a1 & -b1).str());
        BOOST_TEST_EQ((ref_type(-a)&=-b).str(), (test_type(-a1) &= -b1).str());
        BOOST_TEST_EQ(ref_type(a^b).str(), test_type(a1 ^ b1).str());
        BOOST_TEST_EQ((ref_type(a)^=b).str(), (test_type(a1) ^= b1).str());
        BOOST_TEST_EQ(ref_type(-a^b).str(), test_type(-a1 ^ b1).str());
        BOOST_TEST_EQ((ref_type(-a)^=b).str(), (test_type(-a1) ^= b1).str());
        BOOST_TEST_EQ(ref_type(a^-b).str(), test_type(a1 ^ -b1).str());
        BOOST_TEST_EQ((ref_type(a)^=-b).str(), (test_type(a1) ^= -b1).str());
        BOOST_TEST_EQ(ref_type(-a^-b).str(), test_type(-a1 ^ -b1).str());
        BOOST_TEST_EQ((ref_type(-a)^=-b).str(), (test_type(-a1) ^= -b1).str());
        // Shift ops:
        for(unsigned i = 0; i < 128; ++i)
        {
            BOOST_TEST_EQ(ref_type(a << i).str(), test_type(a1 << i).str());
            BOOST_TEST_EQ(ref_type(a >> i).str(), test_type(a1 >> i).str());
        }
        // gcd/lcm
        BOOST_TEST_EQ(ref_type(gcd(a, b)).str(), test_type(gcd(a1, b1)).str());
        BOOST_TEST_EQ(ref_type(lcm(c, d)).str(), test_type(lcm(c1, d1)).str());
        BOOST_TEST_EQ(ref_type(gcd(-a, b)).str(), test_type(gcd(-a1, b1)).str());
        BOOST_TEST_EQ(ref_type(lcm(-c, d)).str(), test_type(lcm(-c1, d1)).str());
        BOOST_TEST_EQ(ref_type(gcd(-a, -b)).str(), test_type(gcd(-a1, -b1)).str());
        BOOST_TEST_EQ(ref_type(lcm(-c, -d)).str(), test_type(lcm(-c1, -d1)).str());
        BOOST_TEST_EQ(ref_type(gcd(a, -b)).str(), test_type(gcd(a1, -b1)).str());
        BOOST_TEST_EQ(ref_type(lcm(c, -d)).str(), test_type(lcm(c1, -d1)).str());
    }

    void t3()
    {
        // Now check operations involving signed integers:
        BOOST_TEST_EQ(ref_type(a + si).str(), test_type(a1 + si).str());
        BOOST_TEST_EQ(ref_type(a + -si).str(), test_type(a1 + -si).str());
        BOOST_TEST_EQ(ref_type(-a + si).str(), test_type(-a1 + si).str());
        BOOST_TEST_EQ(ref_type(si + a).str(), test_type(si + a1).str());
        BOOST_TEST_EQ((ref_type(a)+=si).str(), (test_type(a1) += si).str());
        BOOST_TEST_EQ((ref_type(a)+=-si).str(), (test_type(a1) += -si).str());
        BOOST_TEST_EQ((ref_type(-a)+=si).str(), (test_type(-a1) += si).str());
        BOOST_TEST_EQ((ref_type(-a)+=-si).str(), (test_type(-a1) += -si).str());
        BOOST_TEST_EQ(ref_type(a - si).str(), test_type(a1 - si).str());
        BOOST_TEST_EQ(ref_type(a - -si).str(), test_type(a1 - -si).str());
        BOOST_TEST_EQ(ref_type(-a - si).str(), test_type(-a1 - si).str());
        BOOST_TEST_EQ(ref_type(si - a).str(), test_type(si - a1).str());
        BOOST_TEST_EQ((ref_type(a)-=si).str(), (test_type(a1) -= si).str());
        BOOST_TEST_EQ((ref_type(a)-=-si).str(), (test_type(a1) -= -si).str());
        BOOST_TEST_EQ((ref_type(-a)-=si).str(), (test_type(-a1) -= si).str());
        BOOST_TEST_EQ((ref_type(-a)-=-si).str(), (test_type(-a1) -= -si).str());
        BOOST_TEST_EQ(ref_type(b * si).str(), test_type(b1 * si).str());
        BOOST_TEST_EQ(ref_type(b * -si).str(), test_type(b1 * -si).str());
        BOOST_TEST_EQ(ref_type(-b * si).str(), test_type(-b1 * si).str());
        BOOST_TEST_EQ(ref_type(si * b).str(), test_type(si * b1).str());
        BOOST_TEST_EQ((ref_type(a)*=si).str(), (test_type(a1) *= si).str());
        BOOST_TEST_EQ((ref_type(a)*=-si).str(), (test_type(a1) *= -si).str());
        BOOST_TEST_EQ((ref_type(-a)*=si).str(), (test_type(-a1) *= si).str());
        BOOST_TEST_EQ((ref_type(-a)*=-si).str(), (test_type(-a1) *= -si).str());
        BOOST_TEST_EQ(ref_type(a / si).str(), test_type(a1 / si).str());
        BOOST_TEST_EQ(ref_type(a / -si).str(), test_type(a1 / -si).str());
        BOOST_TEST_EQ(ref_type(-a / si).str(), test_type(-a1 / si).str());
        BOOST_TEST_EQ((ref_type(a)/=si).str(), (test_type(a1) /= si).str());
        BOOST_TEST_EQ((ref_type(a)/=-si).str(), (test_type(a1) /= -si).str());
        BOOST_TEST_EQ((ref_type(-a)/=si).str(), (test_type(-a1) /= si).str());
        BOOST_TEST_EQ((ref_type(-a)/=-si).str(), (test_type(-a1) /= -si).str());
        BOOST_TEST_EQ(ref_type(a % si).str(), test_type(a1 % si).str());
        BOOST_TEST_EQ(ref_type(a % -si).str(), test_type(a1 % -si).str());
        BOOST_TEST_EQ(ref_type(-a % si).str(), test_type(-a1 % si).str());
        BOOST_TEST_EQ((ref_type(a)%=si).str(), (test_type(a1) %= si).str());
        BOOST_TEST_EQ((ref_type(a)%=-si).str(), (test_type(a1) %= -si).str());
        BOOST_TEST_EQ((ref_type(-a)%=si).str(), (test_type(-a1) %= si).str());
        BOOST_TEST_EQ((ref_type(-a)%=-si).str(), (test_type(-a1) %= -si).str());
        BOOST_TEST_EQ(ref_type(a|si).str(), test_type(a1 | si).str());
        BOOST_TEST_EQ((ref_type(a)|=si).str(), (test_type(a1) |= si).str());
        BOOST_TEST_EQ(ref_type(a&si).str(), test_type(a1 & si).str());
        BOOST_TEST_EQ((ref_type(a)&=si).str(), (test_type(a1) &= si).str());
        BOOST_TEST_EQ(ref_type(a^si).str(), test_type(a1 ^ si).str());
        BOOST_TEST_EQ((ref_type(a)^=si).str(), (test_type(a1) ^= si).str());
        BOOST_TEST_EQ(ref_type(si|a).str(), test_type(si|a1).str());
        BOOST_TEST_EQ(ref_type(si&a).str(), test_type(si&a1).str());
        BOOST_TEST_EQ(ref_type(si^a).str(), test_type(si^a1).str());
        BOOST_TEST_EQ(ref_type(gcd(a, si)).str(), test_type(gcd(a1, si)).str());
        BOOST_TEST_EQ(ref_type(gcd(si, b)).str(), test_type(gcd(si, b1)).str());
        BOOST_TEST_EQ(ref_type(lcm(c, si)).str(), test_type(lcm(c1, si)).str());
        BOOST_TEST_EQ(ref_type(lcm(si, d)).str(), test_type(lcm(si, d1)).str());
        BOOST_TEST_EQ(ref_type(gcd(-a, si)).str(), test_type(gcd(-a1, si)).str());
        BOOST_TEST_EQ(ref_type(gcd(-si, b)).str(), test_type(gcd(-si, b1)).str());
        BOOST_TEST_EQ(ref_type(lcm(-c, si)).str(), test_type(lcm(-c1, si)).str());
        BOOST_TEST_EQ(ref_type(lcm(-si, d)).str(), test_type(lcm(-si, d1)).str());
        BOOST_TEST_EQ(ref_type(gcd(-a, -si)).str(), test_type(gcd(-a1, -si)).str());
        BOOST_TEST_EQ(ref_type(gcd(-si, -b)).str(), test_type(gcd(-si, -b1)).str());
        BOOST_TEST_EQ(ref_type(lcm(-c, -si)).str(), test_type(lcm(-c1, -si)).str());
        BOOST_TEST_EQ(ref_type(lcm(-si, -d)).str(), test_type(lcm(-si, -d1)).str());
        BOOST_TEST_EQ(ref_type(gcd(a, -si)).str(), test_type(gcd(a1, -si)).str());
        BOOST_TEST_EQ(ref_type(gcd(si, -b)).str(), test_type(gcd(si, -b1)).str());
        BOOST_TEST_EQ(ref_type(lcm(c, -si)).str(), test_type(lcm(c1, -si)).str());
        BOOST_TEST_EQ(ref_type(lcm(si, -d)).str(), test_type(lcm(si, -d1)).str());
    }

    void t4()
    {
        // Now check operations involving unsigned integers:
        BOOST_TEST_EQ(ref_type(a + ui).str(), test_type(a1 + ui).str());
        BOOST_TEST_EQ(ref_type(-a + ui).str(), test_type(-a1 + ui).str());
        BOOST_TEST_EQ(ref_type(ui + a).str(), test_type(ui + a1).str());
        BOOST_TEST_EQ((ref_type(a)+=ui).str(), (test_type(a1) += ui).str());
        BOOST_TEST_EQ((ref_type(-a)+=ui).str(), (test_type(-a1) += ui).str());
        BOOST_TEST_EQ(ref_type(a - ui).str(), test_type(a1 - ui).str());
        BOOST_TEST_EQ(ref_type(-a - ui).str(), test_type(-a1 - ui).str());
        BOOST_TEST_EQ(ref_type(ui - a).str(), test_type(ui - a1).str());
        BOOST_TEST_EQ((ref_type(a)-=ui).str(), (test_type(a1) -= ui).str());
        BOOST_TEST_EQ((ref_type(-a)-=ui).str(), (test_type(-a1) -= ui).str());
        BOOST_TEST_EQ(ref_type(b * ui).str(), test_type(b1 * ui).str());
        BOOST_TEST_EQ(ref_type(-b * ui).str(), test_type(-b1 * ui).str());
        BOOST_TEST_EQ(ref_type(ui * b).str(), test_type(ui * b1).str());
        BOOST_TEST_EQ((ref_type(a)*=ui).str(), (test_type(a1) *= ui).str());
        BOOST_TEST_EQ((ref_type(-a)*=ui).str(), (test_type(-a1) *= ui).str());
        BOOST_TEST_EQ(ref_type(a / ui).str(), test_type(a1 / ui).str());
        BOOST_TEST_EQ(ref_type(-a / ui).str(), test_type(-a1 / ui).str());
        BOOST_TEST_EQ((ref_type(a)/=ui).str(), (test_type(a1) /= ui).str());
        BOOST_TEST_EQ((ref_type(-a)/=ui).str(), (test_type(-a1) /= ui).str());
        BOOST_TEST_EQ(ref_type(a % ui).str(), test_type(a1 % ui).str());
        BOOST_TEST_EQ(ref_type(-a % ui).str(), test_type(-a1 % ui).str());
        BOOST_TEST_EQ((ref_type(a)%=ui).str(), (test_type(a1) %= ui).str());
        BOOST_TEST_EQ((ref_type(-a)%=ui).str(), (test_type(-a1) %= ui).str());
        BOOST_TEST_EQ(ref_type(a|ui).str(), test_type(a1 | ui).str());
        BOOST_TEST_EQ((ref_type(a)|=ui).str(), (test_type(a1) |= ui).str());
        BOOST_TEST_EQ(ref_type(a&ui).str(), test_type(a1 & ui).str());
        BOOST_TEST_EQ((ref_type(a)&=ui).str(), (test_type(a1) &= ui).str());
        BOOST_TEST_EQ(ref_type(a^ui).str(), test_type(a1 ^ ui).str());
        BOOST_TEST_EQ((ref_type(a)^=ui).str(), (test_type(a1) ^= ui).str());
        BOOST_TEST_EQ(ref_type(ui|a).str(), test_type(ui|a1).str());
        BOOST_TEST_EQ(ref_type(ui&a).str(), test_type(ui&a1).str());
        BOOST_TEST_EQ(ref_type(ui^a).str(), test_type(ui^a1).str());
        BOOST_TEST_EQ(ref_type(gcd(a, ui)).str(), test_type(gcd(a1, ui)).str());
        BOOST_TEST_EQ(ref_type(gcd(ui, b)).str(), test_type(gcd(ui, b1)).str());
        BOOST_TEST_EQ(ref_type(lcm(c, ui)).str(), test_type(lcm(c1, ui)).str());
        BOOST_TEST_EQ(ref_type(lcm(ui, d)).str(), test_type(lcm(ui, d1)).str());
        BOOST_TEST_EQ(ref_type(gcd(-a, ui)).str(), test_type(gcd(-a1, ui)).str());
        BOOST_TEST_EQ(ref_type(lcm(-c, ui)).str(), test_type(lcm(-c1, ui)).str());
        BOOST_TEST_EQ(ref_type(gcd(ui, -b)).str(), test_type(gcd(ui, -b1)).str());
        BOOST_TEST_EQ(ref_type(lcm(ui, -d)).str(), test_type(lcm(ui, -d1)).str());
    }

    void t5()
    {
        //
        // Now integer functions:
        //
        ref_type z1, z2;
        test_type t1, t2;
        divide_qr(a, b, z1, z2);
        divide_qr(a1, b1, t1, t2);
        BOOST_TEST_EQ(z1.str(), t1.str());
        BOOST_TEST_EQ(z2.str(), t2.str());
        BOOST_TEST_EQ(integer_modulus(a, si), integer_modulus(a1, si));
        BOOST_TEST_EQ(lsb(a), lsb(a1));

        for(unsigned i = 0; i < 1000; i += 13)
        {
            BOOST_TEST_EQ(bit_test(a, i), bit_test(a1, i));
        }
        // We have to take care that our powers don't grow too large, otherwise this takes "forever",
        // also don't test for modulo types, as these may give a diffferent result from arbitrary
        // precision types:
        BOOST_TEST_EQ(ref_type(pow(d, ui % 19)).str(), test_type(pow(d1, ui % 19)).str());
        BOOST_TEST_EQ(ref_type(powm(a, b, c)).str(), test_type(powm(a1, b1, c1)).str());
        BOOST_TEST_EQ(ref_type(powm(a, b, ui)).str(), test_type(powm(a1, b1, ui)).str());
        BOOST_TEST_EQ(ref_type(powm(a, ui, c)).str(), test_type(powm(a1, ui, c1)).str());
    }

    void test()
    {
        const unsigned runs = 1000;
        unsigned next_progress_report = 10, progress;
        last_error_count = 0;

        engine.seed(std::time(0));

        BOOST_TEST_EQ(Number(), 0);

        for (unsigned r = 1; r <= runs; ++r)
        {
            progress = 100 * r / runs;
            while (progress >= next_progress_report)
            {
                std::cout << "..." << progress << "%";
                std::cout.flush();
                next_progress_report += 10;
            }

            a = kanooth::numbers::generate_random_bits_number<ref_type>(engine, 1000);
            b = kanooth::numbers::generate_random_bits_number<ref_type>(engine, 512);
            c = kanooth::numbers::generate_random_bits_number<ref_type>(engine, 256);
            d = kanooth::numbers::generate_random_bits_number<ref_type>(engine, 32);

            si = d.convert_to<int>();
            ui = si;

            a1 = static_cast<test_type>(a.str());
            b1 = static_cast<test_type>(b.str());
            c1 = static_cast<test_type>(c.str());
            d1 = static_cast<test_type>(d.str());

            t1();
            t2();
            t3();
            t4();
            t5();

            if(last_error_count != (unsigned)boost::detail::test_errors())
            {
                last_error_count = boost::detail::test_errors();
                std::cout << std::hex << std::showbase;

                std::cout << "a    = " << a << std::endl;
                std::cout << "a1   = " << a1 << std::endl;
                std::cout << "b    = " << b << std::endl;
                std::cout << "b1   = " << b1 << std::endl;
                std::cout << "c    = " << c << std::endl;
                std::cout << "c1   = " << c1 << std::endl;
                std::cout << "d    = " << d << std::endl;
                std::cout << "d1   = " << d1 << std::endl;
            }

        }
        std::cout << std::endl;
    }
};

int main()
{
    tester<kanooth::numbers::boost_integer> test_int;
    //tester<boost::multiprecision::cpp_int> test_int;
    test_int.test();

    return boost::report_errors();
}


//////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::chi_squared::is_math_distribution.hpp   //
//                                                                              //
//  (C) Copyright 2009 Erwann Rogard                                            //
//  Use, modification and distribution are subject to 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 BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_CHI_SQUARED_IS_MATH_DISTRIBUTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_CHI_SQUARED_IS_MATH_DISTRIBUTION_HPP_ER_2009
#include <boost/mpl/bool.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/statistics/detail/distribution_toolkit/meta/is_math_distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{
namespace meta{

    template<typename T,typename P>
    struct is_math_distribution< boost::math::chi_squared_distribution<T,P> >
        : mpl::bool_<true>{};

}// meta
}// distribution
}// toolkit
}// detail
}// statistics
}// boost

#endif


/*
 * discretized_function.cpp
 *
 *  Created on: 22.07.2017
 *      Author: thies
 */

#include <base/ConstantMesh.h>
#include <base/DiscretizedFunction.h>
#include <base/Norm.h>
#include <base/SpaceTimeMesh.h>
#include <base/Util.h>
#include <deal.II/base/exceptions.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/numbers.h>
#include <deal.II/base/point.h>
#include <deal.II/base/quadrature.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/timer.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria.h>
#include <gtest/gtest.h>
#include <norms/H1H1.h>
#include <norms/H1L2.h>
#include <norms/H2L2.h>
#include <norms/H2L2PlusL2H1.h>
#include <norms/L2Coefficients.h>
#include <norms/L2L2.h>
#include <stddef.h>
#include <iostream>
#include <memory>
#include <vector>

namespace {

using namespace dealii;
using namespace wavepi::base;
using namespace wavepi;

template <int dim>
class TestF : public Function<dim> {
 public:
  double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    if ((this->get_time() <= 1) && (p.norm() < 0.5))
      return std::sin(this->get_time() * 2 * numbers::PI);
    else
      return 0.0;
  }
};

template <int dim>
class TestF2 : public Function<dim> {
 public:
  TestF2() : Function<dim>() {}
  double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    if ((this->get_time() <= 0.5) && (p.distance(actor_position) < 0.4))
      return std::sin(this->get_time() * 2 * numbers::PI);
    else
      return 0.0;
  }

 private:
  static const Point<dim> actor_position;
};

template <>
const Point<1> TestF2<1>::actor_position = Point<1>(1.0);
template <>
const Point<2> TestF2<2>::actor_position = Point<2>(1.0, 0.5);
template <>
const Point<3> TestF2<3>::actor_position = Point<3>(1.0, 0.5, 0.0);

template <int dim>
class TestG : public Function<dim> {
 public:
  double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));

    Point<dim> pc = Point<dim>::unit_vector(0);
    pc *= 0.5;

    if (std::abs(this->get_time() - 1.0) < 0.5 && (p.distance(pc) < 0.5))
      return std::sin(this->get_time() * 1 * numbers::PI);
    else
      return 0.0;
  }
};

template <int dim>
class TestH : public Function<dim> {
 public:
  double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));

    return p.norm() * this->get_time();
  }
};

template <int dim>
double rho(const Point<dim> &p, double t) {
  return p.norm() + t + 1.0;
}

template <int dim>
double c_squared(const Point<dim> &p, double t) {
  double tmp = p.norm() * t + 1.0;

  return tmp * tmp;
}

template <int dim>
class TestC : public Function<dim> {
 public:
  double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));

    return 1.0 / (rho(p, this->get_time()) * c_squared(p, this->get_time()));
  }
};

template <int dim>
class TestA : public Function<dim> {
 public:
  double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));

    return 1.0 / rho(p, this->get_time());
  }
};

template <int dim>
class TestNu : public Function<dim> {
 public:
  double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));

    return std::abs(p[0]) * this->get_time();
  }
};

template <int dim>
class TestQ : public Function<dim> {
 public:
  TestQ() : Function<dim>() {}
  double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));

    return p.norm() < 0.5 ? std::sin(this->get_time() / 2 * 2 * numbers::PI) : 0.0;
  }

  static const Point<dim> q_position;
};

template <>
const Point<1> TestQ<1>::q_position = Point<1>(-1.0);
template <>
const Point<2> TestQ<2>::q_position = Point<2>(-1.0, 0.5);
template <>
const Point<3> TestQ<3>::q_position = Point<3>(-1.0, 0.5, 0.0);

// tests whether norm and dot product are consistent
template <int dim>
void run_dot_norm_test(int fe_order, int quad_order, int refines, int n_steps,
                       std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm) {
  deallog << "== " << norm->name() << " ==" << std::endl;

  auto triangulation = std::make_shared<Triangulation<dim>>();
  GridGenerator::hyper_cube(*triangulation, -1, 1);
  Util::set_all_boundary_ids(*triangulation, 0);
  triangulation->refine_global(refines);

  double t_start = 0.0, t_end = 2.0, dt = t_end / n_steps;
  std::vector<double> times;

  for (size_t i = 0; t_start + i * dt <= t_end; i++)
    times.push_back(t_start + i * dt);

  FE_Q<dim> fe(fe_order);
  Quadrature<dim> quad = QGauss<dim>(quad_order);  // exact in poly degree 2n-1 (needed: fe_dim^3)

  std::shared_ptr<SpaceTimeMesh<dim>> mesh = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);

  deallog << std::endl << "----------  n_dofs / timestep: " << mesh->get_dof_handler(0)->n_dofs();
  deallog << ", n_steps: " << times.size() << "  ----------" << std::endl;

  TestQ<dim> q_cont;
  DiscretizedFunction<dim> q(mesh, q_cont);
  q.set_norm(norm);

  double norm_q       = q.norm();
  double sqrt_dot_q_q = std::sqrt(q * q);
  double q_err        = std::abs(norm_q - sqrt_dot_q_q) / (std::abs(norm_q) + 1e-300);

  deallog << std::scientific << "‖q‖ = " << norm_q << ", √(q, q) = " << sqrt_dot_q_q << ", rel. error = " << q_err
          << std::endl;

  TestG<dim> g_cont;
  DiscretizedFunction<dim> g(mesh, g_cont);
  g.set_norm(norm);

  double norm_g       = g.norm();
  double sqrt_dot_g_g = std::sqrt(g * g);
  double g_err        = std::abs(norm_g - sqrt_dot_g_g) / (std::abs(norm_g) + 1e-300);

  deallog << std::scientific << "‖g‖ = " << norm_q << ", √(g, g) = " << sqrt_dot_g_g << ", rel. error = " << g_err
          << std::endl;

  TestF<dim> f_cont;
  DiscretizedFunction<dim> f(mesh, f_cont);
  f.set_norm(norm);

  double norm_f       = f.norm();
  double sqrt_dot_f_f = std::sqrt(f * f);
  double f_err        = std::abs(norm_f - sqrt_dot_f_f) / (std::abs(norm_f) + 1e-300);

  deallog << std::scientific << "‖f‖ = " << norm_q << ", √(f, f) = " << sqrt_dot_f_f << ", rel. error = " << f_err
          << std::endl;

  double tol = 1e-14;

  EXPECT_LT(q_err, tol);
  EXPECT_LT(f_err, tol);
  EXPECT_LT(g_err, tol);

  deallog << std::endl;
}

// tests whether mass matrix operations are inverse to each other
template <int dim>
void run_dot_transform_inverse_test(int fe_order, int quad_order, int refines, int n_steps,
                                    std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm) {
  deallog << "== " << norm->name() << " ==" << std::endl;

  auto triangulation = std::make_shared<Triangulation<dim>>();
  GridGenerator::hyper_cube(*triangulation, -1, 1);
  Util::set_all_boundary_ids(*triangulation, 0);
  triangulation->refine_global(refines);

  double t_start = 0.0, t_end = 2.0, dt = t_end / n_steps;
  std::vector<double> times;

  for (size_t i = 0; t_start + i * dt <= t_end; i++)
    times.push_back(t_start + i * dt);

  FE_Q<dim> fe(fe_order);
  Quadrature<dim> quad = QGauss<dim>(quad_order);  // exact in poly degree 2n-1 (needed: fe_dim^3)

  std::shared_ptr<SpaceTimeMesh<dim>> mesh = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);

  deallog << std::endl << "----------  n_dofs / timestep: " << mesh->get_dof_handler(0)->n_dofs();
  deallog << ", n_steps: " << times.size() << "  ----------" << std::endl;

  const double tol = 1e-2;
  const int N      = 10;

  Timer inverse_timer;
  Timer transform_timer;

  auto norm_vector = std::make_shared<norms::L2Coefficients<dim>>();
  auto norm_h1h1   = std::make_shared<norms::H1H1<dim>>(0.25, 0.25);  // for smoothing of noise

  for (int i = 0; i < N; i++) {
    DiscretizedFunction<dim> t = DiscretizedFunction<dim>::noise(mesh);

    // smooth a bit
    t.set_norm(norm_h1h1);
    t.dot_transform_inverse();
    t.mult_mass();
    t.mult_mass();

    // equip with correct norm
    t.set_norm(norm);

    {
      DiscretizedFunction<dim> x       = t;
      DiscretizedFunction<dim> trans_x = x;

      transform_timer.start();
      trans_x.dot_transform();
      transform_timer.stop();

      inverse_timer.start();
      trans_x.dot_transform_inverse();
      inverse_timer.stop();

      trans_x -= x;

      // error calculation in vector norm
      trans_x.set_norm(norm_vector);
      x.set_norm(norm_vector);
      double err_trans = trans_x.norm() / x.norm();

      deallog << std::scientific << "‖x - T^{-1} T x‖ / ‖x‖         = " << err_trans << std::endl;
      EXPECT_LT(err_trans, tol);
    }

    {
      DiscretizedFunction<dim> x       = t;
      DiscretizedFunction<dim> trans_x = x;

      inverse_timer.start();
      trans_x.dot_transform_inverse();
      inverse_timer.stop();

      transform_timer.start();
      trans_x.dot_transform();
      transform_timer.stop();

      trans_x -= x;

      // error calculation in vector norm
      trans_x.set_norm(norm_vector);
      x.set_norm(norm_vector);
      double err_trans = trans_x.norm() / x.norm();

      deallog << std::scientific << "‖x - T T^{-1} x‖ / ‖x‖         = " << err_trans << std::endl;
      EXPECT_LT(err_trans, tol);
    }

    {
      DiscretizedFunction<dim> x             = t;
      DiscretizedFunction<dim> solve_trans_x = x;

      transform_timer.start();
      solve_trans_x.dot_solve_mass_and_transform();
      transform_timer.stop();

      inverse_timer.start();
      solve_trans_x.dot_transform_inverse();
      inverse_timer.stop();

      x.solve_mass();
      solve_trans_x -= x;

      // error calculation in vector norm
      solve_trans_x.set_norm(norm_vector);
      x.set_norm(norm_vector);
      double err_solve_trans = solve_trans_x.norm() / x.norm();

      deallog << std::scientific << "‖M^{-1} x - T^{-1} [T M^{-1}] x‖ / ‖M^{-1} x‖ = " << err_solve_trans << std::endl;
      EXPECT_LT(err_solve_trans, tol);
    }

    {
      DiscretizedFunction<dim> x            = t;
      DiscretizedFunction<dim> mult_trans_x = x;

      inverse_timer.start();
      mult_trans_x.dot_mult_mass_and_transform_inverse();
      inverse_timer.stop();

      transform_timer.start();
      mult_trans_x.dot_transform();
      transform_timer.stop();

      x.mult_mass();
      mult_trans_x -= x;

      // error calculation in vector norm
      mult_trans_x.set_norm(norm_vector);
      x.set_norm(norm_vector);
      double err_mult_trans = mult_trans_x.norm() / x.norm();

      deallog << std::scientific << "‖M x - T [T^{-1} M] x‖ / ‖M x‖ = " << err_mult_trans << std::endl;
      EXPECT_LT(err_mult_trans, tol);
    }

    deallog << std::endl;
  }

  deallog << "avg time for dot_transform        : " << Util::format_duration(transform_timer.wall_time() / (3 * N))
          << std::endl;
  deallog << "avg time for dot_transform_inverse: " << Util::format_duration(inverse_timer.wall_time() / (3 * N))
          << std::endl;

  deallog << std::endl;
}

// tests whether dot_transform is consistent with dot
template <int dim>
void run_dot_transform_consistent_test(int fe_order, int quad_order, int refines, int n_steps,
                                       std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm) {
  deallog << "== " << norm->name() << " ==" << std::endl;

  auto triangulation = std::make_shared<Triangulation<dim>>();
  GridGenerator::hyper_cube(*triangulation, -1, 1);
  Util::set_all_boundary_ids(*triangulation, 0);
  triangulation->refine_global(refines);

  double t_start = 0.0, t_end = 2.0, dt = t_end / n_steps;
  std::vector<double> times;

  for (size_t i = 0; t_start + i * dt <= t_end; i++)
    times.push_back(t_start + i * dt);

  FE_Q<dim> fe(fe_order);
  Quadrature<dim> quad = QGauss<dim>(quad_order);  // exact in poly degree 2n-1 (needed: fe_dim^3)

  std::shared_ptr<SpaceTimeMesh<dim>> mesh = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);

  deallog << std::endl << "----------  n_dofs / timestep: " << mesh->get_dof_handler(0)->n_dofs();
  deallog << ", n_steps: " << times.size() << "  ----------" << std::endl;

  for (int i = 0; i < 10; i++) {
    DiscretizedFunction<dim> x = DiscretizedFunction<dim>::noise(mesh);
    x.set_norm(norm);

    DiscretizedFunction<dim> y = DiscretizedFunction<dim>::noise(mesh);
    y.set_norm(norm);

    double dot_xy = x * y;

    y.dot_transform();
    x.set_norm(std::make_shared<norms::L2Coefficients<dim>>());
    y.set_norm(std::make_shared<norms::L2Coefficients<dim>>());

    double dot_xy_via_transform = x * y;
    double err                  = std::abs(dot_xy - dot_xy_via_transform) / (std::abs(dot_xy) + 1e-300);

    deallog << std::scientific << "(x, y) = " << dot_xy << ", x^t T y = " << dot_xy_via_transform
            << ", rel. error = " << err << std::endl;

    double tol = 1e-10;
    EXPECT_LT(err, tol);
  }

  deallog << std::endl;
}

template <int dim>
void run_derivative_transpose_test(int fe_order, int quad_order, int refines, int n_steps) {
  auto triangulation = std::make_shared<Triangulation<dim>>();
  GridGenerator::hyper_cube(*triangulation, -1, 1);
  Util::set_all_boundary_ids(*triangulation, 0);
  triangulation->refine_global(refines);

  double t_start = 0.0, t_end = 2.0, dt = t_end / n_steps;
  std::vector<double> times;

  for (size_t i = 0; t_start + i * dt <= t_end; i++)
    times.push_back(t_start + i * dt);

  FE_Q<dim> fe(fe_order);
  Quadrature<dim> quad = QGauss<dim>(quad_order);  // exact in poly degree 2n-1 (needed: fe_dim^3)

  std::shared_ptr<SpaceTimeMesh<dim>> mesh = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);

  deallog << std::endl << "----------  n_dofs / timestep: " << mesh->get_dof_handler(0)->n_dofs();
  deallog << ", n_steps: " << times.size() << "  ----------" << std::endl;

  const double tol = 1e-10;

  for (int i = 0; i < 10; i++) {
    DiscretizedFunction<dim> x = DiscretizedFunction<dim>::noise(mesh);
    x.set_norm(std::make_shared<norms::L2Coefficients<dim>>());

    DiscretizedFunction<dim> y = DiscretizedFunction<dim>::noise(mesh);
    y.set_norm(std::make_shared<norms::L2Coefficients<dim>>());

    auto Dy  = y.calculate_derivative();
    auto Dtx = x.calculate_derivative_transpose();

    double dot_x_Dy  = x * Dy;
    double dot_Dtx_y = Dtx * y;
    double err       = std::abs(dot_Dtx_y - dot_x_Dy) / (std::abs(dot_x_Dy) + 1e-300);

    deallog << std::scientific << "(x, Dy) = " << dot_x_Dy << ", (D^t x, y) = " << dot_Dtx_y << ", rel. error = " << err
            << std::endl;
    EXPECT_LT(err, tol);

    auto D2y  = y.calculate_second_derivative();
    auto D2tx = x.calculate_second_derivative_transpose();

    double dot_x_D2y  = x * D2y;
    double dot_D2tx_y = D2tx * y;
    double err2       = std::abs(dot_D2tx_y - dot_x_D2y) / (std::abs(dot_x_D2y) + 1e-300);

    deallog << std::scientific << "(x, D2y) = " << dot_x_D2y << ", (D2^t x, y) = " << dot_D2tx_y
            << ", rel. error = " << err2 << std::endl;
    EXPECT_LT(err2, tol);

    deallog << std::endl;
  }

  deallog << std::endl;
}

template <int dim>
void run_dot_norm_tests(int fe_order, int quad_order, int refines, int n_steps) {
  run_dot_norm_test<dim>(fe_order, quad_order, refines, n_steps, std::make_shared<norms::L2Coefficients<dim>>());

  run_dot_norm_test<dim>(fe_order, quad_order, refines, n_steps, std::make_shared<norms::L2L2<dim>>());

  run_dot_norm_test<dim>(fe_order, quad_order, refines, n_steps, std::make_shared<norms::H1L2<dim>>(0.5));

  run_dot_norm_test<dim>(fe_order, quad_order, refines, n_steps, std::make_shared<norms::H1H1<dim>>(0.5, 0.5));

  run_dot_norm_test<dim>(fe_order, quad_order, refines, n_steps, std::make_shared<norms::H2L2<dim>>(0.5, 0.25));

  run_dot_norm_test<dim>(fe_order, quad_order, refines, n_steps,
                         std::make_shared<norms::H2L2PlusL2H1<dim>>(0.5, 0.25, 0.5));
}

template <int dim>
void run_dot_transform_inverse_tests(int fe_order, int quad_order, int refines, int n_steps) {
  run_dot_transform_inverse_test<dim>(fe_order, quad_order, refines, n_steps,
                                      std::make_shared<norms::L2Coefficients<dim>>());

  run_dot_transform_inverse_test<dim>(fe_order, quad_order, refines, n_steps, std::make_shared<norms::L2L2<dim>>());

  run_dot_transform_inverse_test<dim>(fe_order, quad_order, refines, n_steps, std::make_shared<norms::H1L2<dim>>(0.5));

  run_dot_transform_inverse_test<dim>(fe_order, quad_order, refines, n_steps,
                                      std::make_shared<norms::H1H1<dim>>(0.5, 0.5));

  run_dot_transform_inverse_test<dim>(fe_order, quad_order, refines, n_steps,
                                      std::make_shared<norms::H2L2<dim>>(0.5, 0.25));

  run_dot_transform_inverse_test<dim>(fe_order, quad_order, refines, n_steps,
                                      std::make_shared<norms::H2L2PlusL2H1<dim>>(0.5, 0.25, 0.5));
}

template <int dim>
void run_dot_transform_consistent_tests(int fe_order, int quad_order, int refines, int n_steps) {
  run_dot_transform_consistent_test<dim>(fe_order, quad_order, refines, n_steps,
                                         std::make_shared<norms::L2Coefficients<dim>>());

  run_dot_transform_consistent_test<dim>(fe_order, quad_order, refines, n_steps, std::make_shared<norms::L2L2<dim>>());

  run_dot_transform_consistent_test<dim>(fe_order, quad_order, refines, n_steps,
                                         std::make_shared<norms::H1L2<dim>>(0.5));

  run_dot_transform_consistent_test<dim>(fe_order, quad_order, refines, n_steps,
                                         std::make_shared<norms::H1H1<dim>>(0.5, 0.5));

  run_dot_transform_consistent_test<dim>(fe_order, quad_order, refines, n_steps,
                                         std::make_shared<norms::H2L2<dim>>(0.5, 0.25));

  run_dot_transform_consistent_test<dim>(fe_order, quad_order, refines, n_steps,
                                         std::make_shared<norms::H2L2<dim>>(0.5, 0.25));

  run_dot_transform_consistent_test<dim>(fe_order, quad_order, refines, n_steps,
                                         std::make_shared<norms::H2L2PlusL2H1<dim>>(0.5, 0.25, 0.5));
}

}  // namespace

TEST(DiscretizedFunction, DotNormConsistent1DFE1) {
  run_dot_norm_tests<1>(1, 3, 10, 128);
  run_dot_norm_tests<1>(1, 4, 9, 256);
}
TEST(DiscretizedFunction, DotNormConsistent1DFE2) {
  run_dot_norm_tests<1>(2, 4, 7, 128);
  run_dot_norm_tests<1>(2, 4, 7, 256);
}

TEST(DiscretizedFunction, DotNormConsistent2DFE1) {
  run_dot_norm_tests<2>(1, 3, 5, 128);
  run_dot_norm_tests<2>(1, 4, 4, 256);
}

TEST(DiscretizedFunction, DotNormConsistent2DFE2) {
  run_dot_norm_tests<2>(2, 4, 4, 128);
  run_dot_norm_tests<2>(2, 4, 4, 256);
}

TEST(DiscretizedFunction, DotNormConsistent3DFE1) {
  run_dot_norm_tests<3>(1, 3, 2, 32);
  run_dot_norm_tests<3>(1, 4, 1, 64);
}

TEST(DiscretizedFunction, DerivativeTranspose1DFE1) {
  run_derivative_transpose_test<1>(1, 3, 10, 128);
  run_derivative_transpose_test<1>(1, 4, 9, 256);
}
TEST(DiscretizedFunction, DerivativeTranspose1DFE2) {
  run_derivative_transpose_test<1>(2, 4, 7, 128);
  run_derivative_transpose_test<1>(2, 4, 7, 256);
}

TEST(DiscretizedFunction, DerivativeTranspose2DFE1) {
  run_derivative_transpose_test<2>(1, 3, 5, 128);
  run_derivative_transpose_test<2>(1, 4, 4, 256);
}

TEST(DiscretizedFunction, DerivativeTranspose2DFE2) {
  run_derivative_transpose_test<2>(2, 4, 4, 128);
  run_derivative_transpose_test<2>(2, 4, 4, 256);
}

TEST(DiscretizedFunction, DerivativeTranspose3DFE1) {
  run_derivative_transpose_test<3>(1, 3, 2, 32);
  run_derivative_transpose_test<3>(1, 4, 1, 64);
}

TEST(DiscretizedFunction, DotTransformInverse1DFE1) {
  run_dot_transform_inverse_tests<1>(1, 3, 10, 128);
  run_dot_transform_inverse_tests<1>(1, 4, 9, 256);
}

TEST(DiscretizedFunction, DotTransformInverse1DFE2) {
  run_dot_transform_inverse_tests<1>(2, 4, 7, 128);
  run_dot_transform_inverse_tests<1>(2, 4, 7, 256);
}

TEST(DiscretizedFunction, DotTransformInverse2DFE1) {
  run_dot_transform_inverse_tests<2>(1, 3, 5, 128);
  run_dot_transform_inverse_tests<2>(1, 4, 4, 256);
}

TEST(DiscretizedFunction, DotTransformInverse2DFE2) {
  run_dot_transform_inverse_tests<2>(2, 4, 4, 128);
  run_dot_transform_inverse_tests<2>(2, 4, 4, 256);
}

TEST(DiscretizedFunction, DotTransformInverse3DFE1) {
  run_dot_transform_inverse_tests<3>(1, 3, 2, 32);
  run_dot_transform_inverse_tests<3>(1, 4, 3, 64);
}

TEST(DiscretizedFunction, DotTransformConsistent1DFE1) {
  run_dot_transform_consistent_tests<1>(1, 3, 10, 128);
  run_dot_transform_consistent_tests<1>(1, 4, 9, 256);
}

TEST(DiscretizedFunction, DotTransformConsistent1DFE2) {
  run_dot_transform_consistent_tests<1>(2, 4, 7, 128);
  run_dot_transform_consistent_tests<1>(2, 4, 7, 256);
}

TEST(DiscretizedFunction, DotTransformConsistent2DFE1) {
  run_dot_transform_consistent_tests<2>(1, 3, 5, 128);
  run_dot_transform_consistent_tests<2>(1, 4, 4, 256);
}

TEST(DiscretizedFunction, DotTransformConsistent2DFE2) {
  run_dot_transform_consistent_tests<2>(2, 4, 4, 128);
  run_dot_transform_consistent_tests<2>(2, 4, 4, 256);
}

TEST(DiscretizedFunction, DotTransformConsistent3DFE1) {
  run_dot_transform_consistent_tests<3>(1, 3, 2, 32);
  run_dot_transform_consistent_tests<3>(1, 4, 3, 64);
}


#include "PCAProjection.h"

#include "datatools/table/TableDataCall.h"
#include "mmcore/param/BoolParam.h"
#include "mmcore/param/IntParam.h"

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <sstream>

using namespace megamol;
using namespace megamol::infovis;
using namespace Eigen;

PCAProjection::PCAProjection(void)
        : megamol::core::Module()
        , dataOutSlot("dataOut", "Ouput")
        , dataInSlot("dataIn", "Input")
        , reduceToNSlot("nComponents", "Number of components (dimensions) to keep")
        , scaleSlot("scale", "Set to scale each column to unit variance")
        , centerSlot("center", "Set to shift the mean centroid to the origin")
        , datahash(0)
        , dataInHash(0)
        , columnInfos() {

    this->dataInSlot.SetCompatibleCall<megamol::datatools::table::TableDataCallDescription>();
    this->MakeSlotAvailable(&this->dataInSlot);

    this->dataOutSlot.SetCallback(megamol::datatools::table::TableDataCall::ClassName(),
        megamol::datatools::table::TableDataCall::FunctionName(0), &PCAProjection::getDataCallback);
    this->dataOutSlot.SetCallback(megamol::datatools::table::TableDataCall::ClassName(),
        megamol::datatools::table::TableDataCall::FunctionName(1), &PCAProjection::getHashCallback);
    this->MakeSlotAvailable(&this->dataOutSlot);

    reduceToNSlot << new ::megamol::core::param::IntParam(2);
    this->MakeSlotAvailable(&reduceToNSlot);

    centerSlot << new ::megamol::core::param::BoolParam(true);
    this->MakeSlotAvailable(&centerSlot);

    scaleSlot << new ::megamol::core::param::BoolParam(false);
    this->MakeSlotAvailable(&scaleSlot);
}

PCAProjection::~PCAProjection(void) {
    this->Release();
}

bool PCAProjection::create(void) {
    return true;
}

void PCAProjection::release(void) {}

bool PCAProjection::getDataCallback(core::Call& c) {

    try {
        megamol::datatools::table::TableDataCall* outCall = dynamic_cast<megamol::datatools::table::TableDataCall*>(&c);
        if (outCall == NULL)
            return false;

        megamol::datatools::table::TableDataCall* inCall =
            this->dataInSlot.CallAs<megamol::datatools::table::TableDataCall>();
        if (inCall == NULL)
            return false;

        inCall->SetFrameID(outCall->GetFrameID());
        if (!(*inCall)())
            return false;

        bool finished = project(inCall);
        if (finished == false)
            return false;

        outCall->SetFrameCount(inCall->GetFrameCount());
        outCall->SetDataHash(this->datahash);

        // set outCall
        if (this->columnInfos.size() != 0) {
            outCall->Set(this->columnInfos.size(), this->data.size() / this->columnInfos.size(),
                this->columnInfos.data(), this->data.data());
        } else {
            outCall->Set(0, 0, NULL, NULL);
        }

    } catch (...) {
        megamol::core::utility::log::Log::DefaultLog.WriteError(
            _T("Failed to execute %hs::processData\n"), ClassName());
        return false;
    }

    return true;
}

bool PCAProjection::getHashCallback(core::Call& c) {
    try {
        megamol::datatools::table::TableDataCall* outCall = dynamic_cast<megamol::datatools::table::TableDataCall*>(&c);
        if (outCall == NULL)
            return false;

        megamol::datatools::table::TableDataCall* inCall =
            this->dataInSlot.CallAs<megamol::datatools::table::TableDataCall>();
        if (inCall == NULL)
            return false;

        inCall->SetFrameID(outCall->GetFrameID());
        if (!(*inCall)(1))
            return false;

        outCall->SetFrameCount(inCall->GetFrameCount());
        outCall->SetDataHash(this->datahash);
    } catch (...) {
        megamol::core::utility::log::Log::DefaultLog.WriteError(
            _T("Failed to execute %hs::getHashCallback\n"), ClassName());
        return false;
    }

    return true;
}

bool megamol::infovis::PCAProjection::project(megamol::datatools::table::TableDataCall* inCall) {

    // check if inData has changed and if Slots have changed
    if (this->dataInHash == inCall->DataHash()) {
        if (!reduceToNSlot.IsDirty() && !scaleSlot.IsDirty() && !centerSlot.IsDirty()) {
            return true; // Nothing to do
        }
    }

    auto columnCount = inCall->GetColumnsCount();
    auto column_infos = inCall->GetColumnsInfos();
    auto rowsCount = inCall->GetRowsCount();
    auto inData = inCall->GetData();

    unsigned int outputDimCount = this->reduceToNSlot.Param<core::param::IntParam>()->Value();
    bool center = this->centerSlot.Param<core::param::BoolParam>()->Value();
    bool scale = this->scaleSlot.Param<core::param::BoolParam>()->Value();

    if (outputDimCount <= 0 || outputDimCount > columnCount) {
        megamol::core::utility::log::Log::DefaultLog.WriteError(
            _T("%hs: No valid Dimension Count has been given\n"), ClassName());
        return false;
    }

    // Load data in a Matrix
    Eigen::MatrixXd inDataMat = Eigen::MatrixXd(rowsCount, columnCount);
    for (int row = 0; row < rowsCount; row++) {
        for (int col = 0; col < columnCount; col++) {
            inDataMat(row, col) = inData[row * columnCount + col];
        }
    }

    // calculate mean for each column
    Eigen::VectorXd mean_vector(columnCount);
    mean_vector = inDataMat.colwise().mean();

    // prepare data
    if (center) {
        // substract mean columnwise
        for (int col = 0; col < columnCount; col++) {
            inDataMat.col(col) -= Eigen::VectorXd::Constant(rowsCount, mean_vector(col));
        }
    }

    if (scale) {
        // scale data to unit variance by dividing by standard deviation
        Eigen::VectorXd stdDev(columnCount);
        for (int col = 0; col < columnCount; col++) {
            stdDev(col) = sqrt(inDataMat.col(col).cwiseProduct(inDataMat.col(col)).sum() / (rowsCount - 1));
            inDataMat.col(col) /= stdDev(col);
        }
    }

    // calculate CovarianceMatrix
    MatrixXd covarianceMatrix = inDataMat;

    /** if center is off: "R ggfortify" doesn't substract mean for the covariance matrix
    //substract mean for cov Matrix
    mean_vector = inDataMat.colwise().mean();
    for (int col = 0; col < columnCount; col++) {
        covarianceMatrix.col(col) -= Eigen::VectorXd::Constant(rowsCount, mean_vector(col));
    }*/

    covarianceMatrix = covarianceMatrix.transpose() * covarianceMatrix;
    covarianceMatrix = covarianceMatrix / (float)(rowsCount - 1);

    // calculate Eigenvalues and Eigenvectors
    EigenSolver<MatrixXd> eigSolver(covarianceMatrix);

    VectorXd eigVal = eigSolver.eigenvalues().real();
    MatrixXd eigVec = eigSolver.eigenvectors().real();

    // sort eigenvalues (with index): descending
    // each eigenvalue represents the variance
    typedef std::pair<float, int> eigenPair;
    std::vector<eigenPair> sorted;
    for (unsigned int i = 0; i < columnCount; ++i) {
        sorted.push_back(std::make_pair(eigVal(i), i));
    }
    std::sort(sorted.begin(), sorted.end(), [&sorted](eigenPair& a, eigenPair& b) { return a.first > b.first; });

    // create Matrix out of sorted (and selected) eigenvectors
    MatrixXd eigVecBasis = MatrixXd(columnCount, outputDimCount);
    for (unsigned int i = 0; i < outputDimCount; ++i) {
        eigVecBasis.col(i) = eigVec.col(sorted[i].second);
    }

    // calculate PCA
    MatrixXd result = inDataMat * eigVecBasis;

    //// center
    // mean_vector = result.colwise().mean();
    //// substract mean adjusted Matrix (shift to mean to zero)
    // for (int col = 0; col < result.cols(); col++) {
    //    result.col(col) -= Eigen::VectorXd::Constant(rowsCount, mean_vector(col));
    //}

    std::stringstream debug;
    debug << std::endl << result << std::endl;

    megamol::core::utility::log::Log::DefaultLog.WriteInfo(debug.str().c_str());

    // generate new columns
    this->columnInfos.clear();
    this->columnInfos.resize(outputDimCount);

    for (int indexX = 0; indexX < outputDimCount; indexX++) {
        columnInfos[indexX]
            .SetName("PC" + std::to_string(indexX))
            .SetType(megamol::datatools::table::TableDataCall::ColumnType::QUANTITATIVE)
            .SetMinimumValue(result.col(indexX).minCoeff())
            .SetMaximumValue(result.col(indexX).maxCoeff());
    }

    // Result Matrix into Output
    this->data.clear();
    this->data.reserve(rowsCount * outputDimCount);

    for (size_t row = 0; row < rowsCount; row++) {
        for (size_t col = 0; col < outputDimCount; col++)
            this->data.push_back(result(row, col));
    }

    this->dataInHash = inCall->DataHash();
    this->datahash++;
    reduceToNSlot.ResetDirty();
    scaleSlot.ResetDirty();
    centerSlot.ResetDirty();

    return true;
}


/**
* This file is part of VIRALC.
*
* Copyright (C) 2020 Thien-Minh Nguyen <thienminh.nguyen at ntu dot edu dot sg>,
* School of EEE
* Nanyang Technological Univertsity, Singapore
*
* For more information please see <https://britsknguyen.github.io>.
* or <https://github.com/britsknguyen/VIRALC>.
* If you use this code, please cite the respective publications as
* listed on the above websites.
*
* VIRALC 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.
*
* VIRALC 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 VIRALC.  If not, see <http://www.gnu.org/licenses/>.
*/

//
// Created by Thien-Minh Nguyen on 15/12/20.
//

#include <stdio.h>
#include <vector>
#include <sys/stat.h>

#define _USE_MATH_DEFINES
#include <math.h>

#include <Eigen/Dense>

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>

using namespace std;
using namespace Eigen;

typedef Eigen::Quaterniond Quaternd;

ros::Subscriber imuSub;
ros::Publisher imuPub;

Matrix4d tf_Bold_Bnew;

void imuCb(const sensor_msgs::ImuConstPtr &msg)
{
    Vector3d w(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);
    Vector3d a(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z);
    Quaternd q(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);

    static Matrix3d R_Bold_Bnew = tf_Bold_Bnew.block<3, 3>(0, 0);
    static Matrix3d R_Bnew_Bold = R_Bold_Bnew.inverse();

    w = R_Bnew_Bold*w;
    a = R_Bnew_Bold*a;
    q = q*R_Bold_Bnew;

    sensor_msgs::Imu imu;
    imu.header = msg->header;

    imu.angular_velocity.x = w(0);
    imu.angular_velocity.y = w(1);
    imu.angular_velocity.z = w(2);

    imu.linear_acceleration.x = a(0);
    imu.linear_acceleration.y = a(1);
    imu.linear_acceleration.z = a(2);

    imu.orientation.x = q.x();
    imu.orientation.y = q.y();
    imu.orientation.z = q.z();
    imu.orientation.w = q.w();

    imuPub.publish(imu);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "imuMod");
    ros::NodeHandle nh("~");

    // IMU topic name
    std::string imu_topic;
    nh.param("imu_topic", imu_topic, std::string("/imu/imu"));

    // Transform
    vector<double> tf_Bold_Bnew_;
    nh.param("tf_Bold_Bnew", tf_Bold_Bnew_, {1, 0, 0, 0,
                                             0, 1, 0, 0,
                                             0, 0, 1, 0,
                                             0, 0, 0, 1});
    tf_Bold_Bnew = Matrix<double, 4, 4, RowMajor>(&tf_Bold_Bnew_[0]);

    // Pub and Sub
    imuSub = nh.subscribe(imu_topic, 100, imuCb);
    imuPub = nh.advertise<sensor_msgs::Imu>(imu_topic + string("_mod"), 100);

    ros::MultiThreadedSpinner spinner(0);
    spinner.spin();

    ros::spin();
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file bounded_degree_mst_oracle.hpp
 * @brief
 * @author Piotr Godlewski
 * @version 1.0
 * @date 2013-06-05
 */
#ifndef PAAL_BOUNDED_DEGREE_MST_ORACLE_HPP
#define PAAL_BOUNDED_DEGREE_MST_ORACLE_HPP

#include "paal/iterative_rounding/min_cut.hpp"
#include "paal/lp/lp_base.hpp"

#include <boost/optional.hpp>
#include <boost/range/as_array.hpp>

#include <vector>

namespace paal {
namespace ir {

/**
 * @class bdmst_violation_checker
 * @brief Violations checker for the separation oracle
 *      in the bounded degree minimum spanning tree problem.
 */
class bdmst_violation_checker {
    using AuxEdge = min_cut_finder::Edge;
    using AuxVertex = min_cut_finder::Vertex;
    using AuxEdgeList = std::vector<AuxEdge>;
    using Violation = boost::optional<double>;

  public:
    using Candidate = std::pair<AuxVertex, AuxVertex>;
    using CandidateList = std::vector<Candidate>;

    /**
     * Returns an iterator range of violated constraint candidates.
     */
    template <typename Problem, typename LP>
    const CandidateList &get_violation_candidates(const Problem &problem,
                                                  const LP &lp) {
        fill_auxiliary_digraph(problem, lp);
        initialize_candidates(problem);
        return m_candidate_list;
    }

    /**
     * Checks if the given constraint candidate is violated an if it is,
     * returns the violation value and violated constraint ID.
     */
    template <typename Problem>
    Violation check_violation(Candidate candidate, const Problem &problem) {
        double violation = find_violation(candidate.first, candidate.second);
        if (problem.get_compare().g(violation, 0)) {
            return violation;
        } else {
            return Violation{};
        }
    }

    /**
     * Adds a violated constraint to the LP.
     */
    template <typename Problem, typename LP>
    void add_violated_constraint(Candidate violating_pair,
                                 const Problem &problem, LP &lp) {
        if (violating_pair != m_min_cut.get_last_cut()) {
            find_violation(violating_pair.first, violating_pair.second);
        }

        auto const &g = problem.get_graph();
        auto const &index = problem.get_index();

        lp::linear_expression expr;
        for (auto const &e : problem.get_edge_map().right) {
            auto u = get(index, source(e.second, g));
            auto v = get(index, target(e.second, g));
            if (m_min_cut.is_in_source_set(u) &&
                m_min_cut.is_in_source_set(v)) {
                expr += e.first;
            }
        }
        lp.add_row(std::move(expr) <= m_min_cut.source_set_size() - 2);
    }

  private:

    /**
     * Creates the auxiliary directed graph used for feasibility testing.
     */
    template <typename Problem, typename LP>
    void fill_auxiliary_digraph(const Problem &problem, const LP &lp) {
        auto const &g = problem.get_graph();
        auto const &index = problem.get_index();
        m_vertices_num = num_vertices(g);
        m_min_cut.init(m_vertices_num);
        m_src_to_v.resize(m_vertices_num);
        m_v_to_trg.resize(m_vertices_num);

        for (auto const &e : problem.get_edge_map().right) {
            lp::col_id col_idx = e.first;
            double col_val = lp.get_col_value(col_idx) / 2;

            if (!problem.get_compare().e(col_val, 0)) {
                auto u = get(index, source(e.second, g));
                auto v = get(index, target(e.second, g));
                m_min_cut.add_edge_to_graph(u, v, col_val, col_val);
            }
        }

        m_src = m_min_cut.add_vertex_to_graph();
        m_trg = m_min_cut.add_vertex_to_graph();

        for (auto v : boost::as_array(vertices(g))) {
            auto aux_v = get(index, v);
            m_src_to_v[aux_v] = m_min_cut
                .add_edge_to_graph(m_src, aux_v, degree_of(problem, v, lp) / 2)
                .first;
            m_v_to_trg[aux_v] =
                m_min_cut.add_edge_to_graph(aux_v, m_trg, 1).first;
        }
    }

    /**
     * Initializes the list of cut candidates.
     */
    template <typename Problem>
    void initialize_candidates(const Problem &problem) {
        auto const &g = problem.get_graph();
        auto const &index = problem.get_index();
        auto src = *(std::next(vertices(g).first, rand() % m_vertices_num));
        auto aux_src = get(index, src);
        m_candidate_list.clear();
        for (auto v : boost::as_array(vertices(g))) {
            if (v != src) {
                auto aux_v = get(index, v);
                m_candidate_list.push_back(std::make_pair(aux_src, aux_v));
                m_candidate_list.push_back(std::make_pair(aux_v, aux_src));
            }
        }
    }

    /**
     * Calculates the sum of the variables for edges incident with a given
     * vertex.
     */
    template <typename Problem, typename LP, typename Vertex>
    double degree_of(const Problem &problem, const Vertex &v, const LP &lp) {
        double res = 0;

        for (auto e : boost::as_array(out_edges(v, problem.get_graph()))) {
            auto col_id = problem.edge_to_col(e);
            if (col_id) {
                res += lp.get_col_value(*col_id);
            }
        }
        return res;
    }

    /**
     * Finds the most violated set of vertices containing \c src and not
     * containing \c trg and returns its violation value.
     * @param src vertex to be contained in the violating set
     * @param trg vertex not to be contained in the violating set
     * @return violation of the found set
     */
    double find_violation(AuxVertex src, AuxVertex trg) {
        double orig_cap = m_min_cut.get_capacity(m_src_to_v[src]);

        m_min_cut.set_capacity(m_src_to_v[src], m_vertices_num);
        // capacity of m_src_to_v[trg] does not change
        m_min_cut.set_capacity(m_v_to_trg[src], 0);
        m_min_cut.set_capacity(m_v_to_trg[trg], m_vertices_num);

        double min_cut_weight = m_min_cut.find_min_cut(m_src, m_trg);
        double violation = m_vertices_num - 1 - min_cut_weight;

        // reset the original values for the capacities
        m_min_cut.set_capacity(m_src_to_v[src], orig_cap);
        // capacity of m_src_to_v[trg] does not change
        m_min_cut.set_capacity(m_v_to_trg[src], 1);
        m_min_cut.set_capacity(m_v_to_trg[trg], 1);

        return violation;
    }

    int m_vertices_num;

    AuxVertex m_src;
    AuxVertex m_trg;

    AuxEdgeList m_src_to_v;
    AuxEdgeList m_v_to_trg;

    CandidateList m_candidate_list;

    min_cut_finder m_min_cut;
};

} //! ir
} //! paal
#endif // PAAL_BOUNDED_DEGREE_MST_ORACLE_HPP