/**
 * @file kde_test.cpp
 * @author Roberto Hueso
 *
 * Test mlpackMain() of kde_main.cpp
 *
 * 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 <string>

#define BINDING_TYPE BINDING_TYPE_TEST

static const std::string testName = "KDE";

#include <mlpack/core.hpp>
#include <mlpack/core/util/mlpack_main.hpp>
#include "test_helper.hpp"
#include <mlpack/methods/kde/kde_main.cpp>

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

using namespace mlpack;

struct KDETestFixture
{
 public:
  KDETestFixture()
  {
      // Cache in the options for this program.
      CLI::RestoreSettings(testName);
  }

  ~KDETestFixture()
  {
      // Clear the settings.
      CLI::ClearSettings();
  }
};

void ResetKDESettings()
{
  CLI::ClearSettings();
  CLI::RestoreSettings(testName);
}

BOOST_FIXTURE_TEST_SUITE(KDEMainTest, KDETestFixture);

/**
  * Ensure that the estimations we get for KDEMain, are the same as the ones we
  * get from the KDE class without any wrappers. Requires normalization.
 **/
BOOST_AUTO_TEST_CASE(KDEGaussianRTreeResultsMain)
{
  // Datasets.
  arma::mat reference = arma::randu(3, 500);
  arma::mat query = arma::randu(3, 100);
  arma::vec kdeEstimations, mainEstimations;
  double kernelBandwidth = 1.5;
  double relError = 0.05;

  kernel::GaussianKernel kernel(kernelBandwidth);
  metric::EuclideanDistance metric;
  KDE<kernel::GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::RTree>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, kdeEstimations);
  // Normalize estimations.
  kdeEstimations /= kernel.Normalizer(reference.n_rows);

  // Main estimations.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("kernel", std::string("gaussian"));
  SetInputParam("tree", std::string("r-tree"));
  SetInputParam("rel_error", relError);
  SetInputParam("bandwidth", kernelBandwidth);

  mlpackMain();

  mainEstimations = std::move(CLI::GetParam<arma::vec>("predictions"));

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(kdeEstimations[i], mainEstimations[i], relError);
}

/**
  * Ensure that the estimations we get for KDEMain, are the same as the ones we
  * get from the KDE class without any wrappers. Doesn't require normalization.
 **/
BOOST_AUTO_TEST_CASE(KDETriangularBallTreeResultsMain)
{
  // Datasets.
  arma::mat reference = arma::randu(3, 300);
  arma::mat query = arma::randu(3, 100);
  arma::vec kdeEstimations, mainEstimations;
  double kernelBandwidth = 3.0;
  double relError = 0.06;

  kernel::TriangularKernel kernel(kernelBandwidth);
  metric::EuclideanDistance metric;
  KDE<kernel::TriangularKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::BallTree>
      kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, kdeEstimations);

  // Main estimations.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("kernel", std::string("triangular"));
  SetInputParam("tree", std::string("ball-tree"));
  SetInputParam("rel_error", relError);
  SetInputParam("bandwidth", kernelBandwidth);

  mlpackMain();

  mainEstimations = std::move(CLI::GetParam<arma::vec>("predictions"));

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(kdeEstimations[i], mainEstimations[i], relError);
}

/**
  * Ensure that the estimations we get for KDEMain, are the same as the ones we
  * get from the KDE class without any wrappers in the monochromatic case.
 **/
BOOST_AUTO_TEST_CASE(KDEMonoResultsMain)
{
  // Datasets.
  arma::mat reference = arma::randu(2, 300);
  arma::vec kdeEstimations, mainEstimations;
  double kernelBandwidth = 2.3;
  double relError = 0.05;

  kernel::EpanechnikovKernel kernel(kernelBandwidth);
  metric::EuclideanDistance metric;
  KDE<kernel::EpanechnikovKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::StandardCoverTree>
    kde(relError, 0.0, kernel, KDEMode::DUAL_TREE_MODE, metric);
  kde.Train(reference);
  // Perform monochromatic KDE.
  kde.Evaluate(kdeEstimations);
  // Normalize.
  kdeEstimations /= kernel.Normalizer(reference.n_rows);

  // Main estimations.
  SetInputParam("reference", reference);
  SetInputParam("kernel", std::string("epanechnikov"));
  SetInputParam("tree", std::string("cover-tree"));
  SetInputParam("rel_error", relError);
  SetInputParam("bandwidth", kernelBandwidth);

  mlpackMain();

  mainEstimations = std::move(CLI::GetParam<arma::vec>("predictions"));

  // Check whether results are equal.
  for (size_t i = 0; i < reference.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(kdeEstimations[i], mainEstimations[i], relError);
}

/**
  * Ensuring that absence of input data is checked.
 **/
BOOST_AUTO_TEST_CASE(KDENoInputData)
{
  // No input data is not provided. Should throw a runtime error.
  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
  * Check result has as many densities as query points.
 **/
BOOST_AUTO_TEST_CASE(KDEOutputSize)
{
  const size_t dim = 3;
  const size_t samples = 110;
  arma::mat reference = arma::randu<arma::mat>(dim, 325);
  arma::mat query = arma::randu<arma::mat>(dim, samples);

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);

  mlpackMain();
  // Check number of output elements.
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::vec>("predictions").size(), samples);
}

/**
  * Check that saved model can be reused.
 **/
BOOST_AUTO_TEST_CASE(KDEModelReuse)
{
  const size_t dim = 3;
  const size_t samples = 100;
  const double relError = 0.05;
  arma::mat reference = arma::randu<arma::mat>(dim, 300);
  arma::mat query = arma::randu<arma::mat>(dim, samples);

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("bandwidth", 2.4);
  SetInputParam("rel_error", 0.05);

  mlpackMain();

  arma::vec oldEstimations = std::move(CLI::GetParam<arma::vec>("predictions"));

  // Change parameters and load model.
  CLI::GetSingleton().Parameters()["reference"].wasPassed = false;
  SetInputParam("query", query);
  SetInputParam("input_model",
      std::move(CLI::GetParam<KDEModel*>("output_model")));

  mlpackMain();

  arma::vec newEstimations = std::move(CLI::GetParam<arma::vec>("predictions"));

  // Check estimations are the same.
  for (size_t i = 0; i < samples; ++i)
    BOOST_REQUIRE_CLOSE(oldEstimations[i], newEstimations[i], relError);
}

/**
  * Ensure that the estimations we get for KDEMain, are the same as the ones we
  * get from the KDE class without any wrappers using single-tree mode.
 **/
BOOST_AUTO_TEST_CASE(KDEGaussianSingleKDTreeResultsMain)
{
  // Datasets.
  arma::mat reference = arma::randu(3, 400);
  arma::mat query = arma::randu(3, 400);
  arma::vec kdeEstimations, mainEstimations;
  double kernelBandwidth = 3.0;
  double relError = 0.06;

  kernel::GaussianKernel kernel(kernelBandwidth);
  metric::EuclideanDistance metric;
  KDE<kernel::GaussianKernel,
      metric::EuclideanDistance,
      arma::mat,
      tree::BallTree>
      kde(relError, 0.0, kernel, KDEMode::SINGLE_TREE_MODE, metric);
  kde.Train(reference);
  kde.Evaluate(query, kdeEstimations);
  kdeEstimations /= kernel.Normalizer(reference.n_rows);

  // Main estimations.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("kernel", std::string("gaussian"));
  SetInputParam("tree", std::string("kd-tree"));
  SetInputParam("algorithm", std::string("single-tree"));
  SetInputParam("rel_error", relError);
  SetInputParam("bandwidth", kernelBandwidth);

  mlpackMain();

  mainEstimations = std::move(CLI::GetParam<arma::vec>("predictions"));

  // Check whether results are equal.
  for (size_t i = 0; i < query.n_cols; ++i)
    BOOST_REQUIRE_CLOSE(kdeEstimations[i], mainEstimations[i], relError);
}

/**
  * Ensure we get an exception when an invalid kernel is specified.
 **/
BOOST_AUTO_TEST_CASE(KDEMainInvalidKernel)
{
  arma::mat reference = arma::randu<arma::mat>(2, 10);
  arma::mat query = arma::randu<arma::mat>(2, 5);

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("kernel", std::string("linux"));

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
  * Ensure we get an exception when an invalid tree is specified.
 **/
BOOST_AUTO_TEST_CASE(KDEMainInvalidTree)
{
  arma::mat reference = arma::randu<arma::mat>(2, 10);
  arma::mat query = arma::randu<arma::mat>(2, 5);

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("tree", std::string("olive"));

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
  * Ensure we get an exception when an invalid algorithm is specified.
 **/
BOOST_AUTO_TEST_CASE(KDEMainInvalidAlgorithm)
{
  arma::mat reference = arma::randu<arma::mat>(2, 10);
  arma::mat query = arma::randu<arma::mat>(2, 5);

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("algorithm", std::string("bogosort"));

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
  * Ensure we get an exception when both reference and input_model are
  * specified.
 **/
BOOST_AUTO_TEST_CASE(KDEMainReferenceAndModel)
{
  arma::mat reference = arma::randu<arma::mat>(2, 10);
  arma::mat query = arma::randu<arma::mat>(2, 5);
  KDEModel* model = new KDEModel();

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("input_model", model);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
  * Ensure we get an exception when an invalid absolute error is specified.
 **/
BOOST_AUTO_TEST_CASE(KDEMainInvalidAbsoluteError)
{
  arma::mat reference = arma::randu<arma::mat>(1, 10);
  arma::mat query = arma::randu<arma::mat>(1, 5);

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);

  Log::Fatal.ignoreInput = true;
  // Invalid value.
  SetInputParam("abs_error", -0.1);
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);

  // Valid value.
  SetInputParam("abs_error", 5.8);
  BOOST_REQUIRE_NO_THROW(mlpackMain());
  Log::Fatal.ignoreInput = false;
}

/**
  * Ensure we get an exception when an invalid relative error is specified.
 **/
BOOST_AUTO_TEST_CASE(KDEMainInvalidRelativeError)
{
  arma::mat reference = arma::randu<arma::mat>(1, 10);
  arma::mat query = arma::randu<arma::mat>(1, 5);

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);

  Log::Fatal.ignoreInput = true;
  // Invalid under 0.
  SetInputParam("rel_error", -0.1);
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);

  // Invalid over 1.
  SetInputParam("rel_error", 1.1);
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);

  // Valid value.
  SetInputParam("rel_error", 0.3);
  BOOST_REQUIRE_NO_THROW(mlpackMain());
  Log::Fatal.ignoreInput = false;
}

/**
  * Ensure we get an exception when an invalid Monte Carlo probability is
  * specified.
 **/
BOOST_AUTO_TEST_CASE(KDEMainInvalidMCProbability)
{
  arma::mat reference = arma::randu<arma::mat>(1, 10);
  arma::mat query = arma::randu<arma::mat>(1, 5);

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("kernel", std::string("gaussian"));
  SetInputParam("monte_carlo", true);

  Log::Fatal.ignoreInput = true;
  // Invalid under 0.
  SetInputParam("mc_probability", -0.1);
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);

  // Invalid over 1.
  SetInputParam("mc_probability", 1.1);
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);

  // Valid value.
  SetInputParam("mc_probability", 0.3);
  BOOST_REQUIRE_NO_THROW(mlpackMain());
  Log::Fatal.ignoreInput = false;
}

/**
  * Ensure we get an exception when an invalid Monte Carlo initial sample size
  * is specified.
 **/
BOOST_AUTO_TEST_CASE(KDEMainInvalidMCInitialSampleSize)
{
  arma::mat reference = arma::randu<arma::mat>(1, 10);
  arma::mat query = arma::randu<arma::mat>(1, 5);

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("kernel", std::string("gaussian"));
  SetInputParam("monte_carlo", true);

  Log::Fatal.ignoreInput = true;
  // Invalid under 0.
  SetInputParam("initial_sample_size", -1);
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);

  // Invalid 0.
  SetInputParam("initial_sample_size", 0);
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);

  // Valid value.
  SetInputParam("initial_sample_size", 20);
  BOOST_REQUIRE_NO_THROW(mlpackMain());
  Log::Fatal.ignoreInput = false;
}

/**
  * Ensure we get an exception when an invalid Monte Carlo entry coefficient
  * is specified.
 **/
BOOST_AUTO_TEST_CASE(KDEMainInvalidMCEntryCoef)
{
  arma::mat reference = arma::randu<arma::mat>(1, 10);
  arma::mat query = arma::randu<arma::mat>(1, 5);

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("kernel", std::string("gaussian"));
  SetInputParam("monte_carlo", true);

  Log::Fatal.ignoreInput = true;
  // Invalid under 1.
  SetInputParam("mc_entry_coef", 0.5);
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);

  // Valid greater than 1.
  SetInputParam("mc_entry_coef", 1.1);
  BOOST_REQUIRE_NO_THROW(mlpackMain());
  Log::Fatal.ignoreInput = false;
}

/**
  * Ensure we get an exception when an invalid Monte Carlo break coefficient
  * is specified.
 **/
BOOST_AUTO_TEST_CASE(KDEMainInvalidMCBreakCoef)
{
  arma::mat reference = arma::randu<arma::mat>(1, 10);
  arma::mat query = arma::randu<arma::mat>(1, 5);

  // Main params.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  SetInputParam("kernel", std::string("gaussian"));
  SetInputParam("monte_carlo", true);

  Log::Fatal.ignoreInput = true;
  // Invalid under 0.
  SetInputParam("mc_break_coef", -0.5);
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);

  // Valid between 0 and 1.
  SetInputParam("mc_break_coef", 0.3);
  BOOST_REQUIRE_NO_THROW(mlpackMain());

  // Invalid greater than 1.
  SetInputParam("mc_break_coef", 1.1);
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
  * Ensure when --monte_carlo flag is true, then KDEMain actually uses Monte
  * Carlo estimations. Since this test has a random component, it might fail
  * (although it's unlikely).
 **/
BOOST_AUTO_TEST_CASE(KDEMainMonteCarloFlag)
{
  // Datasets.
  arma::mat reference = arma::randu(1, 5000);
  arma::mat query = arma::randu(1, 300);
  arma::vec estimations1, estimations2, differences;
  double kernelBandwidth = 0.2;

  // Parameters for estimations.
  SetInputParam("reference", arma::mat(reference));
  SetInputParam("query", arma::mat(query));
  SetInputParam("kernel", std::string("gaussian"));
  SetInputParam("tree", std::string("kd-tree"));
  SetInputParam("bandwidth", kernelBandwidth);
  SetInputParam("monte_carlo", true);

  // Compute estimations 1.
  mlpackMain();
  estimations1 = std::move(CLI::GetParam<arma::vec>("predictions"));

  // Compute estimations 2.
  SetInputParam("reference", reference);
  SetInputParam("query", query);
  mlpackMain();
  estimations2 = std::move(CLI::GetParam<arma::vec>("predictions"));

  // Check whether results are equal.
  differences = arma::abs(estimations1 - estimations2);
  const double sumDifferences = arma::accu(differences);
  BOOST_REQUIRE_GT(sumDifferences, 0);
}

BOOST_AUTO_TEST_SUITE_END();


/**
 * @author pkambadu
 */

#include <iostream>
#include <fstream>
#include <functional>
#include <cstring>
#include <cstdlib>
#include <vector>
#include <utility>
#include <cstdio>
#include <map>

#if defined(_OPENMP)
#include <omp.h>
#endif

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/filesystem.hpp>

#include "dyck_options.hpp"
#include "generate_Galton_Watson.hpp"
#include "dyck_path.hpp"

typedef generate_Galton_Watson_t::graph_type graph_type;
typedef generate_Galton_Watson_t::vertex_index_map_t vertex_index_map_t;
typedef generate_Galton_Watson_t::vertex_name_map_t vertex_name_map_t;
typedef generate_Galton_Watson_t::edge_weight_map_t edge_weight_map_t;
typedef generate_Galton_Watson_t::vertex_distance_map_t vertex_weight_map_t;
typedef boost::graph_traits<graph_type>::vertex_iterator vertex_iter_t;

static const double PI =
    3.1415926535897932384626433832795028841971693993751058209749445923078164;

/**
 * Thin wrapper around boost::uniform<> so that it conforms to random_shuffle.
 */
template <typename EngineType>
struct random_functor_t : std::unary_function<int, int> {
  typedef EngineType engine_type;
  typedef boost::uniform_int<int> uni_int_type;
  engine_type engine;

  random_functor_t (engine_type engine) : engine(engine) {}

  int operator()(const int& ULIMIT) {
    uni_int_type dist (0, ULIMIT-1);
    return dist(engine);
  }
};

/**
 * Print the tree as a DOT file
 */
template <typename Graph>
void print_tree (Graph& G, const dyck_options_t& options) {
#pragma omp critical
  if (options.print_tree) {
    /** Set the properties that you need to extract from the graph */
    boost::dynamic_properties dp;
    vertex_index_map_t index_map = boost::get(boost::vertex_index, G);
    vertex_name_map_t name_map = boost::get(boost::vertex_name, G);
    vertex_weight_map_t weight_map = boost::get(boost::vertex_distance, G);

    dp.property("index", index_map);
    dp.property("name", name_map);
    dp.property("weight", weight_map);

    /** Create the output */
    std::ostream* output;
    if (0==strcmp("stdout", options.dot_out.c_str())) output = &(std::cout);
    else output = new std::ofstream(options.dot_out.c_str());

    boost::write_graphviz_dp(*output, G, dp, "index");

    if (0!=strcmp("stdout", options.dot_out.c_str())) delete output;
  }
}

/**
 * A function that lists all the DOT files in a directory.
 */
std::vector<std::string> get_dot_files (const std::string& dir_name) {
  std::vector<std::string> dot_file_list;

  if (boost::filesystem::exists(dir_name) &&
      boost::filesystem::is_directory(dir_name)) {
    boost::filesystem::directory_iterator end_iter;
    for(boost::filesystem::directory_iterator dir_iter(dir_name);
        dir_iter != end_iter;
        ++dir_iter) {
      if (boost::filesystem::is_regular_file(dir_iter->status())) {
        const boost::filesystem::path current_file = dir_iter->path();
        const boost::filesystem::path current_file_ext =
                                             current_file.extension();
        if (current_file_ext.native() == ".dot") {
          dot_file_list.push_back(current_file.native());
        }
      }
    }
  }

  return dot_file_list;
}

/**
 * The one annoying problem is that OutputIterator's do not define
 * a value type, so it may not always work! Hrmp!
 */
int generate_seed () {
  int r_int;
#pragma omp critical
  {
    std::ifstream rf("/dev/random", std::ios::binary);
    if(rf.is_open())rf.read(reinterpret_cast<char*>(&r_int),sizeof(r_int));
    else r_int = std::time(NULL);
  }

  return r_int;
}

graph_type generate_one_graph (const dyck_options_t& options,
                               int& gen_seed) {
  graph_type G;

  gen_seed = (0>options.seed)?generate_seed():options.seed;
  if (0 == strcmp("Poisson", options.gen_method.c_str())) {
    G = generate_Galton_Watson_t::generate_Poisson(options.n,
                                                   gen_seed,
                                                   options.lambda,
                                                   options.use_random_weights,
                                                   options.verbosity,
                                                   options.use_stupid_tree_gen);
  } else if (0 == strcmp("Binomial", options.gen_method.c_str())) {
    G = generate_Galton_Watson_t::generate_Binomial (options.n,
                                                     gen_seed,
                                                     options.k,
                                                     options.p,
                                                     options.use_random_weights,
                                                     options.verbosity,
                                                 options.use_stupid_tree_gen);
  } else if (0 == strcmp("Geometric", options.gen_method.c_str())) {
    G = generate_Galton_Watson_t::generate_Geometric(options.n,
                                                     gen_seed,
                                                     options.p,
                                                     options.use_random_weights,
                                                     options.verbosity,
                                                 options.use_stupid_tree_gen);
  } else if (0 == strcmp("Binary-0-2", options.gen_method.c_str())) {
    if (false == (options.n & 0x1)) {
      std::cerr << "Binary-0-2 can only generate odd number of nodes\
 due to a shortcoming of Devroye's algorithm. Sorry" << std::endl;
      exit(-1);
    }
    G = generate_Galton_Watson_t::generate_Binary_0_2
                                              (options.n,
                                               gen_seed,
                                               options.p,
                                               options.use_random_weights,
                                               options.verbosity,
                                               options.use_stupid_tree_gen);
  } else if (0 == strcmp("Binary-0-1-2", options.gen_method.c_str())) {
    G = generate_Galton_Watson_t::generate_Binary_0_1_2
                                              (options.n,
                                               gen_seed,
                                               options.use_random_weights,
                                               options.verbosity,
                                               options.use_stupid_tree_gen);
  } else if (0 == strcmp("Graphviz", options.gen_method.c_str())) {
    /** Get the input file */
    std::ifstream input_dot_file(options.dot_in.c_str());

    /** Set the properties that you need to extract from the graph */
    boost::dynamic_properties dp;
    vertex_index_map_t index_map = boost::get(boost::vertex_index, G);
    vertex_name_map_t name_map = boost::get(boost::vertex_name, G);
    vertex_weight_map_t weight_map = boost::get(boost::vertex_distance, G);

    dp.property("index", index_map);
    dp.property("name", name_map);
    dp.property("weight", weight_map);

    /** Read the graph --- note that indices are relabeled, which is fine */
    boost::read_graphviz(input_dot_file, G, dp, "name");

    /** Label it */
    bfs_label(G);

  } else {
    std::cout << "Chosen method of graph generation not supported"
              << std::endl;
  }
  return G;
}

template <typename NameToIndexMap,
          typename WeightMap>
double path_length (const std::string& parent,
                    const std::string& child,
                    const NameToIndexMap& name_map,
                    const WeightMap& weight_map,
                    const dyck_options_t& options) {

  double total_path_length = 0.0;

  if (0<options.verbosity) {
    std::cout << "Path length " << name_map.find(parent)->second << " ---> "
              << name_map.find(child)->second << std::endl;
  }

  /** Get the first place where things mismatch */
  std::string::const_iterator position =
    std::mismatch (parent.begin(), parent.end(), child.begin()).second;

  /** If they don't, we get to go one by one */
  while (position != child.end()) {
    const std::string current_vertex(child.begin(), position+1);
    const int current_vertex_index = name_map.find(current_vertex)->second;
    const double weight = boost::get(weight_map, current_vertex_index);
    if (0<options.verbosity) {
      std::cout << "Adding " << current_vertex_index << " (code:"
              << current_vertex << ") "
              << " of weight " << weight << std::endl;
    }
    total_path_length += weight;
    ++position;
  }

  return total_path_length;
}

/** total path length and generate trees by cutting things off */

std::pair<int,double>
run_one_lca_experiment(dyck_options_t& options,
                       int& gen_seed,
                       int& lca_seed,
                       int& num_nodes_in_G) {
  graph_type G;
  vertex_name_map_t name_map;
  vertex_weight_map_t weight_map;

  /** Generate the seeds if needed */
  lca_seed = (0>options.lca_seed)?generate_seed():options.lca_seed;
  gen_seed = (0>options.seed)?generate_seed():options.seed;

  if (options.verbosity) std::cout << "Generating graph .... " << std::flush;

  do {
    /** Generate a graph */
    G = generate_one_graph (options, gen_seed);

    /** Get the number of nodes in the graph */
    vertex_iter_t v_begin, v_end;
    boost::tie(v_begin, v_end) = boost::vertices(G);
    num_nodes_in_G = std::distance(v_begin, v_end);

    if (false==options.dot_in.empty()) {
      options.n = num_nodes_in_G;
    }
  } while (num_nodes_in_G<options.n);
  if (options.verbosity) std::cout << "DONE" << std::endl;

  name_map = boost::get(boost::vertex_name, G);
  weight_map = boost::get(boost::vertex_distance, G);

  int num_lca_samples = num_nodes_in_G*options.percent_lca_samples;
  std::vector<int> lca_vertices(num_lca_samples);
  const std::string file_name =
           boost::filesystem::path(options.dot_in).filename().string();

  /**
   * We want to generate some vertices from the given set of vertices
   * randomly and form the LCA tree. The thing wrong here is that my
   * compiler doesn't implement std::random_sample and std::iota. Hence
   * that long stupid way to sample elements from the vertex.
   *
   */
  std::vector<int> all_vertices(num_nodes_in_G-1);
  for (size_t i=0; i<all_vertices.size(); ++i) all_vertices[i]=(i+1);
  boost::mt19937 engine(lca_seed);
  random_functor_t<boost::mt19937> shuffle_prng (engine);
  std::random_shuffle (all_vertices.begin(), all_vertices.end(), shuffle_prng);

  if (options.sample_leaves) {
    /**
     * We want to only sample leaf nodes. The logic behind this is that we want
     * to only sample "original" data points. In case of Arvind's data, the
     * intermediate nodes are formed by hierarchical clustering and hence are
     * not original. There are two things to do:
     *
     * (1) Iterate through the shuffled vertices and
     *    -- copy those vertices that are leaves (up to num_lca_samples)
     *    -- count the number of leaves
     * (2) If num_leaves>num_lca_samples, we are golden. However, if this is
     *     not the case (num_lca_samples>=num_leaves), then we want to cut
     *     down on the number of samples.
     */
    int num_leaves = 0;
    std::vector<int>::iterator lca_vertices_iter = lca_vertices.begin();
    for (size_t i=0; i<all_vertices.size(); ++i) {
      const int vertex_id = all_vertices[i];
      if (0 == boost::out_degree(vertex_id, G)) {
        if (num_leaves<num_lca_samples) {
          *lca_vertices_iter = vertex_id;
          ++lca_vertices_iter;
        }
        ++num_leaves;
      }
    }

    if (num_lca_samples>=num_leaves) {
      std::cerr << file_name << ": Sampling " << num_lca_samples
                << " from " << num_leaves;
      num_lca_samples = 0.5*num_leaves;
      std::cerr << ". Changing to sample " << num_lca_samples << std::endl;
      if (0==num_lca_samples) {
        std::cerr << "Error: sampling " << num_lca_samples << std::endl;
        exit(-3);
      }
      lca_vertices.resize(num_lca_samples);
    }
  } else {
    /**
     * Approach 1:
     * We don't care whether we are sampling leaf or non-leaf nodes. As long as
     * it's a non-root node, we are golden.  All we are doing here is creating
     * an array of elements 1...(n-1), randomly shuffling it and then picking
     * the top num_lca_samples.
     */
    std::copy (all_vertices.begin(),
               all_vertices.begin() + num_lca_samples,
               lca_vertices.begin());
  }

  /**
   * Now, build the new tree with intermediate vertices and figure out the
   * number of edges in the new tree using the LCA algorithm to create
   * intermediate nodes.
   */
  if (0<options.verbosity) {
    std::cout << "Sampled LCA vertices are: " << std::endl;
    for (size_t i=0; i<lca_vertices.size(); ++i)
      std::cout << lca_vertices[i] << " (code:"
              << boost::get(name_map, lca_vertices[i]) << ")" << std::endl;
  }

  std::map<std::string, int> all_vertices_name_map;

  /** Create a name map */
  for (size_t i=0; i<num_nodes_in_G; ++i)
    all_vertices_name_map[boost::get(name_map, i)] = i;

  /** 2. Join the vertices one by one to get the LCA tree */
  std::map<std::string,int> new_lca_vertices;
  typedef std::map<std::string, int>::const_iterator MapIterator;
  new_lca_vertices[boost::get(name_map, 0)] = 0;

  std::set<std::pair<int,int> > added_edges_set;
  double total_path_length = 0.0;
  int total_num_edges = 0;

  for (size_t i=0; i<(lca_vertices.size()-1); ++i) {
    for (size_t j=(i+1); j<lca_vertices.size(); ++j) {
      /** Figure out the vertices we are dealing with */
      const int me = lca_vertices[i];
      const std::string my_name = boost::get(name_map, me);
      const int you = lca_vertices[j];
      const std::string your_name = boost::get(name_map, you);

      /** Push these two vertices into new LCA tree --- duplicate elim by map */
      new_lca_vertices[my_name] = me;
      new_lca_vertices[your_name] = you;

      /** Find the LCA, which also happens to be the common prefix */
      const std::string our_common_prefix =
        std::string(my_name.begin(),
        std::mismatch(my_name.begin(), my_name.end(), your_name.begin()).first);

      /** Search for the common ancestor */
      const bool found_ancestor = (new_lca_vertices.end() !=
                            new_lca_vertices.find(our_common_prefix));

      if (false == found_ancestor) {
      const int our_common_ancestor = all_vertices_name_map[our_common_prefix];
        new_lca_vertices[our_common_prefix] = our_common_ancestor;
      }

      /** If the common prefix is not found, then insert it in */
      if (0<options.verbosity) {
        std::cout << "Ancestor of " << me << " (code:" << my_name << ") and "
                  << you << " (code:" << your_name << ") is "
                  << new_lca_vertices[our_common_prefix] << " (code:"
            << our_common_prefix << ")" << (found_ancestor?" (already present)":
                                                    " (had to be added)")
            << std::endl;
      }
    }
  }

  if (options.sample_leaves) {
    std::cerr << file_name << ": " << num_lca_samples
              << " lca-tree (|V|=" << new_lca_vertices.size()
              << ", |E|=" << 2*num_lca_samples-1 << ")" << std::endl;
  }

  if (0<options.verbosity) {
    std::cout << "Sampled (and built) LCA tree has " << new_lca_vertices.size()
              << " nodes and these vertices: " << std::endl;
    for (MapIterator iter = new_lca_vertices.begin();
         iter != new_lca_vertices.end();
         ++iter) {
      std::cout << iter->second << " (code:" << iter->first << ")" << std::endl;
    }
  }

  const MapIterator range_first = new_lca_vertices.begin();
  MapIterator range_last = range_first;
  ++range_last;
  MapIterator iter = range_last;
  while (iter != new_lca_vertices.end()) {
    /** find parent of *iter */
    MapIterator lowest_parent = range_first;
    MapIterator first = range_first;
    while (first != range_last) {
      if (0<options.verbosity) {
        std::cout << "Testing " << first->second << " for "
                    << iter->second << std::endl;
      }
      if (first->first.end()==
          std::mismatch(first->first.begin(),
                        first->first.end(),
                        iter->first.begin()).first) {
        if (0<options.verbosity) {
          std::cout << first->second << " is an ancestor of "
                    << iter->second << std::endl;
        }
        lowest_parent = first;
      }
      ++first;
    }

    if (0<options.verbosity) {
      std::cout << lowest_parent->second << " is the LCA of "
                << iter->second << std::endl;
    }

    /** compute distance from parent to *iter */
    total_path_length += path_length(lowest_parent->first, iter->first,
                                    all_vertices_name_map, weight_map, options);
    ++total_num_edges;

    /** increment range_last */
    ++range_last;
    ++iter;
  }

  print_tree(G, options);

  return std::pair<int,double>(total_num_edges, total_path_length);
}

void run_one_experiment (const dyck_options_t& options,
                         int& gen_seed,
                         int& mle_seed,
                         int& num_nodes_in_G,
                         int& vertex_to_find,
                         double& vertex_height,
                         double& mean_height) {
  graph_type G;
  vertex_name_map_t name_map;
  vertex_weight_map_t weight_map;

  do {
    /** Generate a graph */
    G = generate_one_graph (options, gen_seed);

    /** Get the number of nodes in the graph */
    vertex_iter_t v_begin, v_end;
    boost::tie(v_begin, v_end) = boost::vertices(G);
    num_nodes_in_G = std::distance(v_begin, v_end);
  } while (num_nodes_in_G < (options.n));

  name_map = boost::get(boost::vertex_name, G);
  weight_map = boost::get(boost::vertex_distance, G);

  /**
   * If we want to identify the height of a random (non-root) vertex in the
   * tree, then we want generate a random number between [1,n), where n is
   * the number of nodes in the generated tree.
   */
  vertex_to_find = -1;
  mle_seed = (0>options.mle_seed)?generate_seed():options.mle_seed;
  boost::mt19937 engine (mle_seed);
  boost::uniform_int<int> dist (1, num_nodes_in_G-1);
  vertex_to_find = dist(engine);

  /** Now, compute the Dyck path using depth-first search */
  std::vector<double> y_axis;
  vertex_height = dyck_path (G, name_map, weight_map,
                             std::back_inserter(y_axis),
                             vertex_to_find,
                             options.verbosity);

  /** Compute the mean height of the tree */
  assert (y_axis.size() == 2*num_nodes_in_G);
  mean_height = 0.0;
  for (size_t i=0; i<y_axis.size(); ++i) mean_height += y_axis[i];
  mean_height /= (2*pow(num_nodes_in_G,1.5));

  print_tree (G, options);

#pragma omp critical
  if (options.print_path) {
    /** Write stuff out where needed */
    /** We need to ensure that the x-axis is such that the slope is always 45*/
    std::vector<double> x_axis(y_axis.size());
    x_axis[0] = 0;
    for (size_t i=1; i<x_axis.size(); ++i)
      x_axis[i] = x_axis[i-1] + std::abs(y_axis[i] - y_axis[i-1]);

    /** Remember, DO NOT PRINT THE LAST INDEX -- this is an artifact of DFS */
    std::ostream* output;
    if (0==strcmp("stdout", options.dyck_out.c_str())) output = &(std::cout);
    else output = new std::ofstream(options.dyck_out.c_str());
    for (size_t i=0; i<(x_axis.size()-1); ++i) {
      *output << x_axis[i] << "  " << y_axis[i] << std::endl;
    }
    if (0!=strcmp("stdout", options.dyck_out.c_str())) delete output;
  }

}

int main (int argc, char** argv) {
  dyck_options_t options (argc, argv);
  if (options.exit_on_return) { return -1; }

  if (1<options.verbosity) options.pretty_print();

  std::vector<std::string> dot_file_names;
  if (options.dot_in_dir != "") {
    dot_file_names = get_dot_files(options.dot_in_dir);
    options.num_trials = dot_file_names.size();
    options.gen_method = "Graphviz";
  }

  std::vector<int> gen_seed_vec(options.num_trials);
  std::vector<int> mle_seed_vec(options.num_trials);
  std::vector<int> lca_seed_vec(options.num_trials);
  std::vector<int> num_nodes_vec(options.num_trials);
  std::vector<int> vertex_to_find_vec(options.num_trials);
  std::vector<double> vertex_height_vec(options.num_trials);
  std::vector<double> mean_height_vec(options.num_trials);

  if (options.measure_lca) {

    char header_string[1024];
    int h_count = sprintf (header_string,
            "%9s %9s %2s %10s %2s %7s %7s %7s %5s %12s %12s",
                "dist-type", "f-name", "k", "p", "lm", "n",
                "n-tru", "n-smpl", "lca-edges", "lca-path-ln");
    std::cout << header_string << std::endl;

#pragma omp parallel for num_threads(options.num_threads)
    for (int i=0; i<options.num_trials; ++i) {
      /**
       * TODO: This is a hack. What we are doing is that we are checking if
       * the input method is a DOT file. If so, we are checking if the input
       * mentioned is actually a directory listing of files. In this case,
       * we will populate options.dot_in to be a different file name every
       * single time.
       */
      if (false==options.dot_in_dir.empty()) {
        options.dot_in = dot_file_names[i];
      }
      std::pair<int,double> lca_result =
        run_one_lca_experiment (options, gen_seed_vec[i],
                                lca_seed_vec[i], num_nodes_vec[i]);

      char value_string[1024];
      const std::string file_name =
              boost::filesystem::path(options.dot_in).filename().string();
      int v_count = sprintf (value_string,
               "%9s %9s %2d %.4e %2d %7d %7d %6.2f %12d %.6e",
                               options.gen_method.c_str(),
                               file_name.c_str(),
                               options.k,
                               options.p,
                               options.lambda,
                               options.n,
                               num_nodes_vec[i],
                               options.percent_lca_samples,
                               lca_result.first,
                               lca_result.second);
      std::cout << value_string << std::endl;
    }

    goto END;
  }

#pragma omp parallel for num_threads(options.num_threads)
  for (int i=0; i<options.num_trials; ++i) {
    /**
     * TODO: This is a hack. What we are doing is that we are checking if
     * the input method is a DOT file. If so, we are checking if the input
     * mentioned is actually a directory listing of files. In this case,
     * we will populate options.dot_in to be a different file name every
     * single time.
     */
    if (false==options.dot_in_dir.empty()) {
      options.dot_in = dot_file_names[i];
    }
    run_one_experiment (options, gen_seed_vec[i], mle_seed_vec[i],
                        num_nodes_vec[i], vertex_to_find_vec[i],
                        vertex_height_vec[i], mean_height_vec[i]);
  }

  double true_var;
  if (0 == strcmp("Poisson", options.gen_method.c_str()))
    true_var=options.lambda;
  else if (0 == strcmp("Binomial", options.gen_method.c_str()))
    true_var=options.k*options.p*(1-options.p);
  else if (0 == strcmp("Geometric", options.gen_method.c_str()))
    true_var=(1-options.p)/(options.p*options.p);
  else if (0 == strcmp("Binary-0-2", options.gen_method.c_str()))
    true_var=1;
  else if (0 == strcmp("Binary-0-1-2", options.gen_method.c_str()))
    true_var=2./3.;

  if (options.dump_numbers) {
    for (int i=0; i<options.num_trials; ++i) {
      double x_i = (1.0/(sqrt((double)num_nodes_vec[i])))*vertex_height_vec[i];
      printf (" %-8.3f", x_i);
    }
    printf ("\n");
    for (int i=0;i<options.num_trials;++i) {
      printf(" %-8.3f", mean_height_vec[i]);
    }
    printf ("\n");
  }

  if (options.measure_mle) {
    if (options.verbosity) {
      char header_string[1024];
      int h_count = sprintf (header_string,
            "%9s %9s %2s %10s %2s %7s %7s %7s %7s %12s %12s %12s",
                "dist-type", "filename", "k", "p", "lm", "n",
                "n-tru", "node-#","height-raw","height-nrmzd","height-mean");
      if (1<options.verbosity)
   sprintf (&(header_string[h_count]), " %10s %10s", "gen-seed", "MLE-seed");
      std::cout << header_string << std::endl;
    }

    double sum_of_x_i_sqr = 0.0;
    double mean_of_x_i_sqr = 0.0;
    for (int i=0; i<options.num_trials; ++i) {
      double x_i = (1.0/(sqrt((double)num_nodes_vec[i])))*vertex_height_vec[i];
      sum_of_x_i_sqr += x_i*x_i;
      mean_of_x_i_sqr += x_i;

      if (options.verbosity) {
        char value_string[1024];
        const std::string file_name =
              boost::filesystem::path(dot_file_names[i]).filename().string();
        int v_count = sprintf (value_string,
               "%9s %9s %2d %.4e %2d %7d %7d %7d %.6e %.6e %.6e",
                               options.gen_method.c_str(),
                               file_name.c_str(),
                               options.k,
                               options.p,
                               options.lambda,
                               options.n,
                               num_nodes_vec[i],
                               vertex_to_find_vec[i],
                               vertex_height_vec[i],
                               x_i,
                               mean_height_vec[i]);
        if (1<options.verbosity)
          sprintf (&(value_string[v_count]), " %10u %10u",
                          gen_seed_vec[i], mle_seed_vec[i]);
        std::cout << value_string << std::endl;
      }
    }
    mean_of_x_i_sqr /= options.num_trials;
    mean_of_x_i_sqr *= mean_of_x_i_sqr;

    double mle_estimate_var = (2*options.num_trials)/sum_of_x_i_sqr;
    double ano_estimate_var = PI/(2*mean_of_x_i_sqr);

    std::cout << "True variance = " << true_var
              << " MLE = " << mle_estimate_var
              << " Another = " << ano_estimate_var << std::endl;
    std::cout << " Mean of x_i^2 =  " << mean_of_x_i_sqr << std::endl;
  }

  if (options.test_confidence) {
    int num_batches = options.num_trials / options.batch_size;
    int num_contained = 0;

    int num_nodes = num_nodes_vec[0];
    bool experimental_error = false;
    for (int i=1; i<options.num_trials; ++i) {
      if (num_nodes != num_nodes_vec[i]) {
        experimental_error = true;
        break;
      }
    }

    if (experimental_error) {
      std::cout << "Run this experiment again, please" << std::endl;
      goto END;
    }

    std::vector<double> other_variances;
    if (0 == strcmp("Poisson", options.gen_method.c_str())) {
      other_variances.push_back(1 - 1./2.);
      other_variances.push_back(1 - 1./3.);
      other_variances.push_back(1 - 1./4.);
      other_variances.push_back(1 - 1./5.);
      other_variances.push_back(2);
    } else if (0 == strcmp("Binomial", options.gen_method.c_str())) {
      other_variances.push_back(1);
      other_variances.push_back(2);
      other_variances.push_back(2.0/3.0);
    } else if (0 == strcmp("Geometric", options.gen_method.c_str())) {
      other_variances.push_back(1);
      other_variances.push_back(1 - 1./2.);
      other_variances.push_back(1 - 1./3.);
      other_variances.push_back(1 - 1./4.);
      other_variances.push_back(1 - 1./5.);
    } else if (0 == strcmp("Binary-0-2", options.gen_method.c_str())) {
      other_variances.push_back(1 - 1./2.);
      other_variances.push_back(1 - 1./4.);
      other_variances.push_back(1 - 1./5.);
      other_variances.push_back(2);
      other_variances.push_back(2.0/3.0);
    } else if (0 == strcmp("Binary-0-1-2", options.gen_method.c_str())) {
      other_variances.push_back(1 - 1./2.);
      other_variances.push_back(1 - 1./3.);
      other_variances.push_back(1 - 1./4.);
      other_variances.push_back(1 - 1./5.);
      other_variances.push_back(1);
      other_variances.push_back(2);
    }
    std::vector<double> power_against_others (other_variances.size(), 0.0);

    boost::math::chi_squared chi_dist (2*options.batch_size);
    const double a = boost::math::quantile(chi_dist, 0.025);
    const double b = boost::math::quantile(chi_dist, 0.975);

    for (int i=0; i<options.num_trials; i+=options.batch_size) {
      double sum_of_x_j_sqr = 0.0;
      for (int j=0; j<options.batch_size; ++j) {
        double x_j = (1.0/(sqrt((double)num_nodes_vec[i+j])))*
                                        vertex_height_vec[i+j];
        sum_of_x_j_sqr += x_j*x_j;
      }

      const double A = a/sum_of_x_j_sqr;
      const double B = b/sum_of_x_j_sqr;
      if (A <= true_var && B >= true_var) ++num_contained;
      for (int l=0; l<other_variances.size(); ++l)
        if (A <= other_variances[l] && B >= other_variances[l])
          power_against_others[l]+=1;

      if (1<options.verbosity) {
        std::cout << "Lower = " << A << " Upper = " << B
                  << " sum_of_x_j_sqr = " << sum_of_x_j_sqr << std::endl;
      }
    }
    const double contained =
       100*static_cast<double>(num_contained)/static_cast<double>(num_batches);

    printf ("%12s %5d %3d %7.3f%%(tru-var=%4.3f) ", options.gen_method.c_str(),
                                                    num_batches,
                                                    options.batch_size,
                                                    contained,
                                                    true_var);
    for (int l=0; l<other_variances.size(); ++l)
      printf (" %7.3f%%(var=%4.3f)", 100*(power_against_others[l]/num_batches),
                                     other_variances[l]);
    printf ("\n");
  }

END:

  return 0;
}


//
// Created by mknoe on 04.04.2021.
//
#include <windows.h>        // Must have for Windows platform builds

#include <cmath>
#include <cstdio>
#include <iostream>
#include <sstream>
#include <fstream>
#include <chrono>
#include <cstdlib>
#include <utility>
#include <vector>
#include <algorithm>

#include <iterator>
#include "../exercise01/drawLine.h"
#include "../basics.h"
#include "../bench.h"
#include "../exercise02/drawCircle.h"
#include "bezier.h"
#include <boost/math/interpolators/cardinal_cubic_b_spline.hpp>
#include <boost/random.hpp>
#include <boost/math/statistics/bivariate_statistics.hpp>
#include <random>
#include <boost/math/tools/roots.hpp>
#include "splines.cpp"
#include "splines.h"
#include "vec3.hpp"
#include <boost/config.hpp>
#include <cstddef>
#include <iosfwd>
#include <GL/glu.h>

//https://chi3x10.wordpress.com/2009/10/18/de-boor-algorithm-in-c/
class Point {
public:
    Point() {
        x = 0.;
        y = 0.;
        z = 0.;
    };

    // copy operator
    Point operator=(const Point &pt);

    Point operator+(const Point &pt) const;

    //Point operator-(const Point &pt) const;
    Point operator*(double m) const;

    Point operator/(double m) const;

    double x, y, z;
};

Point Point::operator=(const Point &pt) {
    x = pt.x;
    y = pt.y;
    z = pt.z;
    return *this;
}

Point Point::operator+(const Point &pt) const {
    Point temp;
    temp.x = x + pt.x;
    temp.y = y + pt.y;
    temp.z = z + pt.z;
    return temp;
}

Point Point::operator*(double m) const {
    Point temp;
    temp.x = x * m;
    temp.y = y * m;
    temp.z = z * m;
    return temp;
}

Point Point::operator/(double m) const {
    Point temp;
    temp.x = x / m;
    temp.y = y / m;
    temp.z = z / m;
    return temp;
}

Cordinate deBoorRecursiv(int k, int degree, int i, double x, vector<double> knots, vector<Cordinate> ctrlPoints) {
    // Please see wikipedia page for detail
    // note that the algorithm here kind of traverses in reverse order
    // comapred to that in the wikipedia page
    if (k == 0)
        return ctrlPoints[i];
    else {
        double alpha = (x - knots[i]) / (knots[i + degree + 1 - k] - knots[i]);
        return (deBoorRecursiv(k - 1, degree, i - 1, x, knots, ctrlPoints) * (1 - alpha) +
                deBoorRecursiv(k - 1, degree, i, x, knots, ctrlPoints) * alpha);
    }
}

Cordinate deBoor(int k, double x, vector<double> t, vector<Cordinate> c, int p) {
/*
k: Index of knot interval that contains x.
x: Position.
t: Array of knot positions, needs to be padded as described above.
c: Array of control points.
p: Degree of B-spline.
 */
    vector<Cordinate> d{};
    for (int j = 0; j < p + 1; ++j) {
        d.push_back(c[j + k - p]);
    }
    for (int r = 1; r < p + 1; ++r) {
        for (int j = p; j > r - 1; --j) {
            int alpha = (x - t[j + k - p]) / (t[j + 1 + k - r] - t[j + k - p]);
            d[j] = (1.0 - alpha) * d[j - 1] + alpha * d[j];
        }
    }
    return d[p];
}

int WhichInterval(double x, vector<double> knot, int ti) {
    for (int i = 1; i < ti - 1; i++) {
        if (x < knot[i])
            return (i - 1);
        else if (x == knot[ti - 1])
            return (ti - 1);
    }
    return -1;
}

Cordinate bSpline(vector<Cordinate> d, int n, vector<double> t, int i) {
    auto m = d.size() - 1;
    int ts;
    if (t[i + 1] - 1 >= t[i])
        ts = t[i + 1] - 1;
    else
        ts = t[i];

    // if (0 < i <= m) throw new exception();

    vector<vector<Cordinate>> d0{};
    for (int j = 0; j < n; ++j) {
        d0.emplace_back();
        for (int l = i - n + j; l < i; l++) {
            if (j == 0) {
                d0[0].push_back(d[l]);
            } else {
                double tl = (ts - t[l]) / t[l + n - j] - t[l];
                d0[j][l] = (1 - tl) * d0[j - 1][l - 1] + tl * d0[j - 1][l];
            }
        }
    }
    return d0[n][i];
}

Cordinate closeBSpline(vector<Cordinate> d, int n, vector<double> t, int i) {
    auto m = d.size() - 1;
    int ts;
    if (t[i + 1] - 1 >= t[i])
        ts = t[i + 1] - 1;
    else
        ts = t[i];

    //  if (0 < i <= m) throw new exception();

    vector<vector<Cordinate>> d0{};

    for (int j = 0; j < n; ++j) {
        int l = i - n + j - 1;
        do {
            l++;
            if (l < 0) {
                l = l + m + 1;
                ts = ts - t[0] + t[m + 1];
            } else {
                if (l >= m + 1) {
                    l = l - m - 1;
                    ts = ts - t[m + 1] + t[0];
                }
            }
            if (j == 0) {
                d0[0].push_back(d[l]);
            } else {
                double tl = (ts - t[l]) / t[l + n - j] - t[l];
                d0[j][l] = (1 - tl) * d0[j - 1][l - 1] + tl * d0[j - 1][l];
            }
        } while (l != i);
    }
    return d0[n][i];
}

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

    RGBPixel color = RGBPixel(40, 40, 40);
    RGBPixel color2 = RGBPixel(0, 255, 255);
    RGBPixel color3 = RGBPixel(0, 0, 255);

    Display::init(argc, argv, 256, 256);
    auto display = Display::getInstance();
    ////////// put your framebuffer drawing code here /////////////

    const Cordinate P1 = Cordinate(30, 45);
    const Cordinate P2 = Cordinate(35, 100);
    const Cordinate P3 = Cordinate(70, 95);
    const Cordinate P4 = Cordinate(80, 35);

    std::vector<std::vector<double>> vList = {};
    const vector<Cordinate> list = {P1, P2, P3, P4};

    int k{3};
    vector<double> t{0, 0, 0, 0.5, 0.5, 0.5, 1, 1, 1};
    vector<Cordinate> c{P1, P2, P3, P4};
    auto degree = 2;
    int n = 2;
    vector<Cordinate> points;
//    for (int i = 0; i < list.size() - 1; i++) {
//        auto point = bSpline(list, n, t, i);
//
//        points.push_back(point);
//        display->setPixel(point, color);
//
//    }

    for (int x = 0; x < display->RESOLUTION; x++) {
        Cordinate b = deBoor(k, x, t, c, degree);
        Cordinate b2 = deBoorRecursiv(k, degree, WhichInterval(x, t, t.size()), x, t, c);
        display->setPixel(b, color3);
        display->setPixel(b2, color2);
        cout << "b= " << b << "\n";
        cout << "b2= " << b2 << "\n";
    }
    drawList(list, color);

    /////////////////////////////////
    glutMainLoop();
    return 0;
}


// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2014 cDc and Pierre MOULON.

// 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 "openMVG/multiview/rotation_averaging_l1.hpp"
#include "openMVG/numeric/l1_solver_admm.hpp"

#ifdef HAVE_BOOST
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/foreach.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
using namespace boost;
#else
#include "lemon/adaptors.h"
#include "lemon/dfs.h"
#include "lemon/kruskal.h"
#include "lemon/list_graph.h"
#include "lemon/path.h"
using namespace lemon;
#endif

#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <Eigen/Cholesky>
#include <Eigen/SparseCholesky>

#include <queue>

namespace openMVG   {
namespace rotation_averaging  {
namespace l1  {

/////////////////////////

// given an array of values, compute the X84 threshold as in:
// Hampel FR, Rousseeuw PJ, Ronchetti EM, Stahel WA
// "Robust Statistics: the Approach Based on Influence Functions"
// Wiley Series in Probability and Mathematical Statistics, John Wiley & Sons, 1986
// returns the pair(median,trust_region)
// upper-bound threshold = median+trust_region
// lower-bound threshold = median-trust_region
template<typename TYPE>
inline std::pair<TYPE, TYPE>
ComputeX84Threshold(const TYPE* const values, uint32_t size, TYPE mul=TYPE(5.2))
{
  assert(size > 0);
  typename std::vector<TYPE> data(values, values+size);
  typename std::vector<TYPE>::iterator mid = data.begin() + size / 2;
  std::nth_element(data.begin(), mid, data.end());
  const TYPE median = *mid;
  // threshold = 5.2 * MEDIAN(ABS(values-median));
  for (size_t i=0; i<size; ++i)
    data[i] = std::abs(values[i]-median);
  std::nth_element(data.begin(), mid, data.end());
  return {median, mul*(*mid)};
} // ComputeX84Threshold

/////////////////////////

using Matrix3x3 = openMVG::Mat3;
using IndexArr = std::vector<uint32_t>;

// find the shortest cycle for the given graph and starting vertex
struct Node {
  using InternalType = IndexArr;
  InternalType edges; // array of vertex indices
};
using NodeArr = std::vector<Node>;

struct Link {
  uint32_t ID; // node index
  uint32_t parentID;// parent link
  inline Link(uint32_t ID_=0, uint32_t parentID_=0) : ID(ID_), parentID(parentID_) {}
};
using LinkQue = std::queue<Link>;

#ifdef HAVE_BOOST
using edge_property_t = boost::property<boost::edge_weight_t, float>;
using graph_t = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, uint32_t, edge_property_t>;
using vertex_t = graph_t::vertex_descriptor;
using edge_t = graph_t::edge_descriptor;
using edge_iter = boost::graph_traits<graph_t>::edge_iterator;
#else
using graph_t = lemon::ListGraph;
using map_EdgeMap = graph_t::EdgeMap<double>;
#endif
using MapEdgeIJ2R = std::map<std::pair<uint32_t,uint32_t>, Matrix3x3>;

// Look for the maximum spanning tree along the graph of relative rotations
// since we look for the maximum spanning tree using a minimum spanning tree algorithm
// weight are negated.
uint32_t FindMaximumSpanningTree(const RelativeRotations& RelRs, graph_t& g, MapEdgeIJ2R& mapIJ2R, NodeArr& minGraph)
{
  assert(!RelRs.empty());
#ifdef HAVE_BOOST
  for (size_t p = 0; p < RelRs.size(); ++p) {
    const RelativeRotation& relR = RelRs[p];
    boost::add_edge(relR.i, relR.j, - relR.weight, g);
    mapIJ2R[{relR.i, relR.j}] = relR.Rij;
    mapIJ2R[{relR.j, relR.i}] = relR.Rij.transpose();
  }
  // find the minimum spanning tree
  const size_t nViews = boost::num_vertices(g);
  minGraph.resize(nViews);
  std::vector<edge_t> spanningTree;
  boost::kruskal_minimum_spanning_tree(g, std::back_inserter(spanningTree));
  for (std::vector<edge_t>::const_iterator ei=spanningTree.begin(); ei!=spanningTree.end(); ++ei) {
    const edge_t& edge = *ei;
    minGraph[edge.m_source].edges.push_back(edge.m_target);
    minGraph[edge.m_target].edges.push_back(edge.m_source);
  }
  const size_t nEdges = spanningTree.size();
  return nEdges;
#else

  //A-- Compute the number of node we need
  std::set<uint32_t> setNodes;
  for (size_t p = 0; p < RelRs.size(); ++p) {
    const RelativeRotation& relR = RelRs[p];
    setNodes.insert(relR.i);
    setNodes.insert(relR.j);
  }

  //B-- Create a node graph for each element of the set
  using map_NodeT = std::map<uint32_t, graph_t::Node>;
  map_NodeT map_index_to_node;
  for (const auto & iter : setNodes)
  {
    map_index_to_node[iter] = g.addNode();
  }

  //C-- Create a graph from RelRs with weighted edges
  map_EdgeMap map_edgeMap(g);
  for (size_t p = 0; p < RelRs.size(); ++p) {
    const RelativeRotation& relR = RelRs[p];
    mapIJ2R[{relR.i, relR.j}] = relR.Rij;
    mapIJ2R[{relR.j, relR.i}] = relR.Rij.transpose();

    // add edge to the graph
    graph_t::Edge edge =  g.addEdge(map_index_to_node[relR.i], map_index_to_node[relR.j]);
    map_edgeMap[ edge ] = - relR.weight;
  }

  //D-- Compute the MST of the graph
  std::vector<graph_t::Edge> tree_edge_vec;
  lemon::kruskal(g, map_edgeMap, std::back_inserter(tree_edge_vec));

  const size_t nViews = lemon::countNodes(g);
  minGraph.resize(nViews);

  //E-- Export compute MST
  for (size_t i= 0; i < tree_edge_vec.size(); i++)
  {
    minGraph[g.id(g.u(tree_edge_vec[i]))].edges.push_back(g.id(g.v(tree_edge_vec[i])));
    minGraph[g.id(g.v(tree_edge_vec[i]))].edges.push_back(g.id(g.u(tree_edge_vec[i])));
  }
  return tree_edge_vec.size();
#endif
}
//----------------------------------------------------------------

// Filter the given relative rotations using the known global rotations
// returns the number of inliers
unsigned int FilterRelativeRotations(
  const RelativeRotations& RelRs,
  const Matrix3x3Arr& Rs,
  float threshold,
  std::vector<bool> * vec_inliers)
{
  assert(!RelRs.empty() && !Rs.empty());
  assert(threshold >= 0);
  // compute errors for each relative rotation
  std::vector<float> errors(RelRs.size());
  for (size_t r= 0; r<RelRs.size(); ++r) {
    const RelativeRotation& relR = RelRs[r];
    const Matrix3x3& Ri = Rs[relR.i];
    const Matrix3x3& Rj = Rs[relR.j];
    const Matrix3x3& Rij = relR.Rij;
    const Mat3 eRij(Rj.transpose()*Rij*Ri);
    const openMVG::Vec3 erij;
    ceres::RotationMatrixToAngleAxis((const double*)eRij.data(), (double*)erij.data());
    errors[r] = (float)erij.norm();
  }
  if (threshold == 0) {
    // estimate threshold
    const std::pair<float,float> res = ComputeX84Threshold(&errors[0], errors.size());
    threshold = res.first+res.second;
  }
  if (vec_inliers)  {
    vec_inliers->resize(RelRs.size());
  }
  // mark outliers
  unsigned int nInliers = 0;
  for (size_t r=0; r<errors.size(); ++r) {
    const bool bInlier = errors[r] < threshold;
    if (vec_inliers)
      (*vec_inliers)[r] = bInlier;
    if (bInlier)
      ++nInliers;
  }
  return nInliers;
} // FilterRelativeRotations
//----------------------------------------------------------------

double RelRotationAvgError
(
  const RelativeRotations& RelRs,
  const Matrix3x3Arr& Rs,
  double* pMin=nullptr,
  double* pMax=nullptr
)
{
#ifdef HAVE_BOOST
  boost::accumulators::accumulator_set<double,
    boost::accumulators::stats<
      boost::accumulators::tag::min,
      boost::accumulators::tag::mean,
      boost::accumulators::tag::max>> acc;

  for (int i=0; i < RelRs.size(); ++i) {
    const RelativeRotation& relR = RelRs[i];
    acc(openMVG::FrobeniusNorm(relR.Rij  - (Rs[relR.j]*Rs[relR.i].transpose())));
  }
  if (pMin)
    *pMin = boost::accumulators::min(acc);
  if (pMax)
    *pMax = boost::accumulators::max(acc);
  return boost::accumulators::mean(acc);
#else
  std::vector<double> vec_err(RelRs.size(), 0.0);
  for (size_t i=0; i < RelRs.size(); ++i) {
    const RelativeRotation& relR = RelRs[i];
    vec_err[i] = openMVG::FrobeniusNorm(relR.Rij  - (Rs[relR.j]*Rs[relR.i].transpose()));
  }
  float min, max, mean, median;
  minMaxMeanMedian(vec_err.begin(), vec_err.end(), min, max, mean, median);
  if (pMin)
    *pMin = min;
  if (pMax)
    *pMax = max;
  return mean;
#endif
}
//----------------------------------------------------------------

void InitRotationsMST
(
  const RelativeRotations& RelRs,
  Matrix3x3Arr& Rs,
  const uint32_t nMainViewID
)
{
  assert(!Rs.empty());

  // -- Compute coarse global rotation estimates:
  //   - by finding the maximum spanning tree and linking the relative rotations
  //   - Initial solution is driven by relative rotations data confidence.
  graph_t g;
  MapEdgeIJ2R mapIJ2R;
  NodeArr minGraph;
  // find the Maximum Spanning Tree
  FindMaximumSpanningTree(RelRs, g, mapIJ2R, minGraph);
  g.clear();

  // start from the main view and link all views using the relative rotation estimates
  LinkQue stack;
  stack.push(Link(nMainViewID, uint32_t(0)));
  Rs[nMainViewID] = Matrix3x3::Identity();
  do {
    const Link& link = stack.front();
    const Node& node = minGraph[link.ID];

    for (Node::InternalType::const_iterator pEdge = node.edges.begin();
      pEdge != node.edges.end(); ++pEdge) {
        const size_t edge = *pEdge;
        if (edge == link.parentID) {
          // compute the global rotation for the current node
          assert(mapIJ2R.find({link.parentID, link.ID}) != mapIJ2R.end());
          const Matrix3x3& Rij = mapIJ2R[{link.parentID, link.ID}];
          Rs[link.ID] = Rij * Rs[link.parentID];
        } else {
          // add edge to the processing queue
          stack.push(Link(edge, link.ID));
        }
    }
    stack.pop();
  } while (!stack.empty());
}

// Robustly estimate global rotations from relative rotations as in:
// "Efficient and Robust Large-Scale Rotation Averaging", Chatterjee and Govindu, 2013
// and detect outliers relative rotations and return them with 0 in arrInliers
bool GlobalRotationsRobust(
  const RelativeRotations& RelRs,
  Matrix3x3Arr& Rs,
  const uint32_t nMainViewID,
  float threshold,
  std::vector<bool> * vec_Inliers)
{
  assert(!Rs.empty());

  // -- Compute coarse global rotation estimates:
  InitRotationsMST(RelRs, Rs, nMainViewID);

  // refine global rotations based on the relative rotations
  const bool bOk = RefineRotationsAvgL1IRLS(RelRs, Rs, nMainViewID);

  // find outlier relative rotations
  if (threshold>=0 && vec_Inliers)  {
    FilterRelativeRotations(RelRs, Rs, threshold, vec_Inliers);
  }

  return bOk;
} // GlobalRotationsRobust
//----------------------------------------------------------------

namespace internal
{

// build A in Ax=b
inline void FillMappingMatrix(
  const RelativeRotations& RelRs,
  const uint32_t nMainViewID,
  sMat& A)
{
  A.reserve(A.rows()*2); // estimate of the number of non-zeros (optional)
  sMat::Index i = 0, j = 0;
  for (size_t r=0; r<RelRs.size(); ++r) {
    const RelativeRotation& relR = RelRs[r];
    if (relR.i != nMainViewID) {
      j = 3*(relR.i<nMainViewID ? relR.i : relR.i-1);
      A.insert(i+0,j+0) = -1.0;
      A.insert(i+1,j+1) = -1.0;
      A.insert(i+2,j+2) = -1.0;
    }
    if (relR.j != nMainViewID) {
      j = 3*(relR.j<nMainViewID ? relR.j : relR.j-1);
      A.insert(i+0,j+0) = 1.0;
      A.insert(i+1,j+1) = 1.0;
      A.insert(i+2,j+2) = 1.0;
    }
    i+=3;
  }
  A.makeCompressed();
}

// compute errors for each relative rotation
inline void FillErrorMatrix(
  const RelativeRotations& RelRs,
  const Matrix3x3Arr& Rs,
  Vec & b)
{
  for (size_t r = 0; r < RelRs.size(); ++r) {
    const RelativeRotation& relR = RelRs[r];
    const Matrix3x3& Ri = Rs[relR.i];
    const Matrix3x3& Rj = Rs[relR.j];
    const Matrix3x3& Rij = relR.Rij;
    const Mat3 eRij(Rj.transpose()*Rij*Ri);
    const openMVG::Vec3 erij;
    ceres::RotationMatrixToAngleAxis((const double*)eRij.data(), (double*)erij.data());
    b.block<3,1>(3*r,0) = erij;
  }
}

// apply correction to global rotations
inline void CorrectMatrix(
  const Mat& x,
  const uint32_t nMainViewID,
  Matrix3x3Arr& Rs)
{
  for (size_t r = 0; r < Rs.size(); ++r) {
    if (r == nMainViewID)
      continue;
    Matrix3x3& Ri = Rs[r];
    const uint32_t i = (r<nMainViewID ? r : r-1);
    const openMVG::Vec3 eRid = openMVG::Vec3(x.block<3,1>(3*i,0));
    const Mat3 eRi;
    ceres::AngleAxisToRotationMatrix((const double*)eRid.data(), (double*)eRi.data());
    Ri = Ri*eRi;
  }
}

// L1RA -> L1 Rotation Averaging implementation
bool SolveL1RA
(
  const RelativeRotations& RelRs,
  Matrix3x3Arr& Rs,
  const sMat & A,
  const unsigned int nMainViewID
)
{
  const unsigned nObss = (unsigned)RelRs.size();
  const unsigned nVars = (unsigned)Rs.size()-1; // one view is kept constant
  const unsigned m = nObss*3;
  const unsigned n = nVars*3;

  // init x with 0 that corresponds to trusting completely the initial Ri guess
  Vec x(Vec::Zero(n)), b(m);

  // Current error and the previous one
  double e = std::numeric_limits<double>::max(), ep;
  unsigned iter = 0;
  // L1RA iterate optimization till the desired precision is reached
  do {
    // compute errors for each relative rotation
    FillErrorMatrix(RelRs, Rs, b);

    // solve the linear system using l1 norm
    L1Solver<sMat >::Options options;
    L1Solver<sMat > l1_solver(options, A);
    l1_solver.Solve(b, &x);

    ep = e; e = x.norm();
    if (ep < e)
      break;
    // apply correction to global rotations
    CorrectMatrix(x, nMainViewID, Rs);
  } while (++iter < 32 && e > 1e-5 && (ep-e)/e > 1e-2);

  std::cout << "L1RA Converged in " << iter << " iterations." << std::endl;

  return true;
}

// Iteratively Reweighted Least Squares (IRLS) implementation
bool SolveIRLS
(
  const RelativeRotations& RelRs,
  Matrix3x3Arr& Rs,
  const sMat & A,
  const unsigned int nMainViewID,
  const double sigma
)
{
  const unsigned nObss = (unsigned)RelRs.size();
  const unsigned nVars = (unsigned)Rs.size()-1; // one view is kept constant
  const unsigned m = nObss*3;
  const unsigned n = nVars*3;

  // init x with 0 that corresponds to trusting completely the initial Ri guess
  Vec x(Vec::Zero(n)), b(m);

  // Since the sparsity pattern will not change with each linear solve
  //  compute it once to speed up the solution time.
  using Linear_Solver_T = Eigen::SimplicialLDLT<sMat >;

  Linear_Solver_T linear_solver;
  linear_solver.analyzePattern(A.transpose() * A);
  if (linear_solver.info() != Eigen::Success) {
    std::cerr << "Cholesky decomposition failed." << std::endl;
    return false;
  }

  const double sigmaSq(Square(sigma));

  Eigen::ArrayXd errors, weights;
  Vec xp(n);
  // current error and the previous one
  double e = std::numeric_limits<double>::max(), ep;
  unsigned int iter = 0;
  do
  {
    xp = x;
    // compute errors for each relative rotation
    FillErrorMatrix(RelRs, Rs, b);

    // Compute the weights for each error term
    errors = (A * x - b).array();

    // compute robust errors using the Huber-like loss function
    weights = sigmaSq / (errors.square() + sigmaSq).square();

    // Update the factorization for the weighted values
    const sMat at_weight = A.transpose() * weights.matrix().asDiagonal();
    linear_solver.factorize(at_weight * A);
    if (linear_solver.info() != Eigen::Success) {
      std::cerr << "Failed to factorize the least squares system." << std::endl;
      return false;
    }

    // Solve the least squares problem
    x = linear_solver.solve(at_weight * b);
    if (linear_solver.info() != Eigen::Success) {
      std::cerr << "Failed to solve the least squares system." << std::endl;
      return false;
    }

    // apply correction to global rotations
    CorrectMatrix(x, nMainViewID, Rs);

    ep = e; e = (xp-x).norm();

  } while (++iter < 32 && e > 1e-5 && (ep-e)/e > 1e-2);

  std::cout << "IRLS Converged in " << iter << " iterations." << std::endl;

  return true;
}

} // namespace internal

// Refine the global rotations using to the given relative rotations, similar to:
// "Efficient and Robust Large-Scale Rotation Averaging", Chatterjee and Govindu, 2013
// L1 Rotation Averaging (L1RA) and Iteratively Reweighted Least Squares (IRLS) implementations combined
bool RefineRotationsAvgL1IRLS(
  const RelativeRotations& RelRs,
  Matrix3x3Arr& Rs,
  const uint32_t nMainViewID,
  const double sigma)
{
  assert(!RelRs.empty() && !Rs.empty());
  assert(Rs[nMainViewID] == Matrix3x3::Identity());

  double fMinBefore, fMaxBefore;
  const double fMeanBefore = RelRotationAvgError(RelRs, Rs, &fMinBefore, &fMaxBefore);

  const unsigned nObss = (unsigned)RelRs.size();
  const unsigned nVars = (unsigned)Rs.size()-1; // main view is kept constant
  const unsigned m = nObss*3;
  const unsigned n = nVars*3;

  // build mapping matrix A in Ax=b
  sMat A(m, n);
  internal::FillMappingMatrix(RelRs, nMainViewID, A);

  if (!internal::SolveL1RA(RelRs, Rs, A, nMainViewID))
  {
    std::cerr << "Could not solve the L1 regression step." << std::endl;
    return false;
  }

  if (!internal::SolveIRLS(RelRs, Rs, A, nMainViewID, sigma))
  {
    std::cerr << "Could not solve the ILRS step." << std::endl;
    return false;
  }

  double fMinAfter, fMaxAfter;
  const double fMeanAfter = RelRotationAvgError(RelRs, Rs, &fMinAfter, &fMaxAfter);

  std::cout << "Refine global rotations using L1RA-IRLS and " << nObss << " relative rotations:\n"
    << " error reduced from " << fMeanBefore << "(" <<fMinBefore << " min, " << fMaxBefore << " max)\n"
    << " to " << fMeanAfter << "(" << fMinAfter << "min,"<< fMaxAfter<< "max)" << std::endl;

  return true;
} // RefineRotationsAvgL1IRLS

} // namespace l1
} // namespace rotation_averaging
} // namespace openMVG


#include "ParticleSystem.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/functional/hash/hash.hpp>

#include <algorithm>
#include <unordered_map>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2017
{
namespace Day20
{

ParticleSystem::ParticleSystem(std::vector<Particle> particles) noexcept
    : m_particles{std::move(particles)}
{

}

void ParticleSystem::simulate(unsigned numSteps)
{
    for (unsigned i = 0; i < numSteps; ++i)
    {
        for (auto& particle : m_particles)
        {
            particle.stepTime();
        }
    }
}

void ParticleSystem::simulateWithCollisions(unsigned numSteps)
{
    for (unsigned i = 0; i < numSteps; ++i)
    {
        simulate(1);
        removeAllCollidingParticles();
    }
}

unsigned ParticleSystem::particleIDClosestToOrigin() const
{
    if (m_particles.empty())
    {
        throw std::runtime_error("No particles.");
    }

    const auto minDistanceIter = std::min_element(m_particles.cbegin(), m_particles.cend(), [](const Particle& lhs, const Particle& rhs)
                                                  {
                                                      return lhs.getPosition().cwiseAbs().sum() < rhs.getPosition().cwiseAbs().sum();
                                                  });

    return minDistanceIter->getParticleID();
}

unsigned ParticleSystem::numParticles() const noexcept
{
    return m_particles.size();
}

struct Vector3DHash
{
    size_t operator()(const Particle::Vector3D& vec) const
    {
        std::size_t seed = 0;

        boost::hash_combine(seed, vec.x());
        boost::hash_combine(seed, vec.y());
        boost::hash_combine(seed, vec.z());

        return seed;
    }
};

void ParticleSystem::removeAllCollidingParticles()
{
    std::unordered_multimap<Particle::Vector3D, Particle, Vector3DHash> positionToParticleMap;

    // Move elements of m_particles into the position map
    for (auto& particle : m_particles)
    {
        auto position = particle.getPosition();
        positionToParticleMap.emplace(std::move(position), std::move(particle));
    }

    m_particles.clear();

    // Non-colliding particles are the ones with a unique position
    // Rebuild m_particles by moving these back from the position map
    for (auto& element : positionToParticleMap)
    {
        if (positionToParticleMap.count(element.first) == 1)
        {
            m_particles.push_back(std::move(element.second));
        }
    }
}

}
}
}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns the `n-`th greatest element strictly less than the parameter

    @par Header <boost/simd/function/predecessor.hpp>

    @par Note

     If @c n is null returns @c x else computes the `n`-th greatest representable value strictly less
     than @c x in its type.  @c n must be positive or null.

      - For integer types it saturates at @ref Valmin.

      - For floating point numbers, all @ref Minf  strict predecessors are @ref Nan.

    @see next, prev, successor, nextafter, Minf, Valmin, Nan

    @par Example:

      @snippet predecessor.cpp predecessor

    @par Possible output:

      @snippet predecessor.txt predecessor

  **/
  ///@{
  Value predecessor(Value const& x);
  Value predecessor(Value const& x, IntegerValue const& n);
  ///@}
} }
#endif

#include <boost/simd/function/scalar/predecessor.hpp>
#include <boost/simd/function/simd/predecessor.hpp>

#endif


#include "functions.hpp"
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

namespace gd {

using namespace boost::numeric::ublas;

void py_export_functions() {
	using namespace boost::python;
	def("matmul_2d_3d", matmul_2d_3d);
	def("matmul_3d_3d", matmul_3d_3d);
	def("velocity_cartesian_to_spherical", velocity_cartesian_to_spherical);
	def("add_to_matrix_indexed", add_to_matrix_indexed);
}

void add_to_matrix_indexed(double_matrix M, int_vector _i, int_vector _j, double_vector weight) {
	double* Mp = M.data().begin();
	double* wp = weight.data().begin();
	int* ip = _i.data().begin();
	int* jp = _j.data().begin();
	int size1 = M.size1();
	int size2 = M.size2();
	int length = min(min(_i.size(), _j.size()), weight.size());
	for(int k = 0; k < length; k++) {
		int i = *ip++;
		int j = *jp++;
		double w = *wp++;
		if((i < size1) && (j < size2) && (i >= 0) && (j >= 0)) {
			int index = j + i * size2;
			Mp[index] += w;
		}
	}
}

void matmul_2d_3d(double_matrix M, double_vector x, double_vector y, double_vector xt, double_vector yt, double_vector zt) {
	double* Mp = M.data().begin();
	int Mstride = M.size2();
	double* xp = x.data().begin();
	double* yp = y.data().begin();
	double* xtp = xt.data().begin();
	double* ytp = yt.data().begin();
	double* ztp = zt.data().begin();
	int size = x.size();
	for(int i = 0; i < size; i++) {
		//for(int j = 0; j < 3; j++) {
		*xtp++ = (*xp) * Mp[0+Mstride*0] + (*yp) * Mp[0+Mstride*1];
		*ytp++ = (*xp) * Mp[1+Mstride*0] + (*yp) * Mp[1+Mstride*1];
		*ztp++ = (*xp) * Mp[2+Mstride*0] + (*yp) * Mp[2+Mstride*1];
		xp++; yp++;
		//}
	}
}
void matmul_3d_3d(double_matrix M, double_vector x, double_vector y, double_vector z, double_vector xt, double_vector yt, double_vector zt) {
	double* Mp = M.data().begin();
	int Mstride = M.size2();
	double* xp = x.data().begin();
	double* yp = y.data().begin();
	double* zp = z.data().begin();
	double* xtp = xt.data().begin();
	double* ytp = yt.data().begin();
	double* ztp = zt.data().begin();
	int size = x.size();
	for(int i = 0; i < size; i++) {
		*xtp++ = (*xp) * Mp[0+Mstride*0] + (*yp) * Mp[0+Mstride*1] + (*zp) * Mp[0+Mstride*2];
		*ytp++ = (*xp) * Mp[1+Mstride*0] + (*yp) * Mp[1+Mstride*1] + (*zp) * Mp[1+Mstride*2];
		*ztp++ = (*xp) * Mp[2+Mstride*0] + (*yp) * Mp[2+Mstride*1] + (*zp) * Mp[2+Mstride*2];
		xp++; yp++; zp++;
	}
}

void velocity_cartesian_to_spherical(double_vector xv, double_vector yv, double_vector zv, double_vector vxv, double_vector vyv, double_vector vzv, double_vector vrv, double_vector vphiv, double_vector vthetav) {
	double *xp = xv.data().begin();
	double *yp = yv.data().begin();
	double *zp = zv.data().begin();
	double *vxp = vxv.data().begin();
	double *vyp = vyv.data().begin();
	double *vzp = vzv.data().begin();
	double *vrp = vrv.data().begin();
	double *vphip = vphiv.data().begin();
	double *vthetap = vthetav.data().begin();
	double *xpend = xv.data().end();
	while(xp != xpend) {
		double x = *xp++;
		double y = *yp++;
		double z = *zp++;
		double vx = *vxp++;
		double vy = *vyp++;
		double vz = *vzp++;
		double r = sqrt(x*x+y*y+z*z);
		double rhosq = (x*x+y*y);
		double rho = sqrt(rhosq);
		//double cosatan2xy = x / rho;
		//double sinatan2xy = y / rho;
		*vrp++ = (vx*x + vy*y + vz*z)/r;
		*vphip++ = (vy*x - vx*y)/rho;
		*vthetap++ = (vx*z*x/rho + vy*z*y/rho - vz*rho)/r;

	}
}

}


#include <iostream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/timer.hpp>

#include <vector>
#include <math.h>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
#include "TwoChQSz.hpp"

void TwoChQSz_UpdateQm1fQ(CNRGbasisarray* pSingleSite,CNRGarray* pAeig,
			  CNRGbasisarray* pAbasis,CNRGmatrix* Qm1fNQ){

  // Boost matrices
  boost::numeric::ublas::matrix<double> Zibl;
  boost::numeric::ublas::matrix<double> Zjbl;
  boost::numeric::ublas::matrix<double> fnbasis;

  boost::numeric::ublas::matrix<double> fnw;
  // Check time
  boost::timer t;
  double time_elapsed;

  //Clear all
  for (int ich=1;ich<=2;ich++)
    {
      Qm1fNQ[ich-1].ClearAll();
      Qm1fNQ[ich-1].SyncNRGarray(*pAeig);
    }

  // Note: assumes  Abasis and Aeig have
  // EXACTLY the same block structure... should check.

  double Qi[2];
  double Qj[2];
  double Szi,Szj;

  // Loop over blocks (icount counts for each matrix)
  int icount[2]={0,0};
  for (int ibl=0;ibl<pAeig->NumBlocks();ibl++)
    {
      Qi[0]=pAeig->GetQNumber(ibl,0);
      Qi[1]=pAeig->GetQNumber(ibl,1);
      Szi=pAeig->GetQNumber(ibl,2);
      int Nstibl=pAeig->GetBlockSize(ibl);

      for (int jbl=ibl+1;jbl<pAeig->NumBlocks();jbl++)
	{
	  Qj[0]=pAeig->GetQNumber(jbl,0);
	  Qj[1]=pAeig->GetQNumber(jbl,1);
	  Szj=pAeig->GetQNumber(jbl,2);
	  int Nstjbl=pAeig->GetBlockSize(jbl);

	  //Loop over channels: check if the matrix elements are non-zero
	  for (int ich=1;ich<=2;ich++)
	    {
	      if (  dEqual(Qi[ich-1]+1.0,Qj[ich-1])&&
		   ( dEqual(Szi+0.5,Szj)||dEqual(Szi-0.5,Szj) )  )
		{
		  // Set Zi,Zj
		  cout << " BC: Nshell = " << pAeig->Nshell << endl;
		  cout << "Matrix in channel: " << ich << endl;
		  cout << "  Setting up Block i : " << ibl
		       << " (size " <<  Nstibl
		       << ") x Block j " << jbl
		       << " (size " << Nstjbl << ") of " << pAeig->NumBlocks() << endl;
		  Qm1fNQ[ich-1].MatBlockMap.push_back(ibl);
		  Qm1fNQ[ich-1].MatBlockMap.push_back(jbl);
		  Qm1fNQ[ich-1].MatBlockBegEnd.push_back(icount[ich-1]);
		  icount[ich-1]+=Nstibl*Nstjbl;
		  Qm1fNQ[ich-1].MatBlockBegEnd.push_back(icount[ich-1]-1);

		  cout << "    Setting up BLAS matrices..." << endl;
		  //boost::numeric::ublas::matrix<double> Zibl(Nstibl,Nstibl);
		  //boost::numeric::ublas::matrix<double> Zjbl(Nstjbl,Nstjbl);
		  Zibl.resize(Nstibl,Nstibl);
		  Zjbl.resize(Nstjbl,Nstjbl);

		  Zibl=pAeig->EigVec2BLAS(ibl);
		  Zjbl=pAeig->EigVec2BLAS(jbl);
		  cout << "    ...Zs done ..." << endl;

		  //boost::numeric::ublas::matrix<double> fnbasis (Nstibl,Nstjbl);
		  fnbasis.resize(Nstibl,Nstjbl);
		  // Set-up fn basis: Loop in the basis states
		  int istbl=0;
		  for (int ist=pAbasis->GetBlockLimit(ibl,0);
		       ist<=pAbasis->GetBlockLimit(ibl,1);ist++)
		    {
		      int typei=pAbasis->iType[ist];
		      int stcfi=pAbasis->StCameFrom[ist];

		      int jstbl=0;
		      for (int jst=pAbasis->GetBlockLimit(jbl,0);
			   jst<=pAbasis->GetBlockLimit(jbl,1);jst++)
			{
			  int typej=pAbasis->iType[jst];
			  int stcfj=pAbasis->StCameFrom[jst];

			  fnbasis(istbl,jstbl)=0.0;

			  if (stcfi==stcfj)
			    {
			    fnbasis(istbl,jstbl)=fd_table(ich,-1,typej,typei)+
			      fd_table(ich,1,typej,typei);
			    }
			  jstbl++;
			}
		      // end jst loop
		      istbl++;
		    }
		  // end ist loop
		  cout << "    ...fnbasis done." << endl;

		  cout << "    Multiplying BLAS matrices... " << endl;

		  fnw.resize(Nstibl,Nstjbl);
		  t.restart();
		  noalias(fnw)=prod (Zibl,
				     boost::numeric::ublas::matrix<double>(prod(fnbasis,trans(Zjbl))) );
		  time_elapsed=t.elapsed();
		  cout << "    ...done. Elapsed time:" << time_elapsed << endl;

// 		  cout << "Z(i="<<ibl<<")  : " <<  Zibl << endl;
// 		  cout << "Z(j="<<jbl<<")  : " <<  Zjbl << endl;
// 		  cout << "fbasis   :" <<  fnbasis << endl;
// 		  cout << "Zi.fbasis.ZjT : " <<  fnw << endl;

		  // Add to Qm1fNQ[ich-1]
		  for (int ii=0;ii<fnw.size1();ii++)
		    for (int jj=0;jj<fnw.size2();jj++)
		      Qm1fNQ[ich-1].MatEl.push_back(fnw(ii,jj));

		}
	      //end if Q=Q'+1 etc

	    }
	  // end channel loop

	}
      // end jbl loop

    }
  // end ibl loop

}
// END subroutine

//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
//////////////////////////////////

void TwoChQSz_UpdateMatrixAfterCutting(CNRGbasisarray* pSingleSite,
				       CNRGbasisarray* pAeigCut,
				       CNRGbasisarray* pAbasis,
				       CNRGmatrix* Qm1fNQ,
				       CNRGmatrix* pMQQp1){

  // Boost matrices
  boost::numeric::ublas::matrix<double> Zibl;
  boost::numeric::ublas::matrix<double> Zjbl;
  boost::numeric::ublas::matrix<double> fnbasis;

  boost::numeric::ublas::matrix<double> fnw;
  // Check time
  boost::timer t;
  double time_elapsed;

  //Clear all
  for (int ich=1;ich<=2;ich++)
    {
      Qm1fNQ[ich-1].ClearAll();
      Qm1fNQ[ich-1].SyncNRGarray(*pAeigCut);
    }

  // Note: assumes  Abasis and Aeig have
  // EXACTLY the same block structure... should check.

  double Qi[2];
  double Qj[2];
  double Szi,Szj;

  double qnums[3];

  // Loop over blocks (icount counts for each matrix)
  int icount[2]={0,0};
  for (int ibl=0;ibl<pAeigCut->NumBlocks();ibl++)
    {
      Qi[0]=pAeigCut->GetQNumber(ibl,0);
      Qi[1]=pAeigCut->GetQNumber(ibl,1);
      Szi=pAeigCut->GetQNumber(ibl,2);
      int Nstibl=pAeigCut->GetBlockSize(ibl);

      // Get the corresponding block in pAbasis!
      qnums[0]=Qi[0];
      qnums[1]=Qi[1];
      qnums[2]=Szi;
      int ii_corr=pAbasis->GetBlockFromQNumbers(qnums);
      int NstiblBC=pAbasis->GetBlockSize(ii_corr);

      for (int jbl=ibl+1;jbl<pAeigCut->NumBlocks();jbl++)
	{
	  Qj[0]=pAeigCut->GetQNumber(jbl,0);
	  Qj[1]=pAeigCut->GetQNumber(jbl,1);
	  Szj=pAeigCut->GetQNumber(jbl,2);
	  int Nstjbl=pAeigCut->GetBlockSize(jbl);

	  // Get the corresponding block in pAbasis!
	  qnums[0]=Qj[0];
	  qnums[1]=Qj[1];
	  qnums[2]=Szj;
	  int jj_corr=pAbasis->GetBlockFromQNumbers(qnums);
	  int NstjblBC=pAbasis->GetBlockSize(jj_corr);

	  if ( (!dEqual(Qj[0],pAbasis->GetQNumber(jj_corr,0)))||
	       (!dEqual(Qj[1],pAbasis->GetQNumber(jj_corr,1)))||
	       (!dEqual(Szj,pAbasis->GetQNumber(jj_corr,2))) )
	    {
	      cout << "Ops, problems with GetBlockFromQNumbers" << endl;
	      pAbasis->PrintQNumbers();
	      cout << qnums[0] << " " << qnums[1] << " " << qnums[2] << endl;
	      cout << pAbasis->GetBlockFromQNumbers(qnums) << endl;
	    }

	  //Loop over channels: check if the matrix elements are non-zero
	  for (int ich=1;ich<=2;ich++)
	    {
	      if (  dEqual(Qi[ich-1]+1.0,Qj[ich-1])&&
		   ( dEqual(Szi+0.5,Szj)||dEqual(Szi-0.5,Szj) )  )
		{
		  // Set Zi,Zj
		  cout << " AC : Nshell = " << pAeigCut->Nshell << endl;
		  cout << "Matrix in channel: " << ich << endl;
		  cout << "  Setting up Block i : " << ibl
		       << " (size " <<  Nstibl << " was " << NstiblBC
		       << ") x Block j " << jbl
		       << " (size " << Nstjbl << " was " << NstjblBC
		       << ") of " << pAeigCut->NumBlocks() << endl;
		  Qm1fNQ[ich-1].MatBlockMap.push_back(ibl);
		  Qm1fNQ[ich-1].MatBlockMap.push_back(jbl);
		  Qm1fNQ[ich-1].MatBlockBegEnd.push_back(icount[ich-1]);
		  icount[ich-1]+=Nstibl*Nstjbl;
		  Qm1fNQ[ich-1].MatBlockBegEnd.push_back(icount[ich-1]-1);

		  cout << "    Setting up BLAS matrices..." << endl;
		  //boost::numeric::ublas::matrix<double> Zibl(Nstibl,Nstibl);
		  //boost::numeric::ublas::matrix<double> Zjbl(Nstjbl,Nstjbl);
		  Zibl.resize(Nstibl,NstiblBC);
		  Zjbl.resize(Nstjbl,NstjblBC);

		  //Zibl=pAeigCut->EigVec2BLAS(ibl);
		  //Zjbl=pAeigCut->EigVec2BLAS(jbl);
		  Zibl=pAeigCut->EigVecCut2BLAS(ibl);
		  Zjbl=pAeigCut->EigVecCut2BLAS(jbl);
		  cout << "    ...Zs done ..." << endl;

		  fnbasis.resize(NstiblBC,NstjblBC);
		  // Set-up fn basis: Loop in the basis states
		  // Watch out here... ii_corr, not ibl
		  int istbl=0;
		  for (int ist=pAbasis->GetBlockLimit(ii_corr,0);
		       ist<=pAbasis->GetBlockLimit(ii_corr,1);ist++)
		    {
		      int typei=pAbasis->iType[ist];
		      int stcfi=pAbasis->StCameFrom[ist];

		      int jstbl=0;
		      for (int jst=pAbasis->GetBlockLimit(jj_corr,0);
			   jst<=pAbasis->GetBlockLimit(jj_corr,1);jst++)
			{
			  int typej=pAbasis->iType[jst];
			  int stcfj=pAbasis->StCameFrom[jst];

			  fnbasis(istbl,jstbl)=0.0;

			  if (stcfi==stcfj)
			    {
			    fnbasis(istbl,jstbl)=fd_table(ich,-1,typej,typei)+
			      fd_table(ich,1,typej,typei);
			    }
			  jstbl++;
			}
		      // end jst loop
		      istbl++;
		    }
		  // end ist loop
		  cout << "    ...fnbasis done." << endl;

		  cout << "    Multiplying BLAS matrices... " << endl;

		  fnw.resize(Nstibl,Nstjbl);
		  t.restart();
		  noalias(fnw)=prod (Zibl,
				     boost::numeric::ublas::matrix<double>(prod(fnbasis,trans(Zjbl))) );
		  time_elapsed=t.elapsed();
		  cout << "    ...done. Elapsed time:" << time_elapsed << endl;

// 		  cout << "Z(i="<<ibl<<")  : " <<  Zibl << endl;
// 		  cout << "Z(j="<<jbl<<")  : " <<  Zjbl << endl;
// 		  cout << "fbasis   :" <<  fnbasis << endl;
// 		  cout << "Zi.fbasis.ZjT : " <<  fnw << endl;

		  // Add to Qm1fNQ[ich-1]
		  for (int ii=0;ii<fnw.size1();ii++)
		    for (int jj=0;jj<fnw.size2();jj++)
		      Qm1fNQ[ich-1].MatEl.push_back(fnw(ii,jj));

		}
	      //end if Q=Q'+1 etc

	    }
	  // end channel loop

	}
      // end jbl loop

    }
  // end ibl loop

}
//////////////////////////////////
//////////////////////////////////
//////////////////////////////////
//////////////////////////////////


// =========================================================================
// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
// =========================================================================

#include "NeuralNetwork.h"

#include <cassert>
#include <map>
#include <regex>
#include <sstream>
#include <boost/algorithm/string.hpp>

// -------------------------------------------------------------------------
template< class _TScl >
NeuralNetwork< _TScl >::
NeuralNetwork( )
{
}

// -------------------------------------------------------------------------
template< class _TScl >
NeuralNetwork< _TScl >::
NeuralNetwork( const Self& o )
{
  this->m_L.clear( );
  this->m_L.insert( this->m_L.begin( ), o.m_L.begin( ), o.m_L.end( ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename NeuralNetwork< _TScl >::
Self& NeuralNetwork< _TScl >::
operator=( const Self& o )
{
  this->m_L.clear( );
  this->m_L.insert( this->m_L.begin( ), o.m_L.begin( ), o.m_L.end( ) );
  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
add( unsigned int i, unsigned int o, const std::string& f )
{
  this->add( TLayer( i, o, f ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
add( unsigned int o, const std::string& f )
{
  assert( this->m_L.size( ) > 0 && "At least one layer is needed" );

  this->add( this->m_L.back( ).output_size( ), o, f );
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
add( const TMatrix& w, const TColVector& b, const std::string& f )
{
  this->add( TLayer( w, b, f ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
add( const TLayer& l )
{
  if( this->m_L.size( ) > 0 )
    assert(
      l.input_size( ) == this->m_L.back( ).output_size( ) && "Invalid sizes"
      );
  this->m_L.push_back( l );

  // Normalization transforms
  if( this->m_L.size( ) == 1 )
  {
    unsigned int n = this->m_L[ 0 ].input_size( );
    this->m_NormalizationOffset = TColVector::Zero( n );
    this->m_NormalizationScale = TMatrix::Identity( n, n );
  } // end if
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
load_topology( std::istream& is )
{
  unsigned int L;
  std::map< unsigned int, unsigned long > K;
  std::map< unsigned int, std::string > F;

  // Read parameters
  std::regex r( "\\s+" );
  std::string line;
  while( std::getline( is, line ) )
  {
    line = std::regex_replace( line, r, "" );
    std::vector< std::string > tokens;
    boost::algorithm::split( tokens, line, boost::is_any_of( "=" ) );
    if( tokens.size( ) == 2 )
    {
      if( tokens[ 0 ] == "L" )
      {
        std::istringstream data( tokens[ 1 ] );
        data >> L;
      }
      else
      {
        if( tokens[ 0 ][ 0 ] == 'k' )
        {
          std::istringstream i_str( tokens[ 0 ].substr( 1 ) );
          std::istringstream k_str( tokens[ 1 ] );
          unsigned int i;
          unsigned long k;
          i_str >> i;
          k_str >> k;
          K[ i ] = k;
        }
        else if( tokens[ 0 ][ 0 ] == 'f' )
        {
          std::istringstream i_str( tokens[ 0 ].substr( 1 ) );
          unsigned int i;
          i_str >> i;
          F[ i ] = tokens[ 1 ];
        } // end if
      } // end if
    } // end if
  } // end while

  // Real build
  if( F.size( ) == L && K.size( ) == L + 1 )
    for( unsigned int l = 0; l < L; ++l )
      this->add( K[ l ], K[ l + 1 ], F[ l ] );
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
set( unsigned int l, const TMatrix& w, const TColVector& b )
{
  if( l < this->m_L.size( ) )
  {
    this->m_L[ l ].weights( ) = w;
    this->m_L[ l ].biases( ) = b;
  } // end if
}

// -------------------------------------------------------------------------
template< class _TScl >
unsigned int NeuralNetwork< _TScl >::
number_of_layers( ) const
{
  return( this->m_L.size( ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename NeuralNetwork< _TScl >::
TMatrix& NeuralNetwork< _TScl >::
weights( unsigned int l )
{
  static TMatrix w( 1, 1 );
  if( l < this->m_L.size( ) )
    return( this->m_L[ l ].weights( ) );
  else
    return( w );
}

// -------------------------------------------------------------------------
template< class _TScl >
const typename NeuralNetwork< _TScl >::
TMatrix& NeuralNetwork< _TScl >::
weights( unsigned int l ) const
{
  static const TMatrix w( 1, 1 );
  if( l < this->m_L.size( ) )
    return( this->m_L[ l ].weights( ) );
  else
    return( w );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename NeuralNetwork< _TScl >::
TColVector& NeuralNetwork< _TScl >::
biases( unsigned int l )
{
  static TColVector b( 1 );
  if( l < this->m_L.size( ) )
    return( this->m_L[ l ].biases( ) );
  else
    return( b );
}

// -------------------------------------------------------------------------
template< class _TScl >
const typename NeuralNetwork< _TScl >::
TColVector& NeuralNetwork< _TScl >::
biases( unsigned int l ) const
{
  static const TColVector b( 1 );
  if( l < this->m_L.size( ) )
    return( this->m_L[ l ].biases( ) );
  else
    return( b );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename NeuralNetwork< _TScl >::
TActivation* NeuralNetwork< _TScl >::
sigma( unsigned int l )
{
  if( l < this->m_L.size( ) )
    return( this->m_L[ l ].sigma( ) );
  else
    return( nullptr );
}

// -------------------------------------------------------------------------
template< class _TScl >
const typename NeuralNetwork< _TScl >::
TActivation* NeuralNetwork< _TScl >::
sigma( unsigned int l ) const
{
  if( l < this->m_L.size( ) )
    return( this->m_L[ l ].sigma( ) );
  else
    return( nullptr );
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
setNormalizationOffset( const TColVector& o )
{
  this->m_NormalizationOffset = o;
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
setNormalizationScale( const TMatrix& s )
{
  this->m_NormalizationScale = s;
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
init( )
{
  // Weights
  for( TLayer& l: this->m_L )
    l.init( );

  // Normalization transforms
  unsigned int n = this->m_L[ 0 ].input_size( );
  this->m_NormalizationOffset = TColVector::Zero( n );
  this->m_NormalizationScale = TMatrix::Identity( n, n );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename NeuralNetwork< _TScl >::
TMatrix NeuralNetwork< _TScl >::
f( const TMatrix& x ) const
{
  typename TLayers::const_iterator lIt = this->m_L.begin( );
  TMatrix z = lIt->f(
    this->m_NormalizationScale *
    ( x.colwise( ) + this->m_NormalizationOffset )
    );
  for( lIt++; lIt != this->m_L.end( ); ++lIt )
    z = lIt->f( z );
  return( z );
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
f( std::vector< TMatrix >& a, std::vector< TMatrix >& z ) const
{
  typename TLayers::const_iterator lIt = this->m_L.begin( );
  typename std::vector< TMatrix >::iterator aIt, bIt, zIt;
  aIt = bIt = a.begin( );
  zIt = z.begin( );

  for( bIt++; lIt != this->m_L.end( ); ++lIt, ++aIt, ++bIt, ++zIt )
    *bIt = lIt->f( *aIt, *zIt );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename NeuralNetwork< _TScl >::
TMatrix NeuralNetwork< _TScl >::
t( const TMatrix& x ) const
{
  return( this->m_L.back( ).sigma( )->t( this->f( x ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename NeuralNetwork< _TScl >::
TMatrix NeuralNetwork< _TScl >::
_d( const unsigned int& l, const TMatrix& z ) const
{
  assert( l < this->m_L.size( ) && "Layer does not exist" );

  return( this->m_L[ l ].sigma( )->d( z ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
_read_from( std::istream& i )
{
  unsigned int L;
  i >> L;
  for( unsigned int n = 0; n < L; ++n )
  {
    TLayer l;
    i >> l;
    this->add( l );
  } // end for

  // Read normalization parameters
  unsigned int N = this->m_L[ 0 ].input_size( );
  this->m_NormalizationOffset = TColVector::Zero( N );
  this->m_NormalizationScale = TMatrix::Identity( N, N );
  for( unsigned int n = 0; n < N; ++n )
    i >> this->m_NormalizationOffset( n, 0 );
  for( unsigned int x = 0; x < N; ++x )
    for( unsigned int y = 0; y < N; ++y )
      i >> this->m_NormalizationScale( x, y );
}

// -------------------------------------------------------------------------
template< class _TScl >
void NeuralNetwork< _TScl >::
_copy_to( std::ostream& o ) const
{
  o << this->m_L.size( ) << std::endl;
  for( const TLayer& l: this->m_L )
    o << l << std::endl;
  o << this->m_NormalizationOffset << std::endl;
  o << this->m_NormalizationScale << std::endl;
}

// -------------------------------------------------------------------------
template class NeuralNetwork< float >;
template class NeuralNetwork< double >;
template class NeuralNetwork< long double >;

// eof - $RCSfile$