#ifndef _pyp_hh
#define _pyp_hh

#include "slice-sampler.h"
#include <math.h>
#include <map>
#include <tr1/unordered_map>
//#include <google/sparse_hash_map>

#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/mersenne_twister.hpp>

#include "log_add.h"
#include "mt19937ar.h"

//
// Pitman-Yor process with customer and table tracking
//

template <typename Dish, typename Hash=std::tr1::hash<Dish> >
class PYP : protected std::tr1::unordered_map<Dish, int, Hash>
//class PYP : protected google::sparse_hash_map<Dish, int, Hash>
{
public:
  using std::tr1::unordered_map<Dish,int>::const_iterator;
  using std::tr1::unordered_map<Dish,int>::iterator;
  using std::tr1::unordered_map<Dish,int>::begin;
  using std::tr1::unordered_map<Dish,int>::end;
//  using google::sparse_hash_map<Dish,int>::const_iterator;
//  using google::sparse_hash_map<Dish,int>::iterator;
//  using google::sparse_hash_map<Dish,int>::begin;
//  using google::sparse_hash_map<Dish,int>::end;

  PYP(double a, double b, unsigned long seed = 0, Hash hash=Hash());

  virtual int increment(Dish d, double p0);
  virtual int decrement(Dish d);

  // lookup functions
  int count(Dish d) const;
  double prob(Dish dish, double p0) const;
  double prob(Dish dish, double dcd, double dca,
              double dtd, double dta, double p0) const;
  double unnormalised_prob(Dish dish, double p0) const;

  int num_customers() const { return _total_customers; }
  int num_types() const { return std::tr1::unordered_map<Dish,int>::size(); }
  //int num_types() const { return google::sparse_hash_map<Dish,int>::size(); }
  bool empty() const { return _total_customers == 0; }

  double log_prob(Dish dish, double log_p0) const;
  // nb. d* are NOT logs
  double log_prob(Dish dish, double dcd, double dca,
                       double dtd, double dta, double log_p0) const;

  int num_tables(Dish dish) const;
  int num_tables() const;

  double a() const { return _a; }
  void set_a(double a) { _a = a; }

  double b() const { return _b; }
  void set_b(double b) { _b = b; }

  virtual void clear();
  std::ostream& debug_info(std::ostream& os) const;

  double log_restaurant_prob() const;
  double log_prior() const;
  static double log_prior_a(double a, double beta_a, double beta_b);
  static double log_prior_b(double b, double gamma_c, double gamma_s);

  template <typename Uniform01>
    void resample_prior(Uniform01& rnd);
  template <typename Uniform01>
    void resample_prior_a(Uniform01& rnd);
  template <typename Uniform01>
    void resample_prior_b(Uniform01& rnd);

protected:
  double _a, _b; // parameters of the Pitman-Yor distribution
  double _a_beta_a, _a_beta_b; // parameters of Beta prior on a
  double _b_gamma_s, _b_gamma_c; // parameters of Gamma prior on b

  struct TableCounter {
    TableCounter() : tables(0) {};
    int tables;
    std::map<int, int> table_histogram; // num customers at table -> number tables
  };
  typedef std::tr1::unordered_map<Dish, TableCounter, Hash> DishTableType;
  //typedef google::sparse_hash_map<Dish, TableCounter, Hash> DishTableType;
  DishTableType _dish_tables;
  int _total_customers, _total_tables;

  typedef boost::mt19937 base_generator_type;
  typedef boost::uniform_real<> uni_dist_type;
  typedef boost::variate_generator<base_generator_type&, uni_dist_type> gen_type;

//  uni_dist_type uni_dist;
//  base_generator_type rng; //this gets the seed
//  gen_type rnd; //instantiate: rnd(rng, uni_dist)
                //call: rnd() generates uniform on [0,1)

  // Function objects for calculating the parts of the log_prob for
  // the parameters a and b
  struct resample_a_type {
    int n, m; double b, a_beta_a, a_beta_b;
    const DishTableType& dish_tables;
    resample_a_type(int n, int m, double b, double a_beta_a,
                    double a_beta_b, const DishTableType& dish_tables)
      : n(n), m(m), b(b), a_beta_a(a_beta_a), a_beta_b(a_beta_b), dish_tables(dish_tables) {}

    double operator() (double proposed_a) const {
      double log_prior = log_prior_a(proposed_a, a_beta_a, a_beta_b);
      double log_prob = 0.0;
      double lgamma1a = lgamma(1.0 - proposed_a);
      for (typename DishTableType::const_iterator dish_it=dish_tables.begin(); dish_it != dish_tables.end(); ++dish_it)
        for (std::map<int, int>::const_iterator table_it=dish_it->second.table_histogram.begin();
             table_it !=dish_it->second.table_histogram.end(); ++table_it)
          log_prob += (table_it->second * (lgamma(table_it->first - proposed_a) - lgamma1a));

      log_prob += (proposed_a == 0.0 ? (m-1.0)*log(b)
                   : ((m-1.0)*log(proposed_a) + lgamma((m-1.0) + b/proposed_a) - lgamma(b/proposed_a)));
      assert(std::isfinite(log_prob));
      return log_prob + log_prior;
    }
  };

  struct resample_b_type {
    int n, m; double a, b_gamma_c, b_gamma_s;
    resample_b_type(int n, int m, double a, double b_gamma_c, double b_gamma_s)
      : n(n), m(m), a(a), b_gamma_c(b_gamma_c), b_gamma_s(b_gamma_s) {}

    double operator() (double proposed_b) const {
      double log_prior = log_prior_b(proposed_b, b_gamma_c, b_gamma_s);
      double log_prob = 0.0;
      log_prob += (a == 0.0  ? (m-1.0)*log(proposed_b)
                  : ((m-1.0)*log(a) + lgamma((m-1.0) + proposed_b/a) - lgamma(proposed_b/a)));
      log_prob += (lgamma(1.0+proposed_b) - lgamma(n+proposed_b));
      return log_prob + log_prior;
    }
  };

  /* lbetadist() returns the log probability density of x under a Beta(alpha,beta)
   * distribution. - copied from Mark Johnson's gammadist.c
   */
  static long double lbetadist(long double x, long double alpha, long double beta);

  /* lgammadist() returns the log probability density of x under a Gamma(alpha,beta)
   * distribution - copied from Mark Johnson's gammadist.c
   */
  static long double lgammadist(long double x, long double alpha, long double beta);

};

template <typename Dish, typename Hash>
PYP<Dish,Hash>::PYP(double a, double b, unsigned long seed, Hash)
: std::tr1::unordered_map<Dish, int, Hash>(10), _a(a), _b(b),
//: google::sparse_hash_map<Dish, int, Hash>(10), _a(a), _b(b),
  _a_beta_a(1), _a_beta_b(1), _b_gamma_s(1), _b_gamma_c(1),
  //_a_beta_a(1), _a_beta_b(1), _b_gamma_s(10), _b_gamma_c(0.1),
  _total_customers(0), _total_tables(0)//,
  //uni_dist(0,1), rng(seed == 0 ? (unsigned long)this : seed), rnd(rng, uni_dist)
{
//  std::cerr << "\t##PYP<Dish,Hash>::PYP(a=" << _a << ",b=" << _b << ")" << std::endl;
  //set_deleted_key(-std::numeric_limits<Dish>::max());
}

template <typename Dish, typename Hash>
double
PYP<Dish,Hash>::prob(Dish dish, double p0) const
{
  int c = count(dish), t = num_tables(dish);
  double r = num_tables() * _a + _b;
  //std::cerr << "\t\t\t\tPYP<Dish,Hash>::prob(" << dish << "," << p0 << ") c=" << c << " r=" << r << std::endl;
  if (c > 0)
    return (c - _a * t + r * p0) / (num_customers() + _b);
  else
    return r * p0 / (num_customers() + _b);
}

template <typename Dish, typename Hash>
double
PYP<Dish,Hash>::unnormalised_prob(Dish dish, double p0) const
{
  int c = count(dish), t = num_tables(dish);
  double r = num_tables() * _a + _b;
  if (c > 0) return (c - _a * t + r * p0);
  else       return r * p0;
}

template <typename Dish, typename Hash>
double
PYP<Dish,Hash>::prob(Dish dish, double dcd, double dca,
                     double dtd, double dta, double p0)
const
{
  int c = count(dish) + dcd, t = num_tables(dish) + dtd;
  double r = (num_tables() + dta) * _a + _b;
  if (c > 0)
    return (c - _a * t + r * p0) / (num_customers() + dca + _b);
  else
    return r * p0 / (num_customers() + dca + _b);
}

template <typename Dish, typename Hash>
double
PYP<Dish,Hash>::log_prob(Dish dish, double log_p0) const
{
  using std::log;
  int c = count(dish), t = num_tables(dish);
  double r = log(num_tables() * _a + b);
  if (c > 0)
    return Log<double>::add(log(c - _a * t), r + log_p0)
      - log(num_customers() + _b);
  else
    return r + log_p0 - log(num_customers() + b);
}

template <typename Dish, typename Hash>
double
PYP<Dish,Hash>::log_prob(Dish dish, double dcd, double dca,
                         double dtd, double dta, double log_p0)
const
{
  using std::log;
  int c = count(dish) + dcd, t = num_tables(dish) + dtd;
  double r = log((num_tables() + dta) * _a + b);
  if (c > 0)
    return Log<double>::add(log(c - _a * t), r + log_p0)
      - log(num_customers() + dca + _b);
  else
    return r + log_p0 - log(num_customers() + dca + b);
}

template <typename Dish, typename Hash>
int
PYP<Dish,Hash>::increment(Dish dish, double p0) {
  int delta = 0;
  TableCounter &tc = _dish_tables[dish];

  // seated on a new or existing table?
  int c = count(dish), t = num_tables(dish), T = num_tables();
  double pshare = (c > 0) ? (c - _a*t) : 0.0;
  double pnew = (_b + _a*T) * p0;
  assert (pshare >= 0.0);
  //assert (pnew > 0.0);

  //if (rnd() < pnew / (pshare + pnew)) {
  if (mt_genrand_res53() < pnew / (pshare + pnew)) {
    // assign to a new table
    tc.tables += 1;
    tc.table_histogram[1] += 1;
    _total_tables += 1;
    delta = 1;
  }
  else {
    // randomly assign to an existing table
    // remove constant denominator from inner loop
    //double r = rnd() * (c - _a*t);
    double r = mt_genrand_res53() * (c - _a*t);
    for (std::map<int,int>::iterator
         hit = tc.table_histogram.begin();
         hit != tc.table_histogram.end(); ++hit) {
      r -= ((hit->first - _a) * hit->second);
      if (r <= 0) {
        tc.table_histogram[hit->first+1] += 1;
        hit->second -= 1;
        if (hit->second == 0)
          tc.table_histogram.erase(hit);
        break;
      }
    }
    if (r > 0) {
      std::cerr << r << " " << c << " " << _a << " " << t << std::endl;
      assert(false);
    }
    delta = 0;
  }

  std::tr1::unordered_map<Dish,int,Hash>::operator[](dish) += 1;
  //google::sparse_hash_map<Dish,int,Hash>::operator[](dish) += 1;
  _total_customers += 1;

  return delta;
}

template <typename Dish, typename Hash>
int
PYP<Dish,Hash>::count(Dish dish) const
{
  typename std::tr1::unordered_map<Dish, int>::const_iterator
  //typename google::sparse_hash_map<Dish, int>::const_iterator
    dcit = find(dish);
  if (dcit != end())
    return dcit->second;
  else
    return 0;
}

template <typename Dish, typename Hash>
int
PYP<Dish,Hash>::decrement(Dish dish)
{
  typename std::tr1::unordered_map<Dish, int>::iterator dcit = find(dish);
  //typename google::sparse_hash_map<Dish, int>::iterator dcit = find(dish);
  if (dcit == end()) {
    std::cerr << dish << std::endl;
    assert(false);
  }

  int delta = 0;

  typename std::tr1::unordered_map<Dish, TableCounter>::iterator dtit = _dish_tables.find(dish);
  //typename google::sparse_hash_map<Dish, TableCounter>::iterator dtit = _dish_tables.find(dish);
  if (dtit == _dish_tables.end()) {
    std::cerr << dish << std::endl;
    assert(false);
  }
  TableCounter &tc = dtit->second;

  //std::cerr << "\tdecrement for " << dish << "\n";
  //std::cerr << "\tBEFORE histogram: " << tc.table_histogram << " ";
  //std::cerr << "count: " << count(dish) << " ";
  //std::cerr << "tables: " << tc.tables << "\n";

  //double r = rnd() * count(dish);
  double r = mt_genrand_res53() * count(dish);
  for (std::map<int,int>::iterator hit = tc.table_histogram.begin();
       hit != tc.table_histogram.end(); ++hit)
  {
    //r -= (hit->first - _a) * hit->second;
    r -= (hit->first) * hit->second;
    if (r <= 0)
    {
      if (hit->first > 1)
        tc.table_histogram[hit->first-1] += 1;
      else
      {
        delta = -1;
        tc.tables -= 1;
        _total_tables -= 1;
      }

      hit->second -= 1;
      if (hit->second == 0) tc.table_histogram.erase(hit);
      break;
    }
  }
  if (r > 0) {
    std::cerr << r << " " << count(dish) << " " << _a << " " << num_tables(dish) << std::endl;
    assert(false);
  }

  // remove the customer
  dcit->second -= 1;
  _total_customers -= 1;
  assert(dcit->second >= 0);
  if (dcit->second == 0) {
    erase(dcit);
    _dish_tables.erase(dtit);
    //std::cerr << "\tAFTER histogram: Empty\n";
  }
  else {
    //std::cerr << "\tAFTER histogram: " << _dish_tables[dish].table_histogram << " ";
    //std::cerr << "count: " << count(dish) << " ";
    //std::cerr << "tables: " << _dish_tables[dish].tables << "\n";
  }

  return delta;
}

template <typename Dish, typename Hash>
int
PYP<Dish,Hash>::num_tables(Dish dish) const
{
  typename std::tr1::unordered_map<Dish, TableCounter, Hash>::const_iterator
  //typename google::sparse_hash_map<Dish, TableCounter, Hash>::const_iterator
    dtit = _dish_tables.find(dish);

  //assert(dtit != _dish_tables.end());
  if (dtit == _dish_tables.end())
    return 0;

  return dtit->second.tables;
}

template <typename Dish, typename Hash>
int
PYP<Dish,Hash>::num_tables() const
{
  return _total_tables;
}

template <typename Dish, typename Hash>
std::ostream&
PYP<Dish,Hash>::debug_info(std::ostream& os) const
{
  int hists = 0, tables = 0;
  for (typename std::tr1::unordered_map<Dish, TableCounter, Hash>::const_iterator
  //for (typename google::sparse_hash_map<Dish, TableCounter, Hash>::const_iterator
       dtit = _dish_tables.begin(); dtit != _dish_tables.end(); ++dtit)
  {
    hists += dtit->second.table_histogram.size();
    tables += dtit->second.tables;

//    if (dtit->second.tables <= 0)
//      std::cerr << dtit->first << " " << count(dtit->first) << std::endl;
    assert(dtit->second.tables > 0);
    assert(!dtit->second.table_histogram.empty());

//    os << "Dish " << dtit->first << " has " << count(dtit->first) << " customers, and is sitting at " << dtit->second.tables << " tables.\n";
    for (std::map<int,int>::const_iterator
         hit = dtit->second.table_histogram.begin();
         hit != dtit->second.table_histogram.end(); ++hit) {
//      os << "    " << hit->second << " tables with " << hit->first << " customers." << std::endl;
      assert(hit->second > 0);
    }
  }

  os << "restaurant has "
    << _total_customers << " customers; "
    << _total_tables << " tables; "
    << tables << " tables'; "
    << num_types() << " dishes; "
    << _dish_tables.size() << " dishes'; and "
    << hists << " histogram entries\n";

  return os;
}

template <typename Dish, typename Hash>
void
PYP<Dish,Hash>::clear()
{
  this->std::tr1::unordered_map<Dish,int,Hash>::clear();
  //this->google::sparse_hash_map<Dish,int,Hash>::clear();
  _dish_tables.clear();
  _total_tables = _total_customers = 0;
}

// log_restaurant_prob returns the log probability of the PYP table configuration.
// Excludes Hierarchical P0 term which must be calculated separately.
template <typename Dish, typename Hash>
double
PYP<Dish,Hash>::log_restaurant_prob() const {
  if (_total_customers < 1)
    return (double)0.0;

  double log_prob = 0.0;
  double lgamma1a = lgamma(1.0-_a);

  //std::cerr << "-------------------\n" << std::endl;
  for (typename DishTableType::const_iterator dish_it=_dish_tables.begin();
       dish_it != _dish_tables.end(); ++dish_it) {
    for (std::map<int, int>::const_iterator table_it=dish_it->second.table_histogram.begin();
         table_it !=dish_it->second.table_histogram.end(); ++table_it) {
      log_prob += (table_it->second * (lgamma(table_it->first - _a) - lgamma1a));
      //std::cerr << "|" << dish_it->first->parent << " --> " << dish_it->first->rhs << " " << table_it->first << " " << table_it->second << " " << log_prob;
    }
  }
  //std::cerr << std::endl;

  log_prob += (_a == (double)0.0 ? (_total_tables-1.0)*log(_b) : (_total_tables-1.0)*log(_a) + lgamma((_total_tables-1.0) + _b/_a) - lgamma(_b/_a));
  //std::cerr << "\t\t" << log_prob << std::endl;
  log_prob += (lgamma(1.0 + _b) - lgamma(_total_customers + _b));

  //std::cerr << _total_customers << " " << _total_tables << " " << log_prob << " " << log_prior() << std::endl;
  //std::cerr << _a << " " << _b << std::endl;
  if (!std::isfinite(log_prob)) {
    assert(false);
  }
  //return log_prob;
  if (log_prob > 0.0)
    std::cerr << log_prob << std::endl;
  return log_prob;// + log_prior();
}

template <typename Dish, typename Hash>
double
PYP<Dish,Hash>::log_prior() const {
  double prior = 0.0;
  if (_a_beta_a > 0.0 && _a_beta_b > 0.0 && _a > 0.0)
    prior += log_prior_a(_a, _a_beta_a, _a_beta_b);
  if (_b_gamma_s > 0.0 && _b_gamma_c > 0.0)
    prior += log_prior_b(_b, _b_gamma_c, _b_gamma_s);

  return prior;
}

template <typename Dish, typename Hash>
double
PYP<Dish,Hash>::log_prior_a(double a, double beta_a, double beta_b) {
  return lbetadist(a, beta_a, beta_b);
}

template <typename Dish, typename Hash>
double
PYP<Dish,Hash>::log_prior_b(double b, double gamma_c, double gamma_s) {
  return lgammadist(b, gamma_c, gamma_s);
}

template <typename Dish, typename Hash>
long double PYP<Dish,Hash>::lbetadist(long double x, long double alpha, long double beta) {
  assert(x > 0);
  assert(x < 1);
  assert(alpha > 0);
  assert(beta > 0);
  return (alpha-1)*log(x)+(beta-1)*log(1-x)+lgamma(alpha+beta)-lgamma(alpha)-lgamma(beta);
//boost::math::lgamma
}

template <typename Dish, typename Hash>
long double PYP<Dish,Hash>::lgammadist(long double x, long double alpha, long double beta) {
  assert(alpha > 0);
  assert(beta > 0);
  return (alpha-1)*log(x) - alpha*log(beta) - x/beta - lgamma(alpha);
}

template <typename Dish, typename Hash>
  template <typename Uniform01>
void
PYP<Dish,Hash>::resample_prior(Uniform01& rnd) {
  for (int num_its=5; num_its >= 0; --num_its) {
    resample_prior_b(rnd);
    resample_prior_a(rnd);
  }
  resample_prior_b(rnd);
}

template <typename Dish, typename Hash>
  template <typename Uniform01>
void
PYP<Dish,Hash>::resample_prior_b(Uniform01& rnd) {
  if (_total_tables == 0)
    return;

  //int niterations = 10;   // number of resampling iterations
  int niterations = 5;   // number of resampling iterations
  //std::cerr << "\n## resample_prior_b(), initial a = " << _a << ", b = " << _b << std::endl;
  resample_b_type b_log_prob(_total_customers, _total_tables, _a, _b_gamma_c, _b_gamma_s);
  _b = slice_sampler1d(b_log_prob, _b, rnd, (double) 0.0, std::numeric_limits<double>::infinity(),
  //_b = slice_sampler1d(b_log_prob, _b, mt_genrand_res53, (double) 0.0, std::numeric_limits<double>::infinity(),
                       (double) 0.0, niterations, 100*niterations);
  //std::cerr << "\n## resample_prior_b(), final a = " << _a << ", b = " << _b << std::endl;
}

template <typename Dish, typename Hash>
  template <typename Uniform01>
void
PYP<Dish,Hash>::resample_prior_a(Uniform01& rnd) {
  if (_total_tables == 0)
    return;

  //int niterations = 10;
  int niterations = 5;
  //std::cerr << "\n## Initial a = " << _a << ", b = " << _b << std::endl;
  resample_a_type a_log_prob(_total_customers, _total_tables, _b, _a_beta_a, _a_beta_b, _dish_tables);
  _a = slice_sampler1d(a_log_prob, _a, rnd, std::numeric_limits<double>::min(),
  //_a = slice_sampler1d(a_log_prob, _a, mt_genrand_res53, std::numeric_limits<double>::min(),
                       (double) 1.0, (double) 0.0, niterations, 100*niterations);
}

#endif


#include <Engine/MeshEdit/MinSurf.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

MinSurf::MinSurf(Ptr<TriMesh> triMesh)
	: heMesh(make_shared<HEMesh<V>>())
{
	Init(triMesh);
}

void MinSurf::Clear() {
	heMesh->Clear();
	triMesh = nullptr;
}

bool MinSurf::Init(Ptr<TriMesh> triMesh) {
	Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		return false;
	}

	// triangle mesh's positions ->  half-edge structure's positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	this->triMesh = triMesh;
	return true;
}

bool MinSurf::Run() {
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::MinSurf::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	Minimize();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);
	for (auto v : heMesh->Vertices())
		positions.push_back(v->pos.cast_to<pointf3>());
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	triMesh->Init(indice, positions);

	return true;
}

void MinSurf::Minimize() {

	Laplace_init();

	Predecomposition();

	Solve();

	cout << "INFO::MinSurf::Minimize:" << endl
		<< "\t" << "Success" << endl;
}

void MinSurf::Laplace_init()
{
	size_t nV = heMesh->NumVertices();

	//	generate Laplace Matrix
	vector<Eigen::Triplet<double>> Lij;

	for (size_t i = 0; i < nV; i++)
	{
		V* v1 = heMesh->Vertices()[i];
		Lij.push_back(Eigen::Triplet<double>(i, i, 1));
		if(!v1->IsBoundary())
		{
			double connect_num = v1->AdjVertices().size();
			for (size_t j = 0; j < connect_num; j++)
			{
				Lij.push_back(Eigen::Triplet<double>(i, heMesh->Index(v1->AdjVertices()[j]), -1 / connect_num));
			}
		}
	}

	Laplace_matrix.resize(nV, nV);
	Laplace_matrix.setZero();

	Laplace_matrix.setFromTriplets(Lij.begin(), Lij.end());
	//Laplace_matrix.makeCompressed();
}

void MinSurf::Predecomposition()
{
	solver.compute(Laplace_matrix);
	if (solver.info() != Eigen::Success)
	{
		throw std::exception("Compute Matrix Is Error!");
		return;
	}
}

void MinSurf::Solve()
{
	size_t nV = heMesh->NumVertices();
	Eigen::VectorXd x(nV), y(nV), z(nV);
	Eigen::VectorXd bx(nV), by(nV), bz(nV);
	bx.setZero(); by.setZero(); bz.setZero();

	for (size_t i = 0; i < nV; i++)
	{
		V* v = heMesh->Vertices()[i];
		if (v->IsBoundary())
		{
			bx(i) = v->pos.at(0);
			by(i) = v->pos.at(1);
			bz(i) = v->pos.at(2);
		}
	}

	x = solver.solve(bx);
	y = solver.solve(by);
	z = solver.solve(bz);

	for (size_t i = 0; i < nV; i++)
	{
		heMesh->Vertices()[i]->pos.at(0) = x(i);
		heMesh->Vertices()[i]->pos.at(1) = y(i);
		heMesh->Vertices()[i]->pos.at(2) = z(i);
	}
}


#include <unistd.h> // getopt
#include <cstring> // memcpy
#include <iostream> // cout
#include <sys/stat.h> // stat
#include <chrono> // high_resolution_clock
#include <cstdio> // printf
#include <fstream> // ifstream, ofstream
#include <sstream> // stringstream
#include <ratio>  // milli

#ifdef _OPENMP
#include "omp.h" // omp_set_num_threads
#endif

#include <boost/graph/johnson_all_pairs_shortest.hpp>

#include "inf.hpp"
//#include "getInf.hpp"
#include "util.hpp"
#include "floyd_warshall.hpp"
#include "johnson.hpp"

struct bench_result{
  bool correct;
  double seq_total_time;
  double total_time;
};

template<typename Number>
void print_matrix(int n, int n_blocked, Number *adjacencyMatrix, Number *distanceMatrix, Number *solution, int *successorMatrix){
  if(adjacencyMatrix != nullptr){
    std::cout << "[adjacencyMatrix]\n";
    print_matrix<Number>(adjacencyMatrix, n, n_blocked);
  }
  if(distanceMatrix != nullptr){
    std::cout << "[distanceMatrix]\n";
    print_matrix<Number>(distanceMatrix, n, n_blocked);
  }
  if (solution != nullptr) {
    std::cout << "[solution]\n";
    print_matrix<Number>(solution, n, n);
  }
  if (successorMatrix != nullptr) {
    std::cout << "[successorMatrix]\n";
    print_matrix<int>(successorMatrix, n, n_blocked);
  }
}

template<typename Number> inline char getTypeChar();
template<> inline char getTypeChar<double>(){
  return 'd';
}
template<> inline char getTypeChar<float>(){
  return 'f';
}
template<> inline char getTypeChar<int>(){
  return 'i';
}

template<typename Number>
Number* get_solution(
  const int n,
  double p,
  unsigned long seed){
  size_t size = n * n * sizeof(Number);
  Number *solution = (Number *) malloc(size);
  bool write_solution_to_file = true;
  char chr = getTypeChar<Number>();

  // have we cached the solution before?
  std::string solution_filename = get_solution_filename("apsp", n, p, seed, chr);
  struct stat file_stat;
  bool solution_available = stat(solution_filename.c_str(), &file_stat) != -1 || errno != ENOENT;

  if (solution_available) {
    // std::cout << "Reading reference solution from file: " << solution_filename << "\n";
    std::ifstream in(solution_filename, std::ios::in | std::ios::binary);
    in.read(reinterpret_cast<char *>(solution), size);
    in.close();
    // std::cout << "filename: " << solution_filename << " size: " << size << " bytes read.\n";
  } else {
    const Number *adjacencyMatrix = create_random_adjacencyMatrix<Number>(n, p, seed);
    auto start = std::chrono::high_resolution_clock::now();
    if(p > 0.1){
      floyd_warshall_blocked<Number>(adjacencyMatrix, &solution, n, 32);
    }else{
     johnson_parallel_matrix<Number>(adjacencyMatrix, &solution, n);
    }

    //floyd_warshall<Number>(adjacencyMatrix, &solution, n);
    auto end = std::chrono::high_resolution_clock::now();

    std::chrono::duration<double, std::milli> start_to_end = end - start;
    std::cout << "Algorithm runtime: " << start_to_end.count() << "ms\n";

    delete[] adjacencyMatrix;

    if (write_solution_to_file) {
      std::cout << "Writing solution to file: " << solution_filename << "\n";

      if (system("mkdir -p solution_cache") == -1) {
        std::cerr << "mkdir failed!";
        return nullptr;
      }

      std::ofstream out(solution_filename, std::ios::out | std::ios::binary);
      out.write(reinterpret_cast<const char *>(solution), size);
      out.close();
    }
  }
  return solution;
}

template<typename Number>
double do_floyd_warshall(int n, int block_size, double p, unsigned long seed, bool with_successor, bool check_correctness){

  Number *solution = check_correctness ? get_solution<Number>(n, p, seed) : nullptr;

  Number *adjacencyMatrix = create_random_adjacencyMatrix<Number>(n, p, seed);
  Number *distanceMatrix = nullptr;
  int *successorMatrix = nullptr;

  auto start = std::chrono::high_resolution_clock::now();
  if (with_successor) {
    floyd_warshall_blocked<Number>(adjacencyMatrix, &distanceMatrix, &successorMatrix, n, block_size);
  } else {
    floyd_warshall_blocked<Number>(adjacencyMatrix, &distanceMatrix, n, block_size);floyd_warshall_blocked<Number>(adjacencyMatrix, &distanceMatrix, n, block_size);
  }
  auto end = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double, std::milli> start_to_end = end - start;

  if (check_correctness) {
    if(n <= 256) print_matrix(n, n, adjacencyMatrix, distanceMatrix, solution, successorMatrix);
    correctness_check<Number>(distanceMatrix, n, solution, n);
  }
  if (check_correctness) {
    delete[] solution;
  }
  if (with_successor) {
    delete[] successorMatrix;
  }
  delete[] distanceMatrix;
  delete[] adjacencyMatrix;

  return start_to_end.count();
}

template<typename Number>
bench_result * bench_floyd_warshall(int iterations, int v, int block_size, double p, unsigned long seed, bool with_successor, bool check_correctness) {

  Number *solution = check_correctness ? get_solution<Number>(v, p, seed) : nullptr;
  const Number *adjacencyMatrix = create_random_adjacencyMatrix<Number>(v, p, seed);
  static const Number inf = getInf<Number>();

  bench_result *result = new bench_result;
  result->correct = true;
  result->seq_total_time = 0.0;
  result->total_time = 0.0;

  for (int b = 0; b < iterations; b++) {

    Number *distanceMatrix = nullptr;
    int *successorMatrix = nullptr;

    auto seq_start = std::chrono::high_resolution_clock::now();
    if (with_successor) {
      floyd_warshall<Number>(adjacencyMatrix, &distanceMatrix, &successorMatrix, v);
    } else {
      floyd_warshall<Number>(adjacencyMatrix, &distanceMatrix, v);
    }
    auto seq_end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double, std::milli> seq_start_to_end = seq_end - seq_start;
    result->seq_total_time += seq_start_to_end.count() / iterations;;

    delete[] distanceMatrix;
    if(with_successor){
      delete[] successorMatrix;
    }

    distanceMatrix = nullptr;
    successorMatrix = nullptr;

    auto start = std::chrono::high_resolution_clock::now();
    if (with_successor) {
      floyd_warshall_blocked<Number>(adjacencyMatrix, &distanceMatrix, &successorMatrix, v, block_size);
    } else {
      floyd_warshall_blocked<Number>(adjacencyMatrix, &distanceMatrix, v, block_size);
    }
    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double, std::milli> start_to_end = end - start;
    result->total_time += start_to_end.count() / iterations;;

    if (check_correctness) {
      result->correct = result->correct && correctness_check<Number>(distanceMatrix, v, solution, v);
    }

    delete[] distanceMatrix;
    if(with_successor){
      delete[] successorMatrix;
    }
  }

  if (check_correctness) {
    delete[] solution;
  }
  delete[] adjacencyMatrix;

  return result;
}

template<typename Number>
double do_johnson(const int n, const double p, const unsigned long seed, const bool with_successor, const bool check_correctness){

  Number *solution = check_correctness ? get_solution<Number>(n, p, seed) : nullptr;
  Number *adjacencyMatrix = create_random_adjacencyMatrix<Number>(n, p, seed);
  Number *distanceMatrix = nullptr;
  int *successorMatrix = nullptr;

  auto start = std::chrono::high_resolution_clock::now();
  if (with_successor) {
    johnson_parallel_matrix<Number>(adjacencyMatrix, &distanceMatrix, &successorMatrix, n);
  } else {
    johnson_parallel_matrix<Number>(adjacencyMatrix, &distanceMatrix, n);
  }
  auto end = std::chrono::high_resolution_clock::now();

  if (check_correctness) {
    if(n <= 256) print_matrix(n, n, adjacencyMatrix, distanceMatrix, solution, successorMatrix);
    correctness_check<Number>(distanceMatrix, n, solution, n);
  }

  delete[] distanceMatrix;
  if (with_successor) {
    delete[] successorMatrix;
  }
  delete[] adjacencyMatrix;
  if (check_correctness){
    delete[] solution;
  }

  std::chrono::duration<double, std::milli> start_to_end = end - start;
  return start_to_end.count();
}

template<typename Number>
bench_result * bench_johnson(int iterations, int n, double p, unsigned long seed, bool with_successor, bool check_correctness) {

  Number *solution = check_correctness ? get_solution<Number>(n, p, seed) : nullptr;
  Number *adjacencyMatrix = create_random_adjacencyMatrix<Number>(n, p, seed);

  Number inf = getInf<Number>();

  bench_result *result = new bench_result;
  result->correct = true;
  result->seq_total_time = 0.0;
  result->total_time = 0.0;

  for (int b = 0; b < iterations; b++) {

    Number *distanceMatrix = nullptr;
    int *successorMatrix = nullptr;

    auto start = std::chrono::high_resolution_clock::now();
    graph_t<Number> *gr = init_graph<Number>(adjacencyMatrix, n);
    if (with_successor) {
      johnson_parallel_matrix<Number>(adjacencyMatrix, &distanceMatrix, &successorMatrix, (const int)n);
    } else {
      johnson_parallel_matrix<Number>(adjacencyMatrix, &distanceMatrix, (const int)n);
    }
    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double, std::milli> start_to_end = end - start;
    result->total_time += start_to_end.count() / iterations;

    delete[] distanceMatrix;
    if(with_successor){
      delete[] successorMatrix;
    }

    auto seq_start = std::chrono::high_resolution_clock::now();
    Graph<Number> G(gr->edge_array, gr->edge_array + gr->E, gr->weights, gr->V);
    std::vector<Number> d(num_vertices(G));
    std::vector<int> predecessor(num_vertices(G));
    Number **distanceArray = new Number *[n];
    for (int i = 0; i < n; i++) distanceArray[i] = &adjacencyMatrix[i * n];
    if (with_successor) {
      johnson_all_pairs_shortest_paths(G, distanceArray, distance_map(&d[0]).predecessor_map(&predecessor[0]).distance_inf(inf));
    }else{
      johnson_all_pairs_shortest_paths(G, distanceArray, distance_map(&d[0]).distance_inf(inf));
    }
    auto seq_end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double, std::milli> seq_start_to_end = seq_end - seq_start;
    result->seq_total_time += seq_start_to_end.count() / iterations;

    free_graph<Number>(gr);
    delete[] distanceArray;

    if (solution != nullptr) {
      result->correct = result->correct && correctness_check<Number>(distanceMatrix, n, solution, n);
    }
  }

  if (check_correctness) {
    delete[] solution;
  }
  delete[] adjacencyMatrix;

  return result;
}

template<typename Number>
int do_benchmark(
  int iterations,
  unsigned long seed,
  bool use_floyd_warshall,
  int block_size,
  bool with_successor,
  bool check_correctness
) {

  if (use_floyd_warshall) {
      std::cout << "\n\nFloyd-Warshall's Algorithm benchmarking results for seed=" << seed << " and block size="
                << block_size << "\n";
      print_table_header(check_correctness);

      for (double p = 0.25; p < 1.0; p += 0.25) {
        for (int v = 64; v <= 4096; v *= 2) {
          bench_result *result = bench_floyd_warshall<Number>(iterations, v, block_size, p, seed, with_successor, check_correctness);
          print_table_row(p, v, result->seq_total_time, result->total_time, result->correct, check_correctness);
          delete result;
        }
        print_table_break(check_correctness);
      }
      std::cout << "\n\n";
  } else {  // Using Johnson's Algorithm
      std::cout << "\n\nJohnson's Algorithm benchmarking results for seed=" << seed << "\n";
      print_table_header(check_correctness);
      for (double p = 0.025; p < 0.1; p += 0.025) {
        for (int v = 64; v <= 4096; v *= 2) {
          bench_result *result = bench_johnson<Number>(iterations, v, p, seed, with_successor, check_correctness);
          print_table_row(p, v, result->seq_total_time, result->total_time, result->correct, check_correctness);
          delete result;
        }
        print_table_break(check_correctness);
      }
      std::cout << "\n\n";
  }

  return 0;
}

template<typename Number>
int do_main(
  int n,
  double p,
  unsigned long seed,
  bool with_successor,
  bool use_floyd_warshall,
  int block_size,
  bool check_correctness
) {

  if (use_floyd_warshall) {
      std::cout << "Using Floyd-Warshall's on " << n << "x" << n
                << " with p=" << p << " and seed=" << seed << "\n";
      double start_to_end_count = do_floyd_warshall<Number>(n, block_size, p, seed, with_successor, check_correctness);
      std::cout << "Algorithm runtime: " << start_to_end_count << "ms\n\n";
  } else {  // Using Johnson's Algorithm
      std::cout << "Using Johnson's on " << n << "x" << n
                << " with p=" << p << " and seed=" << seed << "\n";
      double start_to_end_count = do_johnson<Number>(n, p, seed, with_successor, check_correctness);
      std::cout << "Algorithm runtime: " << start_to_end_count << "ms\n\n";
  }

  return 0;
}

int main(int argc, char *argv[]) {
  // parameter defaults
  unsigned long seed = 0;
  int n = 1024;
  double p = 0.01;
  bool use_floyd_warshall = true;
  bool benchmark = false;
  bool check_correctness = false;
  bool with_successor = false;
  int block_size = 16;
  int thread_count = 1;
  char type = 'i';
  int iterations = 5;

  extern char *optarg;
  int opt;
  while ((opt = getopt(argc, argv, "ha:n:p:s:bd:ct:T:Si:")) != -1) {
    switch (opt) {
      case 'h':
      case '?': // illegal command
      case ':': // forgot command's argument
        print_usage();
        return 0;

      case 'a':
        if (optarg[0] == 'j') {
          use_floyd_warshall = false;
        } else if (optarg[0] != 'f') {
          std::cerr << "Illegal algorithm argument, must be f or j\n";
          return -1;
        }
        break;

      case 'p':
        p = std::stod(optarg);
        break;

      case 's':
        seed = std::stoul(optarg);
        break;

      case 'b':
        benchmark = true;
        break;

      case 'n':
        n = std::stoi(optarg);
        break;

      case 'i':
        iterations = std::stoi(optarg);
        break;

      case 'd':
        block_size = std::stoi(optarg);
        break;

      case 'c':
        check_correctness = true;
        break;

      case 't':
        thread_count = std::stoi(optarg);
        break;

      case 'S':
        with_successor = true;
        break;

      case 'T':
        if (optarg[0] == 'i') {
          type = 'i'; // int
        } else if (optarg[0] == 'f') {
          type = 'f'; // float
        } else if (optarg[0] == 'd') {
          type = 'd'; // double
        } else {
          std::cerr << "Illegal type argument (neigher i, f nor d)\n";
          return -1;
        }
        break;
    }
  }

#ifdef _OPENMP
  if(thread_count > 1){
    omp_set_num_threads(thread_count);
  }
#else
  (void) thread_count; // suppress unused warning
#endif

  if(benchmark){
    switch(type){
      case 'i':
        return do_benchmark<int>(iterations, seed, use_floyd_warshall, block_size, with_successor, check_correctness);
      case 'f':
        return do_benchmark<float>(iterations, seed, use_floyd_warshall, block_size, with_successor, check_correctness);
      case 'd':
        return do_benchmark<double>(iterations, seed, use_floyd_warshall, block_size, with_successor, check_correctness);
    }
  }else{
    switch(type){
      case 'i':
        return do_main<int>(n, p, seed, with_successor, use_floyd_warshall, block_size, check_correctness);
      case 'f':
        return do_main<float>(n, p, seed, with_successor, use_floyd_warshall, block_size, check_correctness);
      case 'd':
        return do_main<double>(n, p, seed, with_successor, use_floyd_warshall, block_size, check_correctness);
    }
  }
  return -1;
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/runge_kutta_fehlberg87.hpp

 [begin_description]
 Implementation of the Runge-Kutta-Fehlberg stepper with the generic stepper.
 [end_description]

 Copyright 2011-2013 Mario Mulansky
 Copyright 2012-2013 Karsten Ahnert

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

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_FEHLBERG87_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_FEHLBERG87_HPP_INCLUDED

#include <boost/fusion/container/vector.hpp>
#include <boost/fusion/container/generation/make_vector.hpp>

#include <boost/numeric/odeint/stepper/explicit_error_generic_rk.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/array.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

namespace boost {
namespace numeric {
namespace odeint {

#ifndef DOXYGEN_SKIP
template< class Value = double >
struct rk78_coefficients_a1 : boost::array< Value , 1 >
{
    rk78_coefficients_a1( void )
            {
        (*this)[0] = static_cast< Value >( 2 )/static_cast< Value >( 27 );
            }
};

template< class Value = double >
struct rk78_coefficients_a2 : boost::array< Value , 2 >
{
    rk78_coefficients_a2( void )
            {
        (*this)[0] = static_cast< Value >( 1 )/static_cast< Value >( 36 );
        (*this)[1] = static_cast< Value >( 1 )/static_cast< Value >( 12 );
            }
};

template< class Value = double >
struct rk78_coefficients_a3 : boost::array< Value , 3 >
{
    rk78_coefficients_a3( void )
            {
        (*this)[0] = static_cast< Value >( 1 )/static_cast< Value >( 24 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( 1 )/static_cast< Value >( 8 );
            }
};

template< class Value = double >
struct rk78_coefficients_a4 : boost::array< Value , 4 >
{
    rk78_coefficients_a4( void )
            {
        (*this)[0] = static_cast< Value >( 5 )/static_cast< Value >( 12 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( -25 )/static_cast< Value >( 16 );
        (*this)[3] = static_cast< Value >( 25 )/static_cast< Value >( 16 );
            }
};

template< class Value = double >
struct rk78_coefficients_a5 : boost::array< Value , 5 >
{
    rk78_coefficients_a5( void )
            {
        (*this)[0] = static_cast< Value >( 1 )/static_cast< Value >( 20 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( 0 );
        (*this)[3] = static_cast< Value >( 1 )/static_cast< Value >( 4 );
        (*this)[4] = static_cast< Value >( 1 )/static_cast< Value >( 5 );
            }
};

template< class Value = double >
struct rk78_coefficients_a6 : boost::array< Value , 6 >
{
    rk78_coefficients_a6( void )
            {
        (*this)[0] = static_cast< Value >( -25 )/static_cast< Value >( 108 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( 0 );
        (*this)[3] = static_cast< Value >( 125 )/static_cast< Value >( 108 );
        (*this)[4] = static_cast< Value >( -65 )/static_cast< Value >( 27 );
        (*this)[5] = static_cast< Value >( 125 )/static_cast< Value >( 54 );
            }
};

template< class Value = double >
struct rk78_coefficients_a7 : boost::array< Value , 7 >
{
    rk78_coefficients_a7( void )
            {
        (*this)[0] = static_cast< Value >( 31 )/static_cast< Value >( 300 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( 0 );
        (*this)[3] = static_cast< Value >( 0 );
        (*this)[4] = static_cast< Value >( 61 )/static_cast< Value >( 225 );
        (*this)[5] = static_cast< Value >( -2 )/static_cast< Value >( 9 );
        (*this)[6] = static_cast< Value >( 13 )/static_cast< Value >( 900 );
            }
};

template< class Value = double >
struct rk78_coefficients_a8 : boost::array< Value , 8 >
{
    rk78_coefficients_a8( void )
            {
        (*this)[0] = static_cast< Value >( 2 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( 0 );
        (*this)[3] = static_cast< Value >( -53 )/static_cast< Value >( 6 );
        (*this)[4] = static_cast< Value >( 704 )/static_cast< Value >( 45 );
        (*this)[5] = static_cast< Value >( -107 )/static_cast< Value >( 9 );
        (*this)[6] = static_cast< Value >( 67 )/static_cast< Value >( 90 );
        (*this)[7] = static_cast< Value >( 3 );
            }
};

template< class Value = double >
struct rk78_coefficients_a9 : boost::array< Value , 9 >
{
    rk78_coefficients_a9( void )
            {
        (*this)[0] = static_cast< Value >( -91 )/static_cast< Value >( 108 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( 0 );
        (*this)[3] = static_cast< Value >( 23 )/static_cast< Value >( 108 );
        (*this)[4] = static_cast< Value >( -976 )/static_cast< Value >( 135 );
        (*this)[5] = static_cast< Value >( 311 )/static_cast< Value >( 54 );
        (*this)[6] = static_cast< Value >( -19 )/static_cast< Value >( 60 );
        (*this)[7] = static_cast< Value >( 17 )/static_cast< Value >( 6 );
        (*this)[8] = static_cast< Value >( -1 )/static_cast< Value >( 12 );
            }
};

template< class Value = double >
struct rk78_coefficients_a10 : boost::array< Value , 10 >
{
    rk78_coefficients_a10( void )
            {
        (*this)[0] = static_cast< Value >( 2383 )/static_cast< Value >( 4100 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( 0 );
        (*this)[3] = static_cast< Value >( -341 )/static_cast< Value >( 164 );
        (*this)[4] = static_cast< Value >( 4496 )/static_cast< Value >( 1025 );
        (*this)[5] = static_cast< Value >( -301 )/static_cast< Value >( 82 );
        (*this)[6] = static_cast< Value >( 2133 )/static_cast< Value >( 4100 );
        (*this)[7] = static_cast< Value >( 45 )/static_cast< Value >( 82 );
        (*this)[8] = static_cast< Value >( 45 )/static_cast< Value >( 164 );
        (*this)[9] = static_cast< Value >( 18 )/static_cast< Value >( 41 );
            }
};

template< class Value = double >
struct rk78_coefficients_a11 : boost::array< Value , 11 >
{
    rk78_coefficients_a11( void )
            {
        (*this)[0] = static_cast< Value >( 3 )/static_cast< Value >( 205 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( 0 );
        (*this)[3] = static_cast< Value >( 0 );
        (*this)[4] = static_cast< Value >( 0 );
        (*this)[5] = static_cast< Value >( -6 )/static_cast< Value >( 41 );
        (*this)[6] = static_cast< Value >( -3 )/static_cast< Value >( 205 );
        (*this)[7] = static_cast< Value >( -3 )/static_cast< Value >( 41 );
        (*this)[8] = static_cast< Value >( 3 )/static_cast< Value >( 41 );
        (*this)[9] = static_cast< Value >( 6 )/static_cast< Value >( 41 );
        (*this)[10] = static_cast< Value >( 0 );
            }
};

template< class Value = double >
struct rk78_coefficients_a12 : boost::array< Value , 12 >
{
    rk78_coefficients_a12( void )
            {
        (*this)[0] = static_cast< Value >( -1777 )/static_cast< Value >( 4100 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( 0 );
        (*this)[3] = static_cast< Value >( -341 )/static_cast< Value >( 164 );
        (*this)[4] = static_cast< Value >( 4496 )/static_cast< Value >( 1025 );
        (*this)[5] = static_cast< Value >( -289 )/static_cast< Value >( 82 );
        (*this)[6] = static_cast< Value >( 2193 )/static_cast< Value >( 4100 );
        (*this)[7] = static_cast< Value >( 51 )/static_cast< Value >( 82 );
        (*this)[8] = static_cast< Value >( 33 )/static_cast< Value >( 164 );
        (*this)[9] = static_cast< Value >( 12 )/static_cast< Value >( 41 );
        (*this)[10] = static_cast< Value >( 0 );
        (*this)[11] = static_cast< Value >( 1 );
            }
};

template< class Value = double >
struct rk78_coefficients_b : boost::array< Value , 13 >
{
    rk78_coefficients_b( void )
            {
        (*this)[0] = static_cast< Value >( 0 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( 0 );
        (*this)[3] = static_cast< Value >( 0 );
        (*this)[4] = static_cast< Value >( 0 );
        (*this)[5] = static_cast< Value >( 34 )/static_cast<Value>( 105 );
        (*this)[6] = static_cast< Value >( 9 )/static_cast<Value>( 35 );
        (*this)[7] = static_cast< Value >( 9 )/static_cast<Value>( 35 );
        (*this)[8] = static_cast< Value >( 9 )/static_cast<Value>( 280 );
        (*this)[9] = static_cast< Value >( 9 )/static_cast<Value>( 280 );
        (*this)[10] = static_cast< Value >( 0 );
        (*this)[11] = static_cast< Value >( 41 )/static_cast<Value>( 840 );
        (*this)[12] = static_cast< Value >( 41 )/static_cast<Value>( 840 );
            }
};

template< class Value = double >
struct rk78_coefficients_db : boost::array< Value , 13 >
{
    rk78_coefficients_db( void )
            {
        (*this)[0] = static_cast< Value >( 0 ) - static_cast< Value >( 41 )/static_cast<Value>( 840 );
        (*this)[1] = static_cast< Value >( 0 );
        (*this)[2] = static_cast< Value >( 0 );
        (*this)[3] = static_cast< Value >( 0 );
        (*this)[4] = static_cast< Value >( 0 );
        (*this)[5] = static_cast< Value >( 0 );
        (*this)[6] = static_cast< Value >( 0 );
        (*this)[7] = static_cast< Value >( 0 );
        (*this)[8] = static_cast< Value >( 0 );
        (*this)[9] = static_cast< Value >( 0 );
        (*this)[10] = static_cast< Value >( 0 ) - static_cast< Value >( 41 )/static_cast<Value>( 840 );
        (*this)[11] = static_cast< Value >( 41 )/static_cast<Value>( 840 );
        (*this)[12] = static_cast< Value >( 41 )/static_cast<Value>( 840 );
            }
};

template< class Value = double >
struct rk78_coefficients_c : boost::array< Value , 13 >
{
    rk78_coefficients_c( void )
            {
        (*this)[0] = static_cast< Value >( 0 );
        (*this)[1] = static_cast< Value >( 2 )/static_cast< Value >( 27 );
        (*this)[2] = static_cast< Value >( 1 )/static_cast< Value >( 9 );
        (*this)[3] = static_cast< Value >( 1 )/static_cast<Value>( 6 );
        (*this)[4] = static_cast< Value >( 5 )/static_cast<Value>( 12 );
        (*this)[5] = static_cast< Value >( 1 )/static_cast<Value>( 2 );
        (*this)[6] = static_cast< Value >( 5 )/static_cast<Value>( 6 );
        (*this)[7] = static_cast< Value >( 1 )/static_cast<Value>( 6 );
        (*this)[8] = static_cast< Value >( 2 )/static_cast<Value>( 3 );
        (*this)[9] = static_cast< Value >( 1 )/static_cast<Value>( 3 );
        (*this)[10] = static_cast< Value >( 1 );
        (*this)[11] = static_cast< Value >( 0 );
        (*this)[12] = static_cast< Value >( 1 );
            }
};
#endif // DOXYGEN_SKIP

template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
#ifndef DOXYGEN_SKIP
class runge_kutta_fehlberg78 : public explicit_error_generic_rk< 13 , 8 , 8 , 7 , State , Value , Deriv , Time ,
Algebra , Operations , Resizer >
#else
class runge_kutta_fehlberg78 : public explicit_error_generic_rk
#endif
{

public:
#ifndef DOXYGEN_SKIP
    typedef explicit_error_generic_rk< 13 , 8 , 8 , 7 , State , Value , Deriv , Time ,
            Algebra , Operations , Resizer > stepper_base_type;
#endif
    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;

    #ifndef DOXYGEN_SKIP
    typedef typename stepper_base_type::stepper_type stepper_type;
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    #endif // DOXYGEN_SKIP

    runge_kutta_fehlberg78( const algebra_type &algebra = algebra_type() ) : stepper_base_type(
            boost::fusion::make_vector( rk78_coefficients_a1<Value>() , rk78_coefficients_a2<Value>() , rk78_coefficients_a3<Value>() ,
                    rk78_coefficients_a4<Value>() , rk78_coefficients_a5<Value>() , rk78_coefficients_a6<Value>() ,
                    rk78_coefficients_a7<Value>() , rk78_coefficients_a8<Value>() , rk78_coefficients_a9<Value>() ,
                    rk78_coefficients_a10<Value>() , rk78_coefficients_a11<Value>() , rk78_coefficients_a12<Value>() ) ,
            rk78_coefficients_b<Value>() , rk78_coefficients_db<Value>() , rk78_coefficients_c<Value>() , algebra )
    { }
};

/************* DOXYGEN *************/

/**
 * \class runge_kutta_fehlberg78
 * \brief The Runge-Kutta Fehlberg 78 method.
 *
 * The Runge-Kutta Fehlberg 78 method is a standard method for high-precision applications.
 * The method is explicit and fulfills the Error Stepper concept. Step size control
 * is provided but continuous output is not available for this method.
 *
 * This class derives from explicit_error_stepper_base and inherits its interface via CRTP (current recurring template pattern).
 * Furthermore, it derivs from explicit_error_generic_rk which is a generic Runge-Kutta algorithm with error estimation.
 * For more details see explicit_error_stepper_base and explicit_error_generic_rk.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn runge_kutta_fehlberg78::runge_kutta_fehlberg78( const algebra_type &algebra )
     * \brief Constructs the runge_kutta_cash_fehlberg78 class. This constructor can be used as a default
     * constructor if the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

}
}
}

#endif //BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_FEHLBERG87_HPP_INCLUDED


/*
  @file:    control_base.cpp
  @author:  pdsherman
  @date:    April. 2021

  @brief: control base object
*/

#include <pendulum/LoggingData.h>
#include <pendulum/State.h>
#include <pendulum/Control.h>

#include <hardware/pendulum_hardware/PendulumHardware.hpp>
#include <libs/control/Pid.hpp>
#include <libs/control/DigitalPid.hpp>
#include <libs/control/LeadLag.hpp>
#include <libs/util/ros_util.hpp>

#include <boost/program_options.hpp>
#include <ros/ros.h>
#include <iostream>
#include <thread>   // For this_thread::sleep_until
#include <chrono>

static constexpr double kRunTime_s  = 15.0;
static constexpr int kUpdateCycleTime_us  = 5000;
static constexpr double kMaxForce_N = 80.0;
// Time in seconds between cycles
static constexpr double step_s = static_cast<double>(kUpdateCycleTime_us) / (1e6);

static double Kp = 195.0;
static double Ki = 95.0;
static double Kd = 1.5;

static double Zero = 50.0;
static double Pole = 5.0;
static double Gain = 40.5;

/// Publish data from cycle
void publish(ros::Publisher &log_pub, ros::Publisher &state_pub,
  pendulum::LoggingData &data, pendulum::State &state,
  const double target, const double x, const double u, const double t);

/// Initialize logging to table for test
ros::Publisher initialize_logging(ros::NodeHandle &nh, const std::string &log_table_name);

/// Get current target position for system
double get_target(const double test_time);

bool parse_args(int argc, char *argv[]);

/// ---------------------------------------- ///
/// ----        MAIN FUNCTION           ---- ///
/// ---------------------------------------- ///
int main(int argc, char *argv[])
{
  ros::init(argc, argv, "base_control");
  ros::NodeHandle nh;

  if(!parse_args(argc, argv)) {
    ROS_WARN("Error parsing arguments");
  }
  // ---   Start Logging   --- //
  std::string log_table = "BaseControl";
  ros::Publisher log_pub = initialize_logging(nh, log_table);

  // ---  Display on GUI    --- //
  std::string state_topic = "base_control";
  ros::Publisher state_pub = nh.advertise<pendulum::State>(state_topic, 50);
  util::draw_image(nh, state_topic, 0.0, 0.0, pendulum::DrawSystemRequest::MASS_ONLY, "blue");

  // --- Initialize Variables --- //
  std::shared_ptr<Controller<double, double>> cntrl;
  std::string test_name;
  // if(argc > 1) {
  //   if(std::string(argv[1]) == "pid"){
  cntrl = std::dynamic_pointer_cast<Controller<double, double>>(
    std::make_shared<PID>(step_s, Kp, Ki, Kd, 0.0));
  test_name = "PID";
  ROS_INFO("Using PID control");
  ROS_INFO("Kp: %.2f", Kp);
  ROS_INFO("Kd: %.2f", Kd);
  ROS_INFO("Ki: %.2f", Ki);

  //   } else if(std::string(argv[1]) == "dpid"){
  //     cntrl = std::dynamic_pointer_cast<Controller<double, double>>(
  //       std::make_shared<DigitalPID>(step_s, Kp, Ki, Kd, 0.0));
  //     test_name = "digital-PID";
  //   } else if(std::string(argv[1]) == "lag") {
  //     cntrl = std::dynamic_pointer_cast<Controller<double, double>>(
  //       std::make_shared<LeadLag>(step_s, Zero, Pole, Gain, LeadLag::DigitialTransform::kTustins));
  //     test_name = "lag";
  //   } else {
  //     ROS_WARN("Invalid controller type");
  //     return 0;
  //   }
  // } else {
  //   ROS_WARN("Need to pick a controller type (pid|dpid|lag)");
  //   return 0;
  // }

  PendulumHardware::Positions pos;
  PendulumHardware hw;

  pendulum::State state;
  state.x     = 0.0;
  state.theta = 0.0;

  pendulum::LoggingData data;
  data.test_name = test_name;
  data.data      = std::vector<double>(4);

  double x = 0.0; // Initialize Position
  double u = 0.0; // Input command
  double t = 0.0; // Time since start of test

  // ---   Run Test   --- //
  ros::Duration(1.0).sleep();

  if(!hw.initialize() || !hw.motor_enable()) {
    ROS_ERROR("Failed to initialize hardware.");
    return 0;
  }
  ROS_INFO("Hardware Initialized.");
  ROS_INFO("--- Beginning Test ---");

  std::chrono::steady_clock::time_point time_start = std::chrono::steady_clock::now();
  std::chrono::steady_clock::time_point time_now   = time_start;
  std::chrono::steady_clock::time_point time_loop  = time_start;

  // ***************************************** //
  // ****        MAIN TEST LOOP           **** //
  // ***************************************** //
  while(ros::ok() && t < kRunTime_s) {
    /// Calculate end-time of cycle
    using namespace std::chrono;
    time_loop += microseconds(kUpdateCycleTime_us);

    // Feedback Loop
    double target = get_target(t);
    cntrl->set_target(target);
    u   = cntrl->update(x);

    if(u > kMaxForce_N) {
      ROS_WARN("Command Input MAX Saturation");
      u = kMaxForce_N;
    } else if(u < -kMaxForce_N) {
      ROS_WARN("Command Input MIN Saturation");
      u = -kMaxForce_N;
    }

    pos = hw.update(u);
    x   = pos.position_x;

    // Publish
    int dt   = duration_cast<microseconds>(time_now - time_start).count();
    t = static_cast<double>(dt)/1000000.0;
    publish(log_pub, state_pub, data, state, target, x, u, t);

    //   Wait until end of cycle time
    std::this_thread::sleep_until(time_loop);
    time_now  = steady_clock::now();
  }

  ROS_INFO("--- End of Test Loop ---");
  util::stop_logging(nh, log_table);
  util::remove_image(nh, state_topic);

  return 0;
}

ros::Publisher initialize_logging(ros::NodeHandle &nh, const std::string &log_table_name)
{
  std::string logging_topic = "base_model";
  ros::Publisher log_pub = nh.advertise<pendulum::LoggingData>("/" + logging_topic, 50);

  std::vector<std::string> header({
    "test", "test_time", "target", "input", "x"
  });

  if(!util::drop_logging_table(nh, log_table_name) ||
     !util::start_logging(nh, log_table_name, logging_topic, header)) {
    ROS_WARN("Failed to start logging.");
  }

  return log_pub;
}

void publish(ros::Publisher &log_pub, ros::Publisher &state_pub,
  pendulum::LoggingData &data, pendulum::State &state,
  const double target, const double x, const double u, const double t)
{
  auto timestamp = ros::Time::now();

  state.header.seq += 1;
  state.header.stamp = timestamp;
  state.x = x;
  state_pub.publish(state);

  data.header.seq += 1;
  data.header.stamp = timestamp;
  data.data[0] = t;
  data.data[1] = target;
  data.data[2] = u;
  data.data[3] = x;
  log_pub.publish(data);

  ros::spinOnce();
}

double get_target(const double test_time)
{
  double target = 0.0;
  if(test_time < 0.5)
    target = 0.0;
  else if (test_time < 5.5)
    target = 0.3;
  else
    target = 0.0;

  return target;
}

bool parse_args(int argc, char *argv[])
{
  boost::program_options::options_description desc("Options");
  try {
    desc.add_options()
      ("help", "produce help message")
      ("Kp", boost::program_options::value<double>(), "Proportional Gain")
      ("Kd", boost::program_options::value<double>(), "Derivative Gain")
      ("Ki", boost::program_options::value<double>(), "Integral Gain");

      boost::program_options::variables_map var_map;
      boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).run(), var_map);

      if (var_map.count("help")) {
        std::cout << desc << std::endl;
        return false;
      }

      if (var_map.count("Kp"))
        Kp = var_map["Kp"].as<double>();
      if (var_map.count("Kd"))
        Kd = var_map["Kd"].as<double>();
      if (var_map.count("Ki"))
        Ki = var_map["Ki"].as<double>();

    } catch(const std::invalid_argument& ia) {
      ROS_WARN("Invalid String as Integer. Program will exit");
      return false;
    } catch(...) {
      ROS_WARN("Invalid Option. Program will exit");
      return false;
    }

    return true;
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersSerializeFixture.hpp"
#include "../Deserializer.hpp"

#include <string>
#include <iostream>

BOOST_AUTO_TEST_SUITE(DeserializeParser)

struct FullyConnectedFixture : public ParserFlatbuffersSerializeFixture
{
    explicit FullyConnectedFixture(const std::string & inputShape1,
                                   const std::string & outputShape,
                                   const std::string & weightsShape,
                                   const std::string & dataType)
    {
        m_JsonString = R"(
        {
            inputIds: [0],
            outputIds: [2],
            layers: [{
                layer_type: "InputLayer",
                layer: {
                    base: {
                        layerBindingId: 0,
                        base: {
                            index: 0,
                            layerName: "InputLayer",
                            layerType: "Input",
                            inputSlots: [{
                                index: 0,
                                connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                }],
                            outputSlots: [{
                                index: 0,
                                tensorInfo: {
                                    dimensions: )" + inputShape1 + R"(,
                                    dataType: )" + dataType + R"(,
                                    quantizationScale: 1.0,
                                    quantizationOffset: 0
                                    },
                                }]
                            },
                        }
                    },
                },
            {
            layer_type: "FullyConnectedLayer",
            layer : {
                base: {
                    index:1,
                    layerName: "FullyConnectedLayer",
                    layerType: "FullyConnected",
                    inputSlots: [{
                            index: 0,
                            connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                        }],
                    outputSlots: [{
                        index: 0,
                        tensorInfo: {
                            dimensions: )" + outputShape + R"(,
                            dataType: )" + dataType + R"(,
                            quantizationScale: 2.0,
                            quantizationOffset: 0
                        },
                        }],
                    },
                descriptor: {
                    biasEnabled: false,
                    transposeWeightsMatrix: true
                    },
                weights: {
                    info: {
                             dimensions: )" + weightsShape + R"(,
                             dataType: )" + dataType + R"(,
                             quantizationScale: 1.0,
                             quantizationOffset: 0
                         },
                    data_type: ByteData,
                    data: {
                        data: [
                            2, 3, 4, 5
                            ],
                        }
                    }
                },
            },
            {
            layer_type: "OutputLayer",
            layer: {
                base:{
                    layerBindingId: 0,
                    base: {
                        index: 2,
                        layerName: "OutputLayer",
                        layerType: "Output",
                        inputSlots: [{
                            index: 0,
                            connection: {sourceLayerIndex:1, outputSlotIndex:0 },
                        }],
                        outputSlots: [ {
                            index: 0,
                            tensorInfo: {
                                dimensions: )" + outputShape + R"(,
                                dataType: )" + dataType + R"(
                            },
                        }],
                    }
                }},
            }]
        }
        )";
        Setup();
    }
};

struct FullyConnectedWithNoBiasFixture : FullyConnectedFixture
{
    FullyConnectedWithNoBiasFixture()
        : FullyConnectedFixture("[ 1, 4, 1, 1 ]",     // inputShape
                                "[ 1, 1 ]",           // outputShape
                                "[ 1, 4 ]",           // filterShape
                                "QuantisedAsymm8")     // filterData
    {}
};

BOOST_FIXTURE_TEST_CASE(FullyConnectedWithNoBias, FullyConnectedWithNoBiasFixture)
{
    RunTest<2, armnn::DataType::QAsymmU8>(
         0,
         {{"InputLayer",  { 10, 20, 30, 40 }}},
         {{"OutputLayer", { 400/2 }}});
}

BOOST_AUTO_TEST_SUITE_END()


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>

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

using std::cout;

template <typename MatrixA, typename MatrixB, typename MatrixC>
void test(MatrixA&, MatrixB&, MatrixC&, const char* name)
{
    double aa[][3]= {{0., 2., 0.}, {0., 0., 1.}, {1., 0., 0.}},
	   ba[][3]= {{0., 3., 0.}, {4., 0., 0.}, {1., 0., 0.}};

    MatrixA A(aa);
    MatrixB B(ba);
    MatrixC C;

    cout << "\n\n" << name << "\n";
    cout << "Original matrices:\nA=\n" << A << "B=\n" << B << "\n";

    C= ele_prod(A, B);
    cout << "C= ele_prod(A, B)\n" << C << "\n";
    MTL_THROW_IF(C[0][1] != 6.0, mtl::runtime_error("C[0][1] should be 6.0"));
    MTL_THROW_IF(C[1][0] != 0.0, mtl::runtime_error("C[1][0] should be 0.0"));

    C= A + ele_prod(A, B);
    cout << "C= A + ele_prod(A, B)\n" << C << "\n";
    MTL_THROW_IF(C[0][1] != 8.0, mtl::runtime_error("C[0][1] should be 6.0"));
    MTL_THROW_IF(C[1][2] != 1.0, mtl::runtime_error("C[1][2] should be 1.0"));

    C= ele_prod(ele_prod(A, B), A);
    cout << "C= ele_prod(ele_prod(A, B), A)\n" << C << "\n";
    MTL_THROW_IF(C[0][1] != 12.0, mtl::runtime_error("C[0][1] should be 12.0"));
    MTL_THROW_IF(C[1][0] != 0.0, mtl::runtime_error("C[1][0] should be 0.0"));

    C= ele_prod(A, ele_prod(A, B));
    cout << "C= ele_prod(ele_prod(A, B), A)\n" << C << "\n";
    MTL_THROW_IF(C[0][1] != 12.0, mtl::runtime_error("C[0][1] should be 12.0"));
    MTL_THROW_IF(C[1][0] != 0.0, mtl::runtime_error("C[1][0] should be 0.0"));

    C-= ele_prod(A, B);
    cout << "C-= ele_prod(A, B)\n" << C << "\n";
    MTL_THROW_IF(C[0][1] != 6.0, mtl::runtime_error("C[0][1] should be 6.0"));
    MTL_THROW_IF(C[1][0] != 0.0, mtl::runtime_error("C[1][0] should be 0.0"));

    C+= ele_prod(A, B);
    cout << "C+= ele_prod(A, B)\n" << C << "\n";
    MTL_THROW_IF(C[0][1] != 12.0, mtl::runtime_error("C[0][1] should be 12.0"));
    MTL_THROW_IF(C[1][0] != 0.0, mtl::runtime_error("C[1][0] should be 0.0"));

}

int main(int, char**)
{
    using namespace mtl;

    dense2D<double>                                      dr(3, 3);
    dense2D<double, mat::parameters<col_major> >      dc(3, 3);
    morton_dense<double, recursion::morton_z_mask>       mzd(3, 3);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(3, 3);
    compressed2D<double>                                 cr(3, 3);
    compressed2D<double, mat::parameters<col_major> > cc(3, 3);

    test(dr, dr, dr, "Dense row major");
    test(dc, dr, dr, "Dense column major as sum of dense rows");
    test(dc, dr, dc, "Dense column major as sum of dense rows and column");

    test(mzd, mzd, mzd, "Morton Z-order");
    test(d2r, mzd, d2r, "Hybrid 2 row-major + Morton Z-order");

    test(cr, cr, cr, "Compressed row major");
    test(cc, cr, cc, "Compressed column major + row");

    return 0;
}


#pragma once

#ifndef _MATRIX_H_
#define _MATRIX_H_

// Includes
#include <Eigen/Dense>
#include <viennacl/matrix.hpp>
#include "AbstractLinearAlgebraObject.hpp"

namespace LightBulb
{
	/**
	 * \brief Describes a two dimensional linear algebra data structure, also called matrix.
	 * \note Red or blue?
	 * \tparam DataType The data type which should be stored in the data structure.
	 */
	template<typename DataType = float>
	class Matrix : public AbstractLinearAlgebraObject<Eigen::Matrix<DataType, -1, -1>, viennacl::matrix<DataType>>
	{
	protected:
		// Inherited:
		void copyToEigen() const override
		{
			if (this->eigenValue.rows() != this->viennaclValue.size1() || this->eigenValue.cols() != this->viennaclValue.size2())
				this->eigenValue.resize(this->viennaclValue.size1(), this->viennaclValue.size2());

			viennacl::copy(this->viennaclValue, this->eigenValue);
		}
		void copyToViennaCl() const override
		{
			if (this->eigenValue.rows() != this->viennaclValue.size1() || this->eigenValue.cols() != this->viennaclValue.size2())
				this->viennaclValue.resize(this->eigenValue.rows(), this->eigenValue.cols());

			if (this->eigenValue.size() != 0)
				viennacl::copy(this->eigenValue, this->viennaclValue);
		}
	public:
		/**
		 * \brief Creates a new matrix with a specific size.
		 * \param rows The amount of rows.
		 * \param cols The amount of cols.
		 */
		Matrix(int rows = 0, int cols = 0)
		{
			if (rows > 0 && cols > 0) {
				this->eigenValue = Eigen::Matrix<DataType, -1, -1>(rows, cols);
				this->eigenValueIsDirty = true;
			}
		}
		Matrix(const Eigen::Matrix<DataType, -1, -1>& eigenMatrix)
		{
			this->eigenValue = eigenMatrix;
			this->eigenValueIsDirty = true;
		}
		Matrix(const Matrix& other)
			: AbstractLinearAlgebraObject<Eigen::Matrix<DataType, -1, -1>, viennacl::matrix<DataType>>()
		{
			if (!((other.eigenValueIsDirty && other.eigenValue.size() == 0) || (other.viennaclValueIsDirty && other.viennaclValue.size1() == 0 && other.viennaclValue.size2() == 0) || (other.eigenValue.size() == 0 && other.viennaclValue.size1() == 0 && other.viennaclValue.size2() == 0)))
				this->copyAllFrom(other);
		}
	};

}

#include "LightBulb/IO/MatrixIO.hpp"

#endif


// Copyright (C) 2016 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "gtest/gtest.h"
#include <Eigen/Core>
#include <Eigen/LU>

#include "theia/math/qp_solver.h"
#include "theia/util/random.h"

namespace theia {

// A rigged QP minimization problem with a known output.
//
// 1/2 * x' * P * x + q' * x + r
//     [  5  -2  -1 ]
// P = [ -2   4   3 ]
//     [ -1   3   5 ]
//
// q = [ 2  -35  -47 ]^t
// r = 5
//
// Minimizing this unbounded problem should result in:
//   x = [ 3  5  7 ]^t
TEST(QPSolver, Unbounded) {
  static const double kTolerance = 1e-4;

  Eigen::MatrixXd P(3, 3);
  P << 5, -2, -1, -2, 4, 3, -1, 3, 5;
  Eigen::VectorXd q(3);
  q << 2, -35, -47;
  const double r = 5;

  QPSolver::Options options;
  options.max_num_iterations = 100;
  Eigen::SparseMatrix<double> P_sparse(P.sparseView());
  QPSolver qp_solver(options, P_sparse, q, r);
  Eigen::VectorXd solution;
  ASSERT_TRUE(qp_solver.Solve(&solution));

  // Verify the solution is near (3, 5, 7).
  const Eigen::Vector3d gt_solution(3, 5, 7);
  for (int i = 0; i < 3; i++) {
    EXPECT_NEAR(solution(i), gt_solution(i), kTolerance);
  }

  // Check that the residual is near optimal.
  const double residual =
      0.5 * solution.dot(P * solution) + solution.dot(q) + r;
  const double gt_residual =
      0.5 * gt_solution.dot(P * gt_solution) + gt_solution.dot(q) + r;
  EXPECT_NEAR(residual, gt_residual, kTolerance);
}

TEST(QPSolver, LooseBounds) {
  static const double kTolerance = 1e-4;

  Eigen::MatrixXd P(3, 3);
  P << 5, -2, -1, -2, 4, 3, -1, 3, 5;
  Eigen::VectorXd q(3);
  q << 2, -35, -47;
  const double r = 5;

  QPSolver::Options options;
  options.max_num_iterations = 100;
  Eigen::SparseMatrix<double> P_sparse(P.sparseView());
  QPSolver qp_solver(options, P_sparse, q, r);

  // Set a lower bound that should not affect the output.
  Eigen::VectorXd lower_bound(3);
  lower_bound << 0, 0, 0;
  qp_solver.SetLowerBound(lower_bound);

  // Set an upper bound that should not affect the output.
  Eigen::VectorXd upper_bound(3);
  upper_bound << 10, 10, 10;
  qp_solver.SetUpperBound(upper_bound);
  Eigen::VectorXd solution;

  ASSERT_TRUE(qp_solver.Solve(&solution));

  // Verify the solution is near (3, 5, 7).
  const Eigen::Vector3d gt_solution(3, 5, 7);
  for (int i = 0; i < 3; i++) {
    EXPECT_NEAR(solution(i), gt_solution(i), kTolerance);
  }

  // Check that the residual is near optimal.
  const double residual =
      0.5 * solution.dot(P * solution) + solution.dot(q) + r;
  const double gt_residual =
      0.5 * gt_solution.dot(P * gt_solution) + gt_solution.dot(q) + r;
  EXPECT_NEAR(residual, gt_residual, kTolerance);
}

TEST(QPSolver, TightBounds) {
  static const double kTolerance = 1e-4;

  Eigen::MatrixXd P(3, 3);
  P << 5, -2, -1, -2, 4, 3, -1, 3, 5;
  Eigen::VectorXd q(3);
  q << 2, -35, -47;
  const double r = 5;

  QPSolver::Options options;
  options.absolute_tolerance = 1e-8;
  options.relative_tolerance = 1e-8;
  Eigen::SparseMatrix<double> P_sparse(P.sparseView());
  QPSolver qp_solver(options, P_sparse, q, r);

  // Set a lower bound that constrains the output.
  Eigen::VectorXd lower_bound(3);
  lower_bound << 5, 7, 9;
  qp_solver.SetLowerBound(lower_bound);

  // Set an upper bound that constrains the output.
  Eigen::VectorXd upper_bound(3);
  upper_bound << 10, 12, 14;
  qp_solver.SetUpperBound(upper_bound);

  Eigen::VectorXd solution;
  ASSERT_TRUE(qp_solver.Solve(&solution));

  // Verify the solution is near (5, 7, 9).
  const Eigen::Vector3d gt_solution(5, 7, 9);
  for (int i = 0; i < 3; i++) {
    EXPECT_NEAR(solution(i), gt_solution(i), kTolerance);
  }

  // Check that the residual is near optimal.
  const double residual =
      0.5 * solution.dot(P * solution) + solution.dot(q) + r;
  const double gt_residual =
      0.5 * gt_solution.dot(P * gt_solution) + gt_solution.dot(q) + r;
  EXPECT_NEAR(residual, gt_residual, kTolerance);
}

TEST(QPSolver, InvalidBounds) {
  Eigen::MatrixXd P(3, 3);
  P << 5, -2, -1, -2, 4, 3, -1, 3, 5;
  Eigen::VectorXd q(3);
  q << 2, -35, -47;
  const double r = 5;

  QPSolver::Options options;
  Eigen::SparseMatrix<double> P_sparse(P.sparseView());
  QPSolver qp_solver(options, P_sparse, q, r);

  // Set the upper bound as the lower bound.
  Eigen::VectorXd lower_bound(3);
  lower_bound << 5, 7, 9;
  qp_solver.SetUpperBound(lower_bound);

  // Set the lower bound as the upper bound, making the valid solution space
  // non-existant.
  Eigen::VectorXd upper_bound(3);
  upper_bound << 10, 12, 14;
  qp_solver.SetLowerBound(upper_bound);

  Eigen::VectorXd solution;
  EXPECT_FALSE(qp_solver.Solve(&solution));
}

}  // namespace theia


#include <iostream>

#include <numbers>
#include <Eigen/Geometry>

#include "functional_mesh.h"

///////////////////////////////////////////////////////////////
//                    CFunctionalMesh
///////////////////////////////////////////////////////////////

CFunctionalMesh::CFunctionalMesh()
{
    SetColorFunctor(nullptr);
}

void CFunctionalMesh::m_InvalidateAll()const
{
    m_valid_points=m_valid_normals=m_valid_colors=false;
    m_levels_valid[0]=m_levels_valid[1]=m_levels_valid[2]=false;
}

void CFunctionalMesh::m_SetBoundedBox()const
{
    using eig_size_t=decltype(m_points.rows());
    m_bounded_box.first=m_bounded_box.second=m_points(0,0);
    for(eig_size_t i_s=0;i_s<m_points.rows();++i_s)
    {
        for(eig_size_t i_t=0;i_t<m_points.cols();++i_t)
        {
            for(size_t dim=0;dim<3;dim++)
            {
                const point_t& current=m_points(i_s,i_t);
                m_bounded_box.first[dim]=std::min(current[dim],m_bounded_box.first[dim]);
                m_bounded_box.second[dim]=std::max(current[dim],m_bounded_box.second[dim]);
            }
        }
    }
}

// normal to parametrically defined surface
// defined as || (dr / ds) x (dr / dt) ||
// derivatives are approximated by finite differences

void CFunctionalMesh::m_FillNormals()const
{
    using eig_size_t=decltype(m_points.rows());
    float s_delta=m_grid.s_delta();
    auto get_s_tangent=[this,&s_delta](size_t i,size_t j)->point_t
    {
        return (m_points(i+1,j)-m_points(i-1,j))/(2*s_delta);
    };
    auto get_s_top_bound=[&](size_t j)->point_t
    {
        return (m_points(1,j)-m_points(0,j))/(s_delta);
    };
    auto get_s_down_bound=[&](size_t j)->point_t
    {
        return (m_points(m_points.rows()-1,j)-m_points(m_points.rows()-2,j))/(s_delta);
    };

    float t_delta=m_grid.t_delta();
    auto get_t_tangent=[this,&t_delta](size_t i,size_t j)->point_t
    {
        return (m_points(i,j+1)-m_points(i,j-1) )/(2*t_delta);
    };
    auto get_t_left_bound=[&](size_t i)->point_t
    {
        return (m_points(i,1)-m_points(i,0))/(t_delta);
    };
    auto get_t_right_bound=[&](size_t i)->point_t
    {
        return (m_points(i,m_points.cols()-1)-m_points(i,m_points.cols()-2))/(t_delta);
    };

    m_normals.resize(m_points.rows(),m_points.cols());

    // internal domain
    for(eig_size_t i=1;i<m_normals.rows()-1;++i)
      for(eig_size_t j=1;j<m_normals.cols()-1;++j)
      {
          m_normals(i,j)=get_s_tangent(i,j).cross(get_t_tangent(i,j));
          m_normals(i,j).normalize();
      }
    // left bound
    for(eig_size_t i=1;i<m_normals.rows()-1;++i)
    {
        m_normals(i,0)=get_s_tangent(i,0).cross(get_t_left_bound(i));
        m_normals(i,0).normalize();
    }
    // right bound
    for(eig_size_t i=1;i<m_normals.rows()-1;++i)
    {
        m_normals(i,m_normals.cols()-1)=get_s_tangent(i,0).cross(get_t_right_bound(i));
        m_normals(i,m_normals.cols()-1).normalize();
    }

    // top bound
    for(eig_size_t i=1;i<m_normals.cols()-1;++i)
    {
        m_normals(0,i)=get_s_top_bound(i).cross(get_t_tangent(0,i));
        m_normals(0,i).normalize();
    }
    // down bound
    for(eig_size_t i=1;i<m_normals.cols()-1;++i)
    {
        m_normals(m_normals.rows()-1,i)=get_s_down_bound(i).cross(get_t_tangent(m_normals.rows()-1,i));
        m_normals(m_normals.rows()-1,i).normalize();
    }
    // corners
    m_normals(0,0)=get_s_top_bound(0).cross(get_t_left_bound(0));
    m_normals(0,0).normalize();

    m_normals(0,m_normals.cols()-1)=get_s_top_bound(m_normals.cols()-1).cross(get_t_right_bound(0));
    m_normals(0,m_normals.cols()-1).normalize();

    m_normals(m_normals.rows()-1,m_normals.cols()-1)=get_s_down_bound(m_normals.cols()-1).
                                                     cross(get_t_right_bound(m_normals.rows()-1));
    m_normals(m_normals.rows()-1,m_normals.cols()-1).normalize();

    m_normals(m_normals.rows()-1,0)=get_s_down_bound(0).cross(get_t_left_bound(m_normals.rows()-1));
    m_normals(m_normals.rows()-1,0).normalize();
}

void CFunctionalMesh::m_SetLevelLines(int index,float time)const
{
    assert(index==0||index==1||index==2);
    assert(m_valid_points);
    m_levels[index].clear();
    auto cell_pocess=[index,time,this](float level,int i,int j,int& lines)
    ->std::array<point_t,4>
    {
        auto is_same_sign=[](float _1,float _2){ return (_1>=0) == (_2>=0);};
        std::array<float,4> diffs;
        std::array<point_t,4> cell={m_points(i+1,j),m_points(i+1,j+1),m_points(i,j+1),m_points(i,j)};
        std::array<int,4>   intersections;
        int num_intersetions=0;
        auto intersection_point=[&](int side)
        {
            int next=(1+side)%4;
            return (cell[side]*diffs[next]-cell[next]*diffs[side])/(diffs[next]-diffs[side]);
        };
        diffs[0]=cell[0][index]-level;
        for(int i=0;i<4;++i)
        {
            int next_index=(i+1)%4;
            diffs[next_index]=cell[next_index][index]-level;
            if(!is_same_sign(diffs[i],diffs[next_index])) intersections[num_intersetions++]=i;
        }
        lines=num_intersetions/2;
        if(lines==0) return std::array<point_t,4>{};
        else if(num_intersetions==2)
        {
            return std::array<point_t,4>{intersection_point(intersections[0]),
                                         intersection_point(intersections[1]),
                                         point_t{},point_t{}};
        }
        else if(num_intersetions==4)
        {
            float center_diff=m_points_functor(m_grid.s(i)+m_grid.s_delta()/2,
                                               m_grid.t(j)+m_grid.t_delta()/2,
                                               time)[index]-level;
            if(is_same_sign(diffs[0],center_diff))
            {
                    return {
                                intersection_point(intersections[0]),
                                intersection_point(intersections[1]),
                                intersection_point(intersections[2]),
                                intersection_point(intersections[3])
                            };
            }
            else
            {
                    return {
                                intersection_point(intersections[1]),
                                intersection_point(intersections[2]),
                                intersection_point(intersections[3]),
                                intersection_point(intersections[0])
                            };
            }
        }
        else assert(false);
    };

    float delta=(m_bounded_box.second[index]-m_bounded_box.first[index])/(1+m_num_levels[index]);

    for(size_t k=0;k<m_num_levels[index];++k)
    {
        float level=delta*(k+1);
        m_levels[index].push_back(level_line_t(level));
        auto&current=m_levels[index].back();

        for(size_t i=0;i<m_grid.s_resolution;++i)
            for(size_t j=0;j<m_grid.t_resolution;++j)
            {
                int num_lines;
                auto lines=cell_pocess(level+m_bounded_box.first[index],i,j,num_lines);
                if(num_lines==0) continue;

                if(num_lines==1) current.push_back(lines[0],lines[1]);
                if(num_lines==2) current.push_back(lines[2],lines[3]);
            }
    }
}

CFunctionalMesh& CFunctionalMesh::SetUniformColor(const point_t&p)
{
    SetColorFunctor([p](float,float,float){return p;});
    m_traits.SetColored(true);
    m_valid_colors=false;
    assert(m_traits.IsColored());
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetTraits(const CRenderingTraits&traits)
{
    m_traits=traits;
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetAmbientReflection(float val)
{
    m_material.ambient=val;
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetDiffuseReflection(float val)
{
    m_material.diffuse=val;
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetSpecularReflection(float val)
{
    m_material.specular=val;
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetShininess(float val)
{
    m_material.shininess=val;
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetGrid(grid_t grid)
{
    assert(!grid.empty());
    if(m_grid!=grid) m_InvalidateAll();
    m_grid=grid;
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetResolution(size_t s_resol,size_t t_resol)
{
    if(s_resol==m_grid.s_resolution&&t_resol==m_grid.t_resolution) return*this;
    m_grid.s_resolution=s_resol;
    m_grid.t_resolution=t_resol;
    assert(!m_grid.empty());
    m_InvalidateAll();
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetRange(std::pair<float,float> s_range,std::pair<float,float> t_range)
{
    if(s_range==m_grid.s_range&&t_range==m_grid.t_range) return *this;
    m_grid.s_range=s_range;
    m_grid.t_range=t_range;
    m_InvalidateAll();
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetNumberOfLevelsX(uint32_t x)
{
    m_levels_valid[0]= x==m_num_levels[0];
    m_num_levels[0]=x;
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetNumberOfLevelsY(uint32_t y)
{
    m_levels_valid[1]= y==m_num_levels[1];
    m_num_levels[1]=y;
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetNumberOfLevelsZ(uint32_t z)
{
    m_levels_valid[2]= z==m_num_levels[2];
    m_num_levels[2]=z;
    return *this;
}

void CFunctionalMesh::Clear()
{
    m_points_functor=nullptr;
    m_InvalidateAll();
}

CFunctionalMesh::CUpdateResult CFunctionalMesh::UpdateData(float time)
{
    int update=0;
    if(Empty()) return CUpdateResult(0);
    if(IsDynamic())
    {
       if(time!=m_last_update_time) m_InvalidateAll();
    }
    else
    {
        time=0.0f;
    }

    if(!m_valid_points)
    {
        //std::cout<<"UPDATE POINTS\n";
        if(m_grid.s_resolution+1!=m_points.rows()||m_grid.t_resolution+1!=m_points.cols())
        {
            m_points.resize(m_grid.s_resolution+1,m_grid.t_resolution+1);
            update|=CUpdateResult::update_grid;
        }
        m_fill_functor(m_points,m_grid,time);
        m_SetBoundedBox();
        m_valid_points=true;
        update|=CUpdateResult::update_points;
    }
    if(!m_valid_normals&&m_traits.IsSpecularSurface())
    {
        //std::cout<<"UPDATE NORMALS\n";
        m_normals.resize(m_grid.s_resolution+1,m_grid.t_resolution+1);
        m_FillNormals();
        m_valid_normals=true;
        update|=CUpdateResult::update_normals;
    }
    if(!m_valid_colors&&m_traits.IsColored())
    {
        //std::cout<<"UPDATE COLORS\n";
        m_colors.resize(m_grid.s_resolution+1,m_grid.t_resolution+1);
        m_colors_functor(m_points,m_colors);
        m_valid_colors=true;
        update|=CUpdateResult::update_colors;
    }
    for(int i=0;i<3;++i)
    {
        if(!m_levels_valid[i]&&m_traits.IsLevelLines(i))
        {
            //std::cout<<"UPDATE LEVELS\n";
            m_SetLevelLines(i,time);
            m_levels_valid[i]=true;
            update|=CUpdateResult::update_levels(i);
        }
    }
    m_last_update_time=time;
    if(m_update_callback)
    {
        m_update_callback(*this,CUpdateResult(update));
    }
    return CUpdateResult(update);
}

CFunctionalMesh::CUpdateResult CFunctionalMesh::UpdateData()
{
    return UpdateData(m_last_update_time);
}

// Get functions

CFunctionalMesh::grid_t CFunctionalMesh::GetGrid()const
{
    return m_grid;
}

const material_t&CFunctionalMesh::GetMaterial()const
{
    return m_material;
}

uint32_t CFunctionalMesh::GetNumberOfLevel(int i)const
{
    return m_num_levels[i];
}

const CFunctionalMesh::matrix_t*CFunctionalMesh::Points()const
{
    return m_valid_points? &m_points:nullptr;
}

const CFunctionalMesh::matrix_t*CFunctionalMesh::Colors()const
{
    return m_valid_colors? &m_colors:nullptr;
}

const CFunctionalMesh::matrix_t*CFunctionalMesh::Normals()const
{
    return m_valid_normals? &m_normals:nullptr;
}

const std::pair<CFunctionalMesh::point_t,CFunctionalMesh::point_t>* CFunctionalMesh::BoundedBox()const
{
    return m_valid_points? &m_bounded_box:nullptr;
}

const std::vector<CFunctionalMesh::level_line_t>*  CFunctionalMesh::Levels(int i)const
{
    return m_levels_valid[i]? &m_levels[i]:nullptr;
}

bool CFunctionalMesh::Empty()const
{
    return m_points_functor==nullptr;
}

float CFunctionalMesh::LastUpdateTime()const
{
    return m_last_update_time;
}

CRenderingTraits&CFunctionalMesh::RenderingTraits()
{
    return m_traits;
}

const CRenderingTraits&CFunctionalMesh::RenderingTraits()const
{
    return m_traits;
}

// Set specific surface

CFunctionalMesh& CFunctionalMesh::SetSphere(float r,size_t teta_resol,size_t phi_resol)
{
    using namespace std::numbers;
    SetMeshFunctor(plot::spherical([r](float,float){return r;}));
    SetRange({0.00001,pi_v<float>-0.00001},{0.0f,2*pi_v<float>});
    SetResolution(teta_resol,phi_resol);
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetTorus(float rad,float tubular,size_t rad_resol,size_t tubular_resol)
{
    using namespace std::numbers;
    auto mesh_f=[rad,tubular](float teta,float phi)
    {
        float r=rad+tubular*std::cos(phi);
        return point_t(r*std::cos(teta),r*std::sin(teta),tubular*std::sin(phi));
    };
    SetMeshFunctor(mesh_f);
    SetRange({0.0f,2*pi_v<float>},{0.0f,2*pi_v<float>});
    SetResolution(rad_resol,tubular_resol);
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetCylinder(float rad,float h,size_t phi_resol)
{
    using namespace std::numbers;
    auto mesh_f=[rad](float phi,float z)
    {
        return point_t(rad*std::cos(phi),rad*std::sin(phi),z);
    };
    SetMeshFunctor(mesh_f);
    SetRange({0.0f,2*pi_v<float>},{0.0f,h});
    SetResolution(phi_resol,1);
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetCone(float bottom_rad,float h,size_t phi_resol)
{
    using namespace std::numbers;
    auto mesh_f=[bottom_rad,h](float phi,float z)
    {
        float rad=bottom_rad*(1.0f-z/h);
        return point_t(rad*std::cos(phi),rad*std::sin(phi),z);
    };
    SetMeshFunctor(mesh_f);
    SetRange({0.0f,2*pi_v<float>},{0.0f,h});
    SetResolution(phi_resol,1);
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetPlane(float dx,float dy,size_t x_resol,size_t y_resol)
{
    SetMeshFunctor([](float x,float y){return point_t(x,y,0);});
    SetRange({-dx/2,dx/2},{-dy/2,dy/2});
    SetResolution(x_resol,y_resol);
    return *this;
}

// Transformations

CFunctionalMesh& CFunctionalMesh::SetAnchor(const point_t&p)
{
    m_anchor=p;
    return *this;
}

CFunctionalMesh& CFunctionalMesh::DeltaOrg(float x,float y,float z)
{
    m_rigid.DeltaOrg(x,y,z);
    return *this;
}

CFunctionalMesh& CFunctionalMesh::DeltaOrg(const point_t&p)
{
    m_rigid.DeltaOrg(p);
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetOrg(float x,float y,float z)
{
    m_rigid.SetOrg(x,y,z);
    return *this;
}

CFunctionalMesh& CFunctionalMesh::SetOrg(const point_t&p)
{
    m_rigid.SetOrg(p);
    return *this;
}

CRigidTransform::point_t CFunctionalMesh::GetOrg()const
{
    return m_rigid.GetOrg();
}

const CRigidTransform::matrix_t& CFunctionalMesh::GetTransform()const
{
    return m_rigid.GetTransform();
}