/// \file

#include <iostream>
#include <stdio.h>
#include <fstream>
#include <Eigen/Dense>
#include "Perceptron.hpp"
#include <cstdlib>
#include <tbb/tbb.h>
#include <tbb/task_scheduler_init.h>

#define BLACK '#'
#define WHITE '_'

/// \brief Permet de savoir si dans quelle mode lire les metadonnees d'un fichier IDX (low-endien ou big-endian)
/// \param f Pointeur sur la chaine de caractere contenant le nom du fichier IDX
/// \return le mode selon lequel lire le fichier IDX (0 ou 1)
/// \warning teste uniquement sur des processeurs Intel!!!
int idxEndianness(char* f)
{
  int n = 0;
  char* r = nullptr;
  r = (char*)&n;
  std::ifstream in(f, std::ifstream::in);
  //On lit les 4 premiers octets
  in.read(r, 4);
  if(n != 2049 && n!= 2051)
  {
      return 0;
  }
  else
  {
      return 1;
  }
}

/// \brief Lire les metadonnees d'un fichier IDX (fonction recursive)
/// \param in Flux du fichier IDX a lire
/// \param mode Mode Mode pour lire le fichier selon l'endianness
/// \param n Reference vers la variable dans laquelle mettre la prochaine metadonnee a lire
/// \param args References vers les prochaines variable dans laquelle mettre les metadonnees suivantes a lire
/// \warning teste uniquement sur des processeurs Intel!!!
template<typename... Args>
void idxMeta(std::ifstream& in, int mode, std::size_t& n, Args& ...args)
{
  char* r = nullptr;
  r = (char*)&n;
  //On lit les 4 premiers octets
  in.read(r, 4);
  //Sur Intel, on les inverses
  if(mode==0)
  {
    char tmp;
    tmp = r[0];
    r[0] = r[3];
    r[3] = tmp;
    tmp = r[1];
    r[1] = r[2];
    r[2] = tmp;
  }
  idxMeta(in, mode, args...);
}

/// \brief Dernier appel recursif de idxMeta
/// \warning teste uniquement sur des processeurs Intel!!!
template<>
void idxMeta(std::ifstream& in, int mode, std::size_t& n)
{
  char* r = nullptr;
  r = (char*)&n;
  //On lit les 4 premiers octets
  in.read(r, 4);
  //Sur Intel, on les inverses
  if(mode == 0)
  {
    char tmp;
    tmp = r[0];
    r[0] = r[3];
    r[3] = tmp;
    tmp = r[1];
    r[1] = r[2];
    r[2] = tmp;
  }
}

/// \brief On charge dans une entree-sortie de reseau le prochain element qui se trouve dans un fichier IDX
/// \param fin Flux du fichier IDX a lire
/// \param vout Reference vers l'entree-sortie de reseau dans laquelle on charge le prochain element du fichier
/// \param size Taille du prochain element a lire
/// \return Rien
/// \tparam t Type de l'entree-sortie de reseau dans laquelle on charge le prochain element du fichier IDX
/// \tparam SIZE Taille de l'entree-sortie de reseau dans laquelle on charge le prochain element du fichier IDX
/// \warning teste uniquement sur des processeurs Intel!!!
template<typename t, int SIZE>
void nextIdx(std::ifstream& fin, neuralnetwork::InOut<t, SIZE>& vout, std::size_t size)
{
  for (int i = 0; i < size; ++i)
  {
    unsigned char r;
    fin.read((char*)&r, 1);
    vout(i) = r;
  }
}

/// \brief Conversion d'un label entier en label vecteur pour la classification de caractere numerique
/// \param in Label entier
/// \param out Label vecteur en sortie
/// \return Rien
/// \tparam t Type d'entree du reseau (a priori un double)
/// \tparam INPUT_SIZE Taille d'entree du reseau
template<typename t, int INPUT_SIZE>
void labelToVector(int in, neuralnetwork::InOut<t, INPUT_SIZE>& out)
{
    out << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
    out(in) = 1;
}

/// \brief Affiche une entree-sortie de reseau contenant une images de MNIST (principalement utile au debugage)
/// \param rows Nombre de lignes de l'image (a priori 28)
/// \param cols Nombre de colonnes de l'image (a priori 28)
/// \param v Entree-sortie de reseau a afficher
/// \return Rien
/// \tparam t Type de l'entree-sortie de reseau a afficher
/// \tparam SIZE Taille de l'entree-sortie a afficher
template<typename t, int SIZE>
void printInOut(std::size_t rows, std::size_t cols, neuralnetwork::InOut<t, SIZE> v)
{
  for(int i = 0; i < rows; ++i)
  {
    for(int j = 0; j < cols; ++j)
    {
      if (v(i*rows + j) != 0)
      {
        std::cout << BLACK;
      }
      else
      {
        std::cout << WHITE;
      }
    }
    std::cout << std::endl;
  }
}

/// \brief Entrainement du reseau pour la classification de caracteres
/// \param pathImages Pointeur vers la chaine de caractere contenant le nom de fichier des images d'entrainement
/// \param pathLabels Pointeur vers la chaine de caractere contenant le nom de fichier des labels d'entrainement
/// \param net Reseau a entrainer
/// \param step Pas d'apprentissage
/// \param nbIter Nombre d'iterations a effectuer sur l'ensemble des images d'entrainement
/// \param nbImages Nombre d'images a charger en memoire simultanement
/// \return -1 en cas de probleme d'allocation memoires (trop d'images a charger simultanement)
/// \tparam DEPTH Profondeur du reseau de neurones
template<std::size_t DEPTH>
int train(char* pathImages, char* pathLabels, neuralnetwork::Perceptron<DEPTH>& net, double step, std::size_t nbIter, std::size_t nbImages)
{

  std::size_t magicImTrain = 0, magicLabTrain = 0;
  std::size_t nbImagesTrain = 0;
  std::size_t rows = 0;
  std::size_t cols = 0;
  neuralnetwork::InOut<double, 784>* inIm = NULL;
  neuralnetwork::InOut<double, 1>* inLab = NULL;
  neuralnetwork::InOut<double, 10>* inLabVect = NULL;
  int mode = idxEndianness(pathLabels);
  //Si on a 0 en nombre d'exemples a charger en memoire on chargera tous les exemples de la base de donnees
  if(nbImages == 0)
  {
    std::ifstream flabels(pathLabels, std::ifstream::in);
    idxMeta(flabels, mode, magicLabTrain, nbImagesTrain);
    nbImages = nbImagesTrain;
    flabels.close();
  }

  inIm = new neuralnetwork::InOut<double, 784> [nbImages];
  inLab = new neuralnetwork::InOut<double, 1> [nbImages];
  inLabVect = new neuralnetwork::InOut<double, 10> [nbImages];
  if(inIm == NULL || inLab == NULL || inLabVect == NULL)
  {
    return -1;
  }

  //On entraine autant de fois que voulu
  for(int iter = 0; iter < nbIter; ++iter)
  {
    std::cout << "Iteration [" << iter << "/" << nbIter << "]" << std::endl;
    std::ifstream fimages(pathImages, std::ifstream::in);
    std::ifstream flabels(pathLabels, std::ifstream::in);
    idxMeta(flabels, mode, magicLabTrain, nbImagesTrain);
    idxMeta(fimages, mode, magicImTrain, nbImagesTrain, rows, cols);
    //Tant que l'on a pas fait tous les exemples
    for(int n = 0; n < nbImagesTrain; n+=nbImages)
    {
      std::cout << "Iteration [" << n << "/" << nbImagesTrain << "]" << std::endl;
      //On charge les exemples et les labels
      for(int i = 0; i < nbImages; ++i)
      {
        //On charge l'exemple et le label
        nextIdx(fimages, inIm[i], 784);
        nextIdx(flabels, inLab[i], 1);
        int valLab = inLab[i](0);
        //Conversion du label en vecteur
        labelToVector(valLab, inLabVect[i]);
      }
    //std::cout << "Iteration " << iter + 1 << ", entrainement avec les image de " << n << " a " << n+nbImages << "..." << std::endl;
    //On entraine sur le lot charge
    net.parallel_backpropagation(inIm, inLabVect, nbImages, step);
    //net.backpropagation(inIm, inLabVect, nbImages, step);
    }

    fimages.close();
    flabels.close();
  }

  delete [] inIm;
  delete [] inLab;
  delete [] inLabVect;

}

/// \brief Test du reseau pour la classification de caracteres
/// \param pathImages Pointeur vers la chaine de caractere contenant le nom de fichier des images de test
/// \param pathLabels Pointeur vers la chaine de caractere contenant le nom de fichier des labels de test
/// \param net Reseau a tester
/// \return Pourcentage de reussite
/// \tparam DEPTH Profondeur du reseau de neurones
template<std::size_t DEPTH>
double test(char* pathImages, char* pathLabels, neuralnetwork::Perceptron<DEPTH>& net)
{
  std::size_t magicImTrain = 0, magicLabTrain = 0, magicImTest = 0, magicLabTest = 0;
  std::size_t nbImagesTrain = 0, nbImagesTest = 0;
  std::size_t rows = 0;
  std::size_t cols = 0;
  neuralnetwork::InOut<double, 784> inImTest;
  neuralnetwork::InOut<double, 1> inLabTest;
  int mode = idxEndianness(pathLabels);
  std::ifstream fimagesTest(pathImages, std::ifstream::in);
  std::ifstream flabelsTest(pathLabels, std::ifstream::in);

  idxMeta(flabelsTest, mode, magicLabTest, nbImagesTest);
  idxMeta(fimagesTest, mode, magicImTest, nbImagesTest, rows, cols);

  int count = 0;
  //Pour tous les exemples de tests
  for(int i = 0; i < 10000; ++i)
  {
    neuralnetwork::InOut<double, 10> out;
    neuralnetwork::InOut<double, 10> realOut;

    //On charge les exemples avec leurs labels
    nextIdx(fimagesTest, inImTest, 784);
    nextIdx(flabelsTest, inLabTest, 1);

    //On convertit le label en vecteur
    labelToVector(inLabTest(0), realOut);

    //On fait evaluer l'exemple par le reseau de neurones
    //net.feedForward(inImTest, out, false, false);
    net.parallel_feedForward(inImTest, out);

    //Pour chaque element i du vecteur de sortie, s'il est superieur a 0.5, on estime que le reseau a trouve que l'exemple est i - 1
    //Par exemple si le reseau renvoi [0.12, 0.23, 0.87, 0.34, 0.45, 0.47, 0.37, 0.28, 0.19 , 0.09] alors on interprete que le solution trouvee est [0, 0, 1, 0, 0, 0, 0, 0, 0, 0] et donc que le nombre trouve est 2
    for(int j = 0; j < 10; ++j)
    {
      if(out(j) > 0.5)
      {
        out(j) = 1.0;
      }
      else
      {
        out(j) = 0.0;
      }
    }
    //Si la bonne solution est trouvee, alors on incremente le coompteur
    if(out == realOut)
    {
      ++count;
    }
  }

  //On retourne le pourcentage de reussite
  return count*100.0/10000.0;

}

/// \brief Itere un certain nombre de fois sur la base d'entrainement et apres chaque iteration test le reseau sur la base de test, le taux de reussite de chaque test est stocke dans un fichier
/// \param pathImTrain Pointeur vers la chaine de caractere contenant le nom de fichier des images d'entrainement
/// \param pathLabTrain Pointeur vers la chaine de caractere contenant le nom de fichier des labels d'entrainement
/// \param pathImTest Pointeur vers la chaine de caractere contenant le nom de fichier des images de test
/// \param pathLabTest Pointeur vers la chaine de caractere contenant le nom de fichier des labels de test
/// \param plot Pointeur vers la chaine de caractere contenant le nom de fichier dans lequel stocker les resultats
/// \param net Reseau a entrainer
/// \param step Pas d'apprentissage
/// \param nbIter Nombre d'iterations a effectuer sur l'ensemble des images d'entrainement
/// \param nbImages Nombre d'images a charger en memoire simultanement
/// \return Rien
/// \tparam DEPTH Profondeur du reseau de neurones
template<std::size_t DEPTH>
void bench(char* pathImTrain, char* pathLabTrain, char* pathImTest, char* pathLabTest, char* plot, neuralnetwork::Perceptron<DEPTH>& net, double step, std::size_t nbIter, std::size_t nbImages)
{
  std::ofstream fplot(plot, std::ofstream::out);
  for(int i = 0; i < nbIter; ++i)
  {
    train(pathImTrain, pathLabTrain, net, step, 1, nbImages);
    double resTest = test(pathImTest, pathLabTest, net);
    fplot << resTest << std::endl;
  }
}

int main()
{
  char pathImTrain [] = "../datas/imTrain";
  char pathLabTrain [] = "../datas/labTrain";
  char pathImTest [] = "../datas/imTest";
  char pathLabTest [] = "../datas/labTest";
  char pathPlot [] = "../benchmarks/plot.txt";

  //tbb::task_scheduler_init init(4);

  neuralnetwork::Perceptron<4>  net(
    -0.0001,
    0.0001,
    784,
    200, neuralnetwork::activation::SIGMOID, neuralnetwork::activation::D_SIGMOID,
    200, neuralnetwork::activation::SIGMOID, neuralnetwork::activation::D_SIGMOID,
    200, neuralnetwork::activation::SIGMOID, neuralnetwork::activation::D_SIGMOID,
    10, neuralnetwork::activation::SIGMOID, neuralnetwork::activation::D_SIGMOID
  );
    net.initGraph(0.1);
    train(pathImTrain, pathLabTrain,  net, 0.1, 50, 1000);
    double res = test(pathImTest, pathLabTest, net);
    std::cout << "reussite de " << res << " pourcents apres entrainement" << std::endl;

  return 0;
}


#include "ter.h"

#include <cstdio>
#include <cassert>
#include <iostream>
#include <limits>
#include <sstream>
#include <tr1/unordered_map>
#include <set>
#include <valarray>
#include <boost/functional/hash.hpp>
#include <stdexcept>
#include "tdict.h"

const bool ter_use_average_ref_len = true;
const int ter_short_circuit_long_sentences = -1;

using namespace std;
using namespace std::tr1;

struct COSTS {
  static const float substitution;
  static const float deletion;
  static const float insertion;
  static const float shift;
};
const float COSTS::substitution = 1.0f;
const float COSTS::deletion = 1.0f;
const float COSTS::insertion = 1.0f;
const float COSTS::shift = 1.0f;

static const int MAX_SHIFT_SIZE = 10;
static const int MAX_SHIFT_DIST = 50;

struct Shift {
  unsigned int d_;
  Shift() : d_() {}
  Shift(int b, int e, int m) : d_() {
    begin(b);
    end(e);
    moveto(m);
  }
  inline int begin() const {
    return d_ & 0x3ff;
  }
  inline int end() const {
    return (d_ >> 10) & 0x3ff;
  }
  inline int moveto() const {
    int m = (d_ >> 20) & 0x7ff;
    if (m > 1024) { m -= 1024; m *= -1; }
    return m;
  }
  inline void begin(int b) {
    d_ &= 0xfffffc00u;
    d_ |= (b & 0x3ff);
  }
  inline void end(int e) {
    d_ &= 0xfff003ffu;
    d_ |= (e & 0x3ff) << 10;
  }
  inline void moveto(int m) {
    bool neg = (m < 0);
    if (neg) { m *= -1; m += 1024; }
    d_ &= 0xfffff;
    d_ |= (m & 0x7ff) << 20;
  }
};

class TERScorerImpl {

 public:
  enum TransType { MATCH, SUBSTITUTION, INSERTION, DELETION };

  explicit TERScorerImpl(const vector<WordID>& ref) : ref_(ref) {
    for (int i = 0; i < ref.size(); ++i)
      rwexists_.insert(ref[i]);
  }

  float Calculate(const vector<WordID>& hyp, int* subs, int* ins, int* dels, int* shifts) const {
    return CalculateAllShifts(hyp, subs, ins, dels, shifts);
  }

  inline int GetRefLength() const {
    return ref_.size();
  }

 private:
  vector<WordID> ref_;
  set<WordID> rwexists_;

  typedef unordered_map<vector<WordID>, set<int>, boost::hash<vector<WordID> > > NgramToIntsMap;
  mutable NgramToIntsMap nmap_;

  static float MinimumEditDistance(
      const vector<WordID>& hyp,
      const vector<WordID>& ref,
      vector<TransType>* path) {
    vector<vector<TransType> > bmat(hyp.size() + 1, vector<TransType>(ref.size() + 1, MATCH));
    vector<vector<float> > cmat(hyp.size() + 1, vector<float>(ref.size() + 1, 0));
    for (int i = 0; i <= hyp.size(); ++i)
      cmat[i][0] = i;
    for (int j = 0; j <= ref.size(); ++j)
      cmat[0][j] = j;
    for (int i = 1; i <= hyp.size(); ++i) {
      const WordID& hw = hyp[i-1];
      for (int j = 1; j <= ref.size(); ++j) {
        const WordID& rw = ref[j-1];
	float& cur_c = cmat[i][j];
	TransType& cur_b = bmat[i][j];

        if (rw == hw) {
          cur_c = cmat[i-1][j-1];
          cur_b = MATCH;
        } else {
          cur_c = cmat[i-1][j-1] + COSTS::substitution;
          cur_b = SUBSTITUTION;
        }
	float cwoi = cmat[i-1][j];
        if (cur_c > cwoi + COSTS::insertion) {
          cur_c = cwoi + COSTS::insertion;
          cur_b = INSERTION;
        }
        float cwod = cmat[i][j-1];
        if (cur_c > cwod + COSTS::deletion) {
          cur_c = cwod + COSTS::deletion;
          cur_b = DELETION;
        }
      }
    }

    // trace back along the best path and record the transition types
    path->clear();
    int i = hyp.size();
    int j = ref.size();
    while (i > 0 || j > 0) {
      if (j == 0) {
        --i;
        path->push_back(INSERTION);
      } else if (i == 0) {
        --j;
        path->push_back(DELETION);
      } else {
        TransType t = bmat[i][j];
        path->push_back(t);
        switch (t) {
          case SUBSTITUTION:
          case MATCH:
            --i; --j; break;
          case INSERTION:
            --i; break;
          case DELETION:
            --j; break;
        }
      }
    }
    reverse(path->begin(), path->end());
    return cmat[hyp.size()][ref.size()];
  }

  void BuildWordMatches(const vector<WordID>& hyp, NgramToIntsMap* nmap) const {
    nmap->clear();
    set<WordID> exists_both;
    for (int i = 0; i < hyp.size(); ++i)
      if (rwexists_.find(hyp[i]) != rwexists_.end())
        exists_both.insert(hyp[i]);
    for (int start=0; start<ref_.size(); ++start) {
      if (exists_both.find(ref_[start]) == exists_both.end()) continue;
      vector<WordID> cp;
      int mlen = min(MAX_SHIFT_SIZE, static_cast<int>(ref_.size() - start));
      for (int len=0; len<mlen; ++len) {
        if (len && exists_both.find(ref_[start + len]) == exists_both.end()) break;
        cp.push_back(ref_[start + len]);
	(*nmap)[cp].insert(start);
      }
    }
  }

  static void PerformShift(const vector<WordID>& in,
    int start, int end, int moveto, vector<WordID>* out) {
    // cerr << "ps: " << start << " " << end << " " << moveto << endl;
    out->clear();
    if (moveto == -1) {
      for (int i = start; i <= end; ++i)
       out->push_back(in[i]);
      for (int i = 0; i < start; ++i)
       out->push_back(in[i]);
      for (int i = end+1; i < in.size(); ++i)
       out->push_back(in[i]);
    } else if (moveto < start) {
      for (int i = 0; i <= moveto; ++i)
       out->push_back(in[i]);
      for (int i = start; i <= end; ++i)
       out->push_back(in[i]);
      for (int i = moveto+1; i < start; ++i)
       out->push_back(in[i]);
      for (int i = end+1; i < in.size(); ++i)
       out->push_back(in[i]);
    } else if (moveto > end) {
      for (int i = 0; i < start; ++i)
       out->push_back(in[i]);
      for (int i = end+1; i <= moveto; ++i)
       out->push_back(in[i]);
      for (int i = start; i <= end; ++i)
       out->push_back(in[i]);
      for (int i = moveto+1; i < in.size(); ++i)
       out->push_back(in[i]);
    } else {
      for (int i = 0; i < start; ++i)
       out->push_back(in[i]);
      for (int i = end+1; (i < in.size()) && (i <= end + (moveto - start)); ++i)
       out->push_back(in[i]);
      for (int i = start; i <= end; ++i)
       out->push_back(in[i]);
      for (int i = (end + (moveto - start))+1; i < in.size(); ++i)
       out->push_back(in[i]);
    }
    if (out->size() != in.size()) {
      cerr << "ps: " << start << " " << end << " " << moveto << endl;
      cerr << "in=" << TD::GetString(in) << endl;
      cerr << "out=" << TD::GetString(*out) << endl;
    }
    assert(out->size() == in.size());
    // cerr << "ps: " << TD::GetString(*out) << endl;
  }

  void GetAllPossibleShifts(const vector<WordID>& hyp,
      const vector<int>& ralign,
      const vector<bool>& herr,
      const vector<bool>& rerr,
      const int min_size,
      vector<vector<Shift> >* shifts) const {
    for (int start = 0; start < hyp.size(); ++start) {
      vector<WordID> cp(1, hyp[start]);
      NgramToIntsMap::iterator niter = nmap_.find(cp);
      if (niter == nmap_.end()) continue;
      bool ok = false;
      int moveto;
      for (set<int>::iterator i = niter->second.begin(); i != niter->second.end(); ++i) {
        moveto = *i;
        int rm = ralign[moveto];
        ok = (start != rm &&
              (rm - start) < MAX_SHIFT_DIST &&
              (start - rm - 1) < MAX_SHIFT_DIST);
        if (ok) break;
      }
      if (!ok) continue;
      cp.clear();
      for (int end = start + min_size - 1;
           ok && end < hyp.size() && end < (start + MAX_SHIFT_SIZE); ++end) {
        cp.push_back(hyp[end]);
	vector<Shift>& sshifts = (*shifts)[end - start];
        ok = false;
        NgramToIntsMap::iterator niter = nmap_.find(cp);
        if (niter == nmap_.end()) break;
        bool any_herr = false;
        for (int i = start; i <= end && !any_herr; ++i)
          any_herr = herr[i];
        if (!any_herr) {
          ok = true;
          continue;
        }
        for (set<int>::iterator mi = niter->second.begin();
             mi != niter->second.end(); ++mi) {
          int moveto = *mi;
	  int rm = ralign[moveto];
	  if (! ((rm != start) &&
	        ((rm < start) || (rm > end)) &&
		(rm - start <= MAX_SHIFT_DIST) &&
		((start - rm - 1) <= MAX_SHIFT_DIST))) continue;
          ok = true;
	  bool any_rerr = false;
	  for (int i = 0; (i <= end - start) && (!any_rerr); ++i)
            any_rerr = rerr[moveto+i];
	  if (!any_rerr) continue;
	  for (int roff = 0; roff <= (end - start); ++roff) {
	    int rmr = ralign[moveto+roff];
	    if ((start != rmr) && ((roff == 0) || (rmr != ralign[moveto])))
	      sshifts.push_back(Shift(start, end, moveto + roff));
	  }
        }
      }
    }
  }

  bool CalculateBestShift(const vector<WordID>& cur,
                          const vector<WordID>& hyp,
                          float curerr,
                          const vector<TransType>& path,
                          vector<WordID>* new_hyp,
                          float* newerr,
                          vector<TransType>* new_path) const {
    vector<bool> herr, rerr;
    vector<int> ralign;
    int hpos = -1;
    for (int i = 0; i < path.size(); ++i) {
      switch (path[i]) {
        case MATCH:
	  ++hpos;
	  herr.push_back(false);
	  rerr.push_back(false);
	  ralign.push_back(hpos);
          break;
        case SUBSTITUTION:
	  ++hpos;
	  herr.push_back(true);
	  rerr.push_back(true);
	  ralign.push_back(hpos);
          break;
        case INSERTION:
	  ++hpos;
	  herr.push_back(true);
          break;
	case DELETION:
	  rerr.push_back(true);
	  ralign.push_back(hpos);
          break;
      }
    }
#if 0
    cerr << "RALIGN: ";
    for (int i = 0; i < rerr.size(); ++i)
      cerr << ralign[i] << " ";
    cerr << endl;
    cerr << "RERR: ";
    for (int i = 0; i < rerr.size(); ++i)
      cerr << (bool)rerr[i] << " ";
    cerr << endl;
    cerr << "HERR: ";
    for (int i = 0; i < herr.size(); ++i)
      cerr << (bool)herr[i] << " ";
    cerr << endl;
#endif

    vector<vector<Shift> > shifts(MAX_SHIFT_SIZE + 1);
    GetAllPossibleShifts(cur, ralign, herr, rerr, 1, &shifts);
    float cur_best_shift_cost = 0;
    *newerr = curerr;
    vector<TransType> cur_best_path;
    vector<WordID> cur_best_hyp;

    bool res = false;
    for (int i = shifts.size() - 1; i >=0; --i) {
      float curfix = curerr - (cur_best_shift_cost + *newerr);
      float maxfix = 2.0f * (1 + i) - COSTS::shift;
      if ((curfix > maxfix) || ((cur_best_shift_cost == 0) && (curfix == maxfix))) break;
      for (int j = 0; j < shifts[i].size(); ++j) {
        const Shift& s = shifts[i][j];
	curfix = curerr - (cur_best_shift_cost + *newerr);
	maxfix = 2.0f * (1 + i) - COSTS::shift;  // TODO remove?
        if ((curfix > maxfix) || ((cur_best_shift_cost == 0) && (curfix == maxfix))) continue;
	vector<WordID> shifted(cur.size());
	PerformShift(cur, s.begin(), s.end(), ralign[s.moveto()], &shifted);
	vector<TransType> try_path;
	float try_cost = MinimumEditDistance(shifted, ref_, &try_path);
	float gain = (*newerr + cur_best_shift_cost) - (try_cost + COSTS::shift);
	if (gain > 0.0f || ((cur_best_shift_cost == 0.0f) && (gain == 0.0f))) {
	  *newerr = try_cost;
	  cur_best_shift_cost = COSTS::shift;
	  new_path->swap(try_path);
	  new_hyp->swap(shifted);
	  res = true;
	  // cerr << "Found better shift " << s.begin() << "..." << s.end() << " moveto " << s.moveto() << endl;
	}
      }
    }

    return res;
  }

  static void GetPathStats(const vector<TransType>& path, int* subs, int* ins, int* dels) {
    *subs = *ins = *dels = 0;
    for (int i = 0; i < path.size(); ++i) {
      switch (path[i]) {
        case SUBSTITUTION:
	  ++(*subs);
        case MATCH:
          break;
        case INSERTION:
          ++(*ins); break;
	case DELETION:
          ++(*dels); break;
      }
    }
  }

  float CalculateAllShifts(const vector<WordID>& hyp,
      int* subs, int* ins, int* dels, int* shifts) const {
    BuildWordMatches(hyp, &nmap_);
    vector<TransType> path;
    float med_cost = MinimumEditDistance(hyp, ref_, &path);
    float edits = 0;
    vector<WordID> cur = hyp;
    *shifts = 0;
    if (ter_short_circuit_long_sentences < 0 ||
        ref_.size() < ter_short_circuit_long_sentences) {
      while (true) {
        vector<WordID> new_hyp;
        vector<TransType> new_path;
        float new_med_cost;
        if (!CalculateBestShift(cur, hyp, med_cost, path, &new_hyp, &new_med_cost, &new_path))
          break;
        edits += COSTS::shift;
        ++(*shifts);
        med_cost = new_med_cost;
        path.swap(new_path);
        cur.swap(new_hyp);
      }
    }
    GetPathStats(path, subs, ins, dels);
    return med_cost + edits;
  }
};

class TERScore : public ScoreBase<TERScore> {
  friend class TERScorer;

 public:
  static const unsigned kINSERTIONS = 0;
  static const unsigned kDELETIONS = 1;
  static const unsigned kSUBSTITUTIONS = 2;
  static const unsigned kSHIFTS = 3;
  static const unsigned kREF_WORDCOUNT = 4;
  static const unsigned kDUMMY_LAST_ENTRY = 5;

 TERScore() : stats(0,kDUMMY_LAST_ENTRY) {}
  float ComputePartialScore() const { return 0.0;}
  float ComputeScore() const {
    float edits = static_cast<float>(stats[kINSERTIONS] + stats[kDELETIONS] + stats[kSUBSTITUTIONS] + stats[kSHIFTS]);
    return edits / static_cast<float>(stats[kREF_WORDCOUNT]);
  }
  void ScoreDetails(string* details) const;
  void PlusPartialEquals(const Score& rhs, int oracle_e_cover, int oracle_f_cover, int src_len){}
  void PlusEquals(const Score& delta, const float scale) {
    if (scale==1)
      stats += static_cast<const TERScore&>(delta).stats;
    if (scale==-1)
      stats -= static_cast<const TERScore&>(delta).stats;
    throw std::runtime_error("TERScore::PlusEquals with scale != +-1");
 }
  void PlusEquals(const Score& delta) {
    stats += static_cast<const TERScore&>(delta).stats;
  }

  ScoreP GetZero() const {
    return ScoreP(new TERScore);
  }
  ScoreP GetOne() const {
    return ScoreP(new TERScore);
  }
  void Subtract(const Score& rhs, Score* res) const {
    static_cast<TERScore*>(res)->stats = stats - static_cast<const TERScore&>(rhs).stats;
  }
  void Encode(std::string* out) const {
    ostringstream os;
    os << stats[kINSERTIONS] << ' '
       << stats[kDELETIONS] << ' '
       << stats[kSUBSTITUTIONS] << ' '
       << stats[kSHIFTS] << ' '
       << stats[kREF_WORDCOUNT];
    *out = os.str();
  }
  bool IsAdditiveIdentity() const {
    for (int i = 0; i < kDUMMY_LAST_ENTRY; ++i)
      if (stats[i] != 0) return false;
    return true;
  }
 private:
  valarray<int> stats;
};

ScoreP TERScorer::ScoreFromString(const std::string& data) {
  istringstream is(data);
  TERScore* r = new TERScore;
  is >> r->stats[TERScore::kINSERTIONS]
     >> r->stats[TERScore::kDELETIONS]
     >> r->stats[TERScore::kSUBSTITUTIONS]
     >> r->stats[TERScore::kSHIFTS]
     >> r->stats[TERScore::kREF_WORDCOUNT];
  return ScoreP(r);
}

void TERScore::ScoreDetails(std::string* details) const {
  char buf[200];
  sprintf(buf, "TER = %.2f, %3d|%3d|%3d|%3d (len=%d)",
     ComputeScore() * 100.0f,
     stats[kINSERTIONS],
     stats[kDELETIONS],
     stats[kSUBSTITUTIONS],
     stats[kSHIFTS],
     stats[kREF_WORDCOUNT]);
  *details = buf;
}

TERScorer::~TERScorer() {
  for (vector<TERScorerImpl*>::iterator i = impl_.begin(); i != impl_.end(); ++i)
    delete *i;
}

TERScorer::TERScorer(const vector<vector<WordID> >& refs) : impl_(refs.size()) {
  for (int i = 0; i < refs.size(); ++i)
    impl_[i] = new TERScorerImpl(refs[i]);
}

ScoreP TERScorer::ScoreCCandidate(const vector<WordID>& hyp) const {
  return ScoreP();
}

ScoreP TERScorer::ScoreCandidate(const std::vector<WordID>& hyp) const {
  float best_score = numeric_limits<float>::max();
  TERScore* res = new TERScore;
  int avg_len = 0;
  for (int i = 0; i < impl_.size(); ++i)
    avg_len += impl_[i]->GetRefLength();
  avg_len /= impl_.size();
  for (int i = 0; i < impl_.size(); ++i) {
    int subs, ins, dels, shifts;
    float score = impl_[i]->Calculate(hyp, &subs, &ins, &dels, &shifts);
    // cerr << "Component TER cost: " << score << endl;
    if (score < best_score) {
      res->stats[TERScore::kINSERTIONS] = ins;
      res->stats[TERScore::kDELETIONS] = dels;
      res->stats[TERScore::kSUBSTITUTIONS] = subs;
      res->stats[TERScore::kSHIFTS] = shifts;
      if (ter_use_average_ref_len) {
        res->stats[TERScore::kREF_WORDCOUNT] = avg_len;
      } else {
        res->stats[TERScore::kREF_WORDCOUNT] = impl_[i]->GetRefLength();
      }

      best_score = score;
    }
  }
  return ScoreP(res);
}


#include "CoordinateMatrixAnalysis.h"

#include "CoordinateMatrix.h"
#include <macgyver/Exception.h>

#include <boost/optional.hpp>

#include <cmath>
#include <iomanip>
#include <iostream>

namespace Fmi
{
namespace
{
// We discard any grid cell whose bbox exceeds 1000 kilometers in size

const double cell_size_limit = 1000 * 1000;

enum class Handedness
{
  ClockwiseConvex,
  CounterClockwiseConvex,
  Invalid,
  NotConvex,
  Huge,
  Oblong
};

// ----------------------------------------------------------------------
/*!
 * A polygon is convex if all cross products of adjacent edges are of the same sign,
 * and the sign itself says whether the polygon is clockwise or not.
 * Ref: http://www.easywms.com/node/3602
 * We must disallow non-convex cells such as V-shaped ones, since the intersections
 * formulas may then produce values outside the cell.
 *
 * Note that we permit colinear adjacent edges, since for example in latlon grids
 * the poles are represented by multiple grid points. This test thus passes the
 * redundant case of a rectangle with no area, but the rest of the code can handle it.
 * empty boolean for such grid cells.
 */
// ----------------------------------------------------------------------

Handedness analyze_cell(
    double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
{
  try
  {
    // Disallow cells with any invalid coordinates
    if (std::isnan(x1) || std::isnan(y1) || std::isnan(x2) || std::isnan(y2) || std::isnan(x3) ||
        std::isnan(y3) || std::isnan(x4) || std::isnan(y4))
      return Handedness::Invalid;

    // Check for oblong cells which typically occur at projection discontinuities. One could
    // use the Polsby-Popper test here, but calculating the edge lengths in addition to the
    // areas below would be slower tahn simply testing the cell bbox

    const auto xmin = std::min(std::min(x1, x2), std::min(x3, x4));
    const auto xmax = std::max(std::max(x1, x2), std::max(x3, x4));
    const auto ymin = std::min(std::min(y1, y2), std::min(y3, y4));
    const auto ymax = std::max(std::max(y1, y2), std::max(y3, y4));

    const auto dx = xmax - xmin;
    const auto dy = ymax - ymin;

    // Empty cell?
    if (dx == 0 || dy == 0)
      return Handedness::Invalid;

    // Huge cell? (most likely due to projection instabilities)
    if (dx >= cell_size_limit || dy >= cell_size_limit)
      return Handedness::Huge;

    const auto ratio = dy / dx;

    if (ratio < 0.01 || ratio > 100)
      return Handedness::Oblong;

    // Check for convexness and orientation

    const auto area1 = (x2 - x1) * (y3 - y2) - (y2 - y1) * (x3 - x2);
    const auto area2 = (x3 - x2) * (y4 - y3) - (y3 - y2) * (x4 - x3);
    const auto area3 = (x4 - x3) * (y1 - y4) - (y4 - y3) * (x1 - x4);
    const auto area4 = (x1 - x4) * (y2 - y1) - (y1 - y4) * (x2 - x1);

    if (area1 <= 0 && area2 <= 0 && area3 <= 0 && area4 <= 0)
      return Handedness::ClockwiseConvex;

    if (area1 >= 0 && area2 >= 0 && area3 >= 0 && area4 >= 0)
      return Handedness::CounterClockwiseConvex;

    return Handedness::NotConvex;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace

CoordinateAnalysis analysis(const CoordinateMatrix& coords)
{
  try
  {
    const auto nx = coords.width();
    const auto ny = coords.height();

    BoolMatrix valid(nx - 1, ny - 1, true);
    BoolMatrix clockwise(nx - 1, ny - 1, false);

    std::size_t cw = 0;
    std::size_t ccw = 0;
    std::size_t bad = 0;

#if 0
    std::size_t huge = 0;
    std::size_t oblong = 0;
    std::size_t notconvex = 0;
#endif

    // Go through the coordinates once

    for (std::size_t j = 0; j < ny - 1; j++)
      for (std::size_t i = 0; i < nx - 1; i++)
      {
        auto hand = analyze_cell(coords.x(i, j),
                                 coords.y(i, j),
                                 coords.x(i, j + 1),
                                 coords.y(i, j + 1),
                                 coords.x(i + 1, j + 1),
                                 coords.y(i + 1, j + 1),
                                 coords.x(i + 1, j),
                                 coords.y(i + 1, j));

        if (hand == Handedness::ClockwiseConvex)
        {
          clockwise.set(i, j, true);
          ++cw;
        }
        else if (hand == Handedness::CounterClockwiseConvex)
        {
          // clockwise.set(i, j, false);
          ++ccw;
        }
        else
        {
          valid.set(i, j, false);
          ++bad;
#if 0
          if (hand == Handedness::Huge)
            ++huge;
          else if (hand == Handedness::Oblong)
            ++oblong;
          else if (hand == Handedness::NotConvex)
            ++notconvex;
#endif
        }
      }

    // The coordinates are likely to be upside done if there are many more CCW cells than CW cells

    bool needs_flipping = (ccw > 2 * cw);

    return CoordinateAnalysis{valid, clockwise, needs_flipping};
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace Fmi


/*
* LEGAL NOTICE
* This computer software was prepared by Battelle Memorial Institute,
* hereinafter the Contractor, under Contract No. DE-AC05-76RL0 1830
* with the Department of Energy (DOE). NEITHER THE GOVERNMENT NOR THE
* CONTRACTOR MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY
* LIABILITY FOR THE USE OF THIS SOFTWARE. This notice including this
* sentence must appear on any copies of this computer software.
*
* EXPORT CONTROL
* User agrees that the Software will not be shipped, transferred or
* exported into any country or used in any manner prohibited by the
* United States Export Administration Act or any other applicable
* export laws, restrictions or regulations (collectively the "Export Laws").
* Export of the Software may require some form of license or other
* authority from the U.S. Government, and failure to obtain such
* export control license may result in criminal liability under
* U.S. laws. In addition, if the Software is identified as export controlled
* items under the Export Laws, User represents and warrants that User
* is not a citizen, or otherwise located within, an embargoed nation
* (including without limitation Iran, Syria, Sudan, Cuba, and North Korea)
*     and that User is not otherwise prohibited
* under the Export Laws from receiving the Software.
*
* Copyright 2011 Battelle Memorial Institute.  All Rights Reserved.
* Distributed as open-source under the terms of the Educational Community
* License version 2.0 (ECL 2.0). http://www.opensource.org/licenses/ecl2.php
*
* For further details, see: http://www.globalchange.umd.edu/models/gcam/
*
*/

#if USE_LAPACK
/* This whole file is used only if we are using lapack */
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>

#include "solution/util/include/ublas-helpers.hpp"

#include "solution/util/include/svd_invert_solve.hpp"
#include "util/base/include/util.h"

namespace ublas = boost::numeric::ublas;

// Solve U*S*VT * x = b,
// Where U,S,VT are the SVD of a matrix.  x will replace b on output
int svdInvertSolve(const ublas::matrix<double,ublas::column_major> &U,
                     const ublas::vector<double> &S,
                     const ublas::matrix<double,ublas::column_major> &VT,
                     ublas::vector<double> &b,
                     std::ostream &logf)
{
  const double small = 1.0e-8;
  int nsing = 0;
  ublas::matrix<double,ublas::column_major> Utmp(U.size2(),U.size1()), Vtmp(VT.size2(),VT.size1());
  ublas::matrix<double,ublas::column_major> Ainv(Vtmp.size1(),Utmp.size2());
  ublas::vector<double> tmpvec(Vtmp.size2()); // able to store a row of VT (== a column of V)

  Utmp = ublas::trans(U);
  Vtmp = ublas::trans(VT);

  int nrow = Utmp.size1();
  int ncol = Utmp.size2();

  logf << "Largest singular value = " << S[0] << "\n";

  // multiply Utmp from the left by the inverse of the diagonal matrix S
  double singthresh = S[0]*small;
  for(int i=0;i<nrow;++i) {
    double sinv = S[i];
    if(sinv < singthresh) {
      // Note that the elements of S are all >=0.  For "sufficiently
      // small" singular values we set 1/s = 1/singthresh and log the singular
      // component
      double s= sinv;           // store for output without moving the meat of this branch down below all the output
      nsing++;
      sinv = 1.0/singthresh;    // keep the multiplier on this singular component "reasonable".

      // log the singular component (everything between here and the
      // marker below is strictly diagnostic and not necessary for the
      // algorithm.)

      // the columns of V (== trans(VT)) give the basis vectors for the nullspace
      int kmax = 0;
      double vtmax = fabs(Vtmp(0,i));
      for(int k=0; k<Vtmp.size1(); ++k) {
        tmpvec[k] = Vtmp(k,i);  // column i, kth entry
        double vtabs = fabs(tmpvec[k]);
        if(vtabs > vtmax) {
          vtmax = vtabs;
          kmax = k;
        }
      }
      // suppress printing of small values.
      double prnthresh = 1.0e-4*vtmax;
      for(int k=0; k<tmpvec.size(); ++k)
        if(fabs(tmpvec[k]) < prnthresh)
          tmpvec[k] = 0.0;

      logf << "Singular component:  s= " << s
           << "  kmax= " << kmax << "  vtval= " << vtmax
           << "  sinv set to: " << sinv
           << "\n";
      logf << tmpvec << "\n";
      // end of singular component logging
    }
    else {
      sinv = 1.0/sinv;
    }
    for(int j=0;j<ncol;++j)
      Utmp(i,j) *= sinv;
  }

  Ainv = prod(Vtmp,Utmp);

  ublas::vector<double> bb(b);
  axpy_prod(Ainv,bb,b);

  return nsing;
}

#endif


#include "ScanMatch.h"
#include "common/feature_utils.h"
#include "common/math_utils.h"
#include "common/nanoflann_pcl.h"
#include "common/ros_utils.h"
#include "common/transform_utils.h"

#include <pcl/kdtree/kdtree_flann.h>

#include <Eigen/Eigenvalues>
#include <Eigen/QR>

namespace lidar_slam {

using std::sqrt;
using std::fabs;
using std::asin;
using std::atan2;
using std::pow;

ScanMatch::ScanMatch(const size_t maxIterations)
    : _maxIterations(maxIterations), _deltaTAbort(0.05), _deltaRAbort(0.05),
      _useScore(true), _match_count(0), _fail_match_count(0), _total_score(0),
      _score_threshold(800), _match_percentage_threshold(0.4),
      _referenceCornerCloudDS(new CloudI()),
      _referenceSurfCloudDS(new CloudI()), _CornerCloudDS(new CloudI()),
      _SurfCloudDS(new CloudI()) {

  _downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);
  _downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);

  _fineScore = false;
}

ScanMatch::~ScanMatch() {
  std::cout << "[ScanMatch]\n"
            << " ,match_count:" << _match_count
            << " ,fail_match_count:" << _fail_match_count
            << " ,averageScore:" << getAverageScore() << std::endl;
}

double ScanMatch::getScore(const CloudI &coeffCloud) {
  double score = 0;
  for (int i = 0; i < coeffCloud.points.size(); ++i) {
    const PointI &p = coeffCloud.points[i];
    score += std::exp(-fabs(p.intensity));
  }
  return score;
}

bool ScanMatch::scanMatchScan(const CloudIConPtr &referenceCornerCloud,
                              const CloudIConPtr &referenceSurfCloud,
                              const CloudIConPtr &CornerCloud,
                              const CloudIConPtr &SurfCloud,
                              Twist &transformf) {

  if (referenceCornerCloud->points.size() < 50 ||
      referenceSurfCloud->points.size() < 100) {
    std::cout << "reference cloud points too few." << std::endl;
    return false;
  }
  Twist transform = transformf;

  PointI pointSel, pointOri, pointProj, coeff;
  std::vector<int> pointSearchInd(5, 0);
  std::vector<float> pointSearchSqDis(5, 0);

    nanoflann::KdTreeFLANN<PointI> kdtreeCorner;
    nanoflann::KdTreeFLANN<PointI> kdtreeSurf;
      /*
  pcl::KdTreeFLANN<PointI> kdtreeCorner;
  pcl::KdTreeFLANN<PointI> kdtreeSurf;
  */

  kdtreeCorner.setInputCloud(referenceCornerCloud);
  kdtreeSurf.setInputCloud(referenceSurfCloud);

  bool converge = false;
  bool isDegenerate = false;
  Eigen::Matrix<float, 6, 6> matP;

  size_t CornerNum = CornerCloud->points.size();
  size_t SurfNum = SurfCloud->points.size();

  pcl::PointCloud<PointI> laserCloudOri;
  pcl::PointCloud<PointI> coeffSel;

  int line_match_count = 0;
  int plane_match_count = 0;
  size_t iterCount;
  for (iterCount = 0; iterCount < _maxIterations; iterCount++) {
    laserCloudOri.clear();
    coeffSel.clear();
    line_match_count = 0;
    plane_match_count = 0;

    for (int i = 0; i < CornerNum; i++) {
      pointOri = CornerCloud->points[i];
      pointAssociateToMap(transform, pointOri, pointSel);
      kdtreeCorner.nearestKSearch(pointSel, 5, pointSearchInd,
                                  pointSearchSqDis);
      if (pointSearchSqDis[4] < 5.0) {
        const Eigen::Vector3f &point = pointSel.getVector3fMap();
        Eigen::Vector3f lineA, lineB;
        if (findLine(*referenceCornerCloud, pointSearchInd, lineA, lineB)) {
          PointI coefficients;
          if (getCornerFeatureCoefficients(lineA, lineB, point, coefficients)) {
            laserCloudOri.push_back(pointOri);
            coeffSel.push_back(coefficients);
          }
          line_match_count++;
        }
      }
    }

    for (int i = 0; i < SurfNum; i++) {
      pointOri = SurfCloud->points[i];
      pointAssociateToMap(transform, pointOri, pointSel);
      kdtreeSurf.nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
      if (pointSearchSqDis[4] < 5.0) {
        Eigen::Vector4f planeCoef;
        if (findPlane(*referenceSurfCloud, pointSearchInd, 0.2, planeCoef)) {
          PointI coefficients;
          if (getSurfaceFeatureCoefficients(planeCoef, pointSel,
                                            coefficients)) {
            laserCloudOri.push_back(pointOri);
            coeffSel.push_back(coefficients);
          }
          plane_match_count++;
        }
      }
    }

    float srx = transform.rot_x.sin();
    float crx = transform.rot_x.cos();
    float sry = transform.rot_y.sin();
    float cry = transform.rot_y.cos();
    float srz = transform.rot_z.sin();
    float crz = transform.rot_z.cos();

    size_t laserCloudSelNum = laserCloudOri.points.size();
    if (laserCloudSelNum < 50) {
      ROS_WARN("matched cloud points too few. Matched/Input:  %d / %d", laserCloudSelNum, CornerNum+SurfNum);
      break;
    }

    Eigen::Matrix<float, Eigen::Dynamic, 6> matA(laserCloudSelNum, 6);
    Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, laserCloudSelNum);
    Eigen::Matrix<float, 6, 6> matAtA;
    Eigen::VectorXf matB(laserCloudSelNum);
    Eigen::VectorXf matAtB;
    Eigen::VectorXf matX;

    for (int i = 0; i < laserCloudSelNum; i++) {
      pointOri = laserCloudOri.points[i];
      coeff = coeffSel.points[i];
      /*
      float arx = (crx * sry * srz * pointOri.x + crx * crz * sry * pointOri.y -
                   srx * sry * pointOri.z) *
                      coeff.x +
                  (-srx * srz * pointOri.x - crz * srx * pointOri.y -
                   crx * pointOri.z) *
                      coeff.y +
                  (crx * cry * srz * pointOri.x + crx * cry * crz * pointOri.y -
                   cry * srx * pointOri.z) *
                      coeff.z;

      float ary = ((cry * srx * srz - crz * sry) * pointOri.x +
                   (sry * srz + cry * crz * srx) * pointOri.y +
                   crx * cry * pointOri.z) *
                      coeff.x +
                  ((-cry * crz - srx * sry * srz) * pointOri.x +
                   (cry * srz - crz * srx * sry) * pointOri.y -
                   crx * sry * pointOri.z) *
                      coeff.z;

      float arz = ((crz * srx * sry - cry * srz) * pointOri.x +
                   (-cry * crz - srx * sry * srz) * pointOri.y) *
                      coeff.x +
                  (crx * crz * pointOri.x - crx * srz * pointOri.y) * coeff.y +
                  ((sry * srz + cry * crz * srx) * pointOri.x +
                   (crz * sry - cry * srx * srz) * pointOri.y) *
                      coeff.z;
        */
        float arx = ((crz*sry*crx + srz*srx)* pointOri.y +(srz*crx-crz*sry*srx)* pointOri.z)*coeff.x +
        ((srz*sry*crx-crz*srx)*pointOri.y -(srz*sry*srx+crz*crx)*pointOri.z)*coeff.y +
        (cry*crx*pointOri.y-cry*srx*pointOri.z)*coeff.z;

        float ary = (-crz*sry*pointOri.x+crz*cry*srx*pointOri.y+crz*cry*crx*pointOri.z)*coeff.x +
        (-srz*sry*pointOri.x+srz*cry*srx*pointOri.y +srz*cry*crx*pointOri.z)*coeff.y +
        (-cry*pointOri.x-sry*srx*pointOri.y-sry*crx*pointOri.z)*coeff.z;

        float arz = (-srz*cry*pointOri.x -(srz*sry*srx+crz*crx)*pointOri.y+(crz*srx-srz*sry*crx)*pointOri.z)*coeff.x+
        (crz*cry*pointOri.x+ (crz*sry*srx-srz*crx)*pointOri.y+crz*sry*crx+srz*srx*pointOri.z)*coeff.y+
        0*coeff.z;

      matA(i, 0) = arx;
      matA(i, 1) = ary;
      matA(i, 2) = arz;
      matA(i, 3) = coeff.x;
      matA(i, 4) = coeff.y;
      matA(i, 5) = coeff.z;
      matB(i, 0) = -coeff.intensity;
    }

    matAt = matA.transpose();
    matAtA = matAt * matA;
    matAtB = matAt * matB;
    matX = matAtA.colPivHouseholderQr().solve(matAtB);

    if (iterCount == 0) {
      Eigen::Matrix<float, 1, 6> matE;
      Eigen::Matrix<float, 6, 6> matV;
      Eigen::Matrix<float, 6, 6> matV2;

      Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 6, 6>> esolver(matAtA);
      matE = esolver.eigenvalues().real();
      matV = esolver.eigenvectors().real();

      matV2 = matV;

      isDegenerate = false;
      float eignThre[6] = {100, 100, 100, 100, 100, 100};
      for (int i = 0; i < 6; i++) {
        if (matE(0, i) < eignThre[i]) {
          for (int j = 0; j < 6; j++) {
            matV2(i, j) = 0;
          }
          isDegenerate = true;
        } else {
          break;
        }
      }
      matP = matV.inverse() * matV2;
    }

    if (isDegenerate) {
      Eigen::Matrix<float, 6, 1> matX2(matX);
      matX = matP * matX2;
    }

    transform.rot_x += matX(0, 0);
    transform.rot_y += matX(1, 0);
    transform.rot_z += matX(2, 0);
    transform.pos.x() += matX(3, 0);
    transform.pos.y() += matX(4, 0);
    transform.pos.z() += matX(5, 0);

    float deltaR =
        sqrt(pow(rad2deg(matX(0, 0)), 2) + pow(rad2deg(matX(1, 0)), 2) +
             pow(rad2deg(matX(2, 0)), 2));
    float deltaT = sqrt(pow(matX(3, 0) * 100, 2) + pow(matX(4, 0) * 100, 2) +
                        pow(matX(5, 0) * 100, 2));

    // std::cout << "iterator:" << iterCount << std::endl;
    // transform.print();
    if (deltaR < _deltaRAbort && deltaT < _deltaTAbort) {
      converge = true;
      break;
    }
  }

  if (converge && _useScore) {

    double score = getScore(coeffSel);

    double match_count = line_match_count + plane_match_count;
    float percent = match_count / (CornerNum + SurfNum);
    std::cout << "scan match score:" << score << ",per:" << percent
              << std::endl;

    if (_fineScore) {
      laserCloudOri.clear();
      coeffSel.clear();
      line_match_count = 0;
      plane_match_count = 0;
      for (int i = 0; i < CornerNum; i++) {
        pointOri = CornerCloud->points[i];
        pointAssociateToMap(transform, pointOri, pointSel);
        kdtreeCorner.nearestKSearch(pointSel, 5, pointSearchInd,
                                    pointSearchSqDis);
        if (pointSearchSqDis[0] < 0.02) {
          const Eigen::Vector3f &point = pointSel.getVector3fMap();
          Eigen::Vector3f lineA, lineB;
          if (findLine(*referenceCornerCloud, pointSearchInd, lineA, lineB)) {
            PointI coefficients;
            if (getCornerFeatureCoefficients(lineA, lineB, point,
                                             coefficients)) {
              laserCloudOri.push_back(pointOri);
              coeffSel.push_back(coefficients);
            }
            line_match_count++;
          }
        }
      }

      for (int i = 0; i < SurfNum; i++) {
        pointOri = SurfCloud->points[i];
        pointAssociateToMap(transform, pointOri, pointSel);
        kdtreeSurf.nearestKSearch(pointSel, 5, pointSearchInd,
                                  pointSearchSqDis);
        if (pointSearchSqDis[0] < 0.05) {
          Eigen::Vector4f planeCoef;
          if (findPlane(*referenceSurfCloud, pointSearchInd, 0.2, planeCoef)) {
            PointI coefficients;
            if (getSurfaceFeatureCoefficients(planeCoef, pointSel,
                                              coefficients)) {
              laserCloudOri.push_back(pointOri);
              coeffSel.push_back(coefficients);
            }
            plane_match_count++;
          }
        }
      }

      double score2 = getScore(coeffSel);
      match_count = line_match_count + plane_match_count;
      float percent2 = match_count / (CornerNum + SurfNum);
      std::cout << "scan match score2:" << score2 << " ,per2:" << percent2
                << std::endl;
    }

    if (score < _score_threshold) {
      transformf = transform;
      _fail_match_count++;
      std::cout << "low score!!" << score << ",per:" << percent << std::endl;
      return false;
    }

    if (percent < _match_percentage_threshold) {
      transformf = transform;
      _fail_match_count++;
      std::cout << "low percent!!" << score << ",per:" << percent << std::endl;
      return false;
    }
    _total_score += score;
    _match_count++;
    transformf = transform;

    return true;
  }
  //??? should we take the result when not converge?
  transformf = transform;
  _fail_match_count++;

  return false;
}

bool ScanMatch::scanMatchScan(const CloudIConPtr &referenceCornerCloud,
                              const CloudIConPtr &referenceSurfCloud,
                              const CloudIConPtr &CornerCloud,
                              const CloudIConPtr &SurfCloud,
                              Eigen::Isometry3f &relative_pose) {
  Twist transform;
  convertTransform(relative_pose, transform);
  bool success = scanMatchScan(referenceCornerCloud, referenceSurfCloud,
                               CornerCloud, SurfCloud, transform);
  convertTransform(transform, relative_pose);
  return success;
}

bool ScanMatch::scanMatchLocal(const CloudIConPtr &referenceCornerCloud,
                               const CloudIConPtr &referenceSurfCloud,
                               const CloudIConPtr &CornerCloud,
                               const CloudIConPtr &SurfCloud,
                               Eigen::Isometry3f &relative_pose) {
  Twist transform;
  convertTransform(relative_pose, transform);
  bool success = scanMatchLocal(referenceCornerCloud, referenceSurfCloud,
                                CornerCloud, SurfCloud, transform);
  convertTransform(transform, relative_pose);
  return success;
}

bool ScanMatch::scanMatchLocal(const CloudIConPtr &referenceCornerCloud,
                               const CloudIConPtr &referenceSurfCloud,
                               const CloudIConPtr &CornerCloud,
                               const CloudIConPtr &SurfCloud,
                               Twist &transform) {
  _referenceCornerCloudDS->clear();
  _downSizeFilterCorner.setInputCloud(referenceCornerCloud);
  _downSizeFilterCorner.filter(*_referenceCornerCloudDS);

  _referenceSurfCloudDS->clear();
  _downSizeFilterSurf.setInputCloud(referenceSurfCloud);
  _downSizeFilterSurf.filter(*_referenceSurfCloudDS);

  _CornerCloudDS->clear();
  _downSizeFilterCorner.setInputCloud(CornerCloud);
  _downSizeFilterCorner.filter(*_CornerCloudDS);

  _SurfCloudDS->clear();
  _downSizeFilterSurf.setInputCloud(SurfCloud);
  _downSizeFilterSurf.filter(*_SurfCloudDS);

  return scanMatchScan(_referenceCornerCloudDS, _referenceSurfCloudDS,
                       _CornerCloudDS, _SurfCloudDS, transform);
}

} // end namespace lidar_slam


#pragma once
/**

   @file KinematicVessel.hpp
   @brief Kinematic planar vessel.

*/

#include <atomic>
#include <chrono>
#include <cinttypes>
#include <memory>

#include <boost/statechart/detail/memory.hpp>
#include <boost/statechart/fifo_scheduler.hpp>
#include <boost/statechart/event_base.hpp>

#include <yaml-cpp/yaml.h>

#ifdef _MSC_VER
#pragma warning(push, 0)
#endif
#include <dds/pub/Publisher.hpp>
#include <dds/sub/Subscriber.hpp>
#ifdef _MSC_VER
#pragma warning(pop)
#endif

#include <mimir/IAlgorithm.hpp>

namespace mimir
{
  namespace algorithm
  {

    /**
       @brief Kinematic planar vessel numerical simulation model.

       \rst

       This class implements a kinematic planar vessel that are controlled by inputs,
       which are velocities and rate of turn.

       .. math::
          :nowrap:

          \begin{align}
          \boldsymbol{p} = \begin{bmatrix}N\\E\end{bmatrix} &\in \mathbb{R}^2 &\text{position}\\
          \boldsymbol{\psi} \in \mathbb{S} &&\text{heading}\\
          \boldsymbol{v} = \begin{bmatrix}u\\v\end{bmatrix} &\in \mathbb{R}^2 &\text{linear velocity}\\
          \boldsymbol{r} \in \mathbb{R} &&\text{angular velocity}\\
          x(t) = \begin{bmatrix}\boldsymbol{p}\\\psi\end{bmatrix}, &\quad\mathbb{R} \to \mathbb{R}^2 \times \mathbb{S}&\text{state vector}
          \end{align}

       .. math::
          :nowrap:

          \begin{align}
          \dat x &= \begin{bmatrix}\cos(\psi)& -\sin(\psi) &0\\
                                  \sin(\psi)& \cos(\psi) & 0\\
                                  0&0&1
                                  \end{bmatrix}
                   \begin{bmatrix}u\\v\\r\end{bmatrix}\\
          \boldsymbol{u} &= \begin{bmatrix}u\\v\\r\end{bmatrix}\\
          \boldsymbol{y} &= x
          \end{align}

       +----------------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-------------------+
       | Name                 | Symbol                     | Description                         | Causality     | Variability    | Default              | Unit              |
       +======================+============================+=====================================+===============+================+======================+===================+
       | ``vessel_ctrl``      | :math:`u`                  | Desired: Surge, Sway rate of turn   | ``input``     | ``discrete``   | \                    | [m/s, m/s, rad/s] |
       +----------------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-------------------+
       | ``kinematics``       | :math:`y`                  | ``fkin::Kinematics2D``              | ``output``    | ``continuous`` | \                    | \                 |
       +----------------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-------------------+
       | ``position_course``  | :math:`[p_0,\psi_0]`       | Initial; North, East, Yaw           | ``parameter`` | ``fixed``      | :math:`[0, 50, 0]`   | m                 |
       +----------------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-------------------+

       .. warning::

          This simulation model is very basic and has been abandoned in favor of a higher
          fidelity model implemented using FMI :cite:`fmi2`.

       \endrst

    */
    class KinematicVessel : public IAlgorithm
    {
    public:
      /**
         @brief KinematicVessel constructor.

         The constructor parses the specification from the given YAML
         node. It establishes data structures and sets up DDS readers
         and writers according to the input/output scheme of the
         algorithm. The following YAML code block shows the expected
         layout of the ``KinematicVessel`` map of a input config file. The
         inputs, outputs, and initial conditions are communicated with
         DDS communication. Common is their DDS topic and DDS
         identifier. The specification is deemed self-explanatory.

         \rst

         .. code-block:: yaml

             time_step_ms: 200
             inputs:
               vessel_ctrl:                 # DDS type: fkin::IdVec3d
                 topic: fkinVesselCtrl
                 id: Vessel
                 max_age_ms: -1
             outputs:                       # DDS type: fkin::Kinematics2D
               kinematics:
                 topic: fkinKinematics2D
                 id: Vessel
             initial_conditions:
               position_course:             # DDS type: fkin::IdVec3d
                 topic: fkinPositionCourse
                 id: Vessel
                 max_wait_ms: 50
                 fallback: [0, 50, 0]
         \endrst

         @param [in] config YAML configuration from input file.
         @param [in] scheduler State machine scheduler, needed to post events to state machine.
         @param [in] machine State machine processor handle, needed to post events to state machine.
         @param publisher DDS data writer to send data.
         @param subscriber DDS data reader to receive data.
      */
      explicit KinematicVessel(
          const YAML::Node& config,
          boost::statechart::fifo_scheduler<>& scheduler,
          boost::statechart::fifo_scheduler<>::processor_handle machine,
          dds::pub::Publisher publisher,
          dds::sub::Subscriber subscriber);
      /// Destructor.
      virtual ~KinematicVessel();
      /// See base class.
      virtual void solve(const std::atomic<bool>& cancel_token);
      /// See base class.
      virtual void initialize(const std::atomic<bool>& cancel_token);
      /// See base class.
      virtual void timer(const std::atomic<bool>& cancel_token);
      /// Name identifier of algorithm.
      virtual inline const char* name() { return "KinematicVessel"; }
      /// Helper function that queues event to state machine.
      void event(boost::statechart::event_base * const event);

    private:
      /// March simulation time one time step ahead.
      inline void step_time() { m_next_step += m_time_step; }
      KinematicVessel() = delete;
      class Impl;
      std::unique_ptr<Impl> m_impl; ///< Holds the implementation of the algorithm.
      boost::statechart::fifo_scheduler<> & m_scheduler; ///< Members for state machine.
      boost::statechart::fifo_scheduler<>::processor_handle m_stateMachine; ///< Member for state machine.
      const std::chrono::milliseconds m_time_step; ///< Discrete time step.
      std::chrono::steady_clock::time_point m_next_step; ///< Simulation time point.
      const YAML::Node m_config; ///< YAML configuration for algorithm.
    };

  }
}


// File: shallow_copy_problems_type.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl;

int main(int, char**)
{
    dense2D<double>     A(3, 3), B(3, 3);
    dense2D<float>      C(3, 3);

    A= 4.0;

    B= A;               // Create an alias
    B*= 2.0;            // Changes also A

    C= A;               // Copies the values
    C*= 2.0;            // A is unaffected

    return 0;
}


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "cuNDArray_fileio.h"
#include "cuNDArray_math.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "hoCuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoNDArray_utils.h"
#include "cuPartialDifferenceOperator.h"
#include "cuTvOperator.h"
#include "cuTv1dOperator.h"
#include "cuTvPicsOperator.h"
#include "CBSubsetOperator.h"
#include "osSPSSolver.h"
#include "osMOMSolver.h"
#include "cuOSMOMSolverD.h"
#include "osMOMSolverD2.h"
#include "hoCuNCGSolver.h"
#include "osMOMSolverD3.h"
#include "osMOMSolverL1.h"
#include "osMOMSolverF.h"
#include "osAHZCSolver.h"
#include "ADMMSolver.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>
#include <boost/make_shared.hpp>
#include <GPUTimer.h>
#include <operators/cuGaussianFilterOperator.h>
#include <multiplicationOperatorContainer.h>
#include <cuDownsampleOperator.h>
#include <operators/cuSmallConvOperator.h>
#include <denoise/nonlocalMeans.h>
#include <solvers/osMOMSolverW.h>
#include <operators/cuTFFT.h>
#include "cuSolverUtils.h"
#include "osPDsolver.h"
#include "osLALMSolver.h"
#include "osLALMSolver2.h"
#include "cuATrousOperator.h"
#include "hdf5_utils.h"
#include "cuEdgeATrousOperator.h"
#include "cuDCTOperator.h"
#include "cuDCTDerivativeOperator.h"
#include "dicomWriter.h"
#include "conebeam_projection.h"
#include "weightingOperator.h"
#include "hoNDArray_math.h"
#include "cuNCGSolver.h"
#include "CT_acquisition.h"
#include "cuScaleOperator.h"
#include "cuTVPrimalDualOperator.h"
#include "cuWTVPrimalDualOperator.h"
#include "cuATVPrimalDualOperator.h"
#include "cuSSTVPrimalDualOperator.h"
#include "cuBilateralPriorPrimalDualOperator.h"
#include "cuTV4DPrimalDualOperator.h"
#include "CBSubsetWeightOperator.h"
#include "BilateralPriorOperator.h"
#include "cuPICSDualOperator.h"
#include "cuTVTFFT.h"

using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

boost::shared_ptr<hoCuNDArray<float>> downsample_projections(hoCuNDArray<float>* projections, unsigned int num_downsamples )
{

    if (num_downsamples == 0) return boost::make_shared<hoCuNDArray<float>>(*projections);

    auto tmp = Gadgetron::downsample<float,2>(projections);

    for (int k = 1; k < num_downsamples; k++)
        tmp = Gadgetron::downsample<float,2>(tmp.get());

    return boost::make_shared<hoCuNDArray<float>>(*tmp);
}

boost::shared_ptr<cuNDArray<float> > calculate_prior(boost::shared_ptr<CBCT_binning>  binning,boost::shared_ptr<CBCT_acquisition> ps, hoCuNDArray<float>& projections, std::vector<size_t> is_dims, floatd3 imageDimensions){
    std::cout << "Calculating FDK prior" << std::endl;
    boost::shared_ptr<CBCT_binning> binning_pics=binning->get_3d_binning();
    std::vector<size_t> is_dims3d = is_dims;
    is_dims3d.pop_back();
    boost::shared_ptr< hoCuConebeamProjectionOperator >
            Ep( new hoCuConebeamProjectionOperator() );
    Ep->setup(ps,binning_pics,imageDimensions);
    Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
    Ep->set_domain_dimensions(&is_dims3d);
    Ep->set_use_filtered_backprojection(true);
    boost::shared_ptr<hoCuNDArray<float> > prior3d(new hoCuNDArray<float>(&is_dims3d));
    Ep->mult_MH(&projections,prior3d.get());

    hoCuNDArray<float> tmp_proj(*ps->get_projections());
    Ep->mult_M(prior3d.get(),&tmp_proj);
    float s = dot(ps->get_projections().get(),&tmp_proj)/dot(&tmp_proj,&tmp_proj);
    *prior3d *= s;
    auto prior = boost::make_shared<cuNDArray<float>>(*prior3d);
    return prior;
}

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

    string acquisition_filename;
    string outputFile;

    uintd3 imageSize;
    floatd3 voxelSize;
    int device;
    floatd2 scale_factor;
    unsigned int iterations;
    unsigned int subsets;
    float rho,tau,nlm_noise,bil_weight;
    float tv_weight,pics_weight, wavelet_weight,huber,sigma,dct_weight,sfr_weight,framelet_weight,atv_weight, sstv_weight;
    float framelet_weight4d;
    float tv_4d,atv_4d;
    bool use_non_negativity;
    int reg_iter;

    po::options_description desc("Allowed options");

    desc.add_options()
            ("help", "produce help message")
            ("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
            ("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    ("output,f", po::value<string>(&outputFile)->default_value("reconstruction.hdf5"), "Output filename")
            ("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
            ("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
            ("SAG","Use exact SAG correction if present")
            ("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
            ("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
            ("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    ("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
            ("downsample,D",po::value<floatd2>(&scale_factor)->default_value(floatd2(1,1)),"Downsample projections this factor")
            ("subsets,u",po::value<unsigned int>(&subsets)->default_value(10),"Number of subsets to use")
    ("TV",po::value<float>(&tv_weight)->default_value(0),"Total variation weight in spatial dimensions")
            ("TV4D",po::value<float>(&tv_4d)->default_value(0),"Total variation weight in temporal dimensions")
            ("ATV4D",po::value<float>(&atv_4d)->default_value(0),"Advanced Total variation weight in temporal dimensions")
            ("ATV",po::value<float>(&atv_weight)->default_value(0),"Advanced Total variation weight ")
            ("SSTV",po::value<float>(&sstv_weight)->default_value(0),"Scale space total variation weight ")
            ("PICS",po::value<float>(&pics_weight)->default_value(0),"PICS weight")
            ("Wavelet,W",po::value<float>(&wavelet_weight)->default_value(0),"Weight of the wavelet operator")
            ("Framelet",po::value<float>(&framelet_weight)->default_value(0),"Weight of the framelet operator")
            ("Framelet4D",po::value<float>(&framelet_weight4d)->default_value(0),"Weight of the framelet operator")
            ("Huber",po::value<float>(&huber)->default_value(0),"Huber weight")
            ("use_prior","Use an FDK prior")
            ("use_non_negativity",po::value<bool>(&use_non_negativity)->default_value(true),"Prevent image from having negative attenuation")
            ("sigma",po::value<float>(&sigma)->default_value(0.1),"Sigma for billateral filter")
            ("DCT",po::value<float>(&dct_weight)->default_value(0),"DCT regularization")
            ("SFR",po::value<float>(&sfr_weight)->default_value(0),"SFR regularization")
            ("3D","Only use binning for selecting valid projections")
            ("tau",po::value<float>(&tau)->default_value(1e-5),"Tau value for solver")
            ("reg_iter",po::value<int>(&reg_iter)->default_value(2))
            ("bilateral-weight",po::value<float>(&bil_weight)->default_value(0),"Bilateral weight")
            ("prior",po::value<string>(),"prior image")
            ("projection_weights",po::value<string>(),"Array containing weights to be applied to the projections.")

            ("NLM",po::value<float>(&nlm_noise)->default_value(0),"Use non-local means based on the 3d image")
            ;

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

    if (vm.count("help")) {
        cout << desc << "\n";
        return 1;
    }

    std::stringstream command_line_string;
    std::cout << "Command line options:" << std::endl;
    for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
        boost::any a = it->second.value();
        command_line_string << it->first << ": ";
        if (a.type() == typeid(std::string)) command_line_string << it->second.as<std::string>();
        else if (a.type() == typeid(int)) command_line_string << it->second.as<int>();
        else if (a.type() == typeid(unsigned int)) command_line_string << it->second.as<unsigned int>();
        else if (a.type() == typeid(float)) command_line_string << it->second.as<float>();
        else if (a.type() == typeid(vector_td<float,3>)) command_line_string << it->second.as<vector_td<float,3> >();
        else if (a.type() == typeid(vector_td<float,2>)) command_line_string << it->second.as<vector_td<float,2> >();
        else if (a.type() == typeid(vector_td<int,3>)) command_line_string << it->second.as<vector_td<int,3> >();
        else if (a.type() == typeid(vector_td<unsigned int,3>)) command_line_string << it->second.as<vector_td<unsigned int,3> >();
        else if (a.type() == typeid(bool)) command_line_string << it->second.as<bool>();
        else command_line_string << "Unknown type" << std::endl;
        command_line_string << std::endl;
    }
    std::cout << command_line_string.str();

    cudaSetDevice(device);
    cudaDeviceReset();

    //Really weird stuff. Needed to initialize the device?? Should find real bug.
    cudaDeviceManager::Instance()->lockHandle();
    cudaDeviceManager::Instance()->unlockHandle();

    boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
    ps->load(acquisition_filename);
    ps->get_geometry()->print(std::cout);

    float SDD = ps->get_geometry()->get_SDD();
    float SAD = ps->get_geometry()->get_SAD();

    boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
    if (vm.count("binning")){
        std::cout << "Loading binning data" << std::endl;
        binning->load(vm["binning"].as<string>());
        if (vm.count("3D"))
            binning = binning->get_3d_binning();
    } else binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));
    binning->print(std::cout);

    floatd3 imageDimensions;
    if (vm.count("dimensions")){
        imageDimensions = vm["dimensions"].as<floatd3>();
        voxelSize = imageDimensions/imageSize;
    }
    else imageDimensions = voxelSize*imageSize;

    float lengthOfRay_in_mm = norm(imageDimensions);
    unsigned int numSamplesPerPixel = 3;
    float minSpacing = min(voxelSize)/numSamplesPerPixel;

    unsigned int numSamplesPerRay;
    if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
    else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

    float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
    size_t numProjs = ps->get_projections()->get_size(2);
    size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);
    std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

    std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
    std::cout << "Image size " << imageDimensions << std::endl;
    std::cout << "FRAMELET WEIGHT " << framelet_weight << std::endl;
    is_dims.push_back(binning->get_number_of_bins());

    //scatter_correct(binning,ps,ps->get_projections().get(),is_dims,imageDimensions);

    if (scale_factor[0] != 1 || scale_factor[1] != 1)
        ps->downsample(scale_factor[0],scale_factor[1]);

    //osLALMSolver<cuNDArray<float>> solver;
    osMOMSolverD<cuNDArray<float>> solver;
    //osMOMSolverW<cuNDArray<float>> solver;
    //osMOMSolverL1<cuNDArray<float>> solver;
    //osAHZCSolver<cuNDArray<float>> solver;
    //osMOMSolverF<cuNDArray<float>> solver;
    //ADMMSolver<cuNDArray<float>> solver;
    solver.set_dump(false);

    boost::shared_ptr<cuNDArray<float>> prior;
    if (vm.count("use_prior") ) {
        auto projections = *ps->get_projections();
        prior = calculate_prior(binning,ps,projections,is_dims,imageDimensions);
        solver.set_x0(expand(prior.get(),binning->get_number_of_bins()));
        //prior = calculate_weightImage(binning,ps,projections,is_dims,imageDimensions);
//        solver.set_x0(prior);
    }
    if (vm.count("prior")){
        prior = boost::make_shared<cuNDArray<float>>(*read_nd_array<float>(vm["prior"].as<string>().c_str()));
    }

    solver.set_max_iterations(iterations);
    solver.set_output_mode(osSPSSolver<cuNDArray<float>>::OUTPUT_VERBOSE);
    solver.set_tau(tau);
    solver.set_non_negativity_constraint(use_non_negativity);
    solver.set_huber(huber);
    solver.set_reg_steps(reg_iter);
    //solver.set_rho(rho);

    if (atv_weight > 0) {

        auto ATV = boost::make_shared<cuATVPrimalDualOperator<float>>(0.0);
        ATV->set_weight(atv_weight);
        solver.add_regularization_operator(ATV);

    }

    if (sstv_weight > 0){
        auto SSTV = boost::make_shared<cuSSTVPrimalDualOperator<float>>();
        SSTV->set_weight(sstv_weight);
        solver.add_regularization_operator(SSTV);
    }

    if (tv_weight > 0) {

        //auto TV = boost::make_shared<cuWTVPrimalDualOperator<float>>(0.0,1e-3);
//

        auto TV = boost::make_shared<cuTVPrimalDualOperator<float>>(0.0);
        TV->set_weight(tv_weight);

        solver.add_regularization_operator(TV);

/*
      auto Dx = boost::make_shared<cuPartialDifferenceOperator<float, 3>>(0);
      Dx->set_weight(tv_weight);
      Dx->set_domain_dimensions(&is_dims);
      Dx->set_codomain_dimensions(&is_dims);

      auto Dy = boost::make_shared<cuPartialDifferenceOperator<float, 3>>(1);
      Dy->set_weight(tv_weight);
      Dy->set_domain_dimensions(&is_dims);
      Dy->set_codomain_dimensions(&is_dims);

      auto Dz = boost::make_shared<cuPartialDifferenceOperator<float, 3>>(2);
      Dz->set_weight(tv_weight);
      Dz->set_domain_dimensions(&is_dims);
      Dz->set_codomain_dimensions(&is_dims);
      solver.add_regularization_group({Dx, Dy, Dz});
*/

    }
    if (tv_4d > 0) {

        auto Dt = boost::make_shared<cuPartialDifferenceOperator<float, 4>>(3);
        Dt->set_weight(tv_4d);
        Dt->set_domain_dimensions(&is_dims);
        Dt->set_codomain_dimensions(&is_dims);
        solver.add_regularization_operator(Dt);
/**/

    }
    if (atv_4d > 0) {

        auto TVF =  boost::make_shared<cuTVTFFT>();
        TVF->set_weight(atv_4d);
        TVF->set_domain_dimensions(&is_dims);
        solver.add_regularization_operator(TVF);

/*
		  auto Dt = boost::make_shared<cuTV4DPrimalDualOperator<float>>(0);
		  Dt->set_weight(atv_4d);
		  solver.add_regularization_operator(Dt);
*/

    }

    if (framelet_weight > 0){
        auto stencils = std::vector<vector_td<float,3>>({
                                                                vector_td<float,3>(-std::sqrt(2.0f),0,std::sqrt(2.0f)),vector_td<float,3>(-1,2,-1),vector_td<float,3>(1,2,1) });
        std::cout << "Framelet weight " << framelet_weight << std::endl;
        for (int step_size = 0; step_size < 3; step_size++) {
            for (auto stencil : stencils) {
                std::cout << "Adding operators " << std::endl;
                auto Rx = boost::make_shared<cuSmallConvOperator<float, 4, 3>>(stencil, 0,std::pow(2,step_size));
                Rx->set_weight(framelet_weight);
                Rx->set_domain_dimensions(&is_dims);
                Rx->set_codomain_dimensions(&is_dims);

                auto Ry = boost::make_shared<cuSmallConvOperator<float, 4, 3>>(stencil, 1,std::pow(2,step_size));
                Ry->set_weight(framelet_weight);
                Ry->set_domain_dimensions(&is_dims);
                Ry->set_codomain_dimensions(&is_dims);

                auto Rz = boost::make_shared<cuSmallConvOperator<float, 4, 3>>(stencil, 2,std::pow(2,step_size));
                Rz->set_weight(framelet_weight);
                Rz->set_domain_dimensions(&is_dims);
                Rz->set_codomain_dimensions(&is_dims);
                solver.add_regularization_group({Rx, Ry, Rz});
//                solver.add_regularization_operator(Rx);
//                solver.add_regularization_operator(Ry);
//                solver.add_regularization_operator(Rz);

            }
        }
    }
    if (framelet_weight4d > 0){
        auto stencils = std::vector<vector_td<float,3>>({
                                                                vector_td<float,3>(-std::sqrt(2.0f),0,std::sqrt(2.0f)),vector_td<float,3>(-1,2,-1),vector_td<float,3>(1,2,1) });
        for (int step_size = 0; step_size < 3; step_size++) {
            for (auto stencil : stencils) {
                auto Rt = boost::make_shared<cuSmallConvOperator<float, 4, 3>>(stencil, 3,std::pow(2,step_size));
                Rt->set_weight(framelet_weight4d);
                Rt->set_domain_dimensions(&is_dims);
                Rt->set_codomain_dimensions(&is_dims);
                solver.add_regularization_operator(Rt);
            }
        }
    }

    if (dct_weight > 0){
        auto dctOp = boost::make_shared<cuDCTOperator<float>>();
//        auto dctOp = boost::make_shared<cuTFFT>();
        dctOp->set_domain_dimensions(&is_dims);
        dctOp->set_weight(dct_weight);
        solver.add_regularization_operator(dctOp);
    }

    if (sfr_weight > 0){
        auto sfrOp = boost::make_shared<cuTFFT>();
        sfrOp->set_domain_dimensions(&is_dims);
        sfrOp->set_weight(sfr_weight);
        solver.add_regularization_operator(sfrOp);
    }

    if (bil_weight > 0){
        auto bilOp = boost::make_shared<cuBilateralPriorPrimalDualOperator>(0.002,3.0,prior);
        bilOp->set_weight(bil_weight);
        solver.add_regularization_operator(bilOp);

        /*
        auto bilOp = boost::make_shared<BilateralPriorOperator>();
        bilOp->set_domain_dimensions(&is_dims);
        bilOp->set_codomain_dimensions(&is_dims);
        bilOp->set_weight(bil_weight);
        bilOp->set_sigma_spatial(3.0);
        bilOp->set_sigma_int(0.01);

        bilOp->set_prior(cuBilPrior);
        solver.add_regularization_operator(bilOp,0.01);
         */
    }

    if (pics_weight > 0){
        auto pics = boost::make_shared<cuPICSPrimalDualOperator<float>>();
        pics->set_weight(pics_weight);
        pics->set_prior(prior);
        solver.add_regularization_operator(pics);
    }

    boost::shared_ptr<CBSubsetOperator<cuNDArray>> E;
    if ( vm.count("projection_weights")) {
        auto weights = read_nd_array<float>(vm["projection_weights"].as<string>().c_str());
        if (scale_factor[0] != 1 || scale_factor[1] != 1) {
            cuNDArray<float> tmp_weights(*weights);
            auto dims = *tmp_weights.get_dimensions();

            tmp_weights = downsample_projections(&tmp_weights,scale_factor[0],scale_factor[1]);

            weights = tmp_weights.to_host();
        }

        if (!ps->get_projections()->dimensions_equal(weights.get()))
            throw std::runtime_error("Weight dimensions must match that of the projection data");
        auto EW  = boost::make_shared<CBSubsetWeightOperator<cuNDArray>>(subsets);
        EW->setup(ps,binning,imageDimensions,weights);
        E = EW;
    } else {
        std::cout <<"Normal projections" << std::endl;
        E = boost::make_shared<CBSubsetOperator<cuNDArray> >(subsets);

        E->setup(ps, binning, imageDimensions);
    }
    E->set_domain_dimensions(&is_dims);
    E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

    solver.set_encoding_operator(E);

    auto projections = boost::make_shared<cuNDArray<float>>(*ps->get_projections());
    std::cout << "Projection norm:" << nrm2(projections.get()) << std::endl;

    boost::shared_ptr<cuNDArray<float>> result;
    {
        GPUTimer tim("Solver");
        result = solver.solve(projections.get());
    }
//	global_timer.reset();
    std::cout << "Penguin" << nrm2(result.get()) << std::endl;

    std::cout << "Result sum " << asum(result.get()) << std::endl;

    //apply_mask(result.get(),mask.get());

    std::cout << "Result sum " << asum(result.get()) << std::endl;
    //saveNDArray2HDF5(result.get(),outputFile,imageDimensions,vector_td<float,3>(0),command_line_string.str(),iterations);

    write_nd_array(result.get(),"reconstruction.real");

    if (wavelet_weight > 0){
        osMOMSolverF<cuNDArray<float>> solverF;
        solverF.set_max_iterations(10);
        solverF.set_x0(result);
        solverF.set_encoding_operator(E);
        solverF.set_non_negativity_constraint(true);

        auto wave = boost::make_shared<cuEdgeATrousOperator<float>>();

        wave->set_sigma(sigma);
        wave->set_domain_dimensions(&is_dims);
        if (binning->get_number_of_bins() == 1)
            wave->set_levels({2,2,2});
        else
            wave->set_levels({2,2,2,2});
        wave->set_weight(wavelet_weight);
        solverF.add_regularization_operator(wave);

        result = solverF.solve(projections.get());

    }
/*
	if (nlm_noise > 0){

		float* result_data = result->get_data_ptr();

		for (int i = 0; i < result->get_size(3); i++){
			cuNDArray<float> result_view(prior->get_dimensions(),result_data);

			nonlocal_means_ref(&result_view,prior.get(),nlm_noise);
			result_data += result_view.get_number_of_elements();
		}
	}
*/
    saveNDArray2HDF5(result.get(),outputFile,imageDimensions,floatd3(0,0,0),command_line_string.str(),iterations);
//	write_nd_array(result.get(),"reconstruction.real");
    //write_dicom(result.get(),command_line_string.str(),imageDimensions);

}


/****************************************************************
 *
 * Copyright (c) 2011
 * All rights reserved.
 *
 * Hochschule Bonn-Rhein-Sieg
 * University of Applied Sciences
 * Computer Science Department
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Author:
 * Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, Azamat Shakhimardanov
 * Supervised by:
 * Gerhard K. Kraetzschmar
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * This sofware is published under a dual-license: GNU Lesser General Public
 * License LGPL 2.1 and BSD license. The dual-license implies that users of this
 * code may choose which terms they prefer.
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Hochschule Bonn-Rhein-Sieg nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 2.1 of the
 * License, or (at your option) any later version or the BSD license.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License LGPL and the BSD license for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL and BSD license along with this program.
 *
 ****************************************************************/

#ifndef YOUBOT_UNITS_HPP
#define	YOUBOT_UNITS_HPP
#include <boost/units/io.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/temperature/celsius.hpp>
#include <boost/units/cmath.hpp>
#include <boost/units/make_scaled_unit.hpp>
#include <boost/units/systems/si/prefixes.hpp>

using namespace boost::units;
using namespace boost::units::si;
using namespace boost::units::angle;

//typedef boost::units::si::length meter;
using boost::units::si::meters;
namespace youbot {

typedef boost::units::make_scaled_unit<si::length, boost::units::scale<10, boost::units::static_rational<-3> > >::type millimeter;
typedef boost::units::make_scaled_unit<si::length, boost::units::scale<10, boost::units::static_rational<-2> > >::type centimeter;
BOOST_UNITS_STATIC_CONSTANT(centimeters, centimeter);

typedef boost::units::make_scaled_unit<si::time, boost::units::scale<10, boost::units::static_rational<-3> > >::type millisecond;
//BOOST_UNITS_STATIC_CONSTANT(millimeters, millimeter);

} // namespace youbot

#endif	/* YOUBOT_UNITS_HPP */


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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
 *
 * http://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.
*/

/*!\file NE....cpp
  \brief \ref EMNE_MULTIBODY - C++ input file, Time-Stepping version - O.B.

  A multibody example.
  Direct description of the model.
  Simulation with a Time-Stepping scheme.
*/

#include "SiconosKernel.hpp"
#include "KneeJointR.hpp"
#include "PrismaticJointR.hpp"
#include <boost/math/quaternion.hpp>
using namespace std;

/* Given a position of a point in the Inertial Frame and the configuration vector q of a solid
 * returns a position in the spatial frame.
 */
void fromInertialToSpatialFrame(double *positionInInertialFrame, double *positionInSpatialFrame, SP::SiconosVector  q  )
{
double q0 = q->getValue(3);
double q1 = q->getValue(4);
double q2 = q->getValue(5);
double q3 = q->getValue(6);

::boost::math::quaternion<double>    quatQ(q0, q1, q2, q3);
::boost::math::quaternion<double>    quatcQ(q0, -q1, -q2, -q3);
::boost::math::quaternion<double>    quatpos(0, positionInInertialFrame[0], positionInInertialFrame[1], positionInInertialFrame[2]);
::boost::math::quaternion<double>    quatBuff;

//perform the rotation
quatBuff = quatQ * quatpos * quatcQ;

positionInSpatialFrame[0] = quatBuff.R_component_2()+q->getValue(0);
positionInSpatialFrame[1] = quatBuff.R_component_3()+q->getValue(1);
positionInSpatialFrame[2] = quatBuff.R_component_4()+q->getValue(2);

}
void tipTrajectories(SP::SiconosVector  q, double * traj, double length)
{
  double positionInInertialFrame[3];
  double positionInSpatialFrame[3];
  // Output the position of the tip of beam1
  positionInInertialFrame[0]=length/2;
  positionInInertialFrame[1]=0.0;
  positionInInertialFrame[2]=0.0;

  fromInertialToSpatialFrame(positionInInertialFrame, positionInSpatialFrame, q  );
  traj[0] = positionInSpatialFrame[0];
  traj[1] = positionInSpatialFrame[1];
  traj[2] = positionInSpatialFrame[2];

  // std::cout <<  "positionInSpatialFrame[0]" <<  positionInSpatialFrame[0]<<std::endl;
  // std::cout <<  "positionInSpatialFrame[1]" <<  positionInSpatialFrame[1]<<std::endl;
  // std::cout <<  "positionInSpatialFrame[2]" <<  positionInSpatialFrame[2]<<std::endl;

  positionInInertialFrame[0]=-length/2;
  fromInertialToSpatialFrame(positionInInertialFrame, positionInSpatialFrame, q  );
  traj[3]= positionInSpatialFrame[0];
  traj[4] = positionInSpatialFrame[1];
  traj[5] = positionInSpatialFrame[2];
}

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

    // ================= Creation of the model =======================

    // User-defined main parameters
    unsigned int nDof = 3;
    unsigned int qDim = 7;
    unsigned int nDim = 6;
    double t0 = 0;                   // initial computation time
    double T = 10.0;                  // final computation time
    double h = 0.01;                // time step
    int N = 1000;
    double L1 = 1.0;
    double theta = 1.0;              // theta for MoreauJeanOSI integrator
    double g = 9.81; // Gravity
    double m = 1.;

    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    FILE * pFile;
    pFile = fopen("data.h", "w");
    if (pFile == NULL)
    {
      printf("fopen exampleopen filed!\n");
      fclose(pFile);
    }

    cout << "====> Model loading ..." << endl << endl;

    // -- Initial positions and velocities --

    //First DS
    SP::SiconosVector q10(new SiconosVector(qDim));
    SP::SiconosVector v10(new SiconosVector(nDim));
    SP::SimpleMatrix I1(new SimpleMatrix(3, 3));
    v10->zero();
    I1->eye();
    I1->setValue(0, 0, 0.1);
    // Initial position of the center of gravity CG1
    (*q10)(0) = 0.5 * L1 / sqrt(2.0);
    (*q10)(1) = 0;
    (*q10)(2) = -0.5 * L1 / sqrt(2.0);
    // Initial orientation (a quaternion that gives the rotation w.r.t the spatial frame)
    // angle of the rotation Pi/4
    double angle = M_PI / 4;
    SiconosVector V1(3);
    V1.zero();
    // vector of the rotation (Y-axis)
    V1.setValue(0, 0);
    V1.setValue(1, 1);
    V1.setValue(2, 0);
    // construction of the quaternion
    q10->setValue(3, cos(angle / 2));
    q10->setValue(4, V1.getValue(0)*sin(angle / 2));
    q10->setValue(5, V1.getValue(1)*sin(angle / 2));
    q10->setValue(6, V1.getValue(2)*sin(angle / 2));

    // -- The dynamical system --
    SP::NewtonEulerDS beam1(new NewtonEulerDS(q10, v10, m, I1));
    // -- Set external forces (weight) --
    SP::SiconosVector weight(new SiconosVector(nDof));
    (*weight)(2) = -m * g;
    beam1->setFExtPtr(weight);

    // --------------------
    // --- Interactions ---
    // --------------------
    SP::NonSmoothLaw nslaw1(new EqualityConditionNSL(KneeJointR::numberOfConstraints()));

    SP::SiconosVector P(new SiconosVector(3));
    P->zero();
    // Building the first knee joint for beam1
    // input  - the concerned DS : beam1
    //        - a point in the spatial frame (absolute frame) where the knee is defined P
    SP::NewtonEulerR relation1(new KneeJointR(beam1, P));

    SP::Interaction inter1(new Interaction(KneeJointR::numberOfConstraints(), nslaw1, relation1));

    // -------------
    // --- Model ---
    // -------------
    SP::Model myModel(new Model(t0, T));
    // add the dynamical system in the non smooth dynamical system
    myModel->nonSmoothDynamicalSystem()->insertDynamicalSystem(beam1);
    // link the interaction and the dynamical system
    myModel->nonSmoothDynamicalSystem()->link(inter1, beam1);
    // ------------------
    // --- Simulation ---
    // ------------------

    // -- (1) OneStepIntegrators --
    SP::MoreauJeanCombinedProjectionOSI OSI(new MoreauJeanCombinedProjectionOSI(theta));

    // -- (2) Time discretisation --
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));

    // -- (3) one step non smooth problem
    SP::OneStepNSProblem osnspb(new MLCP());
    SP::OneStepNSProblem osnspb_pos(new MLCPProjectOnConstraints(SICONOS_MLCP_ENUM));

    // -- (4) Simulation setup with (1) (2) (3)

    SP::TimeSteppingCombinedProjection s(new TimeSteppingCombinedProjection(t, OSI, osnspb, osnspb_pos));
    s->setProjectionMaxIteration(1000);
    s->setConstraintTolUnilateral(1e-08);

    // =========================== End of model definition ===========================

    // ================================= Computation =================================

    // --- Simulation initialization ---

    cout << "====> Initialisation ..." << endl << endl;
    myModel->initialize(s);

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 15 + 7;
    SimpleMatrix dataPlot(N, outputSize);
    SimpleMatrix beam1Plot(2,3*N);

    SP::SiconosVector q1 = beam1->q();
    SP::SiconosVector y= inter1->y(0);
    SP::SiconosVector ydot= inter1->y(1);

    // --- Time loop ---
    cout << "====> Start computation ... " << endl << endl;
    // ==== Simulation loop - Writing without explicit event handling =====
    int k = 0;
    boost::progress_display show_progress(N);

    boost::timer time;
    time.restart();
    SP::SiconosVector yAux(new SiconosVector(3));
    yAux->setValue(0, 1);
    SP::SimpleMatrix Jaux(new SimpleMatrix(3, 3));
    Index dimIndex(2);
    Index startIndex(4);
    fprintf(pFile, "double T[%d*%d]={", N + 1, outputSize);
    double beamTipTrajectories[6];

    for (k = 0; k < N-1; k++)
    {
      // solve ...
      //s->newtonSolve(1e-4, 50);

      s->advanceToEvent();
      // --- Get values to be plotted ---
      dataPlot(k, 0) =  s->nextTime();

      dataPlot(k, 1) = (*q1)(0);
      dataPlot(k, 2) = (*q1)(1);
      dataPlot(k, 3) = (*q1)(2);
      dataPlot(k, 4) = (*q1)(3);
      dataPlot(k, 5) = (*q1)(4);
      dataPlot(k, 6) = (*q1)(5);
      dataPlot(k, 7) = (*q1)(6);
      dataPlot(k, 8) = y->norm2();
      dataPlot(k, 9) = ydot->norm2();

      tipTrajectories(q1,beamTipTrajectories,L1);
      beam1Plot(0,3*k) = beamTipTrajectories[0];
      beam1Plot(0,3*k+1) = beamTipTrajectories[1];
      beam1Plot(0,3*k+2) = beamTipTrajectories[2];
      beam1Plot(1,3*k) = beamTipTrajectories[3];
      beam1Plot(1,3*k+1) = beamTipTrajectories[4];
      beam1Plot(1,3*k+2) = beamTipTrajectories[5];

      for (unsigned int jj = 0; jj < outputSize; jj++)
      {
        if ((k || jj))
          fprintf(pFile, ",");
        fprintf(pFile, "%f", dataPlot(k, jj));
      }
      fprintf(pFile, "\n");
       s->nextStep();
      //s->processEvents();
      ++show_progress;
    }
    fprintf(pFile, "};");
    cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
    cout << "Computation Time " << time.elapsed()  << endl;

    // --- Output files ---
    cout << "====> Output file writing ..." << endl;
    dataPlot.resize(k, outputSize);
    ioMatrix::write("NE_1DS_1Knee_MLCP_MoreauJeanCombinedProjection.dat", "ascii", dataPlot, "noDim");
    ioMatrix::write("NE_1DS_1Knee_MLCP_beam1.dat", "ascii", beam1Plot, "noDim");

    SimpleMatrix dataPlotRef(dataPlot);
    dataPlotRef.zero();
    ioMatrix::read("NE_1DS_1Knee_MLCP_MoreauJeanCombinedProjection.ref", "ascii", dataPlotRef);
    std::cout << "Error w.r.t. reference file : " << (dataPlot - dataPlotRef).normInf() << std::endl;

    if ((dataPlot - dataPlotRef).normInf() > 1e-7)
    {
      //(dataPlot - dataPlotRef).display();
      std::cout << "Warning. The results is rather different from the reference file." << std::endl;
      return 1;
    }

    fclose(pFile);
  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught in NE_...cpp" << endl;
  }

}