/**
 *          Copyright Matthias Walter 2010.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 **/

#include "../config.h"
#include <fstream>
#include <iomanip>
#include <map>

#include <boost/logic/tribool.hpp>

#include "total_unimodularity.hpp"
#include "matroid_decomposition.hpp"
#include "unimodularity.hpp"
#include "smith_normal_form.hpp"

template <typename Set, typename Element>
bool contains(const Set& set, const Element& element)
{
  return set.find(element) != set.end();
}

void print_matroid_graph(const unimod::matroid_graph& graph, const std::string& indent = "")
{
  std::cout << boost::num_vertices(graph) << " nodes and " << boost::num_edges(graph) << " edges:";

  typedef boost::graph_traits <unimod::matroid_graph> traits;
  traits::vertex_iterator vertex_iter, vertex_end;
  traits::out_edge_iterator edge_iter, edge_end;

  for (boost::tie(vertex_iter, vertex_end) = boost::vertices(graph); vertex_iter != vertex_end; ++vertex_iter)
  {
    std::cout << '\n' << indent << *vertex_iter << ':';
    for (boost::tie(edge_iter, edge_end) = boost::out_edges(*vertex_iter, graph); edge_iter != edge_end; ++edge_iter)
    {
      int matroid_element = boost::get(unimod::edge_matroid_element, graph, *edge_iter);
      std::cout << ' ' << boost::target(*edge_iter, graph) << " (" << (matroid_element < 0 ? "row " : "column ") << matroid_element << ") ";
    }
  }
  std::cout << '\n';
}

void print_decomposition(const unimod::decomposed_matroid* decomposition, std::string indent = "")
{
  if (decomposition->is_leaf())
  {
    unimod::decomposed_matroid_leaf* leaf = (unimod::decomposed_matroid_leaf*) (decomposition);

    if (leaf->is_R10())
    {
      std::cout << indent << "R10:";
      for (unimod::matroid_element_set::const_iterator iter = leaf->elements().begin(); iter != leaf->elements().end(); ++iter)
        std::cout << " " << *iter;
      std::cout << "\n";
    }
    else if (leaf->is_graphic() && leaf->is_cographic())
    {
      std::cout << indent << "planar binary matroid.\n";
      std::cout << indent << "graph:\n" << indent << "{ ";
      print_matroid_graph(*leaf->graph(), indent + "  ");
      std::cout << indent << "}\n" << indent << "cograph:\n" << indent << "{ ";
      print_matroid_graph(*leaf->cograph(), indent + "  ");
      std::cout << indent << "}\n";
    }
    else if (leaf->is_graphic())
    {
      std::cout << indent << "graphic binary matroid.\n";
      std::cout << indent << "graph:\n" << indent << "{ ";
      print_matroid_graph(*leaf->graph(), indent + "  ");
      std::cout << indent << "}\n";
    }
    else if (leaf->is_cographic())
    {
      std::cout << indent << "cographic binary matroid.\n";
      std::cout << indent << "cograph:\n" << indent << "{ ";
      print_matroid_graph(*leaf->cograph(), indent + "  ");
      std::cout << indent << "}\n";
    }
    else
    {
      std::cout << indent << "irregular matroid.\n";
    }
  }
  else
  {
    unimod::decomposed_matroid_separator* separator = (unimod::decomposed_matroid_separator*) (decomposition);

    if (separator->separation_type() == unimod::decomposed_matroid_separator::ONE_SEPARATION)
    {
      std::cout << indent << "1-separation:\n";

    }
    else if (separator->separation_type() == unimod::decomposed_matroid_separator::TWO_SEPARATION)
    {
      std::cout << indent << "2-separation:\n";
    }
    else if (separator->separation_type() == unimod::decomposed_matroid_separator::THREE_SEPARATION)
    {
      std::cout << indent << "3-separation:\n";
    }
    else
    {
      std::cout << indent << "invalid separation:\n";
    }
    std::cout << indent << "{\n";
    print_decomposition(separator->first(), indent + "  ");
    print_decomposition(separator->second(), indent + "  ");
    std::cout << indent << "}\n";
  }
}

void print_violator(const unimod::integer_matrix& matrix, const unimod::submatrix_indices& violator)
{
  typedef boost::numeric::ublas::matrix_indirect <const unimod::integer_matrix, unimod::submatrix_indices::indirect_array_type> indirect_matrix_t;

  const indirect_matrix_t indirect_matrix(matrix, violator.rows, violator.columns);

  for (size_t row = 0; row < indirect_matrix.size1(); ++row)
  {
    for (size_t column = 0; column < indirect_matrix.size2(); ++column)
    {
      std::cout << std::setw(4) << indirect_matrix(row, column);
    }
    std::cout << '\n';
  }
  std::cout << "\nRow indices in range [0," << (matrix.size1() - 1) << "]:\n";
  for (size_t row = 0; row < violator.rows.size(); ++row)
    std::cout << (row == 0 ? "" : " ") << violator.rows[row];
  std::cout << "\n\nColumn indices in range [0," << (matrix.size2() - 1) << "]:\n";
  for (size_t column = 0; column < violator.columns.size(); ++column)
    std::cout << (column == 0 ? "" : " ") << violator.columns[column] << ' ';
  std::cout << std::endl;
}

void print_result(std::ostream& stream, const std::string& name, boost::logic::tribool result)
{
  stream << std::setw(20) << name << ": ";

  if (result)
    stream << "yes\n";
  else if (!result)
    stream << "no\n";
  else
    stream << "not determined\n";
}

bool test_total_unimodularity(unimod::integer_matrix& matrix, bool show_certificates, unimod::log_level level)
{
  bool result;

  if (show_certificates)
  {
    unimod::submatrix_indices violator;
    unimod::decomposed_matroid* decomposition;

    result = unimod::is_totally_unimodular(matrix, decomposition, violator, level);

    if (result)
    {
      std::cout << "\nThe matrix is totally unimodular due to the following decomposition:\n" << std::endl;

      print_decomposition(decomposition);
    }
    else
    {
      int det = unimod::submatrix_determinant(matrix, violator);
      assert (violator.rows.size() == violator.columns.size());
      std::cout << "\nThe matrix is not totally unimodular due to the following " << violator.rows.size() << " x " << violator.columns.size()
          << " submatrix with determinant " << det << "." << std::endl;
      print_violator(matrix, violator);
    }
    delete decomposition;
  }
  else
  {
    result = unimod::is_totally_unimodular(matrix, level);
    std::cout << "The matrix is " << (result ? "" : "not ") << "totally unimodular." << std::endl;
  }

  return result;
}

int run(const std::string& file_name, const std::set <char>& tests, bool show_certificates, unimod::log_level level)
{
  /// Open the file

  std::ifstream file(file_name.c_str());
  if (!file.good())
  {
    std::cout << "Error: cannot open file \"" << file_name << "\"." << std::endl;
    return EXIT_FAILURE;
  }

  /// Read height and width

  size_t height, width;
  file >> height >> width;
  if (!file.good())
  {
    std::cout << "Error: cannot read matrix size from input file." << std::endl;
    return EXIT_FAILURE;
  }

  /// Read matrix entries

  unimod::integer_matrix matrix(height, width);
  for (size_t row = 0; row < height; ++row)
  {
    for (size_t column = 0; column < width; ++column)
    {
      if (!file.good())
      {
        std::cout << "Error: cannot read matrix data." << std::endl;
      }
      int value;
      file >> value;
      matrix(row, column) = value;
    }
  }

  file.close();
  std::cout << "Read a " << matrix.size1() << " x " << matrix.size2() << " matrix.\n" << std::endl;

  std::map <char, boost::logic::tribool> results;
  for (size_t i = 0; i < 5; ++i)
    results["tUuMm"[i]] = boost::logic::indeterminate;
  size_t rank = 0;
  bool know_rank = false;
  unsigned int k = 0;

  if (contains(tests, 't'))
  {
    /// Let's test for total unimodularity.

    results['t'] = test_total_unimodularity(matrix, show_certificates, level);
    k = 1;
  }

  if (results['t'])
  {
    for (size_t i = 0; i < 4; ++i)
    {
      results["uUmM"[i]] = true;
    }
  }
  else
  {
    if (contains(tests, 'm') || contains(tests, 'M'))
    {
      if (level != unimod::LOG_QUIET)
        std::cout << "Testing matrix for k-modularity... " << std::flush;
      results['m'] = unimod::is_k_modular(matrix, rank, k, unimod::LOG_PROGRESSIVE);
      std::cout << "The matrix is " << (results['m'] ? "" : "not ") << "k-modular.\n" << std::flush;

      results['u'] = (results['m'] && k == 1);
      if (results['m'])
        std::cout << "The matrix is " << (results['u'] ? "" : "not ") << "unimodular.\n" << std::flush;

      if (!results['m'])
        results['M'] = false;
      if (!results['u'])
      {
        results['U'] = false;
        results['t'] = false;
      }
      know_rank = true;
    }
    else if (contains(tests, 'u') || contains(tests, 'U'))
    {
      if (level != unimod::LOG_QUIET)
        std::cout << "Testing matrix for unimodularity... " << std::flush;
      results['u'] = unimod::is_unimodular(matrix, rank, unimod::LOG_QUIET);
      std::cout << "The matrix is " << (results['u'] ? "" : "not ") << "unimodular.\n" << std::flush;

      if (results['u'])
        results['m'] = true;
      if (!results['u'])
      {
        results['U'] = false;
        results['t'] = false;
      }
      know_rank = true;
    }
    if (contains(tests, 'M') && boost::logic::indeterminate(results['M']))
    {
      if (level != unimod::LOG_QUIET)
        std::cout << "Testing transpose of matrix for k-modularity... " << std::flush;
      unimod::matrix_transposed <unimod::integer_matrix> transposed(matrix);
      results['M'] = unimod::is_k_modular(transposed, rank, k, unimod::LOG_QUIET);
      std::cout << "The transpose is " << (results['M'] ? "" : "not ") << "k-modular.\n" << std::flush;

      results['U'] = (results['M'] && k == 1);
      if (results['M'])
        std::cout << "The transpose is " << (results['U'] ? "" : "not ") << "unimodular.\n" << std::flush;

      if (!results['U'])
        results['t'] = false;
      know_rank = true;
    }
    else if (contains(tests, 'U') && boost::logic::indeterminate(results['U']))
    {
      if (level != unimod::LOG_QUIET)
        std::cout << "Testing transpose of matrix for unimodularity... " << std::flush;
      unimod::matrix_transposed <unimod::integer_matrix> transposed(matrix);
      results['U'] = unimod::is_unimodular(transposed, rank, unimod::LOG_QUIET);
      std::cout << "The transpose is " << (results['U'] ? "" : "not ") << "unimodular.\n" << std::flush;

      if (!results['U'])
        results['t'] = false;
      know_rank = true;
    }
  }

  /// Print a summary

  if (know_rank)
    std::cout << "\nSummary of rank " << rank << " matrix:\n\n";
  else
    std::cout << "\nSummary:\n\n";

  print_result(std::cout, "Totally unimodular", results['t']);
  print_result(std::cout, "Strongly unimodular", results['U']);
  print_result(std::cout, "Unimodular", results['u']);
  print_result(std::cout, "Strongly k-modular", results['M']);
  print_result(std::cout, "k-modular", results['m']);
  if (results['m'])
    std::cout << "                  k = " << k << "\n";
  if (know_rank)
    print_result(std::cout, "Dantzig property", results['m'] && rank == matrix.size1());

  std::cout << std::flush;

  return EXIT_SUCCESS;
}

bool extract_option(char c, std::set <char>& tests, bool& certs, unimod::log_level& level, bool& help)
{
  if (c == 't' || c == 'u' || c == 'm' || c == 'U' || c == 'M')
    tests.insert(c);
  else if (c == 'a')
  {
    tests.insert('t');
    tests.insert('u');
    tests.insert('U');
    tests.insert('m');
    tests.insert('M');
  }
  else if (c == 'h')
    help = true;
  else if (c == 'c')
    certs = true;
  else if (c == 'q')
    level = unimod::LOG_QUIET;
  else if (c == 'p')
    level = unimod::LOG_PROGRESSIVE;
  else if (c == 'v')
    level = unimod::LOG_VERBOSE;
  else
    return false;

  return true;
}

int main(int argc, char **argv)
{
  /// Possible parameters
  std::string matrix_file_name = "";
  bool certs = false;
  unimod::log_level level = unimod::LOG_PROGRESSIVE;
  bool help = false;
  std::set <char> tests;

  bool options_done = false;
  for (int a = 1; a < argc; ++a)
  {
    const std::string current = argv[a];
    if (!options_done)
    {
      if (current == std::string("--"))
      {
        options_done = true;
        continue;
      }
      else if (current != "" && current[0] == '-')
      {
        for (size_t i = 1; i < current.size(); ++i)
        {
          if (!extract_option(current[i], tests, certs, level, help))
          {
            std::cerr << "Unknown option: -" << current[i] << "\nSee " << argv[0] << " -h for usage." << std::endl;
            return EXIT_FAILURE;
          }
        }
        continue;
      }
    }

    if (matrix_file_name != "")
    {
      std::cerr << "Matrix file was given twice!\nSee " << argv[0] << " -h for usage." << std::endl;
      return EXIT_FAILURE;
    }
    matrix_file_name = current;
  }

  if (tests.empty())
  {
    tests.insert('t');
    tests.insert('u');
    tests.insert('U');
    tests.insert('m');
    tests.insert('M');
  }

  if (help)
  {
    std::cerr << "Usage: " << argv[0] << " [OPTIONS] [--] MATRIX_FILE\n";
    std::cerr << "Options:\n";
    std::cerr << " -h Shows a help message.\n";
    std::cerr << " -a Test for everything possible (default).\n";
    std::cerr << " -t Test for total unimodularity.\n";
    std::cerr << " -U Test for strong unimodularity.\n";
    std::cerr << " -u Test for unimodularity.\n";
    std::cerr << " -M Test for strong k-modularity.\n";
    std::cerr << " -m Test for k-modularity.\n";
    std::cerr << " -c Prints certificates: Try to find certificates for the results.\n";
    std::cerr << " -p Progressive logging (default).\n";
    std::cerr << " -v Verbose logging.\n";
    std::cerr << " -q No logging at all.\n";
    std::cerr << std::flush;
    return EXIT_SUCCESS;
  }

  if (matrix_file_name == "")
  {
    std::cerr << "No matrix file was given!\nSee " << argv[0] << " -h for usage." << std::endl;
    return EXIT_FAILURE;
  }

  return run(matrix_file_name, tests, certs, level);
}


/**
 * @file feedforward_network_test.cpp
 * @author Marcus Edel
 * @author Palash Ahuja
 *
 * Tests the feed forward network.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/activation_functions/logistic_function.hpp>
#include <mlpack/methods/ann/activation_functions/tanh_function.hpp>

#include <mlpack/methods/ann/init_rules/random_init.hpp>

#include <mlpack/methods/ann/layer/bias_layer.hpp>
#include <mlpack/methods/ann/layer/linear_layer.hpp>
#include <mlpack/methods/ann/layer/base_layer.hpp>
#include <mlpack/methods/ann/layer/dropout_layer.hpp>
#include <mlpack/methods/ann/layer/binary_classification_layer.hpp>
#include <mlpack/methods/ann/layer/dropconnect_layer.hpp>

#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/performance_functions/mse_function.hpp>
#include <mlpack/core/optimizers/rmsprop/rmsprop.hpp>

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

using namespace mlpack;
using namespace mlpack::ann;
using namespace mlpack::optimization;

BOOST_AUTO_TEST_SUITE(FeedForwardNetworkTest);

/**
 * Train and evaluate a vanilla network with the specified structure.
 */
template<
    typename PerformanceFunction,
    typename OutputLayerType,
    typename PerformanceFunctionType,
    typename MatType = arma::mat
>
void BuildVanillaNetwork(MatType& trainData,
                         MatType& trainLabels,
                         MatType& testData,
                         MatType& testLabels,
                         const size_t hiddenLayerSize,
                         const size_t maxEpochs,
                         const double classificationErrorThreshold)
{
  /*
   * Construct a feed forward network with trainData.n_rows input nodes,
   * hiddenLayerSize hidden nodes and trainLabels.n_rows output nodes. The
   * network structure looks like:
   *
   *  Input         Hidden        Output
   *  Layer         Layer         Layer
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |     +>|     |     +>|     |
   * +-----+     | +--+--+     | +-----+
   *             |             |
   *  Bias       |  Bias       |
   *  Layer      |  Layer      |
   * +-----+     | +-----+     |
   * |     |     | |     |     |
   * |     +-----+ |     +-----+
   * |     |       |     |
   * +-----+       +-----+
   */

  LinearLayer<> inputLayer(trainData.n_rows, hiddenLayerSize);
  BiasLayer<> inputBiasLayer(hiddenLayerSize);
  BaseLayer<PerformanceFunction> inputBaseLayer;

  LinearLayer<> hiddenLayer1(hiddenLayerSize, trainLabels.n_rows);
  BiasLayer<> hiddenBiasLayer1(trainLabels.n_rows);
  BaseLayer<PerformanceFunction> outputLayer;

  OutputLayerType classOutputLayer;

  auto modules = std::tie(inputLayer, inputBiasLayer, inputBaseLayer,
                          hiddenLayer1, hiddenBiasLayer1, outputLayer);

  FFN<decltype(modules), decltype(classOutputLayer), RandomInitialization,
      PerformanceFunctionType> net(modules, classOutputLayer);

  RMSprop<decltype(net)> opt(net, 0.01, 0.88, 1e-8,
      maxEpochs * trainData.n_cols, 1e-18);

  net.Train(trainData, trainLabels, opt);

  MatType prediction;
  net.Predict(testData, prediction);

  size_t error = 0;
  for (size_t i = 0; i < testData.n_cols; i++)
  {
    if (arma::sum(arma::sum(
        arma::abs(prediction.col(i) - testLabels.col(i)))) == 0)
    {
      error++;
    }
  }

  double classificationError = 1 - double(error) / testData.n_cols;
  BOOST_REQUIRE_LE(classificationError, classificationErrorThreshold);
}

/**
 * Train the vanilla network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(VanillaNetworkTest)
{
  // Load the dataset.
  arma::mat dataset;
  data::Load("thyroid_train.csv", dataset, true);

  arma::mat trainData = dataset.submat(0, 0, dataset.n_rows - 4,
      dataset.n_cols - 1);
  arma::mat trainLabels = dataset.submat(dataset.n_rows - 3, 0,
      dataset.n_rows - 1, dataset.n_cols - 1);

  data::Load("thyroid_test.csv", dataset, true);

  arma::mat testData = dataset.submat(0, 0, dataset.n_rows - 4,
      dataset.n_cols - 1);
  arma::mat testLabels = dataset.submat(dataset.n_rows - 3, 0,
      dataset.n_rows - 1, dataset.n_cols - 1);

  // Vanilla neural net with logistic activation function.
  // Because 92 percent of the patients are not hyperthyroid the neural
  // network must be significant better than 92%.
  BuildVanillaNetwork<LogisticFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (trainData, trainLabels, testData, testLabels, 8, 200, 0.1);

  dataset.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  for (size_t i = 0; i < dataset.n_cols; ++i)
    dataset.col(i) /= norm(dataset.col(i), 2);

  arma::mat labels = arma::zeros(1, dataset.n_cols);
  labels.submat(0, labels.n_cols / 2, 0, labels.n_cols - 1).fill(1);

  // Vanilla neural net with logistic activation function.
  BuildVanillaNetwork<LogisticFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (dataset, labels, dataset, labels, 30, 30, 0.4);

  // Vanilla neural net with tanh activation function.
  BuildVanillaNetwork<TanhFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
    (dataset, labels, dataset, labels, 10, 30, 0.4);
}

/**
 * Train and evaluate a Dropout network with the specified structure.
 */
template<
    typename PerformanceFunction,
    typename OutputLayerType,
    typename PerformanceFunctionType,
    typename MatType = arma::mat
>
void BuildDropoutNetwork(MatType& trainData,
                         MatType& trainLabels,
                         MatType& testData,
                         MatType& testLabels,
                         const size_t hiddenLayerSize,
                         const size_t maxEpochs,
                         const double classificationErrorThreshold)
{
  /*
   * Construct a feed forward network with trainData.n_rows input nodes,
   * hiddenLayerSize hidden nodes and trainLabels.n_rows output nodes. The
   * network structure looks like:
   *
   *  Input         Hidden        Dropout      Output
   *  Layer         Layer         Layer        Layer
   * +-----+       +-----+       +-----+       +-----+
   * |     |       |     |       |     |       |     |
   * |     +------>|     +------>|     +------>|     |
   * |     |     +>|     |       |     |       |     |
   * +-----+     | +--+--+       +-----+       +-----+
   *             |
   *  Bias       |
   *  Layer      |
   * +-----+     |
   * |     |     |
   * |     +-----+
   * |     |
   * +-----+
   */

  LinearLayer<> inputLayer(trainData.n_rows, hiddenLayerSize);
  BiasLayer<> biasLayer(hiddenLayerSize);
  BaseLayer<PerformanceFunction> hiddenLayer0;
  DropoutLayer<> dropoutLayer0;

  LinearLayer<> hiddenLayer1(hiddenLayerSize, trainLabels.n_rows);
  BaseLayer<PerformanceFunction> outputLayer;

  OutputLayerType classOutputLayer;

  auto modules = std::tie(inputLayer, biasLayer, hiddenLayer0, dropoutLayer0,
                          hiddenLayer1, outputLayer);

  FFN<decltype(modules), decltype(classOutputLayer), RandomInitialization,
      PerformanceFunctionType> net(modules, classOutputLayer);

  RMSprop<decltype(net)> opt(net, 0.01, 0.88, 1e-8,
      maxEpochs * trainData.n_cols, 1e-18);

  net.Train(trainData, trainLabels, opt);

  MatType prediction;
  net.Predict(testData, prediction);

  size_t error = 0;
  for (size_t i = 0; i < testData.n_cols; i++)
  {
    if (arma::sum(arma::sum(
        arma::abs(prediction.col(i) - testLabels.col(i)))) == 0)
    {
      error++;
    }
  }

  double classificationError = 1 - double(error) / testData.n_cols;
  BOOST_REQUIRE_LE(classificationError, classificationErrorThreshold);
}

/**
 * Train the dropout network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(DropoutNetworkTest)
{
  // Load the dataset.
  arma::mat dataset;
  data::Load("thyroid_train.csv", dataset, true);

  arma::mat trainData = dataset.submat(0, 0, dataset.n_rows - 4,
      dataset.n_cols - 1);
  arma::mat trainLabels = dataset.submat(dataset.n_rows - 3, 0,
      dataset.n_rows - 1, dataset.n_cols - 1);

  data::Load("thyroid_test.csv", dataset, true);

  arma::mat testData = dataset.submat(0, 0, dataset.n_rows - 4,
      dataset.n_cols - 1);
  arma::mat testLabels = dataset.submat(dataset.n_rows - 3, 0,
      dataset.n_rows - 1, dataset.n_cols - 1);

  // Vanilla neural net with logistic activation function.
  // Because 92 percent of the patients are not hyperthyroid the neural
  // network must be significant better than 92%.
  BuildDropoutNetwork<LogisticFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (trainData, trainLabels, testData, testLabels, 4, 100, 0.1);

  dataset.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  for (size_t i = 0; i < dataset.n_cols; ++i)
    dataset.col(i) /= norm(dataset.col(i), 2);

  arma::mat labels = arma::zeros(1, dataset.n_cols);
  labels.submat(0, labels.n_cols / 2, 0, labels.n_cols - 1).fill(1);

  // Vanilla neural net with logistic activation function.
  BuildDropoutNetwork<LogisticFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (dataset, labels, dataset, labels, 8, 30, 0.4);

  // Vanilla neural net with tanh activation function.
  BuildDropoutNetwork<TanhFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
    (dataset, labels, dataset, labels, 8, 30, 0.4);
}

/**
 * Train and evaluate a DropConnect network(with a baselayer) with the
 * specified structure.
 */
template<
    typename PerformanceFunction,
    typename OutputLayerType,
    typename PerformanceFunctionType,
    typename MatType = arma::mat
>
void BuildDropConnectNetwork(MatType& trainData,
                             MatType& trainLabels,
                             MatType& testData,
                             MatType& testLabels,
                             const size_t hiddenLayerSize,
                             const size_t maxEpochs,
                             const double classificationErrorThreshold)
{
 /*
  *  Construct a feed forward network with trainData.n_rows input nodes,
  *  hiddenLayerSize hidden nodes and trainLabels.n_rows output nodes. The
  *  network struct that looks like:
  *
  *  Input         Hidden     DropConnect     Output
  *  Layer         Layer         Layer        Layer
  * +-----+       +-----+       +-----+       +-----+
  * |     |       |     |       |     |       |     |
  * |     +------>|     +------>|     +------>|     |
  * |     |     +>|     |       |     |       |     |
  * +-----+     | +--+--+       +-----+       +-----+
  *             |
  *  Bias       |
  *  Layer      |
  * +-----+     |
  * |     |     |
  * |     +-----+
  * |     |
  * +-----+
  *
  *
  */
  LinearLayer<> inputLayer(trainData.n_rows, hiddenLayerSize);
  BiasLayer<> biasLayer(hiddenLayerSize);
  BaseLayer<PerformanceFunction> hiddenLayer0;

  LinearLayer<> hiddenLayer1(hiddenLayerSize, trainLabels.n_rows);
  DropConnectLayer<decltype(hiddenLayer1)> dropConnectLayer0(hiddenLayer1);

  BaseLayer<PerformanceFunction> outputLayer;

  OutputLayerType classOutputLayer;

  auto modules = std::tie(inputLayer, biasLayer, hiddenLayer0,
                          dropConnectLayer0, outputLayer);

  FFN<decltype(modules), decltype(classOutputLayer), RandomInitialization,
              PerformanceFunctionType> net(modules, classOutputLayer);

  RMSprop<decltype(net)> opt(net, 0.01, 0.88, 1e-8,
      maxEpochs * trainData.n_cols, 1e-18);

  net.Train(trainData, trainLabels, opt);

  MatType prediction;
  net.Predict(testData, prediction);

  size_t error = 0;
  for (size_t i = 0; i < testData.n_cols; i++)
  {
      if (arma::sum(arma::sum(
          arma::abs(prediction.col(i) - testLabels.col(i)))) == 0)
      {
          error++;
      }
  }

  double classificationError = 1 - double(error) / testData.n_cols;
  BOOST_REQUIRE_LE(classificationError, classificationErrorThreshold);
}

/**
 * Train and evaluate a DropConnect network(with a linearlayer) with the
 * specified structure.
 */
template<
    typename PerformanceFunction,
    typename OutputLayerType,
    typename PerformanceFunctionType,
    typename MatType = arma::mat
>
void BuildDropConnectNetworkLinear(MatType& trainData,
                                   MatType& trainLabels,
                                   MatType& testData,
                                   MatType& testLabels,
                                   const size_t hiddenLayerSize,
                                   const size_t maxEpochs,
                                   const double classificationErrorThreshold)
{
 /*
  * Construct a feed forward network with trainData.n_rows input nodes,
  * hiddenLayerSize hidden nodes and trainLabels.n_rows output nodes. The
  * network struct that looks like:
  *
  * Input         Hidden       DropConnect     Output
  * Layer         Layer          Layer         Layer
  * +-----+       +-----+       +-----+       +-----+
  * |     |       |     |       |     |       |     |
  * |     +------>|     +------>|     +------>|     |
  * |     |     +>|     |       |     |       |     |
  * +-----+     | +--+--+       +-----+       +-----+
  *             |
  *  Bias       |
  *  Layer      |
  * +-----+     |
  * |     |     |
  * |     +-----+
  * |     |
  * +-----+
  *
  *
  */
  LinearLayer<> inputLayer(trainData.n_rows, hiddenLayerSize);
  BiasLayer<> biasLayer(hiddenLayerSize);
  BaseLayer<PerformanceFunction> hiddenLayer0;

  DropConnectLayer<> dropConnectLayer0(hiddenLayerSize, trainLabels.n_rows);

  BaseLayer<PerformanceFunction> outputLayer;

  OutputLayerType classOutputLayer;
  auto modules = std::tie(inputLayer, biasLayer, hiddenLayer0,
                          dropConnectLayer0, outputLayer);

  FFN<decltype(modules), decltype(classOutputLayer), RandomInitialization,
              PerformanceFunctionType> net(modules, classOutputLayer);

  RMSprop<decltype(net)> opt(net, 0.01, 0.88, 1e-8,
      maxEpochs * trainData.n_cols, 1e-18);

  net.Train(trainData, trainLabels, opt);

  MatType prediction;
  net.Predict(testData, prediction);

  size_t error = 0;
  for (size_t i = 0; i < testData.n_cols; i++)
  {
      if (arma::sum(arma::sum(
          arma::abs(prediction.col(i) - testLabels.col(i)))) == 0)
      {
              error++;
      }
  }

  double classificationError = 1 - double(error) / testData.n_cols;
  BOOST_REQUIRE_LE(classificationError, classificationErrorThreshold);
}
/**
 * Train the dropconnect network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(DropConnectNetworkTest)
{
  // Load the dataset.
  arma::mat dataset;
  data::Load("thyroid_train.csv", dataset, true);

  arma::mat trainData = dataset.submat(0, 0, dataset.n_rows - 4,
      dataset.n_cols - 1);
  arma::mat trainLabels = dataset.submat(dataset.n_rows - 3, 0,
      dataset.n_rows - 1, dataset.n_cols - 1);

  data::Load("thyroid_test.csv", dataset, true);

  arma::mat testData = dataset.submat(0, 0, dataset.n_rows - 4,
      dataset.n_cols - 1);
  arma::mat testLabels = dataset.submat(dataset.n_rows - 3, 0,
      dataset.n_rows - 1, dataset.n_cols - 1);

  // Vanilla neural net with logistic activation function.
  // Because 92 percent of the patients are not hyperthyroid the neural
  // network must be significant better than 92%.
  BuildDropConnectNetwork<LogisticFunction,
                          BinaryClassificationLayer,
                          MeanSquaredErrorFunction>
      (trainData, trainLabels, testData, testLabels, 4, 100, 0.1);

  BuildDropConnectNetworkLinear<LogisticFunction,
                                BinaryClassificationLayer,
                                MeanSquaredErrorFunction>
      (trainData, trainLabels, testData, testLabels, 4, 100, 0.1);

  dataset.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  for (size_t i = 0; i < dataset.n_cols; ++i)
    dataset.col(i) /= norm(dataset.col(i), 2);

  arma::mat labels = arma::zeros(1, dataset.n_cols);
  labels.submat(0, labels.n_cols / 2, 0, labels.n_cols - 1).fill(1);

  // Vanilla neural net with logistic activation function.
  BuildDropConnectNetwork<LogisticFunction,
                          BinaryClassificationLayer,
                          MeanSquaredErrorFunction>
      (dataset, labels, dataset, labels, 8, 30, 0.4);

  BuildDropConnectNetworkLinear<LogisticFunction,
                                BinaryClassificationLayer,
                                MeanSquaredErrorFunction>
      (dataset, labels, dataset, labels, 8, 30, 0.4);
}

BOOST_AUTO_TEST_SUITE_END();


#include "nonThermalLosses.h"
#include <fparameters/parameters.h>
#include <fmath/physics.h>
#include <gsl/gsl_math.h>
#include <boost/property_tree/ptree.hpp>
#include <gsl/gsl_sf_bessel.h>

double adiabaticLosses(double E, double z, double vel_lat, double gamma)  //en [erg/s]
{
	static const double openingAngle = GlobalConfig.get<double>("openingAngle");

	double jetRadius = z*openingAngle;

	return gamma*2.0*(vel_lat*E / (3.0*jetRadius));
	//en el sist lab es sin Gamma
	//termina quedando return 2.0*cLight*E / (3.0*z);
}

double BohmDiffusionCoeff(double E, double B)
{
	double larmorR = E/(electronCharge*B);
	return 1.0/3.0 * larmorR * cLight;
}

double diffusionTimeParallel(double E, double height, double B)
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double diffCoeff = zeda * BohmDiffusionCoeff(E,B);
	return height*height/diffCoeff;
}

double diffusionTimePerpendicular(double E, double height, double B)
{
	double larmorR = E/(electronCharge*B);
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double meanFreePath = larmorR/(3.0*zeda) * pow(height/larmorR,q-1.0);
	double diffCoeff = zeda * BohmDiffusionCoeff(E,B) / (1.0 + P2(meanFreePath/larmorR));
	return height*height/diffCoeff;
}

double diffCoeff_p(double E, Particle& p, double height, double B, double rho)
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double g = E / (p.mass*cLight2);
	double kMin = 1.0/height;
	double vA = B / sqrt(4.0*pi*rho);
	double rL = E/(electronCharge*B);
	return zeda * gsl_pow_2(p.mass*cLight)* (cLight*kMin) *gsl_pow_2(vA/cLight) * pow(rL*kMin,q-2) * g*g;
}

double diffCoeff_g(double g, Particle& p, double height, double B, double rho)
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double kMin = 1.0/height;
	double vA = B / sqrt(4.0*pi*rho);
	double rL = g*p.mass*cLight2/(electronCharge*B);
	return zeda * (cLight*kMin) *gsl_pow_2(vA/cLight) * pow(rL*kMin,q-2) * g*g;
}

double diffCoeff_r(double g, Particle& p, double height, double B)
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double kMin = 1.0/height;
	double rL = g*p.mass*cLight2/(electronCharge*B);
	return 1.0/9.0 * cLight / zeda * rL * pow(kMin*rL,1-q);
}

double diffLength(double g, Particle& p, double r, double height, double B, double vR)
{
	return diffCoeff_r(g,p,height,B)/abs(vR);
}

double diffusionTimeTurbulence(double E, double height, Particle& p, double B)   //en [s]
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double larmorRadius = E/(electronCharge*B);
	double tEscape = height/cLight;
	return 9.0*zeda*tEscape*pow(larmorRadius/height, q-2.0);
}

double accelerationTimeSDA(double E, Particle& p, double B, double height, double rho)
{
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double larmorRadius = E/(electronCharge*B);
	double tEscape = height/cLight;
	double alfvenVel = B/sqrt(4.0*pi*rho);
	return (1.0/zeda) / P2(alfvenVel/cLight) * tEscape * pow(larmorRadius/height,2.0-q);
}

double accelerationRateSDA(double E, Particle& p, double B, double height, double rho)
{
	double gamma = E / (p.mass*cLight2);
	return diffCoeff_g(gamma,p,height,B,rho) / (gamma*gamma);
}

double relaxTime_e(double E, double temp, double dens) {

	double gamma = E / electronRestEnergy;
	double lnLambda = 20.0;
	double theta = boltzmann*temp / electronRestEnergy;

	if (gamma > 2.0 && theta > 0.3) {
		double k1 = gsl_sf_bessel_K1(1.0/theta);
		double k2 = gsl_sf_bessel_Kn(2, 1.0/theta);
		double factor = abs(k1/k2 - 1.0/gamma);
		return 2.0/3.0 * gamma / (dens*thomson*cLight*(lnLambda+9.0/16.0-0.5*log(2.0))) /
				factor;
	} else
		return 4.0*sqrt(pi)*pow(theta, 1.5) / (dens*thomson*cLight*lnLambda);
}

double relaxTime_p(double E, double temp, double dens) {

	double gamma = E / (protonMass*cLight2);
	double lnLambda = 20.0;
	double theta = boltzmann*temp / (protonMass*cLight2);
	double b1 = 70e6*EV_TO_ERG / (protonMass*cLight2);
	double b2 = 500e6*EV_TO_ERG / (protonMass*cLight2);
	if ( gamma-1.0 > b1 && gamma-1.0 < b2 ) {
		double beta = sqrt(1.0-1.0/(gamma*gamma));
		double sigma_h = 2.3e-26;
		return 4.0 * gamma*gamma * beta / (gamma*gamma - 1.0) / (dens*sigma_h*cLight);
	} else
		return 4.0*sqrt(pi)*pow(theta, 1.5) * P2(protonMass/electronMass) / (dens*thomson*cLight*lnLambda);
}

double accelerationRate(double E, double B) //en [s]^-1
{
	double accEff = GlobalConfig.get<double>("nonThermal.injection.PL.accEfficiency");
	return accEff*cLight*electronCharge*B/E;
}

double escapeRate(double size, double vel) //en [1/s]
{
	//static const double Gamma = GlobalConfig.get<double>("Gamma");

	return vel /size;  //ver si necesito algun gamma para escribirlo en el FF
}


#include <svo/direct/elder_zucker.h>
#ifdef __SSSE3__
#include <tmmintrin.h>
#endif
#include <boost/math/special_functions/erf.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

namespace svo {
namespace elder_zucker {

void detectEdges(
    const std::vector<cv::Mat>& img_pyr,
    const double sigma,
    cv::Mat& edge_map,
    cv::Mat& level_map)
{
//  printf("detect edges\n");
  const float pi = 3.14159265358979323846264;
  const float sn = sigma;
  const float alpha_p = 2e-7;
  const int n_levels = img_pyr.size()-1;

  // STEP-1: Use local scale control to realiably estimate the intensity
  // gradient at each image point.
  std::vector<cv::Mat> img_pyr_smoothed(n_levels);
  std::vector<cv::Mat> angle_pyr(n_levels);
  for(int L=0; L<n_levels; ++L)
  {
    // smooth image pyramid
    cv::GaussianBlur(img_pyr[L], img_pyr_smoothed[L], cv::Size(3,3), 0);

    // compute image first derivative
    const int delta=0;
    cv::Mat dx, dy;
    cv::Scharr(img_pyr_smoothed[L], dx, CV_16S, 1, 0, 1, delta, cv::BORDER_DEFAULT );
    cv::Scharr(img_pyr_smoothed[L], dy, CV_16S, 0, 1, 1, delta, cv::BORDER_DEFAULT );

    // compute critical threshold 1
    const float scale = L+1;
    const float s1 = sn * (1.0 / (2.0*sqrt(2.0*pi)*scale*scale));
    const float c1 = s1 * sqrt(-2.0*log(alpha_p));
//    printf("c1 = %f\n", c1);

    // compute angle and magnitude in angle direction
    const int n_rows = img_pyr_smoothed[L].rows;
    const int n_cols = img_pyr_smoothed[L].cols;
    angle_pyr[L] = cv::Mat(dx.size(), CV_32F);
    for(int y=0; y<n_rows; ++y)
    {
      int16_t* p_dx = dx.ptr<int16_t>(y);
      int16_t* p_dy = dy.ptr<int16_t>(y);
      float* p_a = angle_pyr[L].ptr<float>(y);
      for(int x=0; x<n_cols; ++x)
      {
        p_a[x] = std::atan2(p_dy[x], p_dx[x]);
        float mag = std::cos(p_a[x])*p_dx[x]+std::sin(p_a[x])*p_dy[x];
        if(std::abs(mag) < c1)
          p_a[x]=0;
      }
    }
  }

  // find minimum level
  cv::Mat angle(img_pyr_smoothed[0].size(), CV_32FC1, cv::Scalar(0));
  for(int y=0; y<img_pyr_smoothed[0].rows; ++y)
  {
    float* p_a = angle.ptr<float>(y);
    uint8_t* p_lev = level_map.ptr<uint8_t>(y);
    for(int x=0; x<img_pyr_smoothed[0].cols; ++x)
    {
      for(int L=0; L<n_levels; ++L)
      {
        const float a = angle_pyr[L].at<float>(y/(1<<L), x/(1<<L));
        if(a != 0.0) // TODO: what if angle is actually 0.0?
        {
          p_a[x] = a;
          break;
        }
      }
    }
  }

  std::vector<cv::Mat> lap_of_gau_pyr(img_pyr_smoothed.size());
  for(int L=0; L<n_levels; ++L)
  {
    // compute image second derivative
    cv::Mat dxdx1, dydy1, dxdy1;
    getCovarEntries(img_pyr_smoothed[L], dxdx1, dydy1, dxdy1);

    // smooth
    cv::Mat dxdx, dydy, dxdy;
    filterGauss3by316S(dxdx1, dxdx);
    filterGauss3by316S(dydy1, dydy);
    filterGauss3by316S(dxdy1, dxdy);

    // compute critical threshold 2
    const float scale = L+1;
    const float s2 = sn / (4.0 * sqrt(pi/3.0)*scale*scale*scale);
    const float c2 = sqrt(2.0) * s2 * (boost::math::erf_inv(1-alpha_p));

    // compute laplacian of gaussians
    const int n_rows = img_pyr_smoothed[L].rows;
    const int n_cols = img_pyr_smoothed[L].cols;
    lap_of_gau_pyr[L] = cv::Mat(img_pyr_smoothed[L].size(), CV_32F);
    for(int y=0; y<n_rows; ++y)
    {
      int16_t* p_dxdx = dxdx.ptr<int16_t>(y);
      int16_t* p_dxdy = dxdy.ptr<int16_t>(y);
      int16_t* p_dydy = dydy.ptr<int16_t>(y);
      float* p_a = angle.ptr<float>(y);
      float* p_l = lap_of_gau_pyr[L].ptr<float>(y);
      for(int x=0; x<n_cols; ++x)
      {
        const float ca = cos(p_a[x]);
        const float sa = sin(p_a[x]);
        p_l[x] = (ca*ca*p_dxdx[x])+(sa*sa*p_dydy[x])-(2*ca*sa*p_dxdy[x]);
        if(fabs(p_l[x]) < c2)
          p_l[x]=0;
      }
    }
  }

  // find minimum level
  edge_map = cv::Mat(img_pyr_smoothed[0].size(), CV_32FC1, cv::Scalar(0));
  level_map = cv::Mat(img_pyr_smoothed[0].size(), CV_8UC1, cv::Scalar(0));
  for(int y=0; y<img_pyr_smoothed[0].rows; ++y)
  {
    float* p_e = edge_map.ptr<float>(y);
    uint8_t* p_lev = level_map.ptr<uint8_t>(y);
    for(int x=0; x<img_pyr_smoothed[0].cols; ++x)
    {
      for(int L=0; L<n_levels; ++L)
      {
        const float e = lap_of_gau_pyr[L].at<float>(y/(1<<L), x/(1<<L));
        if(e != 0.0)
        {
          p_e[x] = e;
          p_lev[x] = L;
          break;
        }
      }
    }
  }
}

void getCovarEntries(
    const cv::Mat& src,
    cv::Mat& dxdx,
    cv::Mat& dydy,
    cv::Mat& dxdy)
{
#ifdef __SSSE3__
  cv::Mat kernel=cv::Mat::zeros(3,3,CV_8S);
  kernel.at<char>(0,0)=3*8;
  kernel.at<char>(1,0)=10*8;
  kernel.at<char>(2,0)=3*8;
  kernel.at<char>(0,2)=-3*8;
  kernel.at<char>(1,2)=-10*8;
  kernel.at<char>(2,2)=-3*8;

  const unsigned int X=3; // kernel size
  const unsigned int Y=3; // kernel size
  const unsigned int cx=1;
  const unsigned int cy=1;

  // dest will be 16 bit
  dxdx=cv::Mat::zeros(src.rows,src.cols,CV_16S);
  dydy=cv::Mat::zeros(src.rows,src.cols,CV_16S);
  dxdy=cv::Mat::zeros(src.rows,src.cols,CV_16S);

  const unsigned int maxJ=((src.cols-2)/16)*16;
  const unsigned int maxI=src.rows-2;
  const unsigned int stride=src.cols;

  __m128i mask_hi = _mm_set_epi8(0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF);
  __m128i mask_lo = _mm_set_epi8(0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00,0xFF,0x00);

  for(unsigned int i=0; i<maxI; ++i)
  {
    bool end=false;
    for(unsigned int j=0; j<maxJ; )
    {
      //__m128i result = _mm_set_epi16 ( -127,-127,-127,-127,-127,-127,-127,-127,-127,-127);
      __m128i result_hi_dx = _mm_set_epi16 ( 0,0,0,0,0,0,0,0);
      __m128i result_lo_dx = _mm_set_epi16 ( 0,0,0,0,0,0,0,0);
      __m128i result_hi_dy = _mm_set_epi16 ( 0,0,0,0,0,0,0,0);
      __m128i result_lo_dy = _mm_set_epi16 ( 0,0,0,0,0,0,0,0);

      // enter convolution with kernel
      for(unsigned int x=0;x<X;++x)
      {
        //if(dx&&x==1)continue; // jump, 0 kernel
        for(unsigned int y=0;y<Y;++y)
        {
          //if(!dx&&y==1)continue; // jump, 0 kernel
          const char m_dx=kernel.at<char>(y,x);
          const char m_dy=kernel.at<char>(x,y);
          __m128i mult_dx = _mm_set_epi16(m_dx,m_dx,m_dx,m_dx,m_dx,m_dx,m_dx,m_dx);
          __m128i mult_dy = _mm_set_epi16(m_dy,m_dy,m_dy,m_dy,m_dy,m_dy,m_dy,m_dy);
          uchar* p=(src.data+(stride*(i+y))+x+j);
          __m128i i0 = _mm_loadu_si128 ((__m128i*)p);
          __m128i i0_hi=_mm_and_si128(i0,mask_hi);
          __m128i i0_lo=_mm_srli_si128(_mm_and_si128(i0,mask_lo),1);

          if(m_dx!=0)
          {
            __m128i i_hi_dx = _mm_mullo_epi16 (i0_hi, mult_dx);
            __m128i i_lo_dx = _mm_mullo_epi16 (i0_lo, mult_dx);
            result_hi_dx=_mm_add_epi16(result_hi_dx,i_hi_dx);
            result_lo_dx=_mm_add_epi16(result_lo_dx,i_lo_dx);
          }

          if(m_dy!=0)
          {
            __m128i i_hi_dy = _mm_mullo_epi16 (i0_hi, mult_dy);
            __m128i i_lo_dy = _mm_mullo_epi16 (i0_lo, mult_dy);
            result_hi_dy=_mm_add_epi16(result_hi_dy,i_hi_dy);
            result_lo_dy=_mm_add_epi16(result_lo_dy,i_lo_dy);
          }
        }
      }

      // calculate covariance entries - remove precision (ends up being 4 bit), then remove 4 more bits
      __m128i i_hi_dx_dx = _mm_srai_epi16(_mm_mulhi_epi16 (result_hi_dx, result_hi_dx),4);
      __m128i i_hi_dy_dy = _mm_srai_epi16(_mm_mulhi_epi16 (result_hi_dy, result_hi_dy),4);
      __m128i i_hi_dx_dy = _mm_srai_epi16(_mm_mulhi_epi16 (result_hi_dy, result_hi_dx),4);
      __m128i i_lo_dx_dx = _mm_srai_epi16(_mm_mulhi_epi16 (result_lo_dx, result_lo_dx),4);
      __m128i i_lo_dy_dy = _mm_srai_epi16(_mm_mulhi_epi16 (result_lo_dy, result_lo_dy),4);
      __m128i i_lo_dx_dy = _mm_srai_epi16(_mm_mulhi_epi16 (result_lo_dy, result_lo_dx),4);

      // store
      uchar* p_lo_dxdx=(dxdx.data+(2*stride*(i+cy)))+2*cx+2*j;
      uchar* p_hi_dxdx=(dxdx.data+(2*stride*(i+cy)))+2*cx+2*j+16;
      _mm_storeu_si128 ((__m128i*)p_hi_dxdx,_mm_unpackhi_epi16 (i_hi_dx_dx, i_lo_dx_dx));
      _mm_storeu_si128 ((__m128i*)p_lo_dxdx,_mm_unpacklo_epi16 (i_hi_dx_dx, i_lo_dx_dx));
      uchar* p_lo_dydy=(dydy.data+(2*stride*(i+cy)))+2*cx+2*j;
      uchar* p_hi_dydy=(dydy.data+(2*stride*(i+cy)))+2*cx+2*j+16;
      _mm_storeu_si128 ((__m128i*)p_hi_dydy,_mm_unpackhi_epi16 (i_hi_dy_dy, i_lo_dy_dy));
      _mm_storeu_si128 ((__m128i*)p_lo_dydy,_mm_unpacklo_epi16 (i_hi_dy_dy, i_lo_dy_dy));
      uchar* p_lo_dxdy=(dxdy.data+(2*stride*(i+cy)))+2*cx+2*j;
      uchar* p_hi_dxdy=(dxdy.data+(2*stride*(i+cy)))+2*cx+2*j+16;
      _mm_storeu_si128 ((__m128i*)p_hi_dxdy,_mm_unpackhi_epi16 (i_hi_dx_dy, i_lo_dx_dy));
      _mm_storeu_si128 ((__m128i*)p_lo_dxdy,_mm_unpacklo_epi16 (i_hi_dx_dy, i_lo_dx_dy));

      // take care about end
      j+=16;
      if(j>=maxJ&&!end)
      {
        j=stride-2-16;
        end=true;
      }
    }
  }
#endif
}

void filterGauss3by316S(
    const cv::Mat& src,
    cv::Mat& dst)
{
#ifdef __SSSE3__
  // sanity check
  const unsigned int X=3;
  const unsigned int Y=3;
  assert(X%2!=0);
  assert(Y%2!=0);
  int cx=X/2;
  int cy=Y/2;

  // dest will be 16 bit
  dst=cv::Mat::zeros(src.rows,src.cols,CV_16S);
  const unsigned int maxJ=((src.cols-2)/8)*8;
  const unsigned int maxI=src.rows-2;
  const unsigned int stride=src.cols;

  for(unsigned int i=0; i<maxI; ++i)
  {
    bool end=false;
    for(unsigned int j=0; j<maxJ; )
    {
      // enter convolution with kernel. do the multiplication with 2/4 at the same time
      __m128i i00 = _mm_loadu_si128 ((__m128i*)&src.at<short>(i,j));
      __m128i i10 = _mm_slli_epi16(_mm_loadu_si128 ((__m128i*)&src.at<short>(i+1,j)),1);
      __m128i i20 = _mm_loadu_si128 ((__m128i*)&src.at<short>(i+2,j));
      __m128i i01 = _mm_slli_epi16(_mm_loadu_si128 ((__m128i*)&src.at<short>(i,j+1)),1);
      __m128i i11 = _mm_slli_epi16(_mm_loadu_si128 ((__m128i*)&src.at<short>(i+1,j+1)),2);
      __m128i i21 = _mm_slli_epi16(_mm_loadu_si128 ((__m128i*)&src.at<short>(i+2,j+1)),1);
      __m128i i02 = _mm_loadu_si128 ((__m128i*)&src.at<short>(i,j+2));
      __m128i i12 = _mm_slli_epi16(_mm_loadu_si128 ((__m128i*)&src.at<short>(i+1,j+2)),1);
      __m128i i22 = _mm_loadu_si128 ((__m128i*)&src.at<short>(i+2,j+2));
      __m128i result = i11;

      // add up
      result=_mm_add_epi16(result,i00);
      result=_mm_add_epi16(result,i20);
      result=_mm_add_epi16(result,i02);
      result=_mm_add_epi16(result,i22);

      result=_mm_add_epi16(result,i10);
      result=_mm_add_epi16(result,i01);
      result=_mm_add_epi16(result,i12);
      result=_mm_add_epi16(result,i21);

      // store
      //uchar* p_r=(dst.data+(2*stride*(i+cy)))+2*cx+2*j;
      _mm_storeu_si128 ((__m128i*)&dst.at<short>(i+cy,j+cx),result);

      // take care about end
      j+=8;
      if(j>=maxJ&&!end)
      {
              j=stride-2-8;
              end=true;
      }
    }
  }
#endif
}

} // namespace elder_zucker
} // namespace svo


#include <state_estimation/filters/ekf_vs.h>
#include <state_estimation/utilities/data_subset_utilities.h>
#include <state_estimation/utilities/logging.h>
#include <Eigen/Dense>

namespace state_estimation {

void EKFVS::myPredict(const Eigen::VectorXd& u, double dt) {
    system_model_->update(filter_state_.x, u, dt);

    // Update the state and covariance. For the covariance the update will happen on the state
    // subsets then be converted back to the full dimensionality.
    filter_state_.x = system_model_->g();

    const Eigen::MatrixXd cov_subset =
        getSubset(filter_state_.covariance, system_model_->activeStates());
    const Eigen::MatrixXd G_subset = getSubset(system_model_->G(), system_model_->activeStates());
    const Eigen::MatrixXd Rc_subset =
        getSubset(system_model_->Rc(), system_model_->activeControls());
    const Eigen::MatrixXd P_subset =
        getSubset(system_model_->P(), system_model_->activeStates(), {});
    const Eigen::MatrixXd V_subset = getSubset(system_model_->V(), system_model_->activeStates(),
                                               system_model_->activeControls());

    const Eigen::MatrixXd cov_prime_subset = G_subset * cov_subset * G_subset.transpose() +
                                             P_subset * system_model_->Rp() * P_subset.transpose() +
                                             V_subset * Rc_subset * V_subset.transpose();

    convertSubsetToFull(cov_prime_subset, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "EKF predicition update:" << std::endl
              << "g=" << printMatrix(system_model_->g()) << std::endl
              << "G=" << std::endl
              << printMatrix(system_model_->G()) << std::endl
              << "P=" << std::endl
              << printMatrix(system_model_->P()) << std::endl
              << "V=" << std::endl
              << printMatrix(system_model_->V()) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

void EKFVS::myCorrect(const Eigen::VectorXd& z,
                      measurement_models::NonlinearMeasurementModel* model, double dt) {
    // Update our measurement model
    model->update(filter_state_.x, dt);

    // Get our sub matrices/vectors
    const Eigen::MatrixXd cov_subset =
        getSubset(filter_state_.covariance, system_model_->activeStates());
    const Eigen::MatrixXd H_subset =
        getSubset(model->H(), model->activeMeasurements(), system_model_->activeStates());
    const Eigen::MatrixXd meas_cov_subset =
        getSubset(model->covariance(), model->activeMeasurements());

    // Compute the Kalman gain
    const Eigen::MatrixXd cov_H_T = cov_subset * H_subset.transpose();
    const Eigen::MatrixXd K = cov_H_T * (H_subset * cov_H_T + meas_cov_subset).inverse();

    // Update the state
    const Eigen::VectorXd dz_full = model->subtractVectors(z, model->h());
    const Eigen::VectorXd dz_subset = getSubset(dz_full, model->activeMeasurements());
    const Eigen::VectorXd dx_subset = K * dz_subset;
    const Eigen::VectorXd dx_full = convertSubsetToFullZeroed(
        dx_subset, system_model_->activeStates(), system_model_->stateSize());

    filter_state_.x = system_model_->addVectors(filter_state_.x, dx_full);

    // Update the covariance
    const Eigen::MatrixXd I = Eigen::MatrixXd::Identity(cov_subset.rows(), cov_subset.rows());
    const Eigen::MatrixXd cov_prime = (I - K * H_subset) * cov_subset;

    convertSubsetToFull(cov_prime, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "EKF measurement update:" << std::endl
              << "h=" << printMatrix(model->h()) << std::endl
              << "H=" << std::endl
              << printMatrix(model->H()) << std::endl
              << "Q=" << std::endl
              << printMatrix(model->covariance()) << std::endl
              << "K=" << std::endl
              << printMatrix(K) << std::endl
              << "Innovation=" << printMatrix(dx_full) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

}  // namespace state_estimation


#include "rts_object_cost_grid_space_e.h"

#include "cor_type/sources/math/vector2.h"
#include "cor_data_structure/sources/ai/stack_decoder_tmpl_impl.h"

#include <boost/graph/adjacency_list.hpp>

#include "cor_system/sources/thread_pool.h"

#include "cocos2d.h"

namespace cor
{
    namespace cocos2dx_converter
    {
        struct RtsObjectCostGridSpaceExperimentalItnl
        {

        };

        RtsObjectCostGridSpaceExperimental::RtsObjectCostGridSpaceExperimental() : itnl(new RtsObjectCostGridSpaceExperimentalItnl())
        {

        }

        RtsObjectCostGridSpaceExperimental::~RtsObjectCostGridSpaceExperimental()
        {

        }

        RString RtsObjectCostGridSpaceExperimental::run1(cocos2d::DrawNode* draw_node)
        {
            typedef boost::adjacency_list<boost::listS, boost::listS, boost::undirectedS,
                type::Vector2F, boost::property<boost::edge_weight_t, RFloat> > Graph;

            Graph g;

            RStringStream s;

            s << "graph run 1\n";

            auto va = {
                type::Vector2F(100.0f, 100.0f),
                type::Vector2F(200.0f, 100.0f),
                type::Vector2F(300.0f, 100.0f),
                type::Vector2F(100.0f, 250.0f),
                type::Vector2F(200.0f, 400.0f)
            };

            std::vector<Graph::vertex_descriptor> vda;

            for(auto v: va)
            {
                auto vd = boost::add_vertex(g);

                g[vd] = v;

                vda.push_back(vd);
            }

            typedef std::pair<RInt32, RInt32> Pair;

            auto ea = {
                Pair(0, 1),
                Pair(1, 2),
                Pair(0, 3),
                Pair(1, 3),
                Pair(2, 3),
                Pair(3, 4),
                Pair(2, 4)
            };

            for(auto e : ea)
            {
                auto ed = boost::add_edge(vda[e.first], vda[e.second], g[vda[e.first]].distance(g[vda[e.second]]), g);
            }

            auto w = boost::get(boost::edge_weight, g);

            auto vta = boost::vertices(g);
            for(auto v = vta.first; v != vta.second; v++)
            {
                s << "v " << g[*v].x << ", " << g[*v].y << "\n";
                auto ea = boost::out_edges(*v, g);
                for(auto e = ea.first; e != ea.second; e++)
                {
                    s << "e (" << g[boost::source(*e, g)].x << ", " << g[boost::source(*e, g)].y << "), -> " <<
                        "(" << g[boost::target(*e, g)].x << ", " << g[boost::target(*e, g)].y << ") = " << boost::get(w, *e) << "\n";

                    draw_node->drawSegment(cocos2d::Vec2(g[boost::source(*e, g)].x, g[boost::source(*e, g)].y),
                        cocos2d::Vec2(g[boost::target(*e, g)].x, g[boost::target(*e, g)].y), 1.0f,
                        cocos2d::Color4F(1.0f, 0.0f, 0.0f, 1.0f));
                }
            }

            std::map<Graph::vertex_descriptor, RFloat> vertex_cost_table;
            std::map<Graph::vertex_descriptor, Graph::vertex_descriptor> vertex_parent_table;
            struct State
            {
                Graph::vertex_descriptor vd;
                RFloat cost;

                State()
                {
                    vd = nullptr;
                    cost = 0.0f;
                }
            };

            typedef data_structure::InstantStackDecoderTmpl<RFloat, State> StackDecoder;

            StackDecoder sd;

            s << "searching" << "\n";

            sd.set_func([&](const State& state){
                return state.cost;
            }, [&](const State& state, std::function<void(const State&)> f){
                auto ea = boost::out_edges(state.vd, g);
                auto& src = g[state.vd];
                for(auto e = ea.first; e != ea.second; e++)
                {
                    auto target_vd = boost::target(*e, g);
                    auto& tgt = g[target_vd];
                    auto cost = state.cost + src.distance(tgt);

                    if(vertex_cost_table.find(target_vd) != vertex_cost_table.end())
                    {
                        if(vertex_cost_table[target_vd] <= cost)
                        {
                            continue;
                        }
                    }

                    State st;
                    vertex_cost_table[target_vd] = cost;
                    vertex_parent_table[target_vd] = state.vd;
                    st.vd = target_vd;
                    st.cost = cost;

                    s << "(" << g[target_vd].x << ", " << g[target_vd].y << "), " << cost << "\n";

                    f(st);
                }
            }, [&](const State& state){
                return state.vd == vda[4];
            });

            State first_state;
            first_state.vd = vda[0];
            vertex_cost_table[vda[0]] = 0.0f;
            vertex_parent_table[vda[0]] = nullptr;
            sd.push(first_state);

            s << "path" << "\n";
            sd.search_first_n(1, [&](const State& state){
                Graph::vertex_descriptor vd = state.vd;
                while(vd)
                {
                    s << "(" << g[vd].x << ", " << g[vd].y << ")" << "\n";

                    vd = vertex_parent_table[vd];
                }

                return rtrue;
            });

            return s.str();
        }

        RString RtsObjectCostGridSpaceExperimental::run2(cocos2d::Node* root_node, cocos2d::DrawNode* draw_node, Collision2dNodeSP collision, data_structure::CostGridSpaceSP cost_grid_space, RtsObjectGroupSP object_group)
        {
            auto cgs = std::make_shared<RtsObjectCostGridSpace>(collision, cost_grid_space, object_group);
            cgs->set_wall_kind(2);

            cgs->set_index_convert(type::Vector2F(10.0f, 10.0f), type::Vector2F(0.0f, 0.0f));

            struct Character
            {
                type::Vector2F p;
                cocos2d::Color4F c;
                Collision2dNodeRef r;
                RtsObjectSP o;
                cocos2d::Node* n;

                Character(RFloat x, RFloat y, RFloat r, RFloat g, RFloat b, RFloat a)
                {
                    p = type::Vector2F(x, y);
                    c = cocos2d::Color4F(r, g, b, a);
                }
            };

            RStringStream s;

            Character ca[] = {
                Character(50.0f, 50.0f, 1.0f, 0.0f, 0.0f, 1.0f),
                Character(120.0f, 50.0f, 1.0f, 1.0f, 0.0f, 1.0f),
                Character(200.0f, 50.0f, 1.0f, 0.0f, 1.0f, 1.0f),
                Character(50.0f, 100.0f, 0.0f, 1.0f, 0.0f, 1.0f),
                Character(200.0f, 200.0f, 0.0f, 1.0f, 1.0f, 1.0f)
            };

            s << "test run2";

            std::vector<Collision2dNodeRef> refs;

            auto vec_i_to_f = [](type::Vector2I vi){
                return type::Vector2F(vi.x * 10.0f, vi.y * 10.0f);
            };

            auto vec_f_to_i = [](type::Vector2F vf){
                return type::Vector2F(static_cast<RInt32>(vf.x / 10.0f + 0.5f), static_cast<RInt32>(vf.y / 10.0f + 0.5f));
            };

            RSize sz = 30;

            for(RSize i = 0; i < 10; i++)
            {
                auto ip = type::Vector2I(10, i + 5);
                auto& cell = cost_grid_space->ref(ip);
                cell.enter_cost = 2.0f;
                //cell.passable = rfalse;
            }

            for(RSize i = 0; i < 7; i++)
            {
                auto ip = type::Vector2I(10 + i, 10);
                auto& cell = cost_grid_space->ref(ip);
                cell.enter_cost = 2.0f;
                //cell.passable = rfalse;
            }

            for(RSize i = 0; i < sz; i++)
            {
                for(RSize j = 0; j < sz; j++)
                {
                    auto ip = type::Vector2I(j, i);
                    auto vp = vec_i_to_f(ip);
                    auto& cell = cost_grid_space->ref(ip);

                    cocos2d::Node* n;
                    if(cell.enter_cost < 1.5f)
                    {
                        //auto l = cocos2d::LayerColor::create(cocos2d::Color4B(127, 127, 127, 64), 10, 10);
                        //l->setPosition(vp.x, vp.y);
                        //
                        //root_node->addChild(l);
                        //
                        //n = l;

                    }
                    else
                    {
                        auto l = cocos2d::LayerColor::create(cocos2d::Color4B(255, 255, 255, 127), 10, 10);
                        l->setPosition(vp.x, vp.y);

                        root_node->addChild(l);

                        n = l;

                        //
                        auto r = collision->add_o_box(n, 0, type::Box2F(0, 0, 10, 10), [=](cocos2d::Node*, cocos2d::Node*){

                        });
                        refs.push_back(r);

                        //
                        auto o = object_group->create_object(r);
                        o->set_kind(2);
                        cgs->add(o);
                    }

                }
            }

            for(auto& c : ca)
            {
                auto n = cocos2d::LayerColor::create(cocos2d::Color4B(c.c), 10, 10);
                n->setPosition(c.p.x, c.p.y);
                c.n = n;
                root_node->addChild(n);

                //
                auto r = collision->add_o_box(n, 0, type::Box2F(0, 0, 10, 10), [=](cocos2d::Node*, cocos2d::Node*){

                });
                c.r = r;
                refs.push_back(r);

                //
                auto o = object_group->create_object(r);
                o->set_kind(1);
                c.o = o;
                cgs->add(o);

                //
                auto ip = vec_f_to_i(c.p);
                auto& cell = cost_grid_space->ref(ip);

            }

            cgs->make_graph();

            cgs->each_vertices([&](RtsObjectCostGridSpaceCellPtr cell){
                //s << "v (" << cell->position.x << ", " << cell->position.y << ")" << "\n";
            });

            cgs->each_edges([&](RtsObjectCostGridSpaceCellPtr source, RtsObjectCostGridSpaceCellPtr target, RFloat w){
                //s << "e s (" << source->position.x << ", " << source->position.y << "), " <<
                //    "t(" << target->position.x << ", " << target->position.y << ")" << ", w " << w << "\n";

                auto v0 = vec_i_to_f(source->position);
                auto v1 = vec_i_to_f(target->position);
                auto color = cocos2d::Color4F(1.0f, 0.0f, 0.0f, 1.0f);
                if(source->wall || target->wall)
                {
                    color = cocos2d::Color4F(0.0f, 0.0f, 1.0f, 1.0f);
                }

                draw_node->drawSegment(cocos2d::Vec2(v0.x + 5.0f, v0.y + 5.0f), cocos2d::Vec2(v1.x + 5.0f, v1.y + 5.0f), 1.0f, color);
            });

            //cgs->each_cell([&](RtsObjectCostGridSpaceCellPtr cell){
            //    auto l = static_cast<cocos2d::LayerColor*>(cell->root->a[0]->get_node_ref().get_node());
            //    auto c = l->getColor();
            //    auto a = l->getOpacity();
            //    a /= 2;
            //    auto nl = cocos2d::LayerColor::create(cocos2d::Color4B(c.r, c.g, c.b, a), 5, 5);
            //    auto v0 = vec_i_to_f(cell->position);
            //    nl->setPosition(v0.x, v0.y);

            //    root_node->addChild(nl);
            //});

            return s.str();
        }

        RString RtsObjectCostGridSpaceExperimental::thread_run()
        {
            auto job_queue = std::make_shared<cor::system::JobQueue>();
            auto thread_pool = std::make_shared<cor::system::ThreadPool>(job_queue, 8);

            std::vector<cor::RInt32> a, b;

            std::mutex m;

            auto f = [&](cor::RInt32 n){
                a.push_back(n);
                auto na = std::make_shared<cor::RInt32>(n);
                thread_pool->add_job([&b, &m, na](){
                    //{
                    //    std::lock_guard<std::mutex> l(m);
                    //    cor::log_debug("thread ", *na.get());
                    //}
                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
                    *na.get() += 10;
                    //{
                    //    std::lock_guard<std::mutex> l(m);
                    //    cor::log_debug("thread ed ", *na.get());
                    //}
                }, [&b, &m, na](){
                    //{
                    //    std::lock_guard<std::mutex> l(m);
                    //    cor::log_debug("end ", *na.get());
                    //}
                    b.push_back(*na.get());
                });
            };

            for(auto i = 0; i < 1000; i++)
            {
                f(i);
            }

            //cor::log_debug("pre step");

            while(!thread_pool->empty())
            {
                job_queue->step();
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
            }

            //cor::log_debug("post step");

            cor::RString sa, sb;

            sa = cor::algorithm::join(a, ",");
            sb = cor::algorithm::join(b, ",");

            cor::log_debug("sa = ", sa, ", sb = ", sb);

            return sb;
        }
    }
}


//  GAMBIT: Global and Modular BSM Inference Tool
//  *********************************************
///  \file
///
///  Prior object construction routines
///
///
///  *********************************************
///
///  Authors (add name and date if you modify):
///
///  \author Ben Farmer
///          (benjamin.farmer@monash.edu.au)
///  \date 2013 Dec
///
///  \author Gregory Martinez
///          (gregory.david.martinez@gmail.com)
///  \date Feb 2014
///
///  *********************************************

#ifndef PRIOR_GAUSSIAN_HPP
#define PRIOR_GAUSSIAN_HPP

#include <vector>
#include <cmath>

#include "gambit/ScannerBit/cholesky.hpp"
#include "gambit/ScannerBit/priors.hpp"
#include "gambit/Utils/yaml_options.hpp"

#include <boost/math/special_functions/erf.hpp>

namespace Gambit
{
    namespace Priors
    {
        // Gaussian prior. Takes covariance matrix as arguments
        class Gaussian : public BasePrior
        {
        private:
            std::vector <double> mean;
            mutable Cholesky col;

        public:
            // Constructor defined in gaussian.cpp
            Gaussian(const std::vector<std::string>&, const Options&);

            // Transformation from unit interval to the Gaussian
            void transform(const std::vector <double> &unitpars, std::unordered_map <std::string, double> &outputMap) const
            {
                std::vector<double> vec(unitpars.size());

                auto v_it = vec.begin();
                for (auto elem_it = unitpars.begin(), elem_end = unitpars.end(); elem_it != elem_end; elem_it++, v_it++)
                {
                    *v_it = M_SQRT2*boost::math::erf_inv(2.0*(*elem_it) - 1.0);
                }

                col.ElMult(vec);

                v_it = vec.begin();
                auto m_it = mean.begin();
                for (auto str_it = param_names.begin(), str_end = param_names.end(); str_it != str_end; str_it++)
                {
                    outputMap[*str_it] = *(v_it++) + *(m_it++);
                }
            }

            double operator()(const std::vector<double> &vec) const
            {
                    static double norm = std::log(2.0*Gambit::Scanner::pi()*Gambit::Scanner::pow<2>(col.DetSqrt()))/2.0;
                    return -col.Square(vec, mean)/2.0 - norm;
            }
        };

        LOAD_PRIOR(gaussian, Gaussian)
    }
}

#endif


#include <cassert>
#include <cstdlib>
#include <algorithm>
#include <iostream>
#include <random>
#include <tuple>
#include <vector>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <dSFMT.h>
#include <windows.h>

using Number = double;
using Numbers = std::vector<Number>;

class LapTime {
    using TimeIn100nsec = unsigned long long;
    using Time = double;
    static constexpr Time SecIn100nsec = 10000000;
public:
    LapTime(void) : startTime_(now()) {
        return;
    }

    virtual ~LapTime(void) = default;

    Time GetElapsedTime(void) {
        const auto endTime = now();
        const auto duration = endTime - startTime_;
        auto elapsedTime = static_cast<Time>(duration);
        elapsedTime /= SecIn100nsec;
        return elapsedTime;
    }

private:
    TimeIn100nsec now(void) {
        FILETIME timestamp;
        GetSystemTimeAsFileTime(&timestamp);

        TimeIn100nsec timeIn100nsec = timestamp.dwHighDateTime;
        for(size_t i=0; i<sizeof(timestamp.dwLowDateTime); ++i) {
            timeIn100nsec <<= 8;
        }
        timeIn100nsec += timestamp.dwLowDateTime;
        return timeIn100nsec;
    }

    TimeIn100nsec startTime_ {0};
};

void generateNumbers(Numbers& v) {
    LapTime laptime;

    std::random_device seed_gen;
    uint32_t seed = seed_gen();
    dsfmt_t dsfmt;
    dsfmt_init_gen_rand(&dsfmt, seed);

    // dsfmt_fill_array_close_open takes size as int
    const auto intSize = static_cast<int>(v.size());
    assert(static_cast<decltype(v.size())>(intSize) == v.size());
    dsfmt_fill_array_close_open(&dsfmt, v.data(), intSize);
    std::cout << "generateNumbers(dSFMT) " << laptime.GetElapsedTime() << "sec\n";
}

void generateNumbersStd(Numbers& v) {
    LapTime laptime;
    std::random_device seed_gen;
    std::mt19937 engine(seed_gen());
    std::uniform_real_distribution<Number> dist(0.0, 1.0);
    std::for_each(v.begin(), v.end(), [&](auto& x) { x = dist(engine); });
    std::cout << "generateNumbers(std) " << laptime.GetElapsedTime() << "sec\n";
}

std::tuple<Number, Number> accumulateNumbers(const Numbers& v, int loopSize) {
    LapTime laptime;
    using namespace boost::accumulators;
    accumulator_set<Number, stats<tag::mean, tag::variance>> acc;
    for(auto loopIndex = decltype(loopSize){0}; loopIndex < loopSize; ++loopIndex) {
        std::for_each(v.begin(), v.end(), [&](const auto& x) { acc(x); });
    }
    std::cout << "accumulateNumbers " << laptime.GetElapsedTime() << "sec\n";
    return {mean(acc), variance(acc)};
}

int main(int argc, char* argv[]) {
    bool setSize = (argc > 2);

    constexpr Numbers::size_type DefaultVecSize = 20000000ull;
    auto vecSize = DefaultVecSize;
    int loopSize = 1;

    if (setSize) {
        // vecSize must be shorter than INT_MAX
        vecSize = static_cast<decltype(vecSize)>(std::atoi(argv[1]));
        loopSize = std::atoi(argv[2]);
    }

    Numbers v(vecSize, 0.0);
    std::cout << "size: " << vecSize << " loop:" << loopSize << "\n";
    if (setSize) {
        generateNumbers(v);
    } else {
        generateNumbersStd(v);
    }

    const auto [mean, variance] = accumulateNumbers(v, loopSize);
    std::cout << "mean:" << mean << " variance:" << variance << "\n\n";
    return 0;
}

/*
Local Variables:
mode: c++
coding: utf-8-dos
tab-width: nil
c-file-style: "stroustrup"
End:
*/


// g++ -std=c++11 mandel2.cpp -o mandel2 -O3 -lpng

#include <iostream>
#include <array>
#include <complex>
#include <utility>
#include <tuple>
#include <sstream>

#define png_infopp_NULL (png_infopp)NULL
#define int_p_NULL (int*)NULL

#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_io.hpp>

#include <boost/format.hpp>

class FrAdapter;

class Fr
{
public:
	friend class FrAdapter;

	using fl_t = double;
	using cp_t = std::complex<fl_t>;

	Fr() = default;
	Fr(const Fr&) = default;
	~Fr() = default;

	std::pair<fl_t, fl_t> step() const {
		return std::make_pair(
			((xmax_ - xmin_) / res_x_)
			, ((ymax_ - ymin_) / res_y_)
			);
	}

	char project_iter(unsigned i) const {
		return ccc_[i * ccc_.size() / static_cast<fl_t>(max_)];
	}

	unsigned iterate(const cp_t& c) const {
		cp_t cc = c;
		unsigned i = 0;
		while(i < max_) {
			if (abs(cc) > 4.0)
				return i;
			cc = pow(cc, this->pow_) + c;
			++i;
		}
		return max_ - 1;
	}

	fl_t iterate_d(const cp_t& c) const {
		return this->iterate(c) / static_cast<fl_t>(max_);
	}

	void render(std::ostream& o = std::cout) const {
		fl_t fx, fy;
		std::tie(fx, fy) = this->step();
		for(unsigned ry = 0; ry < res_y_; ++ry) {
			fl_t y = ymax_ - ry * fy;
			for(unsigned rx = 0; rx < res_x_; ++rx) {
				unsigned i = this->iterate(cp_t(xmin_ + rx * fx, y));
				o << this->project_iter(i);
			}
			o << std::endl;
		}
	}

	void fit_range_to_res(fl_t fd = 0.5) {
		//std::pair<fl_t, fl_t> old = std::make_pair(res_x_, res_y_);
		fl_t fr = static_cast<fl_t>(res_x_) / static_cast<fl_t>(res_y_) * fd;
		//std::cout << "fr " << fr << std::endl;
		auto lx = (xmax_ - xmin_);
		auto ly = (ymax_ - ymin_);
		fl_t fw = lx / ly;
		//std::cout << "fw " << fw << std::endl;
		if (fr > fw) {
			// stretch x
			auto lxn = ly * fr;
			auto lxd = lxn - lx;
			xmin_ -= lxd / 2.0;
			xmax_ += lxd / 2.0;
		} else if (fr < fw) {
			// stretch y
			// fr = 5/4 = 1.25
			// fw = 3/2 = 1.5
			// fwn= 3/
			auto lyn = lx * 1 / fr;
			auto lyd = lyn - ly;
			ymin_ -= lyd / 2.0;
			ymax_ += lyd / 2.0;
		}
	}

	void set_res(unsigned rx, unsigned ry) {
		std::tie(this->res_x_, this->res_y_) = std::make_tuple(rx, ry);
	}

	void set_pow(fl_t p) {
		this->pow_ = p;
	}

	void set_max(unsigned m) {
		this->max_ = m;
	}

	void set_window(const std::tuple<fl_t, fl_t, fl_t, fl_t>& w) {
		std::tie(xmin_, ymin_, xmax_, ymax_) = w;
		if (xmin_ > xmax_)
			std::swap(xmin_, xmax_);
		if (ymin_ > ymax_)
			std::swap(ymin_, ymax_);
	}

private:
	fl_t xmin_{-2.0}, ymin_{-1.2}, xmax_{0.8}, ymax_{1.2};
	fl_t pow_{2.0};
	unsigned max_ {256};
	unsigned res_x_ {1000}, res_y_{500};
	std::array<char, 14> ccc_ {{'-', '.', ',', ':', ';', '+', '*', '=', 'o', 'O', '0', '#', 'M', ' '}};
};

class FrAdapter {
public:
	typedef boost::gil::point2<ptrdiff_t>   point_t;

    typedef FrAdapter           const_t;
    typedef boost::gil::gray8_pixel_t       value_type;
    typedef value_type          reference;
    typedef value_type          const_reference;
    typedef point_t             argument_type;
    typedef reference           result_type;
    BOOST_STATIC_CONSTANT(bool, is_mutable=false);

    FrAdapter(const point_t& siz = point_t(200,76), Fr::fl_t pow = 2.0) {
    	fr_.set_res(siz.x, siz.y);
    	fr_.fit_range_to_res(1.0);
    	fr_.set_pow(pow);
    }

    Fr::cp_t map_to_complex(const point_t& p) const {
    	return Fr::cp_t(((p.x / static_cast<Fr::fl_t>(fr_.res_x_)) * (fr_.xmax_ - fr_.xmin_)) + fr_.xmin_
    			      , ((p.y / static_cast<Fr::fl_t>(fr_.res_y_)) * (fr_.ymax_ - fr_.ymin_)) + fr_.ymin_
    		);
    }
    result_type operator()(const point_t& p) const {
    	auto c = this->map_to_complex(p);
    	//std::cerr << fr_.xmax_ - fr_.xmin_ << " ";
    	//std::cerr << "(" << p.x << "; " << p.y << ") -> " << c << std::endl; //<< c.real() << ", " << c.imag() << ")" << std::endl;
    	auto i = fr_.iterate_d(c);
    	return value_type((boost::gil::bits8)(pow(i,this->pow_)*255));
    }

    Fr& fr() {return fr_;};

    void set_pow(Fr::fl_t p) {
    	this->pow_ = p;
    }

private:
	Fr fr_;
	Fr::fl_t pow_{0.5};
};

int main(int argc, char const *argv[])
{
	//Fr f;
	//std::cout << f.step().first << " " << f.step().second << std::endl;
	//f.render();
	//f.set_res(200, 76);
	//f.set_res(400, 150);
	//f.set_res(800, 300);
	//f.set_res(4000, 1600);
	//f.render();
	//f.fit_range_to_res();
	//std::cout << f.step().first << " " << f.step().second << std::endl;
	//f.render();
	typedef boost::gil::virtual_2d_locator<FrAdapter,false> locator_t;
	typedef boost::gil::image_view<locator_t> my_virt_view_t;
	FrAdapter::point_t res(1000,1000);

	for(double d = 2.0; d < 2.1; d += 0.025) {
		for(double p = 0.5; p < 0.6; p += 0.1) {
			std::ostringstream oss;
			oss << boost::format{"p_%3.2f_%2.1f.png"} % d % p;
			std::string filename = oss.str();
			std::cout << filename << std::endl;
			FrAdapter fra(res, d);
			//fra.fr().set_window(std::make_tuple(-2.0, -1.5, 1.0, 1.5));
			fra.fr().set_window(std::make_tuple(-0.0, -0.9, 0.4, -0.4));
			fra.fr().fit_range_to_res(1.0);
			fra.set_pow(p);
			my_virt_view_t mandel(res, locator_t(FrAdapter::point_t(0, 0), FrAdapter::point_t(1, 1), fra));

			//boost::gil::gray8s_image_t img(res);
			boost::gil::png_write_view(filename, mandel);

		}
	}

	return 0;
}


#include <TSR.h>
#include <Eigen/Geometry>
#include <ompl/util/RandomNumbers.h>
#include <vector>

using namespace or_ompl;

TSR::TSR() : _initialized(false) {

}

TSR::TSR(const Eigen::Affine3d &T0_w, const Eigen::Affine3d &Tw_e, const Eigen::Matrix<double, 6, 2> &Bw) :
	_T0_w(T0_w), _Tw_e(Tw_e), _Bw(Bw), _initialized(true) {

	_T0_w_inv = _T0_w.inverse();
	_Tw_e_inv = _Tw_e.inverse();

}

bool TSR::deserialize(std::stringstream &ss) {

	// TODO: Do we need this stuff?
	int manipind_ignored;
    ss >> manipind_ignored;

	std::string relativebodyname_ignored;
    ss >> relativebodyname_ignored;

    if( relativebodyname_ignored != "NULL" )
    {
		std::string relativelinkname_ignored;
        ss >> relativelinkname_ignored;
    }

	// Read in the T0_w matrix
	double tmp;
	for(unsigned int c=0; c < 3; c++){
		for(unsigned int r=0; r < 3; r++){
			ss >> tmp;
			_T0_w.matrix()(r,c) = tmp;
		}
	}

	for(unsigned int idx=0; idx < 3; idx++){
		ss >> tmp;
		_T0_w.translation()(idx) = tmp;
	}

	// Read in the Tw_e matrix
	for(unsigned int c=0; c < 3; c++){
		for(unsigned int r=0; r < 3; r++){
			ss >> tmp;
			_Tw_e.matrix()(r,c) = tmp;
		}
	}

	for(unsigned int idx=0; idx < 3; idx++){
		ss >> tmp;
		_Tw_e.translation()(idx) = tmp;
	}

	// Read in the Bw matrix
	for(unsigned int r=0; r < 6; r++){
		for(unsigned int c=0; c < 2; c++){
			ss >> tmp;
			_Bw(r,c) = tmp;
		}
	}

	_T0_w_inv = _T0_w.inverse();
	_Tw_e_inv = _Tw_e.inverse();

	_initialized = true;

    return _initialized;
}

Eigen::Matrix<double, 6, 1> TSR::distance(const Eigen::Affine3d &ee_pose) const {
	Eigen::Matrix<double, 6, 1> dist = Eigen::Matrix<double, 6, 1>::Zero();

	// First compute the pose of the w frame in world coordinates, given the ee_pose
	Eigen::Affine3d w_in_world = ee_pose * _Tw_e_inv;

	// Next compute the pose of the w frame relative to its original pose (as specified by T0_w)
	Eigen::Affine3d w_offset = _T0_w_inv * w_in_world;

	// Now compute the elements of the distance matrix
	dist(0,0) = w_offset.translation()(0);
	dist(1,0) = w_offset.translation()(1);
	dist(2,0) = w_offset.translation()(2);
	dist(3,0) = atan2(w_offset.rotation()(2,1), w_offset.rotation()(2,2));
	dist(4,0) = -asin(w_offset.rotation()(2,0));
	dist(5,0) = atan2(w_offset.rotation()(1,0), w_offset.rotation()(0,0));

	return dist;
}

Eigen::Matrix<double, 6, 1> TSR::displacement(const Eigen::Affine3d &ee_pose) const {

	Eigen::Matrix<double, 6, 1> dist = distance(ee_pose);
	Eigen::Matrix<double, 6, 1> disp = Eigen::Matrix<double, 6, 1>::Zero();

	for(unsigned int idx=0; idx < 6; idx++){

		if(dist(idx,0) < _Bw(idx,0)){
			disp(idx,0) = dist(idx,0) - _Bw(idx,0);
		}else if(dist(idx,0) > _Bw(idx,1)){
			disp(idx,0) = dist(idx,0) - _Bw(idx,1);
		}
	}

	return disp;
}

Eigen::Affine3d TSR::sampleDisplacementTransform(void) const {

	// First sample uniformly betwee each of the bounds of Bw
	std::vector<double> d_sample(6);

	ompl::RNG rng;
	for(unsigned int idx=0; idx < d_sample.size(); idx++){
		if(_Bw(idx,1) > _Bw(idx,0)){
			d_sample[idx] = rng.uniformReal(_Bw(idx,0), _Bw(idx,1));
		}
	}

	Eigen::Affine3d return_tf;
	return_tf.translation() << d_sample[0], d_sample[1], d_sample[2];

	// Convert to a transform matrix
	double roll = d_sample[3];
	double pitch = d_sample[4];
	double yaw = d_sample[5];

	double A = cos(yaw);
	double B = sin(yaw);
	double C = cos(pitch);
	double D = sin(pitch);
	double E = cos(roll);
	double F = sin(roll);
	return_tf.linear() << A*C, A*D*F - B*E, B*F + A*D*E,
		B*C, A*E + B*D*F, B*D*E - A*F,
		-D, C*F, C*E;

	return return_tf;
}

Eigen::Affine3d TSR::sample() const {

	Eigen::Affine3d tf = sampleDisplacementTransform();

	return _T0_w * tf * _Tw_e;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Multipolygon DP simplification example from the mailing list discussion
// about the DP algorithm issue:
// http://lists.osgeo.org/pipermail/ggl/2011-September/001533.html

#include <boost/geometry.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
using namespace boost::geometry;

int main()
{
  typedef model::d2::point_xy<double> point_xy;

  point_xy p1(0.0, 0.0);
  point_xy p2(5.0, 0.0);

  // 1) This is direct call to Pythagoras algo
  typedef strategy::distance::pythagoras<point_xy, point_xy, double> strategy1_type;
  strategy1_type strategy1;
  strategy1_type ::calculation_type d1 = strategy1.apply(p1, p2);

  // 2) This is what is effectively called by simplify
  typedef strategy::distance::comparable::pythagoras<point_xy, point_xy, double> strategy2_type;
  strategy2_type strategy2;
  strategy2_type::calculation_type d2 = strategy2.apply(p1, p2);

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// cross_validation::error::mean_abs_error.hpp                              //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef  BOOST_STATISTICS_DETAIL_CROSS_VALIDATION_ERROR_MEAN_ABS_ERROR_HPP_ER_2009
#define  BOOST_STATISTICS_DETAIL_CROSS_VALIDATION_ERROR_MEAN_ABS_ERROR_HPP_ER_2009
#include <cmath>
#include <boost/iterator/iterator_traits.hpp>
#include <boost/range.hpp>
#include <boost/vector_space/functional/l1_distance.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace cross_validation{
namespace error{

    template<typename It,typename It1>
    typename iterator_value<It>::type
    mean_abs_error(
        It b,
        It e,
        It1 b1
    ){

        typedef iterator_range<It>                          range_;
        typedef typename iterator_difference<It>::type      diff_;
        typedef iterator_range<It1>                         range1_;
        typedef typename iterator_value<It>::type           val_;
        typedef vector_space::template l1_distance<range_>   l1_;

        diff_ d = std::distance(b,e);

        l1_ l1(range_(b,e));
        range1_ range1(
            b1,
            boost::next(
                b1,
                d
            )
        );
        val_ res = l1(range1);
        res /= static_cast<val_>(d);
        res = sqrt( res );
        return res;
    };

}// error
}// cross_validation
}// detail
}// statistics
}// boost

#endif


///
/// \file vandermonde.hpp
///
#ifndef MXPFIT_VANDERMONDE_LEAST_SQUARES_HPP
#define MXPFIT_VANDERMONDE_LEAST_SQUARES_HPP

#include <cassert>

#include <Eigen/Core>
#include <Eigen/IterativeLinearSolvers>

#include <mxpfit/matrix_free_gemv.hpp>
#include <mxpfit/vandermonde_matrix.hpp>

namespace mxpfit
{

namespace detail
{

///
/// \internal
///
/// Compute Cholesky decomposition of the gramian matrix of column Vandermonde
/// matrix.
///
/// This function compute the LDL decomposition of the gramian matrix,
///
/// \f[ G = V^{\ast} V = L^{} D L^{\ast}, \f]
///
/// where \f$ V=[v_{j}^{i}]_{i=1,\dots,m}^{n=1,\dots,n}\f$ is the column
/// Vandermonde matrix, \f$ L \f$ is a lower unit triangular matrix, and \f$ D
/// \f$ is a diagonal matrix.
///
/// \param[in] V  An \f$ m \times n \f$ column Vandermonde matrix to be
///   decomposed.
/// \param[out] ldlt  An \ f$ n \times n \f$ matrix to store the result of
///   decomposition. On exit, the diagonal elements of `ldlt` are those of
///   matrix \f$ D, \f$ and strict lower triangular part contains the off
///   diagonal elements of factor \f$ L \f$.
///
/// \param[out] work  An \f$ n \times 4 \f$ matrix used for workspace
///
template <typename T, typename MatrixT, typename MatrixWork>
void cholesky_vandermonde_gramian(const VandermondeMatrix<T>& V, MatrixT& ldlt,
                                  MatrixWork& work)
{
    using Scalar       = typename VandermondeMatrix<T>::Scalar;
    using CoeffsVector = typename VandermondeMatrix<T>::CoeffsVector;
    using RealScalar   = typename Eigen::NumTraits<Scalar>::Real;
    using Index        = Eigen::Index;

    using Eigen::numext::abs;
    using Eigen::numext::abs2;
    using Eigen::numext::conj;
    using Eigen::numext::real;
    using Eigen::numext::sqrt;

    static const auto tiny   = sqrt(std::numeric_limits<RealScalar>::min());
    constexpr const auto one = Scalar(1);

    auto z        = V.coeffs();
    const Index m = V.rows();
    const Index n = V.cols();

    assert(ldlt.rows() == n && ldlt.cols() == n);
    assert(work.rows() == n && work.cols() >= 4);

    // ----- Initialization
    auto y1 = work.col(0);
    auto y2 = work.col(1);
    auto x1 = work.col(2);
    auto x2 = work.col(3);

    auto gramian = [&](Index i, Index j) {
        const auto arg = conj(z(i)) * z(j);
        return arg == one ? Scalar(m) : (one - std::pow(arg, m)) / (one - arg);
    };

    auto sigma2 = gramian(0, 0);
    auto b0     = ldlt.col(0);
    b0(0)       = one;
    for (Index j = 1; j < n; ++j)
    {
        b0(j) = gramian(j, 0) / sigma2;
    }

    y1 = CoeffsVector::Ones(n) - b0;
    y2 = z.array().conjugate().pow(m);
    y2 -= y2(0) * b0;
    x1 = z.array().conjugate().inverse();
    x1 -= x1(0) * b0;
    x2 = -z.array().conjugate().pow(m - 1);
    x2 -= x2(0) * b0;

    b0(0) = sigma2;

    for (Index k = 1; k < n; ++k)
    {
        auto bk = ldlt.col(k);

        auto mu1 = x1(k);
        auto mu2 = x2(k);
        auto nu1 = conj(y1(k));
        auto nu2 = conj(y2(k));

        auto zk_inv = one / z(k);
        auto denom  = conj(zk_inv) - z(k);

        if (abs(denom) < tiny)
        {
            sigma2         = RealScalar(m);
            const auto bkk = real(ldlt(k, k));
            for (Index j = 0; j < k; ++j)
            {
                const auto bkj = ldlt(k, j);
                sigma2 -= abs2(bkj) * bkk;
            }
        }
        else
        {
            sigma2 = (mu1 * nu1 + mu2 * nu2) / denom;
        }

        bk(k) = sigma2;

        Index nt    = n - k - 1;
        bk.tail(nt) = (conj(mu1) / sigma2 * y1.tail(nt) +
                       conj(mu2) / sigma2 * y2.tail(nt));
        bk.array().tail(nt) /=
            (CoeffsVector::Constant(nt, zk_inv) - z.tail(nt).conjugate())
                .array();

        x1.tail(nt) -= mu1 * bk.tail(nt);
        x2.tail(nt) -= mu2 * bk.tail(nt);
        y1.tail(nt) -= conj(nu1) * bk.tail(nt);
        y2.tail(nt) -= conj(nu2) * bk.tail(nt);
    }

    return;
}

} // namespace detail

///
/// Preconditioner specialized for of Vandermonde matrix
///
template <typename T>
class VandermondePreconditioner
{
public:
    using Scalar     = T;
    using RealScalar = typename Eigen::NumTraits<Scalar>::Real;

    using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using Vector = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

    using StorageIndex = typename Matrix::StorageIndex;
    using Index        = Eigen::Index;

    using VandermondeGEMV = MatrixFreeGEMV<VandermondeMatrix<Scalar>>;
    enum
    {
        ColsAtCompileTime    = Eigen::Dynamic,
        MaxColsAtCompileTime = Eigen::Dynamic,
    };

    VandermondePreconditioner() = default;

    ~VandermondePreconditioner() = default;

    Index rows() const
    {
        return m_ldlt.rows();
    }

    Index cols() const
    {
        return m_ldlt.cols();
    }

    VandermondePreconditioner& analyzePattern(const VandermondeGEMV&)
    {
        return *this;
    }

    VandermondePreconditioner& factorize(const VandermondeGEMV& mat)
    {
        const Index n = mat.cols();

        m_ldlt.resize(n, n);
        m_invdiag.resize(n);
        Matrix work(n, 4);
        //
        // Compute Cholesky decomposition of the Gramian matrix of the form
        // \f$ V^{\ast} V = L D L^{\ast} \f$
        //
        detail::cholesky_vandermonde_gramian(mat.nestedExpression(), m_ldlt,
                                             work);

        for (Index i = 0; i < n; ++i)
        {
            if (m_ldlt(i, i) == Scalar())
            {
                m_invdiag(i) = RealScalar(1);
            }
            else
            {
                m_invdiag(i) = RealScalar(1) / m_ldlt(i, i);
            }
        }
        m_is_initialized = true;
        return *this;
    }

    VandermondePreconditioner& compute(const VandermondeGEMV& mat)
    {
        return factorize(mat);
    }

    template <typename Rhs, typename Dest>
    void _solve_impl(const Rhs& b, Dest& x) const
    {
        auto matL = m_ldlt.template triangularView<Eigen::UnitLower>();
        x         = matL.solve(b);
        x.array() *= m_invdiag.array();
        matL.adjoint().solveInPlace(x);
    }

    template <typename Rhs>
    inline const Eigen::Solve<VandermondePreconditioner, Rhs>
    solve(const Eigen::MatrixBase<Rhs>& b) const
    {
        eigen_assert(m_is_initialized &&
                     "VandermondePreconditioner is not initialized.");
        eigen_assert(m_ldlt.cols() == b.rows() &&
                     "VandermondePreconditioner::solve(): invalid "
                     "number of rows of the right hand side matrix b");
        return Eigen::Solve<VandermondePreconditioner, Rhs>(*this, b.derived());
    }

    Eigen::ComputationInfo info()
    {
        return Eigen::Success;
    }

private:
    Matrix m_ldlt;
    Vector m_invdiag;
    bool m_is_initialized;
};

///
/// ### VandermondeLeastSquaresSolver
///
/// Solve a least squares problem, \f$V \boldsymbol{x}=\boldsymbol{b},\f$ where
/// \f$V\f$ is a column Vandermonde matrix.
///
template <typename T>
using VandermondeLeastSquaresSolver =
    Eigen::LeastSquaresConjugateGradient<MatrixFreeGEMV<VandermondeMatrix<T>>,
                                         VandermondePreconditioner<T>>;

} // namespace mxpfit

#endif /* MXPFIT_VANDERMONDE_LEAST_SQUARES_HPP */