#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 transformedconslaw_main.cc
 * @ brief NPDE exam problem
 * @ author Oliver Rietmann
 * @ date 05.08.2021
 * @ copyright Developed at SAM, ETH Zurich
 */

#include <Eigen/Core>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <utility>

#include "transformedconslaw.h"

const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                       Eigen::DontAlignCols, ", ", "\n");

int main() {
  // Setup
  TRFCL::NonStdCauchyProblemCL prb;
  int M = 500;
  int N = 500;

  // Compute cell points and inital data for plot
  std::pair<double, double> limits = prb.domain();
  Eigen::VectorXd x =
      Eigen::VectorXd::LinSpaced(N, limits.first, limits.second);
  auto z0 = [&prb](double y) { return prb.z0(y); };
  Eigen::VectorXd zeta0 = x.unaryExpr(z0);

  // Print conserved quantity
  auto rho = [&prb](double z) { return prb.rho(z); };
  auto rec = [rho, limits](double t, const Eigen::VectorXd &zeta) -> void {
    Eigen::VectorXd weighted_samples = zeta.unaryExpr(rho) / (zeta.size() - 1);
    double rho_z_integral =
        weighted_samples.sum() * (limits.second - limits.first);
    std::cout << "Integral of rho(z(x,t)) at time t = " << t << ":\t"
              << rho_z_integral << std::endl;
  };

  // Compute solution at final time
  std::cout << std::fixed;
  Eigen::VectorXd zetaT = solveCauchyPrb(M, N, prb, rec);

  // Write x, zeta0 and zetaT to .csv file
  std::ofstream solution_file;
  solution_file.open(CURRENT_BINARY_DIR "/solution.csv");
  solution_file << x.transpose().format(CSVFormat) << std::endl;
  solution_file << zeta0.transpose().format(CSVFormat) << std::endl;
  solution_file << zetaT.transpose().format(CSVFormat) << std::endl;
  solution_file.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/solution.csv" << std::endl;

  // Plot from .csv file using plot.py
  std::system("python3 " CURRENT_SOURCE_DIR "/plot.py " CURRENT_BINARY_DIR
              "/solution.csv " CURRENT_BINARY_DIR "/solution.eps");

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// Authors: Ilgweon Kang and Lutong Wang
//          (respective Ph.D. advisors: Chung-Kuan Cheng, Andrew B. Kahng),
//          based on Dr. Jingwei Lu with ePlace and ePlace-MS
//
//          Many subsequent improvements were made by Mingyu Woo
//          leading up to the initial release.
//
// BSD 3-Clause License
//
// Copyright (c) 2018, The Regents of the University of California
// 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 copyright holder 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 HOLDER 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.
///////////////////////////////////////////////////////////////////////////////

#include <climits>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <string>
#include <ctime>

#include "global.h"
#include "initPlacement.h"
#include "plot.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/IterativeSolvers>

void initial_placement() {
  using namespace Eigen;
  printf("PROC:  Conjugate Gradient (CG) method to obtain the IP\n");

  int itol = 1;
  int itmax = 100;
  int x_iter = 0, y_iter = 0;

  prec tol = 0.000001;
  prec target_tol = 0.000001;
  prec x_err = 0, y_err = 0;

  double time_s = 0;

  HPWL_count();

  if(isSkipIP) {
    return;
  }

  printf("INFO:  The Initial HPWL is %.6lf\n", tot_HPWL);

  if(tot_HPWL <= 0) {
    printf("ERROR: HPWL <= 0, skip initial QP\n");
    return;
  }

  printf("INFO:  The Matrix Size is %d\n", moduleCNT);
  fflush(stdout);

  // malloc to solve PCG
  //
  // Ax = b
  //
  // x : variable vector to solve
  // b : constant vector
  //

  setNbThreads(numThread);

  // BCGSTAB settings
  SMatrix eMatX(moduleCNT, moduleCNT), eMatY(moduleCNT, moduleCNT);

  VectorXf xcg_x(moduleCNT), xcg_b(moduleCNT), ycg_x(moduleCNT),
      ycg_b(moduleCNT);

  for(int i = 0;; i++) {
    if(i >= numInitPlaceIter) {
      break;
    }

    time_start(&time_s);
    CreateSparseMatrix(xcg_x, xcg_b, ycg_x, ycg_b, eMatX, eMatY);

    BiCGSTAB< SMatrix, IdentityPreconditioner > solver;
    solver.setMaxIterations(itmax);

    solver.compute(eMatX);
    xcg_x = solver.solveWithGuess(xcg_b, xcg_x);
    x_err = solver.error();

    solver.compute(eMatY);
    ycg_x = solver.solveWithGuess(ycg_b, ycg_x);
    y_err = solver.error();

    update_module(xcg_x, ycg_x);
    update_pin_by_module();
    update_net_by_pin();
    HPWL_count();

    if(isPlot && i % 5 == 0) {
      SaveCellPlotAsJPEG(string("FIP - Iter: ") + to_string(i), false,
                         string(dir_bnd) + string("/initPlace/initPlacement_") +
                             intoFourDigit(i));
      // SavePlot( string("FIP - Iter: ") + to_string(i) );
    }

    time_end(&time_s);
    printf("INFO:  IP%3d,  CG Error %.6lf,  HPWL %.6lf,  CPUtime %.2lf\n", i,
           max(x_err, y_err), tot_HPWL, time_s);
    fflush(stdout);

    if(fabs(x_err) < target_tol && fabs(y_err) < target_tol && i > 4) {
      break;
    }
  }
}

void build_data_struct(bool initCoordi) {
  MODULE *mdp = NULL;
  TERM *term = NULL;
  PIN *pin = NULL;
  NET *curNet = NULL;
  FPOS pof;

  prec min_x = 0;
  prec min_y = 0;
  prec max_x = 0;
  prec max_y = 0;

  for(int i = 0; i < terminalCNT; i++) {
    term = &terminalInstance[i];

    for(int j = 0; j < term->pinCNTinObject; j++) {
      pin = term->pin[j];
      pin->tier = 0;
    }

  }

  for(int i = 0; i < moduleCNT; i++) {
    mdp = &moduleInstance[i];

    if(initCoordi) {
      mdp->center = place.center;
    }

    mdp->pmin.x = mdp->center.x - 0.5 * mdp->size.x;
    mdp->pmin.y = mdp->center.y - 0.5 * mdp->size.y;

    mdp->pmax.x = mdp->center.x + 0.5 * mdp->size.x;
    mdp->pmax.y = mdp->center.y + 0.5 * mdp->size.y;

    for(int j = 0; j < mdp->pinCNTinObject; j++) {
      pof = mdp->pof[j];
      pin = mdp->pin[j];

      pin->fp.x = mdp->center.x + pof.x;
      pin->fp.y = mdp->center.y + pof.y;

      pin->X_MIN = 0;
      pin->X_MAX = 0;
      pin->Y_MIN = 0;
      pin->Y_MAX = 0;
    }
  }

  term_pmin.x = PREC_MAX;
  term_pmin.x = PREC_MAX;
  term_pmax.x = 0;
  term_pmax.y = 0;

  for(int i = 0; i < terminalCNT; i++) {
    term = &terminalInstance[i];

    term->pmin.x = term->center.x - 0.5 * term->size.x;
    term->pmin.y = term->center.y - 0.5 * term->size.y;

    term->pmax.x = term->center.x + 0.5 * term->size.x;
    term->pmax.y = term->center.y + 0.5 * term->size.y;

    if(term_pmin.x > term->pmin.x)
      term_pmin.x = term->pmin.x;
    if(term_pmin.y > term->pmin.y)
      term_pmin.y = term->pmin.y;
    if(term_pmax.x < term->pmax.x)
      term_pmax.x = term->pmax.x;
    if(term_pmax.y < term->pmax.y)
      term_pmax.y = term->pmax.y;

    for(int j = 0; j < term->pinCNTinObject; j++) {
      pof = term->pof[j];
      pin = term->pin[j];

      pin->fp.x = term->center.x + pof.x;
      pin->fp.y = term->center.y + pof.y;

      pin->X_MIN = 0;
      pin->X_MAX = 0;
      pin->Y_MIN = 0;
      pin->Y_MAX = 0;
    }
  }

  for(int i = 0; i < netCNT; i++) {
    curNet = &netInstance[i];

    min_x = PREC_MAX;
    min_y = PREC_MAX;
    max_x = PREC_MIN;
    max_y = PREC_MIN;

    PIN *pin_xmin = NULL;
    PIN *pin_ymin = NULL;
    PIN *pin_xmax = NULL;
    PIN *pin_ymax = NULL;

    for(int j = 0; j < curNet->pinCNTinObject; j++) {
      pin = curNet->pin[j];
      if(pin_xmin) {
        if(min_x > pin->fp.x) {
          min_x = pin->fp.x;
          pin_xmin->X_MIN = 0;
          pin_xmin = pin;
          pin->X_MIN = 1;
        }
      }
      else {
        min_x = pin->fp.x;  // mdp->center.x ;
        pin_xmin = pin;
        pin->X_MIN = 1;
      }

      if(pin_ymin) {
        if(min_y > pin->fp.y)  // mdp->center.y)
        {
          min_y = pin->fp.y;  // mdp->center.y ;
          pin_ymin->Y_MIN = 0;
          pin_ymin = pin;
          pin->Y_MIN = 1;
        }
      }
      else {
        min_y = pin->fp.y;  // mdp->center.y ;
        pin_ymin = pin;
        pin->Y_MIN = 1;
      }

      if(pin_xmax) {
        if(max_x < pin->fp.x)  // mdp->center.x)
        {
          max_x = pin->fp.x;  // mdp->center.x ;
          pin_xmax->X_MAX = 0;
          pin_xmax = pin;
          pin->X_MAX = 1;
        }
      }
      else {
        max_x = pin->fp.x;  // mdp->center.x ;
        pin_xmax = pin;
        pin->X_MAX = 1;
      }

      if(pin_ymax) {
        if(max_y < pin->fp.y)  // mdp->center.y)
        {
          max_y = pin->fp.y;  // mdp->center.y ;
          pin_ymax->Y_MAX = 0;
          pin_ymax = pin;
          pin->Y_MAX = 1;
        }
      }
      else {
        max_y = pin->fp.y;  // mdp->center.y ;
        pin_ymax = pin;
        pin->Y_MAX = 1;
      }
    }

    curNet->min_x = min_x;
    curNet->min_y = min_y;

    curNet->max_x = max_x;
    curNet->max_y = max_y;
  }
}

void update_module(VectorXf &xcg_x, VectorXf &ycg_x) {
  MODULE *mdp = NULL;
  for(int i = 0; i < moduleCNT; i++) {
    mdp = &moduleInstance[i];

    mdp->center.x = xcg_x(i);
    mdp->center.y = ycg_x(i);

    if((mdp->center.x + 0.5 * mdp->size.x) > place.end.x)
      mdp->center.x = place.end.x - 0.5 * mdp->size.x - Epsilon;

    if((mdp->center.y + 0.5 * mdp->size.y) > place.end.y)
      mdp->center.y = place.end.y - 0.5 * mdp->size.y - Epsilon;

    if((mdp->center.x - 0.5 * mdp->size.x) < place.org.x)
      mdp->center.x = place.org.x + 0.5 * mdp->size.x + Epsilon;

    if((mdp->center.y - 0.5 * mdp->size.y) < place.org.y)
      mdp->center.y = place.org.y + 0.5 * mdp->size.y + Epsilon;

    mdp->pmin.x = mdp->center.x - 0.5 * mdp->size.x;
    mdp->pmin.y = mdp->center.y - 0.5 * mdp->size.y;

    mdp->pmax.x = mdp->center.x + 0.5 * mdp->size.x;
    mdp->pmax.y = mdp->center.y + 0.5 * mdp->size.y;
  }
}

void update_pin_by_module(void) {
  FPOS pof;
  PIN *pin = NULL;
  MODULE *mdp = NULL;

  for(int i = 0; i < moduleCNT; i++) {
    mdp = &moduleInstance[i];
    for(int j = 0; j < mdp->pinCNTinObject; j++) {
      pof = mdp->pof[j];
      pin = mdp->pin[j];

      if(pin->moduleID != i || pin->pinIDinModule != j || pin->term == 1) {
        exit(1);
      }

      pin->fp.x = mdp->center.x + pof.x;
      pin->fp.y = mdp->center.y + pof.y;

      pin->X_MIN = 0;
      pin->X_MAX = 0;
      pin->Y_MIN = 0;
      pin->Y_MAX = 0;
    }
  }
}

void update_net_by_pin() {
  for(int i = 0; i < netCNT; i++) {
    NET *curNet = &netInstance[i];

    PIN *pin_xmin = NULL, *pin_ymin = NULL;
    PIN *pin_xmax = NULL, *pin_ymax = NULL;

    prec min_x = PREC_MAX, min_y = PREC_MAX;
    prec max_x = PREC_MIN, max_y = PREC_MIN;

    for(int j = 0; j < curNet->pinCNTinObject; j++) {
      PIN *pin = curNet->pin[j];

      if(pin_xmin) {
        if(min_x > pin->fp.x)  // mdp->center.x)
        {
          min_x = pin->fp.x;  // mdp->center.x ;
          pin_xmin->X_MIN = 0;
          pin_xmin = pin;
          pin->X_MIN = 1;
        }
      }
      else {
        min_x = pin->fp.x;  // mdp->center.x ;
        pin_xmin = pin;
        pin->X_MIN = 1;
      }

      if(pin_ymin) {
        if(min_y > pin->fp.y)  // mdp->center.y)
        {
          min_y = pin->fp.y;  // mdp->center.y ;
          pin_ymin->Y_MIN = 0;
          pin_ymin = pin;
          pin->Y_MIN = 1;
        }
      }
      else {
        min_y = pin->fp.y;  // mdp->center.y ;
        pin_ymin = pin;
        pin->Y_MIN = 1;
      }

      if(pin_xmax) {
        if(max_x < pin->fp.x)  // mdp->center.x)
        {
          max_x = pin->fp.x;  // mdp->center.x ;
          pin_xmax->X_MAX = 0;
          pin_xmax = pin;
          pin->X_MAX = 1;
        }
      }
      else {
        max_x = pin->fp.x;  // mdp->center.x ;
        pin_xmax = pin;
        pin->X_MAX = 1;
      }

      if(pin_ymax) {
        if(max_y < pin->fp.y)  // mdp->center.y)
        {
          max_y = pin->fp.y;  // mdp->center.y ;
          pin_ymax->Y_MAX = 0;
          pin_ymax = pin;
          pin->Y_MAX = 1;
        }
      }
      else {
        max_y = pin->fp.y;  // mdp->center.y ;
        pin_ymax = pin;
        pin->Y_MAX = 1;
      }
    }
    curNet->min_x = min_x;
    curNet->min_y = min_y;
    curNet->max_x = max_x;
    curNet->max_y = max_y;
  }
}

//
// CreateSparseMatrix Routine
//
// using current Pin's structure,
// based on the B2B models,
// it genereates Sparsematrix into Eigen formats.
//
void CreateSparseMatrix(VectorXf &xcg_x, VectorXf &xcg_b, VectorXf &ycg_x,
                        VectorXf &ycg_b, SMatrix &eMatX, SMatrix &eMatY) {
  int pinCNTinObject = 0;
  int moduleID1 = 0;
  int moduleID2 = 0;
  int is_term1 = 0;
  int is_term2 = 0;

  prec common1;
  prec common2;

  NET *tempNet = NULL;
  PIN *pin1 = NULL;
  PIN *pin2 = NULL;

  MODULE *mdp1 = NULL;
  MODULE *mdp2 = NULL;
  TERM *term1 = NULL;
  TERM *term2 = NULL;

  FPOS center1, center2;
  FPOS fp1, fp2;

  // to easily convert (i, j, value) -> CSR sparse Matrix.
  // using Eigen library..
  vector< T > tripletListX, tripletListY;
  tripletListX.reserve(10000000);
  tripletListY.reserve(10000000);

  // xcg_x & ycg_x update
  MODULE *curModule = NULL;
  for(int i = 0; i < moduleCNT; i++) {
    curModule = &moduleInstance[i];

    // 1d prec array
    xcg_x(i) = curModule->center.x;
    ycg_x(i) = curModule->center.y;

    xcg_b(i) = ycg_b(i) = 0;
  }

  for(int i = 0; i < netCNT; i++) {
    tempNet = &netInstance[i];
    pinCNTinObject = tempNet->pinCNTinObject;
    common1 = 1.0 / ((prec)pinCNTinObject - 1.0);

    for(int j = 0; j < pinCNTinObject; j++) {
      pin1 = tempNet->pin[j];
      moduleID1 = pin1->moduleID;
      fp1 = pin1->fp;

      // if is not Terminal -> moduleInstance
      if(!pin1->term) {
        mdp1 = &moduleInstance[moduleID1];
        center1 = mdp1->center;
        is_term1 = 0;
      }
      // is terminal -> terminalInstance
      else {
        term1 = &terminalInstance[moduleID1];
        center1 = term1->center;
        is_term1 = 1;
      }

      for(int k = j + 1; k < pinCNTinObject; k++) {
        pin2 = tempNet->pin[k];
        moduleID2 = pin2->moduleID;
        fp2 = pin2->fp;

        if(!pin2->term) {
          mdp2 = &moduleInstance[moduleID2];
          center2 = mdp2->center;
          is_term2 = 0;
        }
        else {
          term2 = &terminalInstance[moduleID2];
          center2 = term2->center;
          is_term2 = 1;
        }

        // there is no need to calculate (for same nodes)
        if(moduleID1 == moduleID2 && is_term1 == is_term2) {
          continue;
        }

        if(pin1->X_MIN || pin1->X_MAX || pin2->X_MIN || pin2->X_MAX) {
          prec len_x = fabs(fp1.x - fp2.x);

          prec wt_x = 0.0f;
          if(dge(len_x, MIN_LEN)) {
            wt_x = common1 / len_x;
          }
          else {
            wt_x = common1 / MIN_LEN;
          }

          common2 = (-1.0) * wt_x;

          if(wt_x < 0)
            printf("ERROR WEIGHT\n");

          // both is module
          if(!is_term1 && !is_term2) {
            tripletListX.push_back(T(moduleID1, moduleID1, wt_x));
            tripletListX.push_back(T(moduleID2, moduleID2, wt_x));
            tripletListX.push_back(T(moduleID1, moduleID2, common2));
            tripletListX.push_back(T(moduleID2, moduleID1, common2));

            xcg_b(moduleID1) +=
                common2 * ((fp1.x - center1.x) - (fp2.x - center2.x));
            xcg_b(moduleID2) +=
                common2 * ((fp2.x - center2.x) - (fp1.x - center1.x));
          }
          // 1 is terminal, 2 is module
          else if(is_term1 && !is_term2) {
            tripletListX.push_back(T(moduleID2, moduleID2, wt_x));
            xcg_b(moduleID2) += wt_x * (fp1.x - (fp2.x - center2.x));
          }
          // 2 is terminal, 1 is module
          else if(!is_term1 && is_term2) {
            tripletListX.push_back(T(moduleID1, moduleID1, wt_x));
            xcg_b(moduleID1) += wt_x * (fp2.x - (fp1.x - center1.x));
          }
        }

        if(pin1->Y_MIN || pin1->Y_MAX || pin2->Y_MIN || pin2->Y_MAX) {
          prec len_y = fabs(fp1.y - fp2.y);

          prec wt_y = 0.0f;
          if(dge(len_y, MIN_LEN)) {
            wt_y = common1 / len_y;
          }
          else {
            wt_y = common1 / MIN_LEN;
          }
          common2 = (-1.0) * wt_y;

          // both is module
          if(!is_term1 && !is_term2) {
            tripletListY.push_back(T(moduleID1, moduleID1, wt_y));
            tripletListY.push_back(T(moduleID2, moduleID2, wt_y));
            tripletListY.push_back(T(moduleID1, moduleID2, common2));
            tripletListY.push_back(T(moduleID2, moduleID1, common2));

            ycg_b(moduleID1) +=
                common2 * ((fp1.y - center1.y) - (fp2.y - center2.y));
            ycg_b(moduleID2) +=
                common2 * ((fp2.y - center2.y) - (fp1.y - center1.y));
          }
          // 1 is terminal, 2 is module
          else if(is_term1 && !is_term2) {
            tripletListY.push_back(T(moduleID2, moduleID2, wt_y));
            ycg_b(moduleID2) += wt_y * (fp1.y - (fp2.y - center2.y));
          }
          // 2 is terminal, 1 is module
          else if(!is_term1 && is_term2) {
            tripletListY.push_back(T(moduleID1, moduleID1, wt_y));
            ycg_b(moduleID1) += wt_y * (fp2.y - (fp1.y - center1.y));
          }
        }
      }
    }
  }

  eMatX.setFromTriplets(tripletListX.begin(), tripletListX.end());
  eMatY.setFromTriplets(tripletListY.begin(), tripletListY.end());
}


#include "dSFMTsearch.hpp"
#include "Annihilate.h"
#include <errno.h>
#include <stdlib.h>
#include <getopt.h>
#include <errno.h>
#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <MTToolBox/period.hpp>
#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>
#include "calc_fixpoint.h"

using namespace MTToolBox;
using namespace std;

w128_t calc_fixpoint(const dSFMT& dsfmt, const GF2X& irreducible,
                     const GF2X& quotient)
{
    GF2X a, b, d;
    dSFMT dsfmt_const(dsfmt);
    /* a*irreducible + b*quotient = d */
    XGCD(d, a, b, irreducible, quotient);
    if (deg(d) != 0) {
        cout << "failure d != 1" << endl;
        throw new logic_error("failure d != 1");
    }
    b *= quotient;
    a *= irreducible;
    dsfmt_const.setConst();
    annihilate<w128_t>(&dsfmt_const, b);

    GF2X t1(1, 1);
    SetCoeff(t1, 0);
    /* a*irreducible + b*t1 = d */
    XGCD(d, a, b, irreducible, t1);
    if (deg(d) != 0) {
        cout << "failure d != 1" << endl;
        cout << "deg(d) = " << dec << deg(d) << endl;
        cout << "deg(irreducible) = " << dec << deg(irreducible) << endl;
        cout << "deg(t1) = " << dec << deg(t1) << endl;
        throw new logic_error("failure d != 1");
    }
    annihilate<w128_t>(&dsfmt_const, b);
    return dsfmt_const.getParityValue();
}


#include <iostream>
#include <cmath>
#include <random>
#include <chrono>
#include <dlib/matrix.h>

using namespace std;
using namespace dlib;

/*
int main()
{
    matrix<double,32,32> input;
    matrix<double,28,28> output;
    matrix<double,5,5> filter;

    double acc;

    input = 1;
    filter = 0.1;

    for (int i=0; i<28; i++)
    {
        for (int j=0; j<28; j++)
        {
            acc = 0;
            for (int r=0; r<5;  r++)
            {
                for (int k=0; k<5; k++)
                {
                    acc += input(i+r,j+k) * filter(r,k);
                }
            }
            output(i,j) = acc/25;
        }
    }
    cout << input << endl;
    cout << output << endl;
}
*/

int main()
{
    std::default_random_engine generator;
    std::uniform_real_distribution<double> distribution(-1.0,1.0);

    matrix<double,1,84> input;
    matrix<double,84,10> syn, tmp_syn, tmp;
    matrix<double,1,10> output, y, a, e, g, delta;

    for (int i=0; i<84; i++)
        for (int j=0; j<10; j++)
            syn(i,j) = distribution(generator);

    output(0) = 0;
    output(1) = 1;
    output(2) = 0;
    output(3) = -1;
    output(4) = 0;
    output(5) = 1;
    output(6) = 0;
    output(7) = -1;
    output(8) = 0;
    output(9) = 1;

    input = 1;

    // Learning
    for (int i=0; i<100000; i++)
    {
        y = input * syn;
        a = 1/(1+exp(-y));
        e = output - a;
        g = pointwise_multiply(a,(1 - a));
        delta = pointwise_multiply(g, e);
        //tmp = syn;
        //tmp_syn = trans(input) * delta;
        //syn = tmp + tmp_syn;
        syn += trans(input) * delta;
    }
    cout << syn << endl;
}