// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/LU>

template<typename MatrixType> void inverse(const MatrixType& m)
{
  /* this test covers the following files:
     Inverse.h
  */
  int rows = m.rows();
  int cols = m.cols();

  typedef typename MatrixType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> VectorType;

  MatrixType m1 = MatrixType::Random(rows, cols),
             m2(rows, cols),
             mzero = MatrixType::Zero(rows, cols),
             identity = MatrixType::Identity(rows, rows);

  while(ei_abs(m1.determinant()) < RealScalar(0.1) && rows <= 8)
  {
    m1 = MatrixType::Random(rows, cols);
  }

  m2 = m1.inverse();
  VERIFY_IS_APPROX(m1, m2.inverse() );

  m1.computeInverse(&m2);
  VERIFY_IS_APPROX(m1, m2.inverse() );

  VERIFY_IS_APPROX((Scalar(2)*m2).inverse(), m2.inverse()*Scalar(0.5));

  VERIFY_IS_APPROX(identity, m1.inverse() * m1 );
  VERIFY_IS_APPROX(identity, m1 * m1.inverse() );

  VERIFY_IS_APPROX(m1, m1.inverse().inverse() );

  // since for the general case we implement separately row-major and col-major, test that
  VERIFY_IS_APPROX(m1.transpose().inverse(), m1.inverse().transpose());
}

void test_eigen2_inverse()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( inverse(Matrix<double,1,1>()) );
    CALL_SUBTEST_2( inverse(Matrix2d()) );
    CALL_SUBTEST_3( inverse(Matrix3f()) );
    CALL_SUBTEST_4( inverse(Matrix4f()) );
    CALL_SUBTEST_5( inverse(MatrixXf(8,8)) );
    CALL_SUBTEST_6( inverse(MatrixXcd(7,7)) );
  }
}


/**
 * @file decision_tree_test.cpp
 * @author Ryan Curtin
 *
 * Tests for the DecisionTree class and related classes.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/decision_tree/decision_tree.hpp>
#include <mlpack/methods/decision_tree/information_gain.hpp>
#include <mlpack/methods/decision_tree/gini_gain.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"
#include "serialization.hpp"

using namespace mlpack;
using namespace mlpack::tree;
using namespace mlpack::distribution;

BOOST_AUTO_TEST_SUITE(DecisionTreeTest);

/**
 * Make sure the Gini gain is zero when the labels are perfect.
 */
BOOST_AUTO_TEST_CASE(GiniGainPerfectTest)
{
  arma::Row<size_t> labels;
  labels.zeros(10);

  // Test that it's perfect regardless of number of classes.
  for (size_t c = 1; c < 10; ++c)
    BOOST_REQUIRE_SMALL(GiniGain::Evaluate(labels, c), 1e-5);
}

/**
 * Make sure the Gini gain is -0.5 when the class split between two classes
 * is even.
 */
BOOST_AUTO_TEST_CASE(GiniGainEvenSplitTest)
{
  arma::Row<size_t> labels(10);
  for (size_t i = 0; i < 5; ++i)
    labels[i] = 0;
  for (size_t i = 5; i < 10; ++i)
    labels[i] = 1;

  // Test that it's -0.5 regardless of the number of classes.
  for (size_t c = 2; c < 10; ++c)
    BOOST_REQUIRE_CLOSE(GiniGain::Evaluate(labels, c), -0.5, 1e-5);
}

/**
 * The Gini gain of an empty vector is 0.
 */
BOOST_AUTO_TEST_CASE(GiniGainEmptyTest)
{
  // Test across some numbers of classes.
  arma::Row<size_t> labels;
  for (size_t c = 1; c < 10; ++c)
    BOOST_REQUIRE_SMALL(GiniGain::Evaluate(labels, c), 1e-5);
}

/**
 * The Gini gain is -(1 - 1/k) for k classes evenly split.
 */
BOOST_AUTO_TEST_CASE(GiniGainEvenSplitManyClassTest)
{
  // Try with many different classes.
  for (size_t c = 2; c < 30; ++c)
  {
    arma::Row<size_t> labels(c);
    for (size_t i = 0; i < c; ++i)
      labels[i] = i;

    // Calculate Gini gain and make sure it is correct.
    BOOST_REQUIRE_CLOSE(GiniGain::Evaluate(labels, c), -(1.0 - 1.0 / c), 1e-5);
  }
}

/**
 * The Gini gain should not be sensitive to the number of points.
 */
BOOST_AUTO_TEST_CASE(GiniGainManyPoints)
{
  for (size_t i = 1; i < 20; ++i)
  {
    const size_t numPoints = 100 * i;
    arma::Row<size_t> labels(numPoints);
    for (size_t j = 0; j < numPoints / 2; ++j)
      labels[j] = 0;
    for (size_t j = numPoints / 2; j < numPoints; ++j)
      labels[j] = 1;

    BOOST_REQUIRE_CLOSE(GiniGain::Evaluate(labels, 2), -0.5, 1e-5);
  }
}

/**
 * The information gain should be zero when the labels are perfect.
 */
BOOST_AUTO_TEST_CASE(InformationGainPerfectTest)
{
  arma::Row<size_t> labels;
  labels.zeros(10);

  // Test that it's perfect regardless of number of classes.
  for (size_t c = 1; c < 10; ++c)
    BOOST_REQUIRE_SMALL(InformationGain::Evaluate(labels, c), 1e-5);
}

/**
 * If we have an even split, the information gain should be -1.
 */
BOOST_AUTO_TEST_CASE(InformationGainEvenSplitTest)
{
  arma::Row<size_t> labels(10);
  for (size_t i = 0; i < 5; ++i)
    labels[i] = 0;
  for (size_t i = 5; i < 10; ++i)
    labels[i] = 1;

  // Test that it's -1 regardless of the number of classes.
  for (size_t c = 2; c < 10; ++c)
    BOOST_REQUIRE_CLOSE(InformationGain::Evaluate(labels, c), -1.0, 1e-5);
}

/**
 * The information gain of an empty vector is 0.
 */
BOOST_AUTO_TEST_CASE(InformationGainEmptyTest)
{
  arma::Row<size_t> labels;
  for (size_t c = 1; c < 10; ++c)
    BOOST_REQUIRE_SMALL(InformationGain::Evaluate(labels, c), 1e-5);
}

/**
 * The information gain is log2(1/k) when splitting equal classes.
 */
BOOST_AUTO_TEST_CASE(InformationGainEvenSplitManyClassTest)
{
  // Try with many different numbers of classes.
  for (size_t c = 2; c < 30; ++c)
  {
    arma::Row<size_t> labels(c);
    for (size_t i = 0; i < c; ++i)
      labels[i] = i;

    // Calculate information gain and make sure it is correct.
    BOOST_REQUIRE_CLOSE(InformationGain::Evaluate(labels, c),
        std::log2(1.0 / c), 1e-5);
  }
}

/**
 * The information gain should not be sensitive to the number of points.
 */
BOOST_AUTO_TEST_CASE(InformationGainManyPoints)
{
  for (size_t i = 1; i < 20; ++i)
  {
    const size_t numPoints = 100 * i;
    arma::Row<size_t> labels(numPoints);
    for (size_t j = 0; j < numPoints / 2; ++j)
      labels[j] = 0;
    for (size_t j = numPoints / 2; j < numPoints; ++j)
      labels[j] = 1;

    BOOST_REQUIRE_CLOSE(InformationGain::Evaluate(labels, 2), -1.0, 1e-5);
  }
}

/**
 * Check that the BestBinaryNumericSplit will split on an obviously splittable
 * dimension.
 */
BOOST_AUTO_TEST_CASE(BestBinaryNumericSplitSimpleSplitTest)
{
  arma::vec values("0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0");
  arma::Row<size_t> labels("0 0 0 0 0 1 1 1 1 1 1");

  arma::vec classProbabilities;
  BestBinaryNumericSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate(labels, 2);
  const double gain = BestBinaryNumericSplit<GiniGain>::SplitIfBetter(bestGain,
      values, labels, 2, 3, classProbabilities, aux);

  // Make sure that a split was made.
  BOOST_REQUIRE_GT(gain, bestGain);

  // The split is perfect, so we should be able to accomplish a gain of 0.
  BOOST_REQUIRE_SMALL(gain, 1e-5);

  // The class probabilities, for this split, hold the splitting point, which
  // should be between 4 and 5.
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 1);
  BOOST_REQUIRE_GT(classProbabilities[0], 0.4);
  BOOST_REQUIRE_LT(classProbabilities[0], 0.5);
}

/**
 * Check that the BestBinaryNumericSplit won't split if not enough points are
 * given.
 */
BOOST_AUTO_TEST_CASE(BestBinaryNumericSplitMinSamplesTest)
{
  arma::vec values("0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0");
  arma::Row<size_t> labels("0 0 0 0 0 1 1 1 1 1 1");

  arma::vec classProbabilities;
  BestBinaryNumericSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate(labels, 2);
  const double gain = BestBinaryNumericSplit<GiniGain>::SplitIfBetter(bestGain,
      values, labels, 2, 8, classProbabilities, aux);

  // Make sure that no split was made.
  BOOST_REQUIRE_EQUAL(gain, bestGain);
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 0);
}

/**
 * Check that the BestBinaryNumericSplit doesn't split a dimension that gives no
 * gain.
 */
BOOST_AUTO_TEST_CASE(BestBinaryNumericSplitNoGainTest)
{
  arma::vec values(100);
  arma::Row<size_t> labels(100);
  for (size_t i = 0; i < 100; i += 2)
  {
    values[i] = i;
    labels[i] = 0;
    values[i + 1] = i;
    labels[i + 1] = 1;
  }

  arma::vec classProbabilities;
  BestBinaryNumericSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate(labels, 2);
  const double gain = BestBinaryNumericSplit<GiniGain>::SplitIfBetter(bestGain,
      values, labels, 2, 10, classProbabilities, aux);

  // Make sure there was no split.
  BOOST_REQUIRE_EQUAL(gain, bestGain);
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 0);
}

/**
 * Check that the AllCategoricalSplit will split when the split is obviously
 * better.
 */
BOOST_AUTO_TEST_CASE(AllCategoricalSplitSimpleSplitTest)
{
  arma::vec values("0 0 0 1 1 1 2 2 2 3 3 3");
  arma::Row<size_t> labels("0 0 0 2 2 2 1 1 1 2 2 2");

  arma::vec classProbabilities;
  AllCategoricalSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate(labels, 3);
  const double gain = AllCategoricalSplit<GiniGain>::SplitIfBetter(bestGain,
      values, 4, labels, 3, 3, classProbabilities, aux);

  // Make sure that a split was made.
  BOOST_REQUIRE_GT(gain, bestGain);

  // Since the split is perfect, make sure the new gain is 0.
  BOOST_REQUIRE_SMALL(gain, 1e-5);

  // Make sure the class probabilities now hold the number of children.
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 1);
  BOOST_REQUIRE_EQUAL((size_t) classProbabilities[0], 4);
}

/**
 * Make sure that AllCategoricalSplit respects the minimum number of samples
 * required to split.
 */
BOOST_AUTO_TEST_CASE(AllCategoricalSplitMinSamplesTest)
{
  arma::vec values("0 0 0 1 1 1 2 2 2 3 3 3");
  arma::Row<size_t> labels("0 0 0 2 2 2 1 1 1 2 2 2");

  arma::vec classProbabilities;
  AllCategoricalSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate(labels, 3);
  const double gain = AllCategoricalSplit<GiniGain>::SplitIfBetter(bestGain,
      values, 4, labels, 3, 4, classProbabilities, aux);

  // Make sure it's not split.
  BOOST_REQUIRE_EQUAL(gain, bestGain);
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 0);
}

/**
 * Check that no split is made when it doesn't get us anything.
 */
BOOST_AUTO_TEST_CASE(AllCategoricalSplitNoGainTest)
{
  arma::vec values(300);
  arma::Row<size_t> labels(300);
  for (size_t i = 0; i < 300; i += 3)
  {
    values[i] = (i / 3) % 10;
    labels[i] = 0;
    values[i + 1] = (i / 3) % 10;
    labels[i + 1] = 1;
    values[i + 2] = (i / 3) % 10;
    labels[i + 2] = 2;
  }

  arma::vec classProbabilities;
  AllCategoricalSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate(labels, 3);
  const double gain = AllCategoricalSplit<GiniGain>::SplitIfBetter(bestGain,
      values, 10, labels, 3, 10, classProbabilities, aux);

  // Make sure that there was no split.
  BOOST_REQUIRE_EQUAL(gain, bestGain);
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 0);
}

/**
 * A basic construction of the decision tree---ensure that we can create the
 * tree and that it split at least once.
 */
BOOST_AUTO_TEST_CASE(BasicConstructionTest)
{
  arma::mat dataset(10, 1000, arma::fill::randu);
  arma::Row<size_t> labels(1000);
  for (size_t i = 0; i < 1000; ++i)
    labels[i] = i % 3; // 3 classes.

  // Use default parameters.
  DecisionTree<> d(dataset, labels, 3, 50);

  // Now require that we have some children.
  BOOST_REQUIRE_GT(d.NumChildren(), 0);
}

/**
 * Construct the decision tree on numeric data only and see that we can fit it
 * exactly and achieve perfect performance on the training set.
 */
BOOST_AUTO_TEST_CASE(PerfectTrainingSet)
{
  // Completely random dataset with no structure.
  arma::mat dataset(10, 1000, arma::fill::randu);
  arma::Row<size_t> labels(1000);
  for (size_t i = 0; i < 1000; ++i)
    labels[i] = i % 3; // 3 classes.

  DecisionTree<> d(dataset, labels, 3, 1); // Minimum leaf size of 1.

  // Make sure that we can get perfect accuracy on the training set.
  for (size_t i = 0; i < 1000; ++i)
  {
    size_t prediction;
    arma::vec probabilities;
    d.Classify(dataset.col(i), prediction, probabilities);

    BOOST_REQUIRE_EQUAL(prediction, labels[i]);
    BOOST_REQUIRE_EQUAL(probabilities.n_elem, 3);
    for (size_t j = 0; j < 3; ++j)
    {
      if (labels[i] == j)
        BOOST_REQUIRE_CLOSE(probabilities[j], 1.0, 1e-5);
      else
        BOOST_REQUIRE_SMALL(probabilities[j], 1e-5);
    }
  }
}

/**
 * Make sure class probabilities are computed correctly in the root node.
 */
BOOST_AUTO_TEST_CASE(ClassProbabilityTest)
{
  arma::mat dataset(5, 100, arma::fill::randu);
  arma::Row<size_t> labels(100);
  for (size_t i = 0; i < 100; i += 2)
  {
    labels[i] = 0;
    labels[i + 1] = 1;
  }

  // Create a decision tree that can't split.
  DecisionTree<> d(dataset, labels, 2, 1000);

  BOOST_REQUIRE_EQUAL(d.NumChildren(), 0);

  // Estimate a point's probabilities.
  arma::vec probabilities;
  size_t prediction;
  d.Classify(dataset.col(0), prediction, probabilities);

  BOOST_REQUIRE_EQUAL(probabilities.n_elem, 2);
  BOOST_REQUIRE_CLOSE(probabilities[0], 0.5, 1e-5);
  BOOST_REQUIRE_CLOSE(probabilities[1], 0.5, 1e-5);
}

/**
 * Test that the decision tree generalizes reasonably.
 */
BOOST_AUTO_TEST_CASE(SimpleGeneralizationTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");

  arma::Mat<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // Build decision tree.
  DecisionTree<> d(inputData, labels, 3, 10); // Leaf size of 10.

  // Load testing data.
  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Cannot load test dataset vc2_test.csv!");

  arma::Mat<size_t> trueTestLabels;
  if (!data::Load("vc2_test_labels.txt", trueTestLabels))
    BOOST_FAIL("Cannot load labels for vc2_test_labels.txt");

  // Get the predicted test labels.
  arma::Row<size_t> predictions;
  d.Classify(testData, predictions);

  BOOST_REQUIRE_EQUAL(predictions.n_elem, testData.n_cols);

  // Figure out the accuracy.
  double correct = 0.0;
  for (size_t i = 0; i < predictions.n_elem; ++i)
    if (predictions[i] == trueTestLabels[i])
      ++correct;
  correct /= predictions.n_elem;

  BOOST_REQUIRE_GT(correct, 0.75);
}

/**
 * Test that we can build a decision tree on a simple categorical dataset.
 */
BOOST_AUTO_TEST_CASE(CategoricalBuildTest)
{
  // We'll build a spiral dataset plus two noisy categorical features.  We need
  // to build the distributions for the categorical features (they'll be
  // discrete distributions).
  DiscreteDistribution c1[5];
  // The distribution will be automatically normalized.
  for (size_t i = 0; i < 5; ++i)
  {
    std::vector<arma::vec> probs;
    probs.push_back(arma::vec(4, arma::fill::randu));
    c1[i] = DiscreteDistribution(probs);
  }

  DiscreteDistribution c2[5];
  for (size_t i = 0; i < 5; ++i)
  {
    std::vector<arma::vec> probs;
    probs.push_back(arma::vec(2, arma::fill::randu));
    c2[i] = DiscreteDistribution(probs);
  }

  arma::mat spiralDataset(4, 10000);
  arma::Row<size_t> labels(10000);
  for (size_t i = 0; i < 10000; ++i)
  {
    // One circle every 20000 samples.  Plus some noise.
    const double magnitude = 2.0 + (double(i) / 2000.0) +
        0.5 * mlpack::math::Random();
    const double angle = (i % 2000) * (2 * M_PI) + mlpack::math::Random();

    const double x = magnitude * cos(angle);
    const double y = magnitude * sin(angle);

    spiralDataset(0, i) = x;
    spiralDataset(1, i) = y;

    // Set categorical features c1 and c2.
    if (i < 2000)
    {
      spiralDataset(2, i) = c1[1].Random()[0];
      spiralDataset(3, i) = c2[1].Random()[0];
      labels[i] = 1;
    }
    else if (i < 4000)
    {
      spiralDataset(2, i) = c1[3].Random()[0];
      spiralDataset(3, i) = c2[3].Random()[0];
      labels[i] = 3;
    }
    else if (i < 6000)
    {
      spiralDataset(2, i) = c1[2].Random()[0];
      spiralDataset(3, i) = c2[2].Random()[0];
      labels[i] = 2;
    }
    else if (i < 8000)
    {
      spiralDataset(2, i) = c1[0].Random()[0];
      spiralDataset(3, i) = c2[0].Random()[0];
      labels[i] = 0;
    }
    else
    {
      spiralDataset(2, i) = c1[4].Random()[0];
      spiralDataset(3, i) = c2[4].Random()[0];
      labels[i] = 4;
    }
  }

  // Now create the dataset info.
  data::DatasetInfo di(4);
  di.Type(2) = data::Datatype::categorical;
  di.Type(3) = data::Datatype::categorical;
  // Set mappings.
  di.MapString<double>("0", 2);
  di.MapString<double>("1", 2);
  di.MapString<double>("2", 2);
  di.MapString<double>("3", 2);
  di.MapString<double>("0", 3);
  di.MapString<double>("1", 3);

  // Now shuffle the dataset.
  arma::uvec indices = arma::shuffle(arma::linspace<arma::uvec>(0, 9999,
      10000));
  arma::mat d(4, 10000);
  arma::Row<size_t> l(10000);
  for (size_t i = 0; i < 10000; ++i)
  {
    d.col(i) = spiralDataset.col(indices[i]);
    l[i] = labels[indices[i]];
  }

  // Split into a training set and a test set.
  arma::mat trainingData = d.cols(0, 4999);
  arma::mat testData = d.cols(5000, 9999);
  arma::Row<size_t> trainingLabels = l.subvec(0, 4999);
  arma::Row<size_t> testLabels = l.subvec(5000, 9999);

  // Build the tree.
  DecisionTree<> tree(trainingData, di, trainingLabels, 5, 10);

  // Now evaluate the accuracy of the tree.
  arma::Row<size_t> predictions;
  tree.Classify(testData, predictions);

  BOOST_REQUIRE_EQUAL(predictions.n_elem, testData.n_cols);
  size_t correct = 0;
  for (size_t i = 0; i < testData.n_cols; ++i)
    if (testLabels[i] == predictions[i])
      ++correct;

  // Make sure we got at least 70% accuracy.
  const double correctPct = double(correct) / double(testData.n_cols);
  BOOST_REQUIRE_GT(correctPct, 0.70);
}

/**
 * Make sure that when we ask for a decision stump, we get one.
 */
BOOST_AUTO_TEST_CASE(DecisionStumpTest)
{
  // Use a random dataset.
  arma::mat dataset(10, 1000, arma::fill::randu);
  arma::Row<size_t> labels(1000);
  for (size_t i = 0; i < 1000; ++i)
    labels[i] = i % 3; // 3 classes.

  // Build a decision stump.
  DecisionTree<GiniGain, BestBinaryNumericSplit, AllCategoricalSplit, double,
      true> stump(dataset, labels, 3, 1);

  // Check that it has children.
  BOOST_REQUIRE_EQUAL(stump.NumChildren(), 2);
  // Check that its children doesn't have children.
  BOOST_REQUIRE_EQUAL(stump.Child(0).NumChildren(), 0);
  BOOST_REQUIRE_EQUAL(stump.Child(1).NumChildren(), 0);
}

BOOST_AUTO_TEST_SUITE_END();


/* Copyright (c) 2021 Grumpy Cat Software S.L.
 *
 * This Source Code is licensed under the MIT 2.0 license.
 * the terms can be found in  LICENSE.md at the root of
 * this project, or at http://mozilla.org/MPL/2.0/.
 */

#include <gauss/filters.h>
#include <boost/math/special_functions/factorials.hpp>

namespace {

af::array savitzkyGolay(af::array &y, int window_size, int order, unsigned int deriv, double rate=1.0) {

    // window_size has to be greater than three
    if (window_size < 3) {
        throw std::invalid_argument("Window size must be greater or equal to three");
    }

    // window_size has to be an odd number
    if (window_size % 2 == 0) {
        throw std::invalid_argument("Window size must be an odd number.");
    }

    if (window_size < (order + 2)) {
        throw std::invalid_argument("Window size is too small for polynomials order");
    }

    if (deriv > order) {
        throw std::invalid_argument("Deriv parameter has to be less than order");
    }

    auto const half_window = (window_size - 1) / 2;
    auto const order_range = order + 1;

    // Create a matrix with as many rows as window_size and as many columns as order + 1
    // The contents of each column is going to be a consecutive range
    // from -half_window to half_window.
    // example:
    //      -half_window + 0, ..., -half_window + 0   (as many columns as order range)
    //      -half_window + 1, ..., -half_window + 1
    //      ....
    //                    -1, ..., -1
    //                     0, ..., 0
    //                     1, ..., 1
    //      ....
    //      half_window - 1, ..., half_window - 1
    //      half_window - 0, ..., half_window - 0
    auto const b1 = af::iota(af::dim4(window_size), af::dim4(1, order_range), y.type()) - half_window;

    // This will generate the exponent to each value on b1
    // example:
    //      0, 1, 2, 3, 4, ..., order
    //      0, 1, 2, 3, 4, ..., order
    //      ...
    //      0, 1, 2, 3, 4, ..., order
    //
    // note the explicit usage of tiling on the first dimension.
    auto const bExp =  af::range(af::dim4(window_size, order_range), 1, y.type());

    // we have finally the Vandermonde matrix built.
    auto const b = af::pow(b1, bExp);

    // b could be also be constructed by the following iterative logic
    // but, if an accelerated device is present, the above code would work better,
    // as it doesn't imply an interation with sequential state.
    // b1(af::span, 0) = 1.0;
    // b1(af::span, 2) = b1(af::span, 1) * b1(af::span, 1);
    // b1(af::span, 3) = b1(af::span, 1) * b1(af::span, 2);
    // b1(af::span, 4) = b1(af::span, 1) * b1(af::span, 3);
    // etc...

    // now we need to compute the inverse of b
    auto const bInv = af::pinverse(b);

    // if b is (window_size x order+1), bInv is (order+1 x window_size)
    // we should select the row corresponding to the required derivative
    auto const m = bInv(deriv, af::span);

    // scale it
    auto const mAdj = m * pow(rate, deriv) * boost::math::factorial<double>(deriv);

    // finally, we need to flip the row before applying the conv operator.
    auto const mAdjFlip = af::flip(mAdj, 1);
    return af::convolve(y, mAdjFlip, af::convMode::AF_CONV_DEFAULT);
}

}


#include "stdafx.h"
#include "qmath.h"
#include "config.h"

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

// These are sample tests that show the different features of the framework

using namespace math;

namespace {

BOOST_AUTO_TEST_CASE(BINARY)
{
  	//returns true if the argument is a power of 2.
	BOOST_CHECK( is_pot(2) == true );
	BOOST_CHECK( is_pot(3) == false );
	BOOST_CHECK( is_pot(16) == true );

	BOOST_CHECK( get_min_pot(127) == 128 );
	BOOST_CHECK( get_min_pot(128) == 128 );
	BOOST_CHECK( get_min_pot(129) == 256 );

	//returns the log2 of the argument
	BOOST_CHECK( get_pot(4) == 2 );
	BOOST_CHECK( get_pot(5) == 2 );
	BOOST_CHECK( get_pot(7) == 2 );
	BOOST_CHECK( get_pot(8) == 3 );

	//returns the number of 1 bits in the argument
	BOOST_CHECK( get_on_bits_count(4) == 1 );
	BOOST_CHECK( get_on_bits_count(5) == 2 );
	BOOST_CHECK( get_on_bits_count(7) == 3 );
}

}


//-----------------------------------------------------------------------------
// Copyright (c) 2015-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#ifndef _mitrax__gauss_newton_algorithm__hpp_INCLUDED_
#define _mitrax__gauss_newton_algorithm__hpp_INCLUDED_

#include "make_matrix.hpp"
#include "gaussian_elimination.hpp"
#include "operator.hpp"
#include "norm.hpp"

#include "io/matrix.hpp"

#include <boost/container/vector.hpp>

#include <iostream>

namespace mitrax{

	template < typename F, typename M, row_t R, typename T, typename ... V >
	auto gauss_newton_algorithm(
		F&& f,
		col_vector< M, R > const& start_value,
		T const& threshold,
		boost::container::vector< std::tuple< V ... > > const& data
	){
		using std::abs;

		auto arg = start_value;

		for(;;){
			std::cout << arg << std::endl;

			auto r = make_vector_fn(rows(data.size()),
				[&data, &arg, &f](r_t r){
					return f(arg, data[r]);
				});

			auto d = make_matrix_fn(dim_pair(arg.rows().as_col(), data.size()),
				[&data, &arg, &f, &threshold](c_t c, r_t r){
					auto arg1 = arg;
					arg1[c] += threshold / 128;
					return (
						f(arg1, data[r]) - f(arg, data[r])
					) / (threshold / 128);
				}
			);

			auto trans_d = transpose(d);
// 			std::cout << d << std::endl;
// 			std::cout << trans_d << std::endl;
// 			std::cout << trans_d * d << std::endl;
// 			std::cout << trans_d * r << std::endl;
			auto s = gaussian_elimination(trans_d * d, -trans_d * r);
			auto arg_new = arg + s;

			auto diff = arg_new - arg;

			arg = arg_new;

			if(vector_norm_2sqr(diff) < threshold) break;
		}

		return arg;
	}

	template < typename F, typename M, row_t R, typename T, typename ... V >
	auto levenberg_marquardt_algorithm(
		F&& f,
		col_vector< M, R > const& start_value,
		T const& threshold,
		T mu,
		T const& beta0,
		T const& beta1,
		boost::container::vector< std::tuple< V ... > > const& data
	){
		using std::abs;

		auto arg = start_value;

		auto r = make_vector_fn(rows(data.size()),
			[&data, &arg, &f](r_t r){
				return f(arg, data[r]);
			});

		for(;;){
// 			std::cout << arg << std::endl;

			auto d = make_matrix_fn(dim_pair(arg.rows().as_col(), data.size()),
				[&data, &arg, &f, &threshold](c_t c, r_t r){
					auto arg1 = arg;
					arg1[c] += threshold / 128;
					return (
						f(arg1, data[r]) - f(arg, data[r])
					) / (threshold / 128);
				}
			);

			auto s = [&data, &arg, &r, &d, &f, &mu, beta0, beta1]{ for(;;){
				auto const mu2_matrix = make_diag_matrix_v< T >(
					arg.rows().as_dim(), mu * mu
				);

				auto trans_d = transpose(d);
				auto s = gaussian_elimination(
					trans_d * d + mu2_matrix,
					-trans_d * r
				);

				auto r_new = make_vector_fn(rows(data.size()),
					[&data, &arg, &s, &f](r_t r){
						return f(arg + s, data[r]);
					});

				auto r_norm = vector_norm_2sqr(r);
				auto numerator = (r_norm - vector_norm_2sqr(r_new));
				auto denominator = (r_norm - vector_norm_2sqr(r + d * s));
				auto eps = numerator / denominator;

				if(denominator == 0){
					throw std::runtime_error("eps is infinite");
				}

// 				std::cout << "eps: " << eps << std::endl;
				if(eps <= beta0){
					mu *= 2;
					continue;
				}

				r = r_new;

				if(eps < beta1){
					return s;
				}

				mu /= 2;
				return s;
			} }();

// 			std::cout << "s: " << s << std::endl;
			auto arg_new = arg + s;

			arg = arg_new;

			if(vector_norm_2sqr(s) < threshold) break;
		}

		return arg;
	}

}

#endif


#ifndef NN_3_LAYER_H
#define NN_3_LAYER_H

#include <armadillo>

using namespace arma;

class NN3Layer {
    private:
    mat w;
    mat v;
    double learningRate;
    double sigmoid(double x){
        return  1 / (1 + exp(-x));
    }
    double sigmoid_de(double y){//Differential equation
        return  y * ( 1 - y );
    }
    mat mat_to_sigmoid(mat x){
      x.for_each( [&](mat::elem_type& val) { val=sigmoid(val); } );
      return x;
    }
    mat mat_to_sigmoid_de(mat x){
      x.for_each( [&](mat::elem_type& val) { val=sigmoid_de(val); } );
      return x;
    }
    mat mat_to_learning_rate(mat x){
      x.for_each( [&](mat::elem_type& val) { val=val*this->learningRate; } );
      return x;
    }
    public:
    NN3Layer(int inputLayer,int hiddenLayer,int outputLayer,double learningRate){
        this->learningRate = learningRate;
        this->w =mat(hiddenLayer,inputLayer+1,fill::randu);
        this->v =mat(outputLayer,hiddenLayer+1,fill::randu);
    };
    mat forward_input_to_hidden(mat x){
        mat h;
        //set bias
        mat set_bias(1, 1, fill::ones);
        x = join_rows(x,set_bias);
        //h=sigmoid(wx+bias)
        h=this->mat_to_sigmoid(this->w*x.t());
        return h;
    }
    mat forward_hidden_to_output(mat h){
        mat o;
        //set bias
        mat set_bias(1, 1, fill::ones);
        h = join_cols(h,set_bias);
        //o=sigmoid(vh+bias)
        o=this->mat_to_sigmoid(this->v*h);
        return o;
    }
    //dO
    mat backward_output_to_hidden(mat o,mat d){
      mat error = d-o;
      square(error).print("error:");
      //-1*(d-o)
      mat dError =error;
      dError.for_each( [&](mat::elem_type& val) { val=val*-1; } );
      //sigmoid_de
      return this->mat_to_sigmoid_de(o)%dError;
    }
    //dH
    mat backward_hidden_to_input(mat dO,mat h){
      mat dnet =this->mat_to_sigmoid_de(h);
      mat dH = this->v;
      //remove bias
      dH.reshape(this->v.n_rows,this->v.n_cols-1);
      return (dH.t()*dO) %dnet;
    }
    void backward_update(mat dO,mat h,mat dH,mat x){
      //set bias
      mat set_bias(1, 1, fill::ones);
      //calculate weight update value
      mat dV = dO * join_cols(h,set_bias).t();
      mat dW = dH * join_rows(x,set_bias);
      //update weight
      this->v = this->v - this->mat_to_learning_rate(dV);
      this->w = this->w - this->mat_to_learning_rate(dW);
    }
    void train_step_sgd(mat x,mat d){
      //forward
      mat h = this->forward_input_to_hidden(x);
      mat o = this->forward_hidden_to_output(h);
      //backward
      //calculate gradient
      mat dO = this->backward_output_to_hidden(o,d);
      mat dH = this->backward_hidden_to_input(dO,h);
      //update weight
      this->backward_update(dO,h,dH,x);
    }
    mat only_forward(mat x){
      //forward
      mat h = this->forward_input_to_hidden(x);
      return this->forward_hidden_to_output(h);
    }

};

#endif


#define BOOST_TEST_MODULE Exponentiation test
#include <boost/test/unit_test.hpp>

#include "Variable.h"
#include "Sum.h"
using namespace omnn::math;
using namespace boost::unit_test;

BOOST_AUTO_TEST_CASE(Exponentiation_tests)
{
    auto a = 2_v ^ 3;
    BOOST_TEST(a==8);
    a = 2_v ^ 11;
    BOOST_TEST(a==(1<<11));
    Variable x;
    auto _1 = (x-2)^3;
    auto _2 = (x-2)*(x-2)*(x-2);
    BOOST_TEST(_1==_2);
    _1 = -32;
    _1 ^= .5;
    _2 = 4;
    _2 *= -2_v^(1_v/2);
    BOOST_TEST(_1==_2);
    _1 = -1_v*((-4_v)^(1_v/2))*((-16_v)^((1_v/2)));
    _2 = 8;
    BOOST_TEST(_1==_2);

    _1 = (((x-2)^2)+((x+2)^2));
    _1 = _1(x);
    _2 = ((-1)^(1_v/2))*2;
    BOOST_TEST(_1==_2);
}

BOOST_AUTO_TEST_CASE(Compare_test)
{
    auto _1 = 25 ^ Fraction{1_v,-2};
    auto _2 = (1_v / 5)*(1_v^(1_v/2));
    auto p =Product::cast(_1);
    if (p) for(auto&a:*p)
        std::cout << a.Hash() << std::endl;
    p =Product::cast(_2);
    if (p) for(auto&a:*p)
        std::cout << a.Hash() << std::endl;
    auto c = _1 == _2;
    BOOST_TEST(c);
}

BOOST_AUTO_TEST_CASE(Sqrt_test)
{
    auto a = 25_v;
    auto e = a ^ (1_v/2);
    auto c = e == (1_v^(1_v/2))*5;
    BOOST_TEST(c);
    BOOST_TEST(e != 5);
    // https://math.stackexchange.com/questions/41784/convert-any-number-to-positive-how/41787#comment5776496_41787
    e = a.Sqrt();
    BOOST_TEST(e == 5);
}

BOOST_AUTO_TEST_CASE(Polynomial_Sqrt_test
                     ,*disabled()
                     )
{
    Variable x,y;
    auto a = 4*(x^2)-12*x*y+9*(y^2);
    auto e = a.Sqrt();
    BOOST_TEST(e == 2*x-3*y);
}

BOOST_AUTO_TEST_CASE(Polynomial_Exp_test
                     ,*disabled()
                     )
{
    auto v = "v"_va;
    auto a = "a"_va;
    auto b = "b"_va;

    auto _ = (4_v*(v^2) + 2048)^(1_v/2);
    _.SetView(Valuable::View::Solving);
    _.optimize();

    auto t=4_v;
    auto c=2048_v;
    auto _2ab=2_v*a*b;
    auto d = (-_2ab).sq() + 4_v*(t-a.Sq())*(b.Sq()-c);
    auto x = (_2ab+(d^(1_v/2)))/(2_v*(t-a.Sq()));
    _.eval({{v,x}});
    auto ab = 0_v;
    _.eval({{b,ab}});

    auto e = Exponentiation::cast(_);
    auto eb = e->getBase();
    auto ebs = Sum::cast(eb);
    for(auto&m:*ebs){
//        if (m.) {
//            <#statements#>
//        }
    }
    auto it =ebs->begin();
    auto m1=*it;
    for(auto& v:it->getCommonVars())
        std::cout << v.first.str() << '^' << v.second.str() << std::endl;
    auto m2 =*++it;
    auto ms = m1+m2;
//    Valuable f = Fraction{

//    auto av = _(a);
    _.optimize();
}

BOOST_AUTO_TEST_CASE(Multival_test)
{
    auto a = 1_v ^ (1_v/2);
    BOOST_TEST(!!a.IsMultival() == true);

    DECL_VA(x);
    BOOST_TEST(!!x.IsMultival() == true);
    a = x / x;
    BOOST_TEST(a != 1);
    BOOST_TEST(!!a.IsMultival() == true);
    BOOST_TEST(a == x/x);
}


#include <cassert>
#include <cstdlib>
#include <cstdint>
#include <iostream>
#include <limits>
#include <string>
#include <boost/multiprecision/cpp_dec_float.hpp>

using real_t = boost::multiprecision::number<boost::multiprecision::cpp_dec_float<10000>>;

real_t operator"" _R( long double d ) {
	return real_t{ d };
}

real_t get_fibonacci( uintmax_t const n ) {
	// Use Binet's formula, limited n to decimal for perf increase
	if( n < 1 ) {
		return 0.0_R;
	}
	static real_t const sqrt_five = sqrt( 5.0_R );
	static real_t const a_part = (1.0_R + sqrt_five) / 2.0_R;
	static real_t const b_part = (1.0_R - sqrt_five) / 2.0_R;

	real_t const a = pow( a_part, n );
	real_t const b = pow( b_part, n );
	return round( (a - b)/sqrt_five );
}

int main( int argc, char **argv ) {
//	assert( argc == 2 );
//
//	auto n = strtoull( argv[1], 0, 10 );
	uint64_t n = 1234567;
	std::cout << n << ": " << std::setprecision(std::numeric_limits<real_t>::max_digits10) << get_fibonacci( n ) << '\n';
	return EXIT_SUCCESS;
}


#include <iostream>
#include <string>
#include <algorithm>
#include <utility> // std::pair, std::make_pair
#include <cmath> // float comparison
#include <limits>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "betti_numbers"
#include <boost/test/unit_test.hpp>

#include <gudhi/Simplex_tree.h>
#include <gudhi/Persistent_cohomology.h>

struct MiniSTOptions : Gudhi::Simplex_tree_options_full_featured {
  // Implicitly use 0 as filtration value for all simplices
  static const bool store_filtration = false;
  // The persistence algorithm needs this
  static const bool store_key = true;
  // I have few vertices
  typedef short Vertex_handle;
};

using Mini_simplex_tree = Gudhi::Simplex_tree<MiniSTOptions>;
using Mini_st_persistence =
    Gudhi::persistent_cohomology::Persistent_cohomology<Mini_simplex_tree, Gudhi::persistent_cohomology::Field_Zp>;

/*
 * Compare two intervals by dimension, then by length.
 */
template<class Simplicial_complex>
struct cmp_intervals_by_dim_then_length {
  explicit cmp_intervals_by_dim_then_length(Simplicial_complex * sc)
      : sc_(sc) { }

  template<typename Persistent_interval>
  bool operator()(const Persistent_interval & p1, const Persistent_interval & p2) {
    if (sc_->dimension(get < 0 > (p1)) == sc_->dimension(get < 0 > (p2)))
      return (sc_->filtration(get < 1 > (p1)) - sc_->filtration(get < 0 > (p1))
              > sc_->filtration(get < 1 > (p2)) - sc_->filtration(get < 0 > (p2)));
    else
      return (sc_->dimension(get < 0 > (p1)) > sc_->dimension(get < 0 > (p2)));
  }
  Simplicial_complex* sc_;
};

BOOST_AUTO_TEST_CASE( plain_homology_betti_numbers )
{
  Mini_simplex_tree st;

  /* Complex to build. */
  /*    1   4          */
  /*    o---o          */
  /*   /3\ /           */
  /*  o---o   o        */
  /*  2   0   5        */
  const short tetra0123[] = {0, 1, 2, 3};
  const short edge04[] = {0, 4};
  const short edge14[] = {1, 4};
  const short vertex5[] = {5};
  st.insert_simplex_and_subfaces(tetra0123);
  st.insert_simplex_and_subfaces(edge04);
  st.insert_simplex(edge14);
  st.insert_simplex(vertex5);

  // Sort the simplices in the order of the filtration
  st.initialize_filtration();

  // Class for homology computation
  Mini_st_persistence pcoh(st);

  // Initialize the coefficient field Z/3Z for homology
  pcoh.init_coefficients(3);

  // Compute the persistence diagram of the complex
  pcoh.compute_persistent_cohomology();

  // Print the result. The format is, on each line: 2 dim 0 inf
  // where 2 represents the field, dim the dimension of the feature.
  // 2  0 0 inf
  // 2  0 0 inf
  // 2  1 0 inf
  // means that in Z/2Z-homology, the Betti numbers are b0=2 and b1=1.

  std::cout << "BETTI NUMBERS" << std::endl;

  BOOST_CHECK(pcoh.betti_number(0) == 2);
  BOOST_CHECK(pcoh.betti_number(1) == 1);
  BOOST_CHECK(pcoh.betti_number(2) == 0);

  std::vector<int> bns = pcoh.betti_numbers();
  BOOST_CHECK(bns.size() == 3);
  BOOST_CHECK(bns[0] == 2);
  BOOST_CHECK(bns[1] == 1);
  BOOST_CHECK(bns[2] == 0);

  std::cout << "GET PERSISTENT PAIRS" << std::endl;

  // Custom sort and output persistence
  cmp_intervals_by_dim_then_length<Mini_simplex_tree> cmp(&st);
  auto persistent_pairs = pcoh.get_persistent_pairs();

  std::sort(std::begin(persistent_pairs), std::end(persistent_pairs), cmp);

  BOOST_CHECK(persistent_pairs.size() == 3);
  // persistent_pairs[0] = 2  1 0 inf
  BOOST_CHECK(st.dimension(get<0>(persistent_pairs[0])) == 1);
  BOOST_CHECK(st.filtration(get<0>(persistent_pairs[0])) == 0);
  BOOST_CHECK(get<1>(persistent_pairs[0]) == st.null_simplex());

  // persistent_pairs[1] = 2  0 0 inf
  BOOST_CHECK(st.dimension(get<0>(persistent_pairs[1])) == 0);
  BOOST_CHECK(st.filtration(get<0>(persistent_pairs[1])) == 0);
  BOOST_CHECK(get<1>(persistent_pairs[1]) == st.null_simplex());

  // persistent_pairs[2] = 2  0 0 inf
  BOOST_CHECK(st.dimension(get<0>(persistent_pairs[2])) == 0);
  BOOST_CHECK(st.filtration(get<0>(persistent_pairs[2])) == 0);
  BOOST_CHECK(get<1>(persistent_pairs[2]) == st.null_simplex());

  std::cout << "INTERVALS IN DIMENSION" << std::endl;

  auto intervals_in_dimension_0 = pcoh.intervals_in_dimension(0);
  std::cout << "intervals_in_dimension_0.size() = " << intervals_in_dimension_0.size() << std::endl;
  for (std::size_t i = 0; i < intervals_in_dimension_0.size(); i++)
    std::cout << "intervals_in_dimension_0[" << i << "] = [" << intervals_in_dimension_0[i].first << "," <<
                 intervals_in_dimension_0[i].second << "]" << std::endl;
  BOOST_CHECK(intervals_in_dimension_0.size() == 2);
  BOOST_CHECK(intervals_in_dimension_0[0].first == 0);
  BOOST_CHECK(intervals_in_dimension_0[0].second == std::numeric_limits<Mini_simplex_tree::Filtration_value>::infinity());
  BOOST_CHECK(intervals_in_dimension_0[1].first == 0);
  BOOST_CHECK(intervals_in_dimension_0[1].second == std::numeric_limits<Mini_simplex_tree::Filtration_value>::infinity());

  auto intervals_in_dimension_1 = pcoh.intervals_in_dimension(1);
  std::cout << "intervals_in_dimension_1.size() = " << intervals_in_dimension_1.size() << std::endl;
  for (std::size_t i = 0; i < intervals_in_dimension_1.size(); i++)
    std::cout << "intervals_in_dimension_1[" << i << "] = [" << intervals_in_dimension_1[i].first << "," <<
                 intervals_in_dimension_1[i].second << "]" << std::endl;
  BOOST_CHECK(intervals_in_dimension_1.size() == 1);
  BOOST_CHECK(intervals_in_dimension_1[0].first == 0);
  BOOST_CHECK(intervals_in_dimension_1[0].second == std::numeric_limits<Mini_simplex_tree::Filtration_value>::infinity());

  auto intervals_in_dimension_2 = pcoh.intervals_in_dimension(2);
  std::cout << "intervals_in_dimension_2.size() = " << intervals_in_dimension_2.size() << std::endl;
  BOOST_CHECK(intervals_in_dimension_2.size() == 0);
}

using Simplex_tree = Gudhi::Simplex_tree<>;
using St_persistence =
    Gudhi::persistent_cohomology::Persistent_cohomology<Simplex_tree, Gudhi::persistent_cohomology::Field_Zp>;

BOOST_AUTO_TEST_CASE( betti_numbers )
{
  Simplex_tree st;

  /* Complex to build. */
  /*    1   4          */
  /*    o---o          */
  /*   /3\ /           */
  /*  o---o   o        */
  /*  2   0   5        */
  const short tetra0123[] = {0, 1, 2, 3};
  const short edge04[] = {0, 4};
  const short edge14[] = {1, 4};
  const short vertex5[] = {5};
  st.insert_simplex_and_subfaces(tetra0123, 4.0);
  st.insert_simplex_and_subfaces(edge04, 2.0);
  st.insert_simplex(edge14, 2.0);
  st.insert_simplex(vertex5, 1.0);

  // Sort the simplices in the order of the filtration
  st.initialize_filtration();

  // Class for homology computation
  St_persistence pcoh(st);

  // Initialize the coefficient field Z/3Z for homology
  pcoh.init_coefficients(3);

  // Compute the persistence diagram of the complex
  pcoh.compute_persistent_cohomology();

  // Check the Betti numbers are b0=2, b1=1 and b2=0.
  BOOST_CHECK(pcoh.betti_number(0) == 2);
  BOOST_CHECK(pcoh.betti_number(1) == 1);
  BOOST_CHECK(pcoh.betti_number(2) == 0);

  // Check the Betti numbers are b0=2, b1=1 and b2=0.
  std::vector<int> bns = pcoh.betti_numbers();
  BOOST_CHECK(bns.size() == 3);
  BOOST_CHECK(bns[0] == 2);
  BOOST_CHECK(bns[1] == 1);
  BOOST_CHECK(bns[2] == 0);

  // Check the persistent Betti numbers in [4., 10.] are b0=2, b1=1 and b2=0.
  BOOST_CHECK(pcoh.persistent_betti_number(0, 4., 10.) == 2);
  BOOST_CHECK(pcoh.persistent_betti_number(1, 4., 10.) == 1);
  BOOST_CHECK(pcoh.persistent_betti_number(2, 4., 10.) == 0);

  // Check the persistent Betti numbers in [2., 100.] are b0=2, b1=0 and b2=0.
  BOOST_CHECK(pcoh.persistent_betti_number(0, 2., 100.) == 2);
  BOOST_CHECK(pcoh.persistent_betti_number(1, 2., 100.) == 0);
  BOOST_CHECK(pcoh.persistent_betti_number(2, 2., 100.) == 0);

  // Check the persistent Betti numbers in [1., 1000.] are b0=1, b1=0 and b2=0.
  BOOST_CHECK(pcoh.persistent_betti_number(0, 1., 1000.) == 1);
  BOOST_CHECK(pcoh.persistent_betti_number(1, 1., 1000.) == 0);
  BOOST_CHECK(pcoh.persistent_betti_number(2, 1., 1000.) == 0);

  // Check the persistent Betti numbers in [.9, 1000.] are b0=0, b1=0 and b2=0.
  BOOST_CHECK(pcoh.persistent_betti_number(0, .9, 1000.) == 0);
  BOOST_CHECK(pcoh.persistent_betti_number(1, .9, 1000.) == 0);
  BOOST_CHECK(pcoh.persistent_betti_number(2, .9, 1000.) == 0);

  // Check the persistent Betti numbers in [4.1, 10000.] are b0=2, b1=1 and b2=0.
  bns = pcoh.persistent_betti_numbers(4.1, 10000.);
  BOOST_CHECK(bns[0] == 2);
  BOOST_CHECK(bns[1] == 1);
  BOOST_CHECK(bns[2] == 0);

  // Check the persistent Betti numbers in [2.1, 100000.] are b0=2, b1=0 and b2=0.
  bns = pcoh.persistent_betti_numbers(2.1, 100000.);
  BOOST_CHECK(bns[0] == 2);
  BOOST_CHECK(bns[1] == 0);
  BOOST_CHECK(bns[2] == 0);

  // Check the persistent Betti numbers in [1.1, 1000000.] are b0=1, b1=0 and b2=0.
  bns = pcoh.persistent_betti_numbers(1.1, 1000000.);
  BOOST_CHECK(bns[0] == 1);
  BOOST_CHECK(bns[1] == 0);
  BOOST_CHECK(bns[2] == 0);

  // Check the persistent Betti numbers in [.1, 10000000.] are b0=0, b1=0 and b2=0.
  bns = pcoh.persistent_betti_numbers(.1, 10000000.);
  BOOST_CHECK(bns[0] == 0);
  BOOST_CHECK(bns[1] == 0);
  BOOST_CHECK(bns[2] == 0);

  // Custom sort and output persistence
  cmp_intervals_by_dim_then_length<Simplex_tree> cmp(&st);
  auto persistent_pairs = pcoh.get_persistent_pairs();

  std::sort(std::begin(persistent_pairs), std::end(persistent_pairs), cmp);

  BOOST_CHECK(persistent_pairs.size() == 3);
  // persistent_pairs[0] = 2  1 4 inf
  BOOST_CHECK(st.dimension(get<0>(persistent_pairs[0])) == 1);
  BOOST_CHECK(st.filtration(get<0>(persistent_pairs[0])) == 4);
  BOOST_CHECK(get<1>(persistent_pairs[0]) == st.null_simplex());

  // persistent_pairs[1] = 2  0 2 inf
  BOOST_CHECK(st.dimension(get<0>(persistent_pairs[1])) == 0);
  BOOST_CHECK(st.filtration(get<0>(persistent_pairs[1])) == 2);
  BOOST_CHECK(get<1>(persistent_pairs[1]) == st.null_simplex());

  // persistent_pairs[2] = 2  0 1 inf
  BOOST_CHECK(st.dimension(get<0>(persistent_pairs[2])) == 0);
  BOOST_CHECK(st.filtration(get<0>(persistent_pairs[2])) == 1);
  BOOST_CHECK(get<1>(persistent_pairs[2]) == st.null_simplex());

  std::cout << "INTERVALS IN DIMENSION" << std::endl;

  auto intervals_in_dimension_0 = pcoh.intervals_in_dimension(0);
  std::cout << "intervals_in_dimension_0.size() = " << intervals_in_dimension_0.size() << std::endl;
  for (std::size_t i = 0; i < intervals_in_dimension_0.size(); i++)
    std::cout << "intervals_in_dimension_0[" << i << "] = [" << intervals_in_dimension_0[i].first << "," <<
                 intervals_in_dimension_0[i].second << "]" << std::endl;
  BOOST_CHECK(intervals_in_dimension_0.size() == 2);
  BOOST_CHECK(intervals_in_dimension_0[0].first == 2);
  BOOST_CHECK(intervals_in_dimension_0[0].second == std::numeric_limits<Mini_simplex_tree::Filtration_value>::infinity());
  BOOST_CHECK(intervals_in_dimension_0[1].first == 1);
  BOOST_CHECK(intervals_in_dimension_0[1].second == std::numeric_limits<Mini_simplex_tree::Filtration_value>::infinity());

  auto intervals_in_dimension_1 = pcoh.intervals_in_dimension(1);
  std::cout << "intervals_in_dimension_1.size() = " << intervals_in_dimension_1.size() << std::endl;
  for (std::size_t i = 0; i < intervals_in_dimension_1.size(); i++)
    std::cout << "intervals_in_dimension_1[" << i << "] = [" << intervals_in_dimension_1[i].first << "," <<
                 intervals_in_dimension_1[i].second << "]" << std::endl;
  BOOST_CHECK(intervals_in_dimension_1.size() == 1);
  BOOST_CHECK(intervals_in_dimension_1[0].first == 4);
  BOOST_CHECK(intervals_in_dimension_1[0].second == std::numeric_limits<Mini_simplex_tree::Filtration_value>::infinity());

  auto intervals_in_dimension_2 = pcoh.intervals_in_dimension(2);
  std::cout << "intervals_in_dimension_2.size() = " << intervals_in_dimension_2.size() << std::endl;
  BOOST_CHECK(intervals_in_dimension_2.size() == 0);

  std::cout << "EMPTY COMPLEX" << std::endl;
  Simplex_tree empty;
  empty.initialize_filtration();
  St_persistence pcoh_empty(empty, false);
  pcoh_empty.init_coefficients(2);
  pcoh_empty.compute_persistent_cohomology();
  BOOST_CHECK(pcoh_empty.betti_numbers().size() == 0);
  BOOST_CHECK(pcoh_empty.persistent_betti_numbers(0,1).size() == 0);
}


// Copyright (C) 2016 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "tutorial_nrosy.h"
#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <Eigen/Eigenvalues>
#include <iostream>

using namespace std;
using namespace Eigen;

std::complex<double> find_root(std::complex<double> c0, int n)
{
  // Find the roots of p(t) = (t - c0)^n using
  // https://en.wikipedia.org/wiki/Companion_matrix
  Eigen::MatrixXcd M = Eigen::MatrixXcd::Zero(n,n);
  for (int i=1;i<n;++i)
    M(i,i-1) = std::complex<double>(1,0);
  M(0,n-1) = c0;
  return M.eigenvalues()(0);
}

MatrixXd tutorial_nrosy
        (
        const MatrixXd& V,          // Vertices of the mesh
        const MatrixXi& F,          // Faces
        const MatrixXi& TT,         // Adjacency triangle-triangle
        const VectorXi& soft_id,    // Soft constraints face ids
        const MatrixXd& soft_value, // Soft constraints 3d vectors
        const int n                 // Degree of the n-rosy field
        )
{
  assert(soft_id.size() > 0); // One constraint is necessary to make the solution unique

  Matrix<double,Dynamic,3> T1(F.rows(),3), T2(F.rows(),3);

  // Compute the local reference systems for each face
  for (unsigned i=0;i<F.rows();++i)
  {
    Vector3d e1 =  V.row(F(i, 1)) - V.row(F(i, 0));
    Vector3d e2 =  V.row(F(i, 2)) - V.row(F(i, 0));
    T1.row(i) = e1.normalized();
    T2.row(i) = T1.row(i).cross(T1.row(i).cross(e2)).normalized();
  }

  // Build the sparse matrix, with an energy term for each edge
  std::vector< Triplet<std::complex<double> > > t;
  std::vector< Triplet<std::complex<double> > > tb;

  unsigned count = 0;
  for (unsigned f=0;f<F.rows();++f)
  {
    for (unsigned ei=0;ei<F.cols();++ei)
    {
      // Look up the opposite face
      int g = TT(f,ei);

      // If it is a boundary edge, it does not contribute to the energy
      if (g == -1) continue;

      // Avoid to count every edge twice
      if (f > g) continue;

      // Compute the complex representation of the common edge
      Vector3d e  = (V.row(F(f,(ei+1)%3)) - V.row(F(f,ei)));
      Vector2d vef = Vector2d(e.dot(T1.row(f)),e.dot(T2.row(f))).normalized();
      std::complex<double> ef(vef(0),vef(1));
      Vector2d veg = Vector2d(e.dot(T1.row(g)),e.dot(T2.row(g))).normalized();
      std::complex<double> eg(veg(0),veg(1));

      // Add the term conj(f)^n*ui - conj(g)^n*uj to the energy matrix
      t.push_back(Triplet<std::complex<double> >(count,f,    std::pow(std::conj(ef),n)));
      t.push_back(Triplet<std::complex<double> >(count,g,-1.*std::pow(std::conj(eg),n)));

      ++count;
    }
  }

  // Convert the constraints into the complex polynomial coefficients and add them as soft constraints
  double lambda = 10e6;
  for (unsigned r=0; r<soft_id.size(); ++r)
  {
    int f = soft_id(r);
    Vector3d v = soft_value.row(r);
    std::complex<double> c(v.dot(T1.row(f)),v.dot(T2.row(f)));
    t.push_back(Triplet<std::complex<double> >(count,f, sqrt(lambda)));
    tb.push_back(Triplet<std::complex<double> >(count,0, std::pow(c,n) * std::complex<double>(sqrt(lambda),0)));
    ++count;
  }

  // Solve the linear system
  typedef SparseMatrix<std::complex<double>> SparseMatrixXcd;
  SparseMatrixXcd A(count,F.rows());
  A.setFromTriplets(t.begin(), t.end());
  SparseMatrixXcd b(count,1);
  b.setFromTriplets(tb.begin(), tb.end());
  SimplicialLDLT< SparseMatrixXcd > solver;
  solver.compute(A.adjoint()*A);
  assert(solver.info()==Success);
  MatrixXcd u = solver.solve(A.adjoint()*MatrixXcd(b));
  assert(solver.info()==Success);

  // Convert the interpolated polyvector into Euclidean vectors
  MatrixXd R(F.rows(),3);
  for (int f=0; f<F.rows(); ++f)
  {
    std::complex<double> root = find_root(u(f),n);
    R.row(f) = T1.row(f) * root.real() + T2.row(f) * root.imag();
  }

  return R;
}


// Gao Wang and Kushal K. Dey (c) 2016
// code format configuration: clang-format -style=Google -dump-config >
// ~/.clang-format
#ifndef _PFA_HPP
#define _PFA_HPP

#include <omp.h>
#include <armadillo>
#include <map>
#include <string>

static const double INV_SQRT_2PI = 0.3989422804014327;
static const double INV_SQRT_2PI_LOG = -0.91893853320467267;

inline double normal_pdf(double x, double m, double sd) {
  double a = (x - m) / sd;
  return INV_SQRT_2PI / sd * std::exp(-0.5 * a * a);
};

inline double normal_pdf_log(double x, double m, double sd) {
  double a = (x - m) / sd;
  return INV_SQRT_2PI_LOG - std::log(sd) - 0.5 * a * a;
};

/* The digamma function is the derivative of gammaln.
   Reference:
    J Bernardo,
    Psi ( Digamma ) Function,
    Algorithm AS 103,
    Applied Statistics,
    Volume 25, Number 3, pages 315-317, 1976.
    From http://www.psc.edu/~burkardt/src/dirichlet/dirichlet.f
    (with modifications for negative numbers and extra precision)
*/
inline double digamma(double x) {
  double neginf = -INFINITY;
  static const double c = 12, digamma1 = -0.57721566490153286,
                      trigamma1 = 1.6449340668482264365, /* pi^2/6 */
      s = 1e-6, s3 = 1. / 12, s4 = 1. / 120, s5 = 1. / 252, s6 = 1. / 240,
                      s7 = 1. / 132, s8 = 691. / 32760, s9 = 1. / 12,
                      s10 = 3617. / 8160;
  double result;
  /* Illegal arguments */
  if ((x == neginf) || std::isnan(x)) {
    return NAN;
  }
  /* Singularities */
  if ((x <= 0) && (floor(x) == x)) {
    return neginf;
  }
  /* Negative values */
  /* Use the reflection formula (Jeffrey 11.1.6):
 * digamma(-x) = digamma(x+1) + pi*cot(pi*x)
 *
 * This is related to the identity
 * digamma(-x) = digamma(x+1) - digamma(z) + digamma(1-z)
 * where z is the fractional part of x
 * For example:
 * digamma(-3.1) = 1/3.1 + 1/2.1 + 1/1.1 + 1/0.1 + digamma(1-0.1)
 *               = digamma(4.1) - digamma(0.1) + digamma(1-0.1)
 * Then we use
 * digamma(1-z) - digamma(z) = pi*cot(pi*z)
 */
  if (x < 0) {
    return digamma(1 - x) + M_PI / tan(-M_PI * x);
  }
  /* Use Taylor series if argument <= S */
  if (x <= s) return digamma1 - 1 / x + trigamma1 * x;
  /* Reduce to digamma(X + N) where (X + N) >= C */
  result = 0;
  while (x < c) {
    result -= 1 / x;
    x++;
  }
  /* Use de Moivre's expansion if argument >= C */
  /* This expansion can be computed in Maple via asympt(Psi(x),x) */
  if (x >= c) {
    double r = 1 / x, t;
    result += log(x) - 0.5 * r;
    r *= r;
#if 1
    result -= r * (s3 - r * (s4 - r * (s5 - r * (s6 - r * s7))));
#else
    /* this version for lame compilers */
    t = (s5 - r * (s6 - r * s7));
    result -= r * (s3 - r * (s4 - r * t));
#endif
  }
  return result;
}

template <typename T>
void print(const T &e) {
  std::cout << e << std::endl;
}

class Exception {
 public:
  /// constructor
  /// \param msg error message
  Exception(const std::string &msg) : m_msg(msg) {}

  /// return error message
  const char *message() { return m_msg.c_str(); }

  virtual ~Exception(){};

 private:
  /// error message
  std::string m_msg;
};

/// exception, thrown if out of memory
class StopIteration : public Exception {
 public:
  StopIteration(const std::string msg) : Exception(msg){};
};

/// exception, thrown if index out of range
class IndexError : public Exception {
 public:
  IndexError(const std::string msg) : Exception(msg){};
};

/// exception, thrown if value of range etc
class ValueError : public Exception {
 public:
  ValueError(const std::string msg) : Exception(msg){};
};

/// exception, thrown if system error occurs
class SystemError : public Exception {
 public:
  SystemError(const std::string msg) : Exception(msg){};
};

/// exception, thrown if a runtime error occurs
class RuntimeError : public Exception {
 public:
  RuntimeError(const std::string msg) : Exception(msg){};
};

extern "C" int pfa_em(double *, double *, double *, double *, int *, int *,
                      int *, int *, double *, int *, double *, int *, int *,
                      double *, double *, double *, double *, int *, int *,
                      int *, int *, int *, int *);
extern "C" int pfa_model_loglik(double *, double *, double *, double *, int *,
                                int *, int *, double *);

class PFA {
 public:
  PFA(double *cX, double *cF, double *cP, double *cQ, double *cLout, int N,
      int J, int K, int C)
      :  // mat(aux_mem*, n_rows, n_cols, copy_aux_mem = true, strict = true)
        D(cX, N, J, false, true),
        F(cF, K, J, false, true),
        P(cP, K, K, false, true),
        q(cQ, C, false, true),
        L(cLout, N, K, false, true) {
    // initialize residual sd with sample sd
    s = arma::vectorise(arma::stddev(D));
    W.set_size(F.n_rows, F.n_rows);
    delta.set_size(int((F.n_rows + 1) * F.n_rows / 2), q.n_elem, D.n_rows);
    delta.fill(0);
    n_threads = omp_get_max_threads();
    n_updates = 0;
    for (size_t k1 = 0; k1 < F.n_rows; k1++) {
      for (size_t k2 = 0; k2 <= k1; k2++) {
        // set factor pair coordinates to avoid
        // having to compute it at each iteration
        // (b + 1) * b / 2 - (b - a)
        size_t k1k2 = size_t(k1 * (k1 + 1) / 2 + k2);
        F_pair_coord[std::make_pair(k1, k2)] = k1k2;
        F_pair_coord[std::make_pair(k2, k1)] = k1k2;
      }
    }
  }
  virtual ~PFA() {}

  virtual PFA *clone() const { return new PFA(*this); }

  virtual void write(std::ostream &out, int info) {
    throw RuntimeError("The base write() function should not be called");
  }
  virtual int E_step() {
    throw RuntimeError("The base E_step() function should not be called");
  }

  virtual int M_step() {
    throw RuntimeError("The base M_step() function should not be called");
  }

  void update_model_loglik(arma::vec &true_s, arma::mat &true_q);
  void set_threads(int n) { n_threads = n; }
  void update_ldelta(int core = 0);
  void update_loglik_and_delta();
  void update_factor_model();
  void update_residual_error();
  double get_loglik() { return loglik; }
  double get_bic() {
    // BIC = -2 * loglik + d * log(N)
    // number of parameters:
    // P: (K + 1) * K / 2
    // L: N * K
    // F: K * J
    return -2 * loglik +
           ((P.n_rows + 1) * P.n_rows / 2 + D.n_rows * P.n_rows +
            P.n_rows * D.n_cols) *
               std::log(D.n_rows);
  }

 protected:
  // N by J matrix of data
  arma::mat D;
  // K by K matrix of factor pair frequencies
  arma::mat P;
  // K by J matrix of factors
  arma::mat F;
  // Q by 1 vector of membership grids
  arma::vec q;
  // J by 1 vector of residual standard error
  arma::vec s;
  // K1K2 by Q by N tensor
  arma::cube delta;
  arma::cube ldelta;
  std::map<std::pair<size_t, size_t>, size_t> F_pair_coord;
  // N by K matrix of loadings
  arma::mat L;
  // W = L'L is K X K matrix
  arma::mat W;
  // loglik
  double loglik;
  // number of threads
  int n_threads;
  // updates on the model
  int n_updates;
};

class PFA_EM : public PFA {
 public:
  PFA_EM(double *cX, double *cF, double *cP, double *cQ, double *cLout, int N,
         int J, int K, int C)
      : PFA(cX, cF, cP, cQ, cLout, N, J, K, C) {}

  PFA *clone() const { return new PFA_EM(*this); }

  void update_paired_factor_weights();
  int E_step() {
    update_ldelta();
    update_loglik_and_delta();
    update_paired_factor_weights();
    return 0;
  }

  int M_step() {
    update_factor_model();
    update_residual_error();
    n_updates += 1;
    return 0;
  }

  void write(std::ostream &out, int info) {
    if (info == 0) {
      // D.print(out, "Data Matrix:");
      q.print(out, "Membership grids:");
    }
    if (info == 1) {
      F.print(out, "Factor matrix:");
      P.print(out, "Factor frequency matrix:");
      if (n_updates > 0)
        L.print(out, "Loading matrix:");
      else
        out << "Loading matrix:" << std::endl;
    }
    if (info == 2) {
      // W.print(out, "E[L'L] matrix:");
      s.print(out, "Residual standard deviation of data columns:");
      if (n_updates > 0) {
        ldelta.print(out, "log delta tensor");
        delta.print(out, "delta tensor");
      }
    }
  }
};

class PFA_VEM : public PFA {
 public:
  PFA_VEM(double *cX, double *cF, double *cP, double *cQ, double *cLout,
          double *calphaout, int N, int J, int K, int C, double alpha0)
      : PFA(cX, cF, cP, cQ, cLout, N, J, K, C),
        alpha(calphaout, K, K, false, true),
        alpha0(alpha0) {
    alpha.zeros();
    digamma_alpha.set_size(P.n_rows, P.n_rows);
    digamma_alpha.fill(digamma(alpha0));
    digamma_sum_alpha = 0;
    maxiter = 1000;
    tol = 1E-3;
  }

  PFA *clone() const { return new PFA_VEM(*this); }

  void update_variational_ldelta();
  double get_variational_lowerbound();
  void update_variational_parameters();
  void update_paired_factor_weights();

  int E_step() {
    int status = 0;
    update_ldelta(1);
    size_t niter = 0;
    lowerbound.resize(0);
    while (niter <= maxiter) {
      update_variational_ldelta();
      update_loglik_and_delta();
      update_variational_parameters();
      update_paired_factor_weights();
      lowerbound.push_back(get_variational_lowerbound());
      if (lowerbound.back() != lowerbound.back()) {
        std::cerr
            << "[ERROR] lower bound nan produced in variational approximation!"
            << std::endl;
        status = 1;
        break;
      }
      niter++;
      if (niter > 1) {
        double diff = lowerbound[niter - 1] - lowerbound[niter - 2];
        if (diff < 0.0) {
          std::cerr << "[ERROR] lower bound decreased in variational "
                       "approximation:  \n\tfrom "
                    << lowerbound[niter - 2] << " to " << lowerbound[niter - 1]
                    << "!" << std::endl;
          status = 1;
          break;
        }
        if (diff < tol) break;
      }
      if (niter == maxiter) {
        std::cerr << "[WARNIKNG] variational approximation procedure failed to "
                     "converge at tolerance level "
                  << tol << ", after " << maxiter
                  << " iterations: \n\tlog lower bound starts "
                  << lowerbound.front() << ", ends " << lowerbound.back() << "!"
                  << std::endl;
        status = 1;
        break;
      }
    }
    return status;
  }

  int M_step() {
    update_factor_model();
    update_residual_error();
    n_updates += 1;
    return 0;
  }

  void write(std::ostream &out, int info) {
    if (info == 0) {
      // D.print(out, "Data Matrix:");
      q.print(out, "Membership grids:");
    }
    if (info == 1) {
      F.print(out, "Factor matrix:");
      P.print(out, "Factor frequency matrix:");
      if (n_updates > 0)
        L.print(out, "Loading matrix:");
      else
        out << "Loading matrix:" << std::endl;
    }
    if (info == 2) {
      // W.print(out, "E[L'L] matrix:");
      s.print(out, "Residual standard deviation of data columns:");
      if (n_updates > 0) {
        alpha.print(out, "Dirichlet parameter for factor pairs:");
        out << "log lower bound:" << std::endl;
        for (size_t i = 0; i < lowerbound.size(); ++i) {
          out << lowerbound[i] << ", ";
        }
        out << std::endl;
      }
    }
  }

 private:
  // Dirichlet priors for factors and grids
  double alpha0;
  // K by K matrix of digamma of variational parameter for factor pair
  // frequencies
  arma::mat digamma_alpha;
  arma::mat alpha;
  double digamma_sum_alpha;
  size_t maxiter;
  double tol;
  std::vector<double> lowerbound;
};
#endif


#include <iostream>
#include <cstdio>
#include <ctime>
#include <armadillo>
#include "boost/program_options.hpp"

using namespace std;
using namespace arma;
namespace po = boost::program_options;

// m: numRows, n: numCols
inline double simpleDenseTest_Arma(int m, int n, int num_trials) {

  mat A = randu<mat>(m, n);
  mat B = randu<mat>(m, n);
  mat C = randu<mat>(m, n);
  mat D = randu<mat>(m, n);
  mat E = randu<mat>(m, n);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    mat res = ((A + B) / C - D) % E;

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;

}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double gemmSanityTest_Arma(int m, int n, int k, int num_trials) {

  mat A = randu<mat>(m, n);
  mat C = randu<mat>(n, k);
  mat E = randu<mat>(m, k);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    E += A * C;

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;

  return duration / num_trials;

}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double gemmDenseTest_Arma(int m, int n, int k, int num_trials) {

  mat A = randu<mat>(m, n);
  mat B = randu<mat>(m, n);
  mat C = randu<mat>(n, k);
  mat D = randu<mat>(n, k);
  mat E = randu<mat>(m, k);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    E += (A + B) * (C - D);

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;

  return duration / num_trials;

}

inline double mulDenseTest_Arma(int a, int b, int c, int d, int num_trials) {

  mat A = randu<mat>(a, a);
  mat B = randu<mat>(a, b);
  mat C = randu<mat>(b, c);
  mat D = randu<mat>(c, d);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    mat res = A * B * C * D;

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;
}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double denseVectorTest_Arma(int l, int num_trials) {

  vec a = randu<vec>(l);
  vec b = randu<vec>(l);
  vec c = randu<vec>(l);
  vec d = randu<vec>(l);
  vec e = randu<vec>(l);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {
    e = a + b + c + d;
  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;

  return duration / num_trials;
}

void runArmaTests(int num_trials, int l, int m, int n, int k, int a, int b, int c, int d,
    bool skip_vec, bool skip_simple, bool skip_gemm, bool skip_mult) {

  if (!skip_vec)
    cout << "Armadillo Vectors Test:\t" << denseVectorTest_Arma(l, num_trials) << endl;
  if (!skip_simple)
    cout << "Armadillo Simple Test:\t" << simpleDenseTest_Arma(m, n, num_trials) << endl;
  if (!skip_gemm) {
    cout << "Armadillo gemmSanity Test:\t" << gemmDenseTest_Arma(m, n, k, num_trials) << endl;
    cout << "Armadillo gemm Test:\t" << gemmDenseTest_Arma(m, n, k, num_trials) << endl;
  }
  if (!skip_mult)
    cout << "Armadillo mulDense Test:\t" << mulDenseTest_Arma(a, b, c, d, num_trials) << endl;

}

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

    int l, m, n, k, a, b, c, d, trials;
    bool skip_vec, skip_simple, skip_gemm, skip_mult;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("l", po::value<int>(&l)->default_value(1048576),
                    "length of vectors in vector addition test")
        ("m", po::value<int>(&m)->default_value(1024),
            "numRows of matrices in Simple Test, and gemm Test")
        ("n", po::value<int>(&n)->default_value(1024),
            "numCols of matrices in Simple Test, and gemm Test")
        ("k", po::value<int>(&k)->default_value(1024),
            "numCols of B in gemm Test")
        ("trials", po::value<int>(&trials)->default_value(10), "number of trials")
        ("a", po::value<int>(&a)->default_value(1024),
            "size matrix A in mulDense Test")
        ("b", po::value<int>(&b)->default_value(512),
            "size matrix B in mulDense Test")
        ("c", po::value<int>(&c)->default_value(256),
            "size matrix C in mulDense Test")
        ("d", po::value<int>(&d)->default_value(128),
            "size matrix D in mulDense Test")
        ("skip-vec", po::value<bool>(&skip_vec)->default_value(false),
            "skip vectors Test")
        ("skip-simple", po::value<bool>(&skip_simple)->default_value(false),
            "skip simple Test")
        ("skip-gemm", po::value<bool>(&skip_gemm)->default_value(false),
            "skip gemm Tests")
        ("skip-mult", po::value<bool>(&skip_mult)->default_value(false),
            "skip mulDense Test")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    runArmaTests(trials, l, m, n, k, a, b, c, d, skip_vec, skip_simple, skip_gemm, skip_mult);

    return 0;
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix4d X = Matrix4d::Random(4,4);
Matrix4d A = X + X.transpose();
cout << "Here is a random symmetric 4x4 matrix:" << endl << A << endl;
Tridiagonalization<Matrix4d> triOfA(A);
Matrix4d pm = triOfA.packedMatrix();
cout << "The packed matrix M is:" << endl << pm << endl;
cout << "The diagonal and subdiagonal corresponds to the matrix T, which is:"
     << endl << triOfA.matrixT() << endl;

  return 0;
}


#include "include/MMVII_all.h"
#include <boost/math/special_functions/fpclassify.hpp>

namespace MMVII
{

// return the variance of  exponential distribution of parameter "a" ( i.e proportiona to  "a^|x|")
double Sigma2FromFactExp(double a)
{
   return (2*a) / Square(1-a);
}

// return the value of "a" such that  exponential distribution of parameter "a" has variance S2
// i.e. this the "inverse" function of Sigma2FromFactExp

double FactExpFromSigma2(double aS2)
{
    return (aS2+1 - sqrt(Square(aS2+1)-Square(aS2))  ) / aS2 ;
}

/* *********************************************** */
/*                                                 */
/*        cComputeStdDev<Dim>                     */
/*                                                 */
/* *********************************************** */

#if (0)
///  Class to compute non biased variance from a statisic

/** Class to compute non biased variance from a statisic
    This generalise the standard formula
            EstimVar = N/(N-1) EmpirVar
    To the case where there is a weighting.

    It can be uses with N variable to factorize the computation on Weight
 */

template <const int Dim> class cComputeStdDev
{
    public :
        typedef  double tTab[Dim];

        cComputeStdDev();

        void Add(const  double * aVal,const double & aPds);
        const double  *  ComputeUnBiasedVar() ;
        const double  *  ComputeBiasedVar() ;
        double  DeBiasFactor() const;

    private :
        double    mSomW; ///< Sum of Weight
        double    mSomWW; ///< Sum of Weight ^2
        tTab      mSomWV;  ///< Weighted som of vals
        tTab      mSomWVV; ///< Weighted som of vals ^2
        tTab      mVar;   ///< Buffer to compute the unbiased variance
        tTab      mBVar;   ///< Buffer to compute the empirical variance
};
#endif

template  <const int Dim> cComputeStdDev<Dim>::cComputeStdDev() :
    mSomW   (0.0),
    mSomWW  (0.0)
{
    for (int aD=0 ; aD<Dim ; aD++)
    {
        mSomWV[aD] = 0.0;
        mSomWVV[aD] = 0.0;
    }
}

template  <const int Dim> void cComputeStdDev<Dim>::Add(const  double *  aVal,const double & aPds)
{
    mSomW += aPds;
    mSomWW += Square(aPds);
    for (int aD=0 ; aD<Dim ; aD++)
    {
        mSomWV[aD] += aPds * aVal[aD];
        mSomWVV[aD] += aPds * Square(aVal[aD]);
    }
}
template  <const int Dim>  double cComputeStdDev<Dim>::DeBiasFactor() const
{
    MMVII_INTERNAL_ASSERT_strong(mSomW>0,"No value in DeBiasFactor");
    return   1 - mSomWW/Square(mSomW);
}

template  <const int Dim> bool    cComputeStdDev<Dim>::OkForUnBiasedVar() const
{
   return (mSomW>0)  && (DeBiasFactor()!=0);
}

template  <const int Dim> const double *   cComputeStdDev<Dim>::ComputeUnBiasedVar()
{
    /* At least, this formula is correct :
         - when all weight are equal => 1-1/N
         - when all but one weight are 0 => 0
         - and finally if all are equal or 0
    */
    double aDebias = DeBiasFactor();
    MMVII_INTERNAL_ASSERT_strong(aDebias>0,"No var can be computed in ComputeVar");

    for (int aD=0 ; aD<Dim ; aD++)
    {
        double aAver = mSomWV[aD] / mSomW;
        double aVar  = mSomWVV[aD] / mSomW;
        aVar = aVar - Square(aAver);
        mVar[aD] = std::max(0.0,aVar) /aDebias;
    }

   return mVar;
}

template  <const int Dim> const double *   cComputeStdDev<Dim>::ComputeBiasedVar()
{
    for (int aD=0 ; aD<Dim ; aD++)
    {
        double aAver = mSomWV[aD] / mSomW;
        double aVar  = mSomWVV[aD] / mSomW;
        aVar = aVar - Square(aAver);
        mBVar[aD] = std::max(0.0,aVar);
    }

   return mBVar;
}

template class cComputeStdDev<1>;

void BenchUnbiasedStdDev()
{
    for (int aNbTest = 0 ; aNbTest<1000 ; aNbTest++)
    {
         int aNbVar = 1 + RandUnif_N(3);
         int aNbTir = aNbVar;
         int aNbComb = pow(aNbVar,aNbTir);
         std::vector<double> aVecVals = VRandUnif_0_1(aNbVar);
         std::vector<double> aVecWeight = VRandUnif_0_1(aNbVar);
// aVecWeight =  std::vector<double> (aNbVar,1.0);

         // compute the average of all empirical variance
         double aMoyVar=0;
         for (int aFlag=0 ; aFlag < aNbComb ; aFlag++) // Explore all combinaison
         {
             cComputeStdDev<1> aUBS;
             for (int aVar=0 ; aVar < aNbVar ; aVar++) // All Variable of this realization
             {
                 int aNumVar = (aFlag / round_ni(pow(aNbVar,aVar))) % aNbVar; // "Majic" formula to exdtrac p-adic decomp
                 aUBS.Add(&(aVecVals.at(aNumVar)),aVecWeight.at(aNumVar));
             }
             aMoyVar += aUBS.ComputeBiasedVar()[0];
         }
         aMoyVar /= aNbComb;

         cComputeStdDev<1> aUBS;
StdOut() << "WWW=" ;
         for (int aK=0 ; aK<aNbVar ; aK++)
         {
StdOut() << " " << aVecWeight.at(aK) ;
            aUBS.Add(&(aVecVals.at(aK)),aVecWeight.at(aK));
         }
StdOut() << "\n" ;
         StdOut() << "MOYVAR " << aMoyVar <<  " " <<  aUBS.DeBiasFactor() *  aUBS.ComputeBiasedVar()[0]
                  << " DBF=" <<  aUBS.DeBiasFactor() << "\n";
getchar();
    }
}

/* *********************************************** */
/*                                                 */
/*            Test Exp Filtre                      */
/*                                                 */
/* *********************************************** */

// Test Sigma2FromFactExp
template <class Type> void TestVarFilterExp(cPt2di aP0,cPt2di aP1,const Type & aV0,double aFx,double aFy,int aNbIt)
{
   cPt2di aPMil = (aP0+aP1) / 2;
   cRect2 aRect2(aP0,aP1);

   cIm2D<Type> aIm(aP0,aP1,nullptr,eModeInitImage::eMIA_Null);
   cDataIm2D<Type> & aDIm = aIm.DIm();

   // 1- Make 1 iteration of expon filter on a Direc, check variance and aver
   aDIm.InitDirac(aPMil,aV0);

   ExponentialFilter(true,aDIm,aNbIt,aRect2,aFx,aFy);
   cMatIner2Var<double>  aMat = StatFromImageDist(aDIm);

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S1() - aPMil.x()) < 1e-5  ,"Average Exp")
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S2() - aPMil.y()) < 1e-5  ,"Average Exp")

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S11()-Sigma2FromFactExp(aFx) * aNbIt)<1e-5,"Var Exp");
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S22()-Sigma2FromFactExp(aFy) * aNbIt)<1e-5,"Var Exp");
}

template <class Type> void TestVarFilterExp(cPt2di aSz,double aStdDev,int aNbIter,double aEps)
{
   cIm2D<Type> aIm(cPt2di(0,0),aSz,nullptr,eModeInitImage::eMIA_Null);
   cDataIm2D<Type> & aDIm = aIm.DIm();
   cPt2di aPMil = aSz / 2;
   double aV0 = 2.0;

   // 1- Make 1 iteration of expon filter on a Direc, check variance and aver
   aDIm.InitDirac(aPMil,aV0);

   ExpFilterOfStdDev(aDIm,aNbIter,aStdDev);
   cMatIner2Var<double>  aMat = StatFromImageDist(aDIm);

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S11()-Square(aStdDev))<aEps,"Std dev");
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S22()-Square(aStdDev))<aEps,"Std dev");
}

void BenchStat()
{
   TestVarFilterExp<double>(cPt2di(-2,2),cPt2di(400,375),2.0,0.6,0.67,1);
   TestVarFilterExp<double>(cPt2di(-2,2),cPt2di(400,375),2.0,0.6,0.67,3);

   // Test Sigma2FromFactExp
   for (int aK=1 ; aK<100 ; aK++)
   {
        double aS2 = aK / 5.0;
        double aF = FactExpFromSigma2(aS2);
        {
           // Check both formula are inverse of each others
           double aCheckS2 = Sigma2FromFactExp(aF);
           MMVII_INTERNAL_ASSERT_bench(std::abs( aS2 - aCheckS2 ) < 1e-5  ,"Sigma2FromFactExp")

        }
        {
           // Check formula Sigma2FromFactExp on exponential filters
           TestVarFilterExp<double>(cPt2di(0,0),cPt2di(4000,3),2.0,aF,0,1);
        }
   }

   TestVarFilterExp<double>(cPt2di(300,300),2.0,2,1e-6);
   TestVarFilterExp<float>(cPt2di(300,300),5.0,3,1e-3);
}

};


#define _wassert wassert_awf
#include <cassert>
#define _USE_MATH_DEFINES
#include <cmath>

#include <iostream>
#include <iomanip>

#include <Eigen/Eigen>
#include <Eigen/SparseQR>

#include "eigen_extras.h"

#include <unsupported/Eigen/LevenbergMarquardt>
#include <unsupported/Eigen/SparseExtra>
#include "unsupported/Eigen/src/SparseExtra/BlockSparseQR.h"
#include "unsupported/Eigen/src/SparseExtra/BlockDiagonalSparseQR.h"

#include "MeshTopology.h"
#include "SubdivEvaluator.h"
#include "log3d.h"

using namespace Eigen;

struct Subdiv3D_Functor : Eigen::SparseFunctor<Scalar>
{
  typedef Eigen::SparseFunctor<Scalar> Base;
  typedef typename Base::JacobianType JacobianType;

  // Input data
  Matrix3X data_points;

  // Topology (faces as vertex indices, fixed during shape optimization)
  MeshTopology mesh;

  SubdivEvaluator evaluator;

  // Functor constructor
  Subdiv3D_Functor(const Matrix3X& data_points, const MeshTopology& mesh) :
    Base(mesh.num_vertices*3 + data_points.cols()*2,   /* number of parameters */
         data_points.cols()*3),                        /* number of residuals */
    data_points(data_points),
    mesh(mesh),
    evaluator(mesh)
  {
    initWorkspace();
  }

  // Variables for optimization live in InputType
  struct InputType {
    Matrix3X control_vertices;
    std::vector<SurfacePoint> us;

    Index nVertices() const { return control_vertices.cols();  }
  };

  // And the optimization steps are computed using VectorType.
  // For subdivs (see xx), the correspondences are of type (int, Vec2) while the updates are of type (Vec2).
  // The iteractions between InputType and VectorType are restricted to:
  //   The Jacobian computation takeas an InputType, and its worows must easily convert to VectorType
  //   The increment_in_place operation takes InputType and StepType.
  typedef VectorX VectorType;

  // Workspace variables for evaluation
  Matrix3X S;
  Matrix3X dSdu;
  Matrix3X dSdv;
  SubdivEvaluator::triplets_t dSdX, dSudX, dSvdX;
  void initWorkspace()
  {
    Index nPoints = data_points.cols();
    S.resize(3, nPoints);
    dSdu.resize(3, nPoints);
    dSdv.resize(3, nPoints);
  }

  // Functor functions
  // 1. Evaluate the residuals at x
  int operator()(const InputType& x, ValueType& fvec) {
    evaluator.evaluateSubdivSurface(x.control_vertices, x.us, &S);

    // Fill residuals
    for (int i = 0; i < data_points.cols(); i++)
      fvec.segment(i * 3, 3) = S.col(i) - data_points.col(i);

    return 0;
  }

  // 2. Evaluate jacobian at x
  int df(const InputType& x, JacobianType& fjac)
  {
    // Evaluate surface at x
    evaluator.evaluateSubdivSurface(x.control_vertices, x.us, &S, &dSdX, &dSudX, &dSvdX, &dSdu, &dSdv);

    Index nPoints = data_points.cols();
    Index X_base = nPoints * 2;
    Index ubase = 0;

    // Fill Jacobian columns.
    // 1. Derivatives wrt control vertices.
    Eigen::TripletArray<Scalar, typename JacobianType::Index> jvals(nPoints * 3 * 3);
    for (int i = 0; i < dSdX.size(); ++i) {
      auto const& triplet = dSdX[i];
      assert(0 <= triplet.row() && triplet.row() < nPoints);
      assert(0 <= triplet.col() && triplet.col() < x.nVertices());
      jvals.add(triplet.row() * 3 + 0, X_base + triplet.col() * 3 + 0, triplet.value());
      jvals.add(triplet.row() * 3 + 1, X_base + triplet.col() * 3 + 1, triplet.value());
      jvals.add(triplet.row() * 3 + 2, X_base + triplet.col() * 3 + 2, triplet.value());
    }

    // 2. Derivatives wrt correspondences
    for (int i = 0; i < nPoints; i++) {
      jvals.add(3 * i + 0, ubase + 2 * i + 0, dSdu(0, i));
      jvals.add(3 * i + 1, ubase + 2 * i + 0, dSdu(1, i));
      jvals.add(3 * i + 2, ubase + 2 * i + 0, dSdu(2, i));

      jvals.add(3 * i + 0, ubase + 2 * i + 1, dSdv(0, i));
      jvals.add(3 * i + 1, ubase + 2 * i + 1, dSdv(1, i));
      jvals.add(3 * i + 2, ubase + 2 * i + 1, dSdv(2, i));
    }

    fjac.resize(3 * nPoints, 2 * nPoints + 3 * x.nVertices());
    fjac.setFromTriplets(jvals.begin(), jvals.end());
    fjac.makeCompressed();

    return 0;
  }

  void increment_in_place(InputType* x, StepType const& p)
  {
    Index nPoints = data_points.cols();
    Index X_base = nPoints * 2;
    Index ubase = 0;

    // Increment control vertices
    Index nVertices = x->nVertices();

    assert(p.size() == nVertices * 3 + nPoints * 2);
    assert(x->us.size() == nPoints);

    Map<VectorX>(x->control_vertices.data(), nVertices * 3) += p.tail(nVertices * 3);

    // Increment surface correspondences
    int loopers = 0;
    int totalhops = 0;
    for (int i = 0; i < nPoints; ++i) {
      Vector2 du = p.segment<2>(ubase + 2 * i);
      int nhops = increment_u_crossing_edges(x->control_vertices, x->us[i].face, x->us[i].u, du, &x->us[i].face, &x->us[i].u);
      if (nhops < 0)
        ++loopers;
      totalhops += std::abs(nhops);
    }
    if (loopers > 0)
      std::cerr << "[" << totalhops / Scalar(nPoints) << " hops, " << loopers << " points looped]";
    else if (totalhops > 0)
      std::cerr << "[" << totalhops << "/"  << Scalar(nPoints) << " hops]";
  }

  // "Mesh walking" to update correspondences, as in Fig 3, Taylor et al, CVPR 2014, "Hand shape.."
  int increment_u_crossing_edges(Matrix3X const& X, int face, const Vector2& u, const Vector2& du, int* new_face_out, Vector2* new_u_out)
  {
    const int MAX_HOPS = 7;

    Scalar u1_old = u[0];
    Scalar u2_old = u[1];
    Scalar du1 = du[0];
    Scalar du2 = du[1];
    Scalar u1_new = u1_old + du1;
    Scalar u2_new = u2_old + du2;

    for (int count = 0; ; ++count) {
      bool crossing = (u1_new < 0.f) || (u1_new > 1.f) || (u2_new < 0.f) || (u2_new > 1.f);

      if (!crossing) {
        *new_face_out = face;
        *new_u_out << u1_new, u2_new;
        return count;
      }

      //Find the new face	and the coordinates of the crossing point within the old face and the new face
      int face_new;

      bool face_found = false;

      Scalar dif, aux, u1_cross, u2_cross;

      if (u1_new < 0.f)
      {
        dif = u1_old;
        const Scalar u2t = u2_old - du2*dif / du1;
        if ((u2t >= 0.f) && (u2t <= 1.f))
        {
          face_new = mesh.face_adj(3, face); aux = u2t; face_found = true;
          u1_cross = 0.f; u2_cross = u2t;
        }
      }
      if ((u1_new > 1.f) && (!face_found))
      {
        dif = 1.f - u1_old;
        const Scalar u2t = u2_old + du2*dif / du1;
        if ((u2t >= 0.f) && (u2t <= 1.f))
        {
          face_new = mesh.face_adj(1, face); aux = 1.f - u2t; face_found = true;
          u1_cross = 1.f; u2_cross = u2t;
        }
      }
      if ((u2_new < 0.f) && (!face_found))
      {
        dif = u2_old;
        const Scalar u1t = u1_old - du1*dif / du2;
        if ((u1t >= 0.f) && (u1t <= 1.f))
        {
          face_new = mesh.face_adj(0, face); aux = 1.f - u1t; face_found = true;
          u1_cross = u1t; u2_cross = 0.f;
        }
      }
      if ((u2_new > 1.f) && (!face_found))
      {
        dif = 1.f - u2_old;
        const Scalar u1t = u1_old + du1*dif / du2;
        if ((u1t >= 0.f) && (u1t <= 1.f))
        {
          face_new = mesh.face_adj(2, face); aux = u1t; face_found = true;
          u1_cross = u1t; u2_cross = 1.f;
        }
      }
      assert(face_found);

      // Find the coordinates of the crossing point as part of the new face, and update u_old (as that will be new u in next iter).
      unsigned int conf;
      for (unsigned int f = 0; f < 4; f++)
        if (mesh.face_adj(f, face_new) == face) { conf = f; }

      switch (conf)
      {
      case 0: u1_old = aux; u2_old = 0.f; break;
      case 1: u1_old = 1.f; u2_old = aux; break;
      case 2:	u1_old = 1.f - aux; u2_old = 1.f; break;
      case 3:	u1_old = 0.f; u2_old = 1.f - aux; break;
      }

      // Evaluate the subdivision surface at the edge (with respect to the original face)
      std::vector<SurfacePoint> pts;
      pts.push_back({ face,{ u1_cross, u2_cross } });
      pts.push_back({ face_new, { u1_old, u2_old } });
      Matrix3X S(3, 2);
      Matrix3X Su(3, 2);
      Matrix3X Sv(3, 2);
      evaluator.evaluateSubdivSurface(X, pts, &S, 0, 0, 0, &Su, &Sv);

      Matrix<Scalar, 3, 2> J_Sa;
      J_Sa.col(0) = Su.col(0);
      J_Sa.col(1) = Sv.col(0);

      Matrix<Scalar, 3, 2> J_Sb;
      J_Sb.col(0) = Su.col(1);
      J_Sb.col(1) = Sv.col(1);

      //Compute the new u increments
      Vector2 du_remaining;
      du_remaining << u1_new - u1_cross, u2_new - u2_cross;
      Vector3 prod = J_Sa*du_remaining;
      Matrix22 AtA = J_Sb.transpose()*J_Sb;
      Vector2 AtB = J_Sb.transpose()*prod;

      //Vector2 du_new = AtA.ldlt().solve(AtB);
      Vector2  u_incr = AtA.inverse()*AtB;

      du1 = u_incr[0];
      du2 = u_incr[1];

      if (count == MAX_HOPS) {
        //std::cerr << "Problem!!! Many jumps between the mesh faces for the update of one correspondence. I remove the remaining u_increment!\n";
        auto dmax = std::max(du1, du2);
        Scalar scale = Scalar(0.5 / dmax);
        *new_face_out = face;
        //*new_u_out << u1_old + du1 * scale, u2_old + du2 * scale;
        *new_u_out << 0.5, 0.5;

        assert((*new_u_out)[0] >= 0 && (*new_u_out)[1] <= 1.0 && (*new_u_out)[1] >= 0 && (*new_u_out)[1] <= 1.0);
        return -count;
      }

      u1_new = u1_old + du1;
      u2_new = u2_old + du2;
      face = face_new;
    }
  }

  Scalar estimateNorm(InputType const& x, StepType const& diag)
  {
    Index nVertices = x.nVertices();
    Map<VectorX> xtop{ (Scalar*)x.control_vertices.data(), nVertices * 3 };
    double total = xtop.cwiseProduct(diag.tail(nVertices*3)).stableNorm();
    total = total*total;
    for (int i = 0; i < x.us.size(); ++i) {
      Vector2 const& u = x.us[i].u;
      Vector2 di = diag.segment<2>(2 * i);
      total += u.cwiseProduct(di).squaredNorm();
    }
    return Scalar(sqrt(total));
  }

  // 5. Describe the QR solvers
  // For generic Jacobian, one might use this Dense QR solver.
  typedef SparseQR<JacobianType, COLAMDOrdering<int> > GeneralQRSolver;

  // But for optimal performance, declare QRSolver that understands the sparsity structure.
  // Here it's block-diagonal LHS with dense RHS
  //
  // J1 = [J11   0   0 ... 0
  //         0 J12   0 ... 0
  //                   ...
  //         0   0   0 ... J1N];
  // And
  // J = [J1 J2];

  // QR for J1 subblocks is 2x1
  typedef ColPivHouseholderQR<Matrix<Scalar, 3, 2> > DenseQRSolver3x2;

  // QR for J1 is block diagonal
  typedef BlockDiagonalSparseQR<JacobianType, DenseQRSolver3x2> LeftSuperBlockSolver;

  // QR for J1'J2 is general dense (faster than general sparse by about 1.5x for n=500K)
  typedef ColPivHouseholderQR<Matrix<Scalar, Dynamic, Dynamic> > RightSuperBlockSolver;

  // QR for J is concatenation of the above.
  typedef BlockSparseQR<JacobianType, LeftSuperBlockSolver, RightSuperBlockSolver> SchurlikeQRSolver;

  typedef SchurlikeQRSolver QRSolver;

  // And tell the algorithm how to set the QR parameters.
  void initQRSolver(SchurlikeQRSolver &qr) {
    // set block size
    qr.setBlockParams(data_points.cols() * 2);
    qr.getLeftSolver().setSparseBlockParams(3, 2);
  }
};

void logmesh(log3d& log, MeshTopology const& mesh, Matrix3X const& vertices)
{
  Matrix3Xi tris(3, mesh.quads.cols() * 2);
  tris.block(0, 0, 1, mesh.quads.cols()) = mesh.quads.row(0);
  tris.block(1, 0, 1, mesh.quads.cols()) = mesh.quads.row(2);
  tris.block(2, 0, 1, mesh.quads.cols()) = mesh.quads.row(1);
  tris.block(0, mesh.quads.cols(), 1, mesh.quads.cols()) = mesh.quads.row(0);
  tris.block(1, mesh.quads.cols(), 1, mesh.quads.cols()) = mesh.quads.row(3);
  tris.block(2, mesh.quads.cols(), 1, mesh.quads.cols()) = mesh.quads.row(2);
  log.mesh(tris, vertices);
}

void logsubdivmesh(log3d& log, MeshTopology const& mesh, Matrix3X const& vertices)
{
  log.wiremesh(mesh.quads, vertices);
  SubdivEvaluator evaluator(mesh);
  MeshTopology refined_mesh;
  Matrix3X refined_verts;
  evaluator.generate_refined_mesh(vertices, 3, &refined_mesh, &refined_verts);
  logmesh(log, refined_mesh, refined_verts);
}

int main()
{
  std::cout << "Go\n";
  log3d log("log3d.html", "fit-subdiv-to-3d-points");
  log.ArcRotateCamera();
  log.axes();

  // CREATE DATA SAMPLES
  int nDataPoints = 200;
  Matrix3X data(3, nDataPoints);
  for (int i = 0; i < nDataPoints; i++) {
    if (0) {
      float t = float(i) / float(nDataPoints);
      data(0, i) = 0.1f + 1.3f*cos(80*t);
      data(1, i) = -0.2f + 0.7f*sin(80*t);
      data(2, i) = t;
    }
    else {
      Scalar t = rand() / Scalar(RAND_MAX);
      Scalar s = rand() / Scalar(RAND_MAX);

      auto u = Scalar(2 * EIGEN_PI * t);
      auto v = Scalar(EIGEN_PI * (s - 0.5));
      data(0, i) = 0.1f + 1.3f*cos(u)*cos(v);
      data(1, i) = -0.2f + 0.7f*sin(u)*cos(v);
      data(2, i) = sin(v);
    }
    if (1)
      log.position(log.CreateSphere(0, 0.02), data(0, i), data(1, i), data(2, i));
    else
      log.star(data.col(i));
  }

  MeshTopology mesh;
  Matrix3X control_vertices_gt;
  makeCube(&mesh, &control_vertices_gt);
  int nFaces = int(mesh.quads.cols());

  // INITIAL PARAMS
  typedef Subdiv3D_Functor Functor;

  Functor::InputType params;
  params.control_vertices = control_vertices_gt + 0.1 * MatrixXX::Random(3, control_vertices_gt.cols());
  params.us.resize(nDataPoints);

  // Initialize uvs.
  {
    // 1. Make a list of test points, e.g. centre point of each face
    Matrix3X test_points(3, nFaces);
    std::vector<SurfacePoint> uvs{ size_t(nFaces),{ 0,{ 0.5, 0.5 } } };
    for (int i = 0; i < nFaces; ++i)
      uvs[i].face = i;

    SubdivEvaluator evaluator(mesh);
    evaluator.evaluateSubdivSurface(params.control_vertices, uvs, &test_points);

    for (int i = 0; i < nDataPoints; i++) {
      // Closest test point
      Eigen::Index test_pt_index;
      (test_points.colwise() - data.col(i)).colwise().squaredNorm().minCoeff(&test_pt_index);
      params.us[i] = uvs[test_pt_index];
    }
  }

  logsubdivmesh(log, mesh, params.control_vertices);

  Functor functor(data, mesh);

  // Check Jacobian
  if (0)
  for (float eps = 1e-8f; eps < 1.1e-3f; eps*=10.f) {
    NumericalDiff<Functor> fd{ functor, Functor::Scalar(eps) };
    Functor::JacobianType J;
    Functor::JacobianType J_fd;
    functor.df(params, J);
    fd.df(params, J_fd);
    double diff = (J - J_fd).norm();
    if (diff > 0) {
      std::cerr << "Jacobian diff(eps=" << eps <<"), = " << diff << std::endl;
      write(J, "c:\\tmp\\J.txt");
      write(J_fd, "c:\\tmp\\J_fd.txt");
    }
  }

  Eigen::LevenbergMarquardt< Functor > lm(functor);
  lm.setVerbose(true);
  lm.setMaxfev(10);

  Eigen::LevenbergMarquardtSpace::Status info = lm.minimize(params);
  log.color(0, 1, 0);
  logsubdivmesh(log, mesh, params.control_vertices);

  std::cerr << "Done: err = "<< lm.fnorm() <<"\n";

  // Now, on a refined mesh.
  if (1) {
    log.color(.5, .8, 0);

    MeshTopology mesh1;
    Matrix3X verts1;
    SubdivEvaluator evaluator(mesh);
    evaluator.generate_refined_mesh(params.control_vertices, 1, &mesh1, &verts1);

    {
      log3d log2("log2.html");
      log2.ArcRotateCamera();
      log2.axes();
      log2.wiremesh(mesh1.quads, verts1);
    }

    params.control_vertices = verts1;

    // Initialize uvs.
    {
      // 1. Make a list of test points, e.g. centre point of each face
      int nFaces = mesh1.num_faces();
      Matrix3X test_points(3, nFaces);
      std::vector<SurfacePoint> uvs{ size_t(nFaces),{ 0,{ 0.5, 0.5 } } };
      for (int i = 0; i < nFaces; ++i)
        uvs[i].face = i;

      SubdivEvaluator evaluator(mesh1);
      evaluator.evaluateSubdivSurface(params.control_vertices, uvs, &test_points);

      for (int i = 0; i < nDataPoints; i++) {
        // Closest test point
        Eigen::Index test_pt_index;
        (test_points.colwise() - data.col(i)).colwise().squaredNorm().minCoeff(&test_pt_index);
        params.us[i] = uvs[test_pt_index];
      }
    }

    Functor functor1(data, mesh1);

    Eigen::LevenbergMarquardt< Functor > lm(functor1);
    lm.setVerbose(true);
    lm.setMaxfev(40);

    Eigen::LevenbergMarquardtSpace::Status info = lm.minimize(params);
    logsubdivmesh(log, mesh1, params.control_vertices);

    std::cerr << "Done: err = " << lm.fnorm() << "\n";
  }
}

// Override system assert so one can set a breakpoint in it rather than clicking "Retry" and "Break"
void __cdecl _wassert(_In_z_ wchar_t const* _Message,_In_z_ wchar_t const* _File, _In_ unsigned _Line)
{
  std::wcerr << _File << "(" << _Line << "): ASSERT FAILED [" << _Message << "]\n";

  abort();
}


#include <math/distributions/gammadistribution.hpp>
#include <boost/math/special_functions/gamma.hpp>

namespace QLExtension {

	InverseCumulativeGamma::InverseCumulativeGamma(Real shape, Real scale)
		: dist_(boost::math::gamma_distribution<>(shape, scale))
	{}

	Real InverseCumulativeGamma::operator()(Real x) const {
		return boost::math::quantile(dist_, x);
	}
}


/*
 * This example was gathered from Stack Overflow. Only small modification were introduced.
 *
 * Check here: https://stackoverflow.com/questions/35656237/dynamic-eigen-vectors-in-boostodeint
 *
 * Credit: Sjonnie (user nickname)
 */

#include <iostream>
#include <Eigen/Core>
#include <cstdlib>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_algebra.hpp>

using namespace boost::numeric::odeint;

typedef Eigen::VectorXd state_type;

Eigen::MatrixXd A;

void ODE_function (const state_type &x, state_type &dxdt, const double &t)
{
    dxdt = A * x;
}

void write_states(const state_type &x, const double &t)
{
    std::cout << t << "\t";
    for (int i = 0; i < x.size(); i++)
    {
        std::cout << *(x.data()+i) << "\t";
    }
    std::cout << std::endl;
}

int main()
{
    int nr_of_states = 10;
    state_type x;

    std::cout << "Simulation with " << nr_of_states << " states\n";

    x = state_type(nr_of_states);
    A = Eigen::MatrixXd(nr_of_states, nr_of_states);

    srand(365);

    for (int i = 0; i < A.size(); i++)
    {
        *(A.data()+i) = ((double)rand()/(double)RAND_MAX);
    }

    for (int i = 0; i < x.size(); i++)
    {
        *(x.data()+i) = i;
    }

    typedef runge_kutta_dopri5<state_type, double, state_type, double, vector_space_algebra> stepper;
    integrate_adaptive(stepper(), ODE_function, x, 0.0, 25.0, 0.1, write_states);

    std::cout <<std::endl << "final state vector: " << std::endl << x << std::endl;

    return 0;
}


#include <Eigen/Dense>
#include <cmath>
#include <vector>
#include "Adam.hpp"

#define EIGEN_MPL2_ONLY
using namespace Eigen;
using namespace std;

Adam::Adam(int _N,int _d,MatrixXd _x,VectorXd _label,double _C,double _alpha,double _beta1,double _beta2,double _epsilon,double _lambda,int iter):
  N(_N),
  d(_d),
  alpha(_alpha),
  beta1(_beta1),
  beta2(_beta2),
  epsilon(_epsilon),
  lambda(_lambda),
  C(_C),
  X(_x),
  m(VectorXd::Zero(d)),
  v(VectorXd::Zero(d)),
  label(_label),
  iteration(iter),
  w(VectorXd::Zero(d))
{}

double Adam::Acc(vector<double>& pred,VectorXd &l){
  int t =0;
  double loss =0;
  for(int i = 0; i< pred.size();i++){
    loss += (l(i) - pred[i]) * (l(i) - pred[i]);
    int s = pred[i] > 0.5 ? 1 : 0;
    if(s == l(i)){
      t++;
    }
  }
  return (double)t/pred.size();
}

void Adam::train(){
  for(int iter = 0; iter< iteration; iter++){
    for(int i = 0; i < N; i++){
      double pred = sigma(X,i);
      for(int idx = 0; idx < d; idx++){
        if(X(i,idx) != 0){
          double tbeta = 1-(1-beta1) * pow(lambda,iter);
          double grad = (pred - label(i)) * X(i,idx)+ C * w(idx);
          m[idx] = tbeta * grad + (1-tbeta) * m[idx];
          v[idx] = beta2*pow(grad,2) + (1-beta2) * v[idx];
          double hat_m = m[idx]/(1-pow((1-beta1),iter+1));
          double hat_v = v[idx]/(1-pow((1-beta2),iter+1));
          w(idx) -= alpha*hat_m/(sqrt(hat_v) + epsilon);
        }
      }
    }
    vector<double> ret;
    predict(X,label,ret);
    iterscores.push_back(Acc(ret,label));
  }
}

double Adam::sigma(const MatrixXd& _x, int i){
  return sigmoid(_x.row(i).dot(w));
}

double Adam::sigmoid(double z){
  return 1.0 / (1.0 + exp(-z));
}

void Adam::predict(MatrixXd& _x,VectorXd& _l,vector<double>& ret){
  for(int i = 0; i < _x.rows(); i++){
    ret.push_back(sigma(_x,i));
  }
}


#include <iostream>
#include <Eigen/Dense>

int main()
{
  Eigen::MatrixXd xn;
  Eigen::MatrixXd mu;
  Eigen::VectorXi gk;
  Eigen::VectorXi numk;
  Eigen::MatrixXd::Index minIndex;
  int ni;
  int nn;
  int kk;
  int i;
  int j;
  int l;
  int sum_gk;
  int bef_sum_gk;

  // after :: from file input
  ni = 10;
  kk = 3;

  nn = ni*kk;
  std::cout << "nn " <<  nn << std::endl;

  xn = Eigen::MatrixXd::Random(2, nn);
  gk = Eigen::VectorXi(nn);
  numk = Eigen::VectorXi::Zero(kk);
  std::cout << "xn =" << std::endl << xn << std::endl;

  xn.block(0, nn/kk, 1, nn/kk).array() += 2.0;
  xn.block(0, 2*nn/kk, 1, nn/kk).array() += 4.0;

  mu = xn.block(0, 0, 2, kk);

  std::cout << "mu =" << std::endl << mu << std::endl;

  bef_sum_gk = 0;
  for(i = 0; i < 100; i++){
    std::cout << "loop =" << i << std::endl;
    for(j = 0; j < nn; j++){

// Is which better?
      (mu.colwise() - xn.col(j)).colwise().squaredNorm().minCoeff(&minIndex);
//      (xn.col(j).replicate(1, 3) - mu).colwise().squaredNorm().minCoeff(&minIndex);

      gk(j) = minIndex;
    }
    numk.array() = 0;
    for(l = 0; l < kk; l++){
      mu.col(l).array() = 0.0;
      for(j = 0; j < nn; j++){
	if(gk(j) == l){
	  numk(l) += 1;
	  mu.col(l) += xn.col(j);
	}
      }
// Is which better?
      mu.col(l).array() /= static_cast<double> (numk(l));
//      mu.col(l) /= static_cast<double> (numk(l));
    }
    sum_gk = gk.sum();
    if(bef_sum_gk == sum_gk) break;
    bef_sum_gk = sum_gk;
  }

  std::cout << "numk =" << std::endl << numk << std::endl;
  std::cout << "gk =" << std::endl << gk << std::endl;
  std::cout << "mu =" << std::endl << mu << std::endl;
}