/// \file
/// Maintainer: Luzian Hug
/// Created: 25.03.2018
///
///

#include "kmeans.h"
#include "spatial/uniform_grid.h"
#include <Eigen/Dense>
#include <boost/log/trivial.hpp>
#include <iostream>

namespace MouseTrack {

KMeans::KMeans(int K) : _k(K) {
  // empty
}

std::vector<Cluster> KMeans::operator()(const PointCloud &cloud) const {
  BOOST_LOG_TRIVIAL(trace) << "KMeans algorithm started";
  // Convert point cloud to Eigen vectors

  if (cloud.size() == 0) {
    return std::vector<Cluster>();
  }

  const int dims = cloud.charDim();
  PointList means(dims, K());
  PointList prevMeans;

  PointList points;
  points.resize(dims, cloud.size());
  for (PointIndex i = 0; i < cloud.size(); i += 1) {
    auto v = cloud[i].characteristic();
    points.col(i) = v;
  }

  // TODO: get kmeans++ initialization
  means = points.block(0, 0, dims, K());

  Eigen::VectorXd min = points.rowwise().minCoeff();
  Eigen::VectorXd max = points.rowwise().maxCoeff();
  Eigen::VectorXd bb_size = max - min;

  BOOST_LOG_TRIVIAL(debug) << "KMeans: bb: " << bb_size;

  std::lock_guard<std::mutex> lock(_oracleMutex);
  if (_cachedOracle.get() == nullptr ||
      !(bb_size.array() <= _cachedBoundingBox.array()).all()) {
    bb_size *= 1.5;
    OFactory::Query q;
    q.dimensions = bb_size.size();
    q.bb_size = &bb_size;
    _cachedOracle = oracleFactory().forQuery(q);
    _cachedBoundingBox = bb_size;
  }
  Oracle &oracle = *_cachedOracle;

  std::vector<Cluster> clusters(K()), prevClusters;

  BOOST_LOG_TRIVIAL(debug) << "KMeans: Starting iterations.";
  do {
    // assign clusters
    BOOST_LOG_TRIVIAL(trace) << "KMeans: Assigning clusters";
    oracle.compute(means);
    prevClusters = std::move(clusters);
    clusters = std::vector<Cluster>(K());
    std::vector<std::vector<PointIndex>> allCs = oracle.find_closest(points, 1);
    for (int i = 0; i < points.cols(); ++i) {
      auto &cs = allCs[i];
      if (cs.empty()) {
        BOOST_LOG_TRIVIAL(error)
            << "Couldn't find nearest mean for point " << i << " ("
            << points.col(i) << "), assigning to 1";
        cs.push_back(1);
      }
      clusters[cs[0]].points().push_back(i);
    }

    // calculate new cluster centers
    BOOST_LOG_TRIVIAL(trace) << "KMeans: Assigning centroids";
    prevMeans = means;
    for (PointIndex c = 0; c < clusters.size(); ++c) {
      PointList assigned(dims, clusters[c].points().size());
      for (PointIndex i = 0; i < clusters[c].points().size(); ++i) {
        int pi = clusters[c].points()[i];
        assigned.col(i) = points.col(pi);
      }
      Eigen::VectorXd mean =
          assigned.array().rowwise().sum() / (assigned.cols() + .000001);
      means.col(c) = mean;
    }
  } while (!meansConverged(means, prevMeans) &&
           !assignmentConverged(clusters, prevClusters, cloud.size()));

  std::stringstream ss;
  ss << clusters[0].points().size();
  for (size_t i = 1; i < clusters.size(); ++i) {
    ss << ", " << clusters[i].points().size();
  }

  BOOST_LOG_TRIVIAL(trace) << "K-Means cluster sizes: " << ss.str();

  return clusters;
}

void KMeans::K(int k) { _k = k; }
int KMeans::K() const { return _k; }

void KMeans::centroidThreshold(double threshold) {
  _centroidThreshold = threshold;
}

double KMeans::centroidThreshold() const { return _centroidThreshold; }

void KMeans::assignmentThreshold(double threshold) {
  _assignmentThreshold = threshold;
}

double KMeans::assignmentThreshold() const { return _assignmentThreshold; }

KMeans::OFactory &KMeans::oracleFactory() { return _oracleFactory; }

const KMeans::OFactory &KMeans::oracleFactory() const { return _oracleFactory; }

bool KMeans::meansConverged(const PointList &newMeans,
                            const PointList &lastMeans) const {
  auto centroidChange = (lastMeans - newMeans).colwise().norm();
  Precision change = centroidChange.maxCoeff();
  BOOST_LOG_TRIVIAL(trace) << "worst means change: " << change;
  return change <= centroidThreshold();
}
bool KMeans::assignmentConverged(const std::vector<Cluster> &newClusters,
                                 const std::vector<Cluster> &lastClusters,
                                 int totalPoints) const {
  int switched = 0;
  for (int i = 0; i < K(); ++i) {
    // rough estimate how many points changed assignment
    int delta =
        newClusters[i].points().size() - lastClusters[i].points().size();
    delta = std::abs(delta);
    switched += delta;
  }
  // double-counting??
  // switched /= 2;
  Precision switchedPercentage = switched / (Precision)totalPoints;
  BOOST_LOG_TRIVIAL(trace) << "switched change: " << switched << "/"
                           << totalPoints << " (" << (switchedPercentage * 100)
                           << "%)";
  return switchedPercentage <= assignmentThreshold();
}

} // namespace MouseTrack


#include <boost/random.hpp>

boost::lagged_fibonacci1279 Tools::gen_;

Eigen::MatrixXd Tools::getWGNoise(const Eigen::MatrixXd& std,
	const Eigen::MatrixXd& bias,Index rows, Index cols)
{
	boost::normal_distribution<> g(0, 1);
	Eigen::MatrixXd ret= Eigen::MatrixXd::Zero(rows,cols);
	for (Index i=0;i<rows;++i)
	{
	    for (Index j=0; j<cols; ++j)
            ret(i,j)=g(gen_);
	}
	ret=std*ret+bias;

	return ret;
}


/* Boost check_gmp.cpp test file

 Copyright 2009 Karsten Ahnert
 Copyright 2009 Mario Mulansky

 This file tests the odeint library with the gmp arbitrary precision types

 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)
*/

#define BOOST_TEST_MODULE odeint_gmp

#include <gmpxx.h>

#include <boost/test/unit_test.hpp>
#include <boost/array.hpp>

#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

const int precision = 1024;

typedef mpf_class value_type;
typedef boost::array< value_type , 1 > state_type;

void constant_system( state_type &x , state_type &dxdt , value_type t )
{
    dxdt[0] = value_type( 1.0 , precision );
}

BOOST_AUTO_TEST_CASE( gmp )
{
    /* We have to specify the desired precision in advance! */
    mpf_set_default_prec( precision );

    mpf_t eps_ , unity;
    mpf_init( eps_ ); mpf_init( unity );
    mpf_set_d( unity , 1.0 );
    mpf_div_2exp( eps_ , unity , precision-1 ); // 2^(-precision+1) : smallest number that can be represented with used precision
    value_type eps( eps_ );

    runge_kutta4< state_type , value_type > stepper;
    state_type x;
    x[0] = 0.0;

    stepper.do_step( constant_system , x , 0.0 , 0.1 );

    BOOST_MESSAGE( eps );
    BOOST_CHECK_MESSAGE( abs( x[0] - value_type( 0.1 , precision ) ) < eps , x[0] - 0.1 );
}


/// \file

#ifndef PERCEPTRON_INCLUDED
#define PERCEPTRON_INCLUDED

#include <iostream>
#include <cstdlib>
#include <Eigen/Dense>
#include <tbb/tbb.h>

namespace neuralnetwork
{

  enum InOutType
  {
    DYNAMIC = -1
  };

  /// \brief Les entrees et sorties du Perceptron sont un simple vecteur Eigen
  template<typename t, int INPUT_SIZE>
  using InOut = Eigen::Matrix<t, INPUT_SIZE, 1>;

  /// \brief On definit dans ce namespace quelques fonctions d'activations, on peut tout de meme en definir ailleurs l'important etant qu'elles aient un unique argument de type double et qu'elles renvoient un double
  namespace activation
  {
    double SIGMOID(double x)
    {
      return 1.0/(1 + exp(-x));

    }

    double D_SIGMOID(double x)
    {
      double res = exp(x)/pow(exp(x)+1, 2);
      if (res!=res)
      {
        res = 0.0001;
      }
      return res;
    }
    double AFFINE(double x)
    {
      return x;
    }
    double D_AFFINE(double x)
    {
      return 1.0;
    }
  }

  /// \brief Classe de noeud de propagation (calculs) du graphe de feed forward
  /// \class Forward_Node
  class Forward_Node
  {
    private:

      /// \brief Matrice a laquelle est affecte le noeud du graphe
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>* weights;

      /// \brief Agregation en sortie
      Eigen::Matrix<double, Eigen::Dynamic, 1>* agreg_out;

      /// \brief Activation en sortie
      Eigen::Matrix<double, Eigen::Dynamic, 1>* act_out;

      /// \brief Activation en entree
      Eigen::Matrix<double, Eigen::Dynamic, 1>* act_in;

      /// \brief Fonction d'activation
      double (*f)(double);

      /// \brief Premiere ligne de la matrice dont s'occupe le noeud
      int first_row;

      /// \brief Premiere colonne de la matrice dont s'occupe le noeud (a priori 0)
      int first_col;

      /// \brief Nombre de lignes dont s'occupe le noeud
      int nb_rows;

      /// \brief Nombre de colonnes dont s'occupe le noeud (a priori la largeur de la matrice)
      int nb_cols;

    public:

      /// \brief Constructeur
      /// \param w Matrice a laquelle est affecte le noeud du graphe
      /// \param act Activation en entree
      /// \param agreg Agregation en sortie
      /// \param act2 Activation en sortie
      /// \param f_act Fonction d'activation
      /// \param f_row Premiere ligne de la matrice dont s'occupe le noeud
      /// \param f_col Premiere colonne de la matrice dont s'occupe le noeud (a priori 0)
      /// \param n_rows Nombre de lignes dont s'occupe le noeud
      /// \param n_cols Nombre de colonnes dont s'occupe le noeud (a priori la largeur de la matrice)
      Forward_Node
      (
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>* w,
        Eigen::Matrix<double, Eigen::Dynamic, 1>* act,
        Eigen::Matrix<double, Eigen::Dynamic, 1>* agreg,
        Eigen::Matrix<double, Eigen::Dynamic, 1>* act2,
        double(*f_act)(double),
        int f_row,
        int f_col,
        int n_rows,
        int n_cols
      ):
      weights(w),
      agreg_out(agreg),
      act_out(act2),
      act_in(act),
      f(f_act),
      first_row(f_row),
      first_col(f_col),
      nb_rows(n_rows),
      nb_cols(n_cols)
      {}

      void operator()(tbb::flow::continue_msg m)
      {
        agreg_out->block(first_row, 0, nb_rows, 1) = (weights->block(first_row, first_col, nb_rows, nb_cols) * (*act_in));
        for(int i = first_row; i < first_row + nb_rows; ++i)
        {
          act_out->operator()(i) = f(agreg_out->operator()(i));
        }
      }

  };

  /// \brief Classe de noeud de retropropagation (calculs) du graphe de retropropagation
  /// \class Backward_Node
  class Backward_Node
  {
    private:

      /// \brief Matrice a laquelle est affecte le noeud du graphe
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>* weights;

      /// \brief Agregation en entree
      Eigen::Matrix<double, Eigen::Dynamic, 1>* agreg_in;

      /// \brief Erreur en sortie
      Eigen::Matrix<double, Eigen::Dynamic, 1>* err_out;

      /// \brief Erreur en entree
      Eigen::Matrix<double, Eigen::Dynamic, 1>* err_in;

      /// \brief Derivee de la fonction d'activation
      double (*df)(double);

      /// \brief Premiere ligne de la matrice dont s'occupe le noeud
      int first_row;

      /// \brief Premiere colonne de la matrice dont s'occupe le noeud (a priori 0)
      int first_col;

      /// \brief Nombre de lignes dont s'occupe le noeud
      int nb_rows;

      /// \brief Nombre de colonnes dont s'occupe le noeud (a priori la largeur de la matrice)
      int nb_cols;

    public:
      /// \brief Constructeur
      /// \param w Transposee de la matrice a laquelle est affecte le noeud du graphe
      /// \param err Erreur en entree
      /// \param agreg Agregation en entree
      /// \param err2 Erreur en sortie
      /// \param df_act Derive de la fonction d'activation
      /// \param f_row Premiere ligne de la tranposee dont s'occupe le noeud
      /// \param f_col Premiere colonne de la transposee dont s'occupe le noeud (a priori 0)
      /// \param n_rows Nombre de lignes dont s'occupe le noeud
      /// \param n_cols Nombre de colonnes dont s'occupe le noeud (a priori la largeur de la tranposee)
      Backward_Node
      (
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>* w,
        Eigen::Matrix<double, Eigen::Dynamic, 1>* err,
        Eigen::Matrix<double, Eigen::Dynamic, 1>* agreg,
        Eigen::Matrix<double, Eigen::Dynamic, 1>* err2,
        double(*df_act)(double),
        int f_row,
        int f_col,
        int n_rows,
        int n_cols
      ):
      weights(w),
      agreg_in(agreg),
      err_out(err2),
      err_in(err),
      df(df_act),
      first_row(f_row),
      first_col(f_col),
      nb_rows(n_rows),
      nb_cols(n_cols)
      {}

      void operator()(tbb::flow::continue_msg m)
      {
        err_out->block(first_row, 0, nb_rows, 1) = weights->transpose().block(first_row, first_col, nb_rows, nb_cols)* (*err_in);
        for(int i = first_row; i < first_row + nb_rows; ++i)
        {
          err_out->operator()(i) = df(agreg_in->operator()(i)) * err_out->operator()(i);
        }
      }
  };

  /// \brief Classe de noeud de mise a jour (calculs) du graphe de retropropagation
  /// \class Update_Node
  class Update_Node
  {
    private:

      /// \brief Matrice a laquelle est affecte le noeud du graphe
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>* weights;

      /// \brief Pas d'apprentissage
      double step;

      /// \brief Erreur en entree
      Eigen::Matrix<double, Eigen::Dynamic, 1>* err;

      /// \brief Activation en sortie
      Eigen::Matrix<double, Eigen::Dynamic, 1>* act;

      /// \brief Premiere ligne de la matrice dont s'occupe le noeud
      int first_row;

      /// \brief Premiere colonne de la matrice dont s'occupe le noeud (a priori 0)
      int first_col;

      /// \brief Nombre de lignes dont s'occupe le noeud
      int nb_rows;

      /// \brief Nombre de colonnes dont s'occupe le noeud (a priori la largeur de la matrice)
      int nb_cols;

    public:
      /// \brief Constructeur
      /// \param w Matrice a laquelle est affecte le noeud du graphe
      /// \param s Pas d'apprentissage
      /// \param e Erreur en entree
      /// \param a Activation en sortie
      /// \param f_act Fonction d'activation
      /// \param f_row Premiere ligne de la matrice dont s'occupe le noeud
      /// \param f_col Premiere colonne de la matrice dont s'occupe le noeud (a priori 0)
      /// \param n_rows Nombre de lignes dont s'occupe le noeud
      /// \param n_cols Nombre de colonnes dont s'occupe le noeud (a priori la largeur de la matrice)
      Update_Node
      (
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>* w,
        double s,
        Eigen::Matrix<double, Eigen::Dynamic, 1>* e,
        Eigen::Matrix<double, Eigen::Dynamic, 1>* a,
        int f_row,
        int f_col,
        int n_rows,
        int n_cols
      ):
      weights(w),
      step(s),
      err(e),
      act(a),
      first_row(f_row),
      first_col(f_col),
      nb_rows(n_rows),
      nb_cols(n_cols)
      {}

      void operator()(tbb::flow::continue_msg m)
      {
          for(int i = first_row; i < first_row + nb_rows; ++i)
          {
            weights->block(i, first_col, 1, nb_cols) += step * err->operator()(i) * act->transpose();
          }
      }

  };

  /// \brief Classe de noeud d'attente des graphes de feed forwards et de retropropagation
  /// \class Wait_Node
  class Wait_Node
  {
    private:

    public:
      Wait_Node()
      {}

      void operator()(tbb::flow::continue_msg m)
      {
      }
  };

  /// \brief Classe Perceptron
  /// \class Perceptron
  /// \tparam DEPTH nombres de couches du perceptron sans compter la couche d'entrée
  template<std::size_t DEPTH>
  class Perceptron
  {

    public:

      /// \brief Constructeur, les poids du reseau sont initialises entre deux bornes de maniere aleatoire, on definit egalement la taille des couches et leurs fonctions d'activation
      /// \param min Valeur minimal d'un poids du reseau
      /// \param max Valeur maximal d'un poids du reseau
      /// \param args Trois arguments pour definie une couche, le premier est le nombre de neurone de la couche, le deuxieme est est la fonction d'activation, le troisieme est la derivee de la fonction d'activation. Les trois premiers arguments definissent la premiere couche cachee, les trois suivants la deuxieme couche cachee, etc... la fonction d'activation et sa derivee doivent etre des pointeurs sur fonction qui prennent un double et retourne un double
      template <typename ...Args>
      Perceptron(double min, double max, Args... args);

      /// \brief Initialisation aleatoire des poids du reseau entre deux bornes
      /// \param min Valeur minimal d'un poids du reseau
      /// \param max Valeur maximal d'un poids du reseau
      /// \return Rien
      void initWeights(double min, double max);

      /// \brief Accesseur des poids du reseau
      /// \param couch La couche a laquelle appartient le neurone qui a le poids sur une de ses entrees
      /// \param neuron Le neurone de la couche
      /// \param previous Le neurone de la couche precedente
      /// \return Accesseur sur le poids
      double& weight(std::size_t couch, std::size_t neuron, std::size_t previous);

      /// \brief Nombre de poids de chaque neurone d'une couche
      /// \param couch La couche en question
      /// \return le nombre de poids pour un neurone de la couche en question
      std::size_t weights_count(std::size_t couch);

      /// \brief Profondeur du reseau
      /// \return La profondeur du reseau sans prendre en compte la couche d'entree
      std::size_t depth();

      /// \brief Nombre de neurones d'une couche donnee
      /// \param couch La couche en question
      /// \return Nombre de neurones de la couche en question
      std::size_t couch_size(std::size_t couch);

      //Eigen::Matrix<double, Eigen::Dynamic, 1> agreg(std::size_t couch);
      /// \brief Les dernieres agregations enregistrees pour une couche donnee (methode principalement utile au debugage)
      /// \param couch La couche en question
      /// \return une entree-sortie de neurones qui comporte toutes les dernieres agregation enregistrees de la couche en question
      neuralnetwork::InOut<double,neuralnetwork::InOutType::DYNAMIC> agreg(std::size_t couch);

      //Eigen::Matrix<double, Eigen::Dynamic, 1> activation(std::size_t couch);
      /// \brief Les dernieres activations enregistrees pour une couche donnee (methode principalement utile au debugage)
      /// \param couch La couche en question
      /// \return une entree-sortie de neurones qui comporte toutes les dernieres activations enregistrees de la couche en question
      neuralnetwork::InOut<double,neuralnetwork::InOutType::DYNAMIC> activation(std::size_t couch);

      /// \brief Ecrit la matrice des poids d'une couche, une ligne correspond a un neurone, les poids sont dans l'odre des entrees provenant de la couche precedente et les agregations et activations de cette couche (methode principalement utile au debugage)
      /// \param out reference sur le flux de sortie ou l'on veut afficher les informations
      /// \param i La couche en question
      /// \return Rien
      void printMatrixCouch(std::ostream& out, std::size_t i);

      /// \brief Evalue une entree avec le reseau
      /// \param in L'entree a evaluer
      /// \param out La sortie du reseau
      /// \param saveAgreg Si place a true, les agregations des differentes couches seront enregistrees
      /// \param saveActivation Si place a true, les activations des differentes couches seront enregistrees
      /// \tparam tin type d'entree du reseau (a priori un double)
      /// \tparam INPUT_SIZE taille d'entree du reseau
      /// \tparam tout type des sorties du reseau (a priori un double)
      /// \tparam OUTPUT_SIZE taille de sortie du reseau
      template<typename tin, int INPUT_SIZE, typename tout, int OUTPUT_SIZE>
      void feedForward(neuralnetwork::InOut<tin, INPUT_SIZE>& in, neuralnetwork::InOut<tout, OUTPUT_SIZE>& out, bool saveAgreg, bool saveActivation);

      /// \brief Applique la fonction d'activation sur l'agregation calulee d'une couche
      /// \param in L'aggregation calculee de la couche fi
      /// \param fi La couche en question
      /// \return Rien
      /// \tparam tin type de l'agreation (a priori un double)
      /// \tparam INPUT_SIZE taille de la couche
      /// \tparam tout type de l'activation (a priori un double)
      /// \tparam OUTPUT_SIZE taille de la couche
      /// \todo possibilite de se passer de in si feedForward enregistre systematiquement l'agregation
      template<typename t, int INPUT_SIZE>
      void applyActivation(neuralnetwork::InOut<t, INPUT_SIZE>& in, std::size_t fi);

      /// \brief Applique l'algorithme de retropropagation du gradient avec un jeu d'exemples afin d'entrainer le reseau
      /// \param ins Tableau des exemples d'entrees du reseau
      /// \param outs Tableau des sorties attendues pour chaque exemple de ins (organises dans le meme ordre)
      /// \param n Nombre d'exemples
      /// \param step Pas d'apprentissage
      /// \return Rien
      /// \tparam tin type des entrees (a priori double)
      /// \tparam INPUT_SIZE taille d'entree du reseau
      /// \tparam tout type des sorties du reseau (a priori double)
      /// \tparam OUTPUT_SIZE taille de sortie du reseau
      template<typename tin, int INPUT_SIZE, typename tout, int OUTPUT_SIZE>
      void backpropagation(neuralnetwork::InOut<tin, INPUT_SIZE>* ins, neuralnetwork::InOut<tout, OUTPUT_SIZE>* outs, std::size_t n, double step);

      /// \brief Initailise les graphes TBB pour le feed forward et la retropropagation
      /// \param step Pas d'apprentissage
      /// \param nb_threads En combien de lignes on decoupe chaque matrice et chaque transposee (meme nombre que les threads conseille)
      /// \return Rien
      void initGraph(double step, int nb_threads);

      /// \brief Evalue une entree avec le reseau avec TBB
      /// \param in L'entree a evaluer
      /// \param out La sortie du reseau
      /// \return Rien
      /// \tparam tin type d'entree du reseau (a priori un double)
      /// \tparam INPUT_SIZE taille d'entree du reseau
      /// \tparam tout type des sorties du reseau (a priori un double)
      /// \tparam OUTPUT_SIZE taille de sortie du reseau
      /// \return Rien
      template<typename tin, int INPUT_SIZE, typename tout, int OUTPUT_SIZE>
      void parallel_feedForward(neuralnetwork::InOut<tin, INPUT_SIZE>& in, neuralnetwork::InOut<tout, OUTPUT_SIZE>& out);

      /// \brief Applique l'algorithme de retropropagation du gradient avec un jeu d'exemples afin d'entrainer le reseau avec TBB
      /// \param ins Tableau des exemples d'entrees du reseau
      /// \param outs Tableau des sorties attendues pour chaque exemple de ins (organises dans le meme ordre)
      /// \param n Nombre d'exemples
      /// \param step Pas d'apprentissage
      /// \return Rien
      /// \tparam tin type des entrees (a priori double)
      /// \tparam INPUT_SIZE taille d'entree du reseau
      /// \tparam tout type des sorties du reseau (a priori double)
      /// \tparam OUTPUT_SIZE taille de sortie du reseau
      template<typename tin, int INPUT_SIZE, typename tout, int OUTPUT_SIZE>
      void parallel_backpropagation(neuralnetwork::InOut<tin, INPUT_SIZE>* ins, neuralnetwork::InOut<tout, OUTPUT_SIZE>* outs, std::size_t n, double step);

    private:

      /// \brief Methode utilitaire pour le constructeur, on definit egalement la taille des couches et leurs fonctions d'activation. Cette methode parcourt recursivement toutes les couches
      /// \param n Couche a parametrer
      /// \param input_size Nombres de poids pour les neurones de cette couche, egal au nombre de neurones de la couche precedente
      /// \param neurons Taille de cette couche en nombres de neurones
      /// \param f Fonction d'activation de cette couche
      /// \param df Derivee de la fonction d'activation de cette couche
      /// \param args Parametres pour les couches suivantes
      /// \return Rien
      template<typename... Args>
      void init(unsigned int n, int input_size, int neurons, double (*f)(double), double (*df)(double), Args ...args);

      /// \brief Dernier appel de init pour la derniere couche a parametrer
      void init(unsigned int n, int input_size, int neurons, double (*f)(double), double (*df)(double));

      /// \brief tableau des fonctions d'activations, une par couche
      double (*m_array_f[DEPTH])(double);

      /// \brief tableau des derivees des fonctions d'activations, une par couche
      double (*m_array_df[DEPTH])(double);

      /// \brief profondeur du reseau
      const std::size_t m_size = DEPTH;

      /// \brief tableau des matrices des couches. Une ligne represente les poids d'un neurone, l'element i de la ligne j represente le poids reliant le sortie du neurones i de la couche precedente avec le neurone j de la couche actuelle
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> m_array_matrix[DEPTH];

      /// \brief tableau des agregations
      Eigen::Matrix<double, Eigen::Dynamic, 1> m_array_agreg[DEPTH];

      /// \brief tableau des activations
      Eigen::Matrix<double, Eigen::Dynamic, 1> m_array_act[DEPTH];

      /// \brief tableau des erreurs
      Eigen::Matrix<double, Eigen::Dynamic, 1> m_array_err[DEPTH];

      /// \brief Vecteur d'entree du graph
      Eigen::Matrix<double, Eigen::Dynamic, 1> m_first_graph_vector;

      /// \brief Noeuds des neurones du graphe pour le feed forward
      std::vector<std::vector<tbb::flow::continue_node<tbb::flow::continue_msg>>> m_forward_neurons;

      /// \brief Noeuds d'attente des neurones du graphe pour le feed forward
      std::vector<tbb::flow::continue_node<tbb::flow::continue_msg>> m_forward_waits;

      /// \brief Graphe du reseau pour le feed forward
      tbb::flow::graph m_graph_forward;

      /// \brief Noeuds des neurones du graphe pour la retropropagation
      std::vector<std::vector<tbb::flow::continue_node<tbb::flow::continue_msg>>> m_backward_neurons;

      /// \brief Noeuds d'attente des neurones du graphe pour la retropropagation
      std::vector<tbb::flow::continue_node<tbb::flow::continue_msg>> m_backward_waits;

      /// \brief Graphe du reseau pour le backward
      tbb::flow::graph m_graph_backward;

      /// \brief Noeuds des neurones du graphe pour la mise a jour des poids
      std::vector<std::vector<tbb::flow::continue_node<tbb::flow::continue_msg>>> m_update_neurons;

  };
}

/// \fn std::ostream& operator<<(std::ostream& out, neuralnetwork::Perceptron<DEPTH> p)
/// \brief Affiche les matrices de poids du reseau et les agregations et activations de chaque couche (methode principalement utile au debugage)
/// \param out reference sur le flux de sortie ou l'on veut afficher les informations
/// \param p Perceptron a afficher
/// \return out avec de nouvelles informations concernant le reseau dedans
/// \warning La fonction est incoherente, une partie des informations est ecrite dans out et l'autre dans std::cout. Cette fonction sert principalement au debugage
template<std::size_t DEPTH>
std::ostream& operator<<(std::ostream& out, neuralnetwork::Perceptron<DEPTH> p)
{
  out << DEPTH <<" couches" << std::endl;
  for(int i = 0; i < DEPTH; ++i)
  {
    out << "Couche " << i <<std::endl;
    p.printMatrixCouch(out, i);
    //getchar();
    out << std::endl;
    out << "Agregation:" << std::endl << p.agreg(i);
    //getchar();
    out << std::endl;
    out << "Activation:" << std::endl << p.activation(i);
    //getchar();
    out << std::endl;
  }

  return out;
}

template<std::size_t DEPTH>
template <typename ...Args>
neuralnetwork::Perceptron<DEPTH>::Perceptron(double min, double max, Args... args)
{
  init(0, args...);
  initWeights(min, max);
  m_first_graph_vector.resize(m_array_matrix[0].cols(), 1);
}

template<std::size_t DEPTH>
template<typename... Args>
void neuralnetwork::Perceptron<DEPTH>::init(unsigned int n, int input_size, int neurons, double (*f)(double), double (*df)(double), Args ...args)
{
  //On dimensionne la matrice representant la couche et les vecteurs d'agreations, d'activations et d'erreurs pour cette meme couche
  m_array_err[n].resize(neurons, 1);
  m_array_agreg[n].resize(neurons, 1);
  m_array_act[n].resize(neurons, 1);
  m_array_matrix[n].resize(neurons, input_size);

  //On donne la bonne fonction et la bonne derivee a la couche
  m_array_f[n] = f;
  m_array_df[n] = df;
  if(n < DEPTH )
  {
    init(n+1, neurons, args...);
  }
}

template<std::size_t DEPTH>
void neuralnetwork::Perceptron<DEPTH>::init(unsigned int n, int input_size, int neurons, double (*f)(double), double (*df)(double))
{
  m_array_agreg[n].resize(neurons, 1);
  m_array_act[n].resize(neurons, 1);
  m_array_err[n].resize(neurons, 1);
  m_array_matrix[n].resize(neurons, input_size);
  m_array_f[n] = f;
  m_array_df[n] = df;
}

template<std::size_t DEPTH>
void neuralnetwork::Perceptron<DEPTH>::initWeights(double min, double max)
{
  for(unsigned int i = 0; i < depth() ;++i)
  {
    for(unsigned int j = 0; j < couch_size(i); ++j)
    {
      for(unsigned int k = 0; k < weights_count(i); ++k)
      {
        weight(i, j, k) = (std::rand()/(static_cast<double>(RAND_MAX) + 1.0)) * (max - min) + min;
      }
    }

  }
}

template<std::size_t DEPTH>
void neuralnetwork::Perceptron<DEPTH>::initGraph(double step, int nb_threads)
{

  //Feed forward
  for(unsigned int i = 0; i < depth(); ++i)
  {
    int first_row = 0;
    tbb::flow::continue_node<tbb::flow::continue_msg> new_wait (m_graph_forward, Wait_Node());
    m_forward_waits.push_back(new_wait);
    std::vector<tbb::flow::continue_node<tbb::flow::continue_msg>> new_couch;
    for(int j = 0; j < nb_threads; ++j)
    {
      int nb_rows;
      int nb_cols = m_array_matrix[i].cols();
      //Repartition des lignes entre les noeuds
      if(j < (m_array_matrix[i].rows() % nb_threads))
      {
        nb_rows = couch_size(i) / nb_threads + 1;
      }
      else
      {
        nb_rows = couch_size(i) / nb_threads;
      }
      int first_col = 0;

      //Pour n'importe quel couche hormis la premiere, l'entree est l'activation de la i-1eme couche, sinon c'est le vecteur d'entree
      if(i != 0)
      {
        tbb::flow::continue_node<tbb::flow::continue_msg> new_node (
                                  m_graph_forward,
                                  neuralnetwork::Forward_Node(
                                         &(m_array_matrix[i]),
                                         &(m_array_act[i - 1]),
                                         &(m_array_agreg[i]),
                                         &(m_array_act[i]),
                                         m_array_f[i],
                                         first_row,
                                         first_col,
                                         nb_rows,
                                         nb_cols

                                  )
                                  );
        new_couch.push_back(new_node);
      }
      else
      {
        tbb::flow::continue_node<tbb::flow::continue_msg> new_node (
                                  m_graph_forward,
                                  neuralnetwork::Forward_Node(
                                         &(m_array_matrix[i]),
                                         &(m_first_graph_vector),
                                         &(m_array_agreg[i]),
                                         &(m_array_act[i]),
                                         m_array_f[i],
                                         first_row,
                                         first_col,
                                         nb_rows,
                                         nb_cols
                                  )
                                  );
        new_couch.push_back(new_node);
      }
      first_row += nb_rows;
    }
    m_forward_neurons.push_back(new_couch);
  }

  //On lie les couches aux noeuds barrieres
  for(unsigned int i = 0; i < m_forward_waits.size(); ++i)
  {
    for(unsigned int j = 0; j < m_forward_neurons[i].size(); ++j)
    {
      tbb::flow::make_edge(m_forward_neurons[i][j], m_forward_waits[i]);
      if(i != 0)
      {
        tbb::flow::make_edge(m_forward_waits[i - 1], m_forward_neurons[i][j]);
      }
    }
  }

  //Backward
  for(unsigned int i = 1; i < DEPTH; ++i)
  {
    int first_row = 0;
    tbb::flow::continue_node<tbb::flow::continue_msg> new_wait (m_graph_backward, Wait_Node());
    m_backward_waits.push_back(new_wait);
    std::vector<tbb::flow::continue_node<tbb::flow::continue_msg>> new_couch;
    for(int j = 0; j < nb_threads; ++j)
    {
        int nb_rows;
        int nb_cols = m_array_matrix[i].transpose().cols();
        //Repartition des lignes entre les noeuds
        if(j < (m_array_matrix[i].transpose().rows() % nb_threads))
        {
          nb_rows = m_array_matrix[i].transpose().rows() / nb_threads + 1;
        }
        else
        {
          nb_rows = m_array_matrix[i].transpose().rows() / nb_threads;
        }
        int first_col = 0;
        tbb::flow::continue_node<tbb::flow::continue_msg> new_node (
                                  m_graph_backward,
                                  neuralnetwork::Backward_Node(
                                         &(m_array_matrix[i]),
                                         &(m_array_err[i]),
                                         &(m_array_agreg[i - 1]),
                                         &(m_array_err[i - 1]),
                                         m_array_df[i - 1],
                                         first_row,
                                         first_col,
                                         nb_rows,
                                         nb_cols
                                  )
                                  );
        new_couch.push_back(new_node);
        first_row += nb_rows;
    }
    m_backward_neurons.push_back(new_couch);
  }

  //On lie les couches aux noeuds barrieres
  for(unsigned int i = 0; i < m_backward_neurons.size(); ++i)
  {
    for(unsigned int j = 0; j < m_backward_neurons[i].size(); ++j)
    {
      tbb::flow::make_edge(m_backward_neurons[i][j], m_backward_waits[i]);
      if(i != m_backward_neurons.size() - 1)
      {
        tbb::flow::make_edge(m_backward_waits[i + 1], m_backward_neurons[i][j]);
      }
    }
  }

  //Update
  for(unsigned int i = 0; i < DEPTH; ++i)
  {
    int first_row = 0;
    std::vector<tbb::flow::continue_node<tbb::flow::continue_msg>> new_couch;
    for(int j = 0; j < nb_threads; ++j)
      {
          int nb_rows;
          int nb_cols = m_array_matrix[i].cols();
          //Repartition des lignes entre les noeuds
          if(j < (m_array_matrix[i].rows() % nb_threads))
          {
            nb_rows = couch_size(i) / nb_threads + 1;
          }
          else
          {
            nb_rows = couch_size(i) / nb_threads;
          }
          int first_col = 0;
          //Pour n'importe quel couche hormis la premiere, l'entree est l'activation de la i-1eme couche, sinon c'est le vecteur d'entree
          if(i > 0)
          {
            tbb::flow::continue_node<tbb::flow::continue_msg> new_node (
                                  m_graph_backward,
                                  neuralnetwork::Update_Node(
                                         &(m_array_matrix[i]),
                                         step,
                                         &(m_array_err[i]),
                                         &(m_array_act[i - 1]),
                                         first_row,
                                         first_col,
                                         nb_rows,
                                         nb_cols
                                  )
                                  );
            new_couch.push_back(new_node);
          }
          else
          {
            tbb::flow::continue_node<tbb::flow::continue_msg> new_node (
                                  m_graph_backward,
                                  neuralnetwork::Update_Node(
                                         &(m_array_matrix[i]),
                                         step,
                                         &(m_array_err[i]),
                                         &(m_first_graph_vector),
                                         first_row,
                                         first_col,
                                         nb_rows,
                                         nb_cols
                                  )
                                  );
            new_couch.push_back(new_node);
          }
          first_row += nb_rows;
      }
      m_update_neurons.push_back(new_couch);
    }

    for(unsigned int i = 1; i < m_update_neurons.size(); ++i)
    {
      for(unsigned int j = 0; j < m_update_neurons[i].size(); ++j)
      {
        tbb::flow::make_edge(m_backward_waits[i - 1], m_update_neurons[i][j]);
      }
    }
    for(unsigned int j = 0; j < m_update_neurons[0].size(); ++j)
    {
        tbb::flow::make_edge(m_backward_waits[0], m_update_neurons[0][j]);
    }

}

template<std::size_t DEPTH>
double& neuralnetwork::Perceptron<DEPTH>::weight(std::size_t couch, std::size_t neuron, std::size_t previous)
{
  return m_array_matrix[couch](neuron, previous);
}

template<std::size_t DEPTH>
std::size_t neuralnetwork::Perceptron<DEPTH>::weights_count(std::size_t couch)
{
  return m_array_matrix[couch].cols();
}

template<std::size_t DEPTH>
std::size_t neuralnetwork::Perceptron<DEPTH>::depth()
{
  return m_size;
}

template<std::size_t DEPTH>
std::size_t neuralnetwork::Perceptron<DEPTH>::couch_size(std::size_t couch)
{
  return m_array_matrix[couch].rows();
}

template<std::size_t DEPTH>
void neuralnetwork::Perceptron<DEPTH>::printMatrixCouch(std::ostream& out, std::size_t i)
{
  std::cout << m_array_matrix[i];
}

template<std::size_t DEPTH>
Eigen::Matrix<double, Eigen::Dynamic, 1> neuralnetwork::Perceptron<DEPTH>::agreg(std::size_t couch)
{
  return m_array_agreg[couch];
}

template<std::size_t DEPTH>
Eigen::Matrix<double, Eigen::Dynamic, 1> neuralnetwork::Perceptron<DEPTH>::activation(std::size_t couch)
{
  return m_array_act[couch];
}

template<std::size_t DEPTH>
template<typename t, int INPUT_SIZE>
void neuralnetwork::Perceptron<DEPTH>::applyActivation(neuralnetwork::InOut<t, INPUT_SIZE>& in, std::size_t fi)
{
  for(int i = 0; i < in.size(); ++i)
  {
    in(i) = m_array_f[fi](in(i));
  }
}

template<std::size_t DEPTH>
template<typename tin, int INPUT_SIZE, typename tout, int OUTPUT_SIZE>
void neuralnetwork::Perceptron<DEPTH>::feedForward(neuralnetwork::InOut<tin, INPUT_SIZE>& in, neuralnetwork::InOut<tout, OUTPUT_SIZE>& out, bool saveAgreg, bool saveActivation)
{
  InOut<tin, neuralnetwork::InOutType::DYNAMIC> tmp;

  //feed forward pour le premiere couche
  tmp = m_array_matrix[0]*in;

  if(saveAgreg)
  {
    m_array_agreg[0] = tmp;
  }
  applyActivation(tmp, 0);
  if(saveActivation)
  {
    m_array_act[0] = tmp;
  }

  //feed forward pour les autres couches
  for(unsigned int i = 1; i < DEPTH; ++i)
  {
    tmp = m_array_matrix[i] * tmp;
    if(saveAgreg)
    {
      m_array_agreg[i] = tmp;
    }
    applyActivation(tmp, i);
    if(saveActivation)
    {
      m_array_act[i] = tmp;
    }
  }

  out = tmp;
}

template<std::size_t DEPTH>
template<typename tin, int INPUT_SIZE, typename tout, int OUTPUT_SIZE>
void neuralnetwork::Perceptron<DEPTH>::backpropagation(neuralnetwork::InOut<tin, INPUT_SIZE>* ins, neuralnetwork::InOut<tout, OUTPUT_SIZE>* outs, std::size_t n, double step)
{
  neuralnetwork::InOut<tout, Eigen::Dynamic> out;
  neuralnetwork::InOut<tout, Eigen::Dynamic> err;

  //Pour chaque exemple d'entrainement
  for(unsigned int i = 0; i < n; ++i)
  {
    //On evalue un exemple avec le reseau
    feedForward(ins[i], out, true, true);

    //On calcul l'erreur en sortie

    //difference entre la sortie desiree et la sortie attendue
    err = outs[i] - out;
    //df(agreg)*(sortie_desiree - sortie_attendue)
    for(unsigned int j = 0; j < err.size(); ++j)
    {
      err(j) = m_array_df[DEPTH - 1](m_array_agreg[DEPTH - 1](j)) * err(j);
    }
    m_array_err[DEPTH - 1] = err;

    //Erreur pour les autres couches
    //erreur(j - 1) = df(agreg) * matriceJ.transpose * erreur(j)
    for(unsigned int j = DEPTH - 1; j > 0; --j)
    {
      err = m_array_matrix[j].transpose() * m_array_err[j];
      for(int k = 0; k < err.size(); ++k)
      {
        err(k) = m_array_df[j - 1](m_array_agreg[j - 1](k)) * err(k);
      }
      m_array_err[j - 1] = err;
    }

    //Mise a jour des poids
    for(unsigned int j = 0; j < DEPTH; ++j)
    {
      for(unsigned int k = 0; k < couch_size(j); ++k)
      {
        for(unsigned int l = 0; l < weights_count(j); ++l)
        {
          if(j > 0)
          {
            weight(j, k, l) += step * m_array_err[j](k) * m_array_act[j - 1](l);
          }
          //Pour la premiere couche on prend le vecteur d'entree
          else
          {
            weight(j, k, l) += step * m_array_err[j](k) * ins[i](l);
          }
        }
      }
    }

  }
}

template<std::size_t DEPTH>
template<typename tin, int INPUT_SIZE, typename tout, int OUTPUT_SIZE>
void neuralnetwork::Perceptron<DEPTH>::parallel_feedForward(neuralnetwork::InOut<tin, INPUT_SIZE>& in, neuralnetwork::InOut<tout, OUTPUT_SIZE>& out)
{
  //On initialise le vecteur d'entree
  m_first_graph_vector.resize(in.size());
  for(int i = 0; i < in.size(); ++i)
  {
    m_first_graph_vector[i] = in[i];
  }

  //On lance le feed forward
  for(unsigned int i = 0; i < m_forward_neurons[0].size(); ++i)
  {
    m_forward_neurons[0][i].try_put(tbb::flow::continue_msg());
  }

  m_graph_forward.wait_for_all();
  out = m_array_act[DEPTH - 1];
}

template<std::size_t DEPTH>
template<typename tin, int INPUT_SIZE, typename tout, int OUTPUT_SIZE>
void neuralnetwork::Perceptron<DEPTH>::parallel_backpropagation(neuralnetwork::InOut<tin, INPUT_SIZE>* ins, neuralnetwork::InOut<tout, OUTPUT_SIZE>* outs, std::size_t n, double step)
{
  neuralnetwork::InOut<tout, Eigen::Dynamic> out;
  neuralnetwork::InOut<tout, Eigen::Dynamic> err;

  //Pour chaque exemple d'entrainement
  for(unsigned int i = 0; i < n; ++i)
  {
    //On evalue un exemple avec le reseau
    parallel_feedForward(ins[i], out);
    //feedForward(ins[i], out, true, true);

    //On calcul l'erreur en sortie
    //difference entre la sortie desiree et la sortie attendue
    err = outs[i] - out;
    for(unsigned int j = 0; j < err.size(); ++j)
    {
      err(j) = m_array_df[DEPTH - 1](m_array_agreg[DEPTH - 1](j)) * err(j);
    }
    m_array_err[DEPTH - 1] = err;

    //On lance la retropropagation
    for(unsigned int j = 0; j < m_backward_neurons.back().size(); ++j)
    {
      m_backward_neurons.back()[j].try_put(tbb::flow::continue_msg());
    }
    m_graph_backward.wait_for_all();

  }
}

#endif


#pragma once

#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace icarus::types
{
    using Scalar = float;
    using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
    using Quaternion = Eigen::Quaternion<Scalar>;
}


//  (C) Copyright 2006 Eric Niebler, Olivier Gygi.
//  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)

// Test case for weighted_tail_mean.hpp

#define BOOST_NUMERIC_FUNCTIONAL_STD_COMPLEX_SUPPORT
#define BOOST_NUMERIC_FUNCTIONAL_STD_VALARRAY_SUPPORT
#define BOOST_NUMERIC_FUNCTIONAL_STD_VECTOR_SUPPORT

#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/accumulators/statistics/weighted_tail_mean.hpp>
#include <boost/accumulators/statistics/weighted_tail_quantile.hpp>

using namespace boost;
using namespace unit_test;
using namespace boost::accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // tolerance in %
    double epsilon = 1;

    std::size_t n = 100000; // number of MC steps
    std::size_t c = 25000; // cache size

    accumulator_set<double, stats<tag::non_coherent_weighted_tail_mean<right> >, double >
        acc0( right_tail_cache_size = c );
    accumulator_set<double, stats<tag::non_coherent_weighted_tail_mean<left> >, double >
        acc1( left_tail_cache_size = c );

    // random number generators
    boost::lagged_fibonacci607 rng;

    for (std::size_t i = 0; i < n; ++i)
    {
        double smpl = std::sqrt(rng());
        acc0(smpl, weight = 1./smpl);
    }

    for (std::size_t i = 0; i < n; ++i)
    {
        double smpl = rng();
        acc1(smpl*smpl, weight = smpl);
    }

    // check uniform distribution
    BOOST_CHECK_CLOSE( non_coherent_weighted_tail_mean(acc0, quantile_probability = 0.95), 0.975, epsilon );
    BOOST_CHECK_CLOSE( non_coherent_weighted_tail_mean(acc0, quantile_probability = 0.975), 0.9875, epsilon );
    BOOST_CHECK_CLOSE( non_coherent_weighted_tail_mean(acc0, quantile_probability = 0.99), 0.995, epsilon );
    BOOST_CHECK_CLOSE( non_coherent_weighted_tail_mean(acc0, quantile_probability = 0.999), 0.9995, epsilon );
    BOOST_CHECK_CLOSE( non_coherent_weighted_tail_mean(acc1, quantile_probability = 0.05), 0.025, epsilon );
    BOOST_CHECK_CLOSE( non_coherent_weighted_tail_mean(acc1, quantile_probability = 0.025), 0.0125, epsilon );
    BOOST_CHECK_CLOSE( non_coherent_weighted_tail_mean(acc1, quantile_probability = 0.01), 0.005, epsilon );
    BOOST_CHECK_CLOSE( non_coherent_weighted_tail_mean(acc1, quantile_probability = 0.001), 0.0005, 5*epsilon );
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("weighted_tail_mean test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


//  (C) Copyright Raffi Enficiaud 2014.
//  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)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE dataset_example68
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>
#include <sstream>

namespace bdata = boost::unit_test::data;

// Dataset generating a Fibonacci sequence
class fibonacci_dataset {
public:
    // Samples type is int
    enum { arity = 1 };

    struct iterator {

        iterator() : a(1), b(1) {}

        int operator*() const   { return b; }
        void operator++()
        {
            a = a + b;
            std::swap(a, b);
        }
    private:
        int a;
        int b; // b is the output
    };

    fibonacci_dataset()             {}

    // size is infinite
    bdata::size_t   size() const    { return bdata::BOOST_TEST_DS_INFINITE_SIZE; }

    // iterator
    iterator        begin() const   { return iterator(); }
};

namespace boost { namespace unit_test { namespace data { namespace monomorphic {
  // registering fibonacci_dataset as a proper dataset
  template <>
  struct is_dataset<fibonacci_dataset> : boost::mpl::true_ {};
}}}}

// Creating a test-driven dataset
BOOST_DATA_TEST_CASE(
    test1,
    fibonacci_dataset() ^ bdata::make( { 1, 2, 3, 5, 8, 13, 21, 35, 56 } ),
    fib_sample, exp)
{
      BOOST_TEST(fib_sample == exp);
}
//]


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_XFORM_HPP
#define MCL_XFORM_HPP 1

#include <Eigen/Geometry>

namespace mcl
{

template <typename T>
class XForm
{
protected:
	Eigen::Matrix<T,4,4> data;

public:
	template <typename U> using Vec3 = Eigen::Matrix<U,3,1>;

	XForm()
	{
		setIdentity();
	}

	template <typename U>
	XForm(const XForm<U> &xf)
	{
		data = xf.data.template cast<T>();
	}

	const Eigen::Matrix<T,4,4>& matrix() const
	{
		return data;
	}

	Eigen::Matrix<T,4,4>& matrix()
	{
		return data;
	}

	T operator()(int i, int j) const
	{
		return data(i,j);
	}

	T& operator()(int i, int j)
	{
		return data(i,j);
	}

	Vec3<T> operator*(const Vec3<T> &v)
	{
		return (Eigen::Transform<T,3,Eigen::Affine>(data) * v);
	}

	Eigen::Matrix<T,2,1> operator*(const Eigen::Matrix<T,2,1> &v)
	{
		Vec3<T> v3(v[0], v[1], 0);
		v3 = Eigen::Transform<T,3,Eigen::Affine>(data) * v3;
		return Eigen::Matrix<T,2,1>(v3[0], v3[1]);
	}

	void setZero()
	{
		data.setZero();
	}

	void setIdentity()
	{
		data.setZero();
		data.diagonal().array() = 1;
	}

	// Makes an identity matrix
	static inline XForm<T> identity()
	{
		XForm<T> r;
		r.setIdentity();
		return r;
	}

	// Makes a scale matrix
	// Usage: XForm<float> s = xform::make_scale(1.f, 2.f, 3.f);
	static inline XForm<T> make_scale(T x, T y, T z)
	{
		XForm<T> r;
		r.setZero();
		r(0,0) = x;
		r(1,1) = y;
		r(2,2) = z;
		r(3,3) = 1;
		return r;
	}

	static inline XForm<T> make_scale(T x)
	{
		return make_scale(x,x,x);
	}

	// Makes a translation matrix
	// Usage: XForm<float> t = xform::make_trans(1.f, 2.f, 3.f);
	static inline XForm<T> make_trans(T x, T y, T z)
	{
		XForm<T> r;
		r.setIdentity();
		r(0,3) = x;
		r(1,3) = y;
		r(2,3) = z;
		r(3,3) = z;
		return r;
	}

	static inline XForm<T> make_trans(const Vec3<T> &t)
	{
		return make_trans(t[0],t[1],t[2]);
	}

	// Makes a rotation matrix
	// Usage: Xform<float> t = xform::make_rot(45.f, Vec3f(0,1,0));
	static inline XForm<T> make_rotate(T angle_deg, const Vec3<T> &axis)
	{
		T rad_deg = angle_deg * M_PI / 180.0;
		Eigen::AngleAxis<T> rx(axis[0]*rad_deg, Eigen::Vector3d::UnitX());
		Eigen::AngleAxis<T> ry(axis[1]*rad_deg, Eigen::Vector3d::UnitY());
		Eigen::AngleAxis<T> rz(axis[2]*rad_deg, Eigen::Vector3d::UnitZ());
		Eigen::Quaternion<T> q = rx * ry * rz;
		XForm<T> r;
		r.data.template block<3,3>(0,0) = q.matrix();
		return r;
	}

	static inline XForm<T> make_rotate_to(Vec3<T> d)
	{
		d.normalize();
		const Vec3<T> up(0,1,0);
		T denom = (d+up).dot(d+up);
		XForm<T> xf;
		xf.setIdentity();
		if(denom==0 && std::abs(d[1]-1) <= 1e-12){ return xf; }
		else if (denom==0 && std::abs(d[1]+1) <= 1e-12) {
			return make_rotate(180,Vec3<T>(1,0,0));
		}

		// Householder refl
		Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
		Eigen::Matrix3d R = 2.0 * (d+up) * (d+up).transpose();
		R *= (1.0/denom);
		R -= I;
		for( int i=0; i<3; ++i ){
			for( int j=0; j<3; ++j ){
				xf(i,j) = R(i,j);
			}
		}

		return xf;
	}

	// Makes a view matrix
	// Usage: XForm<float> v = XForm::make_view(eye, viewdir, Vec3f(0,1,0));
	static inline XForm<T> make_view(const Vec3<T> &eye, const Vec3<T> &dir, const Vec3<T> &up)
	{
		Vec3<T> w = dir*-1.f; w.normalize();
		Vec3<T> u = up.cross(w);
		Vec3<T> v = w.cross(u);
		XForm<T> r;
		r.setIdentity();
		for (int i=0; i<3; ++i)
		{
			r.data()[4*i] = u[i];
			r.data()[4*i+1] = v[i];
			r.data()[4*i+2] = w[i];
		}
		r.data()[12] = -eye.dot(u);
		r.data()[13] = -eye.dot(v);
		r.data()[14] = -eye.dot(w);
		return r;
	}

	// Makes a view matrix (from a lookat point)
	// Usage: XForm<float> v = XForm::make_lookat(eye, Vec3f(0,0,0), Vec3f(0,1,0));
	static inline XForm<T> make_lookat(const Vec3<T> &eye, const Vec3<T> &point, const Vec3<T> &up)
	{
		Vec3<T> dir = point-eye;
		return make_view(eye,dir,up);
	}

	// Makes a perspective matrix
	// Usage: XForm<float> p = XForm::make_persp(45, width/height, 1e-3f, 1e6f);
	static inline XForm<T> make_persp(T fov_deg, T aspect, T near, T far)
	{
		T fov = fov_deg * M_PI / 180.f;
		T cossinf = std::cos(fov/2.f) / std::sin(fov/2.f);
		XForm<T> r;
		r.setIdentity();
		r(0,0) = cossinf/aspect;
		r.data.data()[5] = cossinf;
		r.data.data()[10] = -(near+far)/(far-near);
		r.data.data()[14] = -(2.f*near*far)/(far-near);
		r.data.data()[11] = -1.f;
		r.data.data()[15] = 0.f;
		return r;
	}

	static inline std::string to_string(const XForm<T> &xf)
	{
		std::stringstream ss;
		ss << xf.data;
		return ss.str();
	}

	static inline XForm<T> from_string(const std::string &s)
	{
		std::stringstream ss; ss << s;
		XForm<T> result;
		ss >> result.data;
		return result;
	}

	// Multiply every row (vertex) by the xform
	// Works for 2D or 3D (cols)
	template <typename DerivedV>
	void apply(Eigen::MatrixBase<DerivedV> &V)
	{
		int nv = V.rows();
		int nc = V.cols();
		Eigen::Transform<T,3,Eigen::Affine> r(data);
		for (int i=0; i<nv; ++i)
		{
			Vec3<T> vi = Vec3<T>::Zero();
			for (int j=0; j<nc; ++j) { vi[j] = V(i,j); }
			vi = r * vi;
			for (int j=0; j<nc; ++j) { V(i,j) = vi[j]; }
		}
	}
};

template <class T>
static inline XForm<T> operator*(const XForm<T> &xf1, const XForm<T> &xf2)
{
	XForm<T> xf;
	xf.matrix() = xf1.matrix() * xf2.matrix();
	return xf;
}

} // ns mcl

#endif


#include "../data/data.h"

#include <dlib/dnn.h>
#include <dlib/matrix.h>

#include <iostream>
#include <random>

int main() {
  using namespace dlib;

  size_t n = 10000;
  size_t seed = 45345;
  auto data = GenerateData(-1.5, 1.5, n, seed, false);

  std::vector<matrix<double>> x(n);
  std::vector<matrix<double>> y_data(n);

  for (size_t i = 0; i < n; ++i) {
    x[i].set_size(1, 1);
    x[i](0, 0) = data.first[i];

    y_data[i].set_size(1, 1);
    y_data[i](0, 0) = data.second[i];
  }

  // normalize data
  vector_normalizer<matrix<double>> normalizer_x;
  vector_normalizer<matrix<double>> normalizer_y;

  // let the normalizer learn the mean and standard deviation of the samples
  normalizer_x.train(x);
  normalizer_y.train(y_data);

  std::vector<float> y(n);

  // now normalize each sample
  for (size_t i = 0; i < x.size(); ++i) {
    x[i] = normalizer_x(x[i]);
    y_data[i] = normalizer_y(y_data[i]);
    y[i] = static_cast<float>(y_data[i](0, 0));
  }

  using NetworkType = loss_mean_squared<
      fc<1, htan<fc<8, htan<fc<16, htan<fc<32, input<matrix<double>>>>>>>>>>;
  NetworkType network;
  float weight_decay = 0.0001f;
  float momentum = 0.5f;
  sgd solver(weight_decay, momentum);
  dnn_trainer<NetworkType> trainer(network, solver);
  trainer.set_learning_rate(0.01);
  trainer.set_learning_rate_shrink_factor(1);  // disable learning rate changes
  trainer.set_mini_batch_size(64);
  trainer.set_max_num_epochs(500);
  trainer.be_verbose();
  trainer.train(x, y);
  network.clean();

  // auto predictions = network(new_x);

  return 0;
}


//
// Created by Michele Fornino on 5/5/18.

// This is the base code for the MarkovChains project. It provides a few examples of instantiations and usage of the
// classes MarkovChain.h and MarkovChainSimulations.h
//

//#include "DenseDTMC.h"
//#include "DenseCTMC.h"
//#include "SparseCTMC.h"
//#include "SimCTMC.h"

#include "MarkovChain.h"
#include "MarkovChainSimulations.h"
#include <iostream>
//#include <vector>
//#include <Eigen/Dense>
//#include <Eigen/Sparse>
//#include <Eigen/SparseLU>

using namespace Eigen;
using namespace std;
using namespace MarkovChain;
using namespace MarkovChainSimulations;

int main() {

//    typedef SparseMatrix<double, RowMajor> SMr;
//    typedef Triplet<double> T;
    //// Dense Discrete Time Markov Chain
/*
    // Populate Transition Matrix from Rouwenhorst convenience function
    DenseDTMC chain = DenseDTMC::discretizeAR1_Rouwenhorst();

    // Populate DenseDTMC object and compute sample path
    DenseDTMC::ManySimDTMC simulations;
    simulations = chain.simDenseDTMC();
*/
    DenseCTMC chain1; chain1.discretizeAR1_Rouwenhorst();
//    DenseDTMC chain2; chain2.discretizeAR1_Rouwenhorst();
    SimCTMC simulation1 = chain1.simulateMarkovChain(2);
//    SimDTMC simulation2 = chain2.simulateMarkovChain(2);

//    cout << chain1;
//    cout << chain2;
//    cout << simulation1;
//    cout << simulation2;

//#pragma omp declare reduction (merge : std::vector<SimCTMC> : omp_out.insert(omp_out.end(), omp_in.begin(), omp_in.end()))
    MultSimCTMC prova;
//#pragma omp parallel for reduction(merge: prova)
    for (int k = 0; k < 80; k += 1){
        prova.emplace_back(chain1.simulateMarkovChain(2, 10000));
    }

    cout << "DONE SIMULATING!!\n";

    cout << prova[79];

//    MultSimDTMC prova; prova.emplace_back(simulation2); prova.emplace_back(simulation2);
//    saveToFile(prova);

//    simulation2.saveToFile();
//    MarkovChain<ContinuousTime, MatrixXd> chain = MarkovChain<Continuous, MatrixXd>::discretizeAR1_Rouwenhorst();

    //// Sparse Continuous Time Markov Chain
    // Populate Infinitesimal Generator
//    SMr Q(4,4);
//    vector<T> tripletQ; tripletQ.reserve(10);
//    tripletQ.emplace_back(T(0,0,-0.25));
//    tripletQ.emplace_back(T(0,1, 0.25));
//    tripletQ.emplace_back(T(1,0, 0.15));
//    tripletQ.emplace_back(T(1,1,-0.50));
//    tripletQ.emplace_back(T(1,2, 0.35));
//    tripletQ.emplace_back(T(3,3,-0.25));
//    tripletQ.emplace_back(T(3,2, 0.25));
//    tripletQ.emplace_back(T(2,1, 0.25));
//    tripletQ.emplace_back(T(2,2,-0.50));
//    tripletQ.emplace_back(T(2,3, 0.25));
//    Q.setFromTriplets(tripletQ.begin(), tripletQ.end());
//
//    // Populate State Vector (Eigen vector of double)
//    VectorXd S(4); S(0) = -0.5; S(1) = 0; S(2) = 0.5; S(3) = 1;

    // Populate SparseCTMC object and compute sample path
//    SparseCTMC chain = SparseCTMC(Q, S);
//    SparseCTMC::ManySimCTMC simulations = chain.simSparseCTMC();

    //// Dense Continuous Time Markov Chain
/*
    // Populate Infinitesimal Generator
    MatrixXd Q(4,4);
    Q(0,0) = -0.03; Q(0,1) =  0.02; Q(0,2) =  0.01; Q(0,3) =  0.00;
    Q(1,0) =  0.10; Q(1,1) = -0.55; Q(1,2) =  0.30; Q(1,3) =  0.15;
    Q(2,0) =  0.10; Q(2,1) =  0.25; Q(2,2) = -0.60; Q(2,3) =  0.25;
    Q(3,0) =  0.10; Q(3,1) =  0.15; Q(3,2) =  0.30; Q(3,3) = -0.55;

    // Populate State Vector (Eigen vector of double)
    VectorXd S(4); S(0) = -0.5; S(1) = 0; S(2) = 0.5; S(3) = 1;

    // Populate DenseCTMC object and compute sample path
    DenseCTMC chain = DenseCTMC(Q, S);
    DenseCTMC::ManySimCTMC simulations = chain.simDenseCTMC();
*/

    //// Display Results
//    cout << "Stationary Distribution:\n" << chain.getStationaryDistribution() << "\n";
//    cout << "Simulation:\n\n" << simulations[0];

    return 0;
}


/*
    MIT License

    Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com)

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#ifndef TRAJECTORY_HPP
#define TRAJECTORY_HPP

#include "gcopter/root_finder.hpp"

#include <Eigen/Eigen>

#include <iostream>
#include <cmath>
#include <cfloat>
#include <vector>

template <int D>
class Piece
{
public:
    typedef Eigen::Matrix<double, 3, D + 1> CoefficientMat;
    typedef Eigen::Matrix<double, 3, D> VelCoefficientMat;
    typedef Eigen::Matrix<double, 3, D - 1> AccCoefficientMat;

private:
    double duration;
    CoefficientMat coeffMat;

public:
    Piece() = default;

    Piece(double dur, const CoefficientMat &cMat)
        : duration(dur), coeffMat(cMat) {}

    inline int getDim() const
    {
        return 3;
    }

    inline int getDegree() const
    {
        return D;
    }

    inline double getDuration() const
    {
        return duration;
    }

    inline const CoefficientMat &getCoeffMat() const
    {
        return coeffMat;
    }

    inline Eigen::Vector3d getPos(const double &t) const
    {
        Eigen::Vector3d pos(0.0, 0.0, 0.0);
        double tn = 1.0;
        for (int i = D; i >= 0; i--)
        {
            pos += tn * coeffMat.col(i);
            tn *= t;
        }
        return pos;
    }

    inline Eigen::Vector3d getVel(const double &t) const
    {
        Eigen::Vector3d vel(0.0, 0.0, 0.0);
        double tn = 1.0;
        int n = 1;
        for (int i = D - 1; i >= 0; i--)
        {
            vel += n * tn * coeffMat.col(i);
            tn *= t;
            n++;
        }
        return vel;
    }

    inline Eigen::Vector3d getAcc(const double &t) const
    {
        Eigen::Vector3d acc(0.0, 0.0, 0.0);
        double tn = 1.0;
        int m = 1;
        int n = 2;
        for (int i = D - 2; i >= 0; i--)
        {
            acc += m * n * tn * coeffMat.col(i);
            tn *= t;
            m++;
            n++;
        }
        return acc;
    }

    inline Eigen::Vector3d getJer(const double &t) const
    {
        Eigen::Vector3d jer(0.0, 0.0, 0.0);
        double tn = 1.0;
        int l = 1;
        int m = 2;
        int n = 3;
        for (int i = D - 3; i >= 0; i--)
        {
            jer += l * m * n * tn * coeffMat.col(i);
            tn *= t;
            l++;
            m++;
            n++;
        }
        return jer;
    }

    inline CoefficientMat normalizePosCoeffMat() const
    {
        CoefficientMat nPosCoeffsMat;
        double t = 1.0;
        for (int i = D; i >= 0; i--)
        {
            nPosCoeffsMat.col(i) = coeffMat.col(i) * t;
            t *= duration;
        }
        return nPosCoeffsMat;
    }

    inline VelCoefficientMat normalizeVelCoeffMat() const
    {
        VelCoefficientMat nVelCoeffMat;
        int n = 1;
        double t = duration;
        for (int i = D - 1; i >= 0; i--)
        {
            nVelCoeffMat.col(i) = n * coeffMat.col(i) * t;
            t *= duration;
            n++;
        }
        return nVelCoeffMat;
    }

    inline AccCoefficientMat normalizeAccCoeffMat() const
    {
        AccCoefficientMat nAccCoeffMat;
        int n = 2;
        int m = 1;
        double t = duration * duration;
        for (int i = D - 2; i >= 0; i--)
        {
            nAccCoeffMat.col(i) = n * m * coeffMat.col(i) * t;
            n++;
            m++;
            t *= duration;
        }
        return nAccCoeffMat;
    }

    inline double getMaxVelRate() const
    {
        VelCoefficientMat nVelCoeffMat = normalizeVelCoeffMat();
        Eigen::VectorXd coeff = RootFinder::polySqr(nVelCoeffMat.row(0)) +
                                RootFinder::polySqr(nVelCoeffMat.row(1)) +
                                RootFinder::polySqr(nVelCoeffMat.row(2));
        int N = coeff.size();
        int n = N - 1;
        for (int i = 0; i < N; i++)
        {
            coeff(i) *= n;
            n--;
        }
        if (coeff.head(N - 1).squaredNorm() < DBL_EPSILON)
        {
            return getVel(0.0).norm();
        }
        else
        {
            double l = -0.0625;
            double r = 1.0625;
            while (fabs(RootFinder::polyVal(coeff.head(N - 1), l)) < DBL_EPSILON)
            {
                l = 0.5 * l;
            }
            while (fabs(RootFinder::polyVal(coeff.head(N - 1), r)) < DBL_EPSILON)
            {
                r = 0.5 * (r + 1.0);
            }
            std::set<double> candidates = RootFinder::solvePolynomial(coeff.head(N - 1), l, r,
                                                                      FLT_EPSILON / duration);
            candidates.insert(0.0);
            candidates.insert(1.0);
            double maxVelRateSqr = -INFINITY;
            double tempNormSqr;
            for (std::set<double>::const_iterator it = candidates.begin();
                 it != candidates.end();
                 it++)
            {
                if (0.0 <= *it && 1.0 >= *it)
                {
                    tempNormSqr = getVel((*it) * duration).squaredNorm();
                    maxVelRateSqr = maxVelRateSqr < tempNormSqr ? tempNormSqr : maxVelRateSqr;
                }
            }
            return sqrt(maxVelRateSqr);
        }
    }

    inline double getMaxAccRate() const
    {
        AccCoefficientMat nAccCoeffMat = normalizeAccCoeffMat();
        Eigen::VectorXd coeff = RootFinder::polySqr(nAccCoeffMat.row(0)) +
                                RootFinder::polySqr(nAccCoeffMat.row(1)) +
                                RootFinder::polySqr(nAccCoeffMat.row(2));
        int N = coeff.size();
        int n = N - 1;
        for (int i = 0; i < N; i++)
        {
            coeff(i) *= n;
            n--;
        }
        if (coeff.head(N - 1).squaredNorm() < DBL_EPSILON)
        {
            return getAcc(0.0).norm();
        }
        else
        {
            double l = -0.0625;
            double r = 1.0625;
            while (fabs(RootFinder::polyVal(coeff.head(N - 1), l)) < DBL_EPSILON)
            {
                l = 0.5 * l;
            }
            while (fabs(RootFinder::polyVal(coeff.head(N - 1), r)) < DBL_EPSILON)
            {
                r = 0.5 * (r + 1.0);
            }
            std::set<double> candidates = RootFinder::solvePolynomial(coeff.head(N - 1), l, r,
                                                                      FLT_EPSILON / duration);
            candidates.insert(0.0);
            candidates.insert(1.0);
            double maxAccRateSqr = -INFINITY;
            double tempNormSqr;
            for (std::set<double>::const_iterator it = candidates.begin();
                 it != candidates.end();
                 it++)
            {
                if (0.0 <= *it && 1.0 >= *it)
                {
                    tempNormSqr = getAcc((*it) * duration).squaredNorm();
                    maxAccRateSqr = maxAccRateSqr < tempNormSqr ? tempNormSqr : maxAccRateSqr;
                }
            }
            return sqrt(maxAccRateSqr);
        }
    }

    inline bool checkMaxVelRate(const double &maxVelRate) const
    {
        double sqrMaxVelRate = maxVelRate * maxVelRate;
        if (getVel(0.0).squaredNorm() >= sqrMaxVelRate ||
            getVel(duration).squaredNorm() >= sqrMaxVelRate)
        {
            return false;
        }
        else
        {
            VelCoefficientMat nVelCoeffMat = normalizeVelCoeffMat();
            Eigen::VectorXd coeff = RootFinder::polySqr(nVelCoeffMat.row(0)) +
                                    RootFinder::polySqr(nVelCoeffMat.row(1)) +
                                    RootFinder::polySqr(nVelCoeffMat.row(2));
            double t2 = duration * duration;
            coeff.tail<1>()(0) -= sqrMaxVelRate * t2;
            return RootFinder::countRoots(coeff, 0.0, 1.0) == 0;
        }
    }

    inline bool checkMaxAccRate(const double &maxAccRate) const
    {
        double sqrMaxAccRate = maxAccRate * maxAccRate;
        if (getAcc(0.0).squaredNorm() >= sqrMaxAccRate ||
            getAcc(duration).squaredNorm() >= sqrMaxAccRate)
        {
            return false;
        }
        else
        {
            AccCoefficientMat nAccCoeffMat = normalizeAccCoeffMat();
            Eigen::VectorXd coeff = RootFinder::polySqr(nAccCoeffMat.row(0)) +
                                    RootFinder::polySqr(nAccCoeffMat.row(1)) +
                                    RootFinder::polySqr(nAccCoeffMat.row(2));
            double t2 = duration * duration;
            double t4 = t2 * t2;
            coeff.tail<1>()(0) -= sqrMaxAccRate * t4;
            return RootFinder::countRoots(coeff, 0.0, 1.0) == 0;
        }
    }
};

template <int D>
class Trajectory
{
private:
    typedef std::vector<Piece<D>> Pieces;
    Pieces pieces;

public:
    Trajectory() = default;

    Trajectory(const std::vector<double> &durs,
               const std::vector<typename Piece<D>::CoefficientMat> &cMats)
    {
        int N = std::min(durs.size(), cMats.size());
        pieces.reserve(N);
        for (int i = 0; i < N; i++)
        {
            pieces.emplace_back(durs[i], cMats[i]);
        }
    }

    inline int getPieceNum() const
    {
        return pieces.size();
    }

    inline Eigen::VectorXd getDurations() const
    {
        int N = getPieceNum();
        Eigen::VectorXd durations(N);
        for (int i = 0; i < N; i++)
        {
            durations(i) = pieces[i].getDuration();
        }
        return durations;
    }

    inline double getTotalDuration() const
    {
        int N = getPieceNum();
        double totalDuration = 0.0;
        for (int i = 0; i < N; i++)
        {
            totalDuration += pieces[i].getDuration();
        }
        return totalDuration;
    }

    inline Eigen::Matrix3Xd getPositions() const
    {
        int N = getPieceNum();
        Eigen::Matrix3Xd positions(3, N + 1);
        for (int i = 0; i < N; i++)
        {
            positions.col(i) = pieces[i].getCoeffMat().col(D);
        }
        positions.col(N) = pieces[N - 1].getPos(pieces[N - 1].getDuration());
        return positions;
    }

    inline const Piece<D> &operator[](int i) const
    {
        return pieces[i];
    }

    inline Piece<D> &operator[](int i)
    {
        return pieces[i];
    }

    inline void clear(void)
    {
        pieces.clear();
        return;
    }

    inline typename Pieces::const_iterator begin() const
    {
        return pieces.begin();
    }

    inline typename Pieces::const_iterator end() const
    {
        return pieces.end();
    }

    inline typename Pieces::iterator begin()
    {
        return pieces.begin();
    }

    inline typename Pieces::iterator end()
    {
        return pieces.end();
    }

    inline void reserve(const int &n)
    {
        pieces.reserve(n);
        return;
    }

    inline void emplace_back(const Piece<D> &piece)
    {
        pieces.emplace_back(piece);
        return;
    }

    inline void emplace_back(const double &dur,
                             const typename Piece<D>::CoefficientMat &cMat)
    {
        pieces.emplace_back(dur, cMat);
        return;
    }

    inline void append(const Trajectory<D> &traj)
    {
        pieces.insert(pieces.end(), traj.begin(), traj.end());
        return;
    }

    inline int locatePieceIdx(double &t) const
    {
        int N = getPieceNum();
        int idx;
        double dur;
        for (idx = 0;
             idx < N &&
             t > (dur = pieces[idx].getDuration());
             idx++)
        {
            t -= dur;
        }
        if (idx == N)
        {
            idx--;
            t += pieces[idx].getDuration();
        }
        return idx;
    }

    inline Eigen::Vector3d getPos(double t) const
    {
        int pieceIdx = locatePieceIdx(t);
        return pieces[pieceIdx].getPos(t);
    }

    inline Eigen::Vector3d getVel(double t) const
    {
        int pieceIdx = locatePieceIdx(t);
        return pieces[pieceIdx].getVel(t);
    }

    inline Eigen::Vector3d getAcc(double t) const
    {
        int pieceIdx = locatePieceIdx(t);
        return pieces[pieceIdx].getAcc(t);
    }

    inline Eigen::Vector3d getJer(double t) const
    {
        int pieceIdx = locatePieceIdx(t);
        return pieces[pieceIdx].getJer(t);
    }

    inline Eigen::Vector3d getJuncPos(int juncIdx) const
    {
        if (juncIdx != getPieceNum())
        {
            return pieces[juncIdx].getCoeffMat().col(D);
        }
        else
        {
            return pieces[juncIdx - 1].getPos(pieces[juncIdx - 1].getDuration());
        }
    }

    inline Eigen::Vector3d getJuncVel(int juncIdx) const
    {
        if (juncIdx != getPieceNum())
        {
            return pieces[juncIdx].getCoeffMat().col(D - 1);
        }
        else
        {
            return pieces[juncIdx - 1].getVel(pieces[juncIdx - 1].getDuration());
        }
    }

    inline Eigen::Vector3d getJuncAcc(int juncIdx) const
    {
        if (juncIdx != getPieceNum())
        {
            return pieces[juncIdx].getCoeffMat().col(D - 2) * 2.0;
        }
        else
        {
            return pieces[juncIdx - 1].getAcc(pieces[juncIdx - 1].getDuration());
        }
    }

    inline double getMaxVelRate() const
    {
        int N = getPieceNum();
        double maxVelRate = -INFINITY;
        double tempNorm;
        for (int i = 0; i < N; i++)
        {
            tempNorm = pieces[i].getMaxVelRate();
            maxVelRate = maxVelRate < tempNorm ? tempNorm : maxVelRate;
        }
        return maxVelRate;
    }

    inline double getMaxAccRate() const
    {
        int N = getPieceNum();
        double maxAccRate = -INFINITY;
        double tempNorm;
        for (int i = 0; i < N; i++)
        {
            tempNorm = pieces[i].getMaxAccRate();
            maxAccRate = maxAccRate < tempNorm ? tempNorm : maxAccRate;
        }
        return maxAccRate;
    }

    inline bool checkMaxVelRate(const double &maxVelRate) const
    {
        int N = getPieceNum();
        bool feasible = true;
        for (int i = 0; i < N && feasible; i++)
        {
            feasible = feasible && pieces[i].checkMaxVelRate(maxVelRate);
        }
        return feasible;
    }

    inline bool checkMaxAccRate(const double &maxAccRate) const
    {
        int N = getPieceNum();
        bool feasible = true;
        for (int i = 0; i < N && feasible; i++)
        {
            feasible = feasible && pieces[i].checkMaxAccRate(maxAccRate);
        }
        return feasible;
    }
};

#endif


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  EigenSolver<MatrixXf> es;
MatrixXf A = MatrixXf::Random(4,4);
es.compute(A, /* computeEigenvectors = */ false);
cout << "The eigenvalues of A are: " << es.eigenvalues().transpose() << endl;
es.compute(A + MatrixXf::Identity(4,4), false); // re-use es to compute eigenvalues of A+I
cout << "The eigenvalues of A+I are: " << es.eigenvalues().transpose() << endl;

  return 0;
}


#include "LayoutMaker.hpp"

#include <Eigen/SparseCore>
#include <Spectra/SymEigsSolver.h>
#include <Spectra/MatOp/SparseSymMatProd.h>
#include <unordered_set>
#include <unordered_map>
#include <iostream>
#include <numeric>
#include <stack>
#include "UnionFind.hpp"

namespace LayoutMaker {

struct LayoutContext {

	uint32_t next_id;
	const std::shared_ptr<TriangleMesh> p_mesh;
	const uint32_t max_depth;
	const uint32_t max_cluster_size;
	const uint32_t max_spectral_size;
	std::vector<uint32_t> final_cluster;
	const uint32_t max_iterations_eigen;
	const float error_eigen;

	LayoutContext(
		const std::shared_ptr<TriangleMesh> p_mesh,
		uint32_t max_depth,
		uint32_t max_cluster_size,
		uint32_t max_spectral_size,
		uint32_t max_iterations_eigen,
		float error_eigen) :
		next_id(0), p_mesh(p_mesh), max_depth(max_depth),
		max_cluster_size(max_cluster_size),
		max_spectral_size(max_spectral_size),
		max_iterations_eigen(max_iterations_eigen),
		error_eigen(error_eigen)
	{
		final_cluster.resize(p_mesh->get_vertices().size(), 0);
	}
};

void vertex_laplacian_layout(
	LayoutContext& context,
	const uint32_t depth,
	const std::unordered_multimap<uint32_t, uint32_t>& vert2face,
	const std::unordered_set<uint32_t>& vertices_indices) {

	// Termination if conditions fulfilled
	if (vertices_indices.empty()) {
		return;
	}
	if (depth >= context.max_depth) {
		std::cout << "Broke on depth " << depth << " with vertices " << vertices_indices.size() << std::endl;
		assert(false);
	}
	if (depth >= context.max_depth || vertices_indices.size() <= context.max_cluster_size) {
		const uint32_t id = context.next_id++;
		for (uint32_t idx : vertices_indices) {
			context.final_cluster[idx] = id;
		}
		return;
	}

	std::unordered_map<uint32_t, uint32_t> old2new_vert;
	std::vector<uint32_t> new2old_vert(vertices_indices.size());
	old2new_vert.reserve(vertices_indices.size());
	{
		std::unordered_set<uint32_t>::const_iterator it = vertices_indices.begin();
		for (uint32_t i = 0; i < (uint32_t)vertices_indices.size(); ++i, ++it) {
			old2new_vert.insert({ *it, i });
			new2old_vert[i] = *it;
		}
	}

	// Compute second smallest eigenvector
	std::vector<uint32_t> vert_degree(vertices_indices.size(), 0);
	std::vector< Eigen::Triplet<float>> triplet_list;
	triplet_list.reserve(3 * vertices_indices.size());
	// Fill connectivity
	std::unordered_set<uint32_t>::const_iterator it = vertices_indices.begin();
	for (uint32_t v_new = 0; v_new < (uint32_t)vertices_indices.size(); ++v_new, ++it) {
		uint32_t v_old = *it;
		auto range = vert2face.equal_range(v_old);
		std::for_each(range.first, range.second,
			[&](const std::pair<const uint32_t, uint32_t>& f_id) {
				const Eigen::Array3i& face = context.p_mesh->get_faces().at(f_id.second);
				for (uint32_t j = 0; j < 3; ++j) {
					uint32_t v2_old = face[j];
					if (v2_old != v_old) {
						if (vertices_indices.count(v2_old) != 0) {
							vert_degree[v_new] += 1;
							uint32_t v2_new = old2new_vert.at(v2_old);
							triplet_list.push_back(Eigen::Triplet<float>(v_new, v2_new, -1.f));
						}
					}
				}
			});
	}

	// fill degree triplets
	for (uint32_t i = 0; i < (uint32_t)vertices_indices.size(); ++i) {
		triplet_list.push_back(Eigen::Triplet<float>(i, i, (float)vert_degree[i]));
	}

	// Square sparse matrix
	Eigen::SparseMatrix<float> laplacian(vertices_indices.size(), vertices_indices.size());
	laplacian.setFromTriplets(triplet_list.begin(), triplet_list.end());
	laplacian.makeCompressed();

	Spectra::SparseSymMatProd<float> op(laplacian);
	// Get Fiedler vector
	// Compute second smallest eigenvector
	Spectra::SymEigsSolver<Spectra::SparseSymMatProd<float>> eigs(op, 2, 4);
	eigs.init();

	Eigen::Index num_values = eigs.compute(Spectra::SortRule::SmallestAlge,
		context.max_iterations_eigen, context.error_eigen,
		Spectra::SortRule::LargestAlge);

	if (num_values != 2) {
		std::cerr << "Error: num eigenvalues computed is " << num_values << std::endl;
		return;
	}
	// Get results
	if (eigs.info() != Spectra::CompInfo::Successful) {
		std::cout << "Error: No eigenvalues. Computation not successful!" << std::endl;
		return;
	}

	float eigenvalue = eigs.eigenvalues()[0];
	if (eigenvalue <= 0) {
		std::cerr << "Error: Fiedler eigenvalue is less than 0. Not a connected graph!!" << std::endl;
		std::cerr << "Computed eigenvalues " << eigenvalue << std::endl;

		return;
	}

	const Eigen::VectorXf eigenvectors = eigs.eigenvectors().col(0);

	// Output or continue
	{
		uint32_t size_cluster_0 = 0;
		for (uint32_t i = 0; i < (uint32_t)eigenvectors.size(); ++i) {
			if (eigenvectors[i] < 0.0f) {
				size_cluster_0 += 1;
			}
		}
		std::unordered_set<uint32_t> indices_0, indices_1;
		indices_0.reserve(size_cluster_0);
		indices_1.reserve((uint32_t)eigenvectors.size() - size_cluster_0);

		for (uint32_t i = 0; i < (uint32_t)eigenvectors.size(); ++i) {
			if (eigenvectors[i] < 0.0f) {
				indices_0.insert(new2old_vert[i]);
			}
			else {
				indices_1.insert(new2old_vert[i]);
			}
		}

		vertex_laplacian_layout(context, depth + 1, vert2face,
				indices_0);

		vertex_laplacian_layout(context, depth + 1, vert2face,
				indices_1);

	}

}

void vertex_clustering_layout(
	LayoutContext& context,
	const std::unordered_multimap<uint32_t, uint32_t>& vert2face) {
	if (context.p_mesh->get_vertices().empty()) {
		return;
	}

	const std::vector<Eigen::Vector3f>& vertices_mesh = context.p_mesh->get_vertices();

	struct OctNodeTask {
		std::vector<uint32_t> vertices;
		uint32_t depth;
		Eigen::Vector3f mid_coord;
	};

	Eigen::Vector3f minBBox = Eigen::Vector3f::Constant( std::numeric_limits<float>::infinity());
	Eigen::Vector3f maxBBox = Eigen::Vector3f::Constant(-std::numeric_limits<float>::infinity());

	for (const Eigen::Vector3f& v : context.p_mesh->get_vertices()) {
		minBBox = minBBox.cwiseMin(v);
		maxBBox = maxBBox.cwiseMax(v);
	}

	const float octree_size = (maxBBox - minBBox).maxCoeff();

	std::stack<OctNodeTask> tasks;
	std::array<std::vector<uint32_t>, 8> child_verts;

	std::unordered_set<uint32_t> vert_indices_spectral;
	std::vector<std::vector<uint32_t>> vert_indices_per_set_buffer;

	vert_indices_spectral.reserve(vertices_mesh.size() * 3 / 2);

	// Create root node
	{
		OctNodeTask root;
		root.vertices.resize(context.p_mesh->get_vertices().size());
		std::iota(root.vertices.begin(), root.vertices.end(), 0);
		root.depth = 0;
		root.mid_coord = (maxBBox + minBBox) * 0.5f;
		tasks.push(std::move(root));
	}

	// Do not create octree if not needed
	if (tasks.top().vertices.size() < context.max_spectral_size) {
		vert_indices_spectral.clear();
		vert_indices_spectral.insert(tasks.top().vertices.begin(), tasks.top().vertices.end());
		// Spectral classification
		vertex_laplacian_layout(
			context,
			0, // depth
			vert2face,
			vert_indices_spectral
		);
		return;
	}

	// process octree
	while (!tasks.empty()) {
		const OctNodeTask task = std::move(tasks.top());
		tasks.pop();

		const float size_node = octree_size / static_cast<float>(1 << task.depth);

		// Clear childs
		for (auto& v : child_verts)  v.clear();

		// Classify vertices into the 8 childs
		for (const uint32_t& i : task.vertices) {
			const Eigen::Vector3f dir = vertices_mesh[i] - task.mid_coord;
			uint32_t k =
				((dir.x() >= 0.f ? 1 : 0) << 0) +
				((dir.y() >= 0.f ? 1 : 0) << 1) +
				((dir.z() >= 0.f ? 1 : 0) << 2);

			child_verts[k].push_back(i);
		}

		// Keep generating tasks or cluster
		for (uint32_t k = 0; k < 8; ++k) {
			if (child_verts[k].empty()) {
				continue;
			}

			if (child_verts[k].size() < context.max_spectral_size) {
				UnionFind<uint32_t> uf(child_verts[k]);
				for (uint32_t v : child_verts[k]) {
					auto range = vert2face.equal_range(v);
					std::for_each(range.first, range.second,
						[&](const std::pair<const uint32_t, uint32_t>& f_id) {
							const Eigen::Array3i& face = context.p_mesh->get_faces().at(f_id.second);
							for (uint32_t j = 0; j < 3; ++j) {
								uint32_t v2 = face[j];
								if (v2 != v) {
									uf.union_sets(v, v2);
								}
							}
						}
					);
				}

				if (uf.get_num_sets() != 1) {
					if (vert_indices_per_set_buffer.size() < uf.get_num_sets()) {
						vert_indices_per_set_buffer.resize(uf.get_num_sets());
					}
						uf.get_elements_of_sets(&vert_indices_per_set_buffer);
						for (const std::vector<uint32_t>& verts : vert_indices_per_set_buffer) {
							vert_indices_spectral.clear();
							vert_indices_spectral.insert(verts.begin(), verts.end());
							// Spectral classification
							vertex_laplacian_layout(
								context,
								0, // depth
								vert2face,
								vert_indices_spectral
							);
						}
				}
				else {
					vert_indices_spectral.clear();
					vert_indices_spectral.insert(child_verts[k].begin(), child_verts[k].end());
					// Spectral classification
					vertex_laplacian_layout(
						context,
						0, // depth
						vert2face,
						vert_indices_spectral
					);
				}
			}
			else {
				Eigen::Vector3f dir = { k & 0b1 ? 1.f : -1.f, k & 0b10 ? 1.f : -1.f, k & 0b100 ? 1.f : -1.f };
				OctNodeTask newT;
				newT.depth = task.depth + 1;
				newT.mid_coord = task.mid_coord + 0.25f * size_node * dir;
				newT.vertices = child_verts[k];
				tasks.push(std::move(newT));
			}
		}

	}

}

std::vector<uint32_t>
get_mapping_optimized_layout(
	const std::shared_ptr<TriangleMesh> p_mesh,
	const uint32_t max_depth,
	const uint32_t max_cluster_size,
	const uint32_t max_spectral_size,
	const uint32_t max_number_interations_eigen,
	const float eigen_error)

{

	uint32_t num_vertices = (uint32_t) p_mesh->get_vertices().size();

	std::unordered_set<uint32_t> vert_indices;
	vert_indices.reserve(num_vertices);
	for (uint32_t i = 0; i < num_vertices; ++i) {
		vert_indices.insert(i);
	}

	std::unordered_multimap<uint32_t, uint32_t> vert2face;
	vert2face.reserve(num_vertices);
	for (uint32_t f = 0; f < (uint32_t)p_mesh->get_faces().size(); ++f) {
		for (uint32_t j = 0; j < 3; ++j) {
			vert2face.insert({ p_mesh->get_faces()[f][j], f});
		}
	}

	LayoutContext context(p_mesh, max_depth, max_cluster_size, max_spectral_size,
		max_number_interations_eigen, eigen_error);

	vertex_clustering_layout(context, vert2face);

	return context.final_cluster;
}

}


// unit test file sinhc.hpp for the special functions test suite

//  (C) Copyright Hubert Holin 2003.
//  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 <functional>
#include <iomanip>
#include <iostream>
#include <complex>

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

#include <boost/test/unit_test.hpp>

BOOST_TEST_CASE_TEMPLATE_FUNCTION(sinhc_pi_test, T)
{
    using    ::std::abs;

    using    ::std::numeric_limits;

    using    ::boost::math::sinhc_pi;

    BOOST_MESSAGE("Testing sinhc_pi in the real domain for "
        << string_type_name<T>::_() << ".");

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(sinhc_pi<T>(static_cast<T>(0))-static_cast<T>(1)))
        (numeric_limits<T>::epsilon()));
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(sinhc_pi_complex_test, T)
{
    using    ::std::abs;
    using    ::std::sin;

    using    ::std::numeric_limits;

    using    ::boost::math::sinhc_pi;

    BOOST_MESSAGE("Testing sinhc_pi in the complex domain for "
        << string_type_name<T>::_() << ".");

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(sinhc_pi<T>(::std::complex<T>(0, 1))-
             ::std::complex<T>(sin(static_cast<T>(1)))))
        (numeric_limits<T>::epsilon()));
}

void    sinhc_pi_manual_check()
{
    using    ::boost::math::sinhc_pi;

    BOOST_MESSAGE(" ");
    BOOST_MESSAGE("sinc_pi");

    for    (int i = 0; i <= 100; i++)
    {
        BOOST_MESSAGE( ::std::setw(15)
                    << sinhc_pi<float>(static_cast<float>(i-50)/
                                                static_cast<float>(50))
                    << ::std::setw(15)
                    << sinhc_pi<double>(static_cast<double>(i-50)/
                                                static_cast<double>(50))
                    << ::std::setw(15)
                    << sinhc_pi<long double>(static_cast<long double>(i-50)/
                                                static_cast<long double>(50)));
    }

    BOOST_MESSAGE(" ");
}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graph_traits.hpp>
#include <iostream>
#include <ctime>
#include <stdio.h>

using namespace boost;
typedef adjacency_list<> Graph;
typedef rmat_iterator<minstd_rand, Graph> RMATGen;
typedef graph_traits<Graph>::vertex_iterator vertex_iter;
typedef property_map<Graph, vertex_index_t>::type IndexMap;

int main(int argc, char* argv[])
{
	if (argc < 3) {
		fprintf(stderr, "usage: make_graph #vertices, #numedges\n");
		return EXIT_FAILURE;
	}
	std::size_t n = atol(argv[1]);
	std::size_t m = atol(argv[2]);

	fprintf(stderr, "Vertices = %ld\n", n);
	fprintf(stderr, "Edges = %ld\n", m);

	std::clock_t start;
	start = std::clock();
	minstd_rand gen;
	RMATGen gen_it(gen, n, m, 0.57, 0.19, 0.19, 0.05, true);
	RMATGen gen_end;
	for (; gen_it != gen_end; ++gen_it) {
		std::cout << gen_it->first << " " << gen_it->second << std::endl;
	}
#if 0
	// Create graph with 100 nodes and 400 edges
	Graph g(RMATGen(gen, n, m, 0.57, 0.19, 0.19, 0.05, true), RMATGen(), n);

	IndexMap index = get(vertex_index, g);

	// Get vertex set
#if 0
	std::pair<vertex_iter, vertex_iter> vp;
	for (vp = vertices(g); vp.first != vp.second; ++vp.first)
		std::cout << index[*vp.first] <<  " ";
	std::cout << std::endl;
#endif

	// Get edge set
	graph_traits<Graph>::edge_iterator ei, ei_end;
	for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
		std::cout << index[source(*ei, g)]<< " " << index[target(*ei, g)] << "\n";
#endif

	std::cerr << "The time to build the graph = " << (std::clock()-start)/(double)CLOCKS_PER_SEC << std::endl;
	return 0;
}