#include <fstream>
#include <string>

#include <polycrypto/PolyCrypto.h>

#include <xutils/Utils.h>
#include <xutils/Log.h>

#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;
using namespace std;
using namespace libpolycrypto;

/**
 * How do we reason about bandwidth in (t, n) DKG? (i.e., need t shares to reconstruct)
 *  - c   = the size of a poly commitment
 *  - s   = the size of a share
 *  - p   = the size of a proof for a share
 *  - f0c = the size of a Feldman commitment g^f(0) to f(0)
 *  - f0p = the size of the proof for g^f(0) (includes NIZKPoK for Kate et al)
 */

/**
 *  DOWNLOAD
 *  --------
 *  Each player i\in [n] needs to obtain from every other player j \ne i: a poly commitment + share + proof + f(0) comm + f(0) proof
 *   - per-player: (n-1)*(c + s + p + f0c + f0p) commitments + shares + proofs need to be downloaded
 *   - total: n * per-player
 */
template<class T>
T calcDownload(T n, T c, T s, T p, T f0c, T f0p) {
    T perPlayer = (n-1)*(c + s + p + f0c + f0p);

    return perPlayer;
}

/**
 *  UPLOAD
 *  ------
 *  Each player i needs to broadcast their poly commitment + f(0) comm + f(0) proof (same in eJF-DKG and in AMT DKG)
 *  Each player i sends n-1 shares to the n-1 other players (same in eJF-DKG and in AMT DKG)
 *  Each player i publishes the AMT (O(n) best case) or sends proofs individually (O(n log t) worst case)
 *  Assume n = 2^k for some k
 *
 *   - Best case per-player:  (c + f0c + f0p) + (n-1)*s + (2n-1)*c (for the AMT quotient commitments)
 *      - In eJF-DKG, last sum term would have been (n-1)*c (so we are only a bit more expensive)
 *
 *   - Worst case per-player: (c + f0c + f0p) + (n-1)*(s + (log(t-1) + 1)c)
 */
template<class T>
T calcUpload_WorstCase(T n, T c, T s, T p, T f0c, T f0p) {
    T perPlayer = c + f0c + f0p + (n-1)*(s + p);

    return perPlayer;
}

template<class T>
T calcUpload(T n, T c, T s, T p, T f0c, T f0p) {
    return calcUpload_WorstCase(n, c, s, p, f0c, f0p);
}

// NOTE: t is the reconstruction threshold so the poly degrees are t-1
template<class T>
std::tuple<T,T> feldman(T n, T t) {
    const T polyDeg = (t - 1);
    const T commSize = 32 * (polyDeg + 1); // need to send a group element for each coeff of the poly
    const T shareSize = 32;        // share is just a field element
    const T proofSize = 0;         // can verify share against commitment directly (not true for Pedersen though: needs 32-byte r(i))
    const T f0commSize = 0;        // g^f(0) is already part of the Feldman commitment to the first coeff c_0 = f(0)
    const T f0proofSize = 0;       // can verify f(0) directly against comm

    return std::make_tuple(
        calcDownload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize),
        calcUpload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize));
}

template<class T>
std::tuple<T,T> kate(T n) {
    const T commSize = 32;          // g^p(s) is one group element
    const T shareSize = 32;         // share is a field element
    const T proofSize = 32;         // proof is g^{p(s) - p(i) / (x - i)}: one group element
    const T f0commSize = 32;        // one group element
    // a Schnorr signature (e = H(g || g^k || g^x || proverID || bla), s = k + ex)
    // Reference: https://tools.ietf.org/html/rfc8235#page-8
    const T nizkPok = 64;           // basically a Schnorr signature
    const T f0proofSize = 32 + nizkPok; // g^f(0) proof: normal Kate proof + a NIZKPoK of f(0) w.r.t. g

    return std::make_tuple(
        calcDownload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize),
        calcUpload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize));
}

/**
 * This is the bandwidth if we send a constant-sized proof for g^f(0) and a log n-sized proof for f(i)
 */
template<class T>
std::tuple<T,T> amt(T n, T t) {
    // a tree of n = 2^i nodes has i + 1 nodes along any path and that's how many quotient commitments will be in our proof
    // when n is not a power of two, we round up using log2ceil
    const T commSize = 32;
    const T shareSize = 32;
    // we are only evaluating at n points this time, and proving g^f(0) using a normal Kate proof
    //const T numLevels = Utils::log2ceil(n) + 1;
    const T proofSize = (Utils::log2floor(t-1) + 1) * 32;
    const T f0commSize = 32;
    const T nizkPok = 64;           // see kate() description
    const T f0proofSize = 32 + nizkPok;    // proof for g^f(0) is now a normal constant-sized Kate proof (+ NIZKPoK)

    //loginfo << " * numLevels = " << numLevels << " (for n = " << n << ")" << endl;

    return std::make_tuple(
        calcDownload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize),
        calcUpload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize));
}

int main(int argc, char *argv[]) {
    libpolycrypto::initialize(nullptr, 0);

    if(argc < 3) {
        cout << "Usage: " << argv[0] << " <out-file> <max-f>" << endl;
        cout << endl;
        cout << "Estimates bandwidth for Feldman, Kate and AMT DKGs" << endl;
        cout << endl;
        cout << " <out-file>     writes numbers in this file" << endl;
        cout << " <max-f>        computes numbers for (k=f+1, n=2f+1) thresholds for f up to <max-f>" << endl;
        cout << endl;
        return 1;
    }

    string outFile(argv[1]);
    size_t max_f = static_cast<size_t>(std::stoi(argv[2]));

    ofstream fout(outFile);

    if(fout.fail()) {
        throw std::runtime_error("Could not write to output file");
    }

    using NumBytesType = int128_t;
    loginfo << "sizeof(NumBytesType): " << sizeof(NumBytesType) << endl;

    fout << "t,n,dkg,download_bw_bytes,download_bw_hum,upload_bw_bytes,upload_bw_hum,comm_bw_bytes,comm_bw_hum,total_bw_bytes,total_bw_hum" << endl;

    for(size_t p = 2; p <= max_f + 1; p *= 2) {
        size_t f = p - 1;
        size_t t = f + 1;
        size_t n = 2*f + 1;
        loginfo << t << " out of " << n  << " DKG" << endl;

        auto writeRow = [&fout, &n, &t](std::tuple<NumBytesType, NumBytesType> bytes, const char * scheme) {
            NumBytesType perPlayerDown = std::get<0>(bytes);
            NumBytesType perPlayerUp = std::get<1>(bytes);
            fout
                 << t << ","
                 << n << ","
                 << scheme << ","
                 << perPlayerDown << ","
                 << Utils::humanizeBytes(perPlayerDown) << ","
                 << perPlayerUp << ","
                 << Utils::humanizeBytes(perPlayerUp) << ","
                 << perPlayerDown + perPlayerUp << ","
                 << Utils::humanizeBytes(perPlayerDown + perPlayerUp) << ","
                 << n*(perPlayerDown + perPlayerUp) << ","
                 << Utils::humanizeBytes(n*(perPlayerDown + perPlayerUp))
                 << endl;
        };

        writeRow(feldman(n, t), "JF-DKG");
        writeRow(kate(n), "eJF-DKG");
        writeRow(amt(n, t), "AMT DKG");
    }

    loginfo << "All done!" << endl;

    return 0;
}


#include "conex/cone_program.h"
#include "conex/equality_constraint.h"
#include "conex/linear_constraint.h"
#include "gtest/gtest.h"
#include <Eigen/Dense>

namespace conex {

using Eigen::MatrixXd;
using Eigen::VectorXd;
GTEST_TEST(EqualityConstraints, Basic) {
  int num_vars = 3;
  int num_equalities = 1;
  int num_inequalities = 4;

  MatrixXd A = MatrixXd::Random(num_inequalities, num_vars);
  MatrixXd C(num_inequalities, 1);
  MatrixXd b(num_vars, 1);

  MatrixXd optimal_slack(num_inequalities, 1);
  MatrixXd optimal_dual(num_inequalities, 1);
  MatrixXd optimal_y(num_vars, 1);

  optimal_slack.setConstant(1);
  optimal_dual.setConstant(1);
  optimal_slack.topRows(num_inequalities * .5).setZero();
  optimal_dual.bottomRows(num_inequalities - num_inequalities * .5).setZero();
  optimal_y = Eigen::MatrixXd::Random(num_vars, 1);

  C = optimal_slack + A * optimal_y;

  LinearConstraint linear_inequality{A, C};

  MatrixXd eq = MatrixXd::Random(num_equalities, num_vars);
  MatrixXd eq_affine(num_equalities, 1);
  eq = Eigen::MatrixXd::Random(num_equalities, num_vars);
  eq_affine = eq * optimal_y;

  Program prog(num_vars);
  prog.AddConstraint(EqualityConstraints{eq, eq_affine});
  prog.AddConstraint(linear_inequality);

  VectorXd linear_cost(num_vars);
  linear_cost = A.transpose() * optimal_dual;

  VectorXd solution(num_vars);
  Solve(linear_cost, prog, conex::SolverConfiguration(), solution.data());

  EXPECT_NEAR((eq * solution - eq_affine).norm(), 0, 1e-5);
  EXPECT_NEAR((solution - optimal_y).norm(), 0, 1e-5);
}

void DoManySeparate(bool separate) {
  int num_vars = 10;
  int num_inequalities = num_vars + 10;
  int num_equalities = num_vars - 2;

  MatrixXd A = MatrixXd::Random(num_inequalities, num_vars);
  MatrixXd C(num_inequalities, 1);

  VectorXd optimal_slack(num_inequalities);
  VectorXd optimal_dual(num_inequalities);
  VectorXd optimal_y(num_vars);

  optimal_slack.setConstant(1);
  optimal_dual.setConstant(1);
  int m = num_inequalities * .5;
  optimal_slack.topRows(m).setConstant(1e-7);
  optimal_dual.bottomRows(num_inequalities - m).setConstant(1e-7);

  optimal_y = Eigen::MatrixXd::Random(num_vars, 1);

  C = optimal_slack + A * optimal_y;

  LinearConstraint linear_inequality{A, C};

  Program prog(num_vars);
  prog.AddConstraint(linear_inequality);

  MatrixXd eq = MatrixXd::Zero(num_equalities, num_vars);
  Eigen::MatrixXd Bi(1, 3);
  Bi << 1, 2, 3;
  for (int i = 0; i < num_equalities; i++) {
    std::vector<int> vars{0, i + 1, num_vars - 1};
    for (size_t j = 0; j < vars.size(); j++) {
      eq(i, vars.at(j)) = Bi(0, j);
    }
    if (separate) {
      prog.AddConstraint(EqualityConstraints{Bi, eq.row(i) * optimal_y}, vars);
    }
  }

  MatrixXd eq_affine(num_equalities, 1);
  eq_affine = eq * optimal_y;

  if (!separate) {
    prog.AddConstraint(EqualityConstraints{eq, eq_affine});
  }

  VectorXd linear_cost(num_vars);
  linear_cost = A.transpose() * optimal_dual;

  VectorXd solution(num_vars);
  auto config = conex::SolverConfiguration();
  config.final_centering_steps = 10;

  config.initial_centering_steps_coldstart = 0;
  config.max_iterations = 40;
  config.divergence_upper_bound = .5;
  Solve(linear_cost, prog, config, solution.data());

  EXPECT_TRUE((C - A * solution).minCoeff() > -1e8);
  EXPECT_NEAR((eq * solution - eq_affine).norm(), 0, 5e-7);
  EXPECT_GE(linear_cost.dot(solution) + 1e-4, linear_cost.dot(optimal_y));
}

GTEST_TEST(EqualityConstraints, ManyConstraints) {
  for (int i = 0; i < 10; i++) {
    srand(i);
    DoManySeparate(false /*do not split constraints*/);
  }
}

GTEST_TEST(EqualityConstraints, ManySeparateConstraints) {
  for (int i = 0; i < 10; i++) {
    srand(i);
    DoManySeparate(true /* split constraints*/);
  }
}

}  // namespace conex


#include "Utilities/Memory/HashTable.h"

#include <cstdint>
#include <boost/test/unit_test.hpp>

using Utilities::Memory::to_int;

template <typename T>
T wrap_around(const T x)
{
  return x + (1 << (sizeof(T) - 1));
}

BOOST_AUTO_TEST_SUITE(Utilities)
BOOST_AUTO_TEST_SUITE(Memory)

BOOST_AUTO_TEST_SUITE(HashTable_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ToIntIntermediarySteps)
{
  {
    int16_t result {static_cast<int16_t>(-65537)};
    result = result + (1 << (sizeof(int16_t) - 1));

    BOOST_TEST(result == 1);

    BOOST_TEST(wrap_around<int16_t>(static_cast<int16_t>(-65536)) == 2);
    BOOST_TEST(wrap_around<int16_t>(static_cast<int16_t>(-65535)) == 3);
  }
  {
    int x {-69};
    int* int_ptr {&x};

    BOOST_TEST(to_int(int_ptr) == -8);

    x = -42;
    BOOST_TEST(to_int(int_ptr) == -5);

    x = 42;
    BOOST_TEST(to_int(int_ptr) == 5);

    x = 69;
    BOOST_TEST(to_int(int_ptr) == 8);

    x = 9;
    BOOST_TEST(to_int(int_ptr) == 1);
    x = 8;
    BOOST_TEST(to_int(int_ptr) == 1);
    x = 7;
    BOOST_TEST(to_int(int_ptr) == 0);
    x = 6;
    BOOST_TEST(to_int(int_ptr) == 0);
  }
}

BOOST_AUTO_TEST_SUITE_END() // HashTable_tests
BOOST_AUTO_TEST_SUITE_END() // Memory
BOOST_AUTO_TEST_SUITE_END() // Utilities


// -----------------------------------------------------------------------
//
// Copyright (C) 2020  - David Fernández Castellanos
//
// This file is part of the MEPLS software. You can use it, redistribute
// it, and/or modify it under the terms of the Creative Commons Attribution
// 4.0 International Public License. The full text of the license can be
// found in the file LICENSE at the top level of the MEPLS distribution.
//
// -----------------------------------------------------------------------

#include <example.h>
#include <mepls/utils.h>
#include <mepls/solver.h>
#include <mepls/system.h>
#include <mepls/event.h>
#include <mepls/history.h>
#include <mepls/dynamics.h>
#include <cmdparser.hpp>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/path_search.h>
#include <boost/property_tree/json_parser.hpp>

// new headers
#include <omp.h>
#include <deal.II/base/conditional_ostream.h>

struct Parameters
{
   unsigned int seed = 1234567;
   unsigned int n_rep = 1;
   unsigned int Nx = 32;
   unsigned int Ny = 32;
   double G = 30.;
   double nu = 0.3;
   double gamma = 0.05;
   double k = 6.;
   double strain_limit_aqs = 0.05;
   double time_limit_creep = 1e5;
   double ext_stress_creep = 0.;
   double lambda = 1.;
   double temperature = 1.;
   bool verbose = true;

   void declare_entries(dealii::ParameterHandler &prm)
   {
      prm.enter_subsection("Section1");

      prm.declare_entry("seed", mepls::utils::str::to_string(seed), dealii::Patterns::Integer(0), "");
      prm.declare_entry("n_rep", mepls::utils::str::to_string(n_rep), dealii::Patterns::Integer(0), "");
      prm.declare_entry("Nx", mepls::utils::str::to_string(Nx), dealii::Patterns::Integer(0), "");
      prm.declare_entry("Ny", mepls::utils::str::to_string(Ny), dealii::Patterns::Integer(0), "");
      prm.declare_entry("G", mepls::utils::str::to_string(G), dealii::Patterns::Double(0.0), "");
      prm.declare_entry("nu", mepls::utils::str::to_string(nu), dealii::Patterns::Double(0.0), "");
      prm.declare_entry("gamma", mepls::utils::str::to_string(gamma), dealii::Patterns::Double(0.0), "");
      prm.declare_entry("strain_limit_aqs", mepls::utils::str::to_string(strain_limit_aqs), dealii::Patterns::Double(0.0), "");
      prm.declare_entry("time_limit_creep", mepls::utils::str::to_string(time_limit_creep), dealii::Patterns::Double(0.0), "");
      prm.declare_entry("ext_stress_creep", mepls::utils::str::to_string(ext_stress_creep), dealii::Patterns::Double(0.0), "");
      prm.declare_entry("temperature", mepls::utils::str::to_string(temperature), dealii::Patterns::Double(0.0), "");
      prm.declare_entry("lambda", mepls::utils::str::to_string(lambda), dealii::Patterns::Double(0.0), "");
      prm.declare_entry("k", mepls::utils::str::to_string(k), dealii::Patterns::Double(0.0), "");
	  prm.declare_entry("verbose", mepls::utils::str::to_string(verbose), dealii::Patterns::Bool(), "");

      prm.leave_subsection();
   }

   void load_entries(dealii::ParameterHandler &prm)
   {
      prm.enter_subsection("Section1");

      seed = prm.get_integer("seed");
      n_rep = prm.get_integer("n_rep");
      Nx = prm.get_integer("Nx");
      Ny = prm.get_integer("Ny");
      G = prm.get_double("G");
      nu = prm.get_double("nu");
      gamma = prm.get_double("gamma");
      k = prm.get_double("k");
      strain_limit_aqs = prm.get_double("strain_limit_aqs");
      time_limit_creep = prm.get_double("time_limit_creep");
      ext_stress_creep = prm.get_double("ext_stress_creep");
      temperature = prm.get_double("temperature");
      lambda = prm.get_double("lambda");
      verbose = prm.get_bool("verbose");

      prm.leave_subsection();
   }

   void load_file(const std::string &filename)
   {
      dealii::ParameterHandler prm;
      declare_entries(prm);
      prm.parse_input(filename);
      load_entries(prm);
   }

   void generate_file(const std::string &filename)
   {
      std::ofstream outfile(filename);
      dealii::ParameterHandler prm;
      declare_entries(prm);
      prm.print_parameters(outfile, dealii::ParameterHandler::OutputStyle::Text);
   }

};

template<int dim>
void write_data(const mepls::history::History<dim> &creep_history,
                const mepls::history::History<dim> &aqs_history,
                const Parameters &p)
{
   boost::property_tree::ptree data_tree;

   data_tree.put("Name", "Step5");
   data_tree.put("Description", "System undergoing creep deformation, and then driven in "
								"athermal quasistatic shear");

   data_tree.put("Parameters.dim", 2);
   data_tree.put("Parameters.seed", p.seed);
   data_tree.put("Parameters.Nx", p.Nx);
   data_tree.put("Parameters.Ny", p.Ny);
   data_tree.put("Parameters.G", p.G);
   data_tree.put("Parameters.nu", p.nu);
   data_tree.put("Parameters.gamma", p.gamma);
   data_tree.put("Parameters.lambda", p.lambda);
   data_tree.put("Parameters.temperature", p.temperature);
   data_tree.put("Parameters.k", p.k);
   data_tree.put("Parameters.ext_stress_creep", p.ext_stress_creep);
   data_tree.put("Parameters.time_limit_creep", p.time_limit_creep);
   data_tree.put("Parameters.strain_limit_aqs", p.strain_limit_aqs);

   	// -------- creep history ----------
	{
	   std::ostringstream plastic_events_csv;
	   plastic_events_csv << "index,element,eigenstrain_00,eigenstrain_11,eigenstrain_01\n";
	   for(auto &row : creep_history.plastic)
		  plastic_events_csv << row.index << "," << row.element << "," << row.eigenstrain_00 << ","
							 << row.eigenstrain_11 << "," << row.eigenstrain_01 << "\n";

	   data_tree.put("Data.creep.plastic_events", plastic_events_csv.str());

	   std::ostringstream driving_events_csv;
	   driving_events_csv << "index,dtime,dext_stress,dtotal_strain\n";
	   for(auto &row : creep_history.driving)
		  driving_events_csv << row.index << "," << row.dtime << ","
							 << row.dext_stress << "," << row.dtotal_strain << "\n";

	   data_tree.put("Data.creep.driving_events", driving_events_csv.str());

	   std::ostringstream macro_evolution_csv;
	   macro_evolution_csv << "index,ext_stress,total_strain,time,av_vm_stress,av_vm_plastic_strain\n";
	   for(auto &row : creep_history.macro_evolution)
		  macro_evolution_csv << row.index << "," << row.ext_stress << ","
							 << row.total_strain << "," << row.time << ","
							 << row.av_vm_stress << "," << row.av_vm_plastic_strain << "\n";

	   data_tree.put("Data.creep.macro_evolution", macro_evolution_csv.str());
	}

	// -------- aqs history ----------
	{
	   std::ostringstream plastic_events_csv;
	   plastic_events_csv << "index,element,eigenstrain_00,eigenstrain_11,eigenstrain_01\n";
	   for(auto &row : aqs_history.plastic)
		  plastic_events_csv << row.index << "," << row.element << "," << row.eigenstrain_00 << ","
							 << row.eigenstrain_11 << "," << row.eigenstrain_01 << "\n";

	   data_tree.put("Data.AQS.plastic_events", plastic_events_csv.str());

	   std::ostringstream driving_events_csv;
	   driving_events_csv << "index,dtime,dext_stress,dtotal_strain\n";
	   for(auto &row : aqs_history.driving)
		  driving_events_csv << row.index << "," << row.dtime << ","
							 << row.dext_stress << "," << row.dtotal_strain << "\n";

	   data_tree.put("Data.AQS.driving_events", driving_events_csv.str());

	   std::ostringstream macro_evolution_csv;
	   macro_evolution_csv << "index,ext_stress,total_strain,time,av_vm_stress,av_vm_plastic_strain\n";
	   for(auto &row : aqs_history.macro_evolution)
		  macro_evolution_csv << row.index << "," << row.ext_stress << ","
							 << row.total_strain << "," << row.time << ","
							 << row.av_vm_stress << "," << row.av_vm_plastic_strain << "\n";

	   data_tree.put("Data.AQS.macro_evolution", macro_evolution_csv.str());
	}

	// create a descriptive filename
   	std::ostringstream filename;

	filename << "Nx_" << p.Nx
	         << "+gamma_" << std::fixed << std::setprecision(2) << p.gamma
	         << "+lambda_" << p.lambda
	         << "+k_" << p.k
		     << "+G_" << p.G
		     << "+T_" << p.temperature
		     << "+ext_stress_" << p.ext_stress_creep
			 << "+seed_" << p.seed
			 << ".json";

   std::ofstream output_file( filename.str() );
   boost::property_tree::json_parser::write_json(output_file, data_tree);
   output_file.close();
}

void run(const Parameters &p, dealii::ConditionalOStream & cout)
{
   //----- SET UP ------

   constexpr unsigned dim = 2;
   std::mt19937 generator(p.seed);

   dealii::SymmetricTensor<4, dim> C = mepls::utils::tensor::make_isotropic_stiffness<dim>(p.G, p.nu);

   mepls::element::Vector<dim> elements;

   for(double n = 0; n < p.Nx * p.Ny; ++n)
   {
      example::element::Scalar<dim>::Config conf;
      conf.number = n;
      conf.gamma = p.gamma;
      conf.lambda = p.lambda;
      conf.k = p.k;
      conf.temperature = p.temperature;

      auto element = new example::element::Scalar<dim>(conf, generator);
      element->C(C);

      elements.push_back(element);
   }

   mepls::elasticity_solver::LeesEdwards<dim> solver(p.Nx, p.Ny, mepls::elasticity_solver::ControlMode::stress);
   for(auto &element : elements)
      solver.set_elastic_properties(element->number(), element->C());
   solver.setup_and_assembly();

   mepls::element::calculate_local_stress_coefficients_central(elements, solver);
   mepls::element::calculate_ext_stress_coefficients(elements, solver);

   mepls::system::Standard<dim> system(elements, solver, generator);

   //----- CREEP DEFORMATION ------

   mepls::history::History<dim> creep_history("creep_history");
   system.set_history(creep_history);
   creep_history.add_macro(system);

   // apply an external (stress) load of amplitude p.ext_stress_creep
   mepls::dynamics::fixed_load_increment(p.ext_stress_creep, system);
   creep_history.add_macro(system);

   mepls::dynamics::KMC<dim> kmc;

   mepls::utils::ContinueSimulation continue_creep;

	while( continue_creep() )
	{
		cout << system.macrostate["time"] << " " << system.macrostate["total_strain"] <<" "
			  << system.macrostate["ext_stress"] << std::endl;

		kmc(system);
		creep_history.add_macro(system);

		mepls::dynamics::relaxation(system, continue_creep);
		creep_history.add_macro(system);

	  continue_creep(system.macrostate["time"] < p.time_limit_creep, "creep time limit reached");
	}

	 cout << continue_creep << std::endl;

   //----- TRANSITION TO AQS ------

	// remove the external load
    mepls::dynamics::fixed_load_increment(-p.ext_stress_creep, system);
	creep_history.add_macro(system);

	// relax possible unstable slip system after the load change
	mepls::dynamics::relaxation(system, continue_creep);
	creep_history.add_macro(system);

    // in the AQS, we start measuring the macroscale strain from zero
	system.macrostate.clear();

	for(auto & element : elements)
		element->state_to_prestress();

	// we switch the driving mode to strain-controlled during AQS
    solver.set_control_mode(mepls::elasticity_solver::ControlMode::strain);

    // since the type of driving conditions have changed, the local stress change induced by a unit
    // load increment is different. We need to re-compute the ext_stress_coefficients
    mepls::element::calculate_ext_stress_coefficients(elements, solver);

    //----- ATHERMAL QUASISTATIC SHEAR ------

   	mepls::history::History<dim> aqs_history("aqs_history");
  	system.set_history(aqs_history);
   	aqs_history.add_macro(system);

   mepls::utils::ContinueSimulation continue_AQS_simulation;
   while(continue_AQS_simulation())
   {
		cout << system.macrostate["total_strain"] << " " << system.macrostate["ext_stress"] << std::endl;

      mepls::dynamics::extremal_dynamics_step(system);
      aqs_history.add_macro(system);

      mepls::dynamics::relaxation(system, continue_AQS_simulation);
      aqs_history.add_macro(system);

      continue_AQS_simulation(system.macrostate["total_strain"] < p.strain_limit_aqs, "AQS strain limit reached");
   }

	   cout << continue_AQS_simulation << std::endl;

   for(auto &element : elements)
      delete element;

	// write the simulation data, using its own dedicated function
	write_data(creep_history, aqs_history, p);
}

int main(int argc, char *argv[])
{
   // Read the command line arguments. We define the -f 'filename' to pass the
   // path to the parameters file
   cli::Parser parser(argc, argv);
   parser.set_optional<std::string>("f", "file", "./default.prm", "Name of the input configuration file");
   parser.run_and_exit_if_error();
   // you can check https://github.com/FlorianRappl/CmdParser for a further documentation of cli::Parser

   // Create the parameters object
   Parameters p;

   // We try to load the parameters file, but if it doesn't exist, we generate a new one with the name
   // default.prm and default values
   try
   {
      p.load_file(parser.get<std::string>("f"));
   }
   catch(dealii::PathSearch::ExcFileNotFound &)
   {
      p.generate_file(parser.get<std::string>("f"));
      std::cout << "Configuration file " << parser.get<std::string>("f") << " created" << std::endl;
      return 1;
   }

	unsigned int n_rep = p.n_rep;
	if(n_rep < omp_get_max_threads())
		n_rep = omp_get_max_threads();

	// initialize the master engine with the master seed
	std::srand(p.seed);

	#pragma omp parallel
	{
		unsigned int n_threads = omp_get_max_threads();
		unsigned int id = omp_get_thread_num();
		unsigned int rep_per_thread = int( n_rep / n_threads );

		dealii::ConditionalOStream cout(std::cout, id==0 and p.verbose);

		for(unsigned int n = 0; n < rep_per_thread; ++n)
		{
			Parameters p_thread = p;
			#pragma critical
			{
				// generate a seed for a simulation run
				p_thread.seed = std::rand();
			};

			run(p_thread, cout);
		}

	}

   return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_MATRIX_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_MATRIX_HPP

#include <cstddef>

#include <boost/geometry/extensions/algebra/core/tags.hpp>
#include <boost/geometry/extensions/algebra/geometries/concepts/matrix_concept.hpp>

namespace boost { namespace geometry {

namespace model {

template <typename T, std::size_t Rows, std::size_t Cols>
class matrix
{
    BOOST_CONCEPT_ASSERT( (concept::Matrix<matrix>) );

public:

    /// @brief Default constructor, no initialization
    inline matrix()
    {}

    /// @brief Get a coordinate
    /// @tparam I row index
    /// @tparam J col index
    /// @return the cell value
    template <std::size_t I, std::size_t J>
    inline T const& get() const
    {
        BOOST_STATIC_ASSERT(I < Rows);
        BOOST_STATIC_ASSERT(J < Cols);
        return m_values[I + Rows * J];
    }

    /// @brief Set a coordinate
    /// @tparam I row index
    /// @tparam J col index
    /// @param value value to set
    template <std::size_t I, std::size_t J>
    inline void set(T const& value)
    {
        BOOST_STATIC_ASSERT(I < Rows);
        BOOST_STATIC_ASSERT(J < Cols);
        m_values[I + Rows * J] = value;
    }

private:

    T m_values[Rows * Cols];
};

} // namespace model

#ifndef DOXYGEN_NO_TRAITS_SPECIALIZATIONS
namespace traits
{

template <typename CoordinateType, std::size_t Rows, std::size_t Cols>
struct tag<model::matrix<CoordinateType, Rows, Cols> >
{
    typedef matrix_tag type;
};

template <typename CoordinateType, std::size_t Rows, std::size_t Cols>
struct coordinate_type<model::matrix<CoordinateType, Rows, Cols> >
{
    typedef CoordinateType type;
};

//template <typename CoordinateType, std::size_t Rows, std::size_t Cols>
//struct coordinate_system<model::matrix<CoordinateType, Rows, Cols> >
//{
//    typedef cs::cartesian type;
//};

// TODO - move this class to traits.hpp
template <typename Geometry, std::size_t Index>
struct indexed_dimension
{
     BOOST_MPL_ASSERT_MSG(false,
                          NOT_IMPLEMENTED_FOR_THIS_GEOMETRY_OR_INDEX,
                          (Geometry, boost::integral_constant<std::size_t, Index>));
};

template <typename CoordinateType, std::size_t Rows, std::size_t Cols>
struct indexed_dimension<model::matrix<CoordinateType, Rows, Cols>, 0>
    : boost::integral_constant<std::size_t, Rows>
{};

template <typename CoordinateType, std::size_t Rows, std::size_t Cols>
struct indexed_dimension<model::matrix<CoordinateType, Rows, Cols>, 1>
    : boost::integral_constant<std::size_t, Cols>
{};

template <typename CoordinateType, std::size_t Rows, std::size_t Cols, std::size_t I, std::size_t J>
struct indexed_access<model::matrix<CoordinateType, Dimension>, I, J>
{
    typedef CoordinateType coordinate_type;

    static inline coordinate_type get(model::matrix<CoordinateType, Rows, Cols> const& m)
    {
        return m.template get<I, J>();
    }

    static inline void set(model::matrix<CoordinateType, Rows, Cols> & m, coordinate_type const& value)
    {
        m.template set<I, J>(value);
    }
};

} // namespace traits
#endif // DOXYGEN_NO_TRAITS_SPECIALIZATIONS

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_MATRIX_HPP


#include <voxelized_geometry_tools/opencl_voxelization_helpers.h>

#include <cmath>
#include <cstdint>
#include <iostream>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include <Eigen/Geometry>
#include <CL/cl.hpp>

namespace voxelized_geometry_tools
{
namespace pointcloud_voxelization
{
namespace opencl_helpers
{
const char* kRaycastPointKernelCode = R"(
void kernel RaycastPoint(
    global const float* points, global const float* pointcloud_origin_transform,
    const float max_range, global const float* inverse_grid_origin_transform,
    const float inverse_step_size, const float inverse_cell_size,
    const int stride1, const int stride2, const int num_x_cells,
    const int num_y_cells, const int num_z_cells, global int* tracking_grid,
    const int tracking_grid_starting_offset)
{
  const int point_index = get_global_id(0);
  const float px = points[(point_index * 3) + 0];
  const float py = points[(point_index * 3) + 1];
  const float pz = points[(point_index * 3) + 2];
  // Skip invalid points marked with NaN or infinity
  if (isfinite(px) && isfinite(py) && isfinite(pz))
  {
    const float ox = pointcloud_origin_transform[12];
    const float oy = pointcloud_origin_transform[13];
    const float oz = pointcloud_origin_transform[14];
    const float wx = pointcloud_origin_transform[0] * px
                     + pointcloud_origin_transform[4] * py
                     + pointcloud_origin_transform[8] * pz
                     + pointcloud_origin_transform[12];
    const float wy = pointcloud_origin_transform[1] * px
                     + pointcloud_origin_transform[5] * py
                     + pointcloud_origin_transform[9] * pz
                     + pointcloud_origin_transform[13];
    const float wz = pointcloud_origin_transform[2] * px
                     + pointcloud_origin_transform[6] * py
                     + pointcloud_origin_transform[10] * pz
                     + pointcloud_origin_transform[14];
    const float rx = wx - ox;
    const float ry = wy - oy;
    const float rz = wz - oz;
    const float current_ray_length = sqrt((rx * rx) + (ry * ry) + (rz * rz));
    const float num_steps = floor(current_ray_length * inverse_step_size);
    int previous_x_cell = -1;
    int previous_y_cell = -1;
    int previous_z_cell = -1;
    bool ray_crossed_grid = false;
    for (float step = 0.0; step < num_steps; step += 1.0)
    {
      const float elapsed_ratio = step / num_steps;
      if ((elapsed_ratio * current_ray_length) > max_range)
      {
        // We've gone beyond max range of the sensor
        break;
      }
      const float cx = (rx * elapsed_ratio) + ox;
      const float cy = (ry * elapsed_ratio) + oy;
      const float cz = (rz * elapsed_ratio) + oz;
      const float gx =
          inverse_grid_origin_transform[0] * cx
          + inverse_grid_origin_transform[4] * cy
          + inverse_grid_origin_transform[8] * cz
          + inverse_grid_origin_transform[12];
      const float gy =
          inverse_grid_origin_transform[1] * cx
          + inverse_grid_origin_transform[5] * cy
          + inverse_grid_origin_transform[9] * cz
          + inverse_grid_origin_transform[13];
      const float gz =
          inverse_grid_origin_transform[2] * cx
          + inverse_grid_origin_transform[6] * cy
          + inverse_grid_origin_transform[10] * cz
          + inverse_grid_origin_transform[14];
      const int x_cell = (int)(gx * inverse_cell_size);
      const int y_cell = (int)(gy * inverse_cell_size);
      const int z_cell = (int)(gz * inverse_cell_size);
      if (x_cell != previous_x_cell || y_cell != previous_y_cell
          || z_cell != previous_z_cell)
      {
        if (x_cell >= 0 && x_cell < num_x_cells && y_cell >= 0
           && y_cell < num_y_cells && z_cell >= 0 && z_cell < num_z_cells)
        {
          ray_crossed_grid = true;
          const int cell_index =
              (x_cell * stride1) + (y_cell * stride2) + z_cell;
          const int tracking_grid_index =
              tracking_grid_starting_offset + (cell_index * 2);
          atomic_add(&(tracking_grid[tracking_grid_index]), 1);
        }
        else if (ray_crossed_grid)
        {
          break;
        }
      }
      previous_x_cell = x_cell;
      previous_y_cell = y_cell;
      previous_z_cell = z_cell;
    }
    // Set the point itself as filled, if it is in range
    if (current_ray_length <= max_range)
    {
      const float gx =
          inverse_grid_origin_transform[0] * wx
          + inverse_grid_origin_transform[4] * wy
          + inverse_grid_origin_transform[8] * wz
          + inverse_grid_origin_transform[12];
      const float gy =
          inverse_grid_origin_transform[1] * wx
          + inverse_grid_origin_transform[5] * wy
          + inverse_grid_origin_transform[9] * wz
          + inverse_grid_origin_transform[13];
      const float gz =
          inverse_grid_origin_transform[2] * wx
          + inverse_grid_origin_transform[6] * wy
          + inverse_grid_origin_transform[10] * wz
          + inverse_grid_origin_transform[14];
      const int x_cell = (int)(gx * inverse_cell_size);
      const int y_cell = (int)(gy * inverse_cell_size);
      const int z_cell = (int)(gz * inverse_cell_size);
      if (x_cell >= 0 && x_cell < num_x_cells && y_cell >= 0
          && y_cell < num_y_cells && z_cell >= 0 && z_cell < num_z_cells)
      {
        const int cell_index = (x_cell * stride1) + (y_cell * stride2) + z_cell;
        const int tracking_grid_index =
            tracking_grid_starting_offset + (cell_index * 2);
        atomic_add(&(tracking_grid[tracking_grid_index + 1]), 1);
      }
    }
  }
}
)";

const char* kFilterGridsKernelCode = R"(
void kernel FilterGrids(
    const int num_cells, const int num_grids, global const int* tracking_grid,
    global float* filter_grid, const float percent_seen_free,
    const int outlier_points_threshold, const int num_cameras_seen_free)
{
  const int voxel_index = get_global_id(0);
  const int filter_grid_index = voxel_index * 2;
  const float current_occupancy = filter_grid[filter_grid_index];
  if (current_occupancy <= 0.5)
  {
    int cameras_seen_filled = 0;
    int cameras_seen_free = 0;
    for (int idx = 0; idx < num_grids; idx++)
    {
      const int tracking_grid_offset = num_cells * 2 * idx;
      const int tracking_grid_index =
          tracking_grid_offset + filter_grid_index;
      const int free_count = tracking_grid[tracking_grid_index];
      const int filled_count = tracking_grid[tracking_grid_index + 1];
      const int filtered_filled_count =
          (filled_count >= outlier_points_threshold) ? filled_count : 0;
      if (free_count > 0 && filtered_filled_count > 0)
      {
        const float current_percent_seen_free =
            (float)(free_count) / (float)(free_count + filtered_filled_count);
        if (current_percent_seen_free >= percent_seen_free)
        {
          cameras_seen_free += 1;
        }
        else
        {
          cameras_seen_filled += 1;
        }
      }
      else if (free_count > 0)
      {
        cameras_seen_free += 1;
      }
      else if (filtered_filled_count > 0)
      {
        cameras_seen_filled += 1;
      }
    }
    if (cameras_seen_filled > 0)
    {
      filter_grid[filter_grid_index] = 1.0;
    }
    else if (cameras_seen_free >= num_cameras_seen_free)
    {
      filter_grid[filter_grid_index] = 0.0;
    }
    else
    {
      filter_grid[filter_grid_index] = 0.5;
    }
  }
}
)";

static std::string GetRaycastingKernelCode()
{
  return std::string(kRaycastPointKernelCode);
}

static std::string GetFilterKernelCode()
{
  return std::string(kFilterGridsKernelCode);
}

int32_t RetrieveOptionOrDefault(
    const std::map<std::string, int32_t>& options, const std::string& option,
    const int32_t default_value)
{
  auto found_itr = options.find(option);
  if (found_itr != options.end())
  {
    const int32_t value = found_itr->second;
    std::cout << "Option [" << option << "] found with value [" << value << "]"
              << std::endl;
    return value;
  }
  else
  {
    std::cout << "Option [" << option << "] not found, default ["
              << default_value << "]" << std::endl;
    return default_value;
  }
}

class RealOpenCLVoxelizationHelperInterface
    : public OpenCLVoxelizationHelperInterface
{
public:
  explicit RealOpenCLVoxelizationHelperInterface(
      const std::map<std::string, int32_t>& options)
  {
    std::vector<cl::Platform> all_platforms;
    cl::Platform::get(&all_platforms);
    const int32_t platform_index =
        RetrieveOptionOrDefault(options, "OPENCL_PLATFORM_INDEX", 0);
    if (all_platforms.size() > 0 && platform_index >= 0
        && platform_index < static_cast<int32_t>(all_platforms.size()))
    {
      auto& opencl_platform = all_platforms.at(platform_index);
      std::vector<cl::Device> all_devices;
      opencl_platform.getDevices(CL_DEVICE_TYPE_ALL, &all_devices);
      const int32_t device_index =
          RetrieveOptionOrDefault(options, "OPENCL_DEVICE_INDEX", 0);
      if (all_devices.size() > 0 && device_index >= 0
          && device_index < static_cast<int32_t>(all_devices.size()))
      {
        auto& opencl_device = all_devices.at(device_index);
        // Make context + queue
        context_ = std::unique_ptr<cl::Context>(
            new cl::Context({opencl_device}));
        queue_ = std::unique_ptr<cl::CommandQueue>(
            new cl::CommandQueue(*context_, opencl_device));
        // Make kernel programs
        const std::string build_options = "-Werror -cl-fast-relaxed-math";
        cl::Program::Sources raycasting_sources;
        const std::string raycasting_kernel_source = GetRaycastingKernelCode();
        raycasting_sources.push_back({raycasting_kernel_source.c_str(),
                                      raycasting_kernel_source.length()});
        raycasting_program_ = std::unique_ptr<cl::Program>(
            new cl::Program(*context_, raycasting_sources));
        cl::Program::Sources filter_sources;
        const std::string filter_kernel_source = GetFilterKernelCode();
        filter_sources.push_back({filter_kernel_source.c_str(),
                                  filter_kernel_source.length()});
        filter_program_ = std::unique_ptr<cl::Program>(
            new cl::Program(*context_, filter_sources));
        if (raycasting_program_->build({opencl_device}, build_options.c_str())
            != CL_SUCCESS)
        {
          std::cerr << " Error building raycasting kernel: "
                    << raycasting_program_->getBuildInfo<CL_PROGRAM_BUILD_LOG>(
                        opencl_device)
                    << std::endl;
          raycasting_program_.reset();
        }
        if (filter_program_->build({opencl_device}, build_options.c_str())
            != CL_SUCCESS)
        {
          std::cerr << " Error building filter kernel: "
                    << filter_program_->getBuildInfo<CL_PROGRAM_BUILD_LOG>(
                        opencl_device)
                    << std::endl;
          filter_program_.reset();
        }
      }
      else if (all_devices.size() > 0)
      {
        std::cerr << "OPENCL_DEVICE_INDEX = " << device_index
                  << " out of range for " << all_devices.size() << " devices"
                  << std::endl;
      }
      else
      {
        std::cerr << "No OpenCL device available" << std::endl;
      }
    }
    else if (all_platforms.size() > 0)
    {
      std::cerr << "OPENCL_PLATFORM_INDEX = " << platform_index
                << " out of range for " << all_platforms.size() << " platforms"
                << std::endl;
    }
    else
    {
      std::cerr << "No OpenCL platform available" << std::endl;
    }
  }

  bool IsAvailable() const override
  {
    return (context_ && queue_ && raycasting_program_ && filter_program_);
  }

  std::vector<int64_t> PrepareTrackingGrids(
      const int64_t num_cells, const int32_t num_grids) override
  {
    const size_t buffer_size = sizeof(int32_t) * 2 * num_cells * num_grids;
    cl_int err = 0;
    device_tracking_grid_buffer_ = std::unique_ptr<cl::Buffer>(new cl::Buffer(
        *context_, CL_MEM_READ_WRITE, buffer_size, nullptr, &err));
    if (err == 0)
    {
      // This is how we zero the buffer
      cl::Event event;
      err = queue_->enqueueFillBuffer<int32_t>(
          *device_tracking_grid_buffer_, 0, 0, buffer_size, nullptr, &event);
      if (err == CL_SUCCESS)
      {
        err = event.wait();
        if (err == CL_SUCCESS)
        {
          std::vector<int64_t> tracking_grid_offsets(num_grids, 0);
          for (int32_t num_grid = 0; num_grid < num_grids; num_grid++)
          {
            tracking_grid_offsets.at(num_grid) = num_grid * num_cells * 2;
          }
          return tracking_grid_offsets;
        }
        else
        {
          throw std::runtime_error("Failed to wait for event");
        }
      }
      else
      {
        throw std::runtime_error("Failed to enqueueFillBuffer");
      }
    }
    else
    {
      throw std::runtime_error("Failed to allocate tracking grid buffer");
    }
  }

  void RaycastPoints(
      const std::vector<float>& raw_points,
      const Eigen::Isometry3f& pointcloud_origin_transform,
      const float max_range,
      const Eigen::Isometry3f& inverse_grid_origin_transform,
      const float inverse_step_size, const float inverse_cell_size,
      const int32_t num_x_cells, const int32_t num_y_cells,
      const int32_t num_z_cells,
      const int64_t tracking_grid_starting_offset) override
  {
    cl_int err = 0;
    cl::Buffer device_points_buffer(
        *context_, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
        sizeof(float) * raw_points.size(),
        const_cast<void*>(static_cast<const void*>(raw_points.data())),
        &err);
    if (err != CL_SUCCESS)
    {
      throw std::runtime_error("Failed to allocate and copy pointcloud");
    }
    cl::Buffer device_pointcloud_origin_transform_buffer(
        *context_, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
        sizeof(float) * 16,
        const_cast<void*>(static_cast<const void*>(
            pointcloud_origin_transform.data())),
        &err);
    if (err != CL_SUCCESS)
    {
      throw std::runtime_error("Failed to allocate and copy cloud transform");
    }
    cl::Buffer device_inverse_grid_origin_transform_buffer(
        *context_, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
        sizeof(float) * 16,
        const_cast<void*>(static_cast<const void*>(
            inverse_grid_origin_transform.data())),
        &err);
    if (err != CL_SUCCESS)
    {
      throw std::runtime_error("Failed to allocate and copy grid transform");
    }
    const int32_t stride1 = num_y_cells * num_z_cells;
    const int32_t stride2 = num_z_cells;
    // Build kernel
    cl::Kernel raycasting_kernel(*raycasting_program_, "RaycastPoint");
    raycasting_kernel.setArg(0, device_points_buffer);
    raycasting_kernel.setArg(1, device_pointcloud_origin_transform_buffer);
    raycasting_kernel.setArg(2, max_range);
    raycasting_kernel.setArg(3, device_inverse_grid_origin_transform_buffer);
    raycasting_kernel.setArg(4, inverse_step_size);
    raycasting_kernel.setArg(5, inverse_cell_size);
    raycasting_kernel.setArg(6, stride1);
    raycasting_kernel.setArg(7, stride2);
    raycasting_kernel.setArg(8, num_x_cells);
    raycasting_kernel.setArg(9, num_y_cells);
    raycasting_kernel.setArg(10, num_z_cells);
    raycasting_kernel.setArg(11, *device_tracking_grid_buffer_);
    raycasting_kernel.setArg(
        12, static_cast<int32_t>(tracking_grid_starting_offset));
    err = queue_->enqueueNDRangeKernel(
        raycasting_kernel, cl::NullRange, cl::NDRange(raw_points.size() / 3),
        cl::NullRange);
    if (err != CL_SUCCESS)
    {
      throw std::runtime_error("Failed to enqueue raycasting kernel");
    }
  }

  void PrepareFilterGrid(
      const int64_t num_cells, const void* host_data_ptr) override
  {
    cl_int err = 0;
    device_filter_grid_buffer_ = std::unique_ptr<cl::Buffer>(new cl::Buffer(
        *context_, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
        sizeof(float) * num_cells * 2, const_cast<void*>(host_data_ptr), &err));
    if (err != CL_SUCCESS)
    {
      throw std::runtime_error("Failed to allocate and copy filtered buffer");
    }
  }

  void FilterTrackingGrids(
       const int64_t num_cells, const int32_t num_grids,
       const float percent_seen_free, const int32_t outlier_points_threshold,
       const int32_t num_cameras_seen_free) override
  {
    // Build kernel
    cl::Kernel filter_kernel(*filter_program_, "FilterGrids");
    filter_kernel.setArg(0, static_cast<int32_t>(num_cells));
    filter_kernel.setArg(1, num_grids);
    filter_kernel.setArg(2, *device_tracking_grid_buffer_);
    filter_kernel.setArg(3, *device_filter_grid_buffer_);
    filter_kernel.setArg(4, percent_seen_free);
    filter_kernel.setArg(5, outlier_points_threshold);
    filter_kernel.setArg(6, num_cameras_seen_free);
    const cl_int err = queue_->enqueueNDRangeKernel(
        filter_kernel, cl::NullRange, cl::NDRange(num_cells), cl::NullRange);
    if (err != CL_SUCCESS)
    {
      throw std::runtime_error(
          "Failed to enqueue filter kernel [" + std::to_string(err) + "]");
    }
  }

  void RetrieveTrackingGrid(
      const int64_t num_cells,
      const int64_t device_tracking_grid_starting_index,
      void* host_data_ptr) override
  {
    queue_->finish();
    const size_t item_size = sizeof(int32_t) * 2;
    const size_t buffer_size = num_cells * item_size;
    const size_t starting_offset =
        device_tracking_grid_starting_index * sizeof(int32_t);
    const cl_int err = queue_->enqueueReadBuffer(
        *device_tracking_grid_buffer_, CL_TRUE, starting_offset, buffer_size,
        host_data_ptr);
    if (err != CL_SUCCESS)
    {
      throw std::runtime_error("Tracking buffer enqueueReadBuffer failed");
    }
  }

  void RetrieveFilteredGrid(
      const int64_t num_cells, void* host_data_ptr) override
  {
    queue_->finish();
    const size_t buffer_size = sizeof(float) * num_cells * 2;
    const cl_int err = queue_->enqueueReadBuffer(
        *device_filter_grid_buffer_, CL_TRUE, 0, buffer_size, host_data_ptr);
    if (err != CL_SUCCESS)
    {
      throw std::runtime_error("Filtered buffer enqueueReadBuffer failed");
    }
  }

  void CleanupAllocatedMemory() override
  {
    device_tracking_grid_buffer_.reset();
    device_filter_grid_buffer_.reset();
  }

private:
  std::unique_ptr<cl::Context> context_;
  std::unique_ptr<cl::CommandQueue> queue_;
  std::unique_ptr<cl::Program> raycasting_program_;
  std::unique_ptr<cl::Program> filter_program_;
  std::unique_ptr<cl::Buffer> device_tracking_grid_buffer_;
  std::unique_ptr<cl::Buffer> device_filter_grid_buffer_;
};

OpenCLVoxelizationHelperInterface* MakeHelperInterface(
    const std::map<std::string, int32_t>& options)
{
  return new RealOpenCLVoxelizationHelperInterface(options);
}
}  // namespace opencl_helpers
}  // namespace pointcloud_voxelization
}  // namespace voxelized_geometry_tools


/// @file time.hpp
#pragma once

#include <chrono>
#include <stdexcept>
#include <string>
#include <boost/variant.hpp>
#include "adm/detail/named_type.hpp"
#include "adm/export.h"

namespace adm {

  /// @brief storage for ADM times of format 'hh:mm:ss.zzzzzSfffff'
  class FractionalTime {
   public:
    FractionalTime(int64_t numerator, int64_t denominator)
        : _numerator(numerator), _denominator(denominator) {
      if (denominator < 1)
        throw std::invalid_argument(
            "FractionalTime denominator must be positive");
    }

    int64_t numerator() const { return _numerator; }
    int64_t denominator() const { return _denominator; }

    bool operator==(const FractionalTime& other) const {
      return numerator() == other.numerator() &&
             denominator() == other.denominator();
    }

    bool operator!=(const FractionalTime& other) const {
      return !(*this == other);
    }

    /// normalised fraction, such that numerator and denominator have no common
    /// fractors
    ADM_EXPORT FractionalTime normalised() const;

   private:
    int64_t _numerator;
    int64_t _denominator;
  };

  /// representation of ADM times; this can either be decimal times,
  /// represented as nanoseconds, or fractional times, represented as a
  /// FractionalTime
  class Time {
   public:
    // non-explicit, as this should act like a variant
    template <typename Rep, typename Period>
    // NOLINTNEXTLINE(google-explicit-constructor)
    Time(const std::chrono::duration<Rep, Period>& time) : time(time) {}
    // NOLINTNEXTLINE(google-explicit-constructor)
    Time(const FractionalTime& time) : time(time) {}

    /// convert to nanoseconds, rounding down
    ADM_EXPORT std::chrono::nanoseconds asNanoseconds() const;
    ADM_EXPORT FractionalTime asFractional() const;

    bool isNanoseconds() const {
      return time.type() == typeid(std::chrono::nanoseconds);
    }
    bool isFractional() const { return time.type() == typeid(FractionalTime); };

    using Variant = boost::variant<std::chrono::nanoseconds, FractionalTime>;
    const Variant& asVariant() const { return time; }

    bool operator==(const Time& other) const { return time == other.time; }
    bool operator!=(const Time& other) const { return time != other.time; }

   private:
    Variant time;
  };

  /// @brief Tag for NamedType ::Start
  struct StartTag {};
  /// @brief NamedType for the start attribute
  using Start = detail::NamedType<Time, StartTag>;
  /// @brief Tag for NamedType ::End
  struct EndTag {};
  /// @brief NamedType for the end attribute
  using End = detail::NamedType<Time, EndTag>;
  /// @brief Tag for NamedType ::Duration
  struct DurationTag {};
  /// @brief NamedType for the duration attribute
  using Duration = detail::NamedType<Time, DurationTag>;
  /// @brief Tag for NamedType ::Rtime
  struct RtimeTag {};
  /// @brief NamedType for the rtime attribute
  using Rtime = detail::NamedType<Time, RtimeTag>;

  /// @brief Parse an adm timecode and convert it to a std::chrono::duration
  ADM_EXPORT Time parseTimecode(const std::string& timecode);
  /// @brief Format a std::chrono::duration object as an adm timecode string
  ADM_EXPORT std::string formatTimecode(const Time& time);
}  // namespace adm


#pragma once
#include <Eigen/Dense>
#include <vector>
#include <array>
#include <memory>
#include "line_interval.hpp"
#include "plane2d.hpp"

using namespace Eigen;
using namespace std;

#ifndef SHAPE2D_HPP
#define SHAPE2D_HPP

class LineInterval;

class Point2d {
    unsigned long int shapeId;
    int shapeVectorPos;
    Vector2d vec;
    unsigned long int angleId;
  public:
    double angle;
    Point2d(Vector2d _vec, unsigned long int _shapeId, int _shapeVectorPos);
    bool operator<(const Point2d &other) const {
	return angle < other.angle;
    }
    Vector2d Vec();
    double Angle();
    unsigned long int AngleId();
    void setAngleId(unsigned long int _angleId);
    int ShapeVectorPos();
    unsigned long int ShapeId();
};

class Shape2d {
    unsigned long int id;
    int numVertices;
    double weight;
    vector<Point2d> vertices;
    int endVertex;
    //vector<unsigned long int> prevShapeIds;
  public:
    Shape2d(unsigned long int _id, int _numVertices, double _weight);
    bool Complete();
    void addPoint(Point2d point);
    void arrange(vector<LineInterval>& lineIntervals);
    void setVerticesClockwise(int startVertex);
    void setNewVertices(vector<Point2d> tmpVertices);
    //there should probably be different subclasses for target shapes and nontarget shapes at some point
    void calculateOneIntervalTarget(LineInterval& li);
    void calculateOneInterval(LineInterval& li);
    void calculateAllIntervalsTarget(vector<LineInterval>& lineIntervals);
    void calculateAllIntervals(vector<LineInterval>& lineIntervals);
    void computeBoundsTarget(LineInterval& li, array<double, 2> edgePolar);
    void computeBounds(array<double, 3> entryFStart, array<double, 3> entryFEnd, array<double, 3> terminalFStart, array<double, 3> terminalFEnd, LineInterval& li);
    double maxDist();
    //double Weight();
    //unsigned long int Id();
    int EndVertex();
    vector<Vector2d> Vertices();
    vector<Vector2d> VerticesArranged();
    array<double, 2> EntryEdge(LineInterval& li);
    array<double, 2> TerminalEdge(LineInterval& li);
    //vector<unsigned long int> PrevShapeIds();
};

#endif


// 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, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#ifndef MTL_VEC_LOG_INCLUDE
#define MTL_VEC_LOG_INCLUDE

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

namespace mtl { namespace vec {

    /// Logarithm of \a v element-wise
    template <typename Vector>
    log_view<Vector> log(const Vector& v)
    {
        return log_view<Vector>(v);
    }

# ifdef MTL_WITH_MATH_ELEVEN
    /// Binary logarithm of \a v element-wise
    template <typename Vector>
    log2_view<Vector> log2(const Vector& v)
    {
        return log2_view<Vector>(v);
    }
# endif

    /// Decimal logarithm of \a v element-wise
    template <typename Vector>
    log10_view<Vector> log10(const Vector& v)
    {
        return log10_view<Vector>(v);
    }

}} // namespace mtl::vec

#endif // MTL_VEC_LOG_INCLUDE


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

    @Copyright 2016 Numscale SAS
    @copyright 2016 J.T.Lapreste

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_TOINT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_TOINT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/dispatch/meta/as_integer.hpp>
//#include <boost/simd/function/simd/split_low.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  // TODO split_low
//   BOOST_DISPATCH_OVERLOAD ( toint_
//                           , (typename A0)
//                           , bs::sse2_
//                           , bs::pack_<bd::double_<A0>, bs::sse_>
//                           , bs::fast_tag
//                          )
//   {
//     BOOST_FORCEINLINE  bd::as_integer_t<A0>  operator() ( const A0 & a0, const fast_tag &) const BOOST_NOEXCEPT
//     {
//       auto out = _mm_cvttpd_epi32(a0);
//       return bs::split_low(out);
//     }
//   };

  BOOST_DISPATCH_OVERLOAD ( toint_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::single_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0> operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return _mm_cvttps_epi32(a0);
    }
  };
} } }

#endif


#include <shg/binom.h>
#include <boost/math/special_functions/binomial.hpp>
#include <shg/utils.h>
#include "testing.h"

namespace SHG::Testing {

BOOST_AUTO_TEST_SUITE(binom_test)

namespace bdata = boost::unit_test::data;

/**
 * Returns \f$\binom{n}{k}\f$.
 */
template <typename T>
T boost_binom(T n, T k) {
     using boost::math::binomial_coefficient;
     return narrow_cast<T, double>(
          binomial_coefficient<double>(n, k));
}

BOOST_DATA_TEST_CASE(binom_basic_test,
                     bdata::xrange(11) * bdata::xrange(11), xr1,
                     xr2) {
     {
          unsigned const n = xr1;
          unsigned const k = xr2;

          if (k > n)
               BOOST_CHECK_THROW(binom(n, k), std::invalid_argument);
          else
               BOOST_CHECK(binom(n, k) == boost_binom(n, k));
     }
     {
          int const n = xr1;
          int const k = xr2;

          if (k > n)
               BOOST_CHECK_THROW(binom(n, k), std::invalid_argument);
          else
               BOOST_CHECK(binom(n, k) == boost_binom(n, k));
     }
     {
          int const n = xr1 - 5;
          int const k = xr2 - 5;

          if (n < 0 || k < 0 || k > n)
               BOOST_CHECK_THROW(binom(n, k), std::invalid_argument);
          else
               BOOST_CHECK(binom(n, k) == boost_binom(n, k));
     }
}

BOOST_DATA_TEST_CASE(binom_overflow_test, bdata::xrange(11), xr) {
     {
          unsigned const n = std::numeric_limits<unsigned>::max();
          unsigned const k = xr + 2;

          BOOST_CHECK_THROW(binom(n, k), std::overflow_error);
     }
     {
          int const n = std::numeric_limits<int>::max();
          int const k = xr + 2;

          BOOST_CHECK_THROW(binom(n, k), std::overflow_error);
     }
}

BOOST_AUTO_TEST_CASE(multiprecision_test) {
     namespace mp = boost::multiprecision;
     mp::cpp_int const n = 100;
     mp::cpp_int k;
     mp::cpp_int s = 0;

     for (k = 0; k <= n; k++)
          s += binom(n, k);
     BOOST_CHECK(s == mp::pow(mp::cpp_int(2), 100));
}

BOOST_AUTO_TEST_SUITE_END()

}  // namespace SHG::Testing


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE AapiBasicInferenceCallOpTest

#include <algorithm>
#include <cstdlib>
#include <memory>
#include <random>
#include <string>
#include <vector>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/test/unit_test.hpp>

#include <popart/graph.hpp>
#include <popart/ir.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/add.hpp>
#include <popart/op/call.hpp>
#include <popart/op/exchange/hostcopy.hpp>
#include <popart/op/init.hpp>
#include <popart/op/matmul.hpp>
#include <popart/session.hpp>
#include <popart/testdevice.hpp>
#include <popart/util.hpp>
#include <popart/vendored/optional.hpp>

namespace ublas = boost::numeric::ublas;

namespace popart {

ublas::matrix<float> buff2mat(std::vector<float>, int, int);
std::vector<float> mat2buff(ublas::matrix<float>);

/**
 * Basic (inference) CallOp example.
 *
 * Inference on a chain of the same CallOp, which contains a single MatMul.
 *
 * The MatMul in this diagram is inside a subgraph that's called repeatedly:
 *
 *     I      w0
 *     |     /
 *     MatMul
 *      |
 *      |  1.0
 *      |  /
 *      add    w1
 *       |     /
 *       MatMul
 *        |
 *        |  2.0
 *        |  /
 *        add   w2
 *         |     /
 *         MatMul
 *          |
 *          |  3.0
 *          |  /
 *          add   w3
 *           |     /
 *           MatMul
 *            |
 *            |  4.0
 *            |  /
 *            add
 *
 */
BOOST_AUTO_TEST_CASE(TestBasicInference) {
  // Construct IR and main graph.
  auto ir  = std::make_unique<Ir>();
  Graph &g = ir->getMainGraph();

  // Parameters.
  const int nLayers = 5;
  const int r       = 5; // Number of matrix rows.
  const int c       = r; // Number of matrix cols (same as rows).
  const TensorInfo tInfo(DataType::FLOAT, Shape{r, c});

  // Initialise a normal distribution with zero mean and unit variance.
  std::default_random_engine gen(42);
  std::normal_distribution<> d{0, 1};
  // Create a function to sample from the distribution.
  auto sampleFromNormal = [&]() { return d(gen); };

  // Generate random weights on the host.
  std::vector<std::vector<float>> weightsHost = {};
  for (int i = 0; i < nLayers; i++) {
    std::vector<float> weight(tInfo.nelms());
    std::generate(weight.begin(), weight.end(), sampleFromNormal);
    weightsHost.push_back(weight);
  }

  // Build the call subgraph - it only contains a MatMul.
  Graph &callGraph   = ir->createGraph({"callGraph"});
  TensorId wInCall   = addScope(callGraph, "weight(mmLhs)");
  TensorId actInCall = addScope(callGraph, "act(mmRhs)");
  TensorId outInCall = addScope(callGraph, "out");

  callGraph.addInput(wInCall, tInfo);
  callGraph.addInput(actInCall, tInfo);

  callGraph.createConnectedOp<MatMulOp>(
      {{MatMulOp::getLhsInIndex(), wInCall},
       {MatMulOp::getRhsInIndex(), actInCall}},
      {{MatMulOp::getOutIndex(), outInCall}},
      Onnx::Operators::MatMul_9,
      Op::Settings{g, "MatMul"},
      nonstd::nullopt,
      MatMulOp::SerialiseSettings(),
      OptionalDataType());

  callGraph.markAsOutput(outInCall);

  // Create a stream for data input from host.
  TensorId dataStream = "D_stream";
  g.getTensors().addStream(dataStream, tInfo);

  // Init data tensor. This Op is required for HostLoadOp - see hostcopy.hpp.
  TensorId dataPrehostload = "D_prehostload";
  g.createConnectedOp<InitOp>({},
                              {{InitOp::getOutIndex(), dataPrehostload}},
                              Onnx::CustomOperators::Init_1,
                              tInfo,
                              TensorType::ActGrad,
                              InitType::Zero,
                              Op::Settings{g, "Init"});

  // Fill data tensor with data from host.
  TensorId data = "D";
  g.createConnectedOp<HostLoadOp>(
      {{HostLoadOp::getLocalTensorInIndex(), dataPrehostload}},
      {{HostLoadOp::getLocalTensorOutIndex(), data}},
      Onnx::CustomOperators::HostLoad,
      Op::Settings{g, "HostLoad"},
      dataStream);

  // Build the layers of the graph by calling the same subgraph and adding an
  // AddOp after it.
  TensorId act = data;
  for (int i = 0; i < nLayers; i++) {
    std::string layerId = std::to_string(i + 1);
    TensorId weights    = "W" + layerId;
    TensorId callOut    = "out" + layerId;
    g.getTensors().addVarInit(weights, tInfo, weightsHost[i].data());

    g.createConnectedOp<CallOp>(
        {{callGraph.getInputIndex(wInCall), weights},
         {callGraph.getInputIndex(actInCall), act}},
        {{callGraph.getOutputIndex(outInCall), callOut}},
        Onnx::AiGraphcore::OpSet1::Call,
        std::ref(callGraph),
        Op::Settings{g, "CallMm-" + layerId});

    TensorId addOut               = "Ao" + layerId;
    TensorId addRhs               = "AddRhs" + layerId;
    std::vector<float> addRhsHost = {static_cast<float>(i + 1)};
    g.getTensors().addConstInit(
        addRhs, {DataType::FLOAT, {1}}, addRhsHost.data());

    g.createConnectedOp<AddOp>(
        {{AddOp::getArg0InIndex(), callOut}, {AddOp::getArg1InIndex(), addRhs}},
        {{AddOp::getOutIndex(), addOut}},
        Onnx::Operators::Add_7,
        Op::Settings{g, "Add-" + layerId});

    act = addOut;
  }

  // Create a stream for the model's output to host.
  ir->setDataFlow(DataFlow{1, {{act, AnchorReturnType("All")}}});
  g.createConnectedOp<HostStoreOp>(
      {{HostStoreOp::getLocalTensorInIndex(), act}},
      {},
      Onnx::CustomOperators::HostStore,
      Op::Settings{g, "HostStore"},
      ir->getAnchorRemap().getRight(act));

  // Set IR state required for lowering.
  auto &opts                   = ir->getSessionOptions();
  opts.aliasZeroCopy           = true;
  opts.enableExplicitMainLoops = true;
  opts.explicitRecomputation   = true;
  opts.useHostCopyOps          = true;
  ir->updateVertices();
  ir->setIsPrepared();

  // Lower IR.
  const auto session = InferenceSession::createFromIr(
      std::move(ir), createTestDevice(TEST_TARGET));
  session->prepareDevice();

  // Prepare inputs and anchors.
  std::vector<float> dataHost(tInfo.nelms());
  std::generate(dataHost.begin(), dataHost.end(), sampleFromNormal);
  NDArrayWrapper<float> dataWrapper(dataHost.data(), tInfo);
  std::map<TensorId, IArray &> inputs = {{dataStream, dataWrapper}};

  std::vector<float> outHost(tInfo.nelms());
  NDArrayWrapper<float> outWrapper(outHost.data(), tInfo);
  std::map<TensorId, IArray &> anchors = {{act, outWrapper}};

  // Run the model.
  StepIO stepio(inputs, anchors);
  session->weightsFromHost();
  session->run(stepio);

  // Numerical test.
  auto expected = buff2mat(dataHost, r, c);
  for (int i = 0; i < nLayers; i++) {
    auto weights = buff2mat(weightsHost[i], r, c);
    expected     = ublas::prod(weights, expected);
    ublas::scalar_matrix<float> addRhs(r, c, static_cast<float>(i + 1));
    expected += addRhs;
  }
  BOOST_TEST(mat2buff(expected) == outHost);
}

/**
 * Convert an std::vector buffer to a matrix on which linear algebra can be
 * applied.
 *
 * \param buff The input data buffer.
 * \param r The number of rows in the matrix.
 * \param c The number of columns in the matrix.
 * \return linalg::matrix<float> The resulting matrix.
 */
ublas::matrix<float> buff2mat(std::vector<float> buff, int r, int c) {
  ublas::matrix<float> mat(r, c);
  for (int i = 0; i < mat.size1(); ++i)
    for (int j = 0; j < mat.size2(); ++j)
      mat(i, j) = buff[c * i + j];
  return mat;
}

/**
 * Flatten a matrix into an std::vector.
 *
 * \param mat The input matrix.
 * \return std::vector<float> The resulting vector.
 */
std::vector<float> mat2buff(ublas::matrix<float> mat) {
  std::vector<float> buff(mat.size1() * mat.size2());
  for (int i = 0; i < buff.size(); i++)
    buff[i] = mat.data()[i];
  return buff;
}

} // namespace popart


//Bsplines file
#include <Eigen/Sparse>
#include <Eigen/Eigenvalues>
#include <Eigen/StdVector>
#include <cstdlib>
#include <iostream>
#include <mpi.h>
#include <fstream>
#include <iterator>
#include <vector>
#include <algorithm>
#include <complex>
#include <sstream>
#include <math.h>
#include <omp.h>
#include <numeric>
#include <chrono>
#include <ctime>
#include <limits>
#include "bsplines.h"

#define bp(x) cout << "breakpoint" << (#x) << endl

using namespace Eigen;
using namespace std::complex_literals;
using namespace std::chrono;
using std::cout;
using std::endl;

void generateBsplines(InputParameter Inp, double dX,ArrayXd x,ArrayXXd &BB,ArrayXXd &d_BB, ArrayXXd &diff_BB){
    std::vector<Array<double,Dynamic,Dynamic>
        ,aligned_allocator<Array<double,Dynamic,Dynamic> > > B(Inp.n+1);
    for(int i = 0 ; i < Inp.n+1; i++){
        B[i].setZero(x.size(),Inp.N_b-1+2*Inp.n);
    }

    ArrayXd t = ArrayXd::Zero(Inp.N_b+2*Inp.n);

    for(int i = Inp.n+1; i< Inp.N_b +Inp.n;i++){
        B[0].col(i-1).segment((i-Inp.n-1) * Inp.n,Inp.n) = 1;
        t(i-1) = (i-Inp.n-1) * dX;

    }
    t.segment(Inp.N_b + Inp.n - 1,Inp.n+1) = Inp.b;
    for( int k = 1; k < Inp.n+1;k++){
        for(int i = 1; i < Inp.N_b-1+2*Inp.n-k+1; i++){
            double tmp1 = t(i+k-1) - t(i-1);
            double tmp2 = t(i+k+0) - t(i-0);

            if(tmp1 == 0)
                tmp1 =1;
            if(tmp2 == 0)
                tmp2 =1;

            B[k].col(i-1) = ((x - t(i-1))/tmp1) * B[k-1].col(i-1)
                           + (t(i+k) - x)/tmp2 * B[k-1].col(i);
       }
    }
    bp("first order derivaitve of bsplines");
    //d_B is the first derivative
    std::vector<Array<double,Dynamic,Dynamic>
        ,aligned_allocator<Array<double,Dynamic,Dynamic> > > d_B(Inp.n+1);
    for(int i = 0 ; i < Inp.n+1; i++){
        d_B[i].setZero(x.size(),Inp.N_b-1+2*Inp.n);
    }

    for( int k = 2; k < Inp.n+1; k++){
        for(int i = 1; i < Inp.N_b-1+2*Inp.n-k+1; i++){
            double tmp1 = t(i+k-1) - t(i-1);
            double tmp2 = t(i+k+0) - t(i+0);

            if(tmp1 == 0)
                tmp1 =1;
            if(tmp2 == 0)
                tmp2 =1;

            d_B[k].col(i-1) = k * (B[k-1].col(i-1)/tmp1 - B[k-1].col(i)/tmp2);

        }
    }
    bp("scond order derivative of bsplines ");
    //diff of B is the second derivative
    std::vector<Array<double,Dynamic,Dynamic>
        ,aligned_allocator<Array<double,Dynamic,Dynamic> > > diff_B(Inp.n+1);
    for(int i = 0 ; i < Inp.n+1; i++){
        diff_B[i].setZero(x.size(),Inp.N_b-1+2*Inp.n);
    }

    for( int k = 2; k < Inp.n+1; k++){
        for(int i = 1; i < Inp.N_b-1+2*Inp.n-k+1; i++){
            double tmp1 = t(i+k-1) - t(i-1);
            double tmp2 = t(i+k+0) - t(i+0);
            double tmp3 = t(i+k-2) - t(i-1);
            double tmp4 = t(i+k-1) - t(i+0);
            double tmp5 = t(i+k+0) - t(i+1);

            if(tmp1 == 0)
                tmp1 =1;
            if(tmp2 == 0)
                tmp2 =1;
            if(tmp3 == 0)
                tmp3 =1;
            if(tmp4 == 0)
                tmp4 =1;
            if(tmp5 == 0)
                tmp5 =1;

        diff_B[k].col(i-1) = k * (k-1)/tmp1 * (B[k-2].col(i-1)/tmp3 - B[k-2].col(i)/tmp4)
                           - k * (k-1)/tmp2 * (B[k-2].col(i)/tmp4 - B[k-2].col(i+1)/tmp5);
        }
    }
    BB=B[Inp.n];
    d_BB=d_B[Inp.n];
    diff_BB=diff_B[Inp.n];

    cout <<"Bsplines generated"<<endl;

}

void gausLegendreSetup(int k, int nmbr_breakpoints,
        ArrayXd x_break, ArrayXd &x, ArrayXd &weights){
    //Return evenly spaced values within a given interval.
    std::vector<int> I(k);
    std::iota(I.begin(),I.end(),1);

    MatrixXd J = MatrixXd::Zero(k,k);
    for(int i = 0; i < k-1; i++){
        J(i,i+1) = 0.5 / sqrt(1-pow(2*I[i],-2));
        J(i+1,i) = 0.5 / sqrt(1-pow(2*I[i],-2));
    }
    //get the eigenvalues and eigenvectors

    SelfAdjointEigenSolver<MatrixXd> es;
    es.compute(J);
    VectorXd eigval = es.eigenvalues();
    MatrixXd eigvect = es.eigenvectors();
    for(int i = 2; i < nmbr_breakpoints+1;i++){
        x.segment((i-2)*k,k) = (x_break(i-1)-x_break(i-2))*0.5*  eigval.array()
            +(x_break[i-1]+x_break[i-2])*0.5;
        weights.segment((i-2)*k,k) = 2*eigvect.row(0).array().square();

    }

}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "AlgorithmicDifferentiableEisner"

#include <boost/test/unit_test.hpp>
namespace utf = boost::unit_test;

#include "diffdp/algorithm/binary_phrase.h"

// using boost test with intolerance fails (too precise),
// so let's just use the same test as in Dynet.
bool check_grad(float g, float g_act)
{
    float f = std::fabs(g - g_act);
    float m = std::max(std::fabs(g), std::fabs(g_act));
    if (f > 0.01 && m > 0.f)
        f /= m;

    if (f > 0.01 || std::isnan(f))
        return false;
    else
        return true;
}

BOOST_AUTO_TEST_CASE(gradient)
        {
                const unsigned size = 10;
                const float sensitivity = 1e-2;

                diffdp::AlgorithmicDifferentiableBinaryPhraseStructure alg_diff(size);

                // check gradient
                std::vector<float> weights(size * size);
                for (unsigned output_left = 0 ; output_left < size ; ++output_left)
                {
                    for (unsigned output_right = output_left + 1 ; output_right < size ; ++output_right)
                    {
                        for (unsigned input_left = 0 ; input_left < size ; ++input_left)
                        {
                            for (unsigned input_right = input_left + 1; input_right < size; ++input_right)
                            {
                                // compute gradient using the algorithm
                                alg_diff.forward(
                                        [&] (const unsigned left, const unsigned right) -> float
                                        {
                                            return weights.at(left + right * size);
                                        }
                                );
                                alg_diff.backward(
                                        [&] (const unsigned left, const unsigned right) -> float
                                        {
                                            if (left == output_left && right == output_right)
                                                return 1.f;
                                            else
                                                return 0.f;
                                        }
                                );

                                const double computed_gradient = alg_diff.gradient(input_left, input_right);

                                // estimate the gradient
                                const float sensitivity = 1e-3;
                                const double original_weights = weights.at(input_left + input_right * size);

                                weights.at(input_left + input_right * size) = original_weights + sensitivity;
                                alg_diff.forward(
                                        [&] (const unsigned left, const unsigned right) -> float
                                        {
                                            return weights.at(left + right * size);
                                        }
                                );

                                const double output_a = alg_diff.output(output_left, output_right);

                                weights.at(input_left + input_right * size) = original_weights - sensitivity;
                                alg_diff.forward(
                                        [&] (const unsigned left, const unsigned right) -> float
                                        {
                                            return weights.at(left + right * size);
                                        }
                                );
                                const double output_b = alg_diff.output(output_left, output_right);

                                // restore
                                weights.at(input_left + input_right * size) = original_weights;

                                const double estimated_gradient = (output_a - output_b) / (2.f * sensitivity);

                                BOOST_CHECK(check_grad(computed_gradient, estimated_gradient));
                            }
                        }
                    }
                }
        }


#pragma once
#include <Eigen/Dense>
#include "Vectors.hpp"

namespace mdk {
    /// A list of scalars (i.e. doubles).
    using Scalars = Eigen::VectorXd;

    /// A vector (writing \p Eigen::Vector3d everywhere is annoying).
    using Vector = Eigen::Vector3d;

    /**
     * Constant reference to a vector. Given the size of a Vector, passing
     * copies wouldn't be inefficient; it's here chiefly to hide the warnings.
     */
    using VRef = Vector const&;

    /**
     * List of small integers (usually we use this to store various enums
     * and true/false values).
     */
    using Bytes = std::vector<int8_t>;

    using Integers = std::vector<int>;

    /// List of pairs (used in storing VL pairs).
    using Pairs = std::vector<std::pair<int, int>>;
}


#include "cnn/cnn.h"
#include "cnn/training.h"
#include "cnn/expr.h"
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/algorithm/string.hpp>

#include <sys/types.h>
#include <sys/wait.h>
#include <sys/shm.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <utility>
#include <sstream>
#include <random>

using namespace std;
using namespace cnn;
using namespace cnn::expr;

struct SharedObject {
  cnn::real m;
  cnn::real b;
  cnn::real loss;

  cnn::real temp_m;
  cnn::real temp_b;
};

typedef pair<cnn::real, cnn::real> Datum;
const unsigned num_children = 4;
SharedObject* shared_memory = nullptr;

cnn::real ReadReal(int pipe) {
  cnn::real v;
  read(pipe, &v, sizeof(cnn::real));
  return v;
}

void WriteReal(int pipe, cnn::real v) {
  write(pipe, &v, sizeof(cnn::real));
}

template <typename T>
void WriteIntVector(int pipe, const vector<T>& vec) {
  unsigned length = vec.size();
  write(pipe, &length, sizeof(unsigned));
  for (T v : vec) {
    write(pipe, &v, sizeof(T));
  }
}

template<typename T>
vector<T> ReadIntVector(int pipe) {
  unsigned length;
  read(pipe, &length, sizeof(unsigned));
  vector<T> vec(length);
  for (unsigned i = 0; i < length; ++i) {
    read(pipe, &vec[i], sizeof(T));
  }
  return vec;
}

cnn::real Mean(const vector<cnn::real>& values) {
  return accumulate(values.begin(), values.end(), 0.0) / values.size();
}

struct Workload {
  pid_t pid;
  int c2p[2]; // Child to parent pipe
  int p2c[2]; // Parent to child pipe
};

struct ModelParameters {
  Parameters* m;
  Parameters* b;
};

void BuildComputationGraph(ComputationGraph& cg, ModelParameters& model_parameters, cnn::real* x_value, cnn::real* y_value) {
  Expression m = parameter(cg, model_parameters.m);
  Expression b = parameter(cg, model_parameters.b);

  Expression x = input(cg, x_value);
  Expression y_star = input(cg, y_value);
  Expression y = m * x + b;
  Expression loss = squared_distance(y, y_star);
}

vector<Datum> ReadData(string filename) {
  vector<Datum> data;
  ifstream fs(filename);
  if (!fs.is_open()) {
    cerr << "ERROR: Unable to open " << filename << endl;
    exit(1);
  }
  string line;
  while (getline(fs, line)) {
    if (line.size() > 0 && line[0] == '#') {
      continue;
    }
    vector<string> parts;
    boost::split(parts, line, boost::is_any_of("\t"));
    data.push_back(make_pair(atof(parts[0].c_str()), atof(parts[1].c_str())));
  }
  return data;
}

unsigned SpawnChildren(vector<Workload>& workloads) {
  assert (workloads.size() == num_children);
  pid_t pid;
  unsigned cid;
  for (cid = 0; cid < num_children; ++cid) {
    pid = fork();
    if (pid == -1) {
      cerr << "Fork failed. Exiting ...";
      return 1;
    }
    else if (pid == 0) {
      // children shouldn't continue looping
      break;
    }
    workloads[cid].pid = pid;
  }
  return cid;
}

int RunChild(unsigned cid, ComputationGraph& cg, Trainer* trainer, vector<Workload>& workloads,
    const vector<Datum>& data, cnn::real& x_value, cnn::real& y_value, ModelParameters& model_params) {
  assert (cid >= 0 && cid < num_children);
  while (true) {
    // Check if the parent wants us to exit
    bool cont = false;
    read(workloads[cid].p2c[0], &cont, sizeof(bool));
    if (!cont) {
      break;
    }

    // Read in our workload and update our local model
    vector<unsigned> indices = ReadIntVector<unsigned>(workloads[cid].p2c[0]);

    TensorTools::SetElements(model_params.m->values, {shared_memory->m});
    TensorTools::SetElements(model_params.b->values, {shared_memory->b});

    cnn::real loss = 0;
    for (unsigned i : indices) {
      assert (i < data.size());
      auto p = data[i];
      x_value = get<0>(p);
      y_value = get<1>(p);
      loss += as_scalar(cg.forward());
      cg.backward();
      trainer->update(1.0);
    }
    loss /= indices.size();

    // Get our final values of each parameter and send them back to the parent,
    // along with the current loss value
    cnn::real m = as_scalar(model_params.m->values);
    cnn::real b = as_scalar(model_params.b->values);
    shared_memory->temp_m += m;
    shared_memory->temp_b += b;
    shared_memory->loss += loss;

    /*write(workloads[cid].c2p[1], (char*)&m, sizeof(cnn::real));
    write(workloads[cid].c2p[1], (char*)&b, sizeof(cnn::real));
    write(workloads[cid].c2p[1], (char*)&loss, sizeof(cnn::real));*/
    WriteReal(workloads[cid].c2p[1], 0.0);
  }
  return 0;
}

void RunParent(vector<Datum>& data, vector<Workload>& workloads, ModelParameters& model_params, Trainer* trainer) {
  shared_memory->m = TensorTools::AccessElement(model_params.m->values, {0, 0});
  shared_memory->b = TensorTools::AccessElement(model_params.b->values, {0, 0});

  for (unsigned iter = 0; iter < 10; ++iter) {
    shared_memory->loss = 0.0;
    shared_memory->temp_m = 0.0;
    shared_memory->temp_b = 0.0;
    /*vector<cnn::real> m_values;
    vector<cnn::real> b_values;
    vector<cnn::real> loss_values;*/
    for(unsigned cid = 0; cid < num_children; ++cid) {
      unsigned start = (unsigned)(1.0 * cid / num_children * data.size() + 0.5);
      unsigned end = (unsigned)(1.0 * (cid + 1) / num_children * data.size() + 0.5);
      vector<unsigned> indices;
      indices.reserve(end - start);
      for (unsigned i = start; i < end; ++i) {
        indices.push_back(i);
      }
      bool cont = true;
      write(workloads[cid].p2c[1], &cont, sizeof(bool));
      WriteIntVector(workloads[cid].p2c[1], indices);

      /*cnn::real m = ReadReal(workloads[cid].c2p[0]);
      cnn::real b = ReadReal(workloads[cid].c2p[0]);
      cnn::real loss = ReadReal(workloads[cid].c2p[0]);
      m_values.push_back(m);
      b_values.push_back(b);
      loss_values.push_back(loss);*/
    }

    for(unsigned cid = 0; cid < num_children; ++cid) {
      ReadReal(workloads[cid].c2p[0]);
    }

    /*cnn::real m = Mean(m_values);
    cnn::real b = 0.0;
    cnn::real loss = 0.0;
    for (unsigned i = 0; i < m_values.size(); ++i) {
      b += b_values[i];
      loss += loss_values[i];
    }

    b /= b_values.size();*/

    shared_memory->m = shared_memory->temp_m / num_children;
    shared_memory->b = shared_memory->temp_b / num_children;

    // Update parameters to use the new m and b values
    //TensorTools::SetElements(model_params.m->values, {m});
    //TensorTools::SetElements(model_params.b->values, {b});
    trainer->update_epoch();
    //cerr << shared_memory->m << "\t" << iter << "\t" << "loss = " << loss << "\tm = " << m << "\tb = " << b << endl;
    cerr << iter << "\t" << "loss = " << shared_memory->loss << "\tm = " << shared_memory->m << "\tb = " << shared_memory->b << endl;
  }

  // Kill all children one by one and wait for them to exit
  for (unsigned cid = 0; cid < num_children; ++cid) {
    bool cont = false;
    write(workloads[cid].p2c[1], &cont, sizeof(cont));
    wait(NULL);
  }
}

int main(int argc, char** argv) {
  cnn::Initialize(argc, argv);

  if (argc < 2) {
    cerr << "Usage: " << argv[0] << " data.txt" << endl;
    cerr << "Where data.txt contains tab-delimited pairs of cnn::reals." << endl;
    return 1;
  }
  vector<Datum> data = ReadData(argv[1]);
  vector<Workload> workloads(num_children);

  Model model;
  AdamTrainer sgd(&model, 0.0);

  ComputationGraph cg;
  cnn::real x_value, y_value;
  Parameters* m_param = model.add_parameters({1, 1});
  Parameters* b_param = model.add_parameters({1});
  ModelParameters model_params = {m_param, b_param};
  BuildComputationGraph(cg, model_params, &x_value, &y_value);

  unsigned shm_size = 1024;
  assert (sizeof(SharedObject) < shm_size);
  key_t shm_key = ftok("/home/austinma/shared", 'R');
  if (shm_key == -1) {
    cerr << "Unable to get shared memory key" << endl;
    return 1;
  }
  int shm_id = shmget(shm_key, shm_size, 0644 | IPC_CREAT);
  if (shm_id == -1) {
    cerr << "Unable to create shared memory" << endl;
    return 1;
  }
  void* shm_p = shmat(shm_id, nullptr, 0);
  if (shm_p == (void*)-1) {
    cerr << "Unable to get shared memory pointer";
    return 1;
  }
  shared_memory = (SharedObject*)shm_p;

  for (unsigned cid = 0; cid < num_children; cid++) {
    pipe(workloads[cid].p2c);
    pipe(workloads[cid].c2p);
  }

  unsigned cid = SpawnChildren(workloads);
  if (cid < num_children) {
    return RunChild(cid, cg, &sgd, workloads, data, x_value, y_value, model_params);
  }
  else {
    RunParent(data, workloads, model_params, &sgd);
  }
}


#include "CALPHADFreeEnergyFunctionsBinaryThreePhase.h"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

namespace pt = boost::property_tree;

int main(int argc, char* argv[])
{
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::LINEAR;

    // very small temperature to have a very small contribution of
    // pure mixing term
    // Cannot be 0 as it would trigger nans in f(T) evaluations
    double temperature = 1.e-8;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphad3phases.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinaryThreePhase cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    const int npts = 100;
    std::ofstream os("FvsC.dat", std::ios::out);

    std::cout << " Compute energies..." << std::endl;
    cafe.printEnergyVsComposition(temperature, os, npts);
}


#include <GAGPL/GAGLIBRARY/SequenceSpace.h>
#include <GAGPL/CHEMISTRY/Modification.h>
#include <GAGPL/MATH/combination.h>
#include <iostream>
#include <algorithm>
#include <boost/lexical_cast.hpp>

using namespace std;

namespace gag
{

	SequenceList SequenceSpace::generateModificationCombination()
	{
		SequenceList seq_list;

		// 0. Sequentially list the modification symbol.
		set<string> mod_sym_set = gs->getModificationTypes();

		set<string>::iterator iter = mod_sym_set.begin();

		// 0.1 Get available modification sites.

		this->nextModificationSymbol(mod_sym_set, iter, seq_list, ModificationSequence(), gs->getModificationMap());

		return seq_list;
	}

	void SequenceSpace::nextModificationSymbol(set<string>& mod_set, set<string>::iterator iter, SequenceList& seq_list, ModificationSequence mod_seq, Modifier mod_manager)
	{
		// Get currently available modification sites.
		ModificationSites current_sites = mod_manager.getModificationSitesBySymbol(*iter, 1);
		size_t mod_num = (size_t)gs->getModificationConstraint(*iter);

		//ModificationSites mod_sites;
    size_t i = 0;

    // Non-const version of the container.
    std::vector<ModificationPosition> current_vec(current_sites.begin(), current_sites.end());
    std::vector<ModificationPosition> mod_vec;

		for(ModificationSites::iterator mod_iter = current_sites.begin(); mod_iter != current_sites.end(); mod_iter++)
		{
			mod_vec.push_back(*mod_iter);
			if(++i >= mod_num)
				break;
		}

    //ModificationSites::iterator CurIter(current_sites.begin());

		do
		{
			// Set the status of mod_sites to "occupied".
			Modifier new_manager(mod_manager);
			for(auto mod_iter = mod_vec.begin(); mod_iter != mod_vec.end(); mod_iter++)
			{
				new_manager.modifyModificationStatus(*iter, *mod_iter, 0);
			}
			// Select the current combination, set the status and move to the next symbol.
			ModificationSequence current_seq(mod_seq);
      ModificationSites mod_sites(mod_vec.begin(), mod_vec.end());
			current_seq.insert(std::make_pair(*iter, mod_sites));

			set<std::string>::iterator it_copy = iter;
			it_copy++;
			if(it_copy == mod_set.end()) {
				seq_list.push_back(current_seq);
				continue;
			}
			this->nextModificationSymbol(mod_set, it_copy, seq_list, current_seq, new_manager);
		} while(stdcomb::next_combination(current_vec.begin(), current_vec.end(), mod_vec.begin(), mod_vec.end()));

	}

  bool SequenceSpace::nextModificationSymbol( set<string>& mod_set, std::set<string>::iterator mod_iter, ModificationSites prev_occupied_sites )
  {
    if(mod_iter == mod_set.end()) return false;

    // Generate available candidate sites.
    std::vector<ModificationPosition>& candidate_sites = container_seq.at(*mod_iter);
    // Current occupied sites.
    std::vector<ModificationPosition>& occupied_sites = prototype_seq.at(*mod_iter);

    // Not end.
    bool status = nextModificationSymbol(mod_set, std::next(mod_iter, 1), prev_occupied_sites);
    if(!status) { // Can not move deeper.
      if(stdcomb::next_combination(candidate_sites.begin(), candidate_sites.end(),occupied_sites.begin(), occupied_sites.end()))
      {
        // Update prev_occupied_sites.
        std::copy(occupied_sites.begin(), occupied_sites.end(), std::inserter(prev_occupied_sites, prev_occupied_sites.end()));

        // New sequence. Update container_seq and prototype_seq.
        this->updateSites(mod_set, std::next(mod_iter, 1), prev_occupied_sites);

        // Update occupied sites.
        //std::copy(occupied_sites.begin(), occupied_sites.end(), std::inserter(prev_occupied_sites, prev_occupied_sites.end()));

        //bool status = nextModificationSymbol(mod_set, std::next(mod_iter, 1), prev_occupied_sites);
        return true;
      } else { // Can not move further.
        // Return to last level.
        return false;
      }
    } else {
      return true;
    }
  }

	std::string SequenceSpace::getSequenceString( const ModificationSequence& mod_seq )
	{
		SequenceCode seq_code = getSequenceCode(mod_seq);

		std::string seq_str;
		SequenceCode::iterator iter = seq_code.begin();
		for(; iter!=seq_code.end(); iter++)
		{
			seq_str.append(iter->first);
			seq_str.append(boost::lexical_cast<std::string>(iter->second));
		}
		return seq_str;

	}

	SequenceCode SequenceSpace::getSequenceCode( const ModificationSequence& mod_seq )
	{
		SequenceCode seq_code;

		std::string str;

		Branch& bc = gs->getBranchByID(0);

		ModificationSequence::const_iterator iter = mod_seq.find("SO3");
		const ModificationSites& s_sites = iter->second;

		for(size_t i = 0; i < bc.getUnitNum(); i++)
		{
			std::string symbol = bc.getUnitByID(i).getSymbol();

			if(symbol == "DeltaGlcA") {
				str = "D";
			} else if(symbol == "GlcA") {
				str = "G";
			} else if(symbol == "GlcN") { // A, S or H.
				// Check the 2-N position.
				ModificationPosition mod_pos(0, i, 2);
				ModificationSequence::const_iterator ac_iter = mod_seq.find("Ac");
				ModificationSites::const_iterator s_pos_iter = s_sites.find(mod_pos);

				if(ac_iter != mod_seq.end()) {

					const ModificationSites& ac_sites = ac_iter->second;

					ModificationSites::const_iterator ac_pos_iter = ac_sites.find(mod_pos);

					if(ac_pos_iter != ac_sites.end())
						str = "A";
					else if(s_pos_iter != s_sites.end())
						str = "S";
					else {
						str = "H";
					}
				} else {
					if(s_pos_iter != s_sites.end())
						str = "S";
					else {
						str = "H";
					}
				}

			} else if(symbol == "GlcNAc") {
				str = "A";
			} else {
				str = "X"; // Unknown type.
			}
			if(iter == mod_seq.end()) {
				seq_code.push_back(std::make_pair(str,0));
				continue;
			}

			size_t num = 0;

			// Get the number of SO3 on the ring.
			if(str == "D" || str == "G") {
				ModificationSites::const_iterator pos_iter = s_sites.find(ModificationPosition(0, i, 2));
				if(pos_iter != s_sites.end()) {
					num = 2;
				}
			} else if(str == "H" || str == "S" || str == "A") {
				for(size_t j = 3; j < bc.getUnitByID(i).getInternalSites().size(); j++)
				{
					ModificationSites::const_iterator pos_iter = s_sites.find(ModificationPosition(0, i, j));
					if(pos_iter != s_sites.end())
						num += pos_iter->site_id;
				}
			} else {
				// Do nothing.
			}

			seq_code.push_back(std::make_pair(str,num));

		}

		return seq_code;
	}

	MergedCode SequenceSpace::getMergedCode( const ModificationSequence& mod_seq )
	{
		MergedCode merged_code;

		std::string str;

		Branch& bc = gs->getBranchByID(0);

		ModificationSequence::const_iterator iter = mod_seq.find("SO3");
		const ModificationSites& s_sites = iter->second;

		for(size_t i = 0; i < bc.getUnitNum(); i++)
		{
			std::string symbol = bc.getUnitByID(i).getSymbol();

			if(symbol == "DeltaGlcA") {
				str = "D";
			} else if(symbol == "GlcA") {
				str = "G";
			} else if(symbol == "GlcN") { // A, S or H.
				// Check the 2-N position.
				ModificationPosition mod_pos(0, i, 2);
				ModificationSequence::const_iterator ac_iter = mod_seq.find("Ac");
				ModificationSites::const_iterator s_pos_iter = s_sites.find(mod_pos);

				if(ac_iter != mod_seq.end()) {

					const ModificationSites& ac_sites = ac_iter->second;

					ModificationSites::const_iterator ac_pos_iter = ac_sites.find(mod_pos);

					if(ac_pos_iter != ac_sites.end())
						str = "A";
					/*else if(s_pos_iter != s_sites.end())
						str = "S";*/
					else {
						str = "H";
					}
				} else {
					//if(s_pos_iter != s_sites.end())
					//	str = "S";
					//else {
					//	str = "H";
					//}
					str = "H";
				}

			} else if(symbol == "GlcNAc") {
				str = "A";
			} else {
				str = "X"; // Unknown type.
			}
			if(iter == mod_seq.end()) {
				merged_code.push_back(std::make_pair(str,0));
				continue;
			}

			size_t num = 0;

			// Get the number of SO3 on the ring.
			if(str == "D" || str == "G") {
				ModificationSites::const_iterator pos_iter = s_sites.find(ModificationPosition(0, i, 2));
				if(pos_iter != s_sites.end()) {
					num = 1;
				}
			} else if(str == "H" || str == "A") {
				for(size_t j = 1; j < bc.getUnitByID(i).getInternalSites().size(); j++)
				{
					ModificationSites::const_iterator pos_iter = s_sites.find(ModificationPosition(0, i, j));
					if(pos_iter != s_sites.end())
						num += 1;
				}
			} else {
				// Do nothing.
			}

			merged_code.push_back(std::make_pair(str,num));

		}

		return merged_code;
	}

	std::string SequenceSpace::getMergedCodeString( const ModificationSequence& mod_seq )
	{
		SequenceCode seq_code = getMergedCode(mod_seq);

		std::string seq_str;
		SequenceCode::iterator iter = seq_code.begin();
		for(; iter!=seq_code.end(); iter++)
		{
			seq_str.append(iter->first);
			seq_str.append(boost::lexical_cast<std::string>(iter->second));
		}
		return seq_str;
	}

  ModificationSequence SequenceSpace::nextModificationSequence()
  {
    // For the initial situation.
    if(container_seq.size() == 0 && prototype_seq.size() == 0) {
      this->initializeModificationSequence();
      return this->getCurrentSequence();
    } else {
      std::set<std::string> mod_set = gs->getModificationTypes();

      bool flag = this->nextModificationSymbol(mod_set, mod_set.begin(), ModificationSites());

      if(flag) // Get new sequence.
        return this->getCurrentSequence();
      else // The end of enumeration.
        return ModificationSequence();
    }
  }

  void SequenceSpace::updateSites( set<string>& mod_set, std::set<string>::iterator mod_iter, ModificationSites prev_occupied_sites )
  {
    if(mod_iter == mod_set.end()) return;

    // Total candidate sites.
    ModificationSites total_sites = gs->getModificationSitesBySymbol(*mod_iter, 1);
    ModificationSites available_sites = getSiteDifference(total_sites, prev_occupied_sites);

    std::vector<ModificationPosition> available_vec(available_sites.begin(), available_sites.end());
    container_seq[*mod_iter] = available_vec;

    int mod_num = gs->getModificationConstraint(*mod_iter);

    // Select the first n sites for the prototype_seq.
    std::vector<ModificationPosition> initial_vec(available_vec.begin(), std::next(available_vec.begin(), std::min((size_t)mod_num, available_vec.size())));

#ifdef _DEBUG
  if(((size_t)mod_num > available_vec.size()))
  {
    std::cout << *mod_iter << " Not correct!\n";
    std::cout << "Mod number: " << mod_num;
    printModificationSites(available_sites);
  }
#endif // _DEBUG

    prototype_seq[*mod_iter] = initial_vec;

    // Add the current choice into pre_occupied_sites.
    std::copy(initial_vec.begin(), initial_vec.end(), std::inserter(prev_occupied_sites, prev_occupied_sites.end()));
    // Go to a deeper level.
    this->updateSites(mod_set, std::next(mod_iter, 1), prev_occupied_sites);

    return;
  }

  void SequenceSpace::initializeModificationSequence()
  {
    std::set<std::string> mod_set = gs->getModificationTypes();
    this->updateSites(mod_set, mod_set.begin(), ModificationSites());
  }

  ModificationSequence SequenceSpace::getCurrentSequence() const
  {
    ModificationSequence mod_seq;
    for(auto iter = prototype_seq.begin(); iter != prototype_seq.end(); iter++)
    {
      ModificationSites mod_sites(iter->second.begin(), iter->second.end());
      mod_seq.insert(std::make_pair(iter->first, mod_sites));
    }
    return mod_seq;
  }

  //bool SequenceSpace::isQualifiedSequence(const ModificationSequence& mod_seq)
  //{
  //  auto sulfate_iter = mod_seq.find("SO3");
  //  auto ac_iter = mod_seq.find("Ac");

  //  bool decision = true;

  //  // No sulfate.
  //  if(sulfate_iter == mod_seq.end()) return decision;

  //  // MacroPosition =>
  //  //    set of site_id
  //  std::map<MacroPosition, std::map<size_t, ModificationDistribution::iterator>> residue_sulfate;

  //  // Iterate over all sulfated sites, storing the information into residue_sulfate.
  //  for(auto site_iter = sulfate_iter->second.begin(); site_iter != sulfate_iter->second.end(); site_iter++)
  //  {
  //    residue_sulfate[site_iter->macro_pos].insert(std::make_pair(site_iter->site_id, site_iter));
  //  }

  //
  //  for(auto residue_iter = residue_sulfate.begin(); residue_iter != residue_sulfate.end(); residue_iter++)
  //  {
  //    // Check the type of the residue.
  //    std::string residue_type = gs->getBranchByID(residue_iter->first.branch_id).getUnitByID(residue_iter->first.mono_id).getSymbol();

  //    if(residue_type == "DeltaGlcA" || residue_type == "GlcA") continue;

  //    if(residue_type == "GlcN" || residue_type == "GlcNAc") {
  //      auto third_iter = residue_iter->second.find(3);
  //      auto sixth_iter = residue_iter->second.find(6);
  //      auto second_iter = residue_iter->second.find(2);
  //    } else  {
  //      cout << residue_type << "\n";
  //      throw std::runtime_error("Undefined residue type!");
  //    }
  //  }
  //
  //  return decision;
  //}

}