/// \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


/*
    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 "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;
}

}


/*
MIT License

Copyright (c) 2016 Vernam Group

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

#include "../include/fntru.h"
#include <math.h>
#include <NTL/mat_ZZ.h>
#include <NTL/RR.h>

////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
// Compute sigma it is larger than sqrt(q)
long sigma(long n, long logq, long e){
	long s;

	RR n_ 		= to_RR(n);
	RR q_		= to_RR("2");	q_ = power(q_, logq);
	RR e_ 		= power(to_RR("2"), -e);//to_RR(e);
	RR s_;

	s_ = 2*n_*sqrt(log(8*n_*q_)/log(2.0));
	s_ = log(s_)/log(2.0);
	s_ = s_ + to_RR((0.5-2/e_)*logq);
	return s = to_long(s_);
}
// Create fntru
fntru::fntru(int cyclotM_, int bitsizeMod_, int radixSize_,  ZZ messagePrime_){

	n =  new ssntru(cyclotM_, 1, bitsizeMod_, 1, messagePrime_, 1);
	int sig = sigma(*n->Pointer_N(), bitsizeMod_, 80);	//Compute sigma and set

	n->SetSigma(sig);	// Set Sigma
	n->Func_SetModulus();
	n->Func_ModulusFind(1, bitsizeMod_);
	n->Func_ComputeKeys();

	radixSize	= radixSize_;	// Set radix size
	matrixDim 	= (bitsizeMod_/radixSize) + ((bitsizeMod_%radixSize == 0)? 0 : 1); // Compute matrix size = total bitsize / wordsize
/////////////////////////////////////////////////
	//Compute powers of radix
	PowTwo 		= new ZZ[matrixDim];
	ZZ c = to_ZZ("1");
	c = c << radixSize;
	PowTwo[0] = 1;
	for(int i=1; i<matrixDim; i++)
		PowTwo[i] = PowTwo[i-1]*c;
/////////////////////////////////////////////////
	//Set arrays in the matrix
	mulMat.row = new vec_ZZX[matrixDim];
	for(int i=0; i<matrixDim; i++)
		mulMat.row[i].SetLength(matrixDim);
}

//Initialize matrix
void fntru::InitializeCipher(fntru_cipher &m){
	m.row.SetLength(matrixDim);
	m.len = matrixDim;
}
//Compute the bit decomposition oof the given polynomial
void fntru::BitDecomp(vec_ZZX &BD, ZZX &c, int N, int bitlen){
	ZZX r;
	ZZ t;
	int b[radixSize], t_;
	for(int i=0; i<bitlen; i++){
		for(int j=0; j<N; j++){
			t_ = 0;
			for(int k=0; k<radixSize; k++)
				b[k] = bit(coeff(c, j), radixSize*i+k);

			for(int k=0; k<radixSize; k++)
				t_  = t_ + (b[k] << k);

			t = to_ZZ(t_);
			SetCoeff(r, j, t);
		}
		BD[i] = r;
	}
}
///////////////////////////////////////////////
// Create the polynomial by using the bit decomposition vector
void fntru::BitDecompInv(ZZX &out, vec_ZZX &BD, int bitlen){
	ZZX t;
	t = 0;
	for(int i=0; i<bitlen; i++){
		t = t + BD[i]*PowTwo[i];
	}
	out = t;
}

//////////////// FLATTEN ////////////////////////
//Take the ciphertext and apply flattening
void fntru::Flatten(fntru_cipher &c){
	Flatten(c, *n->Pointer_N(), matrixDim);
}

void fntru::Flatten(fntru_cipher &c, int &degree, int &l){
		Flatten(c.row, degree, l);
}
// Apply flattening for the vector
// First apply inverse bit decomposition and later apply bit decomposition
void fntru::Flatten(vec_ZZX &BD, int &degree, int &l){
	ZZX t;

	BitDecompInv(t, BD, l);
	for(int i=0; i<degree; i++)
		SetCoeff(t, i, coeff(t, i) %(*n->Pointer_Q(0)));

	BitDecomp(BD, t, degree, l);
}
// Apply flattening to the matrix (row by row)
void fntru::Flatten(mat_ZZX &c, int &degree, int &l){
	for(int i=0; i<l; i++){
		Flatten(c.row[i], degree, l);
	}
}
//////////////// FLATTEN ////////////////////////

void fntru::Encrypt(fntru_cipher &c, ZZX m){
	ZZX zero;
	ZZX E[matrixDim];
	mat_ZZX tempCipher;

	// Set Temp matrix
	tempCipher.row = new vec_ZZX[matrixDim];
	for(int i=0; i<matrixDim; i++)
		tempCipher.row[i].SetLength(matrixDim);

	// Encryption vector
	zero = 0;
	for(int i=0; i<matrixDim; i++)
		E[i] = n->Prim_Encrypt(zero, 0);

	// Binary Decomp of encryption vector into the temp matrix
	for(int i=0;i<matrixDim; i++)
		BitDecomp(tempCipher.row[i], E[i], *n->Pointer_N(), matrixDim);

	// add message
	for(int i=0; i<matrixDim; i++)
		tempCipher.row[i][i] = tempCipher.row[i][i]+m;

	// flatten
	Flatten(tempCipher, *n->Pointer_N(), matrixDim);

	// convert matrix into the encryption vector
	for(int i=0;i<matrixDim; i++)
		BitDecompInv(c.row[i], tempCipher.row[i], matrixDim);

	// free memory
	for(int i=0; i<matrixDim; i++)
		tempCipher.row[i].kill();
}

void fntru::Decrypt(ZZX &m, fntru_cipher &c){
	m = n->Prim_Decrypt(c.row[0], 0);
}

// if sel == 1: do l*l matrix multiplication by matrix*vector
// if sel == 0: do 1*l matrix multiplication by vector*vector
// When we are computing multiplication, we optimize it by performing
// vector*matrix multiplication. This way we do not need the flattening, since
// we do not do matrix*matrix multiplication.
//
// The function has 2 options. When we do vector=matrix*vector, it results in
// a ciphertext vector. All the elements (ntru ciphers) inside fntru_cipher is computed.
// In case of vector=vector*vector, we only compute the first row (first ntru cipher).
// This way we reduce the computation by a factor of matrixDim (number of ciphertexts inside fntru_cipher)
// If the second option is used it destroys the structure of fntru_cipher. Only first element
// protects its ciphertext form. Therefore, the user should use that as the output while evaluating all the circuit.
void fntru::Mult(fntru_cipher &c, fntru_cipher &a, fntru_cipher &b, int sel){
	if(sel == 1){
		//Bit decompose all the rows of the operand to turn it into a matrix
		for(int i=0; i<matrixDim; i++)
			BitDecomp( mulMat.row[i], a.row[i], *n->Pointer_N(), matrixDim);
		// compute matrix multiplication
		MatrixVectorMul(c.row, mulMat, b.row, matrixDim, *n);

		PolyReduce(c, matrixDim, *n);
		CoeffReduce(c, matrixDim, *n);
		// clear temp matrix
		for(int i=0; i<matrixDim; i++)
			for(int j=0; j<matrixDim; j++)
				mulMat.row[i][j]=0;
	}
	else {
		// Bit decompose only first ciphertext
		BitDecomp( mulMat.row[0], a.row[0], *n->Pointer_N(), matrixDim);
		VectorDotProd(c.row[0], mulMat.row[0], b.row, matrixDim, *n);

		// !!!TODO: Add a function for row
		n->Arith_PolyReduce(c.row[0], c.row[0]);
		for(int j=0; j< (*n->Pointer_N()); j++)
			SetCoeff(c.row[0], j, coeff(c.row[0], j) % (*n->Pointer_Q(0)));
		// Clear Matrix
		for(int j=0; j<matrixDim; j++)
			mulMat.row[0][j]=0;
	}
}

//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
//Compute matrix multiplication
void MatrixMul(mat_ZZX &c, mat_ZZX &a, mat_ZZX &b, int &vectorLength, ssntru &n){
	ZZX t;
	for(int i=0; i<vectorLength; i++){
		for(int j=0; j<vectorLength; j++){
			t = 0;
			for(int k=0; k<vectorLength; k++)
				t = t + a.row[i][k]*b.row[k][j];

			n.Arith_PolyReduce(t,t);
			c.row[i][j] = t;
		}
	}
}

// Compute the dot product of two vectors. In other words, we compute multiplication
// between the first row of the matrix and the input ciphertext vector.
// If thread is defined it uses NTL thread routine to compute the multiplications.
void VectorDotProd(ZZX &c, vec_ZZX &a, vec_ZZX &b, int &vectorLength, ssntru &n){
#ifdef Thread
	ZZX tt[vectorLength];
	PartitionInfo pinfo(vectorLength);
	long cnt = pinfo.NumIntervals();
	NTL_EXEC_INDEX(cnt, index)
			long first, last;
			pinfo.interval(first, last, index);
			for (int k = first; k<last; k++) {
				mul(tt[k], a[k], b[k]);
			}
	NTL_EXEC_INDEX_END

	c = 0;
	for (int k = 0; k<vectorLength; k++)
		c = c + tt[k];
#endif

#ifndef Thread
	c = 0;
	ZZX tt;
	for (int k = 0; k<vectorLength; k++){
		mul(tt, a[k], b[k]);
//		mul(tt, a[k], b[k]);
		c=c+tt;
	}
#endif
}

//Matrix vector multiplication. TODO: Check 3 options for which one is more optimized for speed and area.
void MatrixVectorMul(vec_ZZX &c, mat_ZZX &a, vec_ZZX &b, int &vectorLength, ssntru &n){

#ifdef Thread
#ifdef MULT1
	ZZX acc, tmp;

	ZZX t, t2;
	ZZX tt[vectorLength];

	for (int i = 0; i<vectorLength; i++) {
		//clear(acc);
		acc = 0;

		PartitionInfo pinfo(vectorLength);
		long cnt = pinfo.NumIntervals();

		NTL_EXEC_INDEX(cnt, index)
			long first, last;
			pinfo.interval(first, last, index);
			for (int k = first; k<last; k++) {
				mul(tt[k], a.row[i][k], b[k]);
			}
		NTL_EXEC_INDEX_END

		acc = 0;
		for(int j=0; j<vectorLength; j++)
			acc = acc + tt[j];
		c[i] = acc;
	}
#endif
#ifdef MULT2
	ZZX acc, tmp;

	ZZX t, t2;
	ZZX tt[vectorLength];

	myTimerReal tr;
	double multtime = 0, addtime = 0;
	for (int i = 0; i<vectorLength; i++) {
		//clear(acc);
		acc = 0;
		start(tr);
		PartitionInfo pinfo(vectorLength);
		long cnt = pinfo.NumIntervals();

		NTL_EXEC_INDEX(cnt, index)
			long first, last;
			pinfo.interval(first, last, index);
			for (int k = first; k<last; k++) {
				mul(tt[k], a.row[i][k], b[k]);
			}
		NTL_EXEC_INDEX_END
		stop(tr);
		multtime += getseconds(tr);
		acc = 0;
		start(tr);
		for(int j=0; j<vectorLength; j++)
			acc = acc + tt[j];
		c[i] = acc;
		stop(tr);
		addtime += getseconds(tr);
	}

#endif
#ifdef MULT3
	ZZX tmp;

	ZZX t, t2;
	ZZX acc[vectorLength], tt[vectorLength];

	for(int j=0; j<vectorLength; j++){
		acc[j] = 0;
		c[j] = 0;
	}

	for (int i = 0; i<vectorLength; i++) {

		PartitionInfo pinfo(vectorLength);
		long cnt = pinfo.NumIntervals();

		NTL_EXEC_INDEX(cnt, index)
			long first, last;
			pinfo.interval(first, last, index);
			for (int k = first; k<last; k++) {
				mul(tt[k], a.row[k][i], b[i]);
			}
		NTL_EXEC_INDEX_END

		for(int j=0; j<vectorLength; j++)
			c[j] = c[j] + tt[j];

//		for(int j=0; j<vectorLength; j++)
//			c[j] = c[j] + acc[j];
	}
#endif
#endif

#ifndef Thread
	ZZX acc, tmp;
	ZZX t, t2;
	ZZX tt[vectorLength];

	for (int i = 0; i<vectorLength; i++) {
		acc = 0;
		for (int k = 0; k<vectorLength; k++)
			mul(tt[k], a.row[i][k], b[k]);

		acc = 0;
		for(int j=0; j<vectorLength; j++)
			acc = acc + tt[j];
		c[i] = acc;
	}

#endif
}
// Coefficient modulus reduction for all ciphertexts in fntru ciphertext
void CoeffReduce(fntru_cipher &c, int &vectorLength, ssntru &n){
	for(int i=0; i<vectorLength; i++)
		for(int j=0; j< (*n.Pointer_N()); j++)
			SetCoeff(c.row[i], j, coeff(c.row[i], j) % (*n.Pointer_Q(0)));
}
// Polynomial modulus reduction for all ciphertexts in fntru ciphertext
void PolyReduce(fntru_cipher &c, int &vectorLength, ssntru &n){
	for(int i=0; i<vectorLength; i++)
		n.Arith_PolyReduce(c.row[i], c.row[i]);
}

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

void fntru::AND(fntru_cipher &c, fntru_cipher &a, fntru_cipher &b, int sel){
	Mult(c, a, b, sel);
}

void fntru::XOR(fntru_cipher &c, fntru_cipher &a, fntru_cipher &b, int sel){

	if(sel == 1){
		ZZX t[a.len];
		for(int i=0; i<a.len; i++)
			t[i] = a.row[i] + b.row[i];

		Mult(c, a, b, 1);
		for(int i=0; i<a.len; i++)
			c.row[i] = t[i] - 2*c.row[i];

		PolyReduce(c, c.len, *n);
		CoeffReduce(c, c.len, *n);
	}
	else if(sel == 0){
		ZZX t;
		int vecSize = 1;
		t = a.row[0] + b.row[0];
		Mult(c, a, b, 0);
		c.row[0] = t - 2*c.row[0];
		PolyReduce(c, vecSize, *n);
		CoeffReduce(c, vecSize, *n);
	}
}

void fntru::OR(fntru_cipher &c, fntru_cipher &a, fntru_cipher &b, int sel){

	if(sel == 1){
		ZZX t[a.len];
		for(int i=0; i<a.len; i++)
			t[i] = a.row[i] + b.row[i];

		Mult(c, a, b, 1);
		for(int i=0; i<a.len; i++)
			c.row[i] = t[i] - c.row[i];

		PolyReduce(c, c.len, *n);
		CoeffReduce(c, c.len, *n);
	}
	else if(sel == 0){
		ZZX t;
		int vecSize = 1;
		t = a.row[0] + b.row[0];
		Mult(c, a, b, 0);
		c.row[0] = t - c.row[0];
		PolyReduce(c, vecSize, *n);
		CoeffReduce(c, vecSize, *n);
	}
}

void fntru::NOT(fntru_cipher &c, fntru_cipher &a, int sel){

	if(sel == 1){
		ZZX two;
		two = to_ZZ("1");
		for(int i=0; i<a.len; i++){
			c.row[i] = two - a.row[i];
			two = two << 1;
		}
		CoeffReduce(c, c.len, *n);
	}
	else if(sel == 0){
		int vecSize = 1;
		c.row[0] = to_ZZ("1") - a.row[0];
		CoeffReduce(c, vecSize, *n);
	}
}

void fntru::NAND(fntru_cipher &c, fntru_cipher &a, fntru_cipher &b, int sel){

	Mult(c, a, b, sel);
	if(sel == 1){
		ZZX two;
		two = to_ZZ("1");
		for(int i=0; i<a.len; i++){
			c.row[i] = two - c.row[i];
			two = two << 1;
		}
		CoeffReduce(c, c.len, *n);
	}
	else if(sel == 0){
		int vecSize = 1;
		c.row[0] = to_ZZ("1") - c.row[0];
		CoeffReduce(c, vecSize, *n);
	}
}


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Core>

#include <sophus/se3.h>
#include <sophus/so3.h>

#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

using namespace std;
using Sophus::SE3;
using Sophus::SO3;

/************************************************
 * 本程序演示如何用 gtsam 进行位姿图优化
 * sphere.g2o 是人工生成的一个 Pose graph，我们来优化它。
 * 与 g2o 相似，在 gtsam 中添加的是因子，相当于误差
 * **********************************************/

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"Usage: pose_graph_gtsam sphere.g2o"<<endl;
        return 1;
    }
    ifstream fin ( argv[1] );
    if ( !fin )
    {
        cout<<"file "<<argv[1]<<" does not exist."<<endl;
        return 1;
    }

    // TO-DO：使用gtsam的因子图优化方法
    gtsam::NonlinearFactorGraph::shared_ptr graph ( new gtsam::NonlinearFactorGraph );  // gtsam的因子图
    gtsam::Values::shared_ptr initial ( new gtsam::Values ); // 初始值
    // 从g2o文件中读取节点和边的信息
    int cntVertex=0, cntEdge = 0;
    cout<<"reading from g2o file"<<endl;

    while ( !fin.eof() )
    {
        string tag;
        fin>>tag;
        if ( tag == "VERTEX_SE3:QUAT" )
        {
            // 顶点
            gtsam::Key id;
            fin>>id;
            double data[7];
            for ( int i=0; i<7; i++ ) fin>>data[i];
            // 转换至gtsam的Pose3
            gtsam::Rot3 R = gtsam::Rot3::Quaternion ( data[6], data[3], data[4], data[5] );
            gtsam::Point3 t ( data[0], data[1], data[2] );
            initial->insert ( id, gtsam::Pose3 ( R,t ) );       // 添加初始值
            cntVertex++;
        }
        else if ( tag == "EDGE_SE3:QUAT" )
        {
            // 边，对应到因子图中的因子
            gtsam::Matrix m = gtsam::I_6x6;     // 信息矩阵
            gtsam::Key id1, id2;
            fin>>id1>>id2;
            double data[7];
            for ( int i=0; i<7; i++ ) fin>>data[i];
            gtsam::Rot3 R = gtsam::Rot3::Quaternion ( data[6], data[3], data[4], data[5] );
            gtsam::Point3 t ( data[0], data[1], data[2] );
            for ( int i=0; i<6; i++ )
                for ( int j=i; j<6; j++ )
                {
                    double mij;
                    fin>>mij;
                    m ( i,j ) = mij;
                    m ( j,i ) = mij;
                }

            // g2o的信息矩阵定义方式与gtsam不同，这里对它进行修改
            gtsam::Matrix mgtsam = gtsam::I_6x6;
            mgtsam.block<3,3> ( 0,0 ) = m.block<3,3> ( 3,3 ); // cov rotation
            mgtsam.block<3,3> ( 3,3 ) = m.block<3,3> ( 0,0 ); // cov translation
            mgtsam.block<3,3> ( 0,3 ) = m.block<3,3> ( 0,3 ); // off diagonal
            mgtsam.block<3,3> ( 3,0 ) = m.block<3,3> ( 3,0 ); // off diagonal

            // 单独书写噪声模型
            gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information ( mgtsam );        // 高斯噪声模型
            gtsam::NonlinearFactor::shared_ptr factor (
                new gtsam::BetweenFactor<gtsam::Pose3> ( id1, id2, gtsam::Pose3 ( R,t ), model ) // 添加一个因子
            );
            graph->push_back ( factor );
            cntEdge++;
        }
        if ( !fin.good() )
            break;
    }

    cout<<"read total "<<cntVertex<<" vertices, "<<cntEdge<<" edges."<<endl;
    // 固定第一个顶点，在gtsam中相当于添加一个先验因子
    gtsam::NonlinearFactorGraph graphWithPrior = *graph;
    gtsam::noiseModel::Diagonal::shared_ptr priorModel =
        gtsam::noiseModel::Diagonal::Variances (
            ( gtsam::Vector ( 6 ) <<1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6 ).finished()
        );
    gtsam::Key firstKey = 0;
    for ( const gtsam::Values::ConstKeyValuePair& key_value: *initial )
    {
        cout<<"Adding prior to g2o file "<<endl;
        // prior先验因子
        graphWithPrior.add ( gtsam::PriorFactor<gtsam::Pose3> (
            key_value.key, key_value.value.cast<gtsam::Pose3>(), priorModel )
        );
        break;
    }

    // 开始因子图优化，配置优化选项
    cout<<"optimizing the factor graph"<<endl;
    // 我们使用 LM 优化
    gtsam::LevenbergMarquardtParams params_lm;
    params_lm.setVerbosity("ERROR");
    params_lm.setMaxIterations(20); // 事实上5次就收敛了
    params_lm.setLinearSolverType("MULTIFRONTAL_QR");
    gtsam::LevenbergMarquardtOptimizer optimizer_LM( graphWithPrior, *initial, params_lm );

    // 你可以尝试下 GN
    // gtsam::GaussNewtonParams params_gn;
    // params_gn.setVerbosity("ERROR");
    // params_gn.setMaxIterations(20);
    // params_gn.setLinearSolverType("MULTIFRONTAL_QR");
    // gtsam::GaussNewtonOptimizer optimizer ( graphWithPrior, *initial, params_gn );

    gtsam::Values result = optimizer_LM.optimize();
    cout<<"Optimization complete"<<endl;
    cout<<"initial error: "<<graph->error ( *initial ) <<endl;
    cout<<"final error: "<<graph->error ( result ) <<endl;

    cout<<"done. write to g2o ... "<<endl;
    // 写入 g2o 文件，同样伪装成 g2o 中的顶点和边，以便用 g2o_viewer 查看。
    // 顶点
    ofstream fout ( "result_gtsam.g2o" );
    for ( const gtsam::Values::ConstKeyValuePair& key_value: result )
    {
        gtsam::Pose3 pose = key_value.value.cast<gtsam::Pose3>();
        gtsam::Point3 p = pose.translation();
        gtsam::Quaternion q = pose.rotation().toQuaternion();
        fout<<"VERTEX_SE3:QUAT "<<key_value.key<<" "
            <<p.x() <<" "<<p.y() <<" "<<p.z() <<" "
            <<q.x()<<" "<<q.y()<<" "<<q.z()<<" "<<q.w()<<" "<<endl;
    }
    // 边
    for ( gtsam::NonlinearFactor::shared_ptr factor: *graph )
    {
        gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr f = dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>( factor );
        if ( f )
        {
            gtsam::SharedNoiseModel model = f->noiseModel();
            gtsam::noiseModel::Gaussian::shared_ptr gaussianModel = dynamic_pointer_cast<gtsam::noiseModel::Gaussian>( model );
            if ( gaussianModel )
            {
                // write the edge information
                gtsam::Matrix info = gaussianModel->R().transpose() * gaussianModel->R();
                gtsam::Pose3 pose = f->measured();
                gtsam::Point3 p = pose.translation();
                gtsam::Quaternion q = pose.rotation().toQuaternion();
                fout<<"EDGE_SE3:QUAT "<<f->key1()<<" "<<f->key2()<<" "
                    <<p.x() <<" "<<p.y() <<" "<<p.z() <<" "
                    <<q.x()<<" "<<q.y()<<" "<<q.z()<<" "<<q.w()<<" ";
                gtsam::Matrix infoG2o = gtsam::I_6x6;
                infoG2o.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
                infoG2o.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
                infoG2o.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
                infoG2o.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
                for ( int i=0; i<6; i++ )
                    for ( int j=i; j<6; j++ )
                    {
                        fout<<infoG2o(i,j)<<" ";
                    }
                fout<<endl;
            }
        }
    }
    fout.close();
    cout<<"done."<<endl;
}


// Standard includes
#include <fstream>
#include <iostream>
#include <sstream>
#include <algorithm>
#include <string>
#include <vector>

// For date parsing
#include <boost/date_time/local_time/local_time.hpp>

// For converting WGS84 <-> LTP coordinates
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>

// Main entry point
int main(int argc, char* argv[])
{
	// We are in science here :)
	std::cout.precision(16);

	// Base station coordinates (Friday)
	double org_lat = 51.710979902;
	double org_lon = -0.210839049;
	double org_alt = 141.1027;

	// Setup a converter
	GeographicLib::Geocentric wgs84_ecef(
		GeographicLib::Constants::WGS84_a(),
		GeographicLib::Constants::WGS84_f()
	);
	GeographicLib::LocalCartesian   wgs84_enu(
		org_lat,
		org_lon,
		org_alt,
		wgs84_ecef
	);

	// Argument check
	if (argc < 2)
	{
		std::cout << "Usage: " << argv[0] << " <file> " << std::endl;
		return 1;
	}

	std::ifstream 	namefile(argv[1]);
	std::string 	input;
	int counter = 0;
	int line    = 0;

	// Eat up the heading
	for (int i = 0; i < 15; i++)
		namefile >> input;

  	boost::posix_time::ptime epoch(boost::gregorian::date(1970,1,1));

	// Now process the data
	std::string d, t;
	double x, y, z;
	double lat, lon, alt, en, ee, eu, ede, edn, edu, age, ratio;
	int q, ns;
	while(namefile >> d >> t >> lat >> lon >> alt >> q >> ns >> en >> ee >> eu >> edn >> ede >> edu >> age >> ratio)
	{
		// Extract a meaningful value in seconds
	    std::stringstream ss;
	    boost::local_time::local_date_time ldt(boost::local_time::not_a_date_time);
	    boost::local_time::local_time_input_facet* input_facet = new boost::local_time::local_time_input_facet();
	    input_facet->format("%Y/%m/%d %H:%M:%s");
	    ss.imbue(std::locale(ss.getloc(), input_facet));
		ss << d << " " << t;
 		ss >> ldt;

 		boost::posix_time::ptime a = ldt.utc_time();
    	boost::posix_time::ptime epoch(boost::gregorian::date(1970,1,1));
    	double ms = (a - epoch).total_milliseconds();
    	ms /= 1e3;

 		// Convert value to LTP
		wgs84_enu.Forward(
			lat,
			lon,
			alt,
			x,
			y,
			z
		);

		// Print seconds, WGS84 and LTP
		std::cout << ms << "," << lat << "," << lon << "," << alt << "," << x << "," << y  << "," << z << std::endl;
	}

	// Success
	return 0;
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-ieee
    This function object returns ulp distance between its arguments.

    It is often difficult to answer to the following question: "are
    these two floating computations results similar enough?". The
    ulpdist is a way to answer which is tuned for relative errors
    estimations and peculiarly adapted to cope with the limited bits
    accuracy of floating point representations.

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

    @par Notes

    - If the common type is integral  @c ulpdist is the same as @c dist

    - If the common type is floating point the ulpdist is is computed,
    by the above described method.

    The method is the following:

     - If one and only one of the parameters is @ref Nan the result is @ref Nan,
     if both are Nans the result is @ref Zero

     - Else, properly normalize the two numbers by the same factor in a way
     that the largest of the two numbers exponents will be brought to
     zero

     - Return the absolute difference of these normalized numbers
      divided by the rounding error Eps

    The rounding error is the ulp (unit in the last place) value, i.e. the
    floating number, the exponent of which is 0 and the mantissa is all zeros
    but a 1 in the last digit (it is not hard coded that way however).
    This means \f$2^{-23}\f$ for float and \f$2^{-52}\f$ for double.

    \arg For instance if two floating numbers (of same type) have an ulpdist of
    @ref Zero that means that their floating representation are identical or they are
    both Nans.

    \arg Generally equality up to 0.5 ulp is the best that one can wish beyond
    strict equality.

    \arg Typically if a double is compared to the float representation of
    its floating conversion (they are exceptions as for fully representable
    reals) the ulpdist will be around \f$2^{26.5}\f$ (~\f$10^8\f$)

    \arg  @c ulpdist(1.0,1+Eps\<double\>())==0.5
    \arg  @c ulpdist(1.0,1+Eps\<double\>()/2)==0.0
    \arg  @c ulpdist(1.0,1-Eps\<double\>()/2)==0.25
    \arg  @c ulpdist(1.0,1-Eps\<double\>())==0.5
    \arg  @c ulpdist(double(Pi\<float\>()),Pi\<double\>())==9.84293e+07

    @see ulp, Eps, eps

    @par Example:

      @snippet ulpdist.cpp ulpdist

    @par Possible output:

      @snippet ulpdist.txt ulpdist

  **/
  IEEEValue ulpdist(IEEEValue const& x, IEEEValue const& y);
} }
#endif

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

#endif


#include <Eigen/Dense>
#include <iostream>
#include "general.h"

using EigenMatrix = Eigen::MatrixXd;
using EigenVector = Eigen::VectorXd;

EigenMatrix oneD()
{
    EigenMatrix m(1,4);
    m(0,0) = 3;
    m(0,1) = 2.5;
    m(0,2) = -1;
    m(0,3) = 1.5;
    return m;
}

EigenMatrix lapack_oneD()
{
    auto eigenM = oneD();
    int n = eigenM.cols();
    std::cout << "Cols: " << n << std::endl;
    int nn = n ;
    int rc = nn;
    int nnp = nn;
    int one = 1;
    double *w = nullptr;
    double *aux = nullptr;
    double *aux2 = nullptr;
    w = (double*)malloc(sizeof(double)*nn);
    aux = (double*)malloc(sizeof(double)*nn);
    aux2 = (double*)malloc(sizeof(double)*nn);
    for(int i=0;i<nn;i++)
      w[i] = eigenM(0, i+1) - eigenM(0, i); /* Dy */

    for(int i=0;i<nn-1;i++){
        aux[i] = 2;
        aux2[i] = -1;
    }
    aux[nn-1] = 2;

    dpttrf_(&nnp, aux, aux2, &rc);
    std::cout << "LDL factorization:" << std::endl;
    std::cout << "aux (Diagonal):" << std::endl;
    for(int i=0;i<nn;i++) {
      std::cout << aux[i] << ", ";
    }
    std::cout << std::endl;
    std::cout << "aux2 (SubDiagonal):" << std::endl;
    for(int i=0;i<nn;i++) {
      std::cout << aux2[i] << ", ";
    }
    std::cout << std::endl;
    dpttrs_(&nnp, &one, aux, aux2, w, &nnp, &rc);

    std::cout << "Solution w:" << std::endl;
    for(int i=0;i<nn;i++)
      std::cout << w[i] << ", ";
    std::cout << std::endl;

    EigenVector m(4);
    for(int i=0;i<nn;i++)
      m(i) = w[i];
    std::cout << m << std::endl;

    if(w) free(w);
    if(aux) free(aux);
    if(aux2) free(aux2);

    return m;
}

/* https://eigen.tuxfamily.org/dox/group__TutorialMapClass.html */
EigenMatrix eigen_lapack_equivalent_oneD()
{
    // Solve X in A*X = B, where A is triangular definite
    EigenMatrix eigenM(4,4);
    {
        EigenVector diag(eigenM.cols());
        diag.setConstant(2);
        EigenVector subUpperDiag(eigenM.cols());
        subUpperDiag.setConstant(-1);
        eigenM.diagonal() = diag;
        eigenM.diagonal(1) = subUpperDiag;
        eigenM.diagonal(-1) = subUpperDiag;
    }
    std::cout << "Matrix eigenM:\n" << eigenM << std::endl;
    EigenVector b(eigenM.cols());
    EigenMatrix oneM = oneD();
    for (int i = 0; i < oneM.cols(); ++i) {
        b[i] = oneM(0, i+1) - oneM(0, i); /* Dy */
    }

    // EigenVector v = eigenM.llt().solve(b);
    // EigenVector v = eigenM.ldlt().solve(b);
    Eigen::LDLT<EigenMatrix> ldltOfM(eigenM);
    EigenVector v = ldltOfM.solve(b);
    std::cout << v << std::endl;
    // EigenVector v = eigenM.colPivHouseholderQr().solve(b);
    EigenMatrix ldltMatrix = ldltOfM.matrixLDLT();
    std::cout << ldltMatrix << std::endl;
    EigenMatrix lMatrix = ldltOfM.matrixL();
    std::cout << lMatrix << std::endl;
    std::cout << "Diagonal of LDLT:" << std::endl;
    std::cout << ldltMatrix.diagonal() << std::endl;
    std::cout << "SubDiagonal of LDLT:" << std::endl;
    std::cout << ldltMatrix.diagonal(-1) << std::endl;

    return v;
}

/// The idea is that input and output are raw pointers to couple with existing code
EigenMatrix eigen_interface_raw_pointers_oneD()
{
    // Create the typemaps
    using EigenMatrixMap = Eigen::Map<EigenMatrix>;
    using EigenMatrixReadOnlyMap = Eigen::Map<const EigenMatrix>;
    using EigenVectorMap = Eigen::Map<EigenVector>;
    using EigenVectorReadOnlyMap = Eigen::Map<const EigenVector>;
    // Populate values using Eigen (just because easier)
    EigenMatrix eigenM(4,4);
    {
        EigenVector diag(eigenM.cols());
        diag.setConstant(2);
        EigenVector subUpperDiag(eigenM.cols());
        subUpperDiag.setConstant(-1);
        eigenM.diagonal() = diag;
        eigenM.diagonal(1) = subUpperDiag;
        eigenM.diagonal(-1) = subUpperDiag;
    }
    std::cout << "Matrix eigenM:\n" << eigenM << std::endl;
    EigenVector b(eigenM.cols());
    EigenMatrix oneM = oneD();
    for (int i = 0; i < oneM.cols(); ++i) {
        b[i] = oneM(0, i+1) - oneM(0, i); /* Dy */
    }
    // Set input raw pointers
    int nn = eigenM.cols();
    double *w = (double*)malloc(sizeof(double)*nn);
    for(int i=0;i<nn;i++) w[i] = b[i];
    // w is b as input (Ax=b), and solution x as output in lapack code.
    // http://www.netlib.org/lapack/lapack-3.1.1/html/dpttrs.f.html
    // Create a VectorMap of w to interact with Eigen
    EigenVectorMap wMap(w, b.size());
    EigenVector v = eigenM.ldlt().solve(wMap);
    std::cout << "v:\n" << v << std::endl;
    std::cout << "b:\n" << b << std::endl;
    std::cout << "wMap:\n" << wMap << std::endl;
    wMap = eigenM.ldlt().solve(wMap);
    std::cout << "InPlace wMap:\n" << wMap << std::endl;
    v = wMap;
    if(w) free(w);
    return v;
}

int main()
{
    bool test_passed = true;
    std::cout << "-- oneD Eigen --" << std::endl;
    auto mOneD = oneD();
    std::cout << mOneD << std::endl;
    std::cout << "-- lapack (from PN_TV1_Weighted) --" << std::endl;
    auto mLapack = lapack_oneD();
    std::cout << "-- eigen equivalent cholesky --" << std::endl;
    auto mEigen = eigen_lapack_equivalent_oneD();
    if(mLapack != mEigen){
        std::cerr << "Result vectors between mLapack and mEigen are not equal"  << std::endl;
        test_passed = false;
    }
    std::cout << "-- eigen interface with raw pointers --" << std::endl;
    auto mEigenRaw = eigen_interface_raw_pointers_oneD();
    if(mLapack != mEigenRaw){
        std::cerr << "Result vectors between mLapack and mEigenRaw are not equal"  << std::endl;
        test_passed = false;
    }

    if(!test_passed) return EXIT_FAILURE;
}


// Copyright 2019 Google LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     https://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <trajectory_math/float_comparison.h>
#include <trajectory_math/trajectory_algorithms.h>

#include <Eigen/Geometry>

namespace bookbot {
double TriangleArea(Eigen::Vector2d start_point, Eigen::Vector2d corner_point,
                    Eigen::Vector2d end_point) {
  const Eigen::Vector3d corner_to_start{start_point[0] - corner_point[0],
                                        start_point[1] - corner_point[1], 0};
  const Eigen::Vector3d corner_to_end{end_point[0] - corner_point[0],
                                      end_point[1] - corner_point[1], 0};
  return 0.5 *
         std::abs(Eigen::Vector3d(corner_to_start.cross(corner_to_end))[2]);
}

double ApproximateCurvatureMagnitude(Eigen::Vector2d start_point,
                                     Eigen::Vector2d corner_point,
                                     Eigen::Vector2d end_point) {
  const double area = TriangleArea(start_point, corner_point, end_point);
  const double start_to_corner =
      Eigen::Vector2d(start_point - corner_point).squaredNorm();
  const double start_to_end =
      Eigen::Vector2d(start_point - end_point).squaredNorm();
  const double corner_to_end =
      Eigen::Vector2d(corner_point - end_point).squaredNorm();
  const double normalization = start_to_corner * start_to_end * corner_to_end;
  if (ApproxZero(normalization)) {
    return 0.;
  }
  return 4 * area / std::sqrt(normalization);
}

}  // namespace bookbot


#include "LSCM.h"
#include <assert.h>
#include <math.h>
#include <float.h>
#include <Eigen/Sparse>
#include <iostream>
#include <sstream>
#include <Eigen/IterativeLinearSolvers>

using namespace MeshLib;

typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::VectorXd VectorXd;

LSCM::LSCM(Mesh * mesh) {
	m_mesh = mesh;
}

LSCM::~LSCM(){}

void LSCM::set_coefficients() {
	for (MeshEdgeIterator eiter(m_mesh); !eiter.end(); ++eiter) {
		Edge * e = *eiter;
		e_l(e) = m_mesh->edge_length(e);
	}

	for (MeshFaceIterator fiter(m_mesh); !fiter.end(); ++fiter) {
		Point  p[3];
		Face *f = *fiter;

		double l[3];
		HalfEdge * he = f->halfedge();
		for (int j = 0; j < 3; j++) {
			Edge * e = he->edge();
			l[j] = e_l(e);
			he = he->he_next();
		}

		double a = acos((l[0]*l[0] + l[2]*l[2] - l[1]*l[1]) / (2*l[0]*l[2]));

		p[0] = Point(0, 0, 0);
		p[1] = Point(l[0], 0, 0);
		p[2] = Point(l[2]*cos(a), l[2]*sin(a), 0);

		Point n = (p[1]-p[0]) ^ (p[2]-p[0]);
		double area = n.norm() / 2.0;
		n /= area;

		he = f->halfedge();
		for (int j = 0; j < 3; j++) {
			Point s = (n ^ (p[(j + 1) % 3] - p[j])) / sqrt(area);
			c_s(he) = s;
			he = he->he_next();
		}
	}
}

void LSCM::project() {
	set_coefficients();

	std::vector<Vertex*> vertices;
    std::vector<Face*> faces;

	for (MeshVertexIterator viter(m_mesh); !viter.end(); ++viter){
		Vertex * v = *viter;
		if (v->string().substr(0,3) != "fix") {
			vertices.push_back(v);
		}
		else {
			m_fix_vertices.push_back(v);
		}
	}
	assert(m_fix_vertices.size()>=2);

	for (int k = 0; k < (int)vertices.size(); k++ ){
		v_idx(vertices[k]) = k;
	}
	for (int k = 0; k < (int)m_fix_vertices.size(); k++) {
		v_idx(m_fix_vertices[k]) = k;
		Vertex *v = m_fix_vertices[k];
		std::string tmp;
		double uv0, uv1;
		std::stringstream(v->string()) >> tmp >> uv0 >> uv1;
		v_uv(v) = Point2(uv0, uv1);
	}

	int fn = m_mesh->numFaces();
	int	vfn = m_fix_vertices.size();
	int	vn = m_mesh->numVertices() - vfn;

	typedef Eigen::Triplet<double> T;
	std::vector<T> tripletList1;
	std::vector<T> tripletList2;
	tripletList1.reserve(fn);
	tripletList2.reserve(fn);
	VectorXd b(vfn * 2);

	int fid = 0;
	for (MeshFaceIterator fiter(m_mesh); !fiter.end(); ++fiter, ++fid) {
		Face *f = *fiter;
		HalfEdge *he = f->halfedge();

		for (int j = 0; j < 3; j++) {
			Point s = c_s(he);

			Vertex * v = he->he_next()->target();
			int vid = v_idx(v);

			if (v->string().substr(0,3) != "fix") {
				tripletList1.push_back(T(fid,vid,s[0]));
				tripletList1.push_back(T(fn + fid, vn + vid, s[0]));
				tripletList1.push_back(T(fid, vn + vid, -s[1]));
				tripletList1.push_back(T(fn + fid, vid, s[1]));
			}
			else {
				Point2 uv = v_uv(v);

				tripletList2.push_back(T(fid, vid, s[0]));
				tripletList2.push_back(T(fn + fid, vfn + vid, s[0]));
				tripletList2.push_back(T(fid, vfn + vid, -s[1]));
				tripletList2.push_back(T(fn + fid, vid, s[1]));

				b[vid] = uv[0];
				b[vfn + vid] = uv[1];
			}
			he = he->he_next();
		}
	}
	SpMat A(2*fn, 2*vn);
	SpMat B(2*fn, 2*vfn);

	A.setFromTriplets(tripletList1.begin(), tripletList1.end());
	B.setFromTriplets(tripletList2.begin(), tripletList2.end());

	VectorXd r, x;
	r = B * b;
	r = r * -1;

    // Solve the linear system Ax = r
	Eigen::LeastSquaresConjugateGradient<Eigen::SparseMatrix<double>> lscg;
	lscg.compute(A);
	x = lscg.solve(r);

	for (int i = 0; i < vn; i++) {
		Vertex * v = vertices[i];
		v_uv(v) = Point2(x[i], x[i + vn]);
	}
	for (MeshVertexIterator viter(m_mesh); !viter.end(); viter++) {
		Vertex * v = *viter;
		Point2 p = v_uv(v);
		Point p3(p[0], p[1], 0);
		v->point() = p3;
	}
}