/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file   testJacobianFactor.cpp
 *  @brief  Unit tests for Linear Factor
 *  @author Christian Potthast
 *  @author Frank Dellaert
 **/

#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>

#include <gtsam/inference/VariableSlots.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>

#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/range/adaptor/map.hpp>

using namespace std;
using namespace gtsam;
using namespace boost::assign;

namespace {
  namespace simple {
    // Terms we'll use
    const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> >
      (make_pair(5, Matrix3::Identity()))
      (make_pair(10, 2*Matrix3::Identity()))
      (make_pair(15, 3*Matrix3::Identity()));

    // RHS and sigmas
    const Vector b = Vector3(1., 2., 3.);
    const SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.5));
  }
}

/* ************************************************************************* */
TEST(JacobianFactor, constructors_and_accessors)
{
  using namespace simple;

  // Test for using different numbers of terms
  {
    // b vector only constructor
    JacobianFactor expected(
      boost::make_iterator_range(terms.begin(), terms.begin()), b);
    JacobianFactor actual(b);
    EXPECT(assert_equal(expected, actual));
    EXPECT(assert_equal(b, expected.getb()));
    EXPECT(assert_equal(b, actual.getb()));
    EXPECT(!expected.get_model());
    EXPECT(!actual.get_model());
  }
  {
    // One term constructor
    JacobianFactor expected(
      boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, noise);
    JacobianFactor actual(terms[0].first, terms[0].second, b, noise);
    EXPECT(assert_equal(expected, actual));
    LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back());
    EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1)));
    EXPECT(assert_equal(b, expected.getb()));
    EXPECT(assert_equal(b, actual.getb()));
    EXPECT(noise == expected.get_model());
    EXPECT(noise == actual.get_model());
  }
  {
    // Two term constructor
    JacobianFactor expected(
      boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, noise);
    JacobianFactor actual(terms[0].first, terms[0].second,
      terms[1].first, terms[1].second, b, noise);
    EXPECT(assert_equal(expected, actual));
    LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back());
    EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1)));
    EXPECT(assert_equal(b, expected.getb()));
    EXPECT(assert_equal(b, actual.getb()));
    EXPECT(noise == expected.get_model());
    EXPECT(noise == actual.get_model());
  }
  {
    // Three term constructor
    JacobianFactor expected(
      boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, noise);
    JacobianFactor actual(terms[0].first, terms[0].second,
      terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
    EXPECT(assert_equal(expected, actual));
    LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
    EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
    EXPECT(assert_equal(b, expected.getb()));
    EXPECT(assert_equal(b, actual.getb()));
    EXPECT(noise == expected.get_model());
    EXPECT(noise == actual.get_model());
  }
  {
    // Test three-term constructor with std::map
    JacobianFactor expected(
      boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, noise);
    map<Key,Matrix> mapTerms;
    // note order of insertion plays no role: order will be determined by keys
    mapTerms.insert(terms[2]);
    mapTerms.insert(terms[1]);
    mapTerms.insert(terms[0]);
    JacobianFactor actual(mapTerms, b, noise);
    EXPECT(assert_equal(expected, actual));
    LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
    EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
    EXPECT(assert_equal(b, expected.getb()));
    EXPECT(assert_equal(b, actual.getb()));
    EXPECT(noise == expected.get_model());
    EXPECT(noise == actual.get_model());
  }
  {
    // VerticalBlockMatrix constructor
    JacobianFactor expected(
      boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, noise);
    VerticalBlockMatrix blockMatrix(list_of(3)(3)(3)(1), 3);
    blockMatrix(0) = terms[0].second;
    blockMatrix(1) = terms[1].second;
    blockMatrix(2) = terms[2].second;
    blockMatrix(3) = b;
    JacobianFactor actual(terms | boost::adaptors::map_keys, blockMatrix, noise);
    EXPECT(assert_equal(expected, actual));
    LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
    EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
    EXPECT(assert_equal(b, expected.getb()));
    EXPECT(assert_equal(b, actual.getb()));
    EXPECT(noise == expected.get_model());
    EXPECT(noise == actual.get_model());
  }
}

/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion) {
  HessianFactor hessian(0, (Matrix(4, 4) <<
        1.57,        2.695,         -1.1,        -2.35,
       2.695,      11.3125,        -0.65,      -10.225,
        -1.1,        -0.65,            1,          0.5,
       -2.35,      -10.225,          0.5,         9.25).finished(),
      (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
      73.1725);

  JacobianFactor expected(0, (Matrix(2, 4) <<
      1.2530,   2.1508,   -0.8779,  -1.8755,
           0,   2.5858,    0.4789,  -2.3943).finished(),
      Vector2(-6.2929, -5.7941));

  EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3));
}

/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion2) {
  JacobianFactor jf(0, (Matrix(3, 3) <<
      1, 2, 3,
      0, 2, 3,
      0, 0, 3).finished(),
    Vector3(1, 2, 2));
  HessianFactor hessian(jf);
  EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
}

/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion3) {
  JacobianFactor jf(0, (Matrix(2, 4) <<
      1, 2, 3, 0,
      0, 3, 2, 1).finished(),
    Vector2(1, 2));
  HessianFactor hessian(jf);
  EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
}

/* ************************************************************************* */
namespace simple_graph {

Key keyX(10), keyY(8), keyZ(12);

double sigma1 = 0.1;
Matrix A11 = I_2x2;
Vector2 b1(2, -1);
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1));

double sigma2 = 0.5;
Matrix A21 = -2 * I_2x2;
Matrix A22 = 3 * I_2x2;
Vector2 b2(4, -5);
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2));

double sigma3 = 1.0;
Matrix A32 = -4 * I_2x2;
Matrix A33 = 5 * I_2x2;
Vector2 b3(3, -6);
JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3));

GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3));
Ordering ordering(list_of(keyX)(keyY)(keyZ));
}

/* ************************************************************************* */
TEST( JacobianFactor, construct_from_graph)
{
  using namespace simple_graph;

  Matrix A1(6,2); A1 << A11, A21, Z_2x2;
  Matrix A2(6,2); A2 << Z_2x2, A22, A32;
  Matrix A3(6,2); A3 << Matrix::Zero(4,2), A33;
  Vector b(6); b << b1, b2, b3;
  Vector sigmas(6); sigmas << sigma1, sigma1, sigma2, sigma2, sigma3, sigma3;
  JacobianFactor expected(keyX, A1, keyY, A2, keyZ, A3, b, noiseModel::Diagonal::Sigmas(sigmas));

  // The ordering here specifies the order in which the variables will appear in the combined factor
  JacobianFactor actual(factors, ordering);

  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(JacobianFactor, error)
{
  JacobianFactor factor(simple::terms, simple::b, simple::noise);

  VectorValues values;
  values.insert(5, Vector::Constant(3, 1.0));
  values.insert(10, Vector::Constant(3, 0.5));
  values.insert(15, Vector::Constant(3, 1.0/3.0));

  Vector expected_unwhitened(3); expected_unwhitened << 2.0, 1.0, 0.0;
  Vector actual_unwhitened = factor.unweighted_error(values);
  EXPECT(assert_equal(expected_unwhitened, actual_unwhitened));

  Vector expected_whitened(3); expected_whitened << 4.0, 2.0, 0.0;
  Vector actual_whitened = factor.error_vector(values);
  EXPECT(assert_equal(expected_whitened, actual_whitened));

  double expected_error = 0.5 * expected_whitened.squaredNorm();
  double actual_error = factor.error(values);
  DOUBLES_EQUAL(expected_error, actual_error, 1e-10);
}

/* ************************************************************************* */
TEST(JacobianFactor, matrices_NULL)
{
  // Make sure everything works with NULL noise model
  JacobianFactor factor(simple::terms, simple::b);

  Matrix jacobianExpected(3, 9);
  jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second;
  Vector rhsExpected = simple::b;
  Matrix augmentedJacobianExpected(3, 10);
  augmentedJacobianExpected << jacobianExpected, rhsExpected;

  Matrix augmentedHessianExpected =
    augmentedJacobianExpected.transpose() * augmentedJacobianExpected;

  // Hessian
  EXPECT(assert_equal(Matrix(augmentedHessianExpected.topLeftCorner(9,9)), factor.information()));
  EXPECT(assert_equal(augmentedHessianExpected, factor.augmentedInformation()));

  // Whitened Jacobian
  EXPECT(assert_equal(jacobianExpected, factor.jacobian().first));
  EXPECT(assert_equal(rhsExpected, factor.jacobian().second));
  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian()));

  // Unwhitened Jacobian
  EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first));
  EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted()));

  // hessianDiagonal
  VectorValues expectDiagonal;
  expectDiagonal.insert(5, Vector::Ones(3));
  expectDiagonal.insert(10, 4*Vector::Ones(3));
  expectDiagonal.insert(15, 9*Vector::Ones(3));
  EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal()));

  // hessianBlockDiagonal
  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
  LONGS_EQUAL(3,actualBD.size());
  EXPECT(assert_equal(1*I_3x3,actualBD[5]));
  EXPECT(assert_equal(4*I_3x3,actualBD[10]));
  EXPECT(assert_equal(9*I_3x3,actualBD[15]));
}

/* ************************************************************************* */
TEST(JacobianFactor, matrices)
{
  // And now witgh a non-unit noise model
  JacobianFactor factor(simple::terms, simple::b, simple::noise);

  Matrix jacobianExpected(3, 9);
  jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second;
  Vector rhsExpected = simple::b;
  Matrix augmentedJacobianExpected(3, 10);
  augmentedJacobianExpected << jacobianExpected, rhsExpected;

  Matrix augmentedHessianExpected =
    augmentedJacobianExpected.transpose() * simple::noise->R().transpose()
    * simple::noise->R() * augmentedJacobianExpected;

  // Hessian
  EXPECT(assert_equal(Matrix(augmentedHessianExpected.topLeftCorner(9,9)), factor.information()));
  EXPECT(assert_equal(augmentedHessianExpected, factor.augmentedInformation()));

  // Whitened Jacobian
  EXPECT(assert_equal(simple::noise->R() * jacobianExpected, factor.jacobian().first));
  EXPECT(assert_equal(simple::noise->R() * rhsExpected, factor.jacobian().second));
  EXPECT(assert_equal(simple::noise->R() * augmentedJacobianExpected, factor.augmentedJacobian()));

  // Unwhitened Jacobian
  EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first));
  EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted()));

  // hessianDiagonal
  VectorValues expectDiagonal;
  // below we divide by the variance 0.5^2
  expectDiagonal.insert(5, Vector3(1, 1, 1)/0.25);
  expectDiagonal.insert(10, Vector3(4, 4, 4)/0.25);
  expectDiagonal.insert(15, Vector3(9, 9, 9)/0.25);
  EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal()));

  // hessianBlockDiagonal
  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
  LONGS_EQUAL(3,actualBD.size());
  EXPECT(assert_equal(4*I_3x3,actualBD[5]));
  EXPECT(assert_equal(16*I_3x3,actualBD[10]));
  EXPECT(assert_equal(36*I_3x3,actualBD[15]));
}

/* ************************************************************************* */
TEST(JacobianFactor, operators )
{
  const double sigma = 0.1;
  SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2, sigma);

  Matrix I = I_2x2;
  Vector b = Vector2(0.2,-0.1);
  JacobianFactor lf(1, -I, 2, I, b, sigma0_1);

  VectorValues x;
  Vector2 x1(10,20), x2(30,60);
  x.insert(1, x1);
  x.insert(2, x2);

  // test A*x
  Vector expectedE = (x2 - x1)/sigma;
  Vector actualE = lf * x;
  EXPECT(assert_equal(expectedE, actualE));

  // test A^e
  VectorValues expectedX;
  const double alpha = 0.5;
  expectedX.insert(1, - alpha * expectedE /sigma);
  expectedX.insert(2,   alpha * expectedE /sigma);
  VectorValues actualX = VectorValues::Zero(expectedX);
  lf.transposeMultiplyAdd(alpha, actualE, actualX);
  EXPECT(assert_equal(expectedX, actualX));

  // test gradient at zero
  Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian();
  VectorValues expectedG;
  expectedG.insert(1, Vector2(20,-10));
  expectedG.insert(2, Vector2(-20, 10));
  KeyVector keys {1, 2};
  EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys)));
  VectorValues actualG = lf.gradientAtZero();
  EXPECT(assert_equal(expectedG, actualG));
}

/* ************************************************************************* */
TEST(JacobianFactor, default_error )
{
  JacobianFactor f;
  double actual = f.error(VectorValues());
  DOUBLES_EQUAL(0.0, actual, 1e-15);
}

//* ************************************************************************* */
TEST(JacobianFactor, empty )
{
  // create an empty factor
  JacobianFactor f;
  EXPECT(f.empty());
}

/* ************************************************************************* */
TEST(JacobianFactor, eliminate)
{
  Matrix A01 = (Matrix(3, 3) <<
    1.0, 0.0, 0.0,
    0.0, 1.0, 0.0,
    0.0, 0.0, 1.0).finished();
  Vector3 b0(1.5, 1.5, 1.5);
  Vector3 s0(1.6, 1.6, 1.6);

  Matrix A10 = (Matrix(3, 3) <<
    2.0, 0.0, 0.0,
    0.0, 2.0, 0.0,
    0.0, 0.0, 2.0).finished();
  Matrix A11 = (Matrix(3, 3) <<
    -2.0, 0.0, 0.0,
    0.0, -2.0, 0.0,
    0.0, 0.0, -2.0).finished();
  Vector3 b1(2.5, 2.5, 2.5);
  Vector3 s1(2.6, 2.6, 2.6);

  Matrix A21 = (Matrix(3, 3) <<
    3.0, 0.0, 0.0,
    0.0, 3.0, 0.0,
    0.0, 0.0, 3.0).finished();
  Vector3 b2(3.5, 3.5, 3.5);
  Vector3 s2(3.6, 3.6, 3.6);

  GaussianFactorGraph gfg;
  gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
  gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
  gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));

  Matrix zero3x3 = Matrix::Zero(3,3);
  Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
  Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
  Vector9 b; b << b1, b0, b2;
  Vector9 sigmas; sigmas << s1, s0, s2;

  JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
  GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(list_of(0));
  JacobianFactor::shared_ptr expectedJacobian = boost::dynamic_pointer_cast<
    JacobianFactor>(expected.second);

  GaussianFactorGraph::EliminationResult actual = EliminateQR(gfg, list_of(0));
  JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast<
    JacobianFactor>(actual.second);

  EXPECT(assert_equal(*expected.first, *actual.first));
  EXPECT(assert_equal(*expectedJacobian, *actualJacobian));
}

/* ************************************************************************* */
TEST(JacobianFactor, eliminate2 )
{
  // sigmas
  double sigma1 = 0.2;
  double sigma2 = 0.1;
  Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2).finished();

  // the combined linear factor
  Matrix Ax2 = (Matrix(4, 2) <<
    // x2
    -1., 0.,
    +0.,-1.,
    1., 0.,
    +0.,1.
    ).finished();

  Matrix Al1x1 = (Matrix(4, 4) <<
    // l1   x1
    1., 0., 0.00,  0., // f4
    0., 1., 0.00,  0., // f4
    0., 0., -1.,  0., // f2
    0., 0., 0.00,-1.  // f2
    ).finished();

  // the RHS
  Vector b2(4);
  b2(0) = -0.2;
  b2(1) =  0.3;
  b2(2) =  0.2;
  b2(3) = -0.1;

  vector<pair<Key, Matrix> > meas;
  meas.push_back(make_pair(2, Ax2));
  meas.push_back(make_pair(11, Al1x1));
  JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));

  // eliminate the combined factor
  pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr>
    actual = combined.eliminate(Ordering(list_of(2)));

  // create expected Conditional Gaussian
  double oldSigma = 0.0894427; // from when R was made unit
  Matrix R11 = (Matrix(2, 2) <<
    1.00,  0.00,
    0.00,  1.00
    ).finished()/oldSigma;
  Matrix S12 = (Matrix(2, 4) <<
    -0.20, 0.00,-0.80, 0.00,
    +0.00,-0.20,+0.00,-0.80
    ).finished()/oldSigma;
  Vector d = Vector2(0.2,-0.14)/oldSigma;
  GaussianConditional expectedCG(2, d, R11, 11, S12, noiseModel::Unit::Create(2));

  EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));

  // the expected linear factor
  double sigma = 0.2236;
  Matrix Bl1x1 = (Matrix(2, 4) <<
    // l1          x1
    1.00, 0.00, -1.00,  0.00,
    0.00, 1.00, +0.00, -1.00
    ).finished()/sigma;
  Vector b1 = Vector2(0.0, 0.894427);
  JacobianFactor expectedLF(11, Bl1x1, b1, noiseModel::Unit::Create(2));
  EXPECT(assert_equal(expectedLF, *actual.second,1e-3));
}

/* ************************************************************************* */
TEST(JacobianFactor, EliminateQR)
{
  // Augmented Ab test case for whole factor graph
  Matrix Ab = (Matrix(14, 11) <<
    4.,     0.,     1.,     4.,     1.,     0.,     3.,     6.,     8.,     8.,     1.,
    9.,     2.,     0.,     1.,     6.,     3.,     9.,     6.,     6.,     9.,     4.,
    5.,     3.,     7.,     9.,     5.,     5.,     9.,     1.,     3.,     7.,     0.,
    5.,     6.,     5.,     7.,     9.,     4.,     0.,     1.,     1.,     3.,     5.,
    0.,     0.,     4.,     5.,     6.,     6.,     7.,     9.,     4.,     5.,     4.,
    0.,     0.,     9.,     4.,     8.,     6.,     2.,     1.,     4.,     1.,     6.,
    0.,     0.,     6.,     0.,     4.,     2.,     4.,     0.,     1.,     9.,     6.,
    0.,     0.,     6.,     6.,     4.,     4.,     5.,     5.,     5.,     8.,     6.,
    0.,     0.,     0.,     0.,     8.,     0.,     9.,     8.,     2.,     8.,     0.,
    0.,     0.,     0.,     0.,     0.,     9.,     4.,     6.,     3.,     2.,     0.,
    0.,     0.,     0.,     0.,     1.,     1.,     9.,     1.,     5.,     5.,     3.,
    0.,     0.,     0.,     0.,     1.,     1.,     3.,     3.,     2.,     0.,     5.,
    0.,     0.,     0.,     0.,     0.,     0.,     0.,     0.,     2.,     4.,     6.,
    0.,     0.,     0.,     0.,     0.,     0.,     0.,     0.,     6.,     3.,     4.).finished();

  // Create factor graph
  const SharedDiagonal sig_4D = noiseModel::Isotropic::Sigma(4, 0.5);
  const SharedDiagonal sig_2D = noiseModel::Isotropic::Sigma(2, 0.5);
  GaussianFactorGraph factors = list_of
    (JacobianFactor(list_of(3)(5)(7)(9)(11), VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), Ab.block(0, 0, 4, 11)), sig_4D))
    (JacobianFactor(list_of(5)(7)(9)(11), VerticalBlockMatrix(list_of(2)(2)(2)(2)(1), Ab.block(4, 2, 4, 9)), sig_4D))
    (JacobianFactor(list_of(7)(9)(11), VerticalBlockMatrix(list_of(2)(2)(2)(1), Ab.block(8, 4, 4, 7)), sig_4D))
    (JacobianFactor(list_of(11), VerticalBlockMatrix(list_of(2)(1), Ab.block(12, 8, 2, 3)), sig_2D));

  // extract the dense matrix for the graph
  Matrix actualDense = factors.augmentedJacobian();
  EXPECT(assert_equal(2.0 * Ab, actualDense));

  // Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows)
  Matrix R = 2.0 * (Matrix(10, 11) <<
    -12.1244,  -5.1962,  -5.2786,  -8.6603, -10.5573,  -5.9385, -11.3820,  -7.2581,  -8.7427, -13.4440,  -5.3611,
    0.,   4.6904,   5.0254,   5.5432,   5.5737,   3.0153,  -3.0153,  -3.5635,  -3.9290,  -2.7412,   2.1625,
    0.,       0., -13.8160,  -8.7166, -10.2245,  -8.8666,  -8.7632,  -5.2544,  -6.9192, -10.5537,  -9.3250,
    0.,       0.,       0.,   6.5033,  -1.1453,   1.3179,   2.5768,   5.5503,   3.6524,   1.3491,  -2.5676,
    0.,       0.,       0.,       0.,  -9.6242,  -2.1148,  -9.3509, -10.5846,  -3.5366,  -6.8561,  -3.2277,
    0.,       0.,       0.,       0.,       0.,   9.7887,   4.3551,   5.7572,   2.7876,   0.1611,   1.1769,
    0.,       0.,       0.,       0.,       0.,       0., -11.1139,  -0.6521,  -2.1943,  -7.5529,  -0.9081,
    0.,       0.,       0.,       0.,       0.,       0.,       0.,  -4.6479,  -1.9367,  -6.5170,  -3.7685,
    0.,       0.,       0.,       0.,       0.,       0.,       0.,       0.,   8.2503,   3.3757,   6.8476,
    0.,       0.,       0.,       0.,       0.,       0.,       0.,       0.,       0.,  -5.7095,  -0.0090).finished();

  GaussianConditional expectedFragment(list_of(3)(5)(7)(9)(11), 3,
                                       VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R.topRows(6)),
                                       noiseModel::Unit::Create(6));

  // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!!
  GaussianFactorGraph::EliminationResult actual = EliminateQR(factors, list_of(3)(5)(7));
  const JacobianFactor &actualJF = dynamic_cast<const JacobianFactor&>(*actual.second);

  EXPECT(assert_equal(expectedFragment, *actual.first, 0.001));
  EXPECT_LONGS_EQUAL(size_t(2), actualJF.keys().size());
  EXPECT(assert_equal(Key(9), actualJF.keys()[0]));
  EXPECT(assert_equal(Key(11), actualJF.keys()[1]));
  EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001));
  EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001));
  EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001));
  EXPECT(assert_equal(*noiseModel::Diagonal::Sigmas(Vector4::Ones()), *actualJF.get_model(), 0.001));
}

/* ************************************************************************* */
TEST ( JacobianFactor, constraint_eliminate1 )
{
  // construct a linear constraint
  Vector v(2); v(0)=1.2; v(1)=3.4;
  JacobianFactor lc(1, I_2x2, v, noiseModel::Constrained::All(2));

  // eliminate it
  pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr>
    actual = lc.eliminate(list_of(1));

  // verify linear factor is a null ptr
  EXPECT(actual.second->empty());

  // verify conditional Gaussian
  Vector sigmas = Vector2(0.0, 0.0);
  GaussianConditional expCG(1, v, I_2x2, noiseModel::Diagonal::Sigmas(sigmas));
  EXPECT(assert_equal(expCG, *actual.first));
}

/* ************************************************************************* */
TEST ( JacobianFactor, constraint_eliminate2 )
{
  // Construct a linear constraint
  // RHS
  Vector b(2); b(0)=3.0; b(1)=4.0;

  // A1 - invertible
  Matrix2 A1;  A1 << 2,4, 2,1;

  // A2 - not invertible
  Matrix2 A2;  A2 << 2,4, 2,4;

  JacobianFactor lc(1, A1, 2, A2, b, noiseModel::Constrained::All(2));

  // eliminate x and verify results
  pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr>
    actual = lc.eliminate(list_of(1));

  // LF should be empty
  // It's tricky to create Eigen matrices that are only zero along one dimension
  Matrix m(1,2);
  Matrix Aempty = m.topRows(0);
  Vector bempty = m.block(0,0,0,1);
  JacobianFactor expectedLF(2, Aempty, bempty, noiseModel::Constrained::All(0));
  EXPECT(assert_equal(expectedLF, *actual.second));

  // verify CG
  Matrix2 R; R << 1,2, 0,1;
  Matrix2 S; S << 1,2, 0,0;
  Vector d = Vector2(3.0, 0.6666);
  Vector sigmas = Vector2(0.0, 0.0);
  GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas));
  EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));
}

/* ************************************************************************* */
TEST(JacobianFactor, OverdeterminedEliminate) {
  Matrix Ab(9, 4);
  Ab << 0, 1, 0, 0,  //
      0, 0, 1, 0,    //
      Matrix74::Ones();

  // Call Gaussian version
  Vector9 sigmas = Vector9::Ones();
  SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);

  JacobianFactor factor(0, Ab.leftCols(3), Ab.col(3), diagonal);
  GaussianFactorGraph::EliminationResult actual = factor.eliminate(list_of(0));

  Matrix expectedRd(3, 4);
  expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131,  //
      0.0, -1, 0, 0,                                                 //
      0.0, 0.0, -1, 0;
  GaussianConditional expectedConditional(0, expectedRd.col(3), expectedRd.leftCols(3),
                                          noiseModel::Unit::Create(3));
  EXPECT(assert_equal(expectedConditional, *actual.first, 1e-4));
  EXPECT(actual.second->empty());
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


#ifndef __DEFINES_H
#define __DEFINES_H

#include <posit/posit>
#include <boost/multiprecision/cpp_dec_float.hpp>

using namespace std;
using namespace sw::unum;
using boost::multiprecision::cpp_dec_float_100;

// POSIT CONFIGURATION
#define NBITS 32
#define ES 2

// For value debugging
struct Entry {
    string name;
    cpp_dec_float_100 value;
};

struct find_entry {
    string name;

    find_entry(string name) : name(name) {}

    bool operator()(const Entry &m) const {
        return m.name == name;
    }
};

template <typename T>
std::string to_string_precision(const T a_value, const int n = 20)
{
    std::ostringstream out;
    out << std::setprecision(n) << a_value;
    return out.str();
}

#endif


#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <gtest/gtest.h>

#include "tasktorrent/tasktorrent.hpp"

using namespace std;
using namespace Eigen;
using namespace ttor;

typedef array<int, 2> int2;
typedef array<int, 3> int3;
typedef SparseMatrix<double> SpMat;

int VERB = 0;
int n_threads_ = 4;
int n_ = 50;
int N_ = 10;

SpMat random_SpMat(int n, double p, int seed)
{
    default_random_engine gen;
    gen.seed(seed);
    uniform_real_distribution<double> dist(0.0, 1.0);
    vector<Triplet<double>> triplets;
    for (int i = 0; i < n; ++i)
    {
        for (int j = 0; j < n; ++j)
        {
            auto v_ij = dist(gen);
            if (v_ij < p)
            {
                triplets.push_back(Triplet<double>(i, j, v_ij));
            }
        }
    }
    SpMat A(n, n);
    A.setFromTriplets(triplets.begin(), triplets.end());
    return A;
}

PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> random_perm(int n)
{
    PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> perm(n);
    perm.setIdentity();
    std::random_device rd;
    std::mt19937 g(rd());
    std::shuffle(perm.indices().data(), perm.indices().data() + perm.indices().size(), g);
    return perm;
}

SpMat random_dag(int n, double p)
{
    SpMat A = random_SpMat(n, p, 2019).triangularView<StrictlyLower>();
    auto P = random_perm(n);
    return P.transpose() * A * P;
}

/**
 * Unit tests
 */

TEST(threadpool, tasksrun)
{
    int n_threads = n_threads_;
    int n_tasks = 150;
    vector<int> ok(n_tasks, 0);
    Threadpool_shared tp(n_threads);

    for (int k = 0; k < n_tasks; k++)
    {
        Task *t = new Task();
        t->run = [k, &ok]() { ok[k] = 1; };
        t->fulfill = []() {};
        t->name = "Task_" + to_string(k);
        tp.insert(t, k % n_threads);
    }

    tp.join();

    for (int k = 0; k < n_tasks; k++)
        ASSERT_EQ(ok[k], 1);
}

/**
 * Checks that
 * - bound tasks are running on the right thread
 * - priorities are correctly taken into account when running tasks
 */
TEST(threadpool, priorities_bound)
{
    int n_threads = n_threads_;
    int n_tasks = 50;
    vector<int> counts(n_threads, n_tasks);
    Threadpool_shared tp(n_threads, VERB, "WK_", false);

    for (int t = 0; t < n_threads; t++)
    {
        for (int k = 0; k < n_tasks; k++)
        {
            Task *task = new Task();
            task->priority = (double)k;
            task->run = [k,t,&counts]() {
                counts[t]--;
                EXPECT_EQ(counts[t], k);
            };
            task->fulfill = [](){};
            tp.insert(task, t, true);
        }
    }

    tp.start();
    tp.join();

    for (int k = 0; k < n_threads; k++) {
        EXPECT_EQ(counts[k], 0);
    }
}

/**
 * MiniApps
 */

TEST(graph, mini)
{
    int n_threads = n_threads_;

    // 0 = start immediately ; 1 == start before the FF ; 2 == start after the FF
    int start_kind[] = {0, 1, 2};
    for(auto start: start_kind) {

        Threadpool tp(n_threads, VERB, "WkTest_", (start == 0));
        Taskflow<int> mini_tf(&tp, VERB);
        std::atomic<int> ntasks_done(0);

        vector<vector<int>> out_deps = {
            vector<int>{1, 2},
            vector<int>{3},
            vector<int>{3, 5},
            vector<int>{4, 6},
            vector<int>{5},
            vector<int>{},
            vector<int>{},
            vector<int>{5}};

        vector<int> indegree = {1, 1, 1, 2, 1, 3, 1, 1};

        mini_tf.set_mapping([&](int k) {
                return (k % n_threads);
            })
            .set_indegree([&](int k) {
                return indegree[k];
            })
            .set_task([&](int k) {
                ntasks_done++;
                for (auto k_ : out_deps[k])
                    mini_tf.fulfill_promise(k_);
            })
            .set_name([](int k) {
                return "miniTask_" + to_string(k);
            })
            .set_priority([](int k) {
                return k;
            });

        if(start == 0) { } // Nothing
        if(start == 1) { tp.start(); } // Start before the FF

        mini_tf.fulfill_promise(0);
        mini_tf.fulfill_promise(7);

        if(start == 2) { tp.start(); } // Start after the FF

        tp.join();
        EXPECT_EQ(ntasks_done.load(), 8);
    }
}

TEST(reduction, concurrent)
{
    int n_threads = n_threads_;

    int n_reds = 123;
    int reds_size = 97;

    vector<atomic<bool>> is_running(n_reds);

    for (int i = 0; i < n_reds; i++)
        is_running[i].store(false);

    Threadpool_shared tp(n_threads, VERB);
    Taskflow<int2> reduction_tf(&tp, VERB);

    reduction_tf.set_mapping([&](int2 ij) {
            return ij[0] % n_threads;
        })
        .set_binding([&](int2) {
            return true;
        })
        .set_indegree([](int2) {
            return 1;
        })
        .set_task([&](int2 ij) {
            ASSERT_FALSE(is_running.at(ij[0]).load());
            is_running.at(ij[0]).store(true);
            ASSERT_TRUE(is_running.at(ij[0]).load());
            is_running.at(ij[0]).store(false);
            ASSERT_FALSE(is_running.at(ij[0]).load());
        });

    for (int i = 0; i < n_reds; i++)
        for (int j = 0; j < reds_size; j++)
            reduction_tf.fulfill_promise({i, j});

    tp.join();
}

TEST(reduction, reduc)
{
    int n_threads = n_threads_;
    int n_reds = 2;

    atomic<bool> is_running(false);
    atomic<int> sum(0); // where we do all the reductions

    Threadpool_shared tp(n_threads, VERB);
    Taskflow<int> tf(&tp, VERB); // first & last task
    Taskflow<int> tr(&tp, VERB); // all tasks in the middle

    tf.set_mapping([&](int i) {
          return i % n_threads;
      })
        .set_indegree([&](int i) {
            return (i == 0 ? 1 : n_reds);
        })
        .set_task([&](int i) {
            if (i == 0)
            {
                assert(sum.load() == 0);
                sum++;
                for (int j = 0; j < n_reds; j++)
                {
                    tr.fulfill_promise(j);
                }
            }
            else
            {
                assert(sum.load() == n_reds + 1);
                sum++;
            }
        })
        .set_name([&](int i) {
            return "TF_" + to_string(i);
        });

    tr.set_mapping([&](int) {
          return 0;
      })
        .set_binding([&](int) {
            return true;
        })
        .set_indegree([](int) {
            return 1;
        })
        .set_task([&](int) {
            assert(!is_running.load());
            is_running.store(true);
            assert(is_running.load());
            sum++;
            tf.fulfill_promise(1);
            is_running.store(false);
            assert(!is_running.load());
        })
        .set_name([&](int i) {
            return "TR_" + to_string(i);
        });

    tf.fulfill_promise(0);

    tp.join();

    EXPECT_EQ(sum, n_reds + 2);
}

/**
 * Generates a random DAG, check it runs
 */
TEST(graph, randomdag)
{
    for (auto n_threads : {1, 2, 4, 8, 16, 32})
    {
        for (auto n : {1, 3, 5, 7, 9, 15, 20, 30, 96, 200}) // # of tasks
        {
            for (auto p : {0.0, 0.001, 0.1, 0.2, 0.5}) // Fraction of edges out of the nxns are there
            {
                SpMat G = random_dag(n, p);

                // Count in_deps
                vector<int> indegree(n);
                for (int k = 0; k < G.outerSize(); ++k)
                {
                    for (SpMat::InnerIterator it(G, k); it; ++it)
                    {
                        indegree[it.row()]++;
                    }
                }

                vector<atomic<int>> tasks_ran(n);
                vector<atomic<int>> count_in_deps(n);

                for (int k = 0; k < n; k++)
                {
                    tasks_ran[k].store(0);
                    count_in_deps[k].store(indegree[k]);
                }

                Threadpool_shared tp(n_threads, VERB);
                Taskflow<int> rand_graph_tf(&tp, VERB);

                rand_graph_tf.set_mapping([n_threads](int k) {
                                 return (k % n_threads);
                             })
                    .set_indegree([&indegree](int k) {
                        return max(1, indegree[k]);
                    })
                    .set_task([&G, &rand_graph_tf, &tasks_ran, &count_in_deps](int k) {
                        EXPECT_TRUE(count_in_deps[k].load() >= 0);
                        EXPECT_EQ(tasks_ran[k], 0);
                        tasks_ran[k]++;
                        for (SpMat::InnerIterator it(G, k); it; ++it)
                        {
                            rand_graph_tf.fulfill_promise(it.row());
                            count_in_deps[it.row()]--;
                            EXPECT_TRUE(count_in_deps[it.row()] >= 0);
                        }
                    });

                for (int i = 0; (size_t)i < indegree.size(); i++)
                    if (indegree[i] == 0)
                        rand_graph_tf.fulfill_promise(i);

                tp.join();

                for (int i = 0; i < n; i++)
                {
                    EXPECT_TRUE(tasks_ran[i].load() == 1);
                    EXPECT_TRUE(count_in_deps[i].load() == 0);
                }
            }
        }
    }
}

// Matrix-matrix product
void gemm(int n_threads, int n, int N)
{
    std::default_random_engine gen;
    // std::uniform_real_distribution<double> dist(-1.0, 1.0);
    std::uniform_int_distribution<> dist(-4, 4);
    auto rnd = [&](int, int) { return dist(gen); };

    MatrixXd A = MatrixXd::NullaryExpr(N * n, N * n, rnd);
    MatrixXd B = MatrixXd::NullaryExpr(N * n, N * n, rnd);
    MatrixXd C = MatrixXd::Zero(N * n, N * n);
    timer tref = wctime();
    MatrixXd Cref = A * B;
    timer trefend = wctime();

    Threadpool_shared tp(n_threads, VERB);
    Taskflow<int3> gemm_tf(&tp, VERB);

    gemm_tf.set_mapping([n_threads, N](int3 id) {
               return ((id[0] + N * id[1]) % n_threads);
           })
        .set_indegree([](int3) {
            return 1;
        })
        .set_task([&A, &B, &C, n, N, &gemm_tf](int3 id) {
            int i = id[0], j = id[1], k = id[2];
            C.block(i * n, j * n, n, n) += A.block(i * n, k * n, n, n) * B.block(k * n, j * n, n, n);
            if (k < N - 1)
                gemm_tf.fulfill_promise({i, j, k + 1});
        });

    timer t0 = wctime();

    for (int i = 0; i < N; i++)
        for (int j = 0; j < N; j++)
            gemm_tf.fulfill_promise({i, j, 0});

    tp.join();
    timer t1 = wctime();

    double err = (Cref - C).norm() / Cref.norm();

    if (VERB > 0)
        printf("%d threads, matrix size %dx%d, %dx%d blocks\n", n_threads, n * N, n * N, n, n);
    if (VERB > 0)
        printf("TTOR gemm+dag:   %f seconds elapsed\n", elapsed(t0, t1));
    if (VERB > 0)
        printf("EIGEN plain gemm: %f seconds elapsed\n", elapsed(tref, trefend));
    if (VERB > 0)
        printf("Error |C-Cref|/|Cref| %e\n", err);

    ASSERT_EQ(err, 0.); // We use int only so roundoff errors should be 0
}

TEST(gemm, all)
{
    for (auto n_threads : {1, 2, 4, 8, 16})
    {
        for (auto n : {1, 5, 25})
        {
            for (auto N : {1, 2, 3, 4, 5, 6, 7, 8, 9, 10})
            {
                gemm(n_threads, n, N);
            }
        }
    }
}

TEST(gemm, one)
{
    int n_threads = n_threads_;
    int n = n_;
    int N = N_;
    gemm(n_threads, n, N);
}

void cholesky(int n_threads, int n, int N)
{

    std::default_random_engine gen;
    // std::uniform_real_distribution<double> dist(-1.0, 1.0);
    std::uniform_int_distribution<> dist(-1, 1);
    auto rnd = [&](int i, int j) { return i == j ? 4 : dist(gen); };

    MatrixXd A_ = MatrixXd::NullaryExpr(N * n, N * n, rnd);

    MatrixXd A = A_ * A_.transpose();
    A = A.triangularView<Lower>();
    MatrixXd Aref = A;

    Threadpool_shared tp(n_threads, VERB);
    Taskflow<int> potf_tf(&tp, VERB);
    Taskflow<int2> trsm_tf(&tp, VERB);
    Taskflow<int3> gemm_tf(&tp, VERB);

    DepsLogger dlog(1000000);
    Logger log(1000000);
    tp.set_logger(&log);

    potf_tf.set_mapping([&](int k) {
               return (k % n_threads);
           })
        .set_indegree([](int) {
            return 1;
        })
        .set_task([&](int k) {
            LLT<MatrixXd> llt;
            llt.compute(A.block(k * n, k * n, n, n).selfadjointView<Lower>());
            A.block(k * n, k * n, n, n) = llt.matrixL();
        })
        .set_fulfill([&](int k) {
            for (int i = k + 1; i < N; i++)
            {
                trsm_tf.fulfill_promise({k, i});
                dlog.add_event(make_unique<DepsEvent>(potf_tf.name(k), trsm_tf.name({k, i})));
            }
        })
        .set_name([](int k) {
            return "potf_" + to_string(k);
        })
        .set_priority([](int) {
            return 3.0;
        });

    trsm_tf.set_mapping([&](int2 ki) {
               return ((ki[0] + ki[1] * N) % n_threads);
           })
        .set_indegree([](int2 ki) {
            return (ki[0] == 0 ? 0 : 1) + 1;
        })
        .set_task([&](int2 ki) {
            int k = ki[0];
            int i = ki[1];
            auto L = A.block(k * n, k * n, n, n).triangularView<Lower>().transpose();
            A.block(i * n, k * n, n, n) = L.solve<OnTheRight>(A.block(i * n, k * n, n, n));
        })
        .set_fulfill([&](int2 ki) {
            int k = ki[0];
            int i = ki[1];
            for (int l = k + 1; l <= i; l++)
            {
                gemm_tf.fulfill_promise({k, i, l});
                dlog.add_event(make_unique<DepsEvent>(trsm_tf.name(ki), gemm_tf.name({k, i, l})));
            }
            for (int l = i + 1; l < N; l++)
            {
                gemm_tf.fulfill_promise({k, l, i});
                dlog.add_event(make_unique<DepsEvent>(trsm_tf.name(ki), gemm_tf.name({k, l, i})));
            }
        })
        .set_name([](int2 ki) {
            return "trsm_" + to_string(ki[0]) + "_" + to_string(ki[1]);
        })
        .set_priority([](int2) {
            return 2.0;
        });

    gemm_tf.set_mapping([&](int3 kij) {
               return ((kij[0] + kij[1] * N + kij[2] * N * N) % n_threads);
           })
        .set_indegree([](int3 kij) {
            int k = kij[0];
            int i = kij[1];
            int j = kij[2];
            return (k == 0 ? 0 : 1) + (i == j ? 1 : 2);
        })
        .set_task([&](int3 kij) {
            int k = kij[0];
            int i = kij[1];
            int j = kij[2];
            A.block(i * n, j * n, n, n) -= A.block(i * n, k * n, n, n) * A.block(j * n, k * n, n, n).transpose();
            if (i == j)
                A.block(i * n, j * n, n, n) = A.block(i * n, j * n, n, n).triangularView<Lower>();
            ASSERT_TRUE(k < N - 1);
        })
        .set_fulfill([&](int3 kij) {
            int k = kij[0];
            int i = kij[1];
            int j = kij[2];
            if (k + 1 == i && k + 1 == j)
            {
                potf_tf.fulfill_promise(k + 1);
                dlog.add_event(make_unique<DepsEvent>(gemm_tf.name(kij), potf_tf.name(k + 1)));
            }
            else if (k + 1 == j)
            {
                trsm_tf.fulfill_promise({k + 1, i});
                dlog.add_event(make_unique<DepsEvent>(gemm_tf.name(kij), trsm_tf.name({k + 1, i})));
            }
            else
            {
                gemm_tf.fulfill_promise({k + 1, i, j});
                dlog.add_event(make_unique<DepsEvent>(gemm_tf.name(kij), gemm_tf.name({k + 1, i, j})));
            }
        })
        .set_name([](int3 kij) {
            return "gemm_" + to_string(kij[0]) + "_" + to_string(kij[1]) + "_" + to_string(kij[2]);
        })
        .set_priority([](int3) {
            return 1.0;
        });

    timer t0 = wctime();
    potf_tf.fulfill_promise(0);

    tp.join();
    timer t1 = wctime();

    LLT<MatrixXd> llt;
    timer t2 = wctime();
    llt.compute(Aref.selfadjointView<Lower>());
    timer t3 = wctime();
    MatrixXd Lref = llt.matrixL();

    if (VERB > 0)
    {
        printf("TTOR=%e s.\n", elapsed(t0, t1));
        printf("EIGEN=%e s.\n", elapsed(t2, t3));
    }

    double err = (A - Lref).norm() / Lref.norm();
    if (VERB > 0)
        cout << "Error=" << err << endl;
    EXPECT_TRUE(err < 1e-13) << err << endl;

    std::ofstream logfile;
    logfile.open("cholesky.log");
    logfile << log;
    logfile.close();

    std::ofstream depsfile;
    depsfile.open("cholesky.dot");
    depsfile << dlog;
    depsfile.close();
}

TEST(cholesky, all)
{
    for (auto n_threads : {1, 2, 4, 8, 16})
    {
        for (auto n : {1, 5, 25})
        {
            for (auto N : {1, 2, 3, 4, 5, 6, 7, 8, 9, 10})
            {
                cholesky(n_threads, n, N);
            }
        }
    }
}

TEST(cholesky, many)
{
    cholesky(8, 1, 8);
    cholesky(16, 1, 8);
    cholesky(32, 1, 8);

    // cholesky(100, 5, 5);
    // cholesky(1000, 5, 5);
    // cholesky(1000, 3, 30);
}

TEST(cholesky, one)
{
    int n = n_;
    int N = N_;
    int n_threads = n_threads_;
    cholesky(n_threads, n, N);
}

TEST(ttor, mapreduce)
{

    struct Data
    {
        mutex data_lock;
        vector<int> data;
    };

    int n_threads = n_threads_;
    int N = 10000;
    int n_buckets = 250;
    int reduce_deps = N / n_buckets;
    assert(N % n_buckets == 0);

    Threadpool_shared tp(n_threads, VERB);
    Taskflow<int> mf(&tp, VERB);
    Taskflow<int> rf(&tp, VERB);
    vector<Data> reddata(n_buckets);

    vector<int> data(N);
    default_random_engine gen;
    uniform_int_distribution<> dist(0, 10);
    for (int i = 0; i < N; i++)
        data[i] = dist(gen);

    vector<int> results(n_buckets);

    mf.set_mapping([&](int k) {
          return (k % n_threads);
      })
        .set_indegree([&](int) {
            return 1;
        })
        .set_task([&](int k) {
            int bucket = k % n_buckets;
            int val = data[k];
            reddata[bucket].data_lock.lock();
            reddata[bucket].data.push_back(val);
            reddata[bucket].data_lock.unlock();
            rf.fulfill_promise(bucket);
        })
        .set_name([](int k) {
            return "Map_" + to_string(k);
        });

    rf.set_mapping([&](int k) {
          return (k % n_threads);
      })
        .set_indegree([&](int) {
            return reduce_deps;
        })
        .set_task([&](int k) {
            int result = 0;
            for (auto v : reddata[k].data)
            {
                result += v;
            }
            results[k] = result;
        })
        .set_name([](int k) {
            return "Reduce_" + to_string(k);
        });

    for (int i = 0; i < N; i++)
        mf.fulfill_promise(i);

    tp.join();

    for (int i = 0; i < n_buckets; i++)
    {
        int expected = 0;
        for (int j = 0; j < reduce_deps; j++)
        {
            expected += data[i + j * n_buckets];
        }
        ASSERT_EQ(expected, results[i]);
    }
}

int main(int argc, char **argv)
{
    ::testing::InitGoogleTest(&argc, argv);

    if (argc >= 2)
    {
        n_threads_ = atoi(argv[1]);
        cout << "Number of threads set to " << n_threads_ << "\n";
    }

    if (argc >= 3)
    {
        n_ = atoi(argv[2]);
        cout << "Block size set to " << n_ << "\n";
    }

    if (argc >= 4)
    {
        N_ = atoi(argv[3]);
        cout << "Number of blocks set to " << N_ << "\n";
    }

    if (argc >= 5)
    {
        VERB = atoi(argv[4]);
        cout << "Verbosity level set to " << VERB << "\n";
    }

    const int return_flag = RUN_ALL_TESTS();

    return return_flag;
}


/**
 * @file chrono.hpp
 * @author 然Y (inie0722@gmail.com)
 * @brief
 * @version 0.1
 * @date 2022-03-04
 *
 * @copyright Copyright (c) 2022
 *
 */
#pragma once

#include <stdint.h>

#include <chrono>
#include <string>
#include <sstream>
#include <iomanip>
#include <vector>

#include <boost/algorithm/string.hpp>
#include <boost/date_time.hpp>

namespace mio
{
    /// @brief 时间模块
    namespace chrono
    {
        /**
         * @brief 获取当前时间
         *
         * @return std::chrono::nanoseconds
         */
        inline std::chrono::nanoseconds now()
        {
            using namespace std::chrono;
            return duration_cast<nanoseconds>(system_clock::now().time_since_epoch());
        }

        /**
         * @brief 将字符串 转换为时间
         *
         * @param time 格式 2000-08-21 00:07:22.0
         * @return std::chrono::nanoseconds
         */
        inline std::chrono::nanoseconds to_time(const std::string &time)
        {
            using namespace std::chrono_literals;
            std::vector<std::string> strs;
            boost::split(strs, time, boost::is_any_of("."));

            tm tm_;
            strptime(strs[0].c_str(), "%Y-%m-%d %H:%M:%S", &tm_);
            return std::chrono::nanoseconds(std::chrono::seconds(mktime(&tm_))) + std::chrono::nanoseconds(std::stoull(strs[1]));
        }

        /**
         * @brief 计时器
         *
         */
        class stopwatch
        {
        private:
            std::chrono::nanoseconds start_ = mio::chrono::now();

        public:
            /**
             * @brief 获取 计算器构造至调用时经过的纳秒数
             *
             * @return std::chrono::nanoseconds
             */
            std::chrono::nanoseconds operator()() const
            {
                return mio::chrono::now() - this->start_;
            }
        };
    } // namespace chrono

    /**
     * @brief 将时间转换为字符串
     *
     * @tparam Rep 表示计次数的算术类型
     * @tparam Period 表示计次周期
     * @param rtime
     * @return std::string 格式 2000-08-21 00:07:22.0
     */
    template <typename Rep, typename Period>
    inline std::string to_string(const std::chrono::duration<Rep, Period> &rtime)
    {

        using namespace std::chrono;
        using namespace std::chrono_literals;

        std::chrono::nanoseconds ns = rtime % 1s;

        std::stringstream ss;
        std::chrono::time_point<std::chrono::system_clock> tt(std::chrono::duration_cast<std::chrono::milliseconds>(rtime));
        auto t = std::chrono::system_clock::to_time_t(tt);
        ss << std::put_time(std::localtime(&t), "%F %T.");

        ss << ns.count();
        return ss.str();
    }
} // namespace mio


$NetBSD: patch-tools_virtual__scanner.cpp,v 1.1 2021/05/07 11:36:29 thor Exp $

Support VTK 9, from PR 4262

--- tools/virtual_scanner.cpp.orig	2020-08-14 12:23:03.000000000 +0000
+++ tools/virtual_scanner.cpp
@@ -54,7 +54,12 @@
 #include <pcl/memory.h>  // for pcl::make_shared
 #include <pcl/point_types.h>
 #include <pcl/console/parse.h>
-#include <pcl/visualization/vtk.h>
+
+#include <vtkGeneralTransform.h>
+#include <vtkPlatonicSolidSource.h>
+#include <vtkLoopSubdivisionFilter.h>
+#include <vtkCellLocator.h>
+#include <vtkMath.h>

 #include <boost/algorithm/string.hpp>  // for boost::is_any_of, boost::split, boost::token_compress_on, boost::trim
 #include <boost/filesystem.hpp>  // for boost::filesystem::create_directories, boost::filesystem::exists, boost::filesystem::extension, boost::filesystem::path


/**
 * \file dcs/math/stats/function/rand.hpp
 *
 * \brief Generate a random number according to a given probability distribution
 *  and with a given uniform random number generator.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * 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.
 */

#ifndef DCS_MATH_STATS_FUNCTION_RAND_HPP
#define DCS_MATH_STATS_FUNCTION_RAND_HPP

#include <boost/type_traits/is_base_of.hpp>
#include <boost/utility/enable_if.hpp>
#include <dcs/math/random/base_generator.hpp>
#include <dcs/math/random/generator_traits.hpp>
#include <dcs/math/stats/distribution/distribution_traits.hpp>

namespace dcs { namespace math { namespace stats {

//template <typename DistributionT, typename ValueT>
//typename distribution_traits<DistributionT>::value_type rand(DistributionT const& dist, ::dcs::math::random::base_generator<ValueT>& rng)
//{
//	return dist.rand(rng);
//}

//template <typename DistributionT, typename UniformRandomGeneratorT>
//typename ::boost::disable_if<
//	::boost::is_base_of< ::dcs::math::random::base_generator<typename ::dcs::math::random::generator_traits<UniformRandomGeneratorT>::result_type>, UniformRandomGeneratorT >,
//	typename distribution_traits<DistributionT>::value_type
//>::type rand(DistributionT const& dist, UniformRandomGeneratorT& rng)
//{
//	return dist.rand(rng);
//}

///**
// * \brief Generate a random number according to a given probability distribution
// *  and with a given uniform random number generator.
// *
// * \author Marco Guazzone (marco.guazzone@gmail.com)
// */
//template <typename DistributionT, typename DistributionTraitsT, typename UniformRandomGeneratorT>
//typename DistributionTraitsT::value_type rand(DistributionT const& dist, UniformRandomGeneratorT& rng)
//{
//	return dist.rand(rng);
//}

/**
 * \brief Generate a random number according to a given probability distribution
 *  and with a given uniform random number generator.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename DistributionT, typename UniformRandomGeneratorT>
typename distribution_traits<DistributionT>::value_type rand(DistributionT const& dist, UniformRandomGeneratorT& rng)
{
    return dist.rand(rng);
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_FUNCTION_RAND_HPP


#ifndef SPIRIT_SKIP_HTST

#include <engine/HTST.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>
#include <engine/Hamiltonian_Heisenberg.hpp>
#include <utility/Constants.hpp>
#include <utility/Logging.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>
// #include <Eigen/Array>
#include <Eigen/Eigenvalues>
//#include <unsupported/Eigen/CXX11/Tensor>
#include <GenEigsSolver.h>  // Also includes <MatOp/DenseGenMatProd.h>
#include <GenEigsRealShiftSolver.h>

#include <fmt/format.h>
#include <fmt/ostream.h>

namespace C = Utility::Constants;

namespace Engine
{
    namespace HTST
    {
        // Note the two images should correspond to one minimum and one saddle point
        // Non-extremal images may yield incorrect Hessians and thus incorrect results
        void Calculate(Data::HTST_Info & htst_info, int n_eigenmodes_keep)
        {
            Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, "---- Prefactor calculation");

            const scalar epsilon = 1e-4;
            const scalar epsilon_force = 1e-8;

            auto& image_minimum = *htst_info.minimum->spins;
            auto& image_sp = *htst_info.saddle_point->spins;

            int nos = image_minimum.size();

            if( n_eigenmodes_keep < 0 )
                n_eigenmodes_keep = 2*nos;
            n_eigenmodes_keep = std::min(2*nos, n_eigenmodes_keep);

            vectorfield force_tmp(nos, {0,0,0});
            std::vector<std::string> block;

            // TODO
            bool is_afm = false;

            // The gradient (unprojected)
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluation of the gradient at the initial configuration...");
            vectorfield gradient_minimum(nos, {0,0,0});
            htst_info.minimum->hamiltonian->Gradient(image_minimum, gradient_minimum);

            // Check if the configuration is actually an extremum
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Checking if initial configuration is an extremum...");
            Vectormath::set_c_a(1, gradient_minimum, force_tmp);
            Manifoldmath::project_tangential(force_tmp, image_minimum);
            scalar fmax_minimum = Vectormath::max_abs_component(force_tmp);
            if( fmax_minimum > epsilon_force )
            {
                Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                    "HTST: the initial configuration is not a converged minimum, its max. force component is above the threshold ({} > {})!", fmax_minimum, epsilon_force ));
                return;
            }

            // The gradient (unprojected)
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluation of the gradient at the transition configuration...");
            vectorfield gradient_sp(nos, {0,0,0});
            htst_info.saddle_point->hamiltonian->Gradient(image_sp, gradient_sp);

            // Check if the configuration is actually an extremum
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Checking if transition configuration is an extremum...");
            Vectormath::set_c_a(1, gradient_sp, force_tmp);
            Manifoldmath::project_tangential(force_tmp, image_sp);
            scalar fmax_sp = Vectormath::max_abs_component(force_tmp);
            if( fmax_sp > epsilon_force )
            {
                Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                    "HTST: the transition configuration is not a converged saddle point, its max. force component is above the threshold ({} > {})!", fmax_sp, epsilon_force ));
                return;
            }

            ////////////////////////////////////////////////////////////////////////
            // Saddle point
            {
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Calculation for the Saddle Point");

                // Evaluation of the Hessian...
                MatrixX hessian_sp = MatrixX::Zero(3*nos,3*nos);
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluation of the Hessian...");
                htst_info.saddle_point->hamiltonian->Hessian(image_sp, hessian_sp);

                // Eigendecomposition
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Eigendecomposition...");
                MatrixX hessian_geodesic_sp_3N = MatrixX::Zero(3*nos, 3*nos);
                MatrixX hessian_geodesic_sp_2N = MatrixX::Zero(2*nos, 2*nos);
                htst_info.eigenvalues_sp  = VectorX::Zero(2*nos);
                htst_info.eigenvectors_sp = MatrixX::Zero(2*nos, 2*nos);
                Geodesic_Eigen_Decomposition(image_sp, gradient_sp, hessian_sp,
                    hessian_geodesic_sp_3N, hessian_geodesic_sp_2N, htst_info.eigenvalues_sp, htst_info.eigenvectors_sp);

                // Print some eigenvalues
                block = std::vector<std::string>{"10 lowest eigenvalues at saddle point:"};
                for( int i=0; i<10; ++i )
                    block.push_back(fmt::format("ew[{}]={:^20e}   ew[{}]={:^20e}", i, htst_info.eigenvalues_sp[i], i+2*nos-10, htst_info.eigenvalues_sp[i+2*nos-10]));
                Log.SendBlock(Utility::Log_Level::Info, Utility::Log_Sender::HTST, block, -1, -1);

                // Check if lowest eigenvalue < 0 (else it's not a SP)
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Checking if actually a saddle point...");
                if( htst_info.eigenvalues_sp[0] > -epsilon )
                {
                    Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                        "HTST: the transition configuration is not a saddle point, its lowest eigenvalue is above the threshold ({} > {})!", htst_info.eigenvalues_sp[0], -epsilon ));
                    return;
                }

                // Check if second-lowest eigenvalue < 0 (higher-order SP)
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Checking if higher order saddle point...");
                int n_negative = 0;
                for( int i=0; i < htst_info.eigenvalues_sp.size(); ++i )
                {
                    if( htst_info.eigenvalues_sp[i] < -epsilon )
                        ++n_negative;
                }
                if( n_negative > 1 )
                {
                    Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                        "HTST: the image you passed is a higher order saddle point (N={})!", n_negative ));
                    return;
                }

                // Perpendicular velocity
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Calculating perpendicular velocity at saddle point ('a' factors)...");
                // Calculation of the 'a' parameters...
                htst_info.perpendicular_velocity = VectorX::Zero(2*nos);
                MatrixX basis_sp = MatrixX::Zero(3*nos, 2*nos);
                Manifoldmath::tangent_basis_spherical(image_sp, basis_sp);
                // Manifoldmath::tangent_basis(image_sp, basis_sp);
                // Calculate_Perpendicular_Velocity_2N(image_sp, hessian_geodesic_sp_2N, basis_sp, htst_info.eigenvectors_sp, perpendicular_velocity_sp);
                Calculate_Perpendicular_Velocity(image_sp, htst_info.saddle_point->geometry->mu_s, hessian_geodesic_sp_3N, basis_sp, htst_info.eigenvectors_sp, htst_info.perpendicular_velocity);

                // Reduce the number of saved eigenmodes
                htst_info.eigenvalues_sp.conservativeResize(n_eigenmodes_keep);
                htst_info.eigenvectors_sp.conservativeResize(2*nos, n_eigenmodes_keep);
            }
            // End saddle point
            ////////////////////////////////////////////////////////////////////////

            // Checking for zero modes at the saddle point...
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Checking for zero modes at the saddle point...");
            int n_zero_modes_sp = 0;
            for( int i=0; i < htst_info.eigenvalues_sp.size(); ++i )
            {
                if( std::abs(htst_info.eigenvalues_sp[i] ) <= epsilon)
                    ++n_zero_modes_sp;
            }

            // Deal with zero modes if any (calculate volume)
            htst_info.volume_sp = 1;
            if( n_zero_modes_sp > 0 )
            {
                Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, fmt::format("ZERO MODES AT SADDLE POINT (N={})", n_zero_modes_sp));

                if( is_afm )
                    htst_info.volume_sp = Calculate_Zero_Volume(htst_info.saddle_point);
                else
                    htst_info.volume_sp = Calculate_Zero_Volume(htst_info.saddle_point);
            }

            // Calculate "s"
            htst_info.s = 0;
            for( int i = n_zero_modes_sp+1; i < 2*nos; ++i )
                htst_info.s += std::pow(htst_info.perpendicular_velocity[i], 2) / htst_info.eigenvalues_sp[i];
            htst_info.s = std::sqrt(htst_info.s);

            ////////////////////////////////////////////////////////////////////////
            // Initial state minimum
            {
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Calculation for the Minimum");

                // Evaluation of the Hessian...
                MatrixX hessian_minimum = MatrixX::Zero(3*nos,3*nos);
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluation of the Hessian...");
                htst_info.minimum->hamiltonian->Hessian(image_minimum, hessian_minimum);

                // Eigendecomposition
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Eigendecomposition...");
                MatrixX hessian_geodesic_minimum_3N = MatrixX::Zero(3*nos, 3*nos);
                MatrixX hessian_geodesic_minimum_2N = MatrixX::Zero(2*nos, 2*nos);
                htst_info.eigenvalues_min  = VectorX::Zero(2*nos);
                htst_info.eigenvectors_min = MatrixX::Zero(2*nos, 2*nos);
                Geodesic_Eigen_Decomposition(image_minimum, gradient_minimum, hessian_minimum,
                    hessian_geodesic_minimum_3N, hessian_geodesic_minimum_2N, htst_info.eigenvalues_min, htst_info.eigenvectors_min);

                // Print some eigenvalues
                block = std::vector<std::string>{"10 lowest eigenvalues at minimum:"};
                for( int i=0; i<10; ++i )
                    block.push_back(fmt::format("ew[{}]={:^20e}   ew[{}]={:^20e}", i, htst_info.eigenvalues_min[i], i+2*nos-10, htst_info.eigenvalues_min[i+2*nos-10]));
                Log.SendBlock(Utility::Log_Level::Info, Utility::Log_Sender::HTST, block, -1, -1);

                // Check for eigenvalues < 0 (i.e. not a minimum)
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Checking if actually a minimum...");
                if( htst_info.eigenvalues_min[0] < -epsilon )
                {
                    Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                        "HTST: the initial configuration is not a minimum, its lowest eigenvalue is below the threshold ({} < {})!", htst_info.eigenvalues_min[0], -epsilon ));
                    return;
                }

                // Reduce the number of saved eigenmodes
                htst_info.eigenvalues_min.conservativeResize(n_eigenmodes_keep);
                htst_info.eigenvectors_min.conservativeResize(2*nos, n_eigenmodes_keep);
            }
            // End initial state minimum
            ////////////////////////////////////////////////////////////////////////

            // Checking for zero modes at the minimum...
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Checking for zero modes at the minimum...");
            int n_zero_modes_minimum = 0;
            for( int i=0; i < htst_info.eigenvalues_min.size(); ++i )
            {
                if( std::abs(htst_info.eigenvalues_min[i]) <= epsilon )
                    ++n_zero_modes_minimum;
            }

            // Deal with zero modes if any (calculate volume)
            htst_info.volume_min = 1;
            if( n_zero_modes_minimum > 0 )
            {
                Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, fmt::format("ZERO MODES AT MINIMUM (N={})", n_zero_modes_minimum));

                if( is_afm )
                    htst_info.volume_min = Calculate_Zero_Volume(htst_info.minimum);
                else
                    htst_info.volume_min = Calculate_Zero_Volume(htst_info.minimum);
            }

            ////////////////////////////////////////////////////////////////////////
            // Calculation of the prefactor...
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Calculating prefactor...");

            // Calculate the exponent for the temperature-dependence of the prefactor
            //      The exponent depends on the number of zero modes at the different states
            htst_info.temperature_exponent = 0.5 * (n_zero_modes_minimum - n_zero_modes_sp);

            // Calculate "me"
            htst_info.me = std::pow(2*C::Pi * C::k_B, htst_info.temperature_exponent);

            // Calculate Omega_0, i.e. the entropy contribution
            htst_info.Omega_0 = 1;
            if( n_zero_modes_minimum > n_zero_modes_sp+1 )
            {
                for( int i = n_zero_modes_sp+1; i<n_zero_modes_minimum; ++i )
                    htst_info.Omega_0 /= std::sqrt(htst_info.eigenvalues_sp[i]);
            }
            else if( n_zero_modes_minimum < n_zero_modes_sp+1 )
            {
                for( int i = n_zero_modes_minimum; i<(n_zero_modes_sp+1); ++i )
                    htst_info.Omega_0 *= std::sqrt(htst_info.eigenvalues_min[i]);
            }
            for( int i=std::max(n_zero_modes_minimum, n_zero_modes_sp+1); i < 2*nos; ++i )
                htst_info.Omega_0 *= std::sqrt(htst_info.eigenvalues_min[i] / htst_info.eigenvalues_sp[i]);

            // Calculate the prefactor
            htst_info.prefactor_dynamical = htst_info.me * htst_info.volume_sp / htst_info.volume_min * htst_info.s;
            htst_info.prefactor = C::g_e / (C::hbar * 1e-12) * htst_info.Omega_0 * htst_info.prefactor_dynamical / ( 2*C::Pi );

            Log.SendBlock(Utility::Log_Level::All, Utility::Log_Sender::HTST,
                {
                    "---- Prefactor calculation successful!",
                    fmt::format("exponent    = {:^20e}", htst_info.temperature_exponent),
                    fmt::format("me          = {:^20e}", htst_info.me),
                    fmt::format("m = Omega_0 = {:^20e}", htst_info.Omega_0),
                    fmt::format("s           = {:^20e}", htst_info.s),
                    fmt::format("volume_sp   = {:^20e}", htst_info.volume_sp),
                    fmt::format("volume_min  = {:^20e}", htst_info.volume_min),
                    fmt::format("hbar[meV*s] = {:^20e}", C::hbar*1e-12),
                    fmt::format("v = dynamical prefactor = {:^20e}", htst_info.prefactor_dynamical),
                    fmt::format("prefactor               = {:^20e}", htst_info.prefactor)
                }, -1, -1);
        }

        scalar Calculate_Zero_Volume(const std::shared_ptr<Data::Spin_System> system)
        {
            int   nos             = system->geometry->nos;
            auto& n_cells         = system->geometry->n_cells;
            auto& spins           = *system->spins;
            auto& spin_positions  = system->geometry->positions;
            auto& geometry        = *system->geometry;
            auto& bravais_vectors = system->geometry->bravais_vectors;

            // Dimensionality of the zero mode
            int zero_mode_dimensionality = 0;
            Vector3 zero_mode_length{0,0,0};
            for( int ibasis=0; ibasis<3; ++ibasis )
            {
                // Only a periodical direction can be a true zero mode
                if( system->hamiltonian->boundary_conditions[ibasis] && geometry.n_cells[ibasis] > 1 )
                {
                    // Vector3 shift_pos, test_pos;
                    vectorfield spins_shifted(nos, Vector3{0,0,0});

                    int shift = 0;
                    if( ibasis == 0 )
                        shift = geometry.n_cell_atoms;
                    else if( ibasis == 1 )
                        shift = geometry.n_cell_atoms * geometry.n_cells[0];
                    else if( ibasis == 2 )
                        shift = geometry.n_cell_atoms * geometry.n_cells[0] * geometry.n_cells[1];

                    for (int isite = 0; isite < nos; ++isite)
                    {
                        spins_shifted[(isite + shift) % nos] = spins[isite];
                    }

                    zero_mode_length[ibasis] = Manifoldmath::dist_geodesic(spins, spins_shifted);

                    // Increment zero mode dimensionality
                    ++zero_mode_dimensionality;
                }
            }

            // Calculate the volume depending on the number of periodical boundaries
            scalar zero_volume = 1;
            if( zero_mode_dimensionality == 1 )
            {
                zero_volume = zero_mode_length[0];
            }
            else if( zero_mode_dimensionality == 2 )
            {
                scalar area_factor = ( bravais_vectors[0].normalized().cross(bravais_vectors[1].normalized()) ).norm();
                zero_volume = zero_mode_length[0]*zero_mode_length[1] * area_factor;
            }
            else if( zero_mode_dimensionality == 3 )
            {
                scalar volume_factor = std::abs( (bravais_vectors[0].normalized().cross(bravais_vectors[1].normalized()) ).dot(
                    bravais_vectors[2].normalized()) );
                zero_volume = zero_mode_length[0]*zero_mode_length[1]*zero_mode_length[2] *  volume_factor;
            }

            Log.SendBlock(Utility::Log_Level::Info, Utility::Log_Sender::HTST,
                {
                    fmt::format("ZV zero mode dimensionality = {}", zero_mode_dimensionality),
                    fmt::format("ZV         zero mode length = {}", zero_mode_length.transpose()),
                    fmt::format("ZV = {}", zero_volume)
                }, -1, -1);

            // Return
            return zero_volume;
        }

        void Calculate_Perpendicular_Velocity(const vectorfield & spins, const scalarfield & mu_s, const MatrixX & hessian,
            const MatrixX & basis, const MatrixX & eigenbasis, VectorX & perpendicular_velocity)
        {
            int nos = spins.size();

            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "  Calculate_Perpendicular_Velocity: calculate velocity matrix");

            // Calculate the velocity matrix in the 3N-basis
            MatrixX velocity(3*nos, 3*nos);
            Calculate_Dynamical_Matrix(spins, mu_s, hessian, velocity);

            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "  Calculate_Perpendicular_Velocity: project velocity matrix");

            // Project the velocity matrix into the 2N tangent space
            MatrixX velocity_projected(2*nos, 2*nos);
            velocity_projected = basis.transpose()*velocity*basis;

            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "  Calculate_Perpendicular_Velocity: calculate a");

            // The velocity components orthogonal to the dividing surface
            perpendicular_velocity = eigenbasis.col(0).transpose() * (velocity_projected*eigenbasis);

            // std::cerr << "  Calculate_Perpendicular_Velocity: sorting" << std::endl;
            // std::sort(perpendicular_velocity.data(),perpendicular_velocity.data()+perpendicular_velocity.size());

            std::vector<std::string> block(0);
            for (int i=0; i<10; ++i)
                block.push_back(fmt::format("  a[{}] = {}", i, perpendicular_velocity[i]));
            Log.SendBlock(Utility::Log_Level::Info, Utility::Log_Sender::HTST, block, -1, -1);

            // std::cerr << "without units:" << std::endl;
            // for (int i=0; i<10; ++i)
            //     std::cerr << "  a[" << i << "] = " << perpendicular_velocity[i]/C::mu_B << std::endl;
        }

        void Calculate_Dynamical_Matrix(const vectorfield & spins, const scalarfield & mu_s, const MatrixX & hessian, MatrixX & velocity)
        {
            velocity.setZero();
            int nos = spins.size();

            for (int i=0; i < nos; ++i)
            {
                Vector3 beff{0, 0, 0};

                for (int j=0; j < nos; ++j)
                {
                    velocity(3*i, 3*j)     = spins[i][1]*hessian(3*i+2,3*j)   - spins[i][2]*hessian(3*i+1,3*j);
                    velocity(3*i, 3*j+1)   = spins[i][1]*hessian(3*i+2,3*j+1) - spins[i][2]*hessian(3*i+1,3*j+1);
                    velocity(3*i, 3*j+2)   = spins[i][1]*hessian(3*i+2,3*j+2) - spins[i][2]*hessian(3*i+1,3*j+2);

                    velocity(3*i+1, 3*j)   = spins[i][2]*hessian(3*i,3*j)     - spins[i][0]*hessian(3*i+2,3*j);
                    velocity(3*i+1, 3*j+1) = spins[i][2]*hessian(3*i,3*j+1)   - spins[i][0]*hessian(3*i+2,3*j+1);
                    velocity(3*i+1, 3*j+2) = spins[i][2]*hessian(3*i,3*j+2)   - spins[i][0]*hessian(3*i+2,3*j+2);

                    velocity(3*i+2, 3*j)   = spins[i][0]*hessian(3*i+1,3*j)   - spins[i][1]*hessian(3*i,3*j);
                    velocity(3*i+2, 3*j+1) = spins[i][0]*hessian(3*i+1,3*j+1) - spins[i][1]*hessian(3*i,3*j+1);
                    velocity(3*i+2, 3*j+2) = spins[i][0]*hessian(3*i+1,3*j+2) - spins[i][1]*hessian(3*i,3*j+2);

                    beff -= hessian.block<3, 3>(3*i, 3*j) * spins[j];
                }

                velocity(3*i,   3*i+1) -= beff[2];
                velocity(3*i,   3*i+2) += beff[1];
                velocity(3*i+1, 3*i)   += beff[2];
                velocity(3*i+1, 3*i+2) -= beff[0];
                velocity(3*i+2, 3*i)   -= beff[1];
                velocity(3*i+2, 3*i+1) += beff[0];

                velocity.row(3*i)   /= mu_s[i];
                velocity.row(3*i+1) /= mu_s[i];
                velocity.row(3*i+2) /= mu_s[i];
            }
        }

        void hessian_bordered_3N(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & hessian_out)
        {
            // Calculates a 3Nx3N matrix in the bordered Hessian approach and transforms it into the tangent basis,
            // making the result a 2Nx2N matrix. The bordered Hessian's Lagrange multipliers assume a local extremum.

            int nos = image.size();
            hessian_out = hessian;

            VectorX lambda(nos);
            for (int i=0; i<nos; ++i)
                lambda[i] = image[i].normalized().dot(gradient[i]);

            for (int i=0; i<nos; ++i)
            {
                for (int j=0; j<3; ++j)
                {
                    hessian_out(3*i+j,3*i+j) -= lambda[i];
                }
            }
        }

        // NOTE WE ASSUME A SELFADJOINT MATRIX
        void Eigen_Decomposition(const MatrixX & matrix, VectorX & evalues, MatrixX & evectors)
        {
            // Create a Spectra solver
            Eigen::SelfAdjointEigenSolver<MatrixX> matrix_solver(matrix);

            evalues = matrix_solver.eigenvalues().real();
            evectors = matrix_solver.eigenvectors().real();
        }

        void Eigen_Decomposition_Spectra(int nos, const MatrixX & matrix, VectorX & evalues, MatrixX & evectors, int n_decompose=1)
        {
            int n_steps = std::max(2, nos);

            //      Create a Spectra solver
            Spectra::DenseGenMatProd<scalar> op(matrix);
            Spectra::GenEigsSolver< scalar, Spectra::SMALLEST_REAL, Spectra::DenseGenMatProd<scalar> > matrix_spectrum(&op, n_decompose, n_steps);
            matrix_spectrum.init();

            //      Compute the specified spectrum
            int nconv = matrix_spectrum.compute();

            if (matrix_spectrum.info() == Spectra::SUCCESSFUL)
            {
                evalues = matrix_spectrum.eigenvalues().real();
                evectors = matrix_spectrum.eigenvectors().real();
                // Eigen::Ref<VectorX> evec = evectors.col(0);
            }
            else
            {
                Log(Utility::Log_Level::Error, Utility::Log_Sender::All, "Failed to calculate eigenvectors of the Matrix!");
                evalues.setZero();
                evectors.setZero();
            }
        }

        void Geodesic_Eigen_Decomposition(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian,
            MatrixX & hessian_geodesic_3N, MatrixX & hessian_geodesic_2N, VectorX & eigenvalues, MatrixX & eigenvectors)
        {
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "---------- Geodesic Eigen Decomposition");

            int nos = image.size();

            // Calculate geodesic Hessian in 3N-representation
            hessian_geodesic_3N = MatrixX::Zero(3*nos, 3*nos);
            hessian_bordered_3N(image, gradient, hessian, hessian_geodesic_3N);

            // Transform into geodesic Hessian
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Transforming Hessian into geodesic Hessian...");
            hessian_geodesic_2N = MatrixX::Zero(2*nos, 2*nos);
            // Manifoldmath::hessian_bordered(image, gradient, hessian, hessian_geodesic_2N);
            // Manifoldmath::hessian_projected(image, gradient, hessian, hessian_geodesic_2N);
            // Manifoldmath::hessian_weingarten(image, gradient, hessian, hessian_geodesic_2N);
            // Manifoldmath::hessian_spherical(image, gradient, hessian, hessian_geodesic_2N);
            // Manifoldmath::hessian_covariant(image, gradient, hessian, hessian_geodesic_2N);

            // Do this manually
            MatrixX basis = MatrixX::Zero(3*nos, 2*nos);
            Manifoldmath::tangent_basis_spherical(image, basis);
            // Manifoldmath::tangent_basis(image, basis);
            hessian_geodesic_2N = basis.transpose() * hessian_geodesic_3N * basis;

            // Calculate full eigenspectrum
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Calculation of full eigenspectrum..." );
            // std::cerr << hessian_geodesic_2N.cols() << "   " << hessian_geodesic_2N.rows() << std::endl;
            eigenvalues = VectorX::Zero(2*nos);
            eigenvectors = MatrixX::Zero(2*nos, 2*nos);
            Eigen_Decomposition(hessian_geodesic_2N, eigenvalues, eigenvectors);
            // Eigen_Decomposition_Spectra(hessian_geodesic_2N, eigenvalues, eigenvectors);

            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "---------- Geodesic Eigen Decomposition Done");
        }
    }// end namespace HTST
}// end namespace Engine

#endif


//headers for stl
#include <sstream>
//headers for gazebo
#include <gazebo/gazebo.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/Link.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/math/gzmath.hh>
//headers for sdf
#include <sdf/Param.hh>
#include <sdf/sdf.hh>
//headers fot boost
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
//headers for ROS
#include <ros/ros.h>

namespace gazebo
{
  class water_surface_friction_plugin : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr sdf)
    {
      // Store the pointer to the model
      this->model = _parent;
      this->LoadParams(sdf,"target_link",this->target_link_name);
      this->LoadParams(sdf,"mu_linear",this->mu_linear);
      this->LoadParams(sdf,"mu_angular",this->mu_angular);
      this->LoadParams(sdf,"cruising_speed",this->cruising_speed,(float)1.0);
      this->LoadParams(sdf,"cruising_speed_rot",this->cruising_speed_rot,(float)0.3);
      this->target_link = this->model->GetLink(target_link_name);
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&water_surface_friction_plugin::OnUpdate, this, _1));
    }

    // Called by the world update start event
    public: void OnUpdate(const common::UpdateInfo & /*_info*/)
    {
      math::Vector3 angular_vel = this->target_link->GetRelativeAngularVel();
      math::Vector3 linear_vel = this->target_link->GetRelativeLinearVel();
      math::Vector3 linear_friction_force,angular_friction_torque;
      if(linear_vel.x > this->cruising_speed)
      {
        linear_friction_force.x = -1*std::pow(linear_vel.x,2)*this->mu_linear;
      }
      else
      {
        linear_friction_force.x = -1*linear_vel.x*this->mu_linear;
      }
      if(linear_vel.y > this->cruising_speed)
      {
        linear_friction_force.y = -1*std::pow(linear_vel.y,2)*this->mu_linear;
      }
      else
      {
        linear_friction_force.y = -1*linear_vel.y*this->mu_linear;
      }
      if(linear_vel.z > this->cruising_speed)
      {
        linear_friction_force.z = -1*std::pow(linear_vel.z,2)*this->mu_linear;
      }
      else
      {
        linear_friction_force.z = -1*linear_vel.z*this->mu_linear;
      }

      if(angular_vel.x > this->cruising_speed)
      {
        angular_friction_torque.x = -1*std::pow(angular_vel.x,2)*this->mu_angular;
      }
      else
      {
        angular_friction_torque.x = -1*angular_vel.x*this->mu_angular;
      }
      if(angular_vel.x > this->cruising_speed)
      {
        angular_friction_torque.x = -1*std::pow(angular_vel.x,2)*this->mu_angular;
      }
      else
      {
        angular_friction_torque.z = -1*angular_vel.x*this->mu_angular;
      }
      if(angular_vel.z > this->cruising_speed)
      {
        angular_friction_torque.z = -1*std::pow(angular_vel.z,2)*this->mu_angular;
      }
      else
      {
        angular_friction_torque.z = -1*angular_vel.z*this->mu_angular;
      }
      this->target_link->AddRelativeForce(linear_friction_force);
      this->target_link->AddRelativeTorque(angular_friction_torque);
    }

    template <typename T>
    bool LoadParams(sdf::ElementPtr sdf,std::string key,T& param)
    {
      std::string param_str;
      if(!sdf->HasElement(key))
      {
        ROS_WARN_STREAM("failed to get " << key);
        return false;
      }
      else
      {
        param_str = sdf->GetElement(key)->GetValue()->GetAsString();
      }
      try
      {
        param = boost::lexical_cast<T>(param_str);
      }
      catch(boost::bad_lexical_cast &)
      {
        ROS_WARN_STREAM("failed to casting " << key);
      }
      return true;
    }

    template <typename T>
    bool LoadParams(sdf::ElementPtr sdf,std::string key,T& param, T default_param)
    {
      std::string param_str;
      if(!sdf->HasElement(key))
      {
        param = default_param;
        ROS_INFO_STREAM("unable to get " << key << " ,set default_value = " << default_param);
        return false;
      }
      else
      {
        param_str = sdf->GetElement(key)->GetValue()->GetAsString();
      }
      try
      {
        param = boost::lexical_cast<T>(param_str);
      }
      catch(boost::bad_lexical_cast &)
      {
        ROS_WARN_STREAM("failed to casting " << key);
        param = default_param;
      }
      return true;
    }

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;
    // Pointer to the model
    private: physics::ModelPtr model;
    // Pointer to the link
    private: physics::LinkPtr target_link;
    //parameters
    private: std::string target_link_name;
    private: float mu_linear,mu_angular,cruising_speed,cruising_speed_rot;
  };
  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(water_surface_friction_plugin)
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver 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 Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef ANISOTROPICLIQUID_HPP
#define ANISOTROPICLIQUID_HPP

#include <cmath>
#include <iosfwd>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>

class Element;

#include "DerivativeTypes.hpp"
#include "DerivativeUtils.hpp"
#include "bi_operators/IntegratorForward.hpp"
#include "dielectric_profile/Anisotropic.hpp"
#include "GreensFunction.hpp"

/*! \file AnisotropicLiquid.hpp
 *  \class AnisotropicLiquid
 *  \brief Green's functions for anisotropic liquid, described by a tensorial permittivity
 *  \author Roberto Di Remigio
 *  \date 2015
 *  \tparam DerivativeTraits evaluation strategy for the function and its derivatives
 *  \tparam IntegratorPolicy policy for the calculation of the matrix represenation of S and D
 */

template <typename DerivativeTraits = AD_directional,
          typename IntegratorPolicy = CollocationIntegrator>
class AnisotropicLiquid __final : public GreensFunction<DerivativeTraits, IntegratorPolicy, Anisotropic,
                                     AnisotropicLiquid<DerivativeTraits, IntegratorPolicy> >
{
public:
    /*! \param[in] eigen_eps eigenvalues of the permittivity tensors
     *  \param[in] euler_ang Euler angles in degrees
     */
    AnisotropicLiquid(const Eigen::Vector3d & eigen_eps, const Eigen::Vector3d & euler_ang) :
        GreensFunction<DerivativeTraits, IntegratorPolicy, Anisotropic,
                  AnisotropicLiquid<DerivativeTraits, IntegratorPolicy> >() { this->profile_ = Anisotropic(eigen_eps, euler_ang); }
    /*! \param[in] eigen_eps eigenvalues of the permittivity tensors
     *  \param[in] euler_ang Euler angles in degrees
     */
    AnisotropicLiquid(const Eigen::Vector3d & eigen_eps, const Eigen::Vector3d & euler_ang, double f) :
        GreensFunction<DerivativeTraits, IntegratorPolicy, Anisotropic,
                  AnisotropicLiquid<DerivativeTraits, IntegratorPolicy> >(f) { this->profile_ = Anisotropic(eigen_eps, euler_ang); }
    virtual ~AnisotropicLiquid() {}

    /*! Calculates the matrix representation of the S operator
     *  \param[in] e list of finite elements
     */
    virtual Eigen::MatrixXd singleLayer(const std::vector<Element> & e) const __override
    {
        return this->integrator_.singleLayer(*this, e);
    }
    /*! Calculates the matrix representation of the D operator
     *  \param[in] e list of finite elements
     */
    virtual Eigen::MatrixXd doubleLayer(const std::vector<Element> & e) const __override
    {
        return this->integrator_.doubleLayer(*this, e);
    }

    friend std::ostream & operator<<(std::ostream & os, AnisotropicLiquid & gf) {
        return gf.printObject(os);
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW /* See http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html */
private:
    /*!
     *  Evaluates the Green's function given a pair of points
     *  \param[in] source the source point
     *  \param[in]  probe the probe point
     */
    virtual DerivativeTraits operator()(DerivativeTraits * source, DerivativeTraits * probe) const __override
    {
        // The distance has to be calculated using epsilonInv_ as metric:
        DerivativeTraits scratch = 0.0;
        Eigen::Matrix3d epsilonInv_ = this->profile_.epsilonInv();
        double detEps_ = this->profile_.detEps();
        for (int i = 0; i < 3; ++i) {
	        for (int j = 0; j < 3; ++j) {
		       scratch += (source[i] - probe[i]) * epsilonInv_(i, j) * (source[j] - probe[j]);
	        }
        }
        DerivativeTraits distance = sqrt(scratch);

        return (1.0/(sqrt(detEps_) * distance));
    }
    /*! Returns value of the kernel for the calculation of the \f$\mathcal{D}\f$ integral operator
     *  for the pair of points p1, p2:
     *  \f$ [\boldsymbol{\varepsilon}\nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)]\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  To obtain the kernel of the \f$\mathcal{D}^\dagger\f$ operator call this methods with \f$\mathbf{p}_1\f$
     *  and \f$\mathbf{p}_2\f$ exchanged and with \f$\mathbf{n}_{\mathbf{p}_2} = \mathbf{n}_{\mathbf{p}_1}\f$
     *  \param[in] direction the direction
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double kernelD_impl(const Eigen::Vector3d & direction,
                              const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const __override
    {
        // Since the permittivity is a tensorial quantity,
        // the full gradient is needed to get the kernel of D and D^\dagger
        Eigen::Vector3d scratch = this->profile_.epsilon() * (this->gradientProbe(p1, p2));
        return scratch.dot(direction);
    }
    virtual KernelS exportKernelS_impl() const __override {
      return pcm::bind(&AnisotropicLiquid<DerivativeTraits, IntegratorPolicy>::kernelS, *this, pcm::_1, pcm::_2);
    }
    virtual KernelD exportKernelD_impl() const __override {
      return pcm::bind(&AnisotropicLiquid<DerivativeTraits, IntegratorPolicy>::kernelD, *this, pcm::_1, pcm::_2, pcm::_3);
    }
    virtual std::ostream & printObject(std::ostream & os) __override
    {
        os << "Green's function type: anisotropic liquid" << std::endl;
        os << this->profile_;
        return os;
    }
};

#endif // ANISOTROPICLIQUID_HPP