//
// Created by samjjx on 2018/12/17.
//

#include <cmath>

#include <iostream>
#include <random>
#include <thread>
#include <chrono>

#include "gflags/gflags.h"
#include "glog/logging.h"

#include "driver/engine.hpp"
#include "lib/abstract_data_loader.hpp"
#include "lib/labeled_sample.hpp"
#include "lib/parser.hpp"

#include <boost/algorithm/string.hpp>
#include <boost/range/algorithm_ext.hpp>
#include <boost/range/irange.hpp>
#include <fstream>
#include <lib/svm_loader.hpp>

#include "SVM.hpp"

using namespace csci5570;

DEFINE_string(config_file, "", "The config file path");
DEFINE_string(my_id, "", "Local node id");
DEFINE_string(input, "", "The hdfs input url");

void get_nodes_from_config(std::string config_file, std::vector<Node>& nodes) {
  std::ifstream infile(config_file);
  std::string line;
  while (std::getline(infile, line)) {
    std::vector<std::string> cols;
    boost::split(cols, line, [](char c){return c == ':';});
    uint32_t node_id = std::stoi(cols[0]);
    std::string host_name = cols[1];
    boost::trim(host_name);
    int port = std::stoi(cols[2]);
    nodes.push_back(Node({node_id, host_name, port}));
  }
}

int main(int argc, char** argv) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);
  FLAGS_stderrthreshold = 0;
  FLAGS_colorlogtostderr = true;
  using DataStore = std::vector<lib::SVMSample>;
  DataStore data_store;
  DataStore data_store_backup;

  LOG(INFO) << FLAGS_config_file;
  LOG(INFO) << FLAGS_my_id;
  LOG(INFO) << FLAGS_input;

  std::vector<Node> nodes;
  get_nodes_from_config(FLAGS_config_file, nodes);
  uint32_t my_id = std::stoi(FLAGS_my_id);
  auto node = std::find_if(nodes.begin(), nodes.end(), [my_id](Node& n){return n.id == my_id;});
  if (node == nodes.end()) {
    LOG(INFO) << "My_id not in nodes list.";
    return -1;
  }

  Engine engine(*node, nodes);

  // 1. Start system
  engine.StartEverything();

  /*
   * Begin IO config
   */
//  std::string url = "hdfs:///datasets/classification/a9";
  std::string url = FLAGS_input;
  std::string hdfs_namenode = "proj10";                        // Do not change
  std::string master_host = "proj10";  // Set to worker name
  std::string worker_host = node->hostname;  // Set to worker name
  uint32_t id = node->id;
  int hdfs_namenode_port = 9000;
  int master_port = 19817;  // use a random port number to avoid collision with other users
  const uint32_t n_features = 100;
  /*
   * End IO config
   */

  lib::Parser<lib::SVMSample, DataStore> parser;
  lib::DataLoader<lib::SVMSample, DataStore> data_loader;
  engine.Barrier();

  data_loader.load(url, hdfs_namenode, master_host, worker_host, hdfs_namenode_port, master_port, n_features,
                   parser, &data_store, id, nodes.size());

  // 1.1 Create table
//  const auto kTableId = engine.CreateTable<double>(ModelType::SSP, StorageType::Map, 2);  // table 0
  const auto kTableId = engine.CreateTable<double>(ModelType::BSP, StorageType::Map);  // table 0

  // 1.2 Load data
  engine.Barrier();

  // 2. Start training task
  MLTask task;

  std::vector<WorkerAlloc> worker_alloc;
  for (auto node : nodes) {
    worker_alloc.push_back({node.id, 2});  // node_id, worker_num
  }
  task.SetWorkerAlloc(worker_alloc);
  task.SetTables({kTableId});     // Use table 0

  task.SetLambda([kTableId, &data_store](const Info& info) {
//    LOG(INFO) << info.DebugString();

      // the maximum data range used by current worker

      SVM<double> svm(&data_store);

      std::vector<Key> keys;
      svm.get_keys(keys);

      KVClientTable<double> table = info.CreateKVClientTable<double>(kTableId);

      for (int i = 0; i < 10e2; ++i) {
        // parameters from server
        std::vector<double> theta;
        table.Get(keys, &theta);

        auto correct_ratio = svm.correct_rate(data_store, keys, theta);

        auto res =  svm.compute_gradients(data_store, keys, theta, 0.1);

        table.Add(keys, res);
        LOG(INFO) << "Correct Ratio\t" << correct_ratio;
        table.Clock();
      }
      // algorithm helper
  });
  auto start_time = std::chrono::system_clock::now();
  engine.Run(task);
  auto end_time = std::chrono::system_clock::now();
  LOG(INFO) << "Task completed in " << std::chrono::duration_cast<std::chrono::seconds>(end_time - start_time).count();

  // 3. Stop
  engine.StopEverything();

  return 0;
}


#ifndef SIG_POLAR_SPIN_HPP
#define SIG_POLAR_SPIN_HPP

#include "signlp.hpp"
#include "SigUtil/lib/functional/fold.hpp"
#include "SigUtil/lib/calculation.hpp"
#include <boost/graph/adjacency_list.hpp>

namespace signlp
{

class SpinModel
{
	struct Node
	{
		std::wstring word;
		uint degree;

		bool has_label;
		double label;

		double mean_x;
		double tmp_mean_x;
	};

public:
	using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
		Node,
		boost::property<boost::edge_weight_t, double>
	>;
	using pNode = Graph::vertex_descriptor;
	using pEdge = Graph::edge_descriptor;

	static auto make_node(Graph& g, std::wstring word) ->pNode
	{
		auto v = boost::add_vertex(g);
		g[v].word = word;
		g[v].has_label = false;
		return v;
	}
	static auto make_node(Graph& g, std::wstring word, bool label) ->pNode
	{
		auto v = boost::add_vertex(g);
		g[v].word = word;
		g[v].label = label ? 1 : -1;
		g[v].has_label = true;
		return v;
	}
	static auto make_node(Graph& g, std::wstring word, double label) ->pNode
	{
		auto v = boost::add_vertex(g);
		g[v].word = word;
		g[v].label = label;
		g[v].has_label = true;
		return v;
	}

	static void make_edge(Graph& g, pNode v1, pNode v2, double weight)
	{
		auto e = add_edge(v1, v2, g);
		put(boost::edge_weight, g, e.first, weight);
	};

private:
	Graph graph_;

	const double alpha_;	// ラベル(正解)の反映度
	const double beta_;		// 逆温度

	const sig::array<int, 2> xs_;
	std::unordered_map<pNode, std::vector<pNode>> adj_;
	sig::SimpleRandom<double> rand_d_;

private:
	void init()
	{
		auto nodes = boost::vertices(graph_);

		std::for_each(
			boost::begin(nodes),
			boost::end(nodes),
			[&](pNode n){
				std::vector<pNode> tmp;
				auto adj = adjacent_vertices(n, graph_);
				for (auto it = adj.first, end = adj.second; it != end; ++it){
					tmp.push_back(*it);
				}

				graph_[n].degree = tmp.size();
				adj_.emplace(n, std::move(tmp));

				if (graph_[n].has_label){
					graph_[n].mean_x = graph_[n].label;
					graph_[n].tmp_mean_x = graph_[n].label;
				}
				else{
					double r = rand_d_();
					graph_[n].mean_x = r;
					graph_[n].tmp_mean_x = r;
				}
			}
		);

		auto edges = boost::edges(graph_);

		std::for_each(
			boost::begin(edges),
			boost::end(edges),
			[&](pEdge e){
				auto v1 = boost::source(e, graph_);
				auto v2 = boost::target(e, graph_);
				auto adjn1 = graph_[v1].degree;
				auto adjn2 = graph_[v2].degree;

				double decay = std::sqrt(adjn1 * adjn2);
				auto t = boost::get(boost::get(boost::edge_weight, graph_), e);
				boost::get(boost::get(boost::edge_weight, graph_), e) /= decay;
			}
		);
	}

	void update(uint k)
	{
		const double wx_sum = sig::dotProduct(
			std::plus<double>(),
			[&](uint n){ return graph_[n].mean_x * boost::get(boost::get(boost::edge_weight, graph_), boost::edge(k, n, graph_).first); },
			0, adj_[k]
		);

		const auto exp_wx_sum = sig::map([&](int x_i){	return std::exp(beta_ * x_i * wx_sum); }, xs_);

		auto t = sig::dotProduct(std::plus<double>(), std::multiplies<double>(), 0, exp_wx_sum, xs_) / sig::sum(exp_wx_sum);

		if (!sig::is_number(t)){
			std::cout << "wxsum:" << wx_sum << std::endl;
			std::cout << "exp_wx_sum0:" << exp_wx_sum[0] << std::endl;
			std::cout << "exp_wx_sum1:" << exp_wx_sum[1] << std::endl;
			getchar();
		}

		graph_[k].tmp_mean_x = t;
	}

	void updateL(uint k)
	{
		if (adj_[k].empty()) return;
		/*const double wx_sum = sig::fold_zipWith(
			std::multiply<double>(),
			std::plus<double>(),
			0, w_[k], mean_x_
		);*/
		const double wx_sum = sig::dotProduct(
			std::plus<double>(),
			[&](uint n){ return graph_[n].mean_x * boost::get(boost::get(boost::edge_weight, graph_), boost::edge(k, n, graph_).first); },
			0, adj_[k]
		);

		const auto exp_wx_sum = sig::map([&](int x_i){	return std::exp(beta_ * x_i * wx_sum - alpha_ * std::pow(x_i - graph_[k].label, 2)); }, xs_);

		auto t = sig::dotProduct(std::plus<double>(), std::multiplies<double>(), 0, exp_wx_sum, xs_) / sig::sum(exp_wx_sum);

		if (!sig::is_number(t)){
			std::cout << "wxsum:" << wx_sum << std::endl;
			std::cout << "exp_wx_sum0:" << exp_wx_sum[0] << std::endl;
			std::cout << "exp_wx_sum1:" << exp_wx_sum[1] << std::endl;
			getchar();
		}

		graph_[k].tmp_mean_x = t;
	}

public:
	SpinModel(Graph const& graph, double alpha, double beta) : graph_(graph), alpha_(alpha), beta_(beta), xs_({ -1, 1 }), rand_d_(-1.0, 1.0, true)
	{
		init();
	}

	void train(uint iteration_num, std::function<void(SpinModel const*)> callback)
	{
		for (uint i = 0; i < iteration_num; ++i){
			auto nodes = boost::vertices(graph_);

			std::for_each(
				boost::begin(nodes),
				boost::end(nodes),
				[&](pNode n){ graph_[n].has_label ? updateL(n) : update(n); }
			);

			std::for_each(
				boost::begin(nodes),
				boost::end(nodes),
				[&](pNode n){ graph_[n].mean_x = graph_[n].tmp_mean_x; }
			);

			callback(this);
		}
	}

	auto getScore(std::wstring word) const->sig::Maybe<double>
	{
		auto nodes = boost::vertices(graph_);

		for(auto it = nodes.first, end = nodes.second; it != end; ++it){
			if(graph_[*it].word == word) return graph_[*it].mean_x;
		}
		return boost::none;
	}

	auto getScore() const->std::unordered_map<std::wstring, double>
	{
		std::unordered_map<std::wstring, double> result;
		auto nodes = boost::vertices(graph_);

		for (auto it = nodes.first, end = nodes.second; it != end; ++it){
			result.emplace(graph_[*it].word, graph_[*it].mean_x);
		}
		return result;
	}

	// leave-one-out error
	double getErrorRate() const
	{
		int ct = 0;
		double sum = 0;
		auto nodes = boost::vertices(graph_);

		std::for_each(
			boost::begin(nodes),
			boost::end(nodes),
			[&](pNode n){
				if(graph_[n].has_label){
					sum += (graph_[n].label * graph_[n].mean_x < 0 ? 1 : 0);
					++ct;
				}
			}
		);

		return sum / ct;
	}

	double getMeanPolar() const
	{
		int ct = 0;
		double sum = 0;
		auto nodes = boost::vertices(graph_);

		std::for_each(
			boost::begin(nodes),
			boost::end(nodes),
			[&](pNode n){
				sum += graph_[n].mean_x;
				++ct;
			}
		);

		return sum / ct;
	}
};

}
#endif


#include <armadillo>
#include <boost/program_options.hpp>
#include <json.hpp>
#include <HSMM.hpp>
#include <ProMPs_emission.hpp>

using namespace arma;
using namespace hsmm;
using namespace robotics;
using namespace std;
namespace po = boost::program_options;

field<mat> fromMatToField(const mat& obs) {
    field<mat> ret(obs.n_cols);
    for(int i = 0; i < obs.n_cols; i++)
        ret(i) = obs.col(i);
    return ret;
}

// Running the Viterbi algorithm.
void ViterbiAlgorithm(HSMM& promp_hsmm, const field<field<mat>>& seq_obs,
        string filename) {
    int nseq = seq_obs.n_elem;
    for(int s = 0; s < nseq; s++) {
        const field<mat>& obs = seq_obs(s);
        int nstates = promp_hsmm.nstates_;
        int nobs = obs.n_elem;
        imat psi_duration(nstates, nobs, fill::zeros);
        imat psi_state(nstates, nobs, fill::zeros);
        mat delta(nstates, nobs, fill::zeros);
        cout << "Before pdf" << endl;
        cube log_pdf = promp_hsmm.computeEmissionsLogLikelihood(obs);
        cout << "After pdf" << endl;
        Viterbi(promp_hsmm.transition_, promp_hsmm.pi_, promp_hsmm.duration_,
                log_pdf, delta, psi_duration, psi_state,
                promp_hsmm.min_duration_, nobs);
        cout << "Delta last column" << endl;
        cout << delta.col(nobs - 1) << endl;
        ivec viterbiStates, viterbiDurations;
        viterbiPath(psi_duration, psi_state, delta, viterbiStates,
                viterbiDurations);
        cout << "Viterbi states and durations" << endl;
        imat states_and_durations = join_horiz(viterbiStates, viterbiDurations);
        cout << states_and_durations << endl;
        cout << "Python states list representation" << endl;
        cout << "[";
        for(int i = 0; i < viterbiStates.n_elem; i++)
            cout << viterbiStates[i] <<
                    ((i + 1 == viterbiStates.n_elem)? "]" : ",");
        cout << endl;
        cout << "Python duration list representation" << endl;
        cout << "[";
        for(int i = 0; i < viterbiDurations.n_elem; i++)
            cout << viterbiDurations[i] <<
                    ((i + 1 == viterbiDurations.n_elem)? "]" : ",");
        cout << endl;

        // Saving the matrix of joint states and durations.
        string viterbi_filename(filename);
        if (nseq > 1)
            viterbi_filename += string(".") + to_string(s);
        states_and_durations.save(viterbi_filename, raw_ascii);
    }
}

int main(int argc, char *argv[]) {
    po::options_description desc("Options");
    desc.add_options()
        ("help,h", "Produce help message")
        ("input,i", po::value<string>(), "Path to the input obs")
        ("output,o", po::value<string>(), "Path to the json output params")
        ("nstates", po::value<int>(), "Number of hidden states for the HSMM")
        ("mindur", po::value<int>(), "Minimum duration of a segment")
        ("ndur", po::value<int>(), "Number of different durations supported")
        ("commitid", po::value<string>(), "Git commit id of the experiment")
        ("labels", po::value<string>(), "Path to the provided labels")
        ("viterbi,v", po::value<string>(), "Path to the output viterbi file")
        ("nfiles", po::value<int>()->default_value(1),
                "Number of files (sequences) to process")
        ("debug", "Flag for activating debug mode in HSMM")
        ("nodur", "Flag to deactivate the learning of durations")
        ("notrans", "Flag to deactivate the learning of transitions")
        ("nopi", "Flag to deactivate the learning of initial pmf")
        ("durmomentmatching", "Flag to active the Gaussian moment matching"
                " for the duration learning")
        ("polybasisfun", po::value<int>()->default_value(1), "Order of the "
                "poly basis functions")
        ("noselftransitions", "Flag to deactive self transitions")
        ("initfraction", po::value<double>()->default_value(0.1), "Fraction "
                "of the least squares estimates for omega kept for init")
        ("wpriorvar", po::value<double>(), "Prior variance for Sigma_w")
        ("alphadurprior", po::value<int>()->default_value(1),
                "Alpha for Dirichlet prior for the duration")
        ("trainingiter", po::value<int>()->default_value(10), "Training "
                "iterations")
        ("rbfbasisfun", po::value<int>()->default_value(3), "Number of radial"
                " basis functions to use between 0 and 1. 0 and 1 are removed.")
        ("delta", po::value<double>(), "If this is given a value, the model "
                "switches to segment agnostic and the samples are generated "
                "according to the provided delta")
        ("leaveoneout", po::value<int>(), "Index of the sequence that will be"
                " left out for validation");
    vector<string> required_fields = {"input", "output", "nstates", "mindur",
            "ndur", "viterbi"};
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    if (vm.count("nodur") && vm.count("durmomentmatching")) {
        cout << "Only one choice for duration learning is allowed" << endl;
        return 0;
    }
    for(auto s: required_fields) {
        if (!vm.count(s)) {
            cerr << "Error: You must provide the argument: " << s << endl;
            return 1;
        }
    }
    string input_filename = vm["input"].as<string>();
    string output_filename = vm["output"].as<string>();
    field<field<mat>> seq_obs(1);
    field<Labels> seq_labels(1);
    int njoints;
    int nseq = vm["nfiles"].as<int>();
    seq_obs.set_size(nseq);
    seq_labels.set_size(nseq);
    for(int i = 0; i < seq_obs.n_elem; i++) {
        string name = input_filename;
        if (nseq != 1)
            name += string(".") + to_string(i);
        mat obs;
        obs.load(name, raw_ascii);
        njoints = obs.n_rows;
        int nobs = obs.n_cols;
        cout << "Time series shape: (" << njoints << ", " << nobs <<
            ")." << endl;
        seq_obs(i) = fromMatToField(obs);

        // Reading labels for different obs.
        if (!vm.count("labels"))
            continue;
        string labels_name = vm["labels"].as<string>();
        if (nseq != 1)
            labels_name += string(".") + to_string(i);
        mat labels_mat;
        labels_mat.load(labels_name);
        for(int j = 0; j < labels_mat.n_rows; j++)
            seq_labels(i).setLabel(labels_mat(j, 0), labels_mat(j, 1),
                    labels_mat(j, 2));
    }

    int min_duration = vm["mindur"].as<int>();
    int nstates = vm["nstates"].as<int>();
    int ndurations = vm["ndur"].as<int>();
    mat transition(nstates, nstates);
    transition.fill(1.0 / nstates );
    if (vm.count("noselftransitions")) {
        transition.fill(1.0 / (nstates - 1));
        transition.diag().zeros();
    }
    vec pi(nstates);
    pi.fill(1.0/nstates);
    mat durations(nstates, ndurations);
    durations.fill(1.0 / ndurations);

    // Setting a combination of polynomial and rbf basis functions.
    auto poly = make_shared<ScalarPolyBasis>(vm["polybasisfun"].as<int>());
    auto comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({poly}));
    int nrbf = vm["rbfbasisfun"].as<int>();
    if (nrbf > 0) {
        vec centers = linspace<vec>(0, 1.0, nrbf + 2);
        centers = centers.subvec(1, centers.n_elem - 2);
        auto rbf = shared_ptr<ScalarGaussBasis>(new ScalarGaussBasis(centers,
                    0.25));
        comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({rbf, poly}));
    }
    int n_basis_functions = comb->dim();

    // Instantiating as many ProMPs as hidden states.
    vector<FullProMP> promps;
    for(int i = 0; i < nstates; i++) {
        vec mu_w(n_basis_functions * njoints);
        mu_w.randn();
        mat Sigma_w = eye<mat>(n_basis_functions * njoints,
                    n_basis_functions * njoints);
        mat Sigma_y = 0.001 * eye<mat>(njoints, njoints);
        ProMP promp(mu_w, Sigma_w, Sigma_y);
        FullProMP poly(comb, promp, njoints);
        promps.push_back(poly);
    }

    // Creating the ProMP emission.
    shared_ptr<ProMPsEmission> ptr_emission(new ProMPsEmission(promps));

    // Creating a prior for Sigma_w.
    if (vm.count("wpriorvar")) {
        mat Phi = vm["wpriorvar"].as<double>() * eye<mat>(
                n_basis_functions * njoints, n_basis_functions * njoints);
        NormalInverseWishart iw_prior(Phi, Phi.n_rows + 2);
        ptr_emission->set_Sigma_w_Prior(iw_prior);
    }
    if (vm.count("delta"))
        ptr_emission->setDelta(vm["delta"].as<double>());

    // Settings for the initialization algorithm.
    ptr_emission->setParamsForInitialization(vm["initfraction"].as<double>());

    HSMM promp_hsmm(std::static_pointer_cast<AbstractEmission>(ptr_emission),
            transition, pi, durations, min_duration);
    if (vm.count("nodur"))
        promp_hsmm.setDurationLearningChoice("nodur");
    if (vm.count("durmomentmatching"))
        promp_hsmm.setDurationLearningChoice("momentmatching");
    if (vm.count("alphadurprior")) {
        mat alphas = ones<mat>(nstates, ndurations) *
            vm["alphadurprior"].as<int>();
        promp_hsmm.setDurationDirichletPrior(alphas);
    }
    if (vm.count("notrans"))
        promp_hsmm.learning_transitions_ = false;
    if (vm.count("nopi"))
        promp_hsmm.learning_pi_ = false;
    if (vm.count("debug"))
        promp_hsmm.debug_ = true;

    // Initializing the model from data.
    promp_hsmm.init_params_from_data(seq_obs);

    // Saving the model in a json file.
    std::ofstream initial_params(output_filename);
    nlohmann::json initial_model = promp_hsmm.to_stream();
    if (vm.count("commitid"))
        initial_model["git_commit_id"] = vm["commitid"].as<string>();
    initial_params << std::setw(4) << initial_model << std::endl;
    initial_params.close();

    // Leave one out.
    field<field<mat>> t_seq;
    field<Labels> t_labels;
    if (vm.count("leaveoneout")) {
        int omitted = vm["leaveoneout"].as<int>();
        field<field<mat>> left_one_out(seq_obs.n_elem - 1);
        field<Labels> left_labels(seq_labels.n_elem - 1);
        int idx = 0;
        for(int i = 0; i < seq_obs.n_elem; i++)
            if (i != omitted) {
                left_one_out(idx) = seq_obs(i);
                left_labels(idx) = seq_labels(i);
                idx++;
            }
        t_seq = left_one_out;
        t_labels = left_labels;
        cout << "Leaving one out of the training: " << omitted << endl;
    }
    else {
        t_seq = seq_obs;
        t_labels = seq_labels;
    }

    for(int i = 0; i < vm["trainingiter"].as<int>(); i++) {

        // Reading the current parameters.
        std::ifstream current_params_stream(output_filename);
        nlohmann::json current_params;
        current_params_stream >> current_params;
        promp_hsmm.from_stream(current_params);

        bool convergence_reached = promp_hsmm.fit(t_seq, t_labels, 5, 1e-5);

        // Saving again the parameters after one training iteration.
        std::ofstream output_params(output_filename);
        current_params = promp_hsmm.to_stream();
        if (vm.count("commitid"))
            current_params["git_commit_id"] = vm["commitid"].as<string>();
        output_params << std::setw(4) << current_params << std::endl;
        output_params.close();

        ViterbiAlgorithm(promp_hsmm, seq_obs, vm["viterbi"].as<string>());

        if (convergence_reached)
            break;

    }
    cout << "loglikelihood: " << promp_hsmm.loglikelihood(t_seq) << endl;
    if (vm.count("leaveoneout")) {
        int omitted = vm["leaveoneout"].as<int>();
        field<field<mat>> test = {seq_obs(omitted)};
        field<Labels> test_labels = {seq_labels(omitted)};
        cout << "loglikelihoodtest: " << promp_hsmm.loglikelihood(test) << endl;
    }
    return 0;
}


#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <cstdlib>
#include <boost/algorithm/string.hpp>

#include "torch/torch.h"
#include "graphblas/wrapper/sssp.hpp"

struct Args {
  std::vector<char*> args_;

  Args() {
    char* n = (char*)std::malloc(1);
    n[0] = 0;
    args_.push_back(n);
  }
  ~Args() {
    for (int i = 0; i < args_.size(); i ++)
      free(args_[i]);
  }
  void add(const char* arg) {
    args_.push_back(const_cast<char*>(arg));
  }
  int argc() {
    return args_.size();
  }
  char** argv() {
    return args_.data();
  }
};

int main(int argc, char** argv) {
  const char* input_file = argv[argc-1];

  int nrows, ncols;
  std::vector<int> row_indices, col_indices;
  std::vector<float> vals;
  std::string line;
  std::vector<std::string> flds;
  std::ifstream input_fp(input_file);
  int line_cnt = 0;
  while (std::getline(input_fp, line)) {
    boost::split(flds, line, boost::is_any_of(" "));
    if (flds.size() == 0 || flds[0].size() == 0 || flds[0][0] == '%')
      continue;
    if (line_cnt == 0) {
      nrows = std::stoi(flds[0]);
      ncols = std::stoi(flds[1]);
    } else {
      row_indices.push_back(std::stoi(flds[0]));
      col_indices.push_back(std::stoi(flds[1]));
      vals.push_back(std::stof(flds[2]));
    }
    line_cnt ++;
  }
  input_fp.close();
  if (nrows * ncols != vals.size()) {
    std::cout << "Error: nrows * ncols != vals.size()" << std::endl;
    return 1;
  }
  torch::Device device(torch::DeviceType::CUDA, 0);
  /*
  torch::Tensor data = torch::zeros({nrows, ncols}, torch::TensorOptions(torch::kFloat32).device(device));
  torch::Tensor rowptr = torch::arange(0, nrows * ncols + 1, ncols, torch::TensorOptions(torch::kInt32).device(device));
  torch::Tensor colind = torch::arange(0, ncols, 1, torch::TensorOptions(torch::kInt32).device(device)).repeat(nrows).contiguous();
  for (int i = 0; i < row_indices.size(); i++) {
    data[row_indices[i] - 1][col_indices[i] - 1] = vals[i];
  }
  */
  torch::Tensor data = torch::zeros(20, torch::TensorOptions(torch::kFloat32).device(device));
  torch::Tensor rowptr = torch::arange(0, 21, 4, torch::TensorOptions(torch::kInt32).device(device));
  torch::Tensor colind = torch::zeros(20, torch::TensorOptions(torch::kInt32).device(device));
  int j = 0;
  for (int i = 0; i < row_indices.size(); i++) {
    if (row_indices[i] == col_indices[i])
      continue;
    data[j] = vals[i];
    colind[j] = col_indices[i] - 1;
    j ++;
  }

  std::cout << "Data:" << std::endl;
  std::cout << data << std::endl;
  std::cout << "RowPtr:" << std::endl;
  std::cout << rowptr << std::endl;
  std::cout << "ColInd:" << std::endl;
  std::cout << colind << std::endl;
  std::vector<float> result;
  Args args;
  int status = graphblas::wrapper::sssp(
        nrows, ncols, 20,
        (int*)rowptr.data_ptr(), (int*)colind.data_ptr(), (float*)data.data_ptr(),
        0, result, args.argc(), args.argv());
  std::cout << "status=" << status << std::endl;
  std::cout << "result: ";
  for (int i = 0; i < result.size(); i ++)
    std::cout << "[" << i << "]" << result[i] << ", ";
  std::cout << std::endl;
  return 0;
}


#include <boost/graph/kruskal_min_spanning_tree.hpp>


//=============================================================================
//
//  CLASS LinearConstraintConverter
//
//=============================================================================

#ifndef COMISO_LINEARCONSTRAINTCONVERTER_HH
#define COMISO_LINEARCONSTRAINTCONVERTER_HH

//== INCLUDES =================================================================

#include <CoMISo/Config/CoMISoDefines.hh>
#include "NConstraintInterface.hh"
#include "LinearConstraint.hh"

#include <Base/Code/Quality.hh>
LOW_CODE_QUALITY_SECTION_BEGIN
#include <Eigen/StdVector>
#include <Eigen/Dense>
#include <Eigen/Sparse>
LOW_CODE_QUALITY_SECTION_END

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

/** \class NProblemGmmInterface NProblemGmmInterface.hh <ACG/.../NPRoblemGmmInterface.hh>

    Brief Description.

    A more elaborate description follows.
*/
class COMISODLLEXPORT LinearConstraintConverter
{
public:

  // sparse vector type
  typedef NConstraintInterface::SVectorNC SVectorNC;

  typedef Eigen::SparseMatrix<double,Eigen::ColMajor> SparseMatrixC;
  typedef Eigen::SparseMatrix<double,Eigen::RowMajor> SparseMatrixR;
  typedef Eigen::VectorXd                             VectorXd;

  LinearConstraintConverter(SparseMatrixR& _A, VectorXd& _b)
  : A_(_A), b_(_b), linear_constraints_initialized_(false)
  {}

  LinearConstraintConverter(SparseMatrixC& _A, VectorXd& _b)
  : A_(_A), b_(_b), linear_constraints_initialized_(false)
  {}

  std::vector<NConstraintInterface*>& constraints_nsolver()
  {
    initialize_linear_constraints();
    return constraint_pointers_;
  }

private:

  void initialize_linear_constraints()
  {
    if(!linear_constraints_initialized_)
    {
      // tag as done
      linear_constraints_initialized_ = true;

      int m = A_.rows();
      int n = A_.cols();

      linear_constraints_.clear();
      linear_constraints_.resize(m);

      constraint_pointers_.clear();
      constraint_pointers_.resize(m);

      for( int i=0; i<m; ++i)
      {
        // convert i-th constraint
        linear_constraints_.resize(n);
        linear_constraints_[i].coeffs() =  A_.row(i);
        linear_constraints_[i].b()      = -b_[i];
        // store pointer
        constraint_pointers_[i] = &(linear_constraints_[i]);
      }
    }
  }

private:
  SparseMatrixR A_;
  VectorXd      b_;

  bool                               linear_constraints_initialized_;
  std::vector<LinearConstraint>      linear_constraints_;
  std::vector<NConstraintInterface*> constraint_pointers_;
};

//=============================================================================
} // namespace COMISO
//=============================================================================
//=============================================================================
#endif // ACG_LINEARCONSTRAINTCONVERTER_HH defined
//=============================================================================


// MIT License

// Copyright (c) 2019 Edward Liu

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "registrators/icp_fast.h"

#include <algorithm>
#include <cstdlib>
#include <ctime>
#include <iostream>
#include <limits>
#include <utility>
#include <vector>

#include <boost/typeof/typeof.hpp>

#include "common/macro_defines.h"
#include "common/math.h"
#include "common/performance/simple_prof.h"

namespace static_map {
namespace registrator {

constexpr int kNormalEstimationKnn = 7;
constexpr int kDim = 3;

using Matrix = Eigen::MatrixXd;
using Vector = Eigen::VectorXd;
using OutlierWeights = Matrix;
using data::EigenPointCloud;

struct Matches {
  //!< Squared distances to closest points, dense matrix
  using Dists = Matrix;
  //!< Identifiers of closest points, dense matrix of integers
  using Ids = Eigen::MatrixXi;

  Matches() = default;
  Matches(const int knn, const int points_count)
      : dists(Dists(knn, points_count)), ids(Ids(knn, points_count)) {}

  //!< squared distances to closest points
  Dists dists;
  //!< identifiers of closest points
  Ids ids;

  double GetDistsQuantile(const double quantile) const {
    // REGISTER_FUNC;
    // build array
    CHECK(quantile >= 0.0 && quantile <= 1.0);
    std::vector<double> values;
    values.reserve(dists.rows() * dists.cols());
    for (int x = 0; x < dists.cols(); ++x) {
      for (int y = 0; y < dists.rows(); ++y) {
        if (dists(y, x) != std::numeric_limits<double>::infinity()) {
          values.push_back(dists(y, x));
        }
      }
    }

    // BUG
    // Sometimes this really happens, which should not.
    CHECK(!values.empty());
    // get quantile
    if (quantile == 1.0) {
      return *std::max_element(values.begin(), values.end());
    }
    const int quantile_index = values.size() * quantile;
    std::nth_element(values.begin(), values.begin() + quantile_index,
                     values.end());
    return values[quantile_index];
  }
};

struct ErrorElements {
  EigenPointCloud reading;    //!< reading point cloud
  EigenPointCloud reference;  //!< reference point cloud
  OutlierWeights weights;     //!< weights for every association
  Matches matches;            //!< associations

  ErrorElements();
  ErrorElements(const EigenPointCloud& source_points,
                const EigenPointCloud& target_points,
                const OutlierWeights& weights, const Matches& matches) {
    REGISTER_FUNC;
    CHECK_GT(matches.ids.rows(), 0);
    CHECK_GT(matches.ids.cols(), 0);
    CHECK(matches.ids.cols() == source_points.points.cols());  // nbpts
    CHECK(weights.rows() == matches.ids.rows());               // knn
    CHECK_EQ(weights.rows(), 1);

    const int dim_points = source_points.points.rows();

    // Count points with no weights
    const int points_count = (weights.array() != 0.0).count();
    CHECK_GT(points_count, 0) << "no point to minimize";

    Matrix kept_points(dim_points, points_count);
    std::vector<double> kept_points_factor(points_count);
    Matches kept_matches(1, points_count);
    OutlierWeights kept_weights(1, points_count);

    int j = 0;
    for (int i = 0; i < source_points.points.cols(); ++i) {
      const auto match_dist = matches.dists(0, i);
      if (match_dist == NNS::InvalidValue) {
        continue;
      }

      if (weights(0, i) != 0.0) {
        kept_points.col(j) = source_points.points.col(i);
        kept_points_factor[j] = source_points.factors[i];
        kept_matches.ids(0, j) = matches.ids(0, i);
        kept_matches.dists(0, j) = match_dist;
        kept_weights(0, j) = weights(0, i);
        ++j;
      }
    }
    CHECK_EQ(j, points_count);
    CHECK_EQ(dim_points, target_points.points.rows());

    const int dim_target_normals = target_points.normals.rows();
    Matrix associated_target_normals;
    if (dim_target_normals > 0) {
      associated_target_normals = Matrix(dim_target_normals, points_count);
    }

    Matrix associated_points(dim_points, points_count);
    // Fetch matched points
    for (int i = 0; i < points_count; ++i) {
      const int ref_index(kept_matches.ids(i));
      associated_points.col(i) =
          target_points.points.block(0, ref_index, dim_points, 1);

      if (dim_target_normals > 0)
        associated_target_normals.col(i) =
            target_points.normals.block(0, ref_index, dim_target_normals, 1);
    }

    // Copy final data to structure
    reading.points = kept_points;
    reading.factors = std::move(kept_points_factor);
    reference.points = associated_points;
    reference.normals = associated_target_normals;

    this->weights = kept_weights;
    this->matches = kept_matches;
  }
};

Matches FindClosests(const std::shared_ptr<NNS>& nns_kdtree,
                     const EigenPointCloud& source_cloud) {
  REGISTER_FUNC;
  const int points_count(source_cloud.points.cols());
  const int search_count = 1;
  const double epsilon = 3.16;
  //! A dense integer matrix
  Matches matches(search_count, points_count);
  nns_kdtree->knn(source_cloud.points, matches.ids, matches.dists, search_count,
                  epsilon, NNS::ALLOW_SELF_MATCH);
  return matches;
}

Eigen::MatrixXd CrossProduct(const Eigen::MatrixXd& A,
                             const Eigen::MatrixXd& B) {
  // Expecting matched points
  CHECK_EQ(A.cols(), B.cols());
  // Expecting homogenous coord X eucl. coord
  CHECK_EQ(A.rows(), B.rows());

  constexpr unsigned int x = 0;
  constexpr unsigned int y = 1;
  constexpr unsigned int z = 2;

  Eigen::MatrixXd cross(B.rows(), B.cols());
  cross.row(x) =
      A.row(y).array() * B.row(z).array() - A.row(z).array() * B.row(y).array();
  cross.row(y) =
      A.row(z).array() * B.row(x).array() - A.row(x).array() * B.row(z).array();
  cross.row(z) =
      A.row(x).array() * B.row(y).array() - A.row(y).array() * B.row(x).array();

  return cross;
}

void SolvePossiblyUnderdeterminedLinearSystem(const Matrix& A, const Vector& b,
                                              Vector& x) {  // NOLINT
  CHECK_EQ(A.cols(), A.rows());
  CHECK_EQ(b.cols(), 1);
  CHECK_EQ(b.rows(), A.rows());
  CHECK_EQ(x.cols(), 1);
  CHECK_EQ(x.rows(), A.cols());

  // using Matrix = typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

  BOOST_AUTO(Aqr, A.fullPivHouseholderQr());
  if (!Aqr.isInvertible()) {
    // Solve reduced problem R1 x = Q1^T b instead of QR x = b, where Q = [Q1
    // Q2] and R = [ R1 ; R2 ] such that ||R2|| is small (or zero) and
    // therefore A = QR ~= Q1 * R1
    const int rank = Aqr.rank();
    const int rows = A.rows();
    const Matrix Q1t = Aqr.matrixQ().transpose().block(0, 0, rank, rows);
    const Matrix R1 = (Q1t * A * Aqr.colsPermutation()).block(0, 0, rank, rows);

    // The under-determined system R1 x = Q1^T b is made unique ..
    // by getting the solution of smallest norm (x = R1^T * (R1 * R1^T)^-1
    // Q1^T b.
    x = R1.triangularView<Eigen::Upper>().transpose() *
        (R1 * R1.transpose()).llt().solve(Q1t * b);
    x = Aqr.colsPermutation() * x;

    BOOST_AUTO(ax, (A * x).eval());
    if (!b.isApprox(ax, 1e-5)) {
      x = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
      ax = A * x;

      if ((b - ax).norm() > 1e-5 * std::max(A.norm() * x.norm(), b.norm())) {
        // clang-format off
        LOG(WARNING)
            << "PointMatcher::icp - encountered numerically singular matrix "
               "while minimizing point to plane distance and the current "
               "workaround remained inaccurate."
            << " b=" << b.transpose()
            << " !~ A * x=" << (ax).transpose().eval()
            << ": ||b- ax||=" << (b - ax).norm()
            << ", ||b||=" << b.norm()
            << ", ||ax||=" << ax.norm();
        // clang-format on
      }
    }
  } else {
    // Cholesky decomposition
    x = A.llt().solve(b);
  }
}

Eigen::Matrix4d ComputePointToPlane(const EigenPointCloud& source_cloud,
                                    const EigenPointCloud& target_cloud,
                                    const Eigen::MatrixXd& weights,
                                    const Matches& matches,
                                    bool compensation = false) {
  REGISTER_FUNC;
  const Eigen::MatrixXd target_normals = target_cloud.normals;
  CHECK_EQ(target_normals.rows(), kDim);
  CHECK_EQ(target_normals.cols(), target_cloud.points.cols());
  CHECK_EQ(source_cloud.points.cols(), target_cloud.points.cols());

  // Compute cross product of cross = cross(reading X target_normals)
  const Matrix cross = CrossProduct(source_cloud.points, target_normals);

  // wF = [weights*cross, weights*normals]
  // F  = [cross, normals]
  Matrix wF(target_normals.rows() + cross.rows(), target_normals.cols());
  Matrix F(target_normals.rows() + cross.rows(), target_normals.cols());

  for (int i = 0; i < cross.rows(); i++) {
    wF.row(i) = weights.array() * cross.row(i).array();
    F.row(i) = cross.row(i);
  }
  for (int i = 0; i < target_normals.rows(); i++) {
    wF.row(i + cross.rows()) = weights.array() * target_normals.row(i).array();
    F.row(i + cross.rows()) = target_normals.row(i);
  }

  if (compensation) {
    for (int i = 0; i < target_normals.cols(); ++i) {
      wF.col(i) *= source_cloud.factors[i];
      F.col(i) *= source_cloud.factors[i];
    }
  }

  // Unadjust covariance A = wF * F'
  const Matrix A = wF * F.transpose();
  const Matrix deltas = source_cloud.points - target_cloud.points;

  // dot product of dot = dot(deltas, normals)
  Matrix dotProd = Matrix::Zero(1, target_normals.cols());
  for (int i = 0; i < target_normals.rows(); i++) {
    dotProd += (deltas.row(i).array() * target_normals.row(i).array()).matrix();
  }

  // b = -(wF' * dot)
  const Vector b = -(wF * dotProd.transpose());
  Vector x(A.rows());
  SolvePossiblyUnderdeterminedLinearSystem(A, b, x);

  // Transform parameters to matrix
  Eigen::Matrix4d result;
  Eigen::Transform<double, 3, Eigen::Affine> transform;
  transform =
      Eigen::AngleAxis<double>(x.head(3).norm(), x.head(3).normalized());

  transform.translation() = x.segment(3, 3);
  result = transform.matrix();

  if (result.hasNaN()) {
    // Degenerate situation. This can happen when the source and reading
    // clouds are identical, and then b and x above are 0, and the rotation
    // matrix cannot be determined, it comes out full of NaNs. The correct
    // rotation is the identity.
    result.block(0, 0, kDim, kDim) = Matrix::Identity(kDim, kDim);
  }

  return result;
}

Eigen::Matrix4d ComputePointToPoint(EigenPointCloud& source_cloud,  // NOLINT
                                    EigenPointCloud& target_cloud,  // NOLINT
                                    const Eigen::MatrixXd& weights,
                                    const Matches& matches) {
  CHECK_EQ(source_cloud.points.cols(), target_cloud.points.cols());

  // const int dimCount(mPts.reading.features.rows());
  // const int ptsCount(mPts.reading.features.cols()); //Both point clouds
  // have now the same number of (matched) point

  const Vector w = weights.row(0);
  const double w_sum_inv = 1. / w.sum();
  const Vector meanReading =
      (source_cloud.points.array().rowwise() * w.array().transpose())
          .rowwise()
          .sum() *
      w_sum_inv;
  const Vector meanReference =
      (target_cloud.points.array().rowwise() * w.array().transpose())
          .rowwise()
          .sum() *
      w_sum_inv;

  // Remove the mean from the point clouds
  source_cloud.points.colwise() -= meanReading;
  target_cloud.points.colwise() -= meanReference;

  // Singular Value Decomposition
  const Matrix m(target_cloud.points * w.asDiagonal() *
                 source_cloud.points.transpose());
  const Eigen::JacobiSVD<Matrix> svd(m,
                                     Eigen::ComputeThinU | Eigen::ComputeThinV);
  Matrix rotMatrix(svd.matrixU() * svd.matrixV().transpose());
  // It is possible to get a reflection instead of a rotation. In this case,
  // we take the second best solution, guaranteed to be a rotation. For more
  // details, read the tech report: "Least-Squares Rigid Motion Using SVD",
  // Olga Sorkine http://igl.ethz.ch/projects/ARAP/svd_rot.pdf
  if (rotMatrix.determinant() < 0.) {
    Matrix tmpV = svd.matrixV().transpose();
    tmpV.row(kDim - 1) *= -1.;
    rotMatrix = svd.matrixU() * tmpV;
  }
  const Vector trVector(meanReference - rotMatrix * meanReading);

  Matrix result(Matrix::Identity(4, 4));
  result.topLeftCorner(kDim, kDim) = rotMatrix;
  result.topRightCorner(kDim, 1) = trVector;

  return result;
}

bool CheckConvergence(const std::vector<Eigen::Quaterniond>& rotations,
                      const std::vector<Eigen::Vector3d>& translations) {
  constexpr int kSmoothLength = 4;
  constexpr double kConvergeRotDist = 0.001;
  constexpr double kConvergeTransDist = 0.01;

  double rotation_dist = 0.;
  double translation_dist = 0.;
  bool has_converged = false;
  if (rotations.size() > kSmoothLength) {
    for (size_t i = rotations.size() - 1; i >= rotations.size() - kSmoothLength;
         i--) {
      // Compute the mean derivative
      rotation_dist +=
          std::fabs(rotations[i].angularDistance(rotations[i - 1]));
      translation_dist +=
          std::fabs((translations[i] - translations[i - 1]).norm());
    }

    rotation_dist /= kSmoothLength;
    translation_dist /= kSmoothLength;

    if (rotation_dist < kConvergeRotDist &&
        translation_dist < kConvergeTransDist)
      has_converged = true;
  }

  return has_converged;
}

IcpFast::IcpFast() : Interface() {
  this->type_ = kFastIcp;
  // TODO(edward) This `knn_normal_estimate` is currently of no use, make it
  // useful later.
  REG_REGISTRATOR_INNER_OPTION("knn_normal_estimate",
                               OptionItemDataType::kInt32,
                               options_.knn_for_normal_estimate);
  REG_REGISTRATOR_INNER_OPTION("max_iteration", OptionItemDataType::kInt32,
                               options_.max_iteration);
  REG_REGISTRATOR_INNER_OPTION("dist_outlier_ratio",
                               OptionItemDataType::kFloat32,
                               options_.dist_outlier_ratio);
}

void IcpFast::SetInputSource(InnerCloudPtr cloud) {
  CHECK(cloud);
  CHECK(cloud->GetEigenCloud());
  source_cloud_.reset(new EigenPointCloud(*cloud->GetEigenCloud()));
}

void IcpFast::SetInputTarget(InnerCloudPtr cloud) {
  CHECK(cloud);
  CHECK(cloud->GetEigenCloud());
  CHECK(cloud->GetEigenCloud()->HasNormals());
  target_cloud_.reset(new EigenPointCloud(*cloud->GetEigenCloud()));

  // for debug
  //
  // pcl::PointCloud<pcl::PointXYZINormal> normal_cloud;
  // const int cloud_size = target_cloud_->points.cols();
  // normal_cloud.reserve(cloud_size);
  // for (int i = 0; i < cloud_size; ++i) {
  //   pcl::PointXYZINormal point;
  //   point.x = target_cloud_->points(0, i);
  //   point.y = target_cloud_->points(1, i);
  //   point.z = target_cloud_->points(2, i);
  //   point.normal_x = target_cloud_->normals(0, i);
  //   point.normal_y = target_cloud_->normals(1, i);
  //   point.normal_z = target_cloud_->normals(2, i);
  //   normal_cloud.push_back(point);
  // }
  // pcl::io::savePCDFileBinary("/tmp/normal.pcd", normal_cloud);

  // step2 build KDTREE for closeset point searching
  // nns_kdtree_.reset(
  //     NNS::create(target_cloud_->points, kDim, NNS::KDTREE_LINEAR_HEAP));
}

bool IcpFast::Align(const Eigen::Matrix4d& guess,
                    Eigen::Matrix4d& result) {  // NOLINT
  const int target_points_count = target_cloud_->points.cols();
  const Eigen::Vector3d target_mean =
      target_cloud_->points.rowwise().sum() / target_points_count;
  Eigen::Matrix4d T_target_mean = Eigen::Matrix4d::Identity();
  T_target_mean.block(0, 3, 3, 1) = target_mean;

  target_cloud_->points.colwise() -= target_mean;
  {
    // REGISTER_BLOCK("BuildKdTree");
    nns_kdtree_.reset(
        NNS::create(target_cloud_->points, kDim, NNS::KDTREE_LINEAR_HEAP));
  }
  EigenPointCloud init_source_cloud = *source_cloud_;
  Eigen::Matrix4d T_target_mean_init_guess = T_target_mean.inverse() * guess;
  init_source_cloud.ApplyTransform(T_target_mean_init_guess);

  // initialise all iter status
  Eigen::Matrix4d T_iter = Eigen::Matrix4d::Identity();
  std::vector<Eigen::Quaterniond> rotations_iter;
  std::vector<Eigen::Vector3d> translations_iter;
  rotations_iter.reserve(10);
  translations_iter.reserve(10);
  rotations_iter.push_back(Eigen::Quaterniond::Identity());
  translations_iter.push_back(Eigen::Vector3d::Zero());

  int iterator = 0;
  while (true) {
    REGISTER_BLOCK("Iteration");
    // step1 Find Closest points
    EigenPointCloud step_cloud(init_source_cloud);
    if (this->inner_compensation_) {
      step_cloud.ApplyMotionCompensation(T_iter);
    } else {
      step_cloud.ApplyTransform(T_iter);
    }

    const Matches matches(FindClosests(nns_kdtree_, step_cloud));

    // step2 reject outliers (outliers with weight 0)
    const double limit = matches.GetDistsQuantile(options_.dist_outlier_ratio);
    const Matrix output_weights =
        (matches.dists.array() <= limit).cast<double>();

    // step3 solve point-to-plane and update result
    CHECK_EQ(output_weights.cols(), step_cloud.points.cols());

    ErrorElements error_elements(step_cloud, *target_cloud_, output_weights,
                                 matches);

    T_iter =
        ComputePointToPlane(error_elements.reading, error_elements.reference,
                            error_elements.weights, error_elements.matches,
                            this->inner_compensation_) *
        T_iter;

    // step4 check convergence and jump out
    iterator++;
    rotations_iter.emplace_back(Eigen::Matrix3d(T_iter.block(0, 0, 3, 3)));
    translations_iter.push_back(T_iter.block(0, 3, 3, 1));
    if (CheckConvergence(rotations_iter, translations_iter) ||
        iterator >= options_.max_iteration) {
      const double average_dist =
          error_elements.matches.dists.array().cwiseSqrt().sum() /
          error_elements.matches.dists.cols();
      this->final_score_ = std::exp(-average_dist);
      break;
    }
  }

  target_cloud_->points.colwise() += target_mean;
  result = T_target_mean * T_iter * T_target_mean_init_guess;
  return true;
}

}  // namespace registrator
}  // namespace static_map


#include <frovedis.hpp>
#include <frovedis/ml/graph/graph.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

template <class T>
void call_bfs(const std::string& data_p,
              const std::string& out_p,
              bool if_prep,
              size_t source_vertex,
              int opt_level,
              double threshold,
              size_t depth_limit) {
  graph<T> gr;
  time_spent t(INFO);
  if(if_prep) gr = read_edgelist<T>(data_p);
  else {
    auto mat = make_crs_matrix_load<T>(data_p);
    gr = graph<T>(mat);
  }
  t.show("data loading time: ");
  auto res = gr.bfs(source_vertex, opt_level, threshold, depth_limit);
  t.show("bfs computation time: ");
  res.save(out_p);
  res.debug_print(5);
}

int main(int argc, char* argv[]){
    frovedis::use_frovedis use(argc, argv);
    using namespace boost::program_options;

    options_description opt("option");
    opt.add_options()
        ("help,h", "produce help message")
        ("input,i" , value<std::string>(), "input data path containing either edgelist or adjacency matrix data")
        ("dtype,t" , value<std::string>(), "input data type (int, float or double) [default: int]")
        ("output,o" , value<std::string>(), "output data path to save cc results")
        ("source,s", value<size_t>(), "source vertex id [1 ~ nvert] (default: 1)")
        ("opt-level", value<int>(), "optimization level 0, 1 or 2 (default: 1)")
        ("hyb-threshold", value<double>(), "threshold value for hybrid optimization [0.0 ~ 1.0] (default: 0.4)")
        ("depth-limit", value<size_t>(), "depth limit to which tree is to be traversed (default: Max depth)")
        ("verbose", "set loglevel to DEBUG")
        ("verbose2", "set loglevel to TRACE")
        ("prepare,p" , "whether to generate the CRS matrix from original edgelist file ");

    variables_map argmap;
    store(command_line_parser(argc,argv).options(opt).allow_unregistered().
          run(), argmap);
    notify(argmap);

    bool if_prep = 0; // true if prepare data from raw dataset
    std::string data_p, out_p, dtype = "int";
    size_t source_vertex = 1;
    int opt_level = 1;
    double threshold = 0.4;
    size_t depth_limit = std::numeric_limits<size_t>::max();

    if(argmap.count("help")){
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("input")){
      data_p = argmap["input"].as<std::string>();
    } else {
      std::cerr << "input path is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("output")){
      out_p = argmap["output"].as<std::string>();
    } else {
      std::cerr << "output path is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("prepare")){
      if_prep = true;
    }
    if(argmap.count("dtype")){
      dtype = argmap["dtype"].as<std::string>();
    }
    if(argmap.count("source")){
       source_vertex = argmap["source"].as<size_t>();
    }
    if(argmap.count("opt-level")){
       opt_level = argmap["opt-level"].as<int>();
    }
    if(argmap.count("hyb-threshold")){
       threshold = argmap["hyb-threshold"].as<double>();
    }
    if(argmap.count("depth-limit")){
       depth_limit = argmap["depth-limit"].as<size_t>();
    }
    if(argmap.count("verbose")){
      set_loglevel(DEBUG);
    }
    if(argmap.count("verbose2")){
      set_loglevel(TRACE);
    }

    try {
      if (dtype == "int") {
        call_bfs<int>(data_p, out_p, if_prep, source_vertex,
                      opt_level, threshold, depth_limit);
      }
      else if (dtype == "float") {
        call_bfs<float>(data_p, out_p, if_prep, source_vertex,
                        opt_level, threshold, depth_limit);
      }
      else if (dtype == "double") {
        call_bfs<double>(data_p, out_p, if_prep, source_vertex,
                         opt_level, threshold, depth_limit);
      }
      else {
        std::cerr << "Supported dtypes are only int, float and double!\n";
        std::cerr << opt << std::endl;
        exit(1);
      }
    }
    catch (std::exception& e) {
      std::cout << "exception caught: " << e.what() << std::endl;
    }
    return 0;
}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are 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_MATH_TOOLS_RATIONAL_HPP
#define BOOST_MATH_TOOLS_RATIONAL_HPP

#include <boost/array.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/mpl/int.hpp>

#if BOOST_MATH_POLY_METHOD == 1
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/polynomial_horner1_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_POLY_METHOD == 2
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/polynomial_horner2_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_POLY_METHOD == 3
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/polynomial_horner3_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#endif
#if BOOST_MATH_RATIONAL_METHOD == 1
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/rational_horner1_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_RATIONAL_METHOD == 2
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/rational_horner2_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_RATIONAL_METHOD == 3
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/rational_horner3_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#endif
namespace boost{ namespace math{ namespace tools{

//
// Forward declaration to keep two phase lookup happy:
//
template <class T, class U>
U evaluate_polynomial(const T* poly, U const& z, std::size_t count);

namespace detail{

template <class T, class V, class Tag>
inline V evaluate_polynomial_c_imp(const T* a, const V& val, const Tag*)
{
   return evaluate_polynomial(a, val, Tag::value);
}

} // namespace detail

//
// Polynomial evaluation with runtime size.
// This requires a for-loop which may be more expensive than
// the loop expanded versions above:
//
template <class T, class U>
inline U evaluate_polynomial(const T* poly, U const& z, std::size_t count)
{
   BOOST_ASSERT(count > 0);
   U sum = static_cast<U>(poly[count - 1]);
   for(int i = static_cast<int>(count) - 2; i >= 0; --i)
   {
      sum *= z;
      sum += static_cast<U>(poly[i]);
   }
   return sum;
}
//
// Compile time sized polynomials, just inline forwarders to the
// implementations above:
//
template <std::size_t N, class T, class V>
inline V evaluate_polynomial(const T(&a)[N], const V& val)
{
   typedef mpl::int_<N> tag_type;
   return detail::evaluate_polynomial_c_imp(static_cast<const T*>(a), val, static_cast<tag_type const*>(0));
}

template <std::size_t N, class T, class V>
inline V evaluate_polynomial(const boost::array<T,N>& a, const V& val)
{
   typedef mpl::int_<N> tag_type;
   return detail::evaluate_polynomial_c_imp(static_cast<const T*>(a.data()), val, static_cast<tag_type const*>(0));
}
//
// Even polynomials are trivial: just square the argument!
//
template <class T, class U>
inline U evaluate_even_polynomial(const T* poly, U z, std::size_t count)
{
   return evaluate_polynomial(poly, z*z, count);
}

template <std::size_t N, class T, class V>
inline V evaluate_even_polynomial(const T(&a)[N], const V& z)
{
   return evaluate_polynomial(a, z*z);
}

template <std::size_t N, class T, class V>
inline V evaluate_even_polynomial(const boost::array<T,N>& a, const V& z)
{
   return evaluate_polynomial(a, z*z);
}
//
// Odd polynomials come next:
//
template <class T, class U>
inline U evaluate_odd_polynomial(const T* poly, U z, std::size_t count)
{
   return poly[0] + z * evaluate_polynomial(poly+1, z*z, count-1);
}

template <std::size_t N, class T, class V>
inline V evaluate_odd_polynomial(const T(&a)[N], const V& z)
{
   typedef mpl::int_<N-1> tag_type;
   return a[0] + z * detail::evaluate_polynomial_c_imp(static_cast<const T*>(a) + 1, z*z, static_cast<tag_type const*>(0));
}

template <std::size_t N, class T, class V>
inline V evaluate_odd_polynomial(const boost::array<T,N>& a, const V& z)
{
   typedef mpl::int_<N-1> tag_type;
   return a[0] + z * detail::evaluate_polynomial_c_imp(static_cast<const T*>(a.data()) + 1, z*z, static_cast<tag_type const*>(0));
}

template <class T, class U, class V>
V evaluate_rational(const T* num, const U* denom, const V& z_, std::size_t count);

namespace detail{

template <class T, class U, class V, class Tag>
inline V evaluate_rational_c_imp(const T* num, const U* denom, const V& z, const Tag*)
{
   return boost::math::tools::evaluate_rational(num, denom, z, Tag::value);
}

}
//
// Rational functions: numerator and denominator must be
// equal in size.  These always have a for-loop and so may be less
// efficient than evaluating a pair of polynomials. However, there
// are some tricks we can use to prevent overflow that might otherwise
// occur in polynomial evaluation, if z is large.  This is important
// in our Lanczos code for example.
//
template <class T, class U, class V>
V evaluate_rational(const T* num, const U* denom, const V& z_, std::size_t count)
{
   V z(z_);
   V s1, s2;
   if(z <= 1)
   {
      s1 = static_cast<V>(num[count-1]);
      s2 = static_cast<V>(denom[count-1]);
      for(int i = (int)count - 2; i >= 0; --i)
      {
         s1 *= z;
         s2 *= z;
         s1 += num[i];
         s2 += denom[i];
      }
   }
   else
   {
      z = 1 / z;
      s1 = static_cast<V>(num[0]);
      s2 = static_cast<V>(denom[0]);
      for(unsigned i = 1; i < count; ++i)
      {
         s1 *= z;
         s2 *= z;
         s1 += num[i];
         s2 += denom[i];
      }
   }
   return s1 / s2;
}

template <std::size_t N, class T, class U, class V>
inline V evaluate_rational(const T(&a)[N], const U(&b)[N], const V& z)
{
   return detail::evaluate_rational_c_imp(a, b, z, static_cast<const mpl::int_<N>*>(0));
}

template <std::size_t N, class T, class U, class V>
inline V evaluate_rational(const boost::array<T,N>& a, const boost::array<U,N>& b, const V& z)
{
   return detail::evaluate_rational_c_imp(a.data(), b.data(), z, static_cast<mpl::int_<N>*>(0));
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_RATIONAL_HPP


///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//                 polar v1.0 - August the 15th 2019                         //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//  Copyright 2019 Matteo Tommasini                                          //
//                                                                           //
//  Licensed under the Apache License, Version 2.0 (the "License");          //
//  you may not use this file except in compliance with the License.         //
//  You may obtain a copy of the License at                                  //
//                                                                           //
//      http://www.apache.org/licenses/LICENSE-2.0                           //
//                                                                           //
//  Unless required by applicable law or agreed to in writing, software      //
//  distributed under the License is distributed on an "AS IS" BASIS,        //
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. //
//  See the License for the specific language governing permissions and      //
//  limitations under the License.                                           //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

#ifndef GAUSSIANFCHK_H
#define GAUSSIANFCHK_H

#include <string>
#include <Eigen/Core>

#define NUMBER_OF_ATOMS_KEY              "Number of atoms"
#define ATOMIC_NUMBERS_KEY               "Atomic numbers"
#define ATOMIC_MASSES_KEY                "Real atomic weights"
#define CARTESIAN_COORDINATES_KEY        "Current cartesian coordinates"
#define HESSIAN_KEY                      "Cartesian Force Constants"
#define ELECTRIC_DIPOLE_DERIVATIVES_KEY  "Dipole Derivatives"
#define MAGNETIC_DIPOLE_DERIVATIVES_KEY  "AAT"

#define NUMBER_OF_ATOMS_MSG              "Number of atoms"
#define ATOMIC_NUMBERS_MSG               "Loaded atomic numbers"
#define ATOMIC_MASSES_MSG                "Loaded atomic masses"
#define CARTESIAN_COORDINATES_MSG        "Loaded Cartesian coordinates"
#define HESSIAN_MSG                      "Loaded Hessian (cartesian force constants)"
#define ELECTRIC_DIPOLE_DERIVATIVES_MSG  "Loaded electric dipole derivatives"
#define MAGNETIC_DIPOLE_DERIVATIVES_MSG  "Loaded magnetic dipole derivatives"

#define ATOMIC_NUMBERS_ERR_MSG           "NOT FOUND: atomic numbers"
#define ATOMIC_MASSES_ERR_MSG            "NOT FOUND: atomic masses"
#define CARTESIAN_COORDINATES_ERR_MSG    "NOT FOUND: Cartesian coordinates"
#define HESSIAN_ERR_MSG                  "NOT FOUND: Hessian (cartesian force constants)"
#define ELECTRIC_DIPOLE_DERIVATIVES_ERR_MSG "NOT FOUND: electric dipole derivatives"
#define MAGNETIC_DIPOLE_DERIVATIVES_ERR_MSG "NOT FOUND: magnetic dipole derivatives"

class GaussianFCHK
{
   private:
   std::string filename,
               title;
   int  Number_of_atoms;
   bool has_atomic_numbers,
        has_atomic_masses,
        has_hessian,
        has_cartesian_coordinates,
        has_electric_dipole_derivatives,
        has_magnetic_dipole_derivatives;

   public:
   GaussianFCHK();
   explicit GaussianFCHK(const std::string&);
   Eigen::MatrixXi ReadAtomicNumbers() const;
   Eigen::MatrixXd ReadAtomicMasses() const;
   Eigen::MatrixXd ReadCartesianCoordinates() const;
   Eigen::MatrixXd ReadHessian() const;
   Eigen::MatrixXd ReadElectricDipoleDerivatives() const;
   Eigen::MatrixXd ReadMagneticDipoleDerivatives() const;

   // getters
   int GetNumberOfAtoms() const;
   std::string GetTitle() const;

   // diagnostics
   bool Has_atomic_numbers() const;
   bool Has_atomic_masses() const;
   bool Has_hessian() const;
   bool Has_cartesian_coordinates() const;
   bool Has_electric_dipole_derivatives() const;
   bool Has_magnetic_dipole_derivatives() const;
};

#endif // GAUSSIANFCHK_H


// Demonstration of analyzing RC trees as graphs
// to accompany "Analyzing On-Chip Interconnect with Modern C++"
// Author: Jeff Trull <edaskel@att.net>

/*
Copyright (c) 2014 Jeffrey E. Trull

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <iostream>

#include <boost/graph/connected_components.hpp>

#include "ckt_graph.h"

int main() {
    using namespace boost;
    using namespace std;

    // Build some interesting circuits
    quantity<si::resistance> kohm(1.0 * units::si::kilo  * units::si::ohms);
    quantity<si::capacitance> ff(1.0 * units::si::femto * units::si::farads);

    // A circuit with floating nodes (one dangling, one shared between two other nodes)
    ckt_graph_t float_n;

    auto d1  = add_vertex("d1", float_n);
    auto n2  = add_vertex("n2", float_n);
    auto n3  = add_vertex("n3", float_n);

    // we will model two "nets" here to demonstrate a more complex situation
    // a first driver
    add_edge(d1, n2, kohm, float_n);   // initial resistor
    // in parallel with d1->n2, two capacitors in series
    auto n1 = add_vertex("n1", float_n);
    add_edge(d1, n1, ff, float_n);   // node n1 floats
    add_edge(n1, n2, ff, float_n);
    add_edge(n2, n3, kohm, float_n);
    auto n4 = add_vertex("n4", float_n);
    add_edge(n3, n4, ff, float_n);   // n4 floats
    auto n5 = add_vertex("n5", float_n);
    add_edge(n4, n5, kohm, float_n); // n5 floats despite being resistor-connected

    // second driver
    auto d2 = add_vertex("d2", float_n);
    auto n6 = add_vertex("n6", float_n);
    add_edge(d2, n6, kohm, float_n);
    // n6 couples to n4 of the other net, but that doesn't stop n4 from floating
    add_edge(n6, n4, ff, float_n);

    // Floating nodes are unreachable via resistors from gnd or any driver.  That is, they
    // are either disconnected from any edge, or have at least one capacitor in
    // series between themselves and any driver.
    // For a single net, running DFS from the driver on a filtered resistor-only network,
    // and noting any unreachable nodes would do.
    // For a complex graph with multiple nets, this scales poorly.  The "connected components"
    // algorithm is the answer.  We run it on a resistor-only graph;  any nodes not in the same
    // component as a driver or gnd are floating.

    // run connected components on filtered (resistor-only) graph
    using resonly_graph_t = filtered_graph<ckt_graph_t, resistors_only>;
    resonly_graph_t res_graph(float_n, resistors_only(&float_n));

   // define required color and component property maps
   typedef resonly_graph_t::vertices_size_type comp_number_t;
   typedef map<resonly_graph_t::vertex_descriptor, comp_number_t> CCompsStorageMap;
   CCompsStorageMap comps;      // component map for algorithm results
   typedef map<resonly_graph_t::vertex_descriptor, default_color_type> ColorsStorageMap;
   ColorsStorageMap colors;     // temp storage for algorithm

   // adapt std::map for use by Graph algorithms as "property map"
   boost::associative_property_map<CCompsStorageMap> cpmap(comps);
   boost::associative_property_map<ColorsStorageMap> clrpmap(colors);

   // Run the algorithm
   connected_components(res_graph, cpmap, boost::color_map(clrpmap));

   // Find the components relating to d1, d2, and gnd
   set<comp_number_t> driven_components;
   for (auto comp_entry : comps) {
       if ((float_n[comp_entry.first] == "d1") ||
           (float_n[comp_entry.first] == "d2") ||
           (comp_entry.first == float_n.gnd())) {
           driven_components.insert(comp_entry.second);
       }
   }
   for (auto comp_entry : comps) {
       if (driven_components.find(comp_entry.second) == driven_components.end()) {
           std::cout << "node " << float_n[comp_entry.first] << " is undriven" << std::endl;
       }
   }

}


#ifndef __solver_givens_h__
#define __solver_givens_h__

#include <Eigen/Dense>
#include <vector>
#include <iostream>

namespace hcod{
    class Givens{
        public:
            Givens();
            ~Givens(){};
            void compute_rotation(const Eigen::VectorXd &x, const unsigned int &i, const unsigned int &j);

        private:

        public:
           Eigen::MatrixXd getR(){
               return R_;
           }

        private:
           Eigen::VectorXd x_;
           unsigned int i_, j_, m_;
           Eigen::MatrixXd R_;
    };
}

#endif


// Boost.Range library
//
//  Copyright Neil Groves 2009. 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)
//
//
// For more information, see http://www.boost.org/libs/range/
//
#include <boost/range/algorithm/transform.hpp>

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

#include <boost/assign.hpp>
#include "../test_function/multiply_by_x.hpp"
#include <algorithm>
#include <list>
#include <set>
#include <vector>

namespace boost
{
    namespace
    {
        template< class Container >
        void test_transform_impl1(Container& cont)
        {
            using namespace boost::range_test_function;

            const Container& ccont = cont;

            typedef BOOST_DEDUCED_TYPENAME Container::value_type value_t;

            std::vector<value_t> target(cont.size());
            std::vector<value_t> reference(cont.size());
            typedef BOOST_DEDUCED_TYPENAME std::vector<value_t>::iterator iterator_t;

            multiply_by_x<int> fn(2);

            iterator_t reference_it
                = std::transform(cont.begin(), cont.end(), reference.begin(), fn);

            boost::ignore_unused_variable_warning(reference_it);

            iterator_t test_it
                = boost::transform(cont, target.begin(), fn);

            BOOST_CHECK( test_it == target.end() );
            BOOST_CHECK_EQUAL_COLLECTIONS( reference.begin(), reference.end(),
                                           target.begin(), target.end() );

            BOOST_CHECK( test_it == boost::transform(boost::make_iterator_range(cont), target.begin(), fn) );
            BOOST_CHECK_EQUAL_COLLECTIONS( reference.begin(), reference.end(),
                                           target.begin(), target.end() );

            target.clear();
            target.resize(ccont.size());

            test_it = boost::transform(ccont, target.begin(), fn);

            BOOST_CHECK( test_it == target.end() );
            BOOST_CHECK_EQUAL_COLLECTIONS( reference.begin(), reference.end(),
                                           target.begin(), target.end() );
            BOOST_CHECK( test_it == boost::transform(boost::make_iterator_range(ccont), target.begin(), fn) );
            BOOST_CHECK_EQUAL_COLLECTIONS( reference.begin(), reference.end(),
                                           target.begin(), target.end() );
        }

        template< class Container >
        void test_transform_impl1()
        {
            using namespace boost::assign;

            Container cont;

            test_transform_impl1(cont);

            cont += 1;
            test_transform_impl1(cont);

            cont += 2,3,4,5,6,7;
            test_transform_impl1(cont);
        }

        template< class Container1, class Container2 >
        void test_transform_impl2(Container1& cont1, Container2& cont2)
        {
            const Container1& ccont1 = cont1;
            const Container2& ccont2 = cont2;

            BOOST_CHECK_EQUAL( cont1.size(), cont2.size() );

            typedef BOOST_DEDUCED_TYPENAME Container1::value_type value_t;

            std::vector<value_t> target(cont1.size());
            std::vector<value_t> reference(cont1.size());
            typedef BOOST_DEDUCED_TYPENAME std::vector<value_t>::iterator iterator_t;

            std::multiplies<int> fn;

            iterator_t reference_it
                = std::transform(cont1.begin(), cont1.end(),
                                 cont2.begin(), reference.begin(), fn);

            boost::ignore_unused_variable_warning(reference_it);

            iterator_t test_it
                = boost::transform(cont1, cont2, target.begin(), fn);

            BOOST_CHECK( test_it == target.end() );
            BOOST_CHECK_EQUAL_COLLECTIONS( reference.begin(), reference.end(),
                                           target.begin(), target.end() );

            BOOST_CHECK( test_it == boost::transform(boost::make_iterator_range(cont1), cont2, target.begin(), fn) );
            BOOST_CHECK_EQUAL_COLLECTIONS( reference.begin(), reference.end(),
                                           target.begin(), target.end() );

            BOOST_CHECK( test_it == boost::transform(cont1, boost::make_iterator_range(cont2), target.begin(), fn) );
            BOOST_CHECK_EQUAL_COLLECTIONS( reference.begin(), reference.end(),
                                           target.begin(), target.end() );

            BOOST_CHECK( test_it == boost::transform(boost::make_iterator_range(cont1),
                                                     boost::make_iterator_range(cont2),
                                                     target.begin(), fn) );
            BOOST_CHECK_EQUAL_COLLECTIONS( reference.begin(), reference.end(),
                                           target.begin(), target.end() );

            target.clear();
            target.resize(ccont1.size());

            test_it = boost::transform(ccont1, ccont2, target.begin(), fn);

            BOOST_CHECK( test_it == target.end() );
            BOOST_CHECK_EQUAL_COLLECTIONS( reference.begin(), reference.end(),
                target.begin(), target.end() );
        }

        template< class Container1, class Container2 >
        void test_transform_impl2()
        {
            using namespace boost::assign;

            Container1 cont1;
            Container2 cont2;

            test_transform_impl2(cont1, cont2);

            cont1 += 1;
            cont2 += 2;
            test_transform_impl2(cont1, cont2);

            cont1 += 2,3,4,5,6,7;
            cont2 += 4,6,8,10,12,14;
            test_transform_impl2(cont1, cont2);
        }

        void test_transform()
        {
            test_transform_impl1< std::vector<int> >();
            test_transform_impl1< std::list<int> >();
            test_transform_impl1< std::set<int> >();
            test_transform_impl1< std::multiset<int> >();

            test_transform_impl2< std::vector<int>, std::list<int> >();
            test_transform_impl2< std::list<int>, std::vector<int> >();
            test_transform_impl2< std::set<int>, std::set<int> >();
            test_transform_impl2< std::multiset<int>, std::list<int> >();
        }
    }
}

boost::unit_test::test_suite*
init_unit_test_suite(int argc, char* argv[])
{
    boost::unit_test::test_suite* test
        = BOOST_TEST_SUITE( "RangeTestSuite.algorithm.transform" );

    test->add( BOOST_TEST_CASE( &boost::test_transform ) );

    return test;
}


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

  @copyright 2017 NumScale SAS

  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_ALGORITHM_LEXICOGRAPHICAL_COMPARE_HPP_INCLUDED
#define BOOST_SIMD_ALGORITHM_LEXICOGRAPHICAL_COMPARE_HPP_INCLUDED

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/function/aligned_load.hpp>
#include <boost/simd/function/load.hpp>
#include <boost/simd/function/compare_less.hpp>
#include <boost/simd/function/compare_equal.hpp>
#include <boost/simd/function/all.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/detail/is_aligned.hpp>
#include <iterator>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Returns true if the range [first1, last1) is lexicographically less than the second range
    [first2, last2), and false otherwise

    @param first1, last1  -   the first range of elements to compare
    @param first2, last2  -   the second range of elements to compare

    @par Requirement

      - @c first1 , @c last1  @c first2 , @c last2 must be pointer to Vectorizable type.

    @par Example:

      @snippet lexicographical_comparerange.cpp lexicographical_comparerange

    @par Possible output:

      @snippet lexicographical_comparerange.txt lexicographical_comparerange

  **/
  template<typename T>
  bool lexicographical_compare(T const* first1, T const* last1, const T* first2, const T* last2)
  {
    using vT = pack<T>;
    using  itype_t = typename std::iterator_traits<const T*>::difference_type;
    itype_t d1 =  std::distance(first1, last1);
    itype_t d2 =  std::distance(first2, last2);
    bool shorter =  d1 < d2;
    auto last1b = shorter ?  last1 : first1+d2;

    auto pr = segmented_aligned_range(first1,last1b);

    // prologue
    for( auto const & e : pr.head ){
      if (e < *first2) return true;
      if (*first2 < e) return false;
      ++first2;
    }

    // main SIMD part - checks if we can store efficiently or not
    if(boost::simd::detail::is_aligned(first2, vT::alignment))
    {
      for( auto const& e : pr.body )
      {
        auto e2 = aligned_load<vT>(first2);
        if(compare_less(e, e2)) return true;
        if(compare_less(e2, e)) return false;
        first2 += vT::static_size;
      }
    }
    else
    {
      for( auto const& e : pr.body )
      {
        auto e2 = load<vT>(first2);
        if(compare_less(e, e2)) return true;
        if(compare_less(e2, e)) return false;
        first2 += vT::static_size;
      }
    }

    // epilogue
    for( auto const & e : pr.tail ){
      if (e < *first2) return true;
      if (*first2 < e) return false;
      ++first2;
    }
    return shorter;
  }

  /*!
    @ingroup group-algo

     Returns true if the range [first1, last1) is lexicographically less than the second range
    [first2, first2 + (last1 - first1)), and false otherwise using comp as elementwise comparison

    @param first1, last1  -   the first range of the elements to compare
    @param first2, last2  -   the second range of the elements to compare
    @param comp           -   binary predicate operation function object that will be applied.

    @par Requirement

      - @c first, @c last and @c out must be pointer to Vectorizable type.

      - @c comp must be a polymorphic binary function object, i.e callable on generic types.

      - Two ranges are compared element by element.
      - The first mismatching element defines which range is lexicographically less or greater than the other.
      - If one range is a prefix of another, the shorter range is lexicographically less than the other.
      - If two ranges have equivalent elements and are of the same length, then the ranges are lexicographically equal.
      - An empty range is lexicographically less than any non-empty range.
      - Two empty ranges are lexicographically equal.

      @par Example:
         @snippet lexicographical_comparerange.cpp lexicographical_comparerange
      @par Possible output:
         @snippet lexicographical_comparerange.txt lexicographical_comparerange

  **/
  template<typename T, typename Pred>
  bool lexicographical_compare(T const* first1, T const* last1, T const* first2, T const* last2, Pred comp)
  {
    using vT = pack<T>;
    using  itype_t = typename std::iterator_traits<const T*>::difference_type;
    itype_t d1 =  std::distance(first1, last1);
    itype_t d2 =  std::distance(first2, last2);
    bool shorter =  d1 < d2;
    auto last1b = shorter ?  last1 : first1+d2;

    auto pr = segmented_aligned_range(first1,last1b);
    // prologue
    for( auto const & e : pr.head ){
      if (comp(e, *first2)) return true;
      if (comp(*first2, e)) return false;
      ++first2;
    }

    // main SIMD part - checks if we can store efficiently or not
    if(boost::simd::detail::is_aligned(first2, vT::alignment))
    {
      for( auto const& e : pr.body )
      {
        auto e2 = aligned_load<vT>(first2);
        if(comp(e, e2)) return true;
        if(comp(e2, e)) return false;
        first2 += vT::static_size;
      }
    }
    else
    {
      for( auto const& e : pr.body )
      {
        auto e2 = load<vT>(first2);
        if(comp(e, e2)) return true;
        if(comp(e2, e)) return false;
        first2 += vT::static_size;
      }
    }

    // epilogue
    for( auto const & e : pr.tail ){
      if (comp(e, *first2)) return true;
      if (comp(*first2, e)) return false;
      ++first2;
    }
    return shorter;
  }

} }

#endif


//  (C) Copyright Steve Cleary & John Maddock 2000.
//  Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

//  See http://www.boost.org for most recent version including documentation.

#include <climits>
#include <cwchar>
#include <boost/static_assert.hpp>

#if !defined(WCHAR_MIN)
#define WCHAR_MIN 0
#endif

namespace boost{

namespace my_conditions {
BOOST_STATIC_ASSERT(sizeof(int) * CHAR_BIT >= 32);
BOOST_STATIC_ASSERT(WCHAR_MIN >= 0);

} // namespace my_conditions

} // namespace boost

int main()
{
   return 0;
}


// ****************************************************************************
// test/benchmark.pool_iteration.cpp
//
// benchmarks the varoious ways that we can interate over the pools.
//
// Copyright Chris Glover 2014-2016
//
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// ****************************************************************************

#include "entity/all.hpp"
#include "benchmark/benchmark.h"
#include <boost/iterator/zip_iterator.hpp>

#include "entity/component/detail/get_helper.hpp"
#include "entity/iterator/zip_iterator.hpp"

// -----------------------------------------------------------------------------
//
static const float kFrameTime = 0.016f;

// -----------------------------------------------------------------------------
//
struct jerk
{
	template<typename T>
	void operator()(T a)
	{
		if(a)
			*a += (0.001f *  kFrameTime);
	}
};

// -----------------------------------------------------------------------------
//
struct accelerate
{
	template<typename T>
	void operator()(T av)
	{
		using std::get;
		using boost::get;
		auto a = get<0>(av);
		auto v = get<1>(av);
		if(a && v)
			*v += *a * kFrameTime;
	}
};

// -----------------------------------------------------------------------------
//
struct move
{
	template<typename T>
	void operator()(T vp)
	{
		using std::get;
		using boost::get;
		auto v = get<0>(vp);
		auto p = get<1>(vp);
		if(v && p)
			*p += *v * kFrameTime;
	}
};

// -----------------------------------------------------------------------------
//
template<typename ComponentPool>
class ComponentFixture : public benchmark::Fixture
{
public:

	typedef ComponentPool pool_type;

	ComponentFixture()
		: accel_pool(entities)
		, velocity_pool(entities)
		, position_pool(entities)
	{}

	void SetUp(benchmark::State const& st) override
	{
		int num_entities = st.range_x();

		for (int i = 0; i < num_entities; ++i)
		{
			entities.create();
		}

		for(auto const& e : entities)
		{
			*position_pool.create(e, 0.f) = 0.f;
			*velocity_pool.create(e, 0.f) = 0.f;
			*accel_pool.create(e, 0.f) = 9.8f;
		}
	}

	void IterateRaw(benchmark::State& st)
	{
		while (st.KeepRunning())
		{
			float* a = &*accel_pool.get(entity::make_entity(0));
			//#pragma loop(no_vector)
			for(std::size_t i = 0, s = entities.size(); i < s; ++i)
			{
				a[i] += 0.001f * kFrameTime;
			}

			float* v = &*velocity_pool.get(entity::make_entity(0));
			//#pragma loop(no_vector)
			for(std::size_t i = 0, s = entities.size(); i < s; ++i)
			{
				v[i] += a[i] * kFrameTime;
			}

			float* p = &*position_pool.get(entity::make_entity(0));
			//#pragma loop(no_vector)
			for(std::size_t i = 0, s = entities.size(); i < s; ++i)
			{
				p[i] += v[i] * kFrameTime;
			}
		}
	}

	void IterateIndexed(benchmark::State& st)
	{
		while (st.KeepRunning())
		{
			for(entity::entity_index_t i = 0, s = entities.size(); i < s; ++i)
			{
				auto accel = accel_pool.get(entity::make_entity(i));
				if(accel)
					*accel += 0.001f * kFrameTime;
			}

			for(entity::entity_index_t i = 0, s = entities.size(); i < s; ++i)
			{
				auto accel = accel_pool.get(entity::make_entity(i));
				auto velocity = velocity_pool.get(entity::make_entity(i));
				if(accel && velocity)
					*velocity += *accel * kFrameTime;
			}

			for(entity::entity_index_t i = 0, s = entities.size(); i < s; ++i)
			{
				auto velocity = velocity_pool.get(entity::make_entity(i));
				auto position = position_pool.get(entity::make_entity(i));
				if(velocity && position)
					*position += *velocity * kFrameTime;
			}
		}
	}

	void IterateUnchecked(benchmark::State& st)
	{
		while (st.KeepRunning())
		{
			auto accel_begin = accel_pool.begin();
			auto accel_end = accel_pool.end();
			for(auto accel = accel_begin; accel != accel_end; ++accel)
			{
				*accel += 0.001f * kFrameTime;
			}

			auto velocity_begin = velocity_pool.begin();
			auto velocity_end = velocity_pool.end();
			for(auto accel = accel_begin, velocity = velocity_begin; velocity != velocity_end; ++velocity, ++accel)
			{
				*velocity += *accel * kFrameTime;
			}

			auto position_begin = position_pool.begin();
			auto position_end = position_pool.end();
			for(auto velocity = velocity_begin, position = position_begin; position != position_end; ++position, ++velocity)
			{
				*position += *velocity * kFrameTime;
			}
		}
	}

	void IterateChecked(benchmark::State& st)
	{
		while (st.KeepRunning())
		{
			auto accel_begin = accel_pool.optional_begin();
			auto accel_end = accel_pool.optional_end();
			for(auto accel = accel_begin; accel != accel_end; ++accel)
			{
				**accel += 0.001f * kFrameTime;
			}

			auto velocity_begin = velocity_pool.optional_begin();
			auto velocity_end = velocity_pool.optional_end();
			for(auto accel = accel_begin, velocity = velocity_begin; velocity != velocity_end; ++velocity, ++accel)
			{
				**velocity += **accel * kFrameTime;
			}

			auto position_begin = position_pool.optional_begin();
			auto position_end = position_pool.optional_end();
			for(auto velocity = velocity_begin, position = position_begin; position != position_end; ++position, ++velocity)
			{
				**position += **velocity * kFrameTime;
			}
		}
	}

	void IterateGetHelper(benchmark::State& st)
	{
		while (st.KeepRunning())
		{
			auto a_getter = entity::component::detail::make_get_helper(accel_pool);
			auto v_getter = entity::component::detail::make_get_helper(velocity_pool);
			auto p_getter = entity::component::detail::make_get_helper(position_pool);

			for(entity::entity_index_t i = 0, s = entities.size(); i < s; ++i)
			{
				auto accel = a_getter.get(entity::make_entity(i));
				if(accel)
					*accel += 0.001f * kFrameTime;
			}

			for(entity::entity_index_t i = 0, s = entities.size(); i < s; ++i)
			{
				auto accel = a_getter.get(entity::make_entity(i));
				auto velocity = v_getter.get(entity::make_entity(i));
				if(accel && velocity)
					*velocity += *accel * kFrameTime;
			}

			for(entity::entity_index_t i = 0, s = entities.size(); i < s; ++i)
			{
				auto velocity = v_getter.get(entity::make_entity(i));
				auto position = p_getter.get(entity::make_entity(i));
				if(velocity && position)
					*position += *velocity * kFrameTime;
			}
		}
	}

	void IterateZip(benchmark::State& st)
	{
		while (st.KeepRunning())
		{
			std::for_each(
				accel_pool.optional_begin(),
				accel_pool.optional_end(),
				jerk()
			);

			std::for_each(
				entity::iterator::make_zip_iterator(entities.begin(), accel_pool, velocity_pool),
				entity::iterator::make_zip_iterator(entities.end(), accel_pool, velocity_pool),
				accelerate()
			);

			std::for_each(
				entity::iterator::make_zip_iterator(entities.begin(), velocity_pool, position_pool),
				entity::iterator::make_zip_iterator(entities.end(), velocity_pool, position_pool),
				move()
			);
		}
	}

	void IterateRange(benchmark::State& st)
	{
		while (st.KeepRunning())
		{
			auto ar = entity::range::make_optional_range(accel_pool);
			std::for_each(ar.begin(), ar.end(), jerk());

			auto avr = entity::range::combine(entities, accel_pool, velocity_pool);
			std::for_each(avr.begin(), avr.end(), accelerate());

			auto vpr = entity::range::combine(entities, velocity_pool, position_pool);
			std::for_each(vpr.begin(), vpr.end(), move());
		}
	}

	void IterateOptional(benchmark::State& st)
	{
		while (st.KeepRunning())
		{
			std::for_each(
				accel_pool.optional_begin(),
				accel_pool.optional_end(),
				jerk()
			);

			auto av_begin = boost::make_zip_iterator(boost::make_tuple(accel_pool.optional_begin(), velocity_pool.optional_begin()));
			auto av_end = boost::make_zip_iterator(boost::make_tuple(accel_pool.optional_end(), velocity_pool.optional_end()));
			std::for_each(
				av_begin,
				av_end,
				accelerate()
			);

			auto vp_begin = boost::make_zip_iterator(boost::make_tuple(velocity_pool.optional_begin(), position_pool.optional_begin()));
			auto vp_end = boost::make_zip_iterator(boost::make_tuple(velocity_pool.optional_end(), position_pool.optional_end()));
			std::for_each(
				vp_begin,
				vp_end,
				move()
			);
		}
	}

private:

	entity::entity_pool entities;
	ComponentPool accel_pool;
	ComponentPool velocity_pool;
	ComponentPool position_pool;
};

// Benchmark macros can't handle templates, so we'll use typdefs.
typedef ComponentFixture<entity::component::saturated_pool<float>> SaturatedFixture;
typedef ComponentFixture<entity::component::dense_pool<float>> DenseFixture;
typedef ComponentFixture<entity::component::sparse_pool<float>> SparseFixture;

// Add new pool types here.
#define POOLS				\
	POOL(SaturatedFixture)	\
	POOL(DenseFixture)		\
	POOL(SparseFixture)		\

// Add new tests here.
#define COMMON_TESTS(pool)					\
	TEST(IterateRaw, pool)					\
	TEST(IterateIndexed, pool)				\
	TEST(IterateUnchecked, pool)			\
	TEST(IterateChecked, pool)				\
	TEST(IterateGetHelper, pool)			\
	TEST(IterateZip, pool)					\
	TEST(IterateRange, pool)				\
	TEST(IterateOptional, pool)				\

// -----------------------------------------------------------------------------
// Auto instantiate tests here.
#ifdef _DEBUG
#  define BM_REGISTER(Fixture, Test) BENCHMARK_REGISTER_F(Fixture, Test)->Arg(1024)
#else
#  define BM_REGISTER(Fixture, Test) BENCHMARK_REGISTER_F(Fixture, Test)->Arg(1024)->Arg(1024 * 2048)
#endif

#define POOL(p) COMMON_TESTS(p)
#define TEST(t, p) \
	BENCHMARK_DEFINE_F(p, t)(benchmark::State& st)	\
	{												\
		t(st);										\
	}												\
	BM_REGISTER(p, t);								\

//
POOLS

#undef TEST
#undef POOL


// Copyright (C) 2005  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <sstream>
#include <string>
#include <ctime>
#include <cstdlib>

#include <dlib/entropy_encoder_model.h>
#include <dlib/entropy_decoder_model.h>
#include <dlib/entropy_encoder.h>
#include <dlib/entropy_decoder.h>
#include "tester.h"

namespace
{
    using namespace test;
    using namespace std;
    using namespace dlib;

    logger dlog("test.entropy_coder_model");

    template <
        typename ee,
        typename ed
        >
    void entropy_encoder_model_kernel_test (
    )
    /*!
        requires
            - ee is an implementation of entropy_encoder_model/entropy_encoder_model_kernel_abstract.h
              the alphabet_size for ee is 256
            - ed is an implementation of entropy_decoder_model/entropy_decoder_model_kernel_abstract.h
              the alphabet_size for ed is 256
            - ee and ed must share the same kernel number
        ensures
            - runs tests on ee and ed for compliance with the specs
    !*/
    {

        print_spinner();
        srand(static_cast<unsigned int>(time(0)));

        typedef typename ee::entropy_encoder_type ee_type;
        typedef typename ed::entropy_decoder_type ed_type;

        {

            ee_type ecoder;
            ed_type dcoder;

            ee elen(ecoder);
            ed dlen(dcoder);
            ee elit(ecoder);
            ed dlit(dcoder);

            istringstream sin;
            ostringstream sout;

            ecoder.set_stream(sout);

            unsigned long temp;

            elen.encode(0);
            elit.encode(9);

            elen.encode(0);
            elit.encode(0);

            elen.encode(0);
            elit.encode(4);

            elen.encode(0);
            elit.encode(0);

            elen.encode(0);
            elit.encode(2);

            elen.encode(0);
            elit.encode(0);

            ecoder.clear();
            sin.str(sout.str());
            dcoder.set_stream(sin);

            dlen.decode(temp);
            DLIB_TEST(temp == 0);
            dlit.decode(temp);
            DLIB_TEST(temp == 9);

            dlen.decode(temp);
            DLIB_TEST(temp == 0);
            dlit.decode(temp);
            DLIB_TEST(temp == 0);

            dlen.decode(temp);
            DLIB_TEST(temp == 0);
            dlit.decode(temp);
            DLIB_TEST(temp == 4);

            dlen.decode(temp);
            DLIB_TEST(temp == 0);
            dlit.decode(temp);
            DLIB_TEST(temp == 0);

            dlen.decode(temp);
            DLIB_TEST(temp == 0);
            dlit.decode(temp);
            DLIB_TEST(temp == 2);

            dlen.decode(temp);
            DLIB_TEST(temp == 0);
            dlit.decode(temp);
            DLIB_TEST(temp == 0);

        }

    }

    class entropy_encoder_model_tester : public tester
    {
    public:
        entropy_encoder_model_tester (
        ) :
            tester ("test_entropy_coder_model",
                    "Runs tests on the entropy_encoder_model and entropy_decoder_model components.")
        {}

        void perform_test (
        )
        {
            typedef entropy_encoder::kernel_2a_c ee;
            typedef entropy_decoder::kernel_2a_c ed;

            dlog << LINFO << "testing kernel_1a";
            entropy_encoder_model_kernel_test<
                entropy_encoder_model<256,ee>::kernel_1a,
                entropy_decoder_model<256,ed>::kernel_1a>();

            dlog << LINFO << "testing kernel_2a";
            entropy_encoder_model_kernel_test<
                entropy_encoder_model<256,ee>::kernel_2a,
                entropy_decoder_model<256,ed>::kernel_2a>();

            dlog << LINFO << "testing kernel_3a";
            entropy_encoder_model_kernel_test<
                entropy_encoder_model<256,ee>::kernel_3a,
                entropy_decoder_model<256,ed>::kernel_3a>();

            dlog << LINFO << "testing kernel_4a";
            entropy_encoder_model_kernel_test<
                entropy_encoder_model<256,ee>::kernel_4a,
                entropy_decoder_model<256,ed>::kernel_4a>();

            dlog << LINFO << "testing kernel_4b";
            entropy_encoder_model_kernel_test<
                entropy_encoder_model<256,ee>::kernel_4b,
                entropy_decoder_model<256,ed>::kernel_4b>();

            dlog << LINFO << "testing kernel_5a";
            entropy_encoder_model_kernel_test<
                entropy_encoder_model<256,ee>::kernel_5a,
                entropy_decoder_model<256,ed>::kernel_5a>();

            dlog << LINFO << "testing kernel_5c";
            entropy_encoder_model_kernel_test<
                entropy_encoder_model<256,ee>::kernel_5c,
                entropy_decoder_model<256,ed>::kernel_5c>();

            dlog << LINFO << "testing kernel_6a";
            entropy_encoder_model_kernel_test<
                entropy_encoder_model<256,ee>::kernel_6a,
                entropy_decoder_model<256,ed>::kernel_6a>();

        }
    } a;

}