#ifndef CX_DQMC_CHECKERBOARD_HPP
#define CX_DQMC_CHECKERBOARD_HPP

#include "la.hpp"
#include "parameters.hpp"

#include <boost/multi_array.hpp>

#include <exception>
#include <cmath>
#include <memory>

namespace cx_dqmc {
    namespace checkerboard {

	inline void graph_to_checkerboard(dqmc::parameters& p, cx_dqmc::workspace& ws) {
	    for (int i = 0; i < p.graph.num_bonds(); ++i) {
		ws.bond_used[i] = -1;
	    }

	    int bonds_used = 0;
	    int sites_used = 0;
	    int first_unused_bond = 0;

	    alps::graph_helper<>::bond_iterator itr1, itr1_end;
	    int b, s1, s2;
	    double t, im_t;
	    while (bonds_used < p.graph.num_bonds()) {
		sp_mat hopping(p.N, p.N);
		sp_mat hopping_inv(p.N, p.N);

		cx_sp_mat cx_hopping(p.N, p.N);
		cx_sp_mat cx_hopping_inv(p.N, p.N);

		int elements = 0;

		for (int i = 0; i < p.N; ++i) { ws.site_used[i] = -1; }

		for (boost::tie(itr1, itr1_end) = p.graph.bonds(); itr1 != itr1_end; ++itr1) {
		    b = p.graph.index(*itr1);
		    s1 = p.graph.source(*itr1);
		    s2 = p.graph.target(*itr1);

		    if (ws.bond_used[b] == 1) continue;
		    if (ws.site_used[s1] == 1 || ws.site_used[s2] == 1) continue;

		    ws.bond_used[b] = 1;
		    ws.site_used[s1] = 1;
		    ws.site_used[s2] = 1;
		    t = p.ts[p.graph.bond_type(*itr1)];

		    if (p.complex_hoppings == false) {
			hopping.insert(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping.insert(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping.insert(s1, s2) = sinh(t * p.delta_tau/2.);
			hopping.insert(s2, s1) = sinh(t * p.delta_tau/2.);

			hopping_inv.insert(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping_inv.insert(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping_inv.insert(s1, s2) = -sinh(t * p.delta_tau/2.);
			hopping_inv.insert(s2, s1) = -sinh(t * p.delta_tau/2.);
		    } else {
			im_t = p.im_ts[p.graph.bond_type(*itr1)];
			std::complex<double> cx_t(t, im_t);
			std::complex<double> abs_t(sqrt(t*t + im_t*im_t), 0);
			cx_hopping.insert(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping.insert(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping.insert(s1, s2) = sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t;
			cx_hopping.insert(s2, s1) = sinh(abs_t * p.delta_tau/2.) * std::conj(cx_t / abs_t);

			cx_hopping_inv.insert(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_inv.insert(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_inv.insert(s1, s2) = -(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);
			cx_hopping_inv.insert(s2, s1) = -sinh(abs_t * p.delta_tau/2.) * std::conj(cx_t / abs_t);
		    }
		    // std::cout << "Using " << s1 << " and " << s2 << std::endl;
		    ++bonds_used;
		    ++elements;
		}

		if (elements != 0) {
		    for (int i = 0; i < p.N; ++i) {
			if(ws.site_used[i] == -1) {
			    if (p.complex_hoppings == false) {
				hopping.insert(i, i) = 1.;
				hopping_inv.insert(i, i) = 1.;
			    } else {
				cx_hopping.insert(i, i) = std::complex<double>(1., 0.);
				cx_hopping_inv.insert(i, i) = std::complex<double>(1., 0.);
			    }
			}
		    }
		    // arma::mat(hopping).print("A hopping matrix");

		    if (p.complex_hoppings == false) {
			ws.sparse_hoppings.push_back(hopping);
			ws.sparse_hoppings_inv.push_back(hopping_inv);
		    } else {
			ws.cx_sparse_hoppings.push_back(cx_hopping);
			ws.cx_sparse_hoppings_inv.push_back(cx_hopping_inv);
		    }
		}
	    }
	}

	inline void graph_to_checkerboard_renyi(dqmc::parameters& p, cx_dqmc::workspace& ws) {
	    for (int i = 0; i < p.graph.num_bonds(); ++i) { ws.bond_used[i] = -1; }

	    int bonds_used = 0;
	    int sites_used = 0;
	    int first_unused_bond = 0;

	    alps::graph_helper<>::bond_iterator itr1, itr1_end;
	    int b, s1, s2, s1a, s2a;
	    double t;
	    std::complex<double> one(1., 0.);

	    while (bonds_used < p.graph.num_bonds()) {
		sp_mat hopping_0(ws.vol, ws.vol), hopping_0_inv(ws.vol, ws.vol),
		    hopping_1(ws.vol, ws.vol), hopping_1_inv(ws.vol, ws.vol),
		    hopping_2(ws.vol, ws.vol), hopping_2_inv(ws.vol, ws.vol),
		    hopping_3(ws.vol, ws.vol), hopping_3_inv(ws.vol, ws.vol);

		cx_sp_mat cx_hopping_0(ws.vol, ws.vol), cx_hopping_0_inv(ws.vol, ws.vol),
		    cx_hopping_1(ws.vol, ws.vol), cx_hopping_1_inv(ws.vol, ws.vol),
		    cx_hopping_2(ws.vol, ws.vol), cx_hopping_2_inv(ws.vol, ws.vol),
		    cx_hopping_3(ws.vol, ws.vol), cx_hopping_3_inv(ws.vol, ws.vol);

		int elements = 0;

		for (int i = 0; i < p.N; ++i) ws.site_used[i] = -1;
		for (int i = 0; i < p.N + p.n_B; ++i) {
		    if (p.complex_hoppings == false) {
			hopping_0.insert(i, i) = 1.;
			hopping_0_inv.insert(i, i) = 1.;
			hopping_1.insert(i, i) = 1.;
			hopping_1_inv.insert(i, i) = 1.;
			hopping_2.insert(i, i) = 1.;
			hopping_2_inv.insert(i, i) = 1.;
			hopping_3.insert(i, i) = 1.;
			hopping_3_inv.insert(i, i) = 1.;
		    } else {
			cx_hopping_0.insert(i, i) = one;
			cx_hopping_0_inv.insert(i, i) = one;
			cx_hopping_1.insert(i, i) = one;
			cx_hopping_1_inv.insert(i, i) = one;
			cx_hopping_2.insert(i, i) = one;
			cx_hopping_2_inv.insert(i, i) = one;
			cx_hopping_3.insert(i, i) = one;
			cx_hopping_3_inv.insert(i, i) = one;
		    }
		}

		for (boost::tie(itr1, itr1_end) = p.graph.bonds(); itr1 != itr1_end; ++itr1) {
		    b = p.graph.index(*itr1);
		    s1 = p.graph.source(*itr1);
		    s2 = p.graph.target(*itr1);
		    s1a = s1;
		    s2a = s2;

		    if (ws.bond_used[b] == 1) continue;
		    if (ws.site_used[s1] == 1 || ws.site_used[s2] == 1) continue;

		    ws.bond_used[b] = 1;
		    ws.site_used[s1] = 1;
		    ws.site_used[s2] = 1;

		    t = p.ts[p.graph.bond_type(*itr1)];

		    if (s1 >= p.n_A) s1a = s1 + p.n_B;
		    if (s2 >= p.n_A) s2a = s2 + p.n_B;

		    if (p.complex_hoppings == false) {
			hopping_0.coeffRef(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping_0.coeffRef(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping_0.coeffRef(s1, s2) = sinh(t * p.delta_tau/2.);
			hopping_0.coeffRef(s2, s1) = sinh(t * p.delta_tau/2.);

			hopping_0_inv.coeffRef(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping_0_inv.coeffRef(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping_0_inv.coeffRef(s1, s2) = -sinh(t * p.delta_tau/2.);
			hopping_0_inv.coeffRef(s2, s1) = -sinh(t * p.delta_tau/2.);

			hopping_1.coeffRef(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping_1.coeffRef(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping_1.coeffRef(s1, s2) = sinh(t * p.delta_tau/2.);
			hopping_1.coeffRef(s2, s1) = sinh(t * p.delta_tau/2.);

			hopping_1_inv.coeffRef(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping_1_inv.coeffRef(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping_1_inv.coeffRef(s1, s2) = -sinh(t * p.delta_tau/2.);
			hopping_1_inv.coeffRef(s2, s1) = -sinh(t * p.delta_tau/2.);

			hopping_2.coeffRef(s1a, s1a) = cosh(t * p.delta_tau/2.);
			hopping_2.coeffRef(s2a, s2a) = cosh(t * p.delta_tau/2.);
			hopping_2.coeffRef(s1a, s2a) = sinh(t * p.delta_tau/2.);
			hopping_2.coeffRef(s2a, s1a) = sinh(t * p.delta_tau/2.);

			hopping_2_inv.coeffRef(s1a, s1a) = cosh(t * p.delta_tau/2.);
			hopping_2_inv.coeffRef(s2a, s2a) = cosh(t * p.delta_tau/2.);
			hopping_2_inv.coeffRef(s1a, s2a) = -sinh(t * p.delta_tau/2.);
			hopping_2_inv.coeffRef(s2a, s1a) = -sinh(t * p.delta_tau/2.);

			hopping_3.coeffRef(s1a, s1a) = cosh(t * p.delta_tau/2.);
			hopping_3.coeffRef(s2a, s2a) = cosh(t * p.delta_tau/2.);
			hopping_3.coeffRef(s1a, s2a) = sinh(t * p.delta_tau/2.);
			hopping_3.coeffRef(s2a, s1a) = sinh(t * p.delta_tau/2.);

			hopping_3_inv.coeffRef(s1a, s1a) = cosh(t * p.delta_tau/2.);
			hopping_3_inv.coeffRef(s2a, s2a) = cosh(t * p.delta_tau/2.);
			hopping_3_inv.coeffRef(s1a, s2a) = -sinh(t * p.delta_tau/2.);
			hopping_3_inv.coeffRef(s2a, s1a) = -sinh(t * p.delta_tau/2.);
		    } else {
			double im_t = p.im_ts[p.graph.bond_type(*itr1)];
			std::complex<double> cx_t(t, im_t);
			std::complex<double> abs_t(sqrt(t*t + im_t*im_t), 0);
			cx_hopping_0.coeffRef(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_0.coeffRef(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_0.coeffRef(s1, s2) = sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t;
			cx_hopping_0.coeffRef(s2, s1) = sinh(abs_t * p.delta_tau/2.) * std::conj(cx_t / abs_t);

			cx_hopping_0_inv.coeffRef(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_0_inv.coeffRef(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_0_inv.coeffRef(s1, s2) = -(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t ) ;
			cx_hopping_0_inv.coeffRef(s2, s1) = -sinh(abs_t * p.delta_tau/2.) * std::conj(cx_t / abs_t );

			cx_hopping_1.coeffRef(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_1.coeffRef(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_1.coeffRef(s1, s2) = sinh(abs_t * p.delta_tau/2.)  * cx_t / abs_t;
			cx_hopping_1.coeffRef(s2, s1) = std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);

			cx_hopping_1_inv.coeffRef(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_1_inv.coeffRef(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_1_inv.coeffRef(s1, s2) = -(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);
			cx_hopping_1_inv.coeffRef(s2, s1) = -std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);

			cx_hopping_2.coeffRef(s1a, s1a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_2.coeffRef(s2a, s2a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_2.coeffRef(s1a, s2a) = sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t;
			cx_hopping_2.coeffRef(s2a, s1a) = std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);

			cx_hopping_2_inv.coeffRef(s1a, s1a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_2_inv.coeffRef(s2a, s2a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_2_inv.coeffRef(s1a, s2a) = -(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);
			cx_hopping_2_inv.coeffRef(s2a, s1a) = -std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);

			cx_hopping_3.coeffRef(s1a, s1a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_3.coeffRef(s2a, s2a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_3.coeffRef(s1a, s2a) = sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t;
			cx_hopping_3.coeffRef(s2a, s1a) = std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);

			cx_hopping_3_inv.coeffRef(s1a, s1a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_3_inv.coeffRef(s2a, s2a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_3_inv.coeffRef(s1a, s2a) = -(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);
			cx_hopping_3_inv.coeffRef(s2a, s1a) = -std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);
		    }
		    ++bonds_used;
		    ++elements;
		}

		if (elements != 0) {
		    if (p.complex_hoppings == false) {
			ws.sparse_hoppings_0.push_back(hopping_0);
			ws.sparse_hoppings_0_inv.push_back(hopping_0_inv);

			ws.sparse_hoppings_1.push_back(hopping_1);
			ws.sparse_hoppings_1_inv.push_back(hopping_1_inv);

			ws.sparse_hoppings_2.push_back(hopping_2);
			ws.sparse_hoppings_2_inv.push_back(hopping_2_inv);

			ws.sparse_hoppings_3.push_back(hopping_3);
			ws.sparse_hoppings_3_inv.push_back(hopping_3_inv);
		    } else {
			ws.cx_sparse_hoppings_0.push_back(cx_hopping_0);
			ws.cx_sparse_hoppings_0_inv.push_back(cx_hopping_0_inv);

			ws.cx_sparse_hoppings_1.push_back(cx_hopping_1);
			ws.cx_sparse_hoppings_1_inv.push_back(cx_hopping_1_inv);

			ws.cx_sparse_hoppings_2.push_back(cx_hopping_2);
			ws.cx_sparse_hoppings_2_inv.push_back(cx_hopping_2_inv);

			ws.cx_sparse_hoppings_3.push_back(cx_hopping_3);
			ws.cx_sparse_hoppings_3_inv.push_back(cx_hopping_3_inv);
		    }
		}
	    }
	}

	inline void hop_left(cx_dqmc::workspace * ws, cx_mat_t& M, double pref) {
	    using namespace std;
	    int par = 0;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings[i] * M;
			else
			    M = ws->sparse_hoppings[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings[i] * M;
			else
			    M = ws->sparse_hoppings[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings_inv[i] * M;
			else
			    M = ws->sparse_hoppings_inv[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings_inv[i] * M;
			else
			    M = ws->sparse_hoppings_inv[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings[i] * M;
			else
			    M = ws->cx_sparse_hoppings[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings[i] * M;
			else
			    M = ws->cx_sparse_hoppings[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings_inv[i] * M;
			else
			    M = ws->cx_sparse_hoppings_inv[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings_inv[i] * M;
			else
			    M = ws->cx_sparse_hoppings_inv[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }

	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_left_t(cx_dqmc::workspace * ws, cx_mat_t& M, double pref) {
	    using namespace std;
	    int par = 0;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings[i].transpose() * M;
			else
			    M = ws->sparse_hoppings[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings[i].transpose() * M;
			else
			    M = ws->sparse_hoppings[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings_inv[i].transpose() * M;
			else
			    M = ws->sparse_hoppings_inv[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings_inv[i].transpose() * M;
			else
			    M = ws->sparse_hoppings_inv[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
	    } else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings[i].transpose() * M;
			else
			    M = ws->cx_sparse_hoppings[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings[i].transpose() * M;
			else
			    M = ws->cx_sparse_hoppings[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings_inv[i].transpose() * M;
			else
			    M = ws->cx_sparse_hoppings_inv[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings_inv[i].transpose() * M;
			else
			    M = ws->cx_sparse_hoppings_inv[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }

	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_right(cx_dqmc::workspace * ws, cx_mat_t& M, double pref) {
	    int par = 0;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = M * ws->sparse_hoppings[i];
			else
			    M = ws->hop_temp * ws->sparse_hoppings[i];
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = M * ws->sparse_hoppings[i];
			else
			    M = ws->hop_temp * ws->sparse_hoppings[i];
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = M * ws->sparse_hoppings_inv[i];
			else
			    M = ws->hop_temp * ws->sparse_hoppings_inv[i];
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = M * ws->sparse_hoppings_inv[i];
			else
			    M = ws->hop_temp * ws->sparse_hoppings_inv[i];
			par = (par == 0) ? 1 : 0;
		    }
		}
	    } else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = M * ws->cx_sparse_hoppings[i];
			else
			    M = ws->hop_temp * ws->cx_sparse_hoppings[i];
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = M * ws->cx_sparse_hoppings[i];
			else
			    M = ws->hop_temp * ws->cx_sparse_hoppings[i];
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = M * ws->cx_sparse_hoppings_inv[i];
			else
			    M = ws->hop_temp * ws->cx_sparse_hoppings_inv[i];
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = M * ws->cx_sparse_hoppings_inv[i];
			else
			    M = ws->hop_temp * ws->cx_sparse_hoppings_inv[i];
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_left_renyi(cx_dqmc::workspace * ws,
				   cx_mat_t& M,
				   double pref,
				   int section) {
	    using namespace std;
	    int par = 0;
	    // cout << "section " << section << endl;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i] * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i] * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i] * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i] * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0[i] * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1[i] * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2[i] * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i] * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i] * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i] * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i] * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0[i] * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1[i] * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2[i] * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i] * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i] * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i] * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i] * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0_inv[i] * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1_inv[i] * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2_inv[i] * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3_inv[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i] * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i] * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i] * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i] * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0_inv[i] * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1_inv[i] * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2_inv[i] * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3_inv[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    } else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0[i] * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1[i] * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2[i] * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3[i] * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0[i] * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1[i] * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2[i] * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0[i] * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1[i] * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2[i] * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3[i] * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0[i] * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1[i] * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2[i] * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0_inv[i] * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1_inv[i] * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2_inv[i] * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3_inv[i] * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0_inv[i] * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1_inv[i] * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2_inv[i] * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3_inv[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0_inv[i] * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1_inv[i] * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2_inv[i] * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3_inv[i] * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0_inv[i] * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1_inv[i] * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2_inv[i] * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3_inv[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_left_renyi_t(cx_dqmc::workspace * ws,
				   cx_mat_t& M,
				   double pref,
				   int section) {
	    using namespace std;
	    int par = 0;
	    // cout << "section " << section << endl;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0_inv[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1_inv[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2_inv[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3_inv[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0_inv[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1_inv[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2_inv[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3_inv[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    } else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0_inv[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1_inv[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2_inv[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3_inv[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0_inv[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1_inv[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2_inv[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3_inv[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0_inv[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1_inv[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2_inv[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3_inv[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0_inv[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1_inv[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2_inv[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3_inv[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_right_renyi(cx_dqmc::workspace * ws,
				    cx_mat_t&__restrict__ M,
				    double pref, int section) {
	    int par = 0;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0[i];
			    if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1[i];
			    if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2[i];
			    if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0[i];
			    if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1[i];
			    if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2[i];
			    if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3[i];
			}
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0[i];
			    if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1[i];
			    if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2[i];
			    if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0[i];
			    if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1[i];
			    if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2[i];
			    if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3[i];
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0_inv[i];
			    if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1_inv[i];
			    if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2_inv[i];
			    if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3_inv[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0_inv[i];
			    if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1_inv[i];
			    if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2_inv[i];
			    if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3_inv[i];
			}
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0_inv[i];
			    if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1_inv[i];
			    if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2_inv[i];
			    if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3_inv[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0_inv[i];
			    if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1_inv[i];
			    if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2_inv[i];
			    if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3_inv[i];
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->cx_sparse_hoppings_0[i];
			    if (section == 1) ws->hop_temp = M * ws->cx_sparse_hoppings_1[i];
			    if (section == 2) ws->hop_temp = M * ws->cx_sparse_hoppings_2[i];
			    if (section == 3) ws->hop_temp = M * ws->cx_sparse_hoppings_3[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->cx_sparse_hoppings_0[i];
			    if (section == 1) M = ws->hop_temp * ws->cx_sparse_hoppings_1[i];
			    if (section == 2) M = ws->hop_temp * ws->cx_sparse_hoppings_2[i];
			    if (section == 3) M = ws->hop_temp * ws->cx_sparse_hoppings_3[i];
			}
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->cx_sparse_hoppings_0[i];
			    if (section == 1) ws->hop_temp = M * ws->cx_sparse_hoppings_1[i];
			    if (section == 2) ws->hop_temp = M * ws->cx_sparse_hoppings_2[i];
			    if (section == 3) ws->hop_temp = M * ws->cx_sparse_hoppings_3[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->cx_sparse_hoppings_0[i];
			    if (section == 1) M = ws->hop_temp * ws->cx_sparse_hoppings_1[i];
			    if (section == 2) M = ws->hop_temp * ws->cx_sparse_hoppings_2[i];
			    if (section == 3) M = ws->hop_temp * ws->cx_sparse_hoppings_3[i];
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->cx_sparse_hoppings_0_inv[i];
			    if (section == 1) ws->hop_temp = M * ws->cx_sparse_hoppings_1_inv[i];
			    if (section == 2) ws->hop_temp = M * ws->cx_sparse_hoppings_2_inv[i];
			    if (section == 3) ws->hop_temp = M * ws->cx_sparse_hoppings_3_inv[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->cx_sparse_hoppings_0_inv[i];
			    if (section == 1) M = ws->hop_temp * ws->cx_sparse_hoppings_1_inv[i];
			    if (section == 2) M = ws->hop_temp * ws->cx_sparse_hoppings_2_inv[i];
			    if (section == 3) M = ws->hop_temp * ws->cx_sparse_hoppings_3_inv[i];
			}
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->cx_sparse_hoppings_0_inv[i];
			    if (section == 1) ws->hop_temp = M * ws->cx_sparse_hoppings_1_inv[i];
			    if (section == 2) ws->hop_temp = M * ws->cx_sparse_hoppings_2_inv[i];
			    if (section == 3) ws->hop_temp = M * ws->cx_sparse_hoppings_3_inv[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->cx_sparse_hoppings_0_inv[i];
			    if (section == 1) M = ws->hop_temp * ws->cx_sparse_hoppings_1_inv[i];
			    if (section == 2) M = ws->hop_temp * ws->cx_sparse_hoppings_2_inv[i];
			    if (section == 3) M = ws->hop_temp * ws->cx_sparse_hoppings_3_inv[i];
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}
    }
}
#endif


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

using namespace arma;
using namespace hsmm;
using namespace robotics;
using namespace std;
using json = nlohmann::json;
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")
        ("params,p", po::value<string>(), "Path to the json input params")
        ("output,o", po::value<string>(), "Path to the output viterbi file(s)")
        ("polybasisfun", po::value<int>()->default_value(1), "Order of the "
                "poly basis functions")
	    ("norbf", "Flag to deactivate the radial basis functions")
        ("nsequences,n", po::value<int>()->default_value(1),
                "Number of sequences used for training");
    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("input") || !vm.count("output") || !vm.count("params")) {
        cerr << "Error: You should provide input and output files" << endl;
        return 1;
    }
    string input_filename = vm["input"].as<string>();
    string output_filename = vm["output"].as<string>();
    string params = vm["params"].as<string>();
    int nseq = vm["nsequences"].as<int>();

    field<field<mat>> seq_obs(nseq);
    int njoints;
    for(int i = 0; i < nseq; i++) {
        string name = input_filename;
        if (nseq != 1)
            name += string(".") + to_string(i);
        mat obs;
        obs.load(name, raw_ascii);
        ifstream input_params_file(params);
        json input_params;
        input_params_file >> input_params;
        njoints = obs.n_rows;
        int nobs = obs.n_cols;
        cout << "Time series shape: (" << njoints << ", " << nobs << ")." << endl;
        seq_obs(i) = fromMatToField(obs);
    }

    ifstream input_params_file(params);
    json input_params;
    input_params_file >> input_params;
    int min_duration = input_params["min_duration"];
    int nstates = input_params["nstates"];
    int ndurations = input_params["ndurations"];
    mat transition(nstates, nstates);
    transition.fill(1.0 / (nstates - 1));
    transition.diag().zeros(); // No self-transitions.
    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 rbf = shared_ptr<ScalarGaussBasis>(new ScalarGaussBasis(
                {0.25,0.5,0.75},0.25));
    auto poly = make_shared<ScalarPolyBasis>(vm["polybasisfun"].as<int>());
    auto comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({rbf, poly}));
    if (vm.count("norbf"))
        comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({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.01*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 and parsing the model parameters as json.
    shared_ptr<AbstractEmission> ptr_emission(new ProMPsEmission(promps));
    HSMM promp_hsmm(ptr_emission, transition, pi, durations, min_duration);
    promp_hsmm.from_stream(input_params);

    ViterbiAlgorithm(promp_hsmm, seq_obs, output_filename);
    return 0;
}


#ifndef QST_WAVEFUNCTIONCOMPLEX_HPP
#define QST_WAVEFUNCTIONCOMPLEX_HPP
#include <iostream>
#include <Eigen/Dense>
#include <random>
#include <fstream>
#include <iostream>

namespace qst{

class WavefunctionComplex{

    int N_;            // Number of degrees of freedom (visible units)
    int npar_;         // Number of parameters
    int nparLambda_;   // Number of amplitude parameters
    int nparMu_;       // Number of phase parameters
    Rbm rbmAm_;        // RBM for the amplitude
    Rbm rbmPh_;        // RBM for the phases

    const std::complex<double> I_; // Imaginary unit

    //Random number generator
    std::mt19937 rgen_;

public:
    // Constructor
    WavefunctionComplex(Parameters &par):rbmAm_(par),
                                  rbmPh_(par),
                                  I_(0,1){
        npar_ = rbmAm_.Npar() + rbmPh_.Npar();  // Total number of parameters
        nparLambda_ = rbmAm_.Npar();
        nparMu_ = rbmPh_.Npar();
        N_ = rbmAm_.Nvisible();
        std::random_device rd;
        //rgen_.seed(rd());
        rgen_.seed(13579);
    }

    // Private members access functions
    inline int N()const{
        return N_;
    }
    inline int Npar()const{
        return npar_;
    }
    inline int NparLambda()const{
        return nparLambda_;
    }
    inline int NparMu()const{
        return nparMu_;
    }
    inline int Nchains(){
        return rbmAm_.Nchains();
    }
    inline Eigen::VectorXd VisibleStateRow(int s){
        return rbmAm_.VisibleStateRow(s);
    }

    // Set the state of the wavefunction's degrees of freedom
    inline void SetVisibleLayer(Eigen::MatrixXd v){
        rbmAm_.SetVisibleLayer(v);
    }

    // Initialize the wavefunction parameters
    void InitRandomPars(int seed,double sigma){
        rbmAm_.InitRandomPars(seed,sigma);
        rbmPh_.InitRandomPars(seed,sigma);
    }

    // Amplitude
    double amplitude(const Eigen::VectorXd & v){
        return std::sqrt(rbmAm_.prob(v));//exp(0.5*rbmAm_.LogVal(v));
    }
    // Phase
    double phase(const Eigen::VectorXd & v){
        return std::log(rbmPh_.prob(v));
    }
    // Psi
    std::complex<double> psi(const Eigen::VectorXd & v){
        return amplitude(v)*exp(0.5*I_*phase(v));
    }

    //---- SAMPLING ----/

    // Perform k steps of Gibbs sampling
    void Sample(int steps){
        rbmAm_.Sample(steps);
    }

    //---- DERIVATIVES ----//

    //Compute gradient of effective energy wrt Lambda
    Eigen::VectorXd LambdaGrad(const Eigen::VectorXd & v){
        return rbmAm_.VisEnergyGrad(v);
    }
    //Compute gradient of effective energy wrt all parameters
    Eigen::VectorXd Grad(const Eigen::VectorXd & v){
        Eigen::VectorXd der(npar_);
        der<<rbmAm_.VisEnergyGrad(v),rbmPh_.VisEnergyGrad(v);
        return der;
    }
    //Compute the gradient of the effective energy in an arbitrary basis given by U
    void rotatedGrad(const std::vector<std::string> & basis,
                            const Eigen::VectorXd & state,//VectorRbmT & gradR){
                            std::map<std::string,Eigen::MatrixXcd> & Unitaries,
                            Eigen::VectorXcd &gradR ){
        int t=0,counter=0;
        std::complex<double> U=1.0,den=0.0;
        std::bitset<16> bit;
        std::bitset<16> st;
        std::vector<int> basisIndex;
        Eigen::VectorXd v(N_);
        Eigen::VectorXcd num(npar_);
        //Eigen::VectorXcd gradR(npar_);
        num.setZero();
        basisIndex.clear();
        // Extract the sites where the rotation is non-trivial
        for(int j=0;j<N_;j++){
            if (basis[j]!="Z"){
                t++;
                basisIndex.push_back(j);
            }
        }

        // Loop over the states of the local Hilbert space
        for(int i=0;i<1<<t;i++){
            counter =0;
            bit = i;
            v=state;
            for(int j=0;j<N_;j++){
                if (basis[j] != "Z"){
                    v(j) = bit[counter];
                    counter++;
                }
            }
            U=1.0;
            //Compute the product of the matrix elements of the unitary rotations
            for(int ii=0;ii<t;ii++){
                U = U * Unitaries[basis[basisIndex[ii]]](int(state(basisIndex[ii])),int(v(basisIndex[ii])));
            }
            num += U*Grad(v)*psi(v);
            den += U*psi(v);
        }
        gradR = num/den;
    }

    //---- UTILITIES ----//

    //Get RBM parameters
    Eigen::VectorXd GetParameters(){
        Eigen::VectorXd pars(npar_);
        pars<<rbmAm_.GetParameters(),rbmPh_.GetParameters();
        return pars;
    }

    // Set RBM parameters
    void SetParameters(const Eigen::VectorXd & pars){
        Eigen::VectorXd parsAm(nparLambda_);
        Eigen::VectorXd parsPh(npar_-nparLambda_);

        for(int i=0;i<nparLambda_;i++){
            parsAm(i)=pars(i);
        }
        for(int i=0;i<npar_-nparLambda_;i++){
            parsPh(i)=pars(nparLambda_+i);
        }
        rbmAm_.SetParameters(parsAm);
        rbmPh_.SetParameters(parsPh);
    }

    void LoadWeights(std::string &fileName){
        std::ifstream fin(fileName);
        rbmAm_.LoadWeights(fin);
        rbmPh_.LoadWeights(fin);
    }
};
}

#endif


/*
This file is part of Bohrium and Copyright (c) 2012 the Bohrium team:
http://bohrium.bitbucket.org

Bohrium is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation, either version 3
of the License, or (at your option) any later version.

Bohrium is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the
GNU Lesser General Public License along with bohrium.

If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
#include <Eigen/Dense>
#include <bp_util.h>

using namespace std;
using namespace Eigen;

template <typename T>
Array<T, Dynamic, 1>& cnd(const Array<T, Dynamic, 1>& x)
{
    size_t samples = x.size();
    Array<T, Dynamic, 1> l(samples), k(samples), w(samples);
    Array<T, Dynamic, 1> mask(samples);
    Array<bool, Dynamic, 1> mask_bool(samples);

    T a1 = 0.31938153,
      a2 =-0.356563782,
      a3 = 1.781477937,
      a4 =-1.821255978,
      a5 = 1.330274429,
      pp = 2.5066282746310002; // sqrt(2.0*PI)

    l = abs(x);
    //k = (T)1.0 / ((T)1.0 + (T)0.2316419 * l);
    //k = (T)0.5 / l / (T)0.5;
    k = l / (T)0.5;
    w = (T)1.0 - (T)1.0 / (T)pp * exp((T)-(T)1.0*l*l/(T)2.0) * \
        ((T)a1*k + \
         (T)a2*(pow(k,(T)2)) + \
         (T)a3*(pow(k,(T)3)) + \
         (T)a4*(pow(k,(T)4)) + \
         (T)a5*(pow(k,(T)5)));

    mask_bool= (x < (T)0.0);
    mask = mask_bool.cast<T>();
    return w * (mask) + ((T)1.0-w);
    //return w * (mask) + ((T)1.0-w)* mask;
    //return w * (!mask) + (1.0-w)* mask;
}

template <typename T>
T* pricing(size_t samples, size_t iterations, char flag, T x, T d_t, T r, T v)
{
    T* p    = (T*)malloc(sizeof(T)*samples);    // Intermediate results
    T t     = d_t;                              // Initial delta

    Array<T, Dynamic, 1> d1(samples), d2(samples), res(samples);
    Array<T, Dynamic, 1> s = Array<T, Dynamic, 1>::Random(samples)*4.0 +58.0;      // Model between 58-62

    for(size_t i=0; i<iterations; i++) {
        d1 = (log(s/x) + (r+v*v/2.0)*t) / (v*sqrt(t));
        d2 = d1-v*sqrt(t);

        if (flag == 'c') {

            size_t samples = x.size();
            Array<T, Dynamic, 1> l(samples), k(samples), w(samples);
            Array<T, Dynamic, 1> mask(samples);
            Array<bool, Dynamic, 1> mask_bool(samples);

            T a1 = 0.31938153,
              a2 =-0.356563782,
              a3 = 1.781477937,
              a4 =-1.821255978,
              a5 = 1.330274429,
              pp = 2.5066282746310002; // sqrt(2.0*PI)

            l = abs(x);
            //k = (T)1.0 / ((T)1.0 + (T)0.2316419 * l);
            //k = (T)0.5 / l / (T)0.5;
            k = l / (T)0.5;
            w = (T)1.0 - (T)1.0 / (T)pp * exp((T)-(T)1.0*l*l/(T)2.0) * \
                ((T)a1*k + \
                 (T)a2*(pow(k,(T)2)) + \
                 (T)a3*(pow(k,(T)3)) + \
                 (T)a4*(pow(k,(T)4)) + \
                 (T)a5*(pow(k,(T)5)));

            mask_bool= (x < (T)0.0);
            mask = mask_bool.cast<T>();
            res =  w * (!mask) + (1.0-w)* mask;

            //cnd<T>(d1);
            //res = s * cnd<T>(d1) -x * exp(-r*t) * cnd<T>(d2);
        } else {
/*
            Array<T, Dynamic, 1> tmp1(samples), tmp2(samples);
            tmp1 = -1.0*d2;
            tmp2 = -1.0*d1;
            res = x * exp(-r*t) * cnd<T>(tmp1) - s*cnd<T>(tmp2);
    */
        }

        t += d_t;                               // Increment delta
        p[i] = res.sum() / (T)samples;           // Result from timestep
    }

    return p;
}

int main(int argc, char* argv[])
{
    bp_util_type bp = bp_util_create(argc, argv, 2);    // Grab arguments
    if (bp.args.has_error) {
        return 1;
    }
    const size_t samples    = bp.args.sizes[0];
    const size_t iterations = bp.args.sizes[1];

    bp.timer_start();                                   // Start timer
    double* prices = pricing(                           // Run...
        samples, iterations,
        'c', 65.0, 1.0 / 365.0,
        0.08, 0.3
    );
    bp.timer_stop();                                    // Stop timer

    bp.print("black_scholes(cpp11_bxx)");               // Print restults..
    if (bp.args.verbose) {                              // ..verbosely.
        cout << ", \"output\": [";
        for(size_t i=0; i<iterations; i++) {
            cout << prices[i];
            if (iterations-1!=i) {
                cout << ", ";
            }
        }
        cout << "]" << endl;
    }
    free(prices);                                       // Cleanup

    return 0;
}


//==================================================================================================
/**
  Copyright 2016 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_ARCH_COMMON_DETAIL_GENERIC_D_EXPO_REDUCTION_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_GENERIC_D_EXPO_REDUCTION_HPP_INCLUDED

#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/horn1.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/constant/invlog10_2.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/detail/constant/log10_2hi.hpp>
#include <boost/simd/detail/constant/log10_2lo.hpp>
#include <boost/simd/constant/log_10.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/detail/constant/maxlog10.hpp>
#include <boost/simd/detail/constant/maxlog2.hpp>
#include <boost/simd/detail/constant/minlog.hpp>
#include <boost/simd/detail/constant/minlog10.hpp>
#include <boost/simd/detail/constant/minlog2.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/is_greater_equal.hpp>
#include <boost/simd/function/is_less_equal.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

    template < class A0> struct exp_reduction < A0, bs::tag::exp_, double>
    {
      static BOOST_FORCEINLINE auto isgemaxlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_greater_equal(a0, Maxlog<A0>()))
      {
        return is_greater_equal(a0, Maxlog<A0>());
      }

      static BOOST_FORCEINLINE auto isleminlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_less_equal(a0, Minlog<A0>()))
      {
        return is_less_equal(a0, Minlog<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce( A0 const& a0
                                        , A0& hi, A0& lo, A0& x) BOOST_NOEXCEPT
      {
        A0 k = nearbyint(Invlog_2<A0>()*a0);
        hi = fnms(k, Log_2hi<A0>(), a0); //a0-k*L
        lo = k*Log_2lo<A0>();
        x  = hi-lo;
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 const& x) BOOST_NOEXCEPT
      {
        A0 const t = sqr(x);
        return fnms(t,
                    horn<A0
                         , 0x3fc555555555553eull
                         , 0xbf66c16c16bebd93ull
                         , 0x3f11566aaf25de2cull
                         , 0xbebbbd41c5d26bf1ull
                         , 0x3e66376972bea4d0ull
                    >(t), x); //x-h*t
    }

      static BOOST_FORCEINLINE A0 finalize(A0 const& x, A0 const& c, A0 const& hi, A0 const& lo) BOOST_NOEXCEPT
      {
        return One<A0>()-(((lo-(x*c)/(Two<A0>()-c))-hi));
      }

    };

    template < class A0 > struct exp_reduction < A0, bs::tag::exp2_, double>
    {
      static BOOST_FORCEINLINE auto isgemaxlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_greater_equal(a0, Maxlog2<A0>()))
      {
        return is_greater_equal(a0, Maxlog2<A0>());
      }

      static BOOST_FORCEINLINE auto isleminlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_less_equal(a0, Minlog2<A0>()))
      {
        return is_less_equal(a0, Minlog2<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce(A0 const& a0, A0 const&, A0 const&, A0& x) BOOST_NOEXCEPT
      {
        A0 k = nearbyint(a0);
        x = (a0 - k)*Log_2<A0>();
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 const& x) BOOST_NOEXCEPT
      {
        const A0 t =  sqr(x);
        return fnms(t,
                    horn<A0
                         , 0x3fc555555555553eull
                         , 0xbf66c16c16bebd93ull
                         , 0x3f11566aaf25de2cull
                         , 0xbebbbd41c5d26bf1ull
                         , 0x3e66376972bea4d0ull
                    > (t), x); //x-h*t
      }

      static BOOST_FORCEINLINE A0 finalize(A0 const& x, A0 const& c, A0 const&, A0& ) BOOST_NOEXCEPT
      {
        return oneminus(((-(x*c)/(Two<A0>()-c))-x));
      }
    };

    template < class A0 > struct exp_reduction < A0, bs::tag::exp10_, double>
    {
      static BOOST_FORCEINLINE auto isgemaxlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_greater_equal(a0, Maxlog10<A0>()))
      {
        return is_greater_equal(a0, Maxlog10<A0>());
      }

      static BOOST_FORCEINLINE auto isleminlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_less_equal(a0, Minlog10<A0>()))
      {
        return is_less_equal(a0, Minlog10<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce(A0 const& a0, A0&, A0&, A0& x) BOOST_NOEXCEPT
      {
        A0 k  = nearbyint(Invlog10_2<A0>()*a0);
        x = fnms(k, Log10_2hi<A0>(), a0);
        x = fnms(k, Log10_2lo<A0>(), x);
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 x) BOOST_NOEXCEPT
      {
        A0 xx = sqr(x);
        A0 px = x*horn<A0,
                       0x40a2b4798e134a01ull,
                       0x40796b7a050349e4ull,
                       0x40277d9474c55934ull,
                       0x3fa4fd75f3062dd4ull
                       > (xx);
        A0 x2 =  px/(horn1<A0,
                          0x40a03f37650df6e2ull,
                          0x4093e05eefd67782ull,
                          0x405545fdce51ca08ull
                     //   0x3ff0000000000000ull
                          > (xx)-px);
        return inc(x2+x2);
      }

      static BOOST_FORCEINLINE A0 finalize(A0 const&, A0 const& c, A0 const&, A0& ) BOOST_NOEXCEPT
      {
        return c;
      }
    };
  }
} }
#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include <igl/parallel_transport_angles.h>
#include <Eigen/Geometry>

template <typename DerivedV, typename DerivedF, typename DerivedK>
IGL_INLINE void igl::parallel_transport_angles(
const Eigen::PlainObjectBase<DerivedV>& V,
const Eigen::PlainObjectBase<DerivedF>& F,
const Eigen::PlainObjectBase<DerivedV>& FN,
const Eigen::MatrixXi &E2F,
const Eigen::MatrixXi &F2E,
Eigen::PlainObjectBase<DerivedK> &K)
{
  int numE = E2F.rows();

  Eigen::VectorXi isBorderEdge;
  isBorderEdge.setZero(numE,1);
  for(unsigned i=0; i<numE; ++i)
  {
    if ((E2F(i,0) == -1) || ((E2F(i,1) == -1)))
      isBorderEdge[i] = 1;
  }

  K.setZero(numE);
  // For every non-border edge
  for (unsigned eid=0; eid<numE; ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = E2F(eid,0);
      int fid1 = E2F(eid,1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N0 = FN.row(fid0);
//      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N1 = FN.row(fid1);

      // find common edge on triangle 0 and 1
      int fid0_vc = -1;
      int fid1_vc = -1;
      for (unsigned i=0;i<3;++i)
      {
        if (F2E(fid0,i) == eid)
          fid0_vc = i;
        if (F2E(fid1,i) == eid)
          fid1_vc = i;
      }
      assert(fid0_vc != -1);
      assert(fid1_vc != -1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> common_edge = V.row(F(fid0,(fid0_vc+1)%3)) - V.row(F(fid0,fid0_vc));
      common_edge.normalize();

      // Map the two triangles in a new space where the common edge is the x axis and the N0 the z axis
      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> P;
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> o = V.row(F(fid0,fid0_vc));
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> tmp = -N0.cross(common_edge);
      P << common_edge, tmp, N0;
      //      P.transposeInPlace();

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V0;
      V0.row(0) = V.row(F(fid0,0)) -o;
      V0.row(1) = V.row(F(fid0,1)) -o;
      V0.row(2) = V.row(F(fid0,2)) -o;

      V0 = (P*V0.transpose()).transpose();

      //      assert(V0(0,2) < 1e-10);
      //      assert(V0(1,2) < 1e-10);
      //      assert(V0(2,2) < 1e-10);

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V1;
      V1.row(0) = V.row(F(fid1,0)) -o;
      V1.row(1) = V.row(F(fid1,1)) -o;
      V1.row(2) = V.row(F(fid1,2)) -o;
      V1 = (P*V1.transpose()).transpose();

      //      assert(V1(fid1_vc,2) < 10e-10);
      //      assert(V1((fid1_vc+1)%3,2) < 10e-10);

      // compute rotation R such that R * N1 = N0
      // i.e. map both triangles to the same plane
      double alpha = -atan2(V1((fid1_vc+2)%3,2),V1((fid1_vc+2)%3,1));

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> R;
      R << 1,          0,            0,
      0, cos(alpha), -sin(alpha) ,
      0, sin(alpha),  cos(alpha);
      V1 = (R*V1.transpose()).transpose();

      //      assert(V1(0,2) < 1e-10);
      //      assert(V1(1,2) < 1e-10);
      //      assert(V1(2,2) < 1e-10);

      // measure the angle between the reference frames
      // k_ij is the angle between the triangle on the left and the one on the right
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref0 = V0.row(1) - V0.row(0);
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref1 = V1.row(1) - V1.row(0);

      ref0.normalize();
      ref1.normalize();

      double ktemp = atan2(ref1(1),ref1(0)) - atan2(ref0(1),ref0(0));

      // just to be sure, rotate ref0 using angle ktemp...
      Eigen::Matrix<typename DerivedV::Scalar,2,2> R2;
      R2 << cos(ktemp), -sin(ktemp), sin(ktemp), cos(ktemp);

//      Eigen::Matrix<typename DerivedV::Scalar, 1, 2> tmp1 = R2*(ref0.head(2)).transpose();

      //      assert(tmp1(0) - ref1(0) < 1e-10);
      //      assert(tmp1(1) - ref1(1) < 1e-10);

      K[eid] = ktemp;
    }
  }

}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::parallel_transport_angles<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1> const&, Eigen::Matrix<int, -1, -1, 0, -1, -1> const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
#endif


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Projection_traits_xy_3.h>

#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_with_info_2.h>

#include <CGAL/boost/graph/graph_traits_Delaunay_triangulation_2.h>
#include <CGAL/boost/graph/copy_face_graph.h>

#include <CGAL/Point_set_3.h>
#include <CGAL/Point_set_3/IO.h>
#include <CGAL/compute_average_spacing.h>

#include <CGAL/Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/locate.h>

#include <CGAL/Polygon_mesh_processing/triangulate_hole.h>
#include <CGAL/Polygon_mesh_processing/border.h>
#include <CGAL/Polygon_mesh_processing/remesh.h>

#include <boost/graph/adjacency_list.hpp>
#include <CGAL/boost/graph/split_graph_into_polylines.h>

#include <CGAL/IO/WKT.h>

#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_triangulation_plus_2.h>

#include <CGAL/Polyline_simplification_2/simplify.h>
#include <CGAL/Polyline_simplification_2/Squared_distance_cost.h>

#include <CGAL/Classification.h>

#include <CGAL/Random.h>

#include <fstream>
#include <queue>

#include "include/Color_ramp.h"

///////////////////////////////////////////////////////////////////
//! [TIN DS]

using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel;
using Projection_traits = CGAL::Projection_traits_xy_3<Kernel>;
using Point_2 = Kernel::Point_2;
using Point_3 = Kernel::Point_3;
using Segment_3 = Kernel::Segment_3;

// Triangulated Irregular Network
using TIN = CGAL::Delaunay_triangulation_2<Projection_traits>;

//! [TIN DS]
///////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////
//! [TIN_with_info DS]

// Triangulated Irregular Network (with info)
using Point_set = CGAL::Point_set_3<Point_3>;
using Vbi = CGAL::Triangulation_vertex_base_with_info_2 <Point_set::Index, Projection_traits>;
using Fbi = CGAL::Triangulation_face_base_with_info_2<int, Projection_traits>;
using TDS = CGAL::Triangulation_data_structure_2<Vbi, Fbi>;
using TIN_with_info = CGAL::Delaunay_triangulation_2<Projection_traits, TDS>;

//! [TIN_with_info DS]
///////////////////////////////////////////////////////////////////

namespace Classification = CGAL::Classification;

#ifdef CGAL_LINKED_WITH_TBB
using Concurrency_tag = CGAL::Parallel_tag;
#else
using Concurrency_tag = CGAL::Sequential_tag;
#endif

///////////////////////////////////////////////////////////////////
//! [Contouring functions]

bool face_has_isovalue (TIN::Face_handle fh, double isovalue)
{
  bool above = false, below = false;
  for (int i = 0; i < 3; ++ i)
  {
    // Face has isovalue if one of its vertices is above and another
    // one below
    if (fh->vertex(i)->point().z() > isovalue)
      above = true;
    if (fh->vertex(i)->point().z() < isovalue)
      below = true;
  }

  return (above && below);
}

Segment_3 isocontour_in_face (TIN::Face_handle fh, double isovalue)
{
  Point_3 source;
  Point_3 target;
  bool source_found = false;

  for (int i = 0; i < 3; ++ i)
  {
    Point_3 p0 = fh->vertex((i+1) % 3)->point();
    Point_3 p1 = fh->vertex((i+2) % 3)->point();

    // Check if the isovalue crosses segment (p0,p1)
    if ((p0.z() - isovalue) * (p1.z() - isovalue) > 0)
      continue;

    double zbottom = p0.z();
    double ztop = p1.z();
    if (zbottom > ztop)
    {
      std::swap (zbottom, ztop);
      std::swap (p0, p1);
    }

    // Compute position of segment vertex
    double ratio = (isovalue - zbottom) / (ztop - zbottom);
    Point_3 p = CGAL::barycenter (p0, (1 - ratio), p1,ratio);

    if (source_found)
      target = p;
    else
    {
      source = p;
      source_found = true;
    }
  }

  return Segment_3 (source, target);
}

//! [Contouring functions]
///////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////
//! [Contouring visitor]

template <typename Graph>
class Polylines_visitor
{
private:
  std::vector<std::vector<Point_3> >& polylines;
  Graph& graph;

public:

  Polylines_visitor (Graph& graph, std::vector<std::vector<Point_3> >& polylines)
    : polylines (polylines), graph(graph) { }

  void start_new_polyline()
  {
    polylines.push_back (std::vector<Point_3>());
  }

  void add_node (typename Graph::vertex_descriptor vd)
  {
    polylines.back().push_back (graph[vd]);
  }

  void end_polyline()
  {
    // filter small polylines
    if (polylines.back().size() < 50)
      polylines.pop_back();
  }
};

//! [Contouring visitor]
///////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////
//! [CDT]

namespace PS = CGAL::Polyline_simplification_2;
using CDT_vertex_base = PS::Vertex_base_2<Projection_traits>;
using CDT_face_base = CGAL::Constrained_triangulation_face_base_2<Projection_traits>;
using CDT_TDS = CGAL::Triangulation_data_structure_2<CDT_vertex_base, CDT_face_base>;
using CDT = CGAL::Constrained_Delaunay_triangulation_2<Projection_traits, CDT_TDS>;
using CTP = CGAL::Constrained_triangulation_plus_2<CDT>;

//! [CDT]
///////////////////////////////////////////////////////////////////

int main (int argc, char** argv)
{
  if (argc != 2)
  {
    std::cerr << "Usage: " << argv[0] << " points.ply" << std::endl;
    return EXIT_FAILURE;
  }

  ///////////////////////////////////////////////////////////////////
  //! [Init DSM]

  // Read points
  std::ifstream ifile (argv[1], std::ios_base::binary);
  CGAL::Point_set_3<Point_3> points;
  ifile >> points;
  std::cerr << points.size() << " point(s) read" << std::endl;

  // Create DSM
  TIN dsm (points.points().begin(), points.points().end());

  //! [Init DSM]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Save DSM]

  using Mesh = CGAL::Surface_mesh<Point_3>;

  Mesh dsm_mesh;
  CGAL::copy_face_graph (dsm, dsm_mesh);
  std::ofstream dsm_ofile ("dsm.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (dsm_ofile);
  CGAL::IO::write_PLY (dsm_ofile, dsm_mesh);
  dsm_ofile.close();

  //! [Save DSM]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [TIN_with_info]

  auto idx_to_point_with_info
    = [&](const Point_set::Index& idx) -> std::pair<Point_3, Point_set::Index>
      {
        return std::make_pair (points.point(idx), idx);
      };

  TIN_with_info tin_with_info
    (boost::make_transform_iterator (points.begin(), idx_to_point_with_info),
     boost::make_transform_iterator (points.end(), idx_to_point_with_info));

  //! [TIN_with_info]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Components]

  double spacing = CGAL::compute_average_spacing<Concurrency_tag>(points, 6);
  spacing *= 2;

  auto face_height
    = [&](const TIN_with_info::Face_handle fh) -> double
      {
        double out = 0.;
        for (int i = 0; i < 3; ++ i)
          out = (std::max) (out, CGAL::abs(fh->vertex(i)->point().z() - fh->vertex((i+1)%3)->point().z()));
        return out;
      };

  // Initialize faces info
  for (TIN_with_info::Face_handle fh : tin_with_info.all_face_handles())
    if (tin_with_info.is_infinite(fh) || face_height(fh) > spacing) // Filtered faces are given info() = -2
      fh->info() = -2;
    else // Pending faces are given info() = -1;
      fh->info() = -1;

  // Flooding algorithm
  std::vector<int> component_size;
  for (TIN_with_info::Face_handle fh : tin_with_info.finite_face_handles())
  {
    if (fh->info() != -1)
      continue;

    std::queue<TIN_with_info::Face_handle> todo;
    todo.push(fh);

    int size = 0;
    while (!todo.empty())
    {
      TIN_with_info::Face_handle current = todo.front();
      todo.pop();

      if (current->info() != -1)
        continue;
      current->info() = int(component_size.size());
      ++ size;

      for (int i = 0; i < 3; ++ i)
        todo.push (current->neighbor(i));
    }

    component_size.push_back (size);
  }

  std::cerr << component_size.size() << " connected component(s) found" << std::endl;

  //! [Components]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Save TIN with info]

  Mesh tin_colored_mesh;

  Mesh::Property_map<Mesh::Face_index, CGAL::IO::Color>
    color_map = tin_colored_mesh.add_property_map<Mesh::Face_index, CGAL::IO::Color>("f:color").first;

  CGAL::copy_face_graph (tin_with_info, tin_colored_mesh,
                         CGAL::parameters::face_to_face_output_iterator
                         (boost::make_function_output_iterator
                          ([&](const std::pair<TIN_with_info::Face_handle, Mesh::Face_index>& ff)
                           {
                             // Color unassigned faces gray
                             if (ff.first->info() < 0)
                               color_map[ff.second] = CGAL::IO::Color(128, 128, 128);
                             else
                             {
                               // Random color seeded by the component ID
                               CGAL::Random r (ff.first->info());
                               color_map[ff.second] = CGAL::IO::Color (r.get_int(64, 192),
                                                                   r.get_int(64, 192),
                                                                   r.get_int(64, 192));
                             }
                           })));

  std::ofstream tin_colored_ofile ("colored_tin.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (tin_colored_ofile);
  CGAL::IO::write_PLY (tin_colored_ofile, tin_colored_mesh);
  tin_colored_ofile.close();

  //! [Save TIN with info]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Filtering]

  int min_size = int(points.size() / 2);

  std::vector<TIN_with_info::Vertex_handle> to_remove;
  for (TIN_with_info::Vertex_handle vh : tin_with_info.finite_vertex_handles())
  {
    TIN_with_info::Face_circulator circ = tin_with_info.incident_faces (vh),
      start = circ;

    // Remove a vertex if it's only adjacent to components smaller than threshold
    bool keep = false;
    do
    {
      if (circ->info() >= 0 && component_size[std::size_t(circ->info())] > min_size)
      {
        keep = true;
        break;
      }
    }
    while (++ circ != start);

    if (!keep)
      to_remove.push_back (vh);
  }

  std::cerr << to_remove.size() << " vertices(s) will be removed after filtering" << std::endl;
  for (TIN_with_info::Vertex_handle vh : to_remove)
    tin_with_info.remove (vh);

  //! [Filtering]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Hole filling]

  // Copy and keep track of overly large faces
  Mesh dtm_mesh;

  std::vector<Mesh::Face_index> face_selection;
  Mesh::Property_map<Mesh::Face_index, bool> face_selection_map
   = dtm_mesh.add_property_map<Mesh::Face_index, bool>("is_selected", false).first;

  double limit = CGAL::square (5 * spacing);
  CGAL::copy_face_graph (tin_with_info, dtm_mesh,
                         CGAL::parameters::face_to_face_output_iterator
                         (boost::make_function_output_iterator
                          ([&](const std::pair<TIN_with_info::Face_handle, Mesh::Face_index>& ff)
                           {
                             double longest_edge = 0.;
                             bool border = false;
                             for (int i = 0; i < 3; ++ i)
                             {
                               longest_edge = (std::max)(longest_edge, CGAL::squared_distance
                                                         (ff.first->vertex((i+1)%3)->point(),
                                                          ff.first->vertex((i+2)%3)->point()));

                               TIN_with_info::Face_circulator circ
                                 = tin_with_info.incident_faces (ff.first->vertex(i)),
                                 start = circ;
                               do
                               {
                                 if (tin_with_info.is_infinite (circ))
                                 {
                                   border = true;
                                   break;
                                 }
                               }
                               while (++ circ != start);

                               if (border)
                                 break;
                             }

                             // Select if face is too big AND it's not
                             // on the border (to have closed holes)
                             if (!border && longest_edge > limit)
                             {
                               face_selection_map[ff.second] = true;
                               face_selection.push_back (ff.second);
                             }
                           })));

  // Save original DTM
  std::ofstream dtm_ofile ("dtm.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (dtm_ofile);
  CGAL::IO::write_PLY (dtm_ofile, dtm_mesh);
  dtm_ofile.close();

  std::cerr << face_selection.size() << " face(s) are selected for removal" << std::endl;

  // Expand face selection to keep a well formed 2-manifold mesh after removal
  CGAL::expand_face_selection_for_removal (face_selection, dtm_mesh, face_selection_map);
  face_selection.clear();
  for (Mesh::Face_index fi : faces(dtm_mesh))
    if (face_selection_map[fi])
      face_selection.push_back(fi);

  std::cerr << face_selection.size() << " face(s) are selected for removal after expansion" << std::endl;

  for (Mesh::Face_index fi : face_selection)
    CGAL::Euler::remove_face (halfedge(fi, dtm_mesh), dtm_mesh);
  dtm_mesh.collect_garbage();

  if (!dtm_mesh.is_valid())
    std::cerr << "Invalid mesh!" << std::endl;

  // Save filtered DTM
  std::ofstream dtm_holes_ofile ("dtm_with_holes.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (dtm_holes_ofile);
  CGAL::IO::write_PLY (dtm_holes_ofile, dtm_mesh);
  dtm_holes_ofile.close();

  // Get all holes
  std::vector<Mesh::Halfedge_index> holes;
  CGAL::Polygon_mesh_processing::extract_boundary_cycles (dtm_mesh, std::back_inserter (holes));

  std::cerr << holes.size() << " hole(s) identified" << std::endl;

  // Identify outer hull (hole with maximum size)
  double max_size = 0.;
  Mesh::Halfedge_index outer_hull;
  for (Mesh::Halfedge_index hi : holes)
  {
    CGAL::Bbox_3 hole_bbox;
    for (Mesh::Halfedge_index haf : CGAL::halfedges_around_face(hi, dtm_mesh))
    {
      const Point_3& p = dtm_mesh.point(target(haf, dtm_mesh));
      hole_bbox += p.bbox();
    }
    double size = CGAL::squared_distance (Point_2(hole_bbox.xmin(), hole_bbox.ymin()),
                                          Point_2(hole_bbox.xmax(), hole_bbox.ymax()));
    if (size > max_size)
    {
      max_size = size;
      outer_hull = hi;
    }
  }

  // Fill all holes except the bigest (which is the outer hull of the mesh)
  for (Mesh::Halfedge_index hi : holes)
    if (hi != outer_hull)
      CGAL::Polygon_mesh_processing::triangulate_refine_and_fair_hole
        (dtm_mesh, hi, CGAL::Emptyset_iterator(), CGAL::Emptyset_iterator());

  // Save DTM with holes filled
  std::ofstream dtm_filled_ofile ("dtm_filled.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (dtm_filled_ofile);
  CGAL::IO::write_PLY (dtm_filled_ofile, dtm_mesh);
  dtm_filled_ofile.close();

  //! [Hole filling]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Remeshing]

  CGAL::Polygon_mesh_processing::isotropic_remeshing (faces(dtm_mesh), spacing, dtm_mesh);

  std::ofstream dtm_remeshed_ofile ("dtm_remeshed.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (dtm_remeshed_ofile);
  CGAL::IO::write_PLY (dtm_remeshed_ofile, dtm_mesh);
  dtm_remeshed_ofile.close();

  //! [Remeshing]
  ///////////////////////////////////////////////////////////////////

  TIN dtm_clean (dtm_mesh.points().begin(), dtm_mesh.points().end());

  ///////////////////////////////////////////////////////////////////
  //! [Rastering]

  CGAL::Bbox_3 bbox = CGAL::bbox_3 (points.points().begin(), points.points().end());

  // Generate raster image 1920-pixels large
  std::size_t width = 1920;
  std::size_t height = std::size_t((bbox.ymax() - bbox.ymin()) * 1920 / (bbox.xmax() - bbox.xmin()));

  std::cerr << "Rastering with resolution " << width << "x" << height << std::endl;

  // Use PPM format (Portable PixMap) for simplicity
  std::ofstream raster_ofile ("raster.ppm", std::ios_base::binary);

  // PPM header
  raster_ofile << "P6" << std::endl // magic number
               << width << " " << height << std::endl // dimensions of the image
               << 255 << std::endl; // maximum color value

  // Use rainbow color ramp output
  Color_ramp color_ramp;

  // Keeping track of location from one point to its neighbor allows
  // for fast locate in DT
  TIN::Face_handle location;

  // Query each pixel of the image
  for (std::size_t y = 0; y < height; ++ y)
    for (std::size_t x = 0; x < width; ++ x)
    {
      Point_3 query (bbox.xmin() + x * (bbox.xmax() - bbox.xmin()) / double(width),
                     bbox.ymin() + (height-y) * (bbox.ymax() - bbox.ymin()) / double(height),
                     0); // not relevant for location in 2D

      location = dtm_clean.locate (query, location);

      // Points outside the convex hull will be colored black
      std::array<unsigned char, 3> colors { 0, 0, 0 };
      if (!dtm_clean.is_infinite(location))
      {
        std::array<double, 3> barycentric_coordinates
          = CGAL::Polygon_mesh_processing::barycentric_coordinates
          (Point_2 (location->vertex(0)->point().x(), location->vertex(0)->point().y()),
           Point_2 (location->vertex(1)->point().x(), location->vertex(1)->point().y()),
           Point_2 (location->vertex(2)->point().x(), location->vertex(2)->point().y()),
           Point_2 (query.x(), query.y()),
           Kernel());

        double height_at_query
          = (barycentric_coordinates[0] * location->vertex(0)->point().z()
             + barycentric_coordinates[1] * location->vertex(1)->point().z()
             + barycentric_coordinates[2] * location->vertex(2)->point().z());

        // Color ramp generates a color depending on a value from 0 to 1
        double height_ratio = (height_at_query - bbox.zmin()) / (bbox.zmax() - bbox.zmin());
        colors = color_ramp.get(height_ratio);
      }
      raster_ofile.write ((char*)(&colors), 3);
    }

  raster_ofile.close();

  //! [Rastering]
  ///////////////////////////////////////////////////////////////////

  // Smooth heights with 5 successive Gaussian filters
  double gaussian_variance = 4 * spacing * spacing;
  for (TIN::Vertex_handle vh : dtm_clean.finite_vertex_handles())
  {
    double z = vh->point().z();
    double total_weight = 1;

    TIN::Vertex_circulator circ = dtm_clean.incident_vertices (vh),
      start = circ;

    do
    {
      if (!dtm_clean.is_infinite(circ))
      {
        double sq_dist = CGAL::squared_distance (vh->point(), circ->point());

        double weight = std::exp(- sq_dist / gaussian_variance);
        z += weight * circ->point().z();
        total_weight += weight;
      }
    }
    while (++ circ != start);

    z /= total_weight;

    vh->point() = Point_3 (vh->point().x(), vh->point().y(), z);
  }

  ///////////////////////////////////////////////////////////////////
  //! [Contouring extraction]

  std::array<double, 50> isovalues; // Contour 50 isovalues
  for (std::size_t i = 0; i < isovalues.size(); ++ i)
    isovalues[i] = bbox.zmin() + ((i+1) * (bbox.zmax() - bbox.zmin()) / (isovalues.size() - 2));

  // First find on each face if they are crossed by some isovalues and
  // extract segments in a graph
  using Segment_graph = boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS, Point_3>;
  Segment_graph graph;
  using Map_p2v = std::map<Point_3, Segment_graph::vertex_descriptor>;
  Map_p2v map_p2v;
  for (TIN::Face_handle vh : dtm_clean.finite_face_handles())
    for (double iv : isovalues)
      if (face_has_isovalue (vh, iv))
      {
        Segment_3 segment = isocontour_in_face (vh, iv);
        for (const Point_3& p : { segment.source(), segment.target() })
        {
          // Only insert end points of segments once to get a well connected graph
          Map_p2v::iterator iter;
          bool inserted;
          std::tie (iter, inserted) = map_p2v.insert (std::make_pair (p, Segment_graph::vertex_descriptor()));
          if (inserted)
          {
            iter->second = boost::add_vertex (graph);
            graph[iter->second] = p;
          }
        }
        boost::add_edge (map_p2v[segment.source()], map_p2v[segment.target()], graph);
      }

  //! [Contouring extraction]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Contouring split]

  // Split segments into polylines
  std::vector<std::vector<Point_3> > polylines;
  Polylines_visitor<Segment_graph> visitor (graph, polylines);
  CGAL::split_graph_into_polylines (graph, visitor);

  std::cerr << polylines.size() << " polylines computed, with "
            << map_p2v.size() << " vertices in total" << std::endl;

  // Output to WKT file
  std::ofstream contour_ofile ("contour.wkt");
  contour_ofile.precision(18);
  CGAL::IO::write_multi_linestring_WKT (contour_ofile, polylines);
  contour_ofile.close();

  //! [Contouring split]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Contouring simplify]

  // Construct constrained Delaunay triangulation with polylines as constraints
  CTP ctp;
  for (const std::vector<Point_3>& poly : polylines)
    ctp.insert_constraint (poly.begin(), poly.end());

  // Simplification algorithm with limit on distance
  PS::simplify (ctp, PS::Squared_distance_cost(), PS::Stop_above_cost_threshold (16 * spacing * spacing));

  polylines.clear();
  for (CTP::Constraint_id cid : ctp.constraints())
  {
    polylines.push_back (std::vector<Point_3>());
    polylines.back().reserve (ctp.vertices_in_constraint (cid).size());
    for (CTP::Vertex_handle vh : ctp.vertices_in_constraint(cid))
      polylines.back().push_back (vh->point());
  }

  std::size_t nb_vertices
    = std::accumulate (polylines.begin(), polylines.end(), 0u,
                       [](std::size_t size, const std::vector<Point_3>& poly) -> std::size_t
                       { return size + poly.size(); });

  std::cerr << nb_vertices
            << " vertices remaining after simplification ("
            << 100. * (nb_vertices / double(map_p2v.size())) << "%)" << std::endl;

  // Output to WKT file
  std::ofstream simplified_ofile ("simplified.wkt");
  simplified_ofile.precision(18);
  CGAL::IO::write_multi_linestring_WKT (simplified_ofile, polylines);
  simplified_ofile.close();

  //! [Contouring simplify]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Classification]

  // Get training from input
  Point_set::Property_map<int> training_map;
  bool training_found;
  std::tie (training_map, training_found) = points.property_map<int>("training");

  if (training_found)
  {
    std::cerr << "Classifying ground/vegetation/building" << std::endl;

    // Create labels
    Classification::Label_set labels ({ "ground", "vegetation", "building" });

    // Generate features
    Classification::Feature_set features;
    Classification::Point_set_feature_generator<Kernel, Point_set, Point_set::Point_map>
      generator (points, points.point_map(), 5); // 5 scales

#ifdef CGAL_LINKED_WITH_TBB
    // If TBB is used, features can be computed in parallel
    features.begin_parallel_additions();
    generator.generate_point_based_features (features);
    features.end_parallel_additions();
#else
    generator.generate_point_based_features (features);
#endif

    // Train a random forest classifier
    Classification::ETHZ::Random_forest_classifier classifier (labels, features);
    classifier.train (points.range(training_map));

    // Classify with graphcut regularization
    Point_set::Property_map<int> label_map = points.add_property_map<int>("labels").first;
    Classification::classify_with_graphcut<Concurrency_tag>
      (points, points.point_map(), labels, classifier,
       generator.neighborhood().k_neighbor_query(12), // regularize on 12-neighbors graph
       0.5f, // graphcut weight
       12, // Subdivide to speed-up process
       label_map);

    // Evaluate
    std::cerr << "Mean IoU on training data = "
              << Classification::Evaluation(labels,
                                            points.range(training_map),
                                            points.range(label_map)).mean_intersection_over_union() << std::endl;

    // Save the classified point set
    std::ofstream classified_ofile ("classified.ply");
    CGAL::IO::set_binary_mode (classified_ofile);
    classified_ofile << points;
    classified_ofile.close();
  }

  //! [Classification]
  ///////////////////////////////////////////////////////////////////

  return EXIT_SUCCESS;
}


/*  This file is part of libDAI - http://www.libdai.org/
 *
 *  Copyright (c) 2006-2011, The libDAI authors. All rights reserved.
 *
 *  Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.
 */

#include <algorithm>
#include <cmath>
#include <boost/dynamic_bitset.hpp>
#include <dai/regiongraph.h>
#include <dai/factorgraph.h>
#include <dai/clustergraph.h>

namespace dai {

using namespace std;

void RegionGraph::construct( const FactorGraph &fg, const std::vector<VarSet> &ors, const std::vector<Region> &irs, const std::vector<std::pair<size_t,size_t> > &edges ) {
    // Copy factor graph structure
    FactorGraph::operator=( fg );

    // Copy inner regions
    _IRs = irs;

    // Construct outer regions (giving them counting number 1.0)
    _ORs.clear();
    _ORs.reserve( ors.size() );
    bforeach( const VarSet &alpha, ors )
        _ORs.push_back( FRegion(Factor(alpha, 1.0), 1.0) );

    // For each factor, find an outer region that subsumes that factor.
    // Then, multiply the outer region with that factor.
    _fac2OR.clear();
    _fac2OR.reserve( nrFactors() );
    for( size_t I = 0; I < nrFactors(); I++ ) {
        size_t alpha;
        for( alpha = 0; alpha < nrORs(); alpha++ )
            if( OR(alpha).vars() >> factor(I).vars() ) {
                _fac2OR.push_back( alpha );
                break;
            }
        DAI_ASSERT( alpha != nrORs() );
    }
    recomputeORs();

    // Create bipartite graph
    _G.construct( nrORs(), nrIRs(), edges.begin(), edges.end() );
}

void RegionGraph::constructCVM( const FactorGraph &fg, const std::vector<VarSet> &cl, size_t verbose ) {
    if( verbose )
        cerr << "constructCVM called (" << fg.nrVars() << " vars, " << fg.nrFactors() << " facs, " << cl.size() << " clusters)" << endl;

    // Retain only maximal clusters
    if( verbose )
        cerr << "  Constructing ClusterGraph" << endl;
    ClusterGraph cg( cl );
    if( verbose )
        cerr << "  Erasing non-maximal clusters" << endl;
    cg.eraseNonMaximal();

    // Create inner regions - first pass
    if( verbose )
        cerr << "  Creating inner regions (first pass)" << endl;
    set<VarSet> betas;
    for( size_t alpha = 0; alpha < cg.nrClusters(); alpha++ )
        for( size_t alpha2 = alpha; (++alpha2) != cg.nrClusters(); ) {
            VarSet intersection = cg.cluster(alpha) & cg.cluster(alpha2);
            if( intersection.size() > 0 )
                betas.insert( intersection );
        }

    // Create inner regions - subsequent passes
    if( verbose )
        cerr << "  Creating inner regions (next passes)" << endl;
    set<VarSet> new_betas;
    do {
        new_betas.clear();
        for( set<VarSet>::const_iterator gamma = betas.begin(); gamma != betas.end(); gamma++ )
            for( set<VarSet>::const_iterator gamma2 = gamma; (++gamma2) != betas.end(); ) {
                VarSet intersection = (*gamma) & (*gamma2);
                if( (intersection.size() > 0) && (betas.count(intersection) == 0) )
                    new_betas.insert( intersection );
            }
        betas.insert(new_betas.begin(), new_betas.end());
    } while( new_betas.size() );

    // Create inner regions - final phase
    if( verbose )
        cerr << "  Creating inner regions (final phase)" << endl;
    vector<Region> irs;
    irs.reserve( betas.size() );
    for( set<VarSet>::const_iterator beta = betas.begin(); beta != betas.end(); beta++ )
        irs.push_back( Region(*beta,0.0) );

    // Create edges
    if( verbose )
        cerr << "  Creating edges" << endl;
    vector<pair<size_t,size_t> > edges;
    for( size_t beta = 0; beta < irs.size(); beta++ )
        for( size_t alpha = 0; alpha < cg.nrClusters(); alpha++ )
            if( cg.cluster(alpha) >> irs[beta] )
                edges.push_back( pair<size_t,size_t>(alpha,beta) );

    // Construct region graph
    if( verbose )
        cerr << "  Constructing region graph" << endl;
    construct( fg, cg.clusters(), irs, edges );

    // Calculate counting numbers
    if( verbose )
        cerr << "  Calculating counting numbers" << endl;
    calcCVMCountingNumbers();

    if( verbose )
        cerr << "Done." << endl;
}

void RegionGraph::calcCVMCountingNumbers() {
    // Calculates counting numbers of inner regions based upon counting numbers of outer regions

    vector<vector<size_t> > ancestors(nrIRs());
    boost::dynamic_bitset<> assigned(nrIRs());
    for( size_t beta = 0; beta < nrIRs(); beta++ ) {
        IR(beta).c() = 0.0;
        for( size_t beta2 = 0; beta2 < nrIRs(); beta2++ )
            if( (beta2 != beta) && IR(beta2) >> IR(beta) )
                ancestors[beta].push_back(beta2);
    }

    bool new_counting;
    do {
        new_counting = false;
        for( size_t beta = 0; beta < nrIRs(); beta++ ) {
            if( !assigned[beta] ) {
                bool has_unassigned_ancestor = false;
                for( vector<size_t>::const_iterator beta2 = ancestors[beta].begin(); (beta2 != ancestors[beta].end()) && !has_unassigned_ancestor; beta2++ )
                    if( !assigned[*beta2] )
                        has_unassigned_ancestor = true;
                if( !has_unassigned_ancestor ) {
                    Real c = 1.0;
                    bforeach( const Neighbor &alpha, nbIR(beta) )
                        c -= OR(alpha).c();
                    for( vector<size_t>::const_iterator beta2 = ancestors[beta].begin(); beta2 != ancestors[beta].end(); beta2++ )
                        c -= IR(*beta2).c();
                    IR(beta).c() = c;
                    assigned.set(beta, true);
                    new_counting = true;
                }
            }
        }
    } while( new_counting );
}

bool RegionGraph::checkCountingNumbers() const {
    // Checks whether the counting numbers satisfy the fundamental relation

    bool all_valid = true;
    for( vector<Var>::const_iterator n = vars().begin(); n != vars().end(); n++ ) {
        Real c_n = 0.0;
        for( size_t alpha = 0; alpha < nrORs(); alpha++ )
            if( OR(alpha).vars().contains( *n ) )
                c_n += OR(alpha).c();
        for( size_t beta = 0; beta < nrIRs(); beta++ )
            if( IR(beta).contains( *n ) )
                c_n += IR(beta).c();
        if( fabs(c_n - 1.0) > 1e-15 ) {
            all_valid = false;
            cerr << "WARNING: counting numbers do not satisfy relation for " << *n << "(c_n = " << c_n << ")." << endl;
        }
    }

    return all_valid;
}

void RegionGraph::recomputeORs() {
    for( size_t alpha = 0; alpha < nrORs(); alpha++ )
        OR(alpha).fill( 1.0 );
    for( size_t I = 0; I < nrFactors(); I++ )
        if( fac2OR(I) != -1U )
            OR( fac2OR(I) ) *= factor( I );
}

void RegionGraph::recomputeORs( const VarSet &ns ) {
    for( size_t alpha = 0; alpha < nrORs(); alpha++ )
        if( OR(alpha).vars().intersects( ns ) )
            OR(alpha).fill( 1.0 );
    for( size_t I = 0; I < nrFactors(); I++ )
        if( fac2OR(I) != -1U )
            if( OR( fac2OR(I) ).vars().intersects( ns ) )
                OR( fac2OR(I) ) *= factor( I );
}

void RegionGraph::recomputeOR( size_t I ) {
    DAI_ASSERT( I < nrFactors() );
    if( fac2OR(I) != -1U ) {
        size_t alpha = fac2OR(I);
        OR(alpha).fill( 1.0 );
        for( size_t J = 0; J < nrFactors(); J++ )
            if( fac2OR(J) == alpha )
                OR(alpha) *= factor( J );
    }
}

/// Send RegionGraph to output stream
ostream & operator << (ostream & os, const RegionGraph & rg) {
    os << "digraph RegionGraph {" << endl;
    os << "node[shape=box];" << endl;
    for( size_t alpha = 0; alpha < rg.nrORs(); alpha++ )
        os << "\ta" << alpha << " [label=\"a" << alpha << ": " << rg.OR(alpha).vars() << ", c=" << rg.OR(alpha).c() << "\"];" << endl;
    os << "node[shape=ellipse];" << endl;
    for( size_t beta = 0; beta < rg.nrIRs(); beta++ )
        os << "\tb" << beta << " [label=\"b" << beta << ": " << (VarSet)rg.IR(beta) << ", c=" << rg.IR(beta).c() << "\"];" << endl;
    for( size_t alpha = 0; alpha < rg.nrORs(); alpha++ )
        bforeach( const Neighbor &beta, rg.nbOR(alpha) )
            os << "\ta" << alpha << " -> b" << beta << ";" << endl;
    os << "}" << endl;
    return os;
}

} // end of namespace dai