// (C) Copyright Renaud Detry   2007-2015.
// Distributed under the GNU General Public License and under the
// BSD 3-Clause License (See accompanying file LICENSE.txt).

/** @file */

#include <iostream>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>

#include <nuklei/Random.h>
#include <nuklei/Common.h>
#include <nuklei/Log.h>

#include <boost/random.hpp>

namespace nuklei {

// Either use GSL random gen, or Boost random gen
//#define NUKLEI_USE_BOOST_RANDOM_GEN

// Sync before accessing random gen?
// -> NUKLEI_RANDOM_SYNC_OMP: use OMP exclusive access
//    This one is obsolete, because when OMP is enabled, omp_get_thread_num
//    allows Random methods to access thread-specific random gens.
// -> NUKLEI_RANDOM_SYNC_MUTEX: use posix mutex
//    This makes Nuklei much slower
// -> NUKLEI_RANDOM_SYNC_NONE: default, use this on single-thread, or with OMP.

#define NUKLEI_RANDOM_SYNC_NONE

#ifdef _OPENMP
#include <omp.h>
  static inline int nuklei_thread_num()
  {
    return omp_get_thread_num();
  }
  static inline int nuklei_max_threads()
  {
    // I had issues with omp_get_max_threads(), returning 1 while spawning
    // more than one thread.
    // I had this with static executables.
    // I guess it could be a race condition.
    // Let's just return a large number instead (who would have more than 1000
    // procs anyway?)
    //return omp_get_max_threads();
    return 1024;
  }
#else
  static inline int nuklei_thread_num()
  {
    return 0;
  }
  static inline int nuklei_max_threads()
  {
    return 1;
  }
#endif

  // bRandGens must be a pointer. If not, its construtor may be called after
  // init() is called, which will destroy the bRandGens setup in init().
  static std::vector<boost::mt19937>* bRandGens;
  // Same holds for these:
  static std::vector<gsl_rng*>* gRandGens;
  static std::vector<boost::shared_ptr<boost::mutex> >* mutexes;
  // Keep in mind that it's unsafe to store pointers in std::vectors because
  // they could be discarted without being deallocated. Here it's fine as we
  // won't be resizing or deleting the vector before the end if the program.

  bool Random::initialized_ = Random::init();

  bool Random::init()
  {
    unsigned seed = 0;

    const char * envValPara = getenv("NUKLEI_PARALLELIZATION");
    if (envValPara != NULL)
    {
      std::string para(envValPara);
      if (para == "single" || para == "openmp")
      {
        // all ok
      }
      else if (para == "pthread")
      {
#if defined(NUKLEI_RANDOM_SYNC_OMP) || defined(NUKLEI_RANDOM_SYNC_NONE) || !defined(NUKLEI_RANDOM_SYNC_MUTEX)
        std::cout << "NUKLEI_PARALLELIZATION is set to pthread. "
        "You should manually undefine all NUKLEI_RANDOM_SYNC_* in Random.cpp, "
        "and define NUKLEI_RANDOM_SYNC_MUTEX instead. Note that multithreading "
        "will be slower than with OpenMP. "
        "See http://renaud-detry.net/nuklei/group__faq.html" << std::endl;
        std::terminate();
#endif
      }
      else
      {
        std::cout << "Unknown value '" << para << "' for NUKLEI_PARALLELIZATION"
        " env variable." << std::endl;
        std::terminate();
      }
    }

    const char * envVal = getenv("NUKLEI_RANDOM_SEED");
    if (envVal != NULL)
    {
      const char * log = getenv("NUKLEI_LOG_LEVEL");
      if (log != NULL && numify<unsigned>(log) >= Log::INFO)
        std::cout << "export " << "NUKLEI_RANDOM_SEED" << "="
        << numify<int>(envVal)
        << "\n";
      double seed_d = numify<double>(envVal);
      if (seed_d >= 0)
        seed = numify<unsigned>(envVal);
      else
        seed = time(NULL)*getpid(); // Unsigned don't overflow, they wrap around
    }

    bRandGens = new std::vector<boost::mt19937>(nuklei_max_threads());
    gRandGens = new std::vector<gsl_rng*>(nuklei_max_threads(), NULL);
    for (int i = 0; i < nuklei_max_threads(); i++)
      gRandGens->at(i) = gsl_rng_alloc(gsl_rng_mt19937);

    mutexes = new std::vector<boost::shared_ptr<boost::mutex> >();
    for (int i = 0; i < nuklei_max_threads(); i++)
      mutexes->push_back(boost::shared_ptr<boost::mutex>(new boost::mutex()));

    Random::seed(seed);
    return true;
  }

  void Random::seed(unsigned s)
  {
    if (getenv("NUKLEI_RANDOM_SEED") != NULL)
    {
      // Libraries Nuklei depends on may make use of random numbers.
      // Let's make sure we seed those randomly as well.
      ::srandom(s);
      //BSD implementation of rand differs from random.
      ::srand(s);
    }

    for (int i = 0; i < bRandGens->size(); ++i)
    {
      bRandGens->at(i).seed(s+i);
    }
    for (int i = 0; i < gRandGens->size(); ++i)
    {
      gsl_rng_set(gRandGens->at(i), s+i+1); // +1 because GSL complains when seed == 0
    }
  }

  //This function returns a double precision floating point number
  //uniformly distributed in the range [0,1). The range includes 0.0 but
  //excludes 1.0.
  double Random::uniform()
  {
    double r;
#if defined(NUKLEI_RANDOM_SYNC_OMP)
#  pragma omp critical(nuklei_randomRng)
#elif defined(NUKLEI_RANDOM_SYNC_MUTEX)
    boost::unique_lock<boost::mutex> lock(*mutexes->at(nuklei_thread_num()));
#elif defined(NUKLEI_RANDOM_SYNC_NONE)
#else
#  error Undefined random sync method
#endif
#ifdef NUKLEI_USE_BOOST_RANDOM_GEN
    {
      boost::uniform_01<> dist;
      boost::variate_generator<boost::mt19937&, boost::uniform_01<> >
      die(bRandGens->at(nuklei_thread_num()), dist);
      r = die();
    }
#else
    r = gsl_rng_uniform(gRandGens->at(nuklei_thread_num()));
#endif
    return r;
  }

  //This function returns a double precision floating point number
  //uniformly distributed in the range [a,b). The range includes a but
  //excludes b.
  double Random::uniform(double a, double b)
  {
    NUKLEI_FAST_ASSERT(a < b);
    return a + uniform()*(b-a);
  }

  //This function returns a random integer from 0 to n-1 inclusive by
  //scaling down and/or discarding samples from the generator r. All
  //integers in the range [0,n-1] are produced with equal probability.
  unsigned long int Random::uniformInt(unsigned long int n)
  {
    unsigned long int r;
    // GSL has trouble with concurrent random number generation:
    //   - if a single generator is used, it must be mutexed.
    //   - using one random generator per thread is somehow very slow
    // Random::uniformInt is used *a lot*, everytime a KernelCollection is
    // iterated in random order. Using a mutex here entirely breaks
    // multithreading (n threads on n cpus takes as much time as the same work
    // on a single cpu).  Multithreading will only be fast if done through
    // OpenMP. This is because it is hard to map pthreads to a
    // number. boost::thread::id could be used to implement nuklei_thread_num()
    // (todo?), but random generators could not be cleaned when a thread exits.
#if defined(NUKLEI_RANDOM_SYNC_OMP)
#  pragma omp critical(nuklei_randomRng)
#elif defined(NUKLEI_RANDOM_SYNC_MUTEX)
    boost::unique_lock<boost::mutex> lock(*mutexes->at(nuklei_thread_num()));
#elif defined(NUKLEI_RANDOM_SYNC_NONE)
#else
#  error Undefined random sync method
#endif
#ifdef NUKLEI_USE_BOOST_RANDOM_GEN
    {
      boost::uniform_int<unsigned long> dist(0, n-1);
      boost::variate_generator<boost::mt19937&, boost::uniform_int<unsigned long> >
      die(bRandGens->at(nuklei_thread_num()), dist);
      r = die();
    }
#else
    r = gsl_rng_uniform_int(gRandGens->at(nuklei_thread_num()), n);
#endif
    return r;
  }

  double Random::triangle(double b)
  {
    double r;
#if defined(NUKLEI_RANDOM_SYNC_OMP)
#  pragma omp critical(nuklei_randomRng)
#elif defined(NUKLEI_RANDOM_SYNC_MUTEX)
    boost::unique_lock<boost::mutex> lock(*mutexes->at(nuklei_thread_num()));
#elif defined(NUKLEI_RANDOM_SYNC_NONE)
#else
#  error Undefined random sync method
#endif
    {
      boost::triangle_distribution<> dist(-b/2, 0, b/2);
      boost::variate_generator<boost::mt19937&, boost::triangle_distribution<> >
      die(bRandGens->at(nuklei_thread_num()), dist);
      r = die();
    }
    return r;
  }

  //This function returns a Gaussian random variate, with mean zero and
  //standard deviation sigma. Use the transformation z = \mu + x on the
  //numbers returned by gsl_ran_gaussian to obtain a Gaussian distribution
  //with mean \mu.
  double Random::gaussian(double sigma)
  {
    double r;
#if defined(NUKLEI_RANDOM_SYNC_OMP)
#  pragma omp critical(nuklei_randomRng)
#elif defined(NUKLEI_RANDOM_SYNC_MUTEX)
    boost::unique_lock<boost::mutex> lock(*mutexes->at(nuklei_thread_num()));
#elif defined(NUKLEI_RANDOM_SYNC_NONE)
#else
#  error Undefined random sync method
#endif
#ifdef NUKLEI_USE_BOOST_RANDOM_GEN
    {
      boost::normal_distribution<> dist(0, sigma);
      boost::variate_generator<boost::mt19937&, boost::normal_distribution<> >
      die(bRandGens->at(nuklei_thread_num()), dist);
      r = die();
    }
#else
    r = gsl_ran_gaussian(gRandGens->at(nuklei_thread_num()), sigma);
#endif
    return r;
  }

  double Random::beta(double a, double b)
  {
    double r;
#if defined(NUKLEI_RANDOM_SYNC_OMP)
#  pragma omp critical(nuklei_randomRng)
#elif defined(NUKLEI_RANDOM_SYNC_MUTEX)
    boost::unique_lock<boost::mutex> lock(*mutexes->at(nuklei_thread_num()));
#elif defined(NUKLEI_RANDOM_SYNC_NONE)
#else
#  error Undefined random sync method
#endif
    r = gsl_ran_beta(gRandGens->at(nuklei_thread_num()), a, b);
    return r;
  }

  Vector2 Random::uniformDirection2d()
  {
    Vector2 dir;
#if defined(NUKLEI_RANDOM_SYNC_OMP)
#  pragma omp critical(nuklei_randomRng)
#elif defined(NUKLEI_RANDOM_SYNC_MUTEX)
    boost::unique_lock<boost::mutex> lock(*mutexes->at(nuklei_thread_num()));
#elif defined(NUKLEI_RANDOM_SYNC_NONE)
#else
#  error Undefined random sync method
#endif
#ifdef NUKLEI_USE_BOOST_RANDOM_GEN
    {
      const int dim = 2;
      typedef boost::uniform_on_sphere<double, std::vector<double> > dist_t;
      dist_t dist(dim);
      boost::variate_generator<boost::mt19937&, dist_t >
      die(bRandGens->at(nuklei_thread_num()), dist);
      std::vector<double> r = die();
      dir.X() = r.at(0);
      dir.Y() = r.at(1);
    }
#else
    gsl_ran_dir_2d(gRandGens->at(nuklei_thread_num()), &dir.X(), &dir.Y());
#endif
    return dir;
  }

  Vector3 Random::uniformDirection3d()
  {
    Vector3 dir;
#if defined(NUKLEI_RANDOM_SYNC_OMP)
#  pragma omp critical(nuklei_randomRng)
#elif defined(NUKLEI_RANDOM_SYNC_MUTEX)
    boost::unique_lock<boost::mutex> lock(*mutexes->at(nuklei_thread_num()));
#elif defined(NUKLEI_RANDOM_SYNC_NONE)
#else
#  error Undefined random sync method
#endif
#ifdef NUKLEI_USE_BOOST_RANDOM_GEN
    const int dim = 3;
    typedef boost::uniform_on_sphere<double, std::vector<double> > dist_t;
    dist_t dist(dim);
    boost::variate_generator<boost::mt19937&, dist_t >
    die(bRandGens->at(nuklei_thread_num()), dist);
    std::vector<double> r = die();
    dir.X() = r.at(0);
    dir.Y() = r.at(1);
    dir.Z() = r.at(2);
#else
    gsl_ran_dir_3d(gRandGens->at(nuklei_thread_num()), &dir.X(), &dir.Y(), &dir.Z());
#endif
    return dir;
  }

  Quaternion Random::uniformQuaternion()
  {
    // See Kuffner 2004 and Shoemake 1992.
    // A supposably "slightly faster" way could be read from
    // http://planning.cs.uiuc.edu/node198.html and Arvo 1992, but
    // would need to be tested.
    // Also, how would gsl_ran_dir_nd perform?

#if defined(NUKLEI_RANDOM_QUATERNION_MARSAGLIA_1972)
    coord_t x1, y1, s1, x2, y2, s2;
    for (;;)
    {
      x1 = uniform(-1, 1);
      y1 = uniform(-1, 1);
      s1 = x1*x1 + y1*y1;
      if (s1 < 1) break;
    }
    for (;;)
    {
      x2 = uniform(-1, 1);
      y2 = uniform(-1, 1);
      s2 = x2*x2 + y2*y2;
      if (s2 < 1) break;
    }
    coord_t root = std::sqrt( (1-s1)/s2 );
    Quaternion u(x1,
                 y1,
                 x2 * root,
                 y2 * root);
    return q;
#elif defined(NUKLEI_RANDOM_QUATERNION_GAUSSIAN_PROJ)
    // comparable to gsl_ran_dir_nd
    Quaternion u(gaussian(1),
                 gaussian(1),
                 gaussian(1),
                 gaussian(1));
    u.Normalize();
    return u;
#else // Fastest method (although MARSAGLIA is comparable)
    coord_t s = static_cast<coord_t>(uniform());
    //assert(s <= 1 && s >= 0);
    coord_t s1 = std::sqrt(1-s);
    coord_t s2 = std::sqrt(s);
    coord_t t1 = 2 * M_PI * static_cast<coord_t>(uniform());
    coord_t t2 = 2 * M_PI * static_cast<coord_t>(uniform());
    Quaternion u(std::cos(t2) * s2,
                 std::sin(t1) * s1,
                 std::cos(t1) * s1,
                 std::sin(t2) * s2);
    //assert(nuklei_wmf::Math<coord_t>::FAbs(u.Length()-1) < 1e-6);
    return u;
#endif
  }

  void Random::printRandomState()
  {
    NUKLEI_INFO("Random state: " <<
                NUKLEI_NVP(random()) <<
                "\n              " << NUKLEI_NVP(rand()) <<
                "\n              " << NUKLEI_NVP(Random::uniformInt(1000000)) <<
                "\n              " << NUKLEI_NVP(Random::uniform()));
  }

}


#pragma once
#include <simcem/enum.hpp>
#include <simcem/config.hpp>
#include <boost/algorithm/string.hpp>
#include <stator/symbolic/symbolic.hpp>
#include <algorithm>

namespace simcem {
  struct Reference_t : Enum<Reference_t> {
    using Enum<Reference_t>::Enum;
    enum { Elements298, Elements273, Elements, Self };
    static const constexpr char* const strings[] = {"Elements298", "Elements273", "Elements", "Self"};
  };

  struct Property_t : Enum<Property_t> {
    using Enum<Property_t>::Enum;
    enum { vc, pc, AcentricFactor, Tc};
    static const constexpr char* const strings[] = {"vc", "pc", "AcentricFactor", "Tc"};
  };

  struct Objective_t : Enum<Objective_t> {
    using Enum<Objective_t>::Enum;
    enum { G, H, negS, S, U, A, p, V, T, Cp, Cv, dynvisc, thermcond, density};
    static const constexpr char* const strings[] = {"G", "H", "negS", "S", "U", "A", "p", "V", "T", "Cp", "Cv", "dynvisc", "thermcond", "density"};
  };

  struct T_unit_t : Enum<T_unit_t> {
    using Enum<T_unit_t>::Enum;
    enum { K, C, F, R };
    static const constexpr char* const strings[] = {"K", "C", "F", "R"};
    inline double scale() const {
      static constexpr double data[] = {1.0, 1.0, 5.0/9.0, 5.0/9.0};
      return data[_value];
    }
    inline double origin() const {
      static constexpr double data[] = {0, 273.15, 273.15 - 32.0 * 5.0 / 9.0, 0};
      return data[_value];
    }
  };

  struct Q_unit_t : Enum<Q_unit_t> {
    using Enum<Q_unit_t>::Enum;
    enum { mol, kmol, g, kg, lb};
    static const constexpr char* const strings[] = {"mol", "kmol", "g", "kg", "lb"};
    inline double scale() const {
      static constexpr double data[] = {1.0, 1000.0, 1e-3, 1, 0.45359237};
      return data[_value];
    }
  };

  struct E_unit_t : Enum<E_unit_t> {
    using Enum<E_unit_t>::Enum;
    enum { J, kJ, cal, kcal };
    static const constexpr char* const strings[] = {"J", "kJ", "cal", "kcal"};
    inline double scale() const {
      static constexpr double data[] = {1.0, 1000.0, 4.184, 4184.0};
      return data[_value];
    }
  };

  struct P_unit_t : Enum<P_unit_t> {
    using Enum<P_unit_t>::Enum;
    enum { Pa, bar, at, atm, torr, psi };
    static const constexpr char* const strings[] = {"Pa", "bar", "at", "atm", "torr", "psi"};
    inline double scale() const {
      static constexpr double data[] = {1.0, 1e5, 9.80665e4, 1.01325e5, 101325.0/760, 6.8948e3 };
      return data[_value];
    }
  };

  struct L_unit_t : Enum<L_unit_t> {
    using Enum<L_unit_t>::Enum;
    enum { m, cm, mm, ft, inches};
    static const constexpr char* const strings[] = {"m", "cm", "mm", "ft", "inches"};
    inline double scale() const {
      static constexpr double data[] = {1.0, 0.01, 0.001, 0.3048, 0.3048/12};
      return data[_value];
    }
  };

  struct Data {
    Data():
      _T("K"),
      _Q("mol"),
      _E("J"),
      _P("Pa"),
      _L("m"),
      _mass(0)
    {}

    void xml(stator::xml::Node node) const {
      if (!_source.empty())
	node.add_attribute("Source", _source);
      if (_T != T_unit_t::K)
	node.add_attribute("TUnit", std::string(_T));
      if (_Q != Q_unit_t::mol)
	node.add_attribute("QUnit", std::string(_Q));
      if (_E != E_unit_t::J)
	node.add_attribute("EUnit", std::string(_E));
      if (_P != P_unit_t::Pa)
	node.add_attribute("PUnit", std::string(_P));
      if (_L != L_unit_t::m)
	node.add_attribute("LUnit", std::string(_L));

      for (const auto& comment : _comments) {
	//Don't output empty comments
	if (stator::strip(comment._text).empty() &&
	    (stator::strip(comment._src).empty() || (stator::strip(comment._src) == stator::strip(_source))))
	    continue;
	auto comnode = node.add_node("Comment");
	if (!comment._src.empty())
	  comnode.add_attribute("Source", comment._src);
	comnode = comment._text;
      }
    }

    Data(stator::xml::Node node):
      _T("K"),
      _Q("mol"),
      _E("J"),
      _P("Pa"),
      _L("m"),
      _mass(0)
    {
      //Only load values if present
      for (stator::xml::Node comnode = node.findNode("Comment"); comnode.valid(); ++comnode) {
	std::string src;
	if (comnode.hasAttribute("Source"))
	  src = comnode.getAttribute("Source").as<std::string>();
	_comments.emplace_back(src, comnode);
      }

      if (node.hasAttribute("Source"))
	_source = node.getAttribute("Source").as<std::string>();

      if (node.hasAttribute("TUnit"))
	_T = T_unit_t(node.getAttribute("TUnit"));
      if (node.hasAttribute("QUnit"))
	_Q = Q_unit_t(node.getAttribute("QUnit"));
      if (node.hasAttribute("EUnit"))
	_E = E_unit_t(node.getAttribute("EUnit"));
      if (node.hasAttribute("PUnit"))
	_P = P_unit_t(node.getAttribute("PUnit"));
      if (node.hasAttribute("LUnit"))
	_L = L_unit_t(node.getAttribute("LUnit"));
    }

    Data(std::string comment, std::string source,
	 T_unit_t Tunit, Q_unit_t Qunit, E_unit_t Eunit, P_unit_t Punit, L_unit_t Lunit):
      _comments({{std::string(""),comment}}),
      _source(source),
      _T(Tunit), _Q(Qunit), _E(Eunit), _P(Punit), _L(Lunit),
      _mass(0)
    {}

    double normalise_units(Objective_t variable, double value) const {
      const double origin = (variable == Objective_t::T) ? _T.origin() : 0.0;
      return value * scale(variable) + origin;
    }

    double restore_units(Objective_t variable, double value) const {
      const double origin = (variable == Objective_t::T) ? _T.origin() : 0.0;
      return (value - origin) / scale(variable);
    }

    double scale(Objective_t variable) const {
      double scale = 1;

      double inv_qscale = 1.0 / _Q.scale();
      if ((_Q == Q_unit_t::g) || (_Q == Q_unit_t::kg) || (_Q == Q_unit_t::lb)) {
	if (_mass == 0)
	  stator_throw() << "Cannot use Isobars with mass units without first registering them to a phase.";
	inv_qscale *= _mass;
      }

      switch (variable) {
      case Objective_t::G:
      case Objective_t::H:
      case Objective_t::U:
      case Objective_t::A:
	return _E.scale() * inv_qscale;
      case Objective_t::negS:
      case Objective_t::S:
      case Objective_t::Cp:
      case Objective_t::Cv:
	return _E.scale() * inv_qscale / _T.scale();
      case Objective_t::T:
	return _T.scale();
      case Objective_t::p:
	return _P.scale();
      case Objective_t::V:
	return std::pow(_L.scale(), 3) * inv_qscale;
      case Objective_t::dynvisc:
	return _P.scale(); //i.e. Pa s, but the time scale is always seconds!
      case Objective_t::thermcond:
	return _E.scale() / _L.scale() / _T.scale();
      case Objective_t::density:
	return 1.0 / (inv_qscale * std::pow(_L.scale(), 3));
      default:
	stator_throw() << "Can't handle units for this yet " + std::string(variable);
      };

      return scale;
    }

    std::string LaTeX_units(Objective_t variable) const {
      auto wrapunit = [](std::string u) { return "\\mathrm{"+u+"}"; };
      switch (variable) {
      case Objective_t::G:
      case Objective_t::H:
      case Objective_t::U:
      case Objective_t::A:
	return wrapunit(_E) + "\\," + wrapunit(_Q)+"^{-1}";
      case Objective_t::negS:
      case Objective_t::S:
      case Objective_t::Cp:
      case Objective_t::Cv:
	return wrapunit(_E) + "\\," + wrapunit(_Q)+"^{-1}" + "\\," + wrapunit(_T)+"^{-1}";
      case Objective_t::T:
	return wrapunit(_T);
      case Objective_t::p:
	return wrapunit(_P);
      case Objective_t::V:
	return wrapunit(_L)+"^3" + "\\," + wrapunit(_Q)+"^{-1}";
      case Objective_t::dynvisc:
	return wrapunit(_P)+"\\,"+wrapunit("s");
      case Objective_t::thermcond:
	return wrapunit(_E)+"\\,"+wrapunit(_L)+"^{-1}\\,"+wrapunit(_T)+"^{-1}";
      case Objective_t::density:
	return wrapunit(_Q)+"\\,"+wrapunit(_L)+"^{-3}";
      default:
	stator_throw() << "Can't handle units for this yet " + std::string(variable);
      }
    }

    double normalise_units(Property_t variable, double value) const {
      const double origin = (variable == Property_t::Tc) ? _T.origin() : 0.0;
      return value * scale(variable) + origin;
    }

    double restore_units(Property_t variable, double value) const {
      const double origin = (variable == Property_t::Tc) ? _T.origin() : 0.0;
      return (value - origin) / scale(variable);
    }

    double scale(Property_t variable) const {
      switch (variable) {
      case Property_t::vc:
	return std::pow(_L.scale(), 3) / _Q.scale();
      case Property_t::Tc:
	return _T.scale();
      case Property_t::AcentricFactor:
	return 1.0;
      case Property_t::pc:
	return _P.scale();
      default:
	stator_throw() << "Can't handle units for this yet " + std::string(variable);
      };
    }

    std::string LaTeX_units(Property_t variable) const {
      auto wrapunit = [](std::string u) { return "\\mathrm{"+u+"}"; };
      switch (variable) {
      case Property_t::vc:
	return wrapunit(_L)+"^3\\,"+wrapunit(_Q)+"^{-1}";
      case Property_t::Tc:
	return wrapunit(_T);
      case Property_t::AcentricFactor:
	return "";
      case Property_t::pc:
	return wrapunit(_P);
      default:
	stator_throw() << "Can't handle units for this yet " + std::string(variable);
      }
    }

    void setMass(double mass) { _mass = mass; }

    std::string getSource() { return _source; }

    bool operator==(const Data& d) const {
      return (_comments == d._comments)
	&& (_source == d._source)
	&& (_T == d._T)
	&& (_Q == d._Q)
	&& (_E == d._E)
	&& (_P == d._P)
	&& (_L == d._L)
	&& (_mass == d._mass)
	;
    }

    struct Comment {
      Comment() {}

      Comment(std::string src, std::string text):
	_src(src), _text(text)
      {}

      std::string _src;
      std::string _text;
      bool operator==(const Comment& b) const {
	return (_src == b._src) && (_text == b._text);
      }
    };

    std::vector<Comment> _comments;
    std::string _source;
    T_unit_t _T;
    Q_unit_t _Q;
    E_unit_t _E;
    P_unit_t _P;
    L_unit_t _L;
    double _mass;
  };

  /*! \brief Data structure for a phase of a particular molecule. */
  struct Curve : Data {
    Curve(std::string comment, std::string source, Objective_t variable, Reference_t reference, Objective_t xvar,
	  T_unit_t Tunit, Q_unit_t Qunit, E_unit_t Eunit, P_unit_t Punit, L_unit_t Lunit):
      Data(comment, source, Tunit, Qunit, Eunit, Punit, Lunit),
      _xvar(xvar),
      _variable(variable),
      _reference(reference)
    {}

    Curve(Node xml, Objective_t xvar):
      Data(xml),
      _xvar(xvar),
      _variable(xml.getAttribute("Variable")),
      _reference(xml.getAttribute("Reference"))
    {
      if (xml.hasAttribute("Error"))
	_error = xml.getAttribute("Error");
    }

    virtual ~Curve() {}

    void xml(stator::xml::Node node) const {
      node.add_attribute("Variable", std::string(_variable));

      if (!_error.empty())
	node.add_attribute("Error", _error);

      Data::xml(node);

      node.add_attribute("Reference", std::string(_reference));
      xml_extended(node);
    }

    bool inRange(double x) const {
      return (x >= xmin()) && (x <= xmax());
    }

    virtual double xmin() const = 0;
    virtual double xmax() const = 0;

    virtual sym::Expr getFunction() const = 0;

    virtual void xml_extended(Node) const = 0;

    Objective_t _xvar;
    Objective_t _variable;
    Reference_t _reference;
    double _mass;
    std::string _error;

    Objective_t getVariable() const { return _variable; }

    static shared_ptr<Curve> load(Node, Objective_t);
  };

  struct FunctionCurve: public Curve {
    struct ShomateTerm {
      ShomateTerm(double c, double p): C(c), power(p) {}
      bool operator==(const ShomateTerm&c) const { return (c.C == C) && (power == c.power); }
      double C; double power; };

    static std::string Shomate(const std::vector<ShomateTerm>& terms, double HConst = 0, double SConst = 0) {
      std::string input;

      static auto signwrap = [](std::string a) { return (((a[0] != '-') && (a[0] != '+')) ? "+" : "") + a;  };

      for (const auto& t : terms) {
	std::string coeff = signwrap(stator::repr(t.C));
	if (t.power == -1)
	  input = input + coeff + "*(ln(T)+1)";
	else if (t.power == 0)
	  input = input + coeff + "*T*(1-ln(T))";
	else
	  input = input + signwrap(stator::repr(-t.C / (t.power * (t.power + 1))))+"*T^"+stator::repr(t.power+1);
      }

      if (HConst)
	input = input + "+" + stator::repr(HConst);

      if (SConst)
	input = input + "-T*" + stator::repr(SConst);

      return input;
    }

    static std::string parseOldXML(Node xml) {
      if (xml.getAttribute("Type").as<std::string>() == "Shomate") {
	std::vector<ShomateTerm> terms;

	for (Node tnode = xml.findNode("Term"); tnode.valid(); ++tnode) {
	  const std::string type = tnode.getAttribute("Type");
	  if (type.substr(1,1) != "^") stator_throw() << "Shomate only supports polynomial terms";
	  double C = tnode.getAttribute("C").as<double>();
	  double power = boost::lexical_cast<double>(tnode.getAttribute("Type").as<std::string>().substr(2));
	  terms.push_back(ShomateTerm{C, power});
	}

	double HConst = 0, SConst = 0;
	if (xml.hasNode("HConst"))
	  HConst = xml.getNode("HConst").getAttribute("Value").as<double>();
	if (xml.hasNode("SConst"))
	  SConst = xml.getNode("SConst").getAttribute("Value").as<double>();

	return Shomate(terms, HConst, SConst);
      }

      static auto signwrap = [](std::string a) { return (((a[0] != '-') && (a[0] != '+')) ? "+" : "") + a;  };

      std::string input = "";
      if (xml.hasNode("Function"))
	input = stator::strip(xml.getNode("Function").getValue());

      for (Node tnode = xml.findNode("Term"); tnode.valid(); ++tnode) {
	const std::string type = tnode.getAttribute("Type");
	std::string s_C = tnode.getAttribute("C").as<std::string>();
	s_C = signwrap(s_C);

	if (type.substr(1,1) == "^") {
	  std::string s_power = tnode.getAttribute("Type").as<std::string>().substr(2);
	  input = input + s_C + "*T^" + s_power;
	} else if (type == "lnx") {
	  input = input + s_C +"*ln(T)";
	} else if (type == "xlnx") {
	  input = input + s_C + "*T*ln(T)";
	} else if (type == "exp") {
	  input = input + s_C + "*exp("+parseOldXML(tnode)+")";
	} else
	  stator_throw() << "Unknown term type";
      }

      return input;
    }

    FunctionCurve(Node xml, Objective_t xvar):
      Curve(xml, xvar),
      _x_min(xml.getAttribute("xMin").as<double>()),
      _x_max(xml.getAttribute("xMax").as<double>())
    {
      sym::Var<sym::vidx<'T'>> T;

      _f = sym::Expr(parseOldXML(xml));
      auto df = sym::simplify(sym::derivative(_f, T));

      if (xml.getAttribute("Type").as<std::string>() == "Shomate") {
	double Tref = 298.15;
	if (xml.hasNode("TRef"))
	  Tref = xml.getNode("TRef").getAttribute("Value").as<double>();

	if (xml.hasNode("HRef") && xml.hasNode("SRef")) {
	  auto H_Ref = xml.getNode("HRef").getAttribute("Value").as<double>();
	  auto S_Ref = xml.getNode("SRef").getAttribute("Value").as<double>();

	  auto G_Shift = sym::sub(_f, T = Tref);
	  auto S_Shift = sym::sub(df, T = Tref);

	  _f = _f + H_Ref - G_Shift + Tref * S_Shift - T * (S_Shift + S_Ref);
	}
      }
    }

    virtual double xmin() const {
      return normalise_units(_xvar, _x_min);
    }

    virtual double xmax() const {
      return normalise_units(_xvar, _x_max);
    }

    FunctionCurve(double xmin, double xmax, std::string comment, std::string source, Objective_t variable, Reference_t reference,
		  Objective_t xvar,
		  T_unit_t Tunit, Q_unit_t Qunit, E_unit_t Eunit, P_unit_t Punit, L_unit_t Lunit, sym::Expr f
		  ):
      Curve(comment, source, variable, reference, xvar, Tunit, Qunit, Eunit, Punit, Lunit),
      _x_min(xmin), _x_max(xmax), _f(f)
    {}

    virtual void xml_extended(Node xml) const {
      xml.add_attribute("Type", "Function");
      xml.add_attribute("xMin", _x_min);
      xml.add_attribute("xMax", _x_max);
      xml.add_node("Function") = stator::repr(_f);
    }

    virtual sym::Expr getFunction() const {
      sym::Var<sym::vidx<'T'>> T;
      //Standardise the units of the function
      return scale(_variable) * sym::sub(_f , T = (T - _T.origin()) * (1 / scale(Objective_t(Objective_t::T))));
    }

    virtual std::array<std::string,3> LaTeX() const {
      sym::Expr df = sym::simplify(sym::derivative(_f, sym::VarRT('T')));
      sym::Expr ddf = sym::simplify(sym::derivative(df, sym::VarRT('T')));

      return {{stator::repr<stator::ReprConfig<stator::Latex_output> >(_f),
	    stator::repr<stator::ReprConfig<stator::Latex_output> >(df),
	    stator::repr<stator::ReprConfig<stator::Latex_output> >(ddf)
	    }};
    }

    double _x_min;
    double _x_max;
    sym::Expr _f;
  };

  struct TabulatedCurve: public Curve {
    TabulatedCurve(std::string comment, std::string source, Objective_t variable, Reference_t reference, Objective_t xvar,
		   T_unit_t Tunit, Q_unit_t Qunit, E_unit_t Eunit, P_unit_t Punit, L_unit_t Lunit):
      Curve(comment, source, variable, reference, xvar, Tunit, Qunit, Eunit, Punit, Lunit)
    {}

    TabulatedCurve(Node xml, Objective_t xvar):
      Curve(xml, xvar)
    {
      std::string input = xml.getNode("Data");
      std::vector<std::string> lines;
      boost::split(lines, input, boost::is_any_of("\n"), boost::token_compress_on);

      for (size_t line(0); line < lines.size(); ++line) {
	std::vector<std::string> values;
	boost::trim_right(lines[line]);
	boost::trim_left(lines[line]);
	boost::split(values, lines[line], boost::is_any_of("\t "), boost::token_compress_on);
	if (lines[line] == "")
	  continue;
	if (values.size() == 0)
	  continue;
	if (values[0][0] == '!')
	  continue;
	if ((values.size() != 2) && (values.size() != 3)) {
	  std::ostringstream os;
	  for (const auto & split: values)
	    os << "\"" << split << "\",";

	  stator_throw() << "Tabulated Curve has bad formatting on line " << line+1 << ", xml path " << xml.getPath() << "\nLine content is :\"" << lines[line] << "\"" << "\n" << "string was split as follows " << os.str()
			 << "\nXML:\n" << xml.print();

	}
	if (values.size() == 2)
	  add_point(boost::lexical_cast<double>(values[0]), boost::lexical_cast<double>(values[1]));
	if (values.size() == 3)
	  add_point(boost::lexical_cast<double>(values[0]), boost::lexical_cast<double>(values[1]), boost::lexical_cast<double>(values[2]));
      }
      if  (_values.empty())
	stator_throw() << "Loaded an empty Tabulated Curve? xml path " << xml.getPath()
		       << "\nXML:\n" << xml.print();
    }

    virtual double xmin() const {
      if (_values.empty())
	return +HUGE_VAL;
      return normalise_units(_xvar, _values.front().x);
    }

    virtual double xmax() const {
      if (_values.empty())
	return -HUGE_VAL;
      return normalise_units(_xvar, _values.back().x);
    }

    virtual void xml_extended(Node xml) const {
      xml.add_attribute("Type", "Tabulated");

      std::string data;
      for (const auto& datum : _values) {
	data += "\n" + boost::lexical_cast<std::string>(datum.x) + " " + boost::lexical_cast<std::string>(datum.val);
	if (datum.error != 0)
	  data += " " + boost::lexical_cast<std::string>(datum.error);
      }

      xml.add_node("Data") = data;
    }

    virtual sym::Expr getFunction() const {
      stator_throw() << "Not implemented yet";
      //See commented code below for ideas
    }

    //virtual DataPoint eval_worker(double x) const {
    //  for (size_t i(0); i < _values.size() - 1; ++i)
    //	if ((x >= _values[i].x) && (x <= _values[i+1].x)) {
    //	  const double dfdx  = (_values[i+1].val - _values[i].val) / (_values[i+1].x - _values[i].x);
    //	  const double val = _values[i].val + dfdx * (x - _values[i].x);
    //	  return DataPoint{x, val, dfdx, 0.0};
    //	}
    //
    //  if (x == _values.back().x) {
    //	double dfdx = 0;
    //	if (_values.size() - 1)
    //	  dfdx = (_values[_values.size() - 1].val - _values[_values.size() - 2].val)
    //	    / (_values[_values.size() - 1].x - _values[_values.size() - 2].x);
    //	return DataPoint{x, _values.back().val, dfdx, 0.0};
    //  }
    //
    //  stator_throw() << "Out of range access for Tabulated Curve";
    //}

    void add_point(double x, double val, double error = 0.0) {
      Datum v{x, val, error};
      _values.insert(std::upper_bound(_values.begin(), _values.end(), v), v);
    }

    struct Datum {
      bool operator<(const Datum& d) const { return x < d.x; }
      double x;
      double val;
      double error;
    };
    std::vector<Datum> _values;
  };

  inline
  shared_ptr<Curve> Curve::load(Node xml, Objective_t xvar) {
    try {
      std::string type = xml.getAttribute("Type").as<std::string>();
      if ((type == "Function") || (type == "Shomate"))
	return make_shared<FunctionCurve>(xml, xvar);
      else if (type == "Tabulated")
	return make_shared<TabulatedCurve>(xml, xvar);
      else
	stator_throw() << "Unknown Curve type " << type;
    } catch (const stator::Exception& e) {
      stator_throw() << "Failed parsing Curve\n" << xml.getPath() << "\n" << e.what();
    }
  }
}


/*
 * This file belongs to the Galois project, a C++ library for exploiting
 * parallelism. The code is being released under the terms of the 3-Clause BSD
 * License (a copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include "galois/Galois.h"
#include "galois/Reduction.h"
#include "galois/Bag.h"
#include "galois/Timer.h"
#include "galois/graphs/LCGraph.h"
#include "galois/ParallelSTL.h"
#include "llvm/Support/CommandLine.h"
#include "Lonestar/BoilerPlate.h"

#include "galois/graphs/Graph.h"
#include "galois/runtime/Profile.h"

#include <boost/iterator/transform_iterator.hpp>

#include <utility>
#include <vector>
#include <algorithm>
#include <iostream>
#include <fstream>

//#define DEBUG 0

const char* name = "K-Cliques";
const char* desc = "Counts the number of K-Cliques in a graph for any given K";
const char* url  = 0;

namespace cll = llvm::cl;
static cll::opt<std::string>
    inputFilename(cll::Positional, cll::desc("<input file>"), cll::Required);

static cll::opt<unsigned int>
    clique_size("k", // not uint64_t due to a bug in llvm cl
                cll::desc("Clique Size"), cll::init(3));

typedef galois::graphs::LC_CSR_Graph<uint32_t, void>::with_numa_alloc<
    true>::type ::with_no_lockable<true>::type Graph;

// typedef galois::graphs::MorphGraph<int,int,true,false,false,true> DAG;
typedef galois::graphs::MorphGraph<int, int, true> DAG;

typedef Graph::GraphNode GNode;
typedef DAG::GraphNode DAGNode;

struct WorkItem {
  std::vector<DAGNode> clique;    // set of graph nodes that form a clique
  std::vector<DAGNode> neighbors; // set of neighbors of the clique
};

/**
 * Compares the given two graph nodes n1 and n2.
 * It returns true if
 * 		(1) degree(n1) < degree(n2) (or)
 * 		(2) (degree(n1) = degree(n2)) and (node id of n1 < node id of n2)
 *
 * \param g is the input graph g
 * \param n1 is the first input node
 * \param n2 is the second input node
 * \return True if n1 < n2 else False.
 */

bool lessThan(Graph& g, const GNode& n1, const GNode& n2) {
  int n1_degree = std::distance(g.edge_begin(n1), g.edge_end(n1));
  int n2_degree = std::distance(g.edge_begin(n2), g.edge_end(n2));

  if (n1_degree < n2_degree) {
    return true;
  } else if ((n1_degree == n2_degree) && (g.getData(n1) < g.getData(n2))) {
    return true;
  } else {
    return false;
  }
}

/**
 * Extracts neighbors of a given node of directed acyclic graph (DAG)
 *
 * \param n is the node of a DAG
 * \param neighbors is the output vector that contains the neighbors of node n
 * \param dag is the input DAG
 */

void getNeighbors(DAGNode& n, std::vector<DAGNode>& neighbors, DAG& dag) {
  int i = 0;
  for (auto jj : dag.edges(n)) {
    DAGNode dst  = dag.getEdgeDst(jj);
    neighbors[i] = dst;
    i++;
  }
}

/**
 * Gives the intersection two neighbor vector. It requires the vectors be sorted
 * according to the node ids.
 *
 * \param jointNeighbors is the output neighbor vector
 * \param srcNeighbors is the first input neighbor vector
 * \param dstNeighbors is the second input neighbor vector
 */

size_t intersect(std::vector<DAGNode>& jointNeighbors,
                 std::vector<DAGNode>& srcNeighbors,
                 std::vector<DAGNode>& dstNeighbors) {
  std::vector<DAGNode>::iterator srcItr = srcNeighbors.begin();
  std::vector<DAGNode>::iterator dstItr = dstNeighbors.begin();
  size_t size                           = 0;

  while (srcItr != srcNeighbors.end() && dstItr != dstNeighbors.end()) {
    if (*srcItr < *dstItr) {
      ++srcItr;
    } else if (*dstItr < *srcItr) {
      ++dstItr;
    } else {
      jointNeighbors[size] = (*srcItr);
      size++;
      ++srcItr;
      ++dstItr;
    }
  }
  return size;
}

/**
 * Prints edges of a DAG
 *
 * \param dag is the input directed acyclic graph
 */

void printDAG(DAG& dag) {
  std::cout << "DAG: "
            << "\n";
  for (DAG::iterator ii = dag.begin(); ii != dag.end(); ++ii) {
    DAGNode src = *ii;
    for (DAG::edge_iterator jj = dag.edge_begin(src); jj != dag.edge_end(src);
         ++jj) {
      DAGNode dst = dag.getEdgeDst(jj);
      std::cout << "(" << dag.getData(src) << "," << dag.getData(dst) << ")\n";
    }
  }
}

/**
 * Prints the details of a work item.
 *
 * \param w is the input work item
 * \param dag is a directed acyclic graph
 */

void print_workitem(WorkItem& w, DAG& dag) {
  std::cout << "\n-------------------------------------------------------- \n";
  std::cout << "Clique: ";
  for (std::vector<DAGNode>::iterator v = w.clique.begin(); v != w.clique.end();
       ++v) {
    std::cout << dag.getData(*v) << " ";
  }

  std::cout << "Neighborhood:";
  for (std::vector<DAGNode>::iterator nitr = w.neighbors.begin();
       nitr != w.neighbors.end(); ++nitr) {
    std::cout << dag.getData(*nitr) << " ";
  }
  std::cout << "\n-------------------------------------------------------- \n";
}

/**
 * Prints the vertices of a clique
 *
 * \param w is the input work item which contains a clique
 * \param dag is a directed acyclic graph
 */

void print_clique(WorkItem& w, DAG& dag) {

  std::cout << "Clique: ";
  for (std::vector<DAGNode>::iterator v = w.clique.begin(); v != w.clique.end();
       ++v) {
    std::cout << dag.getData(*v) << " ";
  }
  std::cout << "\n\n";
}

/**
 * Prints the vertices that are present in the neighbors
 *
 * \param neighbors is the input vector whose vertices need to be printed
 * \param dag is a directed acyclic graph
 * \param size the number of vertices in the neighbors to be printed
 */

void print_neighbors(std::vector<DAGNode> neighbors, DAG& dag, int size) {

  int i = 0;
  std::cout << "Neighbors ";
  for (std::vector<DAGNode>::iterator nitr = neighbors.begin();
       nitr != neighbors.end() && (i < size); ++nitr, ++i) {
    std::cout << dag.getData(*nitr) << " ";
  }
  std::cout << "\n";
}

/**
 * This procedure computes the number of k-cliques of a given graph.
 *
 * \param k is the clique size
 * \param dag is a directed acyclic graph, which is obtained using an input
 * graph.
 */

void compute_kcliques(unsigned int k, DAG& dag) {

  galois::InsertBag<WorkItem>
      items; // worklist that monitors the cliques and their neighborhood
  galois::GAccumulator<size_t> num_kcliques; // maintains the number k cliques

  // Initialize the worklist. Each work item contains a single vertex, and all
  // its neighbors.

  galois::StatTimer TWorkInit("Work list initialization");
  galois::StatTimer Tsort("Sorting");
  galois::StatTimer TCliqueResize("Clique Resize");
  galois::StatTimer TNeighborResize("Neighbor Resize");
  galois::StatTimer TProcessWorkItem("Processing work item");
  galois::StatTimer TProcessEdge("Processing Edges");
  galois::StatTimer TIntersect("Neighbor Intersection");
  galois::StatTimer TGetNeighbor("Get Neighbors");

  TWorkInit.start();
  galois::do_all(
      galois::iterate(dag),
      [&](DAGNode n) {
        WorkItem w;

        TCliqueResize.start();
        w.clique.resize(1);
        TCliqueResize.stop();

        w.clique[0]    = n;
        int neigh_size = std::distance(dag.edge_begin(n), dag.edge_end(n));

        TNeighborResize.start();
        w.neighbors.resize(neigh_size);
        TNeighborResize.stop();
        getNeighbors(n, w.neighbors, dag);

        Tsort.start();
        std::sort(w.neighbors.begin(), w.neighbors.end());
        Tsort.stop();
        items.push(w);

#ifdef DEBUG
        print_workitem(w, dag);
#endif
      },
      galois::loopname("Initialize"));
  TWorkInit.stop();

  // Process a work item w containing a clique C and neighbood N.
  auto processClique = [&](WorkItem& w, auto& ctx) {
    TProcessWorkItem.start();

    if (w.clique.size() == k) {
      // If the clique size is k, increment number of k-cliques by 1. No new
      // work item is generated here.
      num_kcliques += 1;

#ifdef DEBUG
      print_clique(w, dag);
#endif
    } else if (w.clique.size() == k - 1) {
      /**
       * Clique size is k-1, so form a new clique of size k by adding a vertex
       * from the neighborhood. So, for a neighborhood for size N, new N
       * K-cliques are formed.
       */

#ifdef DEBUG
      std::cout << "Processing Work Item:";
      print_workitem(w, dag);
#endif

      for (std::vector<DAGNode>::iterator neighitr = w.neighbors.begin();
           neighitr != w.neighbors.end(); ++neighitr) {
        WorkItem nextItem;

        TCliqueResize.start();
        nextItem.clique.resize(w.clique.size() + 1);
        TCliqueResize.stop();

        int i = 0;
        for (std::vector<DAGNode>::iterator v = w.clique.begin();
             v != w.clique.end(); ++v) {
          nextItem.clique[i] = (*v);
          i++;
        }
        nextItem.clique[i] = *neighitr;

#ifdef DEBUG
        print_clique(nextItem, dag);
#endif

        num_kcliques += 1;
      }
    } else {
      /**
       * For each edge u->v present in N, construct a new work W with (C', N')
       * and insert it into the work list where C' = C U {u,v} and N' =
       * intersection{N, neighbors of u, neighbors of v)
       */

      for (std::vector<DAGNode>::iterator src = w.neighbors.begin();
           src != w.neighbors.end(); ++src) {
        for (std::vector<DAGNode>::iterator dst = w.neighbors.begin();
             dst != w.neighbors.end(); ++dst) {
          if ((dag.getData(*src) != dag.getData(*dst)) &&
              (dag.findEdge(*src, *dst) !=
               dag.edge_end(*src, galois::MethodFlag::UNPROTECTED))) {

            TProcessEdge.start();
#ifdef DEBUG
            std::cout << "Processing Work Item:";
            print_workitem(w, dag);
#endif

            WorkItem nextItem;

            TCliqueResize.start();
            nextItem.clique.resize(w.clique.size() + 2);
            TCliqueResize.stop();

            int i = 0;

            for (std::vector<DAGNode>::iterator itr = w.clique.begin();
                 itr != w.clique.end(); ++itr) {
              nextItem.clique[i] = (*itr);
              i++;
            }
            nextItem.clique[i]     = *src;
            nextItem.clique[i + 1] = *dst;

            TGetNeighbor.start();
            std::vector<DAGNode> srcNeighbors;
            std::vector<DAGNode> dstNeighbors;
            std::vector<DAGNode> jointNeighbors;
            std::vector<DAGNode> tmpNeighbors;

            size_t srcDegree =
                std::distance(dag.edge_begin(*src), dag.edge_end(*src));
            size_t dstDegree =
                std::distance(dag.edge_begin(*dst), dag.edge_end(*dst));

            TGetNeighbor.stop();

            TNeighborResize.start();
            srcNeighbors.resize(srcDegree);
            dstNeighbors.resize(dstDegree);
            tmpNeighbors.resize(std::min(srcDegree, w.neighbors.size()));
            TNeighborResize.stop();

            TGetNeighbor.start();
            getNeighbors(*src, srcNeighbors, dag);
            TGetNeighbor.stop();

            Tsort.start();
            std::sort(srcNeighbors.begin(), srcNeighbors.end());
            Tsort.stop();

            getNeighbors(*dst, dstNeighbors, dag);

            Tsort.start();
            std::sort(dstNeighbors.begin(), dstNeighbors.end());
            Tsort.stop();

            TIntersect.start();
            size_t tmp_neighborsize =
                intersect(tmpNeighbors, srcNeighbors, w.neighbors);
            TIntersect.stop();

            nextItem.neighbors.resize(tmp_neighborsize);

            TIntersect.start();
            size_t nextItem_neighborsize =
                intersect(nextItem.neighbors, dstNeighbors, tmpNeighbors);
            TIntersect.stop();

            TNeighborResize.start();
            nextItem.neighbors.resize(nextItem_neighborsize);
            TNeighborResize.stop();

            ctx.push(nextItem);

#ifdef DEBUG
            std::cout << "Processing an edge with Source : "
                      << dag.getData(*src)
                      << " Destination : " << dag.getData(*dst) << "\n";
            std::cout << "Source Neighborhood: ";
            print_neighbors(srcNeighbors, dag, srcNeighbors.size());
            std::cout << "Temporary Neighborhood: Intersection of Source "
                         "Neighborhood and WorkItem Neighborhood: ";
            print_neighbors(tmpNeighbors, dag, tmp_neighborsize);
            std::cout << "Destination Neighborhood: ";
            print_neighbors(dstNeighbors, dag, dstNeighbors.size());
            std::cout << "NextItem Neighborhood: Intersection of Temporary "
                         "Neighborhood and Destination Neighborhood: ";
            print_neighbors(nextItem.neighbors, dag, nextItem_neighborsize);
#endif

            TProcessEdge.stop();
          }
        }
      }
    }
    TProcessWorkItem.stop();
  };

  // parallel loop that processes work list until it is empty.
  galois::for_each(
      galois::iterate(items), // initial range using initializer list
      processClique,          // operator
      galois::loopname("process clique"), galois::steal());

  std::cout << "Number of K-Cliques: " << num_kcliques.reduce() << "\n";
}

/*
 * For a given graph (G), it constructs a directed acyclic graph (DAG)
 * corresponding to it. The graph DAG represents a total order among the
 * vertices of the graph G. Procedure: V (DAG) = V(G) An edge u->v is present in
 * DAG if (1) (u,v) is in E(G) and (2) (degree(u) < degree(v)) or (degree(u) =
 * degree(v) and u < v)
 *
 * \param graph is the input graph
 * \param dag is the output directed acyclic graph
 */
void constructDAG(Graph& graph, DAG& dag) {

  std::vector<DAGNode> nodes;
  nodes.resize(graph.size());

  // Creating the DAG vertices
  galois::do_all(galois::iterate(graph),
                 [&graph, &dag, &nodes](GNode N) {
                   int index    = graph.getData(N);
                   nodes[index] = dag.createNode(index);
                   dag.addNode(nodes[index]);
                 } // operator as lambda expression
  );

  // Adding the edges depending on the degree of source and destination.
  galois::do_all(galois::iterate(graph),
                 [&graph, &dag, &nodes](GNode N) {
                   for (Graph::edge_iterator edge :
                        graph.out_edges(N, galois::MethodFlag::UNPROTECTED)) {
                     GNode dst = graph.getEdgeDst(edge);
                     if (lessThan(graph, N, dst)) {
                       DAGNode srcNode = nodes[graph.getData(N)];
                       DAGNode dstNode = nodes[graph.getData(dst)];
                       dag.addEdge(srcNode, dstNode);
                     }
                   }
                 } // operator as lambda expression
  );

#ifdef DEBUG
  printDAG(dag);
#endif
}

/**
 * Read a graph from the given input file (inputFileName)
 *
 * \param graph is the output graph
 */
void readGraph(Graph& graph) {

  galois::graphs::readGraph(graph, inputFilename);
  size_t index = 0;
  for (GNode n : graph) {
    graph.getData(n) = index++;
  }
}

/**
 * This is an implementation for computing number of k-cliques in a given graph.
 * It uses the edge parallel algorithm described in Danisch et al. WWW 2018
 * (https://dl.acm.org/citation.cfm?id=3186125). It is implemented using Galois
 * framework.
 */

int main(int argc, char** argv) {
  galois::SharedMemSys G;
  LonestarStart(argc, argv, name, desc, url);

  Graph graph;
  DAG dag;

  // Read the input Graph
  galois::StatTimer Tinitial("GraphReadingTime");
  Tinitial.start();
  readGraph(graph);
  Tinitial.stop();

  // Constructing a DAG from the original graph
  galois::StatTimer Tdag("DAG Construction Time");
  Tdag.start();
  constructDAG(graph, dag);
  Tdag.stop();

  galois::preAlloc(numThreads + 16 * (graph.size() + graph.sizeEdges()) /
                                    galois::runtime::pagePoolSize());
  galois::reportPageAlloc("MeminfoPre");

  galois::StatTimer T;
  T.start();
  // the main procedure that computes k-cliques
  compute_kcliques(clique_size, dag);

  T.stop();

  galois::reportPageAlloc("MeminfoPost");
  return 0;
}


#ifndef MTL_COORDINATE2D_INCLUDE
#define MTL_COORDINATE2D_INCLUDE

#include <vector>
#include <cassert>
#include <boost/mpl/bool.hpp>

#include <boost/numeric/mtl/operation/is_negative.hpp>
#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/operation/sort.hpp>
#include <boost/numeric/mtl/operation/iota.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/utility/is_row_major.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>

namespace mtl {  namespace mat {

/// Sparse matrix structure in coordinate format
template <typename Value, typename Parameters = mat::parameters<> >
class coordinate2D
  : public base_matrix<Value, Parameters>,
    public const_crtp_base_matrix< coordinate2D<Value, Parameters>, Value, typename Parameters::size_type >,
    public crtp_matrix_assign< coordinate2D<Value, Parameters>, Value, typename Parameters::size_type >,
    public mat_expr< coordinate2D<Value, Parameters> >
{
  public:
    typedef Value                                                       value_type;
    typedef Value&                                                      reference;
    typedef Value const&                                                const_reference;
    typedef typename Parameters::size_type				  size_type;
    typedef typename Parameters::dimensions                             dim_type;
    typedef typename Parameters::orientation                            orientation;

    typedef coordinate2D                                                self;
    typedef base_matrix<Value, Parameters>                              super;
    typedef crtp_matrix_assign<self, Value, size_type>                  assign_base;

    typedef std::vector< size_type >                                    row_index_array_type ;
    typedef std::vector< size_type >                                    column_index_array_type ;
    typedef std::vector< value_type >                                   value_array_type ;

    /// Common constructor
    explicit coordinate2D(size_type nrows, size_type ncols, size_type expected= 0)
      : super(dim_type(nrows, ncols))
    {
	if (expected > 0) {
	    rows.reserve(expected);
	    cols.reserve(expected);
	    values.reserve(expected);
	}
	my_is_sorted= true;
    }

    using assign_base::operator=;

    size_type nnz() const { return rows.size(); } ///< Number of non-zeros

    value_array_type const& value_array() const { return values; } ///< Array of values (const)
    value_array_type& value_array() { return values; }             ///< Array of values (mutable)

    row_index_array_type const& row_index_array() const { return rows; }       ///< Array of rows  (const)
    column_index_array_type const& column_index_array() const {	return cols; } ///< Array of columns (const)

    row_index_array_type& row_index_array() { return rows; }       ///< Array of rows   (mutable)
    column_index_array_type& column_index_array() { return cols; } ///< Array of columns  (mutable)

    /// Drop all entries
    void make_empty()
    {
        rows.resize(0); cols.resize(0); values.resize(0);
        my_is_sorted= true; // haha
    }

    /// Insert an entry at the end of the row-,col- and value-array
    void push_back(size_type r, size_type c, const_reference v)
    {
	rows.push_back(r); cols.push_back(c); values.push_back(v); my_is_sorted= false;
    }

    /// Insert an entry at the end of the row-,col- and value-array, like push_back
    void insert(size_type r, size_type c, const_reference v) {	push_back(r, c, v); }

    /// Whether the entries are sorted
    bool is_sorted() const { return my_is_sorted; }

    /// sorting standard by rows
    void sort()
    {
	if (nnz() > 0)
	    sort(mtl::traits::is_row_major<Parameters>());
	my_is_sorted= true;
    }

  private:
    // sorting by rows
    void sort(boost::mpl::true_)
    {
	mtl::vec::sort_xy(rows, cols, values);
    }

    // sorting by columns
    void sort(boost::mpl::false_)
    {
	mtl::vec::sort_xy(cols, rows, values);
    }

    template <typename OStream, typename Vector>
    void print_stl_vector(OStream& os, const Vector& v) const
    {
	os << "[";
	for (unsigned i= 0; i < v.size(); i++)
	    os << v[i] << (i+1 < v.size() ? "," : "");
	os << "]\n";
    }

  public:
    template <typename OStream>
    void print_internal(OStream& os) const
    {
       	os << "rows   = "; print_stl_vector(os, rows);
	os << "cols   = "; print_stl_vector(os, cols);
	os << "values = "; print_stl_vector(os, values);
    }

    void print_internal() const { print(std::cout); }

    ///operator * for  vector= coordinaten-matrix * vector
    template <typename Vector >
    Vector operator*(const Vector& x)
    {

	Vector res(this->num_rows());
	res= 0;
	for (size_type i= 0; i < nnz(); i++)
	    res[rows[i]]+=  values[i] * x[cols[i]];
	return res;
    }

    value_type operator() (const size_type r, const size_type c) const
    {
	MTL_CRASH_IF(is_negative(r) || r >= this->num_rows()
		  || is_negative(c) || c >= this->num_cols(), "Index out of range!");

#if 0
	if (my_is_sorted)
	    return find(r, c, mtl::traits::is_row_major<Parameters>());
#endif

	for (size_type i= 0; i < nnz(); i++)
	    if (rows[i] == r && cols[i] == c)
		return values[i];
	return value_type(0);
    }

    template <typename Updater>
    void compress(Updater up)
    {
	if (!my_is_sorted)
	    sort();

	size_type i= 0, j= 1, end= rows.size();
	for (; j < end; ++j)
	    if (rows[i] == rows[j] && cols[i] == cols[j]) {
		up(values[i], values[j]);
	    } else {
		i++;
		if (i != j) {
		    rows[i]= rows[j];
		    cols[i]= cols[j];
		    values[i]= values[j];
		}
	    }
	if (end > 0) i++;
	rows.resize(i);
	cols.resize(i);
	values.resize(i);
   }

    template <typename Matrix, typename Updater> friend struct coordinate2D_inserter;

  private:
    row_index_array_type      rows;
    column_index_array_type   cols;
    value_array_type          values;
    bool                      my_is_sorted;
};

// ================
// Free functions
// ================

/// Number of rows
template <typename Value, typename Parameters>
typename coordinate2D<Value, Parameters>::size_type
inline num_rows(const coordinate2D<Value, Parameters>& matrix)
{
    return matrix.num_rows();
}

/// Number of columns
template <typename Value, typename Parameters>
typename coordinate2D<Value, Parameters>::size_type
inline num_cols(const coordinate2D<Value, Parameters>& matrix)
{
    return matrix.num_cols();
}

/// Size of the matrix, i.e. the number of row times columns
template <typename Value, typename Parameters>
typename coordinate2D<Value, Parameters>::size_type
inline size(const coordinate2D<Value, Parameters>& matrix)
{
    return matrix.num_cols() * matrix.num_rows();
}

/// Number of NoZeros of the matrix
template <typename Value, typename Parameters>
typename coordinate2D<Value, Parameters>::size_type
inline nnz(const coordinate2D<Value, Parameters>& matrix)
{
    return matrix.nnz();
}

template <typename Matrix,
	  typename Updater = mtl::operations::update_store<typename Matrix::value_type> >
struct coordinate2D_inserter
{
    typedef coordinate2D_inserter                       self;
    typedef Matrix                                      matrix_type;
    typedef typename matrix_type::size_type             size_type;
    typedef typename matrix_type::value_type            value_type;
    typedef operations::update_proxy<self, size_type>   proxy_type;

    // We only support storing so far !!!
    // STATIC_ASSERT((boost::is_same<Updater, mtl::operations::update_store<value_type> >::value), "We only support storing so far");

    coordinate2D_inserter(matrix_type& matrix, size_type slot_size= 1)
      : matrix(matrix)
    {
	std::size_t ns= slot_size * matrix.dim1();
	if (ns > matrix.nnz()) {
	    matrix.rows.reserve(ns);
	    matrix.cols.reserve(ns);
	    matrix.values.reserve(ns);
	}
    }

    ~coordinate2D_inserter() { matrix.compress(Updater()); }

 private:

    struct update_proxy
    {
	// self is type of inserter not update_proxy !!!
	update_proxy(self& ref, size_type row, size_type col) : ref(ref), row(row), col(col) {}

	template <typename Value>
	update_proxy& operator<< (Value const& val)
	{
	    ref.matrix.push_back(row, col, val);
	    return *this;
	}
	self& ref;
	size_type row, col;
    };

    proxy_type operator() (size_type row, size_type col)
    {
	return proxy_type(*this, row, col);
    }

    struct bracket_proxy
    {
	bracket_proxy(self& ref, size_type row) : ref(ref), row(row) {}

	proxy_type operator[](size_type col)
	{
	    return proxy_type(ref, row, col);
	}

	self&      ref;
	size_type  row;
    };

  public:

    bracket_proxy operator[] (size_type row)
    {
	return bracket_proxy(*this, row);
    }

    template <typename Value>
    void update(size_type row, size_type col, Value val)
    {
	matrix.push_back(row, col, val);
    }

    template <typename Modifier, typename Value>
    void modify(size_type row, size_type col, Value val)
    {
	matrix.push_back(row, col, val);
    }

    template <typename EMatrix, typename Rows, typename Cols>
    self& operator<< (const mat::element_matrix_t<EMatrix, Rows, Cols>& elements)
    {
	using mtl::size;
	for (unsigned ri= 0; ri < size(elements.rows); ri++)
	    for (unsigned ci= 0; ci < size(elements.cols); ci++)
		update (elements.rows[ri], elements.cols[ci], elements.matrix(ri, ci));
	return *this;
    }

    template <typename EMatrix, typename Rows, typename Cols>
    self& operator<< (const mat::element_array_t<EMatrix, Rows, Cols>& elements)
    {
	using mtl::size;
	for (unsigned ri= 0; ri < size(elements.rows); ri++)
	    for (unsigned ci= 0; ci < size(elements.cols); ci++)
		update (elements.rows[ri], elements.cols[ci], elements.array[ri][ci]);
	return *this;
    }

  protected:
    matrix_type&         matrix;
};

struct coordinate_key
{
    typedef std::size_t                               size_t;

    explicit coordinate_key(size_t offset) : offset(offset) {}

    bool operator== (coordinate_key const& other) const { return offset == other.offset; }
    bool operator!= (coordinate_key const& other) const { return offset != other.offset; }

    size_t offset;
};

// Cursor over every element
template <typename Value, typename Parameters>
struct coordinate_minor_cursor
 : public coordinate_key
{
    typedef coordinate_minor_cursor<Value, Parameters>   self;
    typedef typename Parameters::size_type               size_type;
    typedef const coordinate2D<Value, Parameters>&       matrix_ref_type;
    static const int                                     level= 2;

    coordinate_minor_cursor(matrix_ref_type ref, size_type offset)
      : coordinate_key(offset), ref(ref)  {}

    bool operator!=(const self& that) const
    {
	assert(&ref == &that.ref);
	return this->offset != that.offset;
    }

    self& operator++() { this->offset++; return *this; }
    coordinate_key operator*() const { return *this; }

    matrix_ref_type ref;
};

template <typename Value, typename Parameters>
struct coordinate_major_cursor
{
    typedef coordinate_major_cursor<Value, Parameters>   self;
    typedef typename Parameters::size_type               size_type;
    typedef const coordinate2D<Value, Parameters>&       matrix_ref_type;
    typedef coordinate_minor_cursor<Value, Parameters>   inner_cursor;
    static const int                                     level= 2;

    void find_next_offset(boost::mpl::true_)
    {
	size_type i= offset;
	for ( ; i < nnz(ref) && ref.row_index_array()[i] <= major; i++) ;
	next_offset= i;
    }

    void find_next_offset(boost::mpl::false_)
    {
	size_type i= offset;
	for ( ; i < nnz(ref) && ref.col_index_array()[i] <= major; i++) ;
	next_offset= i;
    }

    void find_next_offset() { find_next_offset(mtl::traits::is_row_major<Parameters>()); }

    coordinate_major_cursor(matrix_ref_type ref, size_type major, size_type offset)
      : ref(ref), major(major), offset(offset)
    {
	find_next_offset();
    }

    bool operator!=(const self& that) const
    {
	assert(&ref == &that.ref);
	return this->offset != that.offset;
    }

    self& operator++()
    {
	offset= next_offset;
	major++;
	find_next_offset();
	return *this;
    }

    matrix_ref_type ref;
    size_type       major, offset, next_offset;
};

template <typename Value, typename Parameters>
struct coordinate_minor_range_generator
{
    typedef coordinate_major_cursor<Value, Parameters>   outer_cursor_type;
    typedef coordinate_minor_cursor<Value, Parameters>   type;
    static const int                                     level= 2;

    type begin(outer_cursor_type c) const { return type(c.ref, c.offset); }
    type end(outer_cursor_type c) const { return type(c.ref, c.next_offset); }
};

template <typename Value, typename Parameters>
struct coordinate_row_range_generator
{
    typedef const coordinate2D<Value, Parameters>&       matrix_ref_type;
    typedef coordinate_major_cursor<Value, Parameters>   type;
    typedef complexity_classes::linear_cached            complexity;
    static const int                                     level= 1;

    type begin(matrix_ref_type A) const { return type(A, 0, 0); }
    type end(matrix_ref_type A) const { return type(A, num_rows(A), nnz(A)); }
};

template <typename Value, typename Parameters>
struct coordinate_col_range_generator
{
    typedef const coordinate2D<Value, Parameters>&       matrix_ref_type;
    typedef coordinate_major_cursor<Value, Parameters>   type;
    typedef complexity_classes::linear_cached            complexity;
    static const int                                     level= 1;

    type begin(matrix_ref_type A) const { return type(A, 0, 0); }
    type end(matrix_ref_type A) const { return type(A, num_cols(A), nnz(A)); }
};

}} // namespace mtl::matrix

namespace mtl { namespace traits {

    // Cursor over all rows
    // Supported if row major matrix
    template <typename Value, typename Parameters>
    struct range_generator<glas::tag::row, mat::coordinate2D<Value, Parameters> >
      : boost::mpl::if_<
	    boost::is_same<typename Parameters::orientation, row_major>
	  , mat::coordinate_row_range_generator<Value, Parameters>
 	  , range_generator<tag::unsupported, mat::coordinate2D<Value, Parameters> >
        >::type {};

    template <typename Value, typename Parameters>
    struct range_generator<glas::tag::col, mat::coordinate2D<Value, Parameters> >
      : boost::mpl::if_<
	    boost::is_same<typename Parameters::orientation, col_major>
	  , mat::coordinate_col_range_generator<Value, Parameters>
 	  , range_generator<tag::unsupported, mat::coordinate2D<Value, Parameters> >
        >::type {};

    template <class Value, class Parameters>
    struct range_generator<glas::tag::nz, mat::coordinate_major_cursor<Value, Parameters> >
      : mat::coordinate_minor_range_generator<Value, Parameters>
    {};

}} // namespace mtl::traits

namespace mtl {
	using mat::coordinate2D;
}

#endif // MTL_COORDINATE2D_INCLUDE


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
#define CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_

#include <stdexcept>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/utils/to-string.hpp"

namespace crocoddyl {

/**
 * @brief This class DifferentialActionModelAbstract represents a first-order
 * ODE, i.e.
 * \f[
 * \mathbf{\dot{v}} = \mathbf{f}(\mathbf{q}, \mathbf{v}, \boldsymbol{\tau})
 * \f]
 * where \f$ xout = \mathbf{\dot{v}} \f$ and represents the  acceleration of the
 * system. Note that Jacobians Fx and Fu in the
 * DifferentialActionDataAbstract are in \f$ \mathbb{R}^{nv\times ndx} \f$ and
 * \f$ \mathbb{R}^{nv\times nu} \f$, respectively.
 *
 * Then we use the acceleration to integrate the system, and as consequence we
 * obtain:
 * \f[
 * \mathbf{\dot{x}} = (\mathbf{v}, \mathbf{\dot{v}}) = \mathbf{f}(\mathbf{x},\mathbf{u})
 * \f]
 * where this \f$ f \f$ function is different to the other one.
 * So \f$ xout \f$ is interpreted here as \f$ vdout \f$ or \f$ aout \f$.
 */

template <typename _Scalar>
class DifferentialActionModelAbstractTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  DifferentialActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t& nu,
                                     const std::size_t& nr = 0);
  virtual ~DifferentialActionModelAbstractTpl();

  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u) = 0;
  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
                        const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) = 0;
  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();

  void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
  void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  const std::size_t& get_nu() const;
  const std::size_t& get_nr() const;
  const boost::shared_ptr<StateAbstract>& get_state() const;

  const VectorXs& get_u_lb() const;
  const VectorXs& get_u_ub() const;
  bool const& get_has_control_limits() const;

  void set_u_lb(const VectorXs& u_lb);
  void set_u_ub(const VectorXs& u_ub);

 protected:
  std::size_t nu_;                          //!< Control dimension
  std::size_t nr_;                          //!< Dimension of the cost residual
  boost::shared_ptr<StateAbstract> state_;  //!< Model of the state
  VectorXs unone_;                          //!< Neutral state
  VectorXs u_lb_;                           //!< Lower control limits
  VectorXs u_ub_;                           //!< Upper control limits
  bool has_control_limits_;                 //!< Indicates whether any of the control limits is finite

  void update_has_control_limits();
};

template <typename _Scalar>
struct DifferentialActionDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  template <template <typename Scalar> class Model>
  explicit DifferentialActionDataAbstractTpl(Model<Scalar>* const model)
      : cost(0.),
        xout(model->get_state()->get_nv()),
        Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
        Fu(model->get_state()->get_nv(), model->get_nu()),
        r(model->get_nr()),
        Lx(model->get_state()->get_ndx()),
        Lu(model->get_nu()),
        Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
        Lxu(model->get_state()->get_ndx(), model->get_nu()),
        Luu(model->get_nu(), model->get_nu()) {
    xout.setZero();
    r.setZero();
    Fx.setZero();
    Fu.setZero();
    Lx.setZero();
    Lu.setZero();
    Lxx.setZero();
    Lxu.setZero();
    Luu.setZero();
  }
  virtual ~DifferentialActionDataAbstractTpl() {}

  Scalar cost;
  VectorXs xout;
  MatrixXs Fx;
  MatrixXs Fu;
  VectorXs r;
  VectorXs Lx;
  VectorXs Lu;
  MatrixXs Lxx;
  MatrixXs Lxu;
  MatrixXs Luu;
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/diff-action-base.hxx"

#endif  // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_


/**
 * @file uninformed_bidirectional_pruning.hpp
 * @author Leonardo Arcari (leonardo1.arcari@gmail.com)
 * @version 1.0.0
 * @date 2018-10-28
 *
 * @copyright Copyright (c) 2018 Leonardo Arcari
 *
 * MIT Licence
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#ifndef BOOST_GRAPH_PRUNING_ALGORITHMS_HPP
#define BOOST_GRAPH_PRUNING_ALGORITHMS_HPP

#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/properties.hpp>

#include <arlib/details/arlib_utils.hpp>
#include <arlib/routing_kernels/bidirectional_dijkstra.hpp>
#include <arlib/routing_kernels/visitor.hpp>
#include <arlib/type_traits.hpp>

#include <arlib/details/ubp_impl.hpp>

#include <limits>
#include <unordered_set>
#include <vector>

/**
 * An Alternative-Routing library for Boost.Graph
 */
namespace arlib {
template <typename Graph>
using PrunedGraph =
    boost::filtered_graph<Graph, details::pruned_edges<edge_of_t<Graph>>>;
/**
 * An implementation of Uninformed Bidirectional Pruning for Boost::Graph.
 *
 * This implementation refers to the following publication:
 *
 * Andreas Paraskevopoulos, Christos Zaroliagis. Improved Alternative Route
 * Planning. Daniele Frigioni and Sebastian Stiller. ATMOS - 13th Workshop on
 * Algorithmic Approaches for Transportation Modelling, Optimization, and
 * Systems - 2013, Sep 2013, Sophia Antipolis, France. Schloss
 * Dagstuhl–Leibniz-Zentrum fuer Informatik, 33, pp.108–122, 2013, OpenAccess
 * Series in Informatics (OASIcs).
 *
 * @tparam Graph A Boost::VertexAndEdgeListGraph
 * @tparam WeightMap The weight or "length" of each edge in the graph. The
 *         weights must all be non-negative, and the algorithm will throw a
 *         negative_edge exception is one of the edges is negative. The type
 *         WeightMap must be a model of Readable Property Map. The edge
 *         descriptor type of the graph needs to be usable as the key type for
 *         the weight map. The value type for this map must be the same as the
 *         value type of the distance map.
 * @tparam RevWeightMap Same as `WeightMap`, but for
 *         `boost::reverse_graph<Graph>`
 * @param G The input graph.
 * @param weight_f The weight map of `G`.
 * @param rev_G A `boost::reverse_graph` of `G`
 * @param weight_b The weight map of `rev_G`.
 * @param s The source node.
 * @param t The target node.
 * @param tau The pruning factor.
 * @return A pruned copy of `G`.
 */
template <typename Graph, typename WeightMap, typename RevWeightMap,
          typename Vertex = vertex_of_t<Graph>>
PrunedGraph<Graph>
uninformed_bidirectional_pruner(const Graph &G, WeightMap const &weight_f,
                                boost::reverse_graph<Graph> const &rev_G,
                                RevWeightMap const &weight_b, Vertex s,
                                Vertex t, double tau) {
  using namespace boost;
  using Length = typename boost::property_traits<WeightMap>::value_type;
  using Edge = typename graph_traits<Graph>::edge_descriptor;
  using RevEdge =
      typename graph_traits<boost::reverse_graph<Graph>>::edge_descriptor;

  BOOST_CONCEPT_ASSERT((VertexAndEdgeListGraphConcept<Graph>));
  BOOST_CONCEPT_ASSERT(
      (VertexAndEdgeListGraphConcept<boost::reverse_graph<Graph>>));
  BOOST_CONCEPT_ASSERT((LvaluePropertyMapConcept<WeightMap, Edge>));
  BOOST_CONCEPT_ASSERT((LvaluePropertyMapConcept<RevWeightMap, RevEdge>));

  // Instantiate data structures
  auto distance_f_vec = std::vector<Length>(num_vertices(G), 6);
  auto predecessor_f_vec =
      std::vector<Vertex>(vertices(G).first, vertices(G).second);
  auto vertex_id = get(vertex_index, G);
  auto distance_f =
      make_iterator_property_map(std::begin(distance_f_vec), vertex_id);
  auto predecessor_f =
      make_iterator_property_map(std::begin(predecessor_f_vec), vertex_id);

  // Reverse tree
  auto vertex_id_b = get(vertex_index, G);
  auto distance_b_vec = std::vector<Length>(num_vertices(rev_G));
  auto predecessor_b_vec =
      std::vector<Vertex>(vertices(rev_G).first, vertices(rev_G).second);
  auto predecessor_b =
      make_iterator_property_map(std::begin(predecessor_b_vec), vertex_id_b);

  auto distance_b =
      make_iterator_property_map(std::begin(distance_b_vec), vertex_id_b);

  // Pruning visitor
  auto pruning_visitor = UninformedBiPrunerVisitor<Vertex>{tau};

  bidirectional_dijkstra(G, s, t, predecessor_f, distance_f, weight_f, rev_G,
                         predecessor_b, distance_b, weight_b, pruning_visitor);

  // auto pruned_G = Graph{G};
  auto prd_edges = std::unordered_set<Edge, boost::hash<Edge>>{};

  auto sp = details::build_edge_list_from_dijkstra(G, s, t, predecessor_f);
  auto final_distance =
      details::compute_length_from_edges(sp.begin(), sp.end(), weight_f);
  for (auto [v_it, v_end] = pruning_visitor.get_pruned_vertices();
       v_it != v_end; ++v_it) {
    bool should_prune =
        details::pruning_policy(s, t, *v_it, predecessor_f, predecessor_b,
                                distance_f, distance_b, tau, final_distance);
    if (should_prune) {
      for (auto [adj_it, adj_end] = adjacent_vertices(*v_it, G);
           adj_it != adj_end; ++adj_it) {
        auto u = *adj_it;
        auto v = *v_it;

        auto [uv_e, uv_exists] = edge(u, v, G);
        if (uv_exists) {
          prd_edges.insert(uv_e);
        }

        auto [vu_e, vu_exists] = edge(v, u, G);
        if (vu_exists) {
          prd_edges.insert(vu_e);
        }
      }
    }
  }

  const auto pruned_G =
      filtered_graph(G, details::pruned_edges{std::move(prd_edges)});
  return pruned_G;
}

/**
 * An implementation of Uninformed Bidirectional Pruning for Boost::Graph.
 *
 * This overload takes an input graph modeling `PropertyGraph` concept having at
 * least one edge property with tag `boost::edge_weight_t`. Moreover it does not
 * require an explicit `WeightMap` parameter, because it is directly gathered
 * from the `PropertyGraph`.
 *
 * @see uninformed_bidirectional_pruner(const Graph &G, WeightMap const
 *                                      &weight_f, boost::reverse_graph<Graph>
 *                                      const &rev_G, RevWeightMap const
 *                                      &weight_b, Vertex s, Vertex t, double
 *                                      tau)
 *
 * @tparam PropertyGraph A Boost::PropertyGraph having at least one edge
 *         property with tag boost::edge_weight_t.
 *
 * @return A pruned copy of `G`.
 */
template <typename PropertyGraph, typename Vertex = vertex_of_t<PropertyGraph>>
PrunedGraph<PropertyGraph>
uninformed_bidirectional_pruner(const PropertyGraph &G, Vertex s, Vertex t,
                                double tau) {
  using namespace boost;
  using Edge = typename graph_traits<PropertyGraph>::edge_descriptor;

  BOOST_CONCEPT_ASSERT(
      (PropertyGraphConcept<PropertyGraph, Edge, edge_weight_t>));

  auto weight = get(edge_weight, G);
  auto rev = make_reverse_graph(G);
  auto weight_b = get(edge_weight, rev);
  return uninformed_bidirectional_pruner(G, weight, rev, weight_b, s, t, tau);
}
} // namespace arlib

#endif


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

#include "SiconosMatrixSetBlock.hpp"
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include "SiconosMatrix.hpp"
#include "SiconosException.hpp"

void setBlock(SPC::SiconosMatrix  input_matrix, SP::SiconosMatrix output_matrix, const Index& dim, const Index& start)
{
  // To copy a subBlock of input_matrix into a subBlock of output_matrix.
  // dim[0], dim[1]: number of rows and columns of the sub-block
  // start[0], start[1]: position (row, column) of the first element of the subBlock in input_matrix
  // start[2], start[3]: position (row, column) of the first element of the subBlock in output_matrix

  if(input_matrix == output_matrix)  // useless op => nothing to be done.
    return;

  Siconos::UBLAS_TYPE numIn = input_matrix->num();
  Siconos::UBLAS_TYPE numOut = output_matrix->num();

  if(numOut == Siconos::ZERO || numOut == Siconos::IDENTITY)  // if output_matrix = 0 or Identity => read-only
    THROW_EXCEPTION("output_matrix is read-only (zero or identity matrix?).");

  // Check dimension
  Index MDim(4); // dim. of matrices input_matrix and output_matrix.
  MDim[0] = input_matrix->size(0);
  MDim[1] = input_matrix->size(1);
  MDim[2] = output_matrix->size(0);
  MDim[3] = output_matrix->size(1);

  for(unsigned int i = 0; i < 4 ; ++i)
    if(start[i] >= MDim[i])
      THROW_EXCEPTION("matrices, setBlock(input_matrix, ...): sub-block indices are out of range.");

  // index position of the last element in subBlock ...
  Index end(4);
  end[0] = dim[0] + start[0];
  end[1] = dim[1] + start[1];
  end[2] = dim[0] + start[2];
  end[3] = dim[1] + start[3];

  for(unsigned int i = 0; i < 4 ; ++i)
    if(end[i] > MDim[i])
      THROW_EXCEPTION("sub-block last indices are out of range.");

  // Elements from row/col start[i] to row/col (end[i]-1) will be copied.

  // If both matrices input_matrix and output_matrix are block, exception.
  if(numIn == Siconos::BLOCK && numOut == Siconos::BLOCK)
    THROW_EXCEPTION("not yet implemented for input_matrix and output_matrix both BlockMatrix. Try to use setBlock on the sub-matrices?");

  if(numOut == Siconos::BLOCK)  // if output_matrix is a BlockMatrix.
  {

    // Steps:
    // A - Find the blocks of output_matrix that "own" indices start[2] and end[2] ie
    //     the first and last sub-block to be set in a block-column
    //         --> numbers blockStart0 and blockEnd0
    // B - Find the  Block of output_matrix that "owns" index start[3] and end[3] ie
    //     the first sub-block to be set in a block-row
    //         --> numbers blockStart1 and blockEnd1
    //
    //        => The blocks concerned in output_matrix, are those between (block) rows blockStart0 and blockEnd0
    //           and (block) columns blockStart1 and blockEnd1.
    //
    // C - Loop through the concerned blocks (name = currentBlock) of output_matrix and call setBlock(input_matrix, currentBlock, subSize, currentPos).
    //     subSize: dim. of the considered sub-block of currentBlock to be set
    //     currentPos: same as "start" vector but for currentBlock
    //

    // A - Block-Row position: we look for the block of output_matrix that include index start[2] and end[2].
    //
    unsigned int blockStart0 = 0;
    SPC::Index tab = output_matrix->tabRow();
    while(start[2] >= (*tab)[blockStart0] && blockStart0 < tab->size())
      blockStart0 ++;
    // Relative position in the block blockStart0 of the first element to be set.
    unsigned int posOut0 = start[2];
    if(blockStart0 != 0)
      posOut0 -= (*tab)[blockStart0 - 1];

    unsigned int blockEnd0 = blockStart0;
    while(end[2] > (*tab)[blockEnd0] && blockEnd0 < tab->size())
      blockEnd0 ++;

    // Size of the last sub-block in the column of block
    unsigned int lastBlockSize0 = end[2];
    if(blockEnd0 != 0)
      lastBlockSize0 -= (*tab)[blockEnd0 - 1];

    // B - Block-Col position: we look for the block of output_matrix that include index start[3] and end[3].
    unsigned int blockStart1 = 0;
    tab = output_matrix->tabCol();
    while(start[3] >= (*tab)[blockStart1] && blockStart1 < tab->size())
      blockStart1 ++;
    // Relative position in the block blockStart1 of the first element to be set.
    unsigned int posOut1 = start[3];
    if(blockStart1 != 0)
      posOut1 -= (*tab)[blockStart1 - 1];

    unsigned int blockEnd1 = blockStart1;
    while(end[3] > (*tab)[blockEnd1] && blockEnd1 < tab->size())
      blockEnd1 ++;

    // Size of the last sub-block in the row of block
    unsigned int lastBlockSize1 = end[3];
    if(blockEnd1 != 0)
      lastBlockSize1 -= (*tab)[blockEnd1 - 1];

    //C - Next, 3 steps for each row:
    // - set first sub-block in the row (number blockStart1)
    // - set all other blocks in the row except the last one
    // - set last block (number blockEnd1)
    // Same process for other rows ...

    // The current considered block
    SP::SiconosMatrix   currentBlock = output_matrix->block(blockStart0, blockStart1);

    // dim of the subBlock of currentBlock to be set.
    Index subSize(2);
    // indices of the first element of input_matrix (resp. currentBlock) to be read (resp. set)  (same as start for input_matrix and output_matrix).
    Index currentPos(4);

    // currentBlock position in output_matrix.
    unsigned int numRow = blockStart0;
    unsigned int numCol = blockStart1;

    // Init currentPos
    // row and col position for first element to be read in input_matrix,
    currentPos[0] = start[0];
    currentPos[1] = start[1];
    // row and col position for first element in sub-block of Mout (namely currentBlock).
    currentPos[2] = posOut0;
    currentPos[3] = posOut1;

    while(numRow != blockEnd0 + 1)
    {

      while(numCol != blockEnd1 + 1)
      {
        // Get the block of output_matrix from which a sub-block will be set ...
        currentBlock = output_matrix->block(numRow, numCol);

        // Set subSize[0], dim (rows) and subSize[1], dim (columns) of the sub-block.
        // subSize[0] is only required for the first block in the row, after it remains constant.
        subSize[1] = currentBlock->size(1);

        // Warning: test "a" must be done before test "b"
        if(numCol == blockEnd1)  // if last column of blocks -> test "a"
          subSize[1] = lastBlockSize1;

        if(numCol == blockStart1)  // -> test "b"
        {
          subSize[1] -= posOut1;
          subSize[0] = currentBlock->size(0);
          if(numRow == blockEnd0)  // if last row of blocks
            subSize[0] = lastBlockSize0;
          if(numRow == blockStart0)  // if first row of blocks
            subSize[0] -= posOut0;
        }

        // Set sub-block
        setBlock(input_matrix, currentBlock, subSize, currentPos);

        // Update currentPos:
        // col position for first element to be read in input_matrix,
        currentPos[1] += subSize[1] ;
        // col position for first element to be set in sub-block.
        currentPos[3] = 0;
        numCol++;
      }

      numCol = blockStart1;
      numRow++;

      // Update currentPos:
      // row position for first element to be read in input_matrix,
      currentPos[0] += subSize[0] ;
      // col position for first element to be read in input_matrix,
      currentPos[1] = start[1] ;
      // row position for first element to be set in sub-block.
      currentPos[2] = 0;
      // col position for first element to be set in sub-block.
      currentPos[3] = posOut1;

    }

  }
  else if(numIn == Siconos::BLOCK)  // If input_matrix is a BlockMatrix.
  {

    // Same process as for numOut == 0

    unsigned int blockStart0 = 0;
    SPC::Index tab = input_matrix->tabRow();
    while(start[0] >= (*tab)[blockStart0] && blockStart0 < tab->size())
      blockStart0 ++;
    // Relative position in the block blockStart0 of the first element to be set.
    unsigned int posOut0 = start[0];
    if(blockStart0 != 0)
      posOut0 -= (*tab)[blockStart0 - 1];

    unsigned int blockEnd0 = blockStart0;
    while(end[0] > (*tab)[blockEnd0] && blockEnd0 < tab->size())
      blockEnd0 ++;

    // Size of the last sub-block in the column of block
    unsigned int lastBlockSize0 = end[0];
    if(blockEnd0 != 0)
      lastBlockSize0 -= (*tab)[blockEnd0 - 1];

    // B - Block-Col position: we look for the block of output_matrix that include index start[3] and end[3].
    unsigned int blockStart1 = 0;
    tab = input_matrix->tabCol();
    while(start[1] >= (*tab)[blockStart1] && blockStart1 < tab->size())
      blockStart1 ++;
    // Relative position in the block blockStart1 of the first element to be set.
    unsigned int posOut1 = start[1];
    if(blockStart1 != 0)
      posOut1 -= (*tab)[blockStart1 - 1];

    unsigned int blockEnd1 = blockStart1;
    while(end[1] > (*tab)[blockEnd1] && blockEnd1 < tab->size())
      blockEnd1 ++;

    // Size of the last sub-block in the row of block
    unsigned int lastBlockSize1 = end[1];
    if(blockEnd1 != 0)
      lastBlockSize1 -= (*tab)[blockEnd1 - 1];

    //C - Next, 3 steps for each row:
    // - set first sub-block in the row (number blockStart1)
    // - set all other blocks in the row except the last one
    // - set last block (number blockEnd1)
    // Same process for other rows ...

    // The current considered block
    SPC::SiconosMatrix  currentBlock = input_matrix->block(blockStart0, blockStart1);

    // dim of the subBlock of currentBlock to be set.
    Index subSize(2);
    // indices of the first element of input_matrix (resp. currentBlock) to be read (resp. set)  (same as start for input_matrix and output_matrix).
    Index currentPos(4);

    // currentBlock position in output_matrix.
    unsigned int numRow = blockStart0;
    unsigned int numCol = blockStart1;

    // Init currentPos
    // row and col position for first element to be read in input_matrix,
    currentPos[0] = posOut0;
    currentPos[1] = posOut1;
    // row and col position for first element in sub-block of Mout (namely currentBlock).
    currentPos[2] = start[2];
    currentPos[3] = start[3];

    while(numRow != blockEnd0 + 1)
    {

      while(numCol != blockEnd1 + 1)
      {
        // Get the block of output_matrix from which a sub-block will be set ...
        currentBlock = input_matrix->block(numRow, numCol);

        // Set subSize[0], dim (rows) and subSize[1], dim (columns) of the sub-block.
        // subSize[0] is only required for the first block in the row, after it remains constant.
        subSize[1] = currentBlock->size(1);
        // Warning: test "a" must be done before test "b"
        if(numCol == blockEnd1)  // if last column of blocks -> test "a"
          subSize[1] = lastBlockSize1;

        if(numCol == blockStart1)  // -> test "b"
        {
          subSize[1] -= posOut1;
          subSize[0] = currentBlock->size(0);
          if(numRow == blockEnd0)  // if last row of blocks
            subSize[0] = lastBlockSize0;
          if(numRow == blockStart0)  // if first row of blocks
            subSize[0] -= posOut0;
        }

        // Set sub-block
        setBlock(currentBlock, output_matrix, subSize, currentPos);

        // Update currentPos:
        // col position for first element to be read in input_matrix,
        currentPos[1] = 0 ;
        // col position for first element to be set in sub-block.
        currentPos[3] += subSize[1];
        numCol++;
      }

      numCol = blockStart1;
      numRow++;

      // Update currentPos:
      // row position for first element to be read in input_matrix,
      currentPos[0] = 0;
      // col position for first element to be read in input_matrix,
      currentPos[1] = posOut1;
      // row position for first element to be set in sub-block.
      currentPos[2] += subSize[0] ;
      // col position for first element to be set in sub-block.
      currentPos[3] = start[3];

    }
    output_matrix->resetFactorizationFlags();

  }
  else // neither input_matrix nor output_matrix is a BlockMatrix.
  {
    if(numOut == Siconos::DENSE)
    {
      ublas::matrix_range<DenseMat> out_range(*output_matrix->dense(),
                                              ublas::range(start[2],end[2]),
                                              ublas::range(start[3], end[3]));
      if(numIn == Siconos::DENSE)
      {
        ublas::matrix_range<DenseMat> in_range(*input_matrix->dense(),
                                               ublas::range(start[0],end[0]),
                                               ublas::range(start[1], end[1]));
        noalias(out_range) = in_range;
      }
      else if(numIn == Siconos::SYMMETRIC)
      {
        ublas::matrix_range<SymMat> in_range(*input_matrix->sym(),
                                             ublas::range(start[0],end[0]),
                                             ublas::range(start[1], end[1]));
        noalias(out_range) = in_range;
      }
      else if(numIn == Siconos::SPARSE)
      {
        ublas::matrix_range<SparseMat> in_range(*input_matrix->sparse(),
                                                ublas::range(start[0],end[0]),
                                                ublas::range(start[1], end[1]));
        noalias(out_range) = in_range;
      }
      else if(numIn == Siconos::IDENTITY)
      {
        ublas::matrix_range<IdentityMat> in_range(*input_matrix->identity(),
                                                  ublas::range(start[0],end[0]),
                                                  ublas::range(start[1], end[1]));
        noalias(out_range) = in_range;
      }
      else if(numIn == Siconos::ZERO)
        out_range *= 0.;
      else
        THROW_EXCEPTION("unconsistent types between input_matrix and output_matrix.");
    }
    else if(numOut == Siconos::SPARSE)
    {
      ublas::matrix_range<SparseMat> out_range(*output_matrix->sparse(),
                                               ublas::range(start[2],end[2]),
                                               ublas::range(start[3], end[3]));
      if(numIn == Siconos::DENSE)
      {
        ublas::matrix_range<DenseMat> in_range(*input_matrix->dense(),
                                               ublas::range(start[0],end[0]),
                                               ublas::range(start[1], end[1]));
        noalias(out_range) = in_range;
      }
      else if(numIn == Siconos::SYMMETRIC)
      {
        ublas::matrix_range<SymMat> in_range(*input_matrix->sym(),
                                             ublas::range(start[0],end[0]),
                                             ublas::range(start[1], end[1]));
        noalias(out_range) = in_range;
      }
      else if(numIn == Siconos::SPARSE)
      {
        ublas::matrix_range<SparseMat> in_range(*input_matrix->sparse(),
                                                ublas::range(start[0],end[0]),
                                                ublas::range(start[1], end[1]));
        noalias(out_range) = in_range;
      }
      else if(numIn == Siconos::IDENTITY)
      {
        ublas::matrix_range<IdentityMat> in_range(*input_matrix->identity(),
                                                  ublas::range(start[0],end[0]),
                                                  ublas::range(start[1], end[1]));
        noalias(out_range) = in_range;
      }
      else if(numIn == Siconos::ZERO)
        out_range *= 0.;
      else
        THROW_EXCEPTION("unconsistent types between input_matrix and output_matrix.");
    }

    output_matrix->resetFactorizationFlags();
  }
}