/*
 * cte.cpp
 *
 *  Created on: 14 Sep 2018
 *      Author: radu
 *
 * Copyright (c) 2015, International Business Machines Corporation
 * and University of California Irvine. All rights reserved.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "cte.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

using namespace boost;
typedef adjacency_list<vecS, vecS, undirectedS, no_property,
		property<edge_weight_t, int> > BoostGraph;
typedef graph_traits<BoostGraph>::edge_descriptor BoostEdge;
typedef graph_traits<BoostGraph>::vertex_descriptor BoostVertex;
typedef std::pair<int, int> E;

namespace merlin {

// Initialize the clique tree
void cte::init() {

	// Prologue
	if (m_verbose > 0) {
		std::cout << "[CTE] + inference task   : " << m_task << std::endl;
	}

	if (m_query.empty() == false) {
		if (m_verbose > 0) {
			std::cout << "[CTE] + query vars       : ";
			std::copy(m_query.begin(), m_query.end(), std::ostream_iterator<vindex>(std::cout, " "));
			std::cout << std::endl;
		}

		// Add a dummy factor over the query variables
		if (m_query.size() <= MERLIN_MAXSIZE_JOINT_MARGINAL) {
			variable_set scope;
			for (size_t i = 0; i < m_query.size(); ++i) {
				scope |= m_gmo.var(m_query[i]);
			}

			factor q(scope, 1.0);
			m_gmo.add_factor(q);
		}
	}

	if (m_order.size() == 0) { // if we need to construct an elimination ordering
		m_order = m_gmo.order(m_order_method);
	}

	if (m_verbose > 0) {
		std::cout << "[CTE] + ordering method  : " << m_order_method << std::endl;
		std::cout << "[CTE] + elimination      : ";
		std::copy(m_order.begin(), m_order.end(),
				std::ostream_iterator<size_t>(std::cout, " "));
		std::cout << std::endl;

		// Calculate the induced width of the elimination ordering
		size_t wstar = m_gmo.induced_width(m_order);
		std::cout << "[CTE] + induced width    : " << wstar << std::endl;
		std::cout << "[CTE] + exact inference  : Yes" << std::endl;
		std::cout << "[CTE] + ordering time    : " << (timeSystem() - m_start_time) << " seconds" << std::endl;
		std::cout << "[CTE] Building clique tree ... " << std::endl;
	}

	// Get the factors scopes
	size_t n = m_gmo.num_nodes();
	vector<variable_set> fin;
	for (vector<factor>::const_iterator i = m_gmo.get_factors().begin();
			i != m_gmo.get_factors().end(); ++i) {
		fin.push_back((*i).vars());
	}

	// Create the graph
	graph g(n);
	g.init(fin);

	// Triangulate the graph
	g.triangulate(m_order);

	// Find the maximal cliques
	std::vector<std::set<size_t> > clusters = g.maximal_cliques(m_order);

	// Build the clique tree
	build_clique_tree(clusters);

	if (m_verbose > 0) {
		std::cout << "[CTE] Created clique tree with " << m_clusters.size()
				<< " clique factors" << std::endl;
	}

	size_t max_clique_size = 0, max_sepset_size = 0;
	for (size_t i = 0; i < m_edges.size(); ++i) {
		detail::edge* e = m_edges[i];
		max_clique_size = std::max(max_clique_size, e->first->clique.size());
		max_clique_size = std::max(max_clique_size, e->second->clique.size());
		max_sepset_size = std::max(max_sepset_size, e->sepset.size());
	}

	// Initialize beliefs (marginals)
	m_logz = 0;
	m_beliefs.clear();
	m_beliefs.resize(m_gmo.nvar(), factor(1.0));

	// Output the bucket tree statistics
	if (m_verbose > 0) {
		double elapsed = (timeSystem() - m_start_time);
		std::cout << "[CTE] Number of cliques  : " << m_clusters.size() << std::endl;
		std::cout << "[CTE] Number of edges    : " << m_edges.size() << std::endl;
		std::cout << "[CTE] Max clique size    : " << max_clique_size << std::endl;
		std::cout << "[CTE] Max separator size : " << max_sepset_size << std::endl;
		std::cout << "[CTE] Finished initialization in " << elapsed << " seconds" << std::endl;
	}
}

// Build the join tree and initialize the cluster factors
void cte::build_clique_tree(std::vector<std::set<size_t> >& cliques) {

	size_t n = cliques.size();
	std::vector<std::vector<int> > adjacencies;
	adjacencies.resize(n);
	for (std::vector<std::vector<int> >::iterator it = adjacencies.begin();
			it != adjacencies.end(); ++it) {
		it->resize(n);
	}

	// create nodes
	m_clusters.resize(n);
	for (size_t i = 0; i < n; ++i) {
		m_clusters[i].id = i;
		for (std::set<size_t>::iterator j = cliques[i].begin();
				j != cliques[i].end(); ++j) {
			m_clusters[i].clique |= var(*j);
		}
	}

	// fill in the weights
	for (size_t i = 0; i < n; ++i) {
		for (size_t j = 0; j < n; ++j) {
			if (i != j) {
				detail::node& ni = m_clusters[i];
				detail::node& nj = m_clusters[j];

				std::set<int> sep;
				std::set_intersection(ni.clique.begin(), ni.clique.end(),
						nj.clique.begin(), nj.clique.end(),
						std::inserter(sep, sep.begin()));

				if (sep.empty()) {
					adjacencies[i][j] = (100000);
				} else {
					adjacencies[i][j] = -(sep.size());
				}
			}
		}
	}

	// create a boost weighted graph and run Kruskal on it
	const int num_nodes = n;
	std::vector<E> temp1;
	std::vector<int> temp2;
	for (int i = 0; i < num_nodes - 1; ++i) {
		for (int j = i + 1; j < num_nodes; ++j) {
			if (i != j) {
				temp1.push_back( E(i, j) );
				temp2.push_back( adjacencies[i][j] );
			}
		}
	}

	E* edge_array = new E[temp1.size()];
	int* weights = new int[temp1.size()];
	for (size_t i = 0; i < temp1.size(); ++i) {
		edge_array[i] = temp1[i];
		weights[i] = temp2[i];
	}

	std::size_t num_edges = temp1.size();

	BoostGraph g(edge_array, edge_array + num_edges, weights, num_nodes);
	std::vector<BoostEdge> spanning_tree;

	kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

	if (m_debug) {
		std::cout << "Undirected junction tree (MST):" << std::endl;
		property_map<BoostGraph, edge_weight_t>::type weight = get(edge_weight, g);
		for (std::vector<BoostEdge>::iterator ei = spanning_tree.begin();
				ei != spanning_tree.end(); ++ei) {
			std::cout << source(*ei, g) << " <--> " << target(*ei, g)
					<< " with weight of " << weight[*ei] << "\n";
		}
	}

	// select the root and redirect edges outwards
	m_root = &(m_clusters.back());
	size_t root = m_root->id;
	std::set<size_t> visited;
	std::stack<size_t> dfs;
	dfs.push(root);
	while ( !dfs.empty() ) {

		size_t c = dfs.top(); // current node
		dfs.pop();

		// get all unvisited children and direct them towards n
		for (std::vector<BoostEdge>::iterator ei = spanning_tree.begin();
				ei != spanning_tree.end(); ++ei) {
			size_t src = (size_t) source(*ei, g);
			size_t trg = (size_t) target(*ei, g);
			detail::node* from = NULL, *to = NULL;

			if (src == c &&
				std::find(visited.begin(), visited.end(), trg) == visited.end()) {

				from = &(m_clusters[trg]); // from
				to = &(m_clusters[src]); // to
				dfs.push(trg);
			}

			if (trg == c &&
				std::find(visited.begin(), visited.end(), src) == visited.end()) {

				from = &(m_clusters[src]); // from
				to = &(m_clusters[trg]); // to
				dfs.push(src);
			}

			// create the directed edge
			if (from != NULL && to != NULL) {
				detail::edge* e = new detail::edge(from, to);
				m_edges.push_back(e);
				from->edges.push_back(e);
				to->edges.push_back(e);
				to->children.push_back(from);
				from->parent = to;
			}
		}

		visited.insert(c);
	}

	// find the message order (schedule) by a bfs of the junction tree
	std::queue<detail::node*> bfs;
	bfs.push(m_root);
	while (!bfs.empty()) {
		detail::node* c = bfs.front();
		bfs.pop();
		vector<detail::edge*>& elist = c->edges;
		for (vector<detail::edge*>::iterator it = elist.begin();
				it != elist.end(); ++it) {
			detail::edge* e = (*it);
			if (e->second->id == c->id) {
				m_messages.push_back(e);
			}
		}

		for (vector<detail::node*>::iterator it = c->children.begin();
				it != c->children.end(); ++it) {
			bfs.push(*it);
		}
	}

	// safety checks
	assert(m_messages.size() == m_edges.size());

	// reverse the order to start from the leaves
	std::reverse(m_messages.begin(), m_messages.end());

	// clean up
	delete[] weights;
	delete[] edge_array;

	if (m_debug) {
		std::ofstream fout("kruskal.dot");
		fout << "digraph JT {\n" << " rankdir=LR\n" << " size=\"3,3\"\n"
				<< " ratio=\"filled\"\n" << " edge[style=\"bold\"]\n"
				<< " node[shape=\"circle\"]\n";
		// nodes
		for (size_t i = 0; i < m_clusters.size(); ++i) {
			std::stringstream label;
			label << m_clusters[i].id << ": " << m_clusters[i].clique;
			fout << "node" << m_clusters[i].id
				<< "[ label = \"" << label.str() << "\"];\n";
		}

		// edges
		for (size_t i = 0; i < m_edges.size(); ++i) {
			detail::edge* e = m_edges[i];
			fout << "node" << e->first->id << " -> " << "node" << e->second->id << ";\n";
		}

		fout << "}\n";
		fout.close();
	}

	// Map variables to clusters
	m_var2clique.resize(m_gmo.nvar(), -1);
	for (size_t i = 0; i < m_clusters.size(); ++i) {
		detail::node& cl = m_clusters[i];
		variable_set& vars = cl.clique;
		for (variable_set::const_iterator j = vars.begin();
				j != vars.end(); ++j) {
			size_t v = (*j).label();
			if (m_var2clique[v] < 0) {
				m_var2clique[v] = cl.id;
			}
		}
	}

	// Allocate original functions to clusters and compute clique factors
	size_t idx = 0;
	for (vector<factor>::const_iterator fi = m_gmo.get_factors().begin();
			fi != m_gmo.get_factors().end(); ++fi, ++idx) {

		const factor& f = (*fi);
		for (size_t ci = 0; ci != m_clusters.size(); ++ci) {
			detail::node& cl = m_clusters[ci];
			if ((f.vars() & cl.clique) == f.vars()) {
				cl.originals.push_back(idx);
				cl.theta *= f;
				break;
			}
		}
	}

	if (m_debug) {
		std::cout << "Initial cluster factors:" << std::endl;
		for (size_t ci = 0; ci != m_clusters.size(); ++ci) {
			detail::node& cl = m_clusters[ci];
			std::cout << " " << ci << " -> " << cl.theta << std::endl;
		}
	}
}

// Update the collection of factors, recompute the clique factors and reset the
// messages. We assume there is a one-to-one mapping between the old factors
// and the new factors. This method is to be used only for EM param learning.
void cte::reinit(const std::vector<factor>& factors) {

	// Set the new factors
	for (size_t i = 0; i < factors.size(); ++i) {
		m_gmo.set_factor(i, factors[i]);
	}

	// Recompute the clique factors
	for (size_t ci = 0; ci != m_clusters.size(); ++ci) {
		detail::node& cl = m_clusters[ci];
		cl.theta = factor(1.0);
		for (size_t j = 0; j < cl.originals.size(); ++j) {
			const factor& f = m_gmo.get_factor(cl.originals[j]);
			cl.theta *= f;
		}
	}

	// Reset the edge messages
	for (vector<detail::edge*>::iterator it = m_edges.begin();
			it != m_edges.end(); ++it) {
		(*it)->reset();
	}

	// Reset the log partition function and beliefs
	m_logz = 0;
	m_beliefs.clear();
	m_beliefs.resize(m_gmo.nvar(), factor(1.0));

	if (m_debug) {
		std::cout << "Initial cluster factors:" << std::endl;
		for (size_t ci = 0; ci != m_clusters.size(); ++ci) {
			detail::node& cl = m_clusters[ci];
			std::cout << " " << ci << " -> " << cl.theta << std::endl;
		}
	}
}

// Forward message propagation (from leaves to root)
void cte::forward() {
	double timestamp = timeSystem();
	for (vector<detail::edge*>::iterator i = m_messages.begin();
			i != m_messages.end(); ++i) {
		detail::edge* e = (*i);
		e->messageFwd();
	}

	if (m_verbose > 0) {
		std::cout << "[CTE] Finished forward pass in " << (timeSystem() - timestamp)
			<< " seconds" << std::endl;
	}
}

// Forward message propagation with evidence (from leaves to root)
void cte::forward(std::vector<int>& evidence) {
	for (vector<detail::edge*>::iterator i = m_messages.begin();
			i != m_messages.end(); ++i) {
		detail::edge* e = (*i);
		e->messageFwd(evidence);

		if (m_debug) {
			std::cout << " -> forward msg from "
				<< e->first->id << " to " << e->second->id
				<< ": " << e->fwd << std::endl;
		}
	}
}

// Forward message on an edge
void detail::edge::messageFwd() {

	// collect the original functions
	factor F = first->theta;

	// collect the incoming messages (should already be projected)
	for (size_t i = 0; i < first->edges.size(); ++i) {
		detail::edge* e = first->edges[i];
		if (e == this) {
			continue; // skip current edge
		}
		if (e->first->id == first->id) {
			F *= e->bwd; //e->getMessage2();
		} else if (e->second->id == first->id) {

			F *= e->fwd; //e->getMessage1();
		}
	}

	// marginalize the eliminator
	variable_set elim = (first->clique - sepset);
	if (elim.size() > 0) {
		F = F.sum(elim);
	}

	// update the forward message
	this->fwd = F;
}

// Forward message computed with evidence on an edge
void detail::edge::messageFwd(std::vector<int>& evidence) {

	// collect the original functions
	factor F = first->theta;
	F = F.condition(evidence);

	// collect the incoming messages (should already be projected)
	for (size_t i = 0; i < first->edges.size(); ++i) {
		detail::edge* e = first->edges[i];
		if (e == this) {
			continue; // skip current edge
		}
		if (e->first->id == first->id) {
			F *= e->bwd.condition(evidence);
		} else if (e->second->id == first->id) {

			F *= e->fwd.condition(evidence);
		}
	}

	// Find the evidence variables in the scope of the cluster
	variable_set evid;
	for (variable_set::const_iterator i = first->clique.begin();
			i != first->clique.end(); ++i) {
		int v = i->label();
		if (evidence[v] >= 0) {
			evid |= *i;
		}
	}

	// Marginalize the non-evidence eliminator
	//F = F.condition(evidence);
	variable_set elim = (first->clique - sepset);
	elim -= evid;
	if (elim.size() > 0) {
		F = F.sum(elim);
	}

	// update the forward message
	this->fwd = F;

}

// Backward message propagation (from root to leaves)
void cte::backward() {
	double timestamp = timeSystem();
	for (vector<detail::edge*>::reverse_iterator ri = m_messages.rbegin();
			ri != m_messages.rend(); ++ri) {
		detail::edge* e = (*ri);
		e->messageBwd();
	}

	if (m_verbose > 0) {
		std::cout << "[CTE] Finished backward pass in " << (timeSystem() - timestamp)
			<< " seconds" << std::endl;
	}
}

// Backward message propagation with evidence (from root to leaves)
void cte::backward(std::vector<int>& evidence) {
	for (vector<detail::edge*>::reverse_iterator ri = m_messages.rbegin();
			ri != m_messages.rend(); ++ri) {
		detail::edge* e = (*ri);
		e->messageBwd(evidence);

		if (m_debug) {
			std::cout << " <- backward msg from "
				<< e->second->id << " to " << e->first->id
				<< ": " << e->bwd << std::endl;
		}
	}
}

// Backward message on an edge
void detail::edge::messageBwd() {

	// collect the original functions
	factor F = second->theta;

	// collect the incoming messages
	for (size_t i = 0; i < second->edges.size(); ++i) {
		detail::edge* e = second->edges[i];
		if (e == this) {
			continue; // skip current edge
		}
		if (e->first->id == second->id) {
			F *= e->bwd; //e->getMessage2();
		} else if (e->second->id == second->id) {
			F *= e->fwd; //e->getMessage1();
		}
	}

	// marginalize the eliminator
	variable_set elim = (second->clique - sepset);
	if (elim.size() > 0) {
		F = F.sum(elim);
	}

	this->bwd = F;
}

// Backward message computed with evidence on an edge
void detail::edge::messageBwd(std::vector<int>& evidence) {

	// collect the original functions
	factor F = second->theta;
	F = F.condition(evidence);

	// collect the incoming messages
	for (size_t i = 0; i < second->edges.size(); ++i) {
		detail::edge* e = second->edges[i];
		if (e == this) {
			continue; // skip current edge
		}
		if (e->first->id == second->id) {
			F *= e->bwd.condition(evidence); //e->getMessage2();
		} else if (e->second->id == second->id) {
			F *= e->fwd.condition(evidence); //e->getMessage1();
		}
	}

	// find the evidence variables in the scope of the cluster
	variable_set evid;
	for (variable_set::const_iterator i = second->clique.begin();
			i != second->clique.end(); ++i) {
		int v = i->label();
		if (evidence[v] >= 0) {
			evid |= *i;
		}
	}

	// marginalize the eliminator
	variable_set elim = (second->clique - sepset);
	elim -= evid;
	if (elim.size() > 0) {
		F = F.sum(elim);
	}

	this->bwd = F;

}

// Clique tree calibration
void cte::calibrate() {

	forward();
	backward();
}

// Update the beliefs (marginals or max-marginals)
void cte::update() {

	// Compute the clique beliefs
	for (size_t i = 0; i < m_clusters.size(); ++i) {
		detail::node& cl = m_clusters[i];
		cl.belief = cl.theta;
		for (vector<detail::edge*>::iterator ei = cl.edges.begin();
				ei != cl.edges.end(); ++ei) {
			detail::edge* e = (*ei);
			if (e->second->id == cl.id) {
				cl.belief *= e->fwd;
			}
			if (e->first->id == cl.id) {
				cl.belief *= e->bwd;
			}
		}
	}

	// Compute the log partition function
	m_logz = std::log(m_root->belief.sum());

	// Compute the marginals
	for (size_t v = 0; v < m_var2clique.size(); ++v) {
		size_t ci = m_var2clique[v];
		detail::node& cl = m_clusters[ci];
		variable VX = m_gmo.var(v);

		m_beliefs[v] = marg(cl.belief, VX);
		m_beliefs[v].normalize(); // normalize
	}
}

// Compute the joint marginal of a set of variables
void cte::joint_marginal(const variable_set& scope) {

	// Find the shallowest cliques that contain the scope
	std::vector<int> nodes;
	variable_set temp = scope;
	while (temp.size() > 0) {
		int max_id = -1, max_score = -1;
		std::queue<detail::node*> bfs;
		bfs.push(m_root);
		while (!bfs.empty()) {
			detail::node* c = bfs.front();
			bfs.pop();

			int score = (c->clique & temp).size();
			if (score > 0 && score > max_score) {
				max_score = score;
				max_id = c->id;
			}

			for (vector<detail::node*>::iterator it = c->children.begin();
					it != c->children.end(); ++it) {
				bfs.push(*it);
			}
		}

		assert(max_id >= 0);
		nodes.push_back(max_id);
		temp = temp - (temp & m_clusters[max_id].clique);
	}

	if (m_debug) {
		std::cout << "[DEBUG] Found the shallowest nodes: ";
		std::copy(nodes.begin(), nodes.end(),
				std::ostream_iterator<int>(std::cout, " "));
		std::cout << std::endl;
	}

	// Check if the scope is included in one clique
	if (nodes.size() == 1) { // one clique
		m_marginal = marg(m_clusters[nodes.at(0)].belief, scope);
		m_marginal.normalize();
	} else { // multiple cliques

		// Collect all relevant factors
		vector<factor> factors;
		factors.push_back(m_root->belief);
		for (size_t i = 0; i < nodes.size(); ++i) {
			detail::node* c = &(m_clusters.at(nodes[i]));
			while (c != NULL) {
				for (vector<detail::edge*>::iterator ei = c->edges.begin();
						ei != c->edges.end(); ++ei) {
					detail::edge* e = (*ei);
					if (e->first->id == c->id) {
						factor f = c->belief;
						f /= e->fwd;
						factors.push_back(f);
						break;
					}
				}

				c = c->parent;
				if (c == m_root) {
					break;
				}
			}
		}

		variable_set all_vars, elim_vars;
		for (vector<factor>::iterator fi = factors.begin();
				fi != factors.end(); ++fi) {
			all_vars |= fi->vars();
		}
		elim_vars = (all_vars - scope);

		// Run variables elimination over these factors
		variable_order_t order;
		for (size_t i = 0; i < m_order.size(); ++i) {
			size_t v = m_order[i];
			if (elim_vars.contains(m_gmo.var(v))) {
				order.push_back(v);
			}
		}

		if (m_debug) {
			std::cout << "[DEBUG] All vars: " << all_vars << std::endl;
			std::cout << "[DEBUG] Scope: " << scope << std::endl;
			std::cout << "[DEBUG] Elim: " << elim_vars << std::endl;
			std::cout << "[DEBUG] Factors: " << factors.size() << std::endl;
			std::cout << "[DEBUG] Elim order: ";
			std::copy(order.begin(), order.end(),
					std::ostream_iterator<size_t>(std::cout, " "));
			std::cout << std::endl;
		}

		// Run variable elimination
		for (size_t i = 0; i < order.size(); ++i) {
			size_t v = order[i];
			variable VX = m_gmo.var(v);

			// Collect all factors mentioning VX
			factor f(1.0);
			for (vector<factor>::iterator it = factors.begin();
					it != factors.end();) {
				if (it->variables().contains(VX)) {
					f *= (*it);
					it = factors.erase(it);
				} else {
					++it;
				}
			}

			// Eliminate the variable
			f = elim(f, VX);
			factors.push_back(f);
		}

		// Compute the joint marginal
		m_marginal = factor(1.0);
		for (vector<factor>::iterator it = factors.begin();
				it != factors.end(); ++it) {
			m_marginal *= (*it);
			m_marginal.normalize();
		}
	}
}

// The scope must be included in one cluster (Bayes nets only)
void cte::joint_marginal(const variable_set& scope, std::vector<int>& evidence) {

	// Get the cluster that contains the scope
	bool found = false;
	size_t j = 0;
	for (size_t i = 0; i < m_clusters.size(); ++i) {
		detail::node& cl = m_clusters[i];
		variable_set temp = (cl.clique & scope);
		if (temp == scope) {
			found = true;
			j = i;
			break;
		}
	}

	// Safety check
	assert(found);

	// Compute the joint marginal (belief) subject to evidence
	m_marginal = factor(scope, 0.0);
	detail::node& n = m_clusters[j];
	n.belief = n.theta.condition(evidence);
	for (vector<detail::edge*>::iterator ei = n.edges.begin();
			ei != n.edges.end(); ++ei) {
		detail::edge* e = (*ei);
		if (e->second->id == n.id) {
			n.belief *= e->fwd;
		}
		if (e->first->id == n.id) {
			n.belief *= e->bwd;
		}
	}

	// Update the belief entries subject to evidence
	variable_set v_rem;
	for (variable_set::const_iterator vi = n.clique.begin();
			vi != n.clique.end(); ++vi) {
		if (!scope.contains(*vi) && evidence[vi->label()] < 0) {
			v_rem |= *vi;
		}
	}
	n.belief = n.belief.sum(v_rem);

	if (m_debug) {
		std::cout << "[DEBUG] Joint marginal scope: " << m_marginal.vars() << std::endl;
		std::cout << "[DEBUG] Actual belief scope:  " << n.belief << std::endl;
	}

	// Fill in all configurations of the marginal (including the evidence)
	index_config cv1(m_marginal.vars(), true);
	config_index cv2(n.belief.vars(), true);
	for (size_t i = 0; i < m_marginal.num_states(); ++i) {
		std::map<size_t, size_t> config = cv1.convert(i);
		if (is_compatible(config, evidence)) {
			size_t j = cv2.convert(config);
			double v = n.belief.get(j);
			m_marginal.set(i, v);
		} else {
			m_marginal.set(i, 0.0);
		}
	}

	// Normalize by P(evidence)
	m_marginal /= std::exp(m_logz);

	if (m_debug) {
		std::cout << "[DEBUG] Joint marginal: " << m_marginal << std::endl;
	}
}

// Check if a variable configuration is compatible with the evidence vector
bool cte::is_compatible(std::map<size_t, size_t>& config,
		std::vector<int>& evidence) {
	bool ok = true;
	for (std::map<size_t, size_t>::iterator mi = config.begin();
			mi != config.end(); ++mi) {
		size_t var = mi->first;
		size_t val = mi->second;
		if (evidence[var] < 0) {
			continue; // skip non-evidence variable
		} else {
			if (val != evidence[var]) {
				ok = false;
				break;
			}
		}
	}

	return ok;
}

// Run the clique-tree algorithm with evidence (for EM learning)
// Returns *true* if P(evidence) > 0, and *false* otherwise
bool cte::propagate_evidence(std::vector<int>& evidence) {

	// Save the current evidence
	if (m_debug) {
		std::cout << "[CTE] Propagate evidence: ";
		std::copy(evidence.begin(), evidence.end(), std::ostream_iterator<int>(std::cout, " "));
		std::cout << std::endl;
	}

	m_evidence = evidence;
	forward(evidence);
	backward(evidence);

	// Compute the posterior marginals for all variables (evidence, non-evidence)
	for (size_t i = 0; i < m_clusters.size(); ++i) {
		detail::node& cl = m_clusters[i];
		cl.belief = cl.theta.condition(evidence);
		for (vector<detail::edge*>::iterator ei = cl.edges.begin();
				ei != cl.edges.end(); ++ei) {
			detail::edge* e = (*ei);
			if (e->second->id == cl.id) {
				cl.belief *= e->fwd;
			}
			if (e->first->id == cl.id) {
				cl.belief *= e->bwd;
			}
		}
	}

	// Compute the root clique belief (to get log partition function)
	m_root->belief = m_root->theta.condition(evidence);
	for (vector<detail::edge*>::iterator ei = m_root->edges.begin();
		ei != m_root->edges.end(); ++ei) {
		detail::edge* e = (*ei);
		if (e->second->id == m_root->id) {
			m_root->belief *= e->fwd;
		}
		if (e->first->id == m_root->id) {
			m_root->belief *= e->bwd;
		}
	}

	// Compute the probability of evidence
	bool result = true;
	factor::value pe = m_root->belief.sum();
	if (pe == 0.0) {
		result = false;
	}

	// Update the log partition function
	m_logz = std::log(m_root->belief.sum());

	// Compute the marginals
	for (size_t v = 0; v < m_var2clique.size(); ++v) {
		size_t ci = m_var2clique[v];
		detail::node& cl = m_clusters[ci];
		variable VX = m_gmo.var(v);

		if (evidence[v] >= 0) {
			factor tmp(variable_set(VX), 0.0);
			tmp.set(evidence[v], 1.0);
			m_beliefs[v] = tmp;
		} else {
			m_beliefs[v] = marg(cl.belief, VX);
			m_beliefs[v].normalize(); // normalize
		}
	}

	if (m_debug) {
		std::cout << "[CTE] Finished propagating evidence with logZ = "
				<< m_logz << " (" << std::exp(m_logz) << ")" << std::endl;
		std::cout << "[CTE] Posterior marginals:" << std::endl;
		for (size_t i = 0; i < m_beliefs.size(); ++i) {
			std::cout << " " << m_beliefs[i] << std::endl;
		}
	}

	return result;
}

/// Run the clique-tree elimination algorithm.
void cte::run() {

	// Start the timer and store it
	m_start_time = timeSystem();

	init();
	calibrate();
	update();
	if (!m_query.empty()) {
		variable_set scope;
		for (size_t i = 0; i < m_query.size(); ++i) {
			scope |= m_gmo.var(m_query[i]);
		}

		joint_marginal(scope);
	}

	// Output solution (UAI output format)
	if (m_verbose > 0) {
		std::cout << "[CTE] Finished in " << (timeSystem() - m_start_time)
			<< " seconds" << std::endl;
	}

	// Compute the joint marginal given in query

	switch (m_task) {
	case Task::PR:	// partition function
		{
			std::cout << "PR" << std::endl;
			std::cout << std::fixed << std::setprecision(MERLIN_PRECISION)
				<< m_logz << " (" << std::scientific
				<< std::setprecision(MERLIN_PRECISION)
				<< std::exp(m_logz) << ")" << std::endl;

			if (isinf(m_logz)) {
				std::cout << "STATUS" << std::endl;
				std::cout << "false: Inconsistent evidence or underflow" << std::endl;
			} else {
				std::cout << "STATUS" << std::endl;
				std::cout << "true: Consistent evidence" << std::endl;
			}

			break;
		}
	case Task::MAR:	// marginals
		{
			std::cout << "PR" << std::endl;
			std::cout << std::fixed << std::setprecision(MERLIN_PRECISION)
				<< m_logz << " (" << std::scientific
				<< std::setprecision(MERLIN_PRECISION)
				<< std::exp(m_logz) << ")" << std::endl;

			if (isinf(m_logz)) {
				std::cout << "STATUS" << std::endl;
				std::cout << "false: Inconsistent evidence or underflow" << std::endl;
			} else {
				std::cout << "STATUS" << std::endl;
				std::cout << "true: Consistent evidence" << std::endl;
			}

			std::cout << "MAR" << std::endl;
			std::cout << m_gmo.nvar();
			for (vindex v = 0; v < m_gmo.nvar(); ++v) {
				variable VX = m_gmo.var(v);
				std::cout << " " << VX.states();
				for (size_t k = 0; k < VX.states(); ++k) {
					std::cout << " " << std::fixed
						<< std::setprecision(MERLIN_PRECISION)
						<< belief(VX)[k];
				}
			}
			std::cout << std::endl;
			if (!m_query.empty()) {
				variable_set scope = m_marginal.variables();
				std::cout << "JOINT_MAR : " << scope << std::endl;
				std::vector<size_t> dims(scope.size());
				size_t j = 0;
				for (variable_set::const_iterator si = scope.begin(); si != scope.end(); ++si) {
					dims[j++] = (*si).states();
				}

				for (size_t index = 0; index < m_marginal.numel(); ++index) {
					std::vector<size_t> I(dims.size(), 0); // configuration of source variables
					size_t i = index;
					for (size_t v = 0; v < dims.size(); ++v) {
						I[v] = i % dims[v];
						i -= I[v];
						i /= dims[v];
					}

					for (size_t j = 0; j < I.size(); ++j) {
						std::cout << I[j] << " ";
					}
					std::cout << ": " << std::fixed
							<< std::setprecision(MERLIN_PRECISION)
							<< m_marginal[index]
							<< std::endl;
				}
			}

			break;
		}
	default:
		break;
	}
}

/// Write the solution to the output stream
void cte::write_solution(std::ostream& out, const std::map<size_t, size_t>& evidence,
		const std::map<size_t, size_t>& old2new, const graphical_model& orig,
		const std::set<size_t>& dummies, int output_format) {

	if (output_format == MERLIN_OUTPUT_JSON) {
		out << "{";
		out << " \"algorithm\" : \"cte\", ";
		switch (m_task) {
		case Task::PR:
			{
				double val = m_logz + std::log(orig.get_global_const());
				double prob = std::exp(val);

				out << " \"task\" : \"PR\", ";
				out << " \"value\" : " << std::fixed
					<< std::setprecision(MERLIN_PRECISION)
					<< (m_logz + std::log(orig.get_global_const())) << ", ";
				if (prob == 0.0) { // probability of evidence is 0
					out << " \"status\" : \"false\", ";
					out << " \"message\" : \"Inconsistent evidence or underflow\" ";
				} else {
					out << " \"status\" : \"true\", ";
					out << " \"message\" : \"Consistent evidence\" ";
				}

				break;
			}
		case Task::MAR:
			{
				double val = m_logz + std::log(orig.get_global_const());
				double prob = std::exp(val);

				out << " \"task\" : \"MAR\", ";
				out << " \"value\" : " << std::fixed
					<< std::setprecision(MERLIN_PRECISION)
					<< (m_logz + std::log(orig.get_global_const())) << ", ";

				if (prob == 0.0) { // probability of evidence is 0
					out << " \"status\" : \"false\", ";
					out << " \"message\" : \"Inconsistent evidence or underflow\", ";
					out << " \"marginals\" : [] ";
				} else {
					out << " \"status\" : \"true\", ";
					out << " \"message\" : \"Consistent evidence\", ";
					out << " \"marginals\" : [ ";

					for (vindex i = 0; i < orig.nvar(); ++i) {
						if (dummies.find(i) != dummies.end()) {
							continue; // skip dummy variables from output
						}

						variable v = orig.var(i);
						out << "{";
						out << " \"variable\" : " << v.label() << ", ";
						out << " \"states\" : " << v.states() << ", ";
						out << " \"probabilities\" : [";
						try { // evidence variable
							size_t val = evidence.at(i);
							for (size_t k = 0; k < v.states(); ++k) {
								out << std::fixed
									<< std::setprecision(MERLIN_PRECISION)
									<< (k == val ? 1.0 : 0.0);
								if (k != v.states() - 1) {
									out << ", ";
								}
							}
							out << "] ";
						} catch(std::out_of_range& e) { // non-evidence variable
							vindex vx = old2new.at(i);
							variable VX = var(vx);
							for (size_t k = 0; k < VX.states(); ++k) {
								out << std::fixed
									<< std::setprecision(MERLIN_PRECISION)
									<< belief(VX)[k];
								if (k != VX.states() - 1) {
									out << ", ";
								}
							}
							out << "] ";
						}
						out << "}";
						if (i != orig.nvar() - 1) {
							out << ", ";
						}
					} // end for
					out << "] ";

					if (!m_query.empty()) {
						out << ", ";
						out << "\"joint_marginal\" : {";
						variable_set scope = m_marginal.variables();
						out << "\"scope\" : [";
						for (size_t i = 0; i < m_query.size(); ++i) {
							out << m_query[i];
							if (i < m_query.size() - 1) {
								out << ",";
							}
						}
						out << "], " << std::endl;
						out << "\"probabilities\" : [";

						std::vector<size_t> dims(scope.size());
						size_t j = 0;
						for (variable_set::const_iterator si = scope.begin();
								si != scope.end(); ++si) {
							dims[j++] = (*si).states();
						}

						size_t N = m_marginal.numel();
						for (size_t index = 0; index < N; ++index) {
							std::vector<size_t> I(dims.size(), 0); // configuration of source variables
							size_t i = index;
							for (size_t v = 0; v < dims.size(); ++v) {
								I[v] = i % dims[v];
								i -= I[v];
								i /= dims[v];
							}

							out << "{\"config\" : [";
							for (size_t j = 0; j < I.size(); ++j) {
								out << I[j];
								if (j < I.size() - 1) {
									out << ",";
								}
							}
							out << "], ";
							out << "\"value\" : " << std::fixed
									<< std::setprecision(MERLIN_PRECISION)
									<< m_marginal[index]
									<< "}";
							if (index < N - 1) {
								out << ", ";
							}
						}
						out << "]}";
					}
				}

				break;
			}
		default:
			break;
		}

		out << "}";
	} else if (output_format == MERLIN_OUTPUT_UAI) {
		switch (m_task) {
		case Task::PR:
		case Task::MAR:
			{
				double val = m_logz + std::log(orig.get_global_const());
				double prob = std::exp(val);

				out << "PR" << std::endl;
				out << std::fixed << std::setprecision(MERLIN_PRECISION)
					<< (m_logz + std::log(orig.get_global_const())) << " ("
					<< std::scientific << std::setprecision(MERLIN_PRECISION)
					<< std::exp(m_logz + std::log(orig.get_global_const()))
					<< ")" << std::endl;

				if (prob == 0.0) {
					out << "STATUS" << std::endl;
					out << "false: Inconsistent evidence or underflow" << std::endl;
				} else {
					out << "STATUS" << std::endl;
					out << "true: Consistent evidence" << std::endl;
				}

				out << "MAR" << std::endl;
				out << orig.nvar() - dummies.size();
				for (vindex i = 0; i < orig.nvar(); ++i) {
					if (dummies.find(i) != dummies.end()) {
						continue; // skip dummy variables from output
					}

					variable v = orig.var(i);
					try { // evidence variable
						size_t val = evidence.at(i);
						out << " " << v.states();
						for (size_t k = 0; k < v.states(); ++k) {
							out << " " << std::fixed
								<< std::setprecision(MERLIN_PRECISION)
								<< (k == val ? 1.0 : 0.0);
						}
					} catch(std::out_of_range& e) { // non-evidence variable
						vindex vx = old2new.at(i);
						variable VX = var(vx);
						out << " " << VX.states();
						for (size_t k = 0; k < VX.states(); ++k) {
							out << " " << std::fixed
								<< std::setprecision(MERLIN_PRECISION)
								<< belief(VX)[k];
						}
					}
				} // end for
				out << std::endl;

				if (!m_query.empty()) {
					variable_set scope = m_marginal.variables();
					out << "JOINT_MAR : [";
					for (size_t i = 0; i < m_query.size(); ++i) {
						out << m_query[i];
						if (i < m_query.size() - 1) {
							out << ",";
						}
					}
					out << "]" << std::endl;

					std::vector<size_t> dims(scope.size());
					size_t j = 0;
					for (variable_set::const_iterator si = scope.begin();
							si != scope.end(); ++si) {
						dims[j++] = (*si).states();
					}

					for (size_t index = 0; index < m_marginal.numel(); ++index) {
						std::vector<size_t> I(dims.size(), 0); // configuration of source variables
						size_t i = index;
						for (size_t v = 0; v < dims.size(); ++v) {
							I[v] = i % dims[v];
							i -= I[v];
							i /= dims[v];
						}

						for (size_t j = 0; j < I.size(); ++j) {
							out << I[j] << " ";
						}
						out << ": " << std::fixed
								<< std::setprecision(MERLIN_PRECISION)
								<< m_marginal[index]
								<< std::endl;
					}
				}

				break;
			}
		default:
			break;
		}
	} else {
		std::string err_msg("Unknown output format.");
		throw std::runtime_error(err_msg);
	}
}

} // namespace


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

#include <third_party/align_point_clouds.h>

#include <glog/logging.h>
#include <Eigen/Dense>

namespace theia {

void AlignPointCloudsUmeyama(const std::vector<Eigen::Vector3d>& left,
                             const std::vector<Eigen::Vector3d>& right,
                             Eigen::Matrix3d* rotation,
                             Eigen::Vector3d* translation, double* scale) {
  std::vector<double> weights(left.size(), 1.0);
  AlignPointCloudsUmeyamaWithWeights(left, right, weights, rotation,
                                     translation, scale);
}

void AlignPointCloudsUmeyamaWithWeights(
    const std::vector<Eigen::Vector3d>& left,
    const std::vector<Eigen::Vector3d>& right,
    const std::vector<double>& weights, Eigen::Matrix3d* rotation,
    Eigen::Vector3d* translation, double* scale) {
  CHECK_EQ(left.size(), right.size());
  CHECK_EQ(left.size(), weights.size());
  CHECK_NOTNULL(rotation);
  CHECK_NOTNULL(translation);
  CHECK_NOTNULL(scale);

  // Fill outputs (useful when it fails)
  *scale = 1.0;
  *translation = Eigen::Vector3d::Zero();
  *rotation = Eigen::Matrix3d::Identity();

  const size_t num_points = left.size();
  Eigen::Map<const Eigen::Matrix<double, 3, Eigen::Dynamic> > left_points(
      left[0].data(), 3, num_points);
  Eigen::Map<const Eigen::Matrix<double, 3, Eigen::Dynamic> > right_points(
      right[0].data(), 3, num_points);

  Eigen::Vector3d left_centroid, right_centroid;
  left_centroid.setZero();
  right_centroid.setZero();
  double weights_sum = 0.0;
  for (size_t i = 0; i < num_points; i++) {
    CHECK_GE(weights[i], 0)
        << "The point weight must be greater or equal to zero.";
    weights_sum += weights[i];
    left_centroid += left[i] * weights[i];
    right_centroid += right[i] * weights[i];
  }
  // Check if the sum is valid
  CHECK_GT(weights_sum, 0) << "The sum of weights must be greater than zero.";

  left_centroid /= weights_sum;
  right_centroid /= weights_sum;

  double sigma = 0.0;
  for (size_t i = 0; i < num_points; i++) {
    sigma += (left[i] - left_centroid).squaredNorm() * weights[i];
  }
  sigma /= weights_sum;

  // Calculate cross correlation matrix based on the points shifted about the
  // centroid.
  Eigen::Matrix3d cross_correlation = Eigen::Matrix3d::Zero();
  for (int i = 0; i < num_points; i++) {
    cross_correlation += weights[i] * (left_points.col(i) - left_centroid) *
                         (right_points.col(i) - right_centroid).transpose();
  }
  cross_correlation /= weights_sum;

  // Compute SVD decomposition of the cross correlation.
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(
      cross_correlation.transpose(), Eigen::ComputeFullU | Eigen::ComputeFullV);

  const Eigen::Matrix3d& umatrix = svd.matrixU();
  const Eigen::Matrix3d& vtmatrix = svd.matrixV().transpose();
  const Eigen::Vector3d& singular_values = svd.singularValues();

  const double det = umatrix.determinant() * vtmatrix.determinant();
  Eigen::Matrix3d s = Eigen::Matrix3d::Identity();
  s(2, 2) = det > 0 ? 1 : -1;

  *scale =
      (singular_values(0) + singular_values(1) + s(2, 2) * singular_values(2)) /
      sigma;
  *rotation = umatrix * s * vtmatrix;
  *translation = right_centroid - (*scale) * (*rotation) * left_centroid;
}

}  // namespace theia


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = std::vector<int>;
using VI2D = std::vector<vector<int>>;
using VLL = std::vector<long long>;
using VLL2D = std::vector<vector<long long>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T, std::size_t N>
struct make_vector_type {
	using type =
		typename std::vector<typename make_vector_type<T, (N - 1)>::type>;
};

template <typename T>
struct make_vector_type<T, 0> {
	using type = typename std::vector<T>;
};

template <typename T, size_t N>
auto make_vector_impl(const std::vector<std::size_t>& ls, T init_value) {
	if constexpr(N == 0) {
		return std::vector<T>(ls[N], init_value);
	} else {
		return typename make_vector_type<T, N>::type(
			ls[N], make_vector_impl<T, (N - 1)>(ls, init_value));
	}
}

template <typename T, std::size_t N>
auto make_vector(const std::size_t (&ls)[N], T init_value) {
	std::vector<std::size_t> dimensions(N);
	for(int i = 0; i < N; i++) {
		dimensions[N - i - 1] = ls[i];
	}
	return make_vector_impl<T, N - 1>(dimensions, init_value);
}

template <typename T>
std::vector<T> make_vector(std::size_t size, T init_value) {
	return std::vector<T>(size, init_value);
}

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max<T>(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min<T>(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

#ifndef ATCODER_FENWICKTREE_HPP
#define ATCODER_FENWICKTREE_HPP 1

#include <cassert>
#include <type_traits>
#include <vector>

namespace atcoder {

namespace internal {

#ifndef _MSC_VER
template <class T>
using is_signed_int128 =
	typename std::conditional<std::is_same<T, __int128_t>::value ||
								  std::is_same<T, __int128>::value,
							  std::true_type,
							  std::false_type>::type;

template <class T>
using is_unsigned_int128 =
	typename std::conditional<std::is_same<T, __uint128_t>::value ||
								  std::is_same<T, unsigned __int128>::value,
							  std::true_type,
							  std::false_type>::type;

template <class T>
using make_unsigned_int128 =
	typename std::conditional<std::is_same<T, __int128_t>::value,
							  __uint128_t,
							  unsigned __int128>;

template <class T>
using is_integral = typename std::conditional<std::is_integral<T>::value ||
												  is_signed_int128<T>::value ||
												  is_unsigned_int128<T>::value,
											  std::true_type,
											  std::false_type>::type;

template <class T>
using is_signed_int = typename std::conditional<(is_integral<T>::value &&
												 std::is_signed<T>::value) ||
													is_signed_int128<T>::value,
												std::true_type,
												std::false_type>::type;

template <class T>
using is_unsigned_int =
	typename std::conditional<(is_integral<T>::value &&
							   std::is_unsigned<T>::value) ||
								  is_unsigned_int128<T>::value,
							  std::true_type,
							  std::false_type>::type;

template <class T>
using to_unsigned = typename std::conditional<
	is_signed_int128<T>::value,
	make_unsigned_int128<T>,
	typename std::conditional<std::is_signed<T>::value,
							  std::make_unsigned<T>,
							  std::common_type<T>>::type>::type;

#else

template <class T>
using is_integral = typename std::is_integral<T>;

template <class T>
using is_signed_int =
	typename std::conditional<is_integral<T>::value && std::is_signed<T>::value,
							  std::true_type,
							  std::false_type>::type;

template <class T>
using is_unsigned_int =
	typename std::conditional<is_integral<T>::value &&
								  std::is_unsigned<T>::value,
							  std::true_type,
							  std::false_type>::type;

template <class T>
using to_unsigned = typename std::conditional<is_signed_int<T>::value,
											  std::make_unsigned<T>,
											  std::common_type<T>>::type;

#endif

template <class T>
using is_signed_int_t = std::enable_if_t<is_signed_int<T>::value>;

template <class T>
using is_unsigned_int_t = std::enable_if_t<is_unsigned_int<T>::value>;

template <class T>
using to_unsigned_t = typename to_unsigned<T>::type;

} // namespace internal

// Reference: https://en.wikipedia.org/wiki/Fenwick_tree
template <class T>
struct fenwick_tree {
	using U = internal::to_unsigned_t<T>;

	public:
	fenwick_tree() : _n(0) {}
	fenwick_tree(int n) : _n(n), data(n) {}

	void add(int p, T x) {
		assert(0 <= p && p < _n);
		p++;
		while(p <= _n) {
			data[p - 1] += U(x);
			p += p & -p;
		}
	}

	T sum(int l, int r) {
		assert(0 <= l && l <= r && r <= _n);
		return sum(r) - sum(l);
	}

	private:
	int _n;
	std::vector<U> data;

	U sum(int r) {
		U s = 0;
		while(r > 0) {
			s += data[r - 1];
			r -= r & -r;
		}
		return s;
	}
};

} // namespace atcoder

#endif // ATCODER_FENWICKTREE_HPP

using namespace atcoder;
int main() {
	int n;
	cin >> n;
	VI a(n);
	EACH(e, a) { cin >> e; }

	auto fw = fenwick_tree<ll>(n + 1);
	ll inv_sum = 0;
	for(int i = n - 1; i >= 0; i--) {
		fw.add(a[i], 1);
		inv_sum += fw.sum(0, a[i]);
	}

	REP(i, n) {
		cout << inv_sum << endl;
		inv_sum -= a[i];
		inv_sum += n - a[i] - 1;
	}
	return 0;
}


// Copyright (C) 2018  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.
#include "opaque_types.h"
#include <dlib/python.h>
#include "dlib/pixel.h"
#include <dlib/image_transforms.h>
#include <dlib/image_processing.h>

using namespace dlib;
using namespace std;

namespace py = pybind11;

// ----------------------------------------------------------------------------------------

template <typename T>
numpy_image<T> py_resize_image (
    const numpy_image<T>& img,
    unsigned long rows,
    unsigned long cols
)
{
    numpy_image<T> out;
    set_image_size(out, rows, cols);
    resize_image(img, out);
    return out;
}

// ----------------------------------------------------------------------------------------

template <typename T>
numpy_image<T> py_equalize_histogram (
    const numpy_image<T>& img
)
{
    numpy_image<T> out;
    equalize_histogram(img,out);
    return out;
}

// ----------------------------------------------------------------------------------------

class py_hough_transform
{
public:

    py_hough_transform(
        unsigned long size
    ) : ht(size)
    {
        DLIB_CASSERT(size > 0);
    }

    unsigned long size(
    ) const { return ht.size(); }

    long nr(
    ) const { return ht.nr(); }

    long nc(
    ) const { return ht.nc(); }

    line get_line (
        const point& p
    ) const
    {
        DLIB_CASSERT(rectangle(0,0,size()-1,size()-1).contains(p));
        auto temp = ht.get_line(p);
        return line(temp.first, temp.second);
    }

    double get_line_angle_in_degrees (
        const point& p
    ) const
    {
        DLIB_CASSERT(rectangle(0,0,size()-1,size()-1).contains(p));
        return ht.get_line_angle_in_degrees(p);
    }

    py::tuple get_line_properties (
        const point& p
    ) const
    {
        DLIB_CASSERT(rectangle(0,0,size()-1,size()-1).contains(p));
        double angle_in_degrees;
        double radius;
        ht.get_line_properties(p, angle_in_degrees, radius);
        return py::make_tuple(angle_in_degrees, radius);
    }

    point get_best_hough_point (
        const point& p,
        const numpy_image<float>& himg
    )
    {
        DLIB_ASSERT(himg.nr() == size() && himg.nc() == size() &&
            rectangle(0,0,size()-1,size()-1).contains(p) == true,
            "\t point hough_transform::get_best_hough_point()"
            << "\n\t Invalid arguments given to this function."
            << "\n\t himg.nr(): " << himg.nr()
            << "\n\t himg.nc(): " << himg.nc()
            << "\n\t size():    " << size()
            << "\n\t p:         " << p
        );
        return ht.get_best_hough_point(p,himg);
    }

    template <
        typename T
        >
    numpy_image<float> compute_ht (
        const numpy_image<T>& img,
        const rectangle& box
    ) const
    {
        numpy_image<float> out;
        ht(img, box, out);
        return out;
    }

    template <
        typename T
        >
    numpy_image<float> compute_ht2 (
        const numpy_image<T>& img
    ) const
    {
        numpy_image<float> out;
        ht(img, out);
        return out;
    }

    template <
        typename T
        >
    py::list find_pixels_voting_for_lines (
        const numpy_image<T>& img,
        const rectangle& box,
        const std::vector<point>& hough_points,
        const unsigned long angle_window_size = 1,
        const unsigned long radius_window_size = 1
    ) const
    {
        return vector_to_python_list(ht.find_pixels_voting_for_lines(img, box, hough_points, angle_window_size, radius_window_size));
    }

    template <
        typename T
        >
    py::list find_pixels_voting_for_lines2 (
        const numpy_image<T>& img,
        const std::vector<point>& hough_points,
        const unsigned long angle_window_size = 1,
        const unsigned long radius_window_size = 1
    ) const
    {
        return vector_to_python_list(ht.find_pixels_voting_for_lines(img, hough_points, angle_window_size, radius_window_size));
    }

    std::vector<point> find_strong_hough_points(
        const numpy_image<float>& himg,
        const float hough_count_threshold,
        const double angle_nms_thresh,
        const double radius_nms_thresh
    )
    {
        return ht.find_strong_hough_points(himg, hough_count_threshold, angle_nms_thresh, radius_nms_thresh);
    }

private:
    hough_transform ht;
};

// ----------------------------------------------------------------------------------------

void register_hough_transform(py::module& m)
{
    const char* class_docs =
"This object is a tool for computing the line finding version of the Hough transform \n\
given some kind of edge detection image as input.  It also allows the edge pixels \n\
to be weighted such that higher weighted edge pixels contribute correspondingly \n\
more to the output of the Hough transform, allowing stronger edges to create \n\
correspondingly stronger line detections in the final Hough transform.";

    const char* doc_constr =
"requires \n\
    - size_ > 0 \n\
ensures \n\
    - This object will compute Hough transforms that are size_ by size_ pixels.   \n\
      This is in terms of both the Hough accumulator array size as well as the \n\
      input image size. \n\
    - size() == size_";
        /*!
            requires
                - size_ > 0
            ensures
                - This object will compute Hough transforms that are size_ by size_ pixels.
                  This is in terms of both the Hough accumulator array size as well as the
                  input image size.
                - size() == size_
        !*/

    py::class_<py_hough_transform>(m, "hough_transform", class_docs)
        .def(py::init<unsigned long>(), doc_constr, py::arg("size_"))
        .def("size", &py_hough_transform::size,
            "returns the size of the Hough transforms generated by this object.  In particular, this object creates Hough transform images that are size() by size() pixels in size.")
        .def("get_line", &py_hough_transform::get_line, py::arg("p"),
"requires \n\
    - rectangle(0,0,size()-1,size()-1).contains(p) == true \n\
      (i.e. p must be a point inside the Hough accumulator array) \n\
ensures \n\
    - returns the line segment in the original image space corresponding \n\
      to Hough transform point p.  \n\
    - The returned points are inside rectangle(0,0,size()-1,size()-1).")
    /*!
        requires
            - rectangle(0,0,size()-1,size()-1).contains(p) == true
              (i.e. p must be a point inside the Hough accumulator array)
        ensures
            - returns the line segment in the original image space corresponding
              to Hough transform point p.
            - The returned points are inside rectangle(0,0,size()-1,size()-1).
    !*/

        .def("get_line_angle_in_degrees", &py_hough_transform::get_line_angle_in_degrees, py::arg("p"),
"requires \n\
    - rectangle(0,0,size()-1,size()-1).contains(p) == true \n\
      (i.e. p must be a point inside the Hough accumulator array) \n\
ensures \n\
    - returns the angle, in degrees, of the line corresponding to the Hough \n\
      transform point p.")
    /*!
        requires
            - rectangle(0,0,size()-1,size()-1).contains(p) == true
              (i.e. p must be a point inside the Hough accumulator array)
        ensures
            - returns the angle, in degrees, of the line corresponding to the Hough
              transform point p.
    !*/

        .def("get_line_properties", &py_hough_transform::get_line_properties, py::arg("p"),
"requires \n\
    - rectangle(0,0,size()-1,size()-1).contains(p) == true \n\
      (i.e. p must be a point inside the Hough accumulator array) \n\
ensures \n\
    - Converts a point in the Hough transform space into an angle, in degrees, \n\
      and a radius, measured in pixels from the center of the input image. \n\
    - let ANGLE_IN_DEGREES == the angle of the line corresponding to the Hough \n\
      transform point p.  Moreover: -90 <= ANGLE_IN_DEGREES < 90. \n\
    - RADIUS == the distance from the center of the input image, measured in \n\
      pixels, and the line corresponding to the Hough transform point p. \n\
      Moreover: -sqrt(size()*size()/2) <= RADIUS <= sqrt(size()*size()/2) \n\
    - returns a tuple of (ANGLE_IN_DEGREES, RADIUS)" )
    /*!
        requires
            - rectangle(0,0,size()-1,size()-1).contains(p) == true
              (i.e. p must be a point inside the Hough accumulator array)
        ensures
            - Converts a point in the Hough transform space into an angle, in degrees,
              and a radius, measured in pixels from the center of the input image.
            - let ANGLE_IN_DEGREES == the angle of the line corresponding to the Hough
              transform point p.  Moreover: -90 <= ANGLE_IN_DEGREES < 90.
            - RADIUS == the distance from the center of the input image, measured in
              pixels, and the line corresponding to the Hough transform point p.
              Moreover: -sqrt(size()*size()/2) <= RADIUS <= sqrt(size()*size()/2)
            - returns a tuple of (ANGLE_IN_DEGREES, RADIUS)
    !*/

        .def("get_best_hough_point", &py_hough_transform::get_best_hough_point, py::arg("p"), py::arg("himg"),
"requires \n\
    - himg has size() rows and columns. \n\
    - rectangle(0,0,size()-1,size()-1).contains(p) == true \n\
ensures \n\
    - This function interprets himg as a Hough image and p as a point in the \n\
      original image space.  Given this, it finds the maximum scoring line that \n\
      passes though p.  That is, it checks all the Hough accumulator bins in \n\
      himg corresponding to lines though p and returns the location with the \n\
      largest score.   \n\
    - returns a point X such that get_rect(himg).contains(X) == true")
    /*!
        requires
            - himg has size() rows and columns.
            - rectangle(0,0,size()-1,size()-1).contains(p) == true
        ensures
            - This function interprets himg as a Hough image and p as a point in the
              original image space.  Given this, it finds the maximum scoring line that
              passes though p.  That is, it checks all the Hough accumulator bins in
              himg corresponding to lines though p and returns the location with the
              largest score.
            - returns a point X such that get_rect(himg).contains(X) == true
    !*/

        .def("__call__", &py_hough_transform::compute_ht<uint8_t>, py::arg("img"), py::arg("box"))
        .def("__call__", &py_hough_transform::compute_ht<uint16_t>, py::arg("img"), py::arg("box"))
        .def("__call__", &py_hough_transform::compute_ht<uint32_t>, py::arg("img"), py::arg("box"))
        .def("__call__", &py_hough_transform::compute_ht<uint64_t>, py::arg("img"), py::arg("box"))
        .def("__call__", &py_hough_transform::compute_ht<int8_t>, py::arg("img"), py::arg("box"))
        .def("__call__", &py_hough_transform::compute_ht<int16_t>, py::arg("img"), py::arg("box"))
        .def("__call__", &py_hough_transform::compute_ht<int32_t>, py::arg("img"), py::arg("box"))
        .def("__call__", &py_hough_transform::compute_ht<int64_t>, py::arg("img"), py::arg("box"))
        .def("__call__", &py_hough_transform::compute_ht<float>, py::arg("img"), py::arg("box"))
        .def("__call__", &py_hough_transform::compute_ht<double>, py::arg("img"), py::arg("box"),
"requires \n\
    - box.width() == size() \n\
    - box.height() == size() \n\
ensures \n\
    - Computes the Hough transform of the part of img contained within box. \n\
      In particular, we do a grayscale version of the Hough transform where any \n\
      non-zero pixel in img is treated as a potential component of a line and \n\
      accumulated into the returned Hough accumulator image.  However, rather than \n\
      adding 1 to each relevant accumulator bin we add the value of the pixel \n\
      in img to each Hough accumulator bin.  This means that, if all the \n\
      pixels in img are 0 or 1 then this routine performs a normal Hough \n\
      transform.  However, if some pixels have larger values then they will be \n\
      weighted correspondingly more in the resulting Hough transform. \n\
    - The returned hough transform image will be size() rows by size() columns. \n\
    - The returned image is the Hough transform of the part of img contained in \n\
      box.  Each point in the Hough image corresponds to a line in the input box. \n\
      In particular, the line for hough_image[y][x] is given by get_line(point(x,y)).  \n\
      Also, when viewing the Hough image, the x-axis gives the angle of the line \n\
      and the y-axis the distance of the line from the center of the box.  The \n\
      conversion between Hough coordinates and angle and pixel distance can be \n\
      obtained by calling get_line_properties()." )
    /*!
        requires
            - box.width() == size()
            - box.height() == size()
        ensures
            - Computes the Hough transform of the part of img contained within box.
              In particular, we do a grayscale version of the Hough transform where any
              non-zero pixel in img is treated as a potential component of a line and
              accumulated into the returned Hough accumulator image.  However, rather than
              adding 1 to each relevant accumulator bin we add the value of the pixel
              in img to each Hough accumulator bin.  This means that, if all the
              pixels in img are 0 or 1 then this routine performs a normal Hough
              transform.  However, if some pixels have larger values then they will be
              weighted correspondingly more in the resulting Hough transform.
            - The returned hough transform image will be size() rows by size() columns.
            - The returned image is the Hough transform of the part of img contained in
              box.  Each point in the Hough image corresponds to a line in the input box.
              In particular, the line for hough_image[y][x] is given by get_line(point(x,y)).
              Also, when viewing the Hough image, the x-axis gives the angle of the line
              and the y-axis the distance of the line from the center of the box.  The
              conversion between Hough coordinates and angle and pixel distance can be
              obtained by calling get_line_properties().
    !*/

        .def("__call__", &py_hough_transform::compute_ht2<uint8_t>, py::arg("img"))
        .def("__call__", &py_hough_transform::compute_ht2<uint16_t>, py::arg("img"))
        .def("__call__", &py_hough_transform::compute_ht2<uint32_t>, py::arg("img"))
        .def("__call__", &py_hough_transform::compute_ht2<uint64_t>, py::arg("img"))
        .def("__call__", &py_hough_transform::compute_ht2<int8_t>, py::arg("img"))
        .def("__call__", &py_hough_transform::compute_ht2<int16_t>, py::arg("img"))
        .def("__call__", &py_hough_transform::compute_ht2<int32_t>, py::arg("img"))
        .def("__call__", &py_hough_transform::compute_ht2<int64_t>, py::arg("img"))
        .def("__call__", &py_hough_transform::compute_ht2<float>, py::arg("img"))
        .def("__call__", &py_hough_transform::compute_ht2<double>, py::arg("img"),
            "    simply performs: return self(img, get_rect(img)).  That is, just runs the hough transform on the whole input image.")

        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines<uint8_t>, py::arg("img"), py::arg("box"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines<uint16_t>, py::arg("img"), py::arg("box"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines<uint32_t>, py::arg("img"), py::arg("box"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines<uint64_t>, py::arg("img"), py::arg("box"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines<int8_t>, py::arg("img"), py::arg("box"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines<int16_t>, py::arg("img"), py::arg("box"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines<int32_t>, py::arg("img"), py::arg("box"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines<int64_t>, py::arg("img"), py::arg("box"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines<float>, py::arg("img"), py::arg("box"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines<double>, py::arg("img"), py::arg("box"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1,
"requires \n\
    - box.width() == size() \n\
    - box.height() == size() \n\
    - for all valid i: \n\
        - rectangle(0,0,size()-1,size()-1).contains(hough_points[i]) == true \n\
          (i.e. hough_points must contain points in the output Hough transform \n\
          space generated by this object.) \n\
    - angle_window_size >= 1 \n\
    - radius_window_size >= 1 \n\
ensures \n\
    - This function computes the Hough transform of the part of img contained \n\
      within box.  It does the same computation as __call__() defined above, \n\
      except instead of accumulating into an image we create an explicit list \n\
      of all the points in img that contributed to each line (i.e each point in \n\
      the Hough image). To do this we take a list of Hough points as input and \n\
      only record hits on these specifically identified Hough points.  A \n\
      typical use of find_pixels_voting_for_lines() is to first run the normal \n\
      Hough transform using __call__(), then find the lines you are interested \n\
      in, and then call find_pixels_voting_for_lines() to determine which \n\
      pixels in the input image belong to those lines. \n\
    - This routine returns a vector, CONSTITUENT_POINTS, with the following \n\
      properties: \n\
        - CONSTITUENT_POINTS.size() == hough_points.size() \n\
        - for all valid i: \n\
            - Let HP[i] = centered_rect(hough_points[i], angle_window_size, radius_window_size) \n\
            - Any point in img with a non-zero value that lies on a line \n\
              corresponding to one of the Hough points in HP[i] is added to \n\
              CONSTITUENT_POINTS[i].  Therefore, when this routine finishes, \n\
              #CONSTITUENT_POINTS[i] will contain all the points in img that \n\
              voted for the lines associated with the Hough accumulator bins in \n\
              HP[i]. \n\
            - #CONSTITUENT_POINTS[i].size() == the number of points in img that \n\
              voted for any of the lines HP[i] in Hough space.  Note, however, \n\
              that if angle_window_size or radius_window_size are made so large \n\
              that HP[i] overlaps HP[j] for i!=j then the overlapping regions \n\
              of Hough space are assign to HP[i] or HP[j] arbitrarily. \n\
              Therefore, all points in CONSTITUENT_POINTS are unique, that is, \n\
              there is no overlap in points between any two elements of \n\
              CONSTITUENT_POINTS." )
    /*!
        requires
            - box.width() == size()
            - box.height() == size()
            - for all valid i:
                - rectangle(0,0,size()-1,size()-1).contains(hough_points[i]) == true
                  (i.e. hough_points must contain points in the output Hough transform
                  space generated by this object.)
            - angle_window_size >= 1
            - radius_window_size >= 1
        ensures
            - This function computes the Hough transform of the part of img contained
              within box.  It does the same computation as __call__() defined above,
              except instead of accumulating into an image we create an explicit list
              of all the points in img that contributed to each line (i.e each point in
              the Hough image). To do this we take a list of Hough points as input and
              only record hits on these specifically identified Hough points.  A
              typical use of find_pixels_voting_for_lines() is to first run the normal
              Hough transform using __call__(), then find the lines you are interested
              in, and then call find_pixels_voting_for_lines() to determine which
              pixels in the input image belong to those lines.
            - This routine returns a vector, CONSTITUENT_POINTS, with the following
              properties:
                - CONSTITUENT_POINTS.size() == hough_points.size()
                - for all valid i:
                    - Let HP[i] = centered_rect(hough_points[i], angle_window_size, radius_window_size)
                    - Any point in img with a non-zero value that lies on a line
                      corresponding to one of the Hough points in HP[i] is added to
                      CONSTITUENT_POINTS[i].  Therefore, when this routine finishes,
                      #CONSTITUENT_POINTS[i] will contain all the points in img that
                      voted for the lines associated with the Hough accumulator bins in
                      HP[i].
                    - #CONSTITUENT_POINTS[i].size() == the number of points in img that
                      voted for any of the lines HP[i] in Hough space.  Note, however,
                      that if angle_window_size or radius_window_size are made so large
                      that HP[i] overlaps HP[j] for i!=j then the overlapping regions
                      of Hough space are assign to HP[i] or HP[j] arbitrarily.
                      Therefore, all points in CONSTITUENT_POINTS are unique, that is,
                      there is no overlap in points between any two elements of
                      CONSTITUENT_POINTS.
    !*/
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines2<uint8_t>, py::arg("img"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines2<uint16_t>, py::arg("img"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines2<uint32_t>, py::arg("img"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines2<uint64_t>, py::arg("img"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines2<int8_t>, py::arg("img"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines2<int16_t>, py::arg("img"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines2<int32_t>, py::arg("img"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines2<int64_t>, py::arg("img"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines2<float>, py::arg("img"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1)
        .def("find_pixels_voting_for_lines", &py_hough_transform::find_pixels_voting_for_lines2<double>, py::arg("img"), py::arg("hough_points"), py::arg("angle_window_size")=1, py::arg("radius_window_size")=1,
"    performs: return find_pixels_voting_for_lines(img, get_rect(img), hough_points, angle_window_size, radius_window_size); \n\
That is, just runs the routine on the whole input image." )

        .def("find_strong_hough_points", &py_hough_transform::find_strong_hough_points, py::arg("himg"), py::arg("hough_count_threshold"), py::arg("angle_nms_thresh"), py::arg("radius_nms_thresh"),
"requires \n\
    - himg has size() rows and columns. \n\
    - angle_nms_thresh >= 0 \n\
    - radius_nms_thresh >= 0 \n\
ensures \n\
    - This routine finds strong lines in a Hough transform and performs \n\
      non-maximum suppression on the detected lines.  Recall that each point in \n\
      Hough space is associated with a line. Therefore, this routine finds all \n\
      the pixels in himg (a Hough transform image) with values >= \n\
      hough_count_threshold and performs non-maximum suppression on the \n\
      identified list of pixels.  It does this by discarding lines that are \n\
      within angle_nms_thresh degrees of a stronger line or within \n\
      radius_nms_thresh distance (in terms of radius as defined by \n\
      get_line_properties()) to a stronger Hough point. \n\
    - The identified lines are returned as a list of coordinates in himg." );
    /*!
        requires
            - himg has size() rows and columns.
            - angle_nms_thresh >= 0
            - radius_nms_thresh >= 0
        ensures
            - This routine finds strong lines in a Hough transform and performs
              non-maximum suppression on the detected lines.  Recall that each point in
              Hough space is associated with a line. Therefore, this routine finds all
              the pixels in himg (a Hough transform image) with values >=
              hough_count_threshold and performs non-maximum suppression on the
              identified list of pixels.  It does this by discarding lines that are
              within angle_nms_thresh degrees of a stronger line or within
              radius_nms_thresh distance (in terms of radius as defined by
              get_line_properties()) to a stronger Hough point.
            - The identified lines are returned as a list of coordinates in himg.
    !*/

}

// ----------------------------------------------------------------------------------------

std::vector<point> py_remove_incoherent_edge_pixels (
    const std::vector<point>& line,
    const numpy_image<float>& horz_gradient,
    const numpy_image<float>& vert_gradient,
    double angle_threshold
)
{

    DLIB_CASSERT(num_rows(horz_gradient) == num_rows(vert_gradient));
    DLIB_CASSERT(num_columns(horz_gradient) == num_columns(vert_gradient));
    DLIB_CASSERT(angle_threshold >= 0);
    for (auto& p : line)
        DLIB_CASSERT(get_rect(horz_gradient).contains(p), "All line points must be inside the given images.");

    return remove_incoherent_edge_pixels(line, horz_gradient, vert_gradient, angle_threshold);
}

// ----------------------------------------------------------------------------------------

template <typename T>
numpy_image<T> py_transform_image (
    const numpy_image<T>& img,
    const point_transform_projective& map_point,
    long rows,
    long columns
)
{
    DLIB_CASSERT(rows > 0 && columns > 0, "The requested output image dimensions are invalid.");
    numpy_image<T> out_;
    image_view<numpy_image<T>> out(out_);
    out.set_size(rows, columns);

    transform_image(img, out_, interpolate_bilinear(), map_point);

    return out_;
}
// ----------------------------------------------------------------------------------------

template <typename T>
numpy_image<T> py_extract_image_4points (
    const numpy_image<T>& img,
    const py::list& corners,
    long rows,
    long columns
)
{
    DLIB_CASSERT(rows >= 0);
    DLIB_CASSERT(columns >= 0);
    DLIB_CASSERT(len(corners) == 4);

    numpy_image<T> out;
    set_image_size(out, rows, columns);
    try
    {
        extract_image_4points(img, out, python_list_to_array<dpoint,4>(corners));
        return out;
    }
    catch (py::cast_error&){}

    try
    {
        extract_image_4points(img, out, python_list_to_array<line,4>(corners));
        return out;
    }
    catch(py::cast_error&)
    {
        throw dlib::error("extract_image_4points() requires the corners argument to be a list of 4 dpoints or 4 lines.");
    }
}

// ----------------------------------------------------------------------------------------

template <typename T>
numpy_image<T> py_mbd (
    const numpy_image<T>& img,
    size_t iterations,
    bool do_left_right_scans
)
{
    numpy_image<T> out;
    min_barrier_distance(img, out, iterations, do_left_right_scans);
    return out;
}

numpy_image<unsigned char> py_mbd2 (
    const numpy_image<rgb_pixel>& img,
    size_t iterations,
    bool do_left_right_scans
)
{
    numpy_image<unsigned char> out;
    min_barrier_distance(img, out, iterations, do_left_right_scans);
    return out;
}

// ----------------------------------------------------------------------------------------

void bind_image_classes2(py::module& m)
{

    const char* docs = "Resizes img, using bilinear interpolation, to have the indicated number of rows and columns.";

    m.def("resize_image", &py_resize_image<uint8_t>, py::arg("img"), py::arg("rows"), py::arg("cols"));
    m.def("resize_image", &py_resize_image<uint16_t>, py::arg("img"), py::arg("rows"), py::arg("cols"));
    m.def("resize_image", &py_resize_image<uint32_t>, py::arg("img"), py::arg("rows"), py::arg("cols"));
    m.def("resize_image", &py_resize_image<uint64_t>, py::arg("img"), py::arg("rows"), py::arg("cols"));
    m.def("resize_image", &py_resize_image<int8_t>, py::arg("img"), py::arg("rows"), py::arg("cols"));
    m.def("resize_image", &py_resize_image<int16_t>, py::arg("img"), py::arg("rows"), py::arg("cols"));
    m.def("resize_image", &py_resize_image<int32_t>, py::arg("img"), py::arg("rows"), py::arg("cols"));
    m.def("resize_image", &py_resize_image<int64_t>, py::arg("img"), py::arg("rows"), py::arg("cols"));
    m.def("resize_image", &py_resize_image<float>, py::arg("img"), py::arg("rows"), py::arg("cols"));
    m.def("resize_image", &py_resize_image<double>, docs, py::arg("img"), py::arg("rows"), py::arg("cols"));
    m.def("resize_image", &py_resize_image<rgb_pixel>, docs, py::arg("img"), py::arg("rows"), py::arg("cols"));

    docs = "Returns a histogram equalized version of img.";
    m.def("equalize_histogram", &py_equalize_histogram<uint8_t>, py::arg("img"));
    m.def("equalize_histogram", &py_equalize_histogram<uint16_t>, docs, py::arg("img"));

    m.def("min_barrier_distance", &py_mbd<uint8_t>, py::arg("img"), py::arg("iterations")=10, py::arg("do_left_right_scans")=true);
    m.def("min_barrier_distance", &py_mbd<uint16_t>, py::arg("img"), py::arg("iterations")=10, py::arg("do_left_right_scans")=true);
    m.def("min_barrier_distance", &py_mbd<uint32_t>, py::arg("img"), py::arg("iterations")=10, py::arg("do_left_right_scans")=true);
    m.def("min_barrier_distance", &py_mbd<uint64_t>, py::arg("img"), py::arg("iterations")=10, py::arg("do_left_right_scans")=true);
    m.def("min_barrier_distance", &py_mbd<int8_t>, py::arg("img"), py::arg("iterations")=10, py::arg("do_left_right_scans")=true);
    m.def("min_barrier_distance", &py_mbd<int16_t>, py::arg("img"), py::arg("iterations")=10, py::arg("do_left_right_scans")=true);
    m.def("min_barrier_distance", &py_mbd<int32_t>, py::arg("img"), py::arg("iterations")=10, py::arg("do_left_right_scans")=true);
    m.def("min_barrier_distance", &py_mbd<int64_t>, py::arg("img"), py::arg("iterations")=10, py::arg("do_left_right_scans")=true);
    m.def("min_barrier_distance", &py_mbd<float>, py::arg("img"), py::arg("iterations")=10, py::arg("do_left_right_scans")=true);
    m.def("min_barrier_distance", &py_mbd<double>, py::arg("img"), py::arg("iterations")=10, py::arg("do_left_right_scans")=true);
    m.def("min_barrier_distance", &py_mbd2, py::arg("img"), py::arg("iterations")=10, py::arg("do_left_right_scans")=true,
"requires \n\
    - iterations > 0 \n\
ensures \n\
    - This function implements the salient object detection method described in the paper: \n\
        \"Minimum barrier salient object detection at 80 fps\" by Zhang, Jianming, et al.  \n\
      In particular, we compute the minimum barrier distance between the borders of \n\
      the image and all the other pixels.  The resulting image is returned.  Note that \n\
      the paper talks about a bunch of other things you could do beyond computing \n\
      the minimum barrier distance, but this function doesn't do any of that. It's \n\
      just the vanilla MBD. \n\
    - We will perform iterations iterations of MBD passes over the image.  Larger \n\
      values might give better results but run slower. \n\
    - During each MBD iteration we make raster scans over the image.  These pass \n\
      from top->bottom, bottom->top, left->right, and right->left.  If \n\
      do_left_right_scans==false then the left/right passes are not executed. \n\
      Skipping them makes the algorithm about 2x faster but might reduce the \n\
      quality of the output."
    /*!
        requires
            - iterations > 0
        ensures
            - This function implements the salient object detection method described in the paper:
                "Minimum barrier salient object detection at 80 fps" by Zhang, Jianming, et al.
              In particular, we compute the minimum barrier distance between the borders of
              the image and all the other pixels.  The resulting image is returned.  Note that
              the paper talks about a bunch of other things you could do beyond computing
              the minimum barrier distance, but this function doesn't do any of that. It's
              just the vanilla MBD.
            - We will perform iterations iterations of MBD passes over the image.  Larger
              values might give better results but run slower.
            - During each MBD iteration we make raster scans over the image.  These pass
              from top->bottom, bottom->top, left->right, and right->left.  If
              do_left_right_scans==false then the left/right passes are not executed.
              Skipping them makes the algorithm about 2x faster but might reduce the
              quality of the output.
    !*/
    );

    register_hough_transform(m);

    m.def("normalize_image_gradients", normalize_image_gradients<numpy_image<double>>, py::arg("img1"), py::arg("img2"));
    m.def("normalize_image_gradients", normalize_image_gradients<numpy_image<float>>, py::arg("img1"), py::arg("img2"),
"requires \n\
    - img1 and img2 have the same dimensions. \n\
ensures \n\
    - This function assumes img1 and img2 are the two gradient images produced by a \n\
      function like sobel_edge_detector().  It then unit normalizes the gradient \n\
      vectors. That is, for all valid r and c, this function ensures that: \n\
        - img1[r][c]*img1[r][c] + img2[r][c]*img2[r][c] == 1  \n\
          unless both img1[r][c] and img2[r][c] were 0 initially, then they stay zero.");
    /*!
        requires
            - img1 and img2 have the same dimensions.
        ensures
            - This function assumes img1 and img2 are the two gradient images produced by a
              function like sobel_edge_detector().  It then unit normalizes the gradient
              vectors. That is, for all valid r and c, this function ensures that:
                - img1[r][c]*img1[r][c] + img2[r][c]*img2[r][c] == 1
                  unless both img1[r][c] and img2[r][c] were 0 initially, then they stay zero.
    !*/

    m.def("remove_incoherent_edge_pixels", &py_remove_incoherent_edge_pixels, py::arg("line"), py::arg("horz_gradient"),
        py::arg("vert_gradient"), py::arg("angle_thresh"),
"requires \n\
    - horz_gradient and vert_gradient have the same dimensions. \n\
    - horz_gradient and vert_gradient represent unit normalized vectors.  That is, \n\
      you should have called normalize_image_gradients(horz_gradient,vert_gradient) \n\
      or otherwise caused all the gradients to have unit norm. \n\
    - for all valid i: \n\
        get_rect(horz_gradient).contains(line[i]) \n\
ensures \n\
    - This routine looks at all the points in the given line and discards the ones that \n\
      have outlying gradient directions.  To be specific, this routine returns a set \n\
      of points PTS such that:  \n\
        - for all valid i,j: \n\
            - The difference in angle between the gradients for PTS[i] and PTS[j] is  \n\
              less than angle_threshold degrees.   \n\
        - len(PTS) <= len(line) \n\
        - PTS is just line with some elements removed." );
    /*!
        requires
            - horz_gradient and vert_gradient have the same dimensions.
            - horz_gradient and vert_gradient represent unit normalized vectors.  That is,
              you should have called normalize_image_gradients(horz_gradient,vert_gradient)
              or otherwise caused all the gradients to have unit norm.
            - for all valid i:
                get_rect(horz_gradient).contains(line[i])
        ensures
            - This routine looks at all the points in the given line and discards the ones that
              have outlying gradient directions.  To be specific, this routine returns a set
              of points PTS such that:
                - for all valid i,j:
                    - The difference in angle between the gradients for PTS[i] and PTS[j] is
                      less than angle_threshold degrees.
                - len(PTS) <= len(line)
                - PTS is just line with some elements removed.
    !*/

    py::register_exception<no_convex_quadrilateral>(m, "no_convex_quadrilateral");

    m.def("extract_image_4points", &py_extract_image_4points<uint8_t>, py::arg("img"), py::arg("corners"), py::arg("rows"), py::arg("columns"));
    m.def("extract_image_4points", &py_extract_image_4points<uint16_t>, py::arg("img"), py::arg("corners"), py::arg("rows"), py::arg("columns"));
    m.def("extract_image_4points", &py_extract_image_4points<uint32_t>, py::arg("img"), py::arg("corners"), py::arg("rows"), py::arg("columns"));
    m.def("extract_image_4points", &py_extract_image_4points<uint64_t>, py::arg("img"), py::arg("corners"), py::arg("rows"), py::arg("columns"));
    m.def("extract_image_4points", &py_extract_image_4points<int8_t>, py::arg("img"), py::arg("corners"), py::arg("rows"), py::arg("columns"));
    m.def("extract_image_4points", &py_extract_image_4points<int16_t>, py::arg("img"), py::arg("corners"), py::arg("rows"), py::arg("columns"));
    m.def("extract_image_4points", &py_extract_image_4points<int32_t>, py::arg("img"), py::arg("corners"), py::arg("rows"), py::arg("columns"));
    m.def("extract_image_4points", &py_extract_image_4points<int64_t>, py::arg("img"), py::arg("corners"), py::arg("rows"), py::arg("columns"));
    m.def("extract_image_4points", &py_extract_image_4points<float>, py::arg("img"), py::arg("corners"), py::arg("rows"), py::arg("columns"));
    m.def("extract_image_4points", &py_extract_image_4points<double>, py::arg("img"), py::arg("corners"), py::arg("rows"), py::arg("columns"));
    m.def("extract_image_4points", &py_extract_image_4points<rgb_pixel>, py::arg("img"), py::arg("corners"), py::arg("rows"), py::arg("columns"),
"requires \n\
    - corners is a list of dpoint or line objects. \n\
    - len(corners) == 4 \n\
    - rows >= 0 \n\
    - columns >= 0 \n\
ensures \n\
    - The returned image has the given number of rows and columns. \n\
    - if (corners contains dpoints) then \n\
        - The 4 points in corners define a convex quadrilateral and this function \n\
          extracts that part of the input image img and returns it.  Therefore, \n\
          each corner of the quadrilateral is associated to a corner of the \n\
          extracted image and bilinear interpolation and a projective mapping is \n\
          used to transform the pixels in the quadrilateral into the output image. \n\
          To determine which corners of the quadrilateral map to which corners of \n\
          the returned image we fit the tightest possible rectangle to the \n\
          quadrilateral and map its vertices to their nearest rectangle corners. \n\
          These corners are then trivially mapped to the output image (i.e.  upper \n\
          left corner to upper left corner, upper right corner to upper right \n\
          corner, etc.). \n\
    - else \n\
        - This routine finds the 4 intersecting points of the given lines which \n\
          form a convex quadrilateral and uses them as described above to extract \n\
          an image.   i.e. It just then calls: extract_image_4points(img, \n\
          intersections_between_lines, rows, columns). \n\
        - If no convex quadrilateral can be made from the given lines then this \n\
          routine throws no_convex_quadrilateral."
    /*!
        requires
            - corners is a list of dpoint or line objects.
            - len(corners) == 4
            - rows >= 0
            - columns >= 0
        ensures
            - The returned image has the given number of rows and columns.
            - if (corners contains dpoints) then
                - The 4 points in corners define a convex quadrilateral and this function
                  extracts that part of the input image img and returns it.  Therefore,
                  each corner of the quadrilateral is associated to a corner of the
                  extracted image and bilinear interpolation and a projective mapping is
                  used to transform the pixels in the quadrilateral into the output image.
                  To determine which corners of the quadrilateral map to which corners of
                  the returned image we fit the tightest possible rectangle to the
                  quadrilateral and map its vertices to their nearest rectangle corners.
                  These corners are then trivially mapped to the output image (i.e.  upper
                  left corner to upper left corner, upper right corner to upper right
                  corner, etc.).
            - else
                - This routine finds the 4 intersecting points of the given lines which
                  form a convex quadrilateral and uses them as described above to extract
                  an image.   i.e. It just then calls: extract_image_4points(img,
                  intersections_between_lines, rows, columns).
                - If no convex quadrilateral can be made from the given lines then this
                  routine throws no_convex_quadrilateral.
    !*/
          );

    m.def("transform_image", &py_transform_image<uint8_t>, py::arg("img"), py::arg("map_point"), py::arg("rows"), py::arg("columns"));
    m.def("transform_image", &py_transform_image<uint16_t>, py::arg("img"), py::arg("map_point"), py::arg("rows"), py::arg("columns"));
    m.def("transform_image", &py_transform_image<uint32_t>, py::arg("img"), py::arg("map_point"), py::arg("rows"), py::arg("columns"));
    m.def("transform_image", &py_transform_image<uint64_t>, py::arg("img"), py::arg("map_point"), py::arg("rows"), py::arg("columns"));
    m.def("transform_image", &py_transform_image<int8_t>, py::arg("img"), py::arg("map_point"), py::arg("rows"), py::arg("columns"));
    m.def("transform_image", &py_transform_image<int16_t>, py::arg("img"), py::arg("map_point"), py::arg("rows"), py::arg("columns"));
    m.def("transform_image", &py_transform_image<int32_t>, py::arg("img"), py::arg("map_point"), py::arg("rows"), py::arg("columns"));
    m.def("transform_image", &py_transform_image<int64_t>, py::arg("img"), py::arg("map_point"), py::arg("rows"), py::arg("columns"));
    m.def("transform_image", &py_transform_image<float>, py::arg("img"), py::arg("map_point"), py::arg("rows"), py::arg("columns"));
    m.def("transform_image", &py_transform_image<double>, py::arg("img"), py::arg("map_point"), py::arg("rows"), py::arg("columns"));
    m.def("transform_image", &py_transform_image<rgb_pixel>, py::arg("img"), py::arg("map_point"), py::arg("rows"), py::arg("columns"),
"requires \n\
    - rows > 0 \n\
    - columns > 0 \n\
ensures \n\
    - Returns an image that is the given rows by columns in size and contains a \n\
      transformed part of img.  To do this, we interpret map_point as a mapping \n\
      from pixels in the returned image to pixels in the input img.  transform_image()  \n\
      uses this mapping and bilinear interpolation to fill the output image with an \n\
      interpolated copy of img.   \n\
    - Any locations in the output image that map to pixels outside img are set to 0."
    /*!
        requires
            - rows > 0
            - columns > 0
        ensures
            - Returns an image that is the given rows by columns in size and contains a
              transformed part of img.  To do this, we interpret map_point as a mapping
              from pixels in the returned image to pixels in the input img.  transform_image()
              uses this mapping and bilinear interpolation to fill the output image with an
              interpolated copy of img.
            - Any locations in the output image that map to pixels outside img are set to 0.
    !*/
        );

}


// json parser
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
namespace pt = boost::property_tree;

#include "PermittivityDrude.h"

PermittivityDrude::PermittivityDrude(double omega_p, double gamma)
    : omega_p(omega_p), gamma(gamma) {}

// constructor for drude model from .json file
PermittivityDrude::PermittivityDrude(const std::string &input_file) {

  // Create a root
  pt::ptree root;

  // Load the json file in this ptree
  pt::read_json(input_file, root);

  // check if type is right
  std::string type = root.get<std::string>("Permittivity.type");
  assert(type == "drude");

  // read parameters
  this->gamma = root.get<double>("Permittivity.gamma");
  this->omega_p = root.get<double>("Permittivity.omega_p");
}

// calculate the permittivity
std::complex<double> PermittivityDrude::calculate(double omega) const {
  // dummies for result and complex unit
  std::complex<double> result;
  std::complex<double> I(0.0, 1.0);

  // calculate the result
  result = 1.0 - omega_p * omega_p / (omega * (omega + I * gamma));

  return result;
}

// calculate the permittivity scaled by omega
std::complex<double>
PermittivityDrude::calculate_times_omega(double omega) const {
  // dummies for result and complex unit
  std::complex<double> result;
  std::complex<double> I(0.0, 1.0);

  // calculate the result
  result = omega - omega_p * omega_p / (omega + I * gamma);

  return result;
}

void PermittivityDrude::print_info(std::ostream &stream) const {
  stream << "# PermittivityDrude\n#\n"
         << "# omega_p = " << omega_p << "\n"
         << "# gamma = " << gamma << "\n";
}