#include <algorithm>
#include <numeric>
#include <functional>
#include <limits>
#include <iostream>
#include <cmath>
#include <cassert>

#include <boost/lambda/lambda.hpp>

#include "BeliefPropagation.h"
#include "LogSumExp.h"

using namespace boost::lambda;

namespace Grante {

BeliefPropagation::BeliefPropagation(const FactorGraph* fg,
	MessageSchedule sched)
	: InferenceMethod(fg), verbose(false), max_iter(100), conv_tol(1.0e-5),
		sched(sched), min_sum(false),
		log_z(std::numeric_limits<double>::quiet_NaN())
{
	// Setup message indices
	const std::vector<Factor*>& factors = fg->Factors();
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		const std::vector<unsigned int>& fac_vars = factors[fi]->Variables();
		for (unsigned int fvi = 0; fvi < fac_vars.size(); ++fvi) {
			// 1. Message from variable to factor
			unsigned int msg_id =
				static_cast<unsigned int>(msg_for_factor_srcvar.size());
			msglist_for_factor[fi].push_back(msg_id);
			msg_for_factor_srcvar.push_back(fac_vars[fvi]);

			// 2. Message from factor to variable
			msg_id = static_cast<unsigned int>(msg_for_var_srcfactor.size());
			msglist_for_var[fac_vars[fvi]].push_back(msg_id);
			msg_for_var_srcfactor.push_back(fi);
		}
	}

	// If using a sequential schedule, obtain a message order and id's
	if (sched == Sequential) {
		FactorGraphStructurizer::ComputeEulerianMessageTrail(fg, order);
		order_msgid.resize(order.size());

		for (size_t oi = 0; oi < order.size(); ++oi) {
			unsigned int vi = order[oi].VariableNode();
			unsigned int fi = order[oi].FactorNode();
			if (order[oi].steptype == FactorGraphStructurizer::LeafIsFactorNode) {
				// message: fi -> vi
				msg_list_t::const_iterator mli = msglist_for_var.find(vi);
				assert(mli != msglist_for_var.end());
				const std::vector<unsigned int>& msg_list = mli->second;
				for (unsigned int mi = 0; mi < msg_list.size(); ++mi) {
					if (msg_for_var_srcfactor[msg_list[mi]] != fi)
						continue;

					order_msgid[oi] = msg_list[mi];
					break;
				}
			} else {
				// message: vi -> fi
				msg_list_t::const_iterator mli = msglist_for_factor.find(fi);
				assert(mli != msglist_for_factor.end());
				const std::vector<unsigned int>& msg_list = mli->second;
				for (unsigned int mi = 0; mi < msg_list.size(); ++mi) {
					if (msg_for_factor_srcvar[msg_list[mi]] != vi)
						continue;

					order_msgid[oi] = msg_list[mi];
					break;
				}
			}
		}
	}
}

BeliefPropagation::~BeliefPropagation() {
}

InferenceMethod* BeliefPropagation::Produce(const FactorGraph* fg) const {
	return (new BeliefPropagation(fg));
}

void BeliefPropagation::SetParameters(bool verbose,
	unsigned int max_iter, double conv_tol) {
	this->verbose = verbose;
	this->max_iter = max_iter;
	assert(conv_tol >= 0.0);
	this->conv_tol = conv_tol;
}

void BeliefPropagation::PerformInference() {
	InferenceInitialize();

	// Min-sum variables
	double best_energy = std::numeric_limits<double>::infinity();

	// Perform message passing
	double conv_measure = std::numeric_limits<double>::infinity();
	for (unsigned int iter = 1; (max_iter == 0 || iter <= max_iter) &&
		conv_measure >= conv_tol; ++iter)
	{
		if (verbose) {
			std::cout << "iter " << iter << ", conv " << conv_measure;
			if (min_sum)
				std::cout << ", E* " << best_energy;
			std::cout << std::endl;
		}

		if (sched == ParallelSync) {
			PerformInferenceStepParallel();
		} else if (sched == Sequential) {
			PerformInferenceSequential();
		} else {
			assert(0);
		}

		// Convergence measure: maximum update to marginals
		// TODO
		if (min_sum) {
			conv_measure = ComputeVariableBeliefs();
			std::vector<unsigned int> cur_state(fg->Cardinalities().size());
			double cur_energy = ReconstructMinimumEnergyState(cur_state);
			if (cur_energy < best_energy) {
				best_energy = cur_energy;
				best_state = cur_state;
			}
		} else {
			conv_measure = ConstructMarginals();
		}
	}
	ConstructMarginals();
	ComputeVariableBeliefs();
	if (min_sum) {
		if (verbose) {
			std::cout << "Converged, tol " << conv_measure << ", E* "
				<< best_energy << std::endl;
		}
	} else {
		log_z = -ComputeBetheFreeEnergy();
		if (verbose) {
			std::cout << "Converged, tol " << conv_measure
				<< ", log_z(Bethe) " << log_z << std::endl;
		}
	}

	InferenceTeardown();
}

void BeliefPropagation::InferenceInitialize() {
	// Initialize messages
	const std::vector<unsigned int>& card = fg->Cardinalities();
	//  i) factor-to-variable
	msg_for_var.resize(msg_for_var_srcfactor.size());
	for (unsigned int vi = 0; vi < card.size(); ++vi) {
		msg_list_t::const_iterator mli = msglist_for_var.find(vi);
		assert(mli != msglist_for_var.end());
		const std::vector<unsigned int>& ml = mli->second;

		for (unsigned int mli = 0; mli < ml.size(); ++mli) {
			assert(ml[mli] < msg_for_var.size());

			// Resize and fill (value does not matter for parallel schedule,
			// but does for sequential)
			msg_for_var[ml[mli]].resize(card[vi]);
			std::fill(msg_for_var[ml[mli]].begin(),
				msg_for_var[ml[mli]].end(), 0.0);
		}
	}
	// ii) variable-to-factor
	msg_for_factor.resize(msg_for_factor_srcvar.size());
	for (unsigned int mfi = 0; mfi < msg_for_factor_srcvar.size(); ++mfi) {
		unsigned int vi = msg_for_factor_srcvar[mfi];
		assert(vi < card.size());
		// Resize and fill with log(1) = 0.
		msg_for_factor[mfi].resize(card[vi]);
		std::fill(msg_for_factor[mfi].begin(), msg_for_factor[mfi].end(), 0.0);
	}

	// Initialize marginals (beliefs)
	const std::vector<Factor*>& factors = fg->Factors();
	marginals.resize(factors.size());
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		marginals[fi].resize(factors[fi]->Type()->ProdCardinalities());
		std::fill(marginals[fi].begin(), marginals[fi].end(), 0.0);
	}
}

void BeliefPropagation::InferenceTeardown() {
	// Clear messages
	msg_for_var.clear();
	msg_for_factor.clear();
}

void BeliefPropagation::ClearInferenceResult() {
	marginals.clear();
	var_beliefs.clear();
}

const std::vector<double>& BeliefPropagation::Marginal(
	unsigned int factor_id) const {
	assert(factor_id < marginals.size());
	return (marginals[factor_id]);
}

const std::vector<std::vector<double> >& BeliefPropagation::Marginals() const
{
	return (marginals);
}

double BeliefPropagation::LogPartitionFunction() const {
	return (log_z);
}

// NOT IMPLEMENTED
void BeliefPropagation::Sample(std::vector<std::vector<unsigned int> >& states,
	unsigned int sample_count) {
	assert(0);
}

double BeliefPropagation::MinimizeEnergy(std::vector<unsigned int>& state) {
	min_sum = true;
	PerformInference();
	state.resize(fg->Cardinalities().size());
	std::copy(best_state.begin(), best_state.end(), state.begin());
	min_sum = false;
	return (fg->EvaluateEnergy(state));
}

void BeliefPropagation::PerformInferenceStepParallel() {
	// 1. factor-to-variable
	PassFactorToVariable();
	// 2. variable-to-factor
	PassVariableToFactor();
}

void BeliefPropagation::PerformInferenceSequential() {
	// Perform one sequential pass over all messages
	for (unsigned int oi = 0; oi < order.size(); ++oi) {
		unsigned int vi = order[oi].VariableNode();
		unsigned int fi = order[oi].FactorNode();
		unsigned int msg_id = order_msgid[oi];

		if (order[oi].steptype == FactorGraphStructurizer::LeafIsFactorNode) {
			// message: fi -> vi
			const Factor* factor = fg->Factors()[fi];
			PassFactorToVariable(factor, vi, msg_for_var[msg_id],
				msglist_for_factor[fi]);
		} else {
			// message: vi -> fi
			PassVariableToFactor(fi, msg_for_factor[msg_id], vi);
		}
	}
}

void BeliefPropagation::PassFactorToVariable() {
	const std::vector<Factor*>& factors = fg->Factors();
	const std::vector<unsigned int>& card = fg->Cardinalities();

	// For each variable
	for (unsigned int vi = 0; vi < card.size(); ++vi) {
		msg_list_t::const_iterator mli = msglist_for_var.find(vi);
		assert(mli != msglist_for_var.end());
		const std::vector<unsigned int>& ml = mli->second;

		// For each factor connected to that variable:
		// Send message from factor to variable
		for (unsigned int mli = 0; mli < ml.size(); ++mli) {
			assert(ml[mli] < msg_for_var.size());

			unsigned int factor_index = msg_for_var_srcfactor[ml[mli]];
			const Factor* factor = factors[factor_index];
			PassFactorToVariable(factor, vi, msg_for_var[ml[mli]],
				msglist_for_factor[factor_index]);
		}
	}
}

// Single message variant
void BeliefPropagation::PassFactorToVariable(const Factor* factor,
	unsigned int vi, std::vector<double>& msg,
	const std::vector<unsigned int>& msglist_for_factor_cur) {
	// Obtain type and adjacent variables of the factor
	const FactorType* ftype = factor->Type();

	const std::vector<unsigned int>& fvars = factor->Variables();
	unsigned int fvi_to = static_cast<unsigned int>(
		std::find(fvars.begin(), fvars.end(), vi) - fvars.begin());

	// Target message to be computed
	// r_{m->n}(x_n) = log sum_{x_m \ n} exp(
	//    -E(x_m) + sum_{n' \in N(m) \ n} q_{n'->m}(x_{n'}) )
	std::fill(msg.begin(), msg.end(), 0.0);

	// Compute the message within the factor type
	ftype->ComputeBPMessage(factor, vi, fvi_to,
		msglist_for_factor_cur, msg_for_factor,
		msg_for_factor_srcvar, msg, min_sum);
}

void BeliefPropagation::PassVariableToFactor() {
	const std::vector<Factor*>& factors = fg->Factors();
	// For all factors
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		// Obtain messages directed to factor fi
		msg_list_t::const_iterator mli = msglist_for_factor.find(fi);
		assert(mli != msglist_for_factor.end());
		const std::vector<unsigned int>& ml = mli->second;

		// For all adjacent variables
		for (unsigned int mli = 0; mli < ml.size(); ++mli) {
			assert(ml[mli] < msg_for_factor.size());

			std::vector<double>& msg = msg_for_factor[ml[mli]];
			unsigned int from_var = msg_for_factor_srcvar[ml[mli]];
			PassVariableToFactor(fi, msg, from_var);
		}
	}
}

void BeliefPropagation::PassVariableToFactor(unsigned int fi,
	std::vector<double>& msg, unsigned int from_var) {
	// Target message to be computed
	//    q_{n->m}(x_n) = sum_{m' \in M(n) \ m} r_{m'->n}(x_n),
	// (26.11) McKay, in log-domain.
	std::fill(msg.begin(), msg.end(), 0.0);
	msg_list_t::const_iterator mvi = msglist_for_var.find(from_var);
	assert(mvi != msglist_for_var.end());
	const std::vector<unsigned int>& mliv = mvi->second;
	for (std::vector<unsigned int>::const_iterator fvi = mliv.begin();
		fvi != mliv.end(); ++fvi) {
		unsigned int for_var_msg_index = *fvi;
		// Skip messages from the target factor
		if (msg_for_var_srcfactor[for_var_msg_index] == fi)
			continue;

		// Add log-sum messages, (26.11)
		std::transform(msg_for_var[for_var_msg_index].begin(),
			msg_for_var[for_var_msg_index].end(),
			msg.begin(), msg.begin(), std::plus<double>());
	}

	// Normalization for numerical stability,
	//   i) sum-product: log-sum-exp = 0,
	//  ii) min-sum: sum = 0.
	double norm_delta = min_sum ?
		(std::accumulate(msg.begin(), msg.end(), 0.0) /
			static_cast<double>(msg.size()))
		: LogSumExp::Compute(msg);
	std::transform(msg.begin(), msg.end(), msg.begin(), _1 - norm_delta);
}

double BeliefPropagation::ConstructMarginals() {
	const std::vector<Factor*>& factors = fg->Factors();
	// Compute mean and variance of log_z estimate
	double marg_max_diff = -std::numeric_limits<double>::infinity();

	// Compute marginals of all factors
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		const Factor* factor = factors[fi];
		const FactorType* ftype = factor->Type();

		// Obtain messages directed to factor fi
		msg_list_t::const_iterator mli = msglist_for_factor.find(fi);
		assert(mli != msglist_for_factor.end());
		const std::vector<unsigned int>& msglist_for_factor_cur = mli->second;

		double cur_marg_max_diff = ftype->ComputeBPMarginal(factor,
			msglist_for_factor_cur, msg_for_factor, marginals[fi], min_sum);
		if (cur_marg_max_diff > marg_max_diff)
			marg_max_diff = cur_marg_max_diff;

	}

	return (marg_max_diff);
}

double BeliefPropagation::ComputeVariableBeliefs() {
	const std::vector<unsigned int>& card = fg->Cardinalities();
	var_beliefs.resize(card.size());

	// For each variable
	double max_change = -std::numeric_limits<double>::infinity();
	for (unsigned int vi = 0; vi < card.size(); ++vi) {
		// Initialize beliefs
		std::vector<double> var_belief_vi_old(var_beliefs[vi]);
		var_beliefs[vi].resize(card[vi]);
		std::fill(var_beliefs[vi].begin(), var_beliefs[vi].end(), 0.0);

		// For each message directed to variable vi
		msg_list_t::const_iterator mli = msglist_for_var.find(vi);
		assert(mli != msglist_for_var.end());
		const std::vector<unsigned int>& ml = mli->second;
		for (unsigned int mli = 0; mli < ml.size(); ++mli) {
			assert(ml[mli] < msg_for_var.size());
			std::vector<double>& msg = msg_for_var[ml[mli]];

			// sum_{m \in M(vi)} log r_{m->n}(x_n)
			assert(msg.size() == card[vi]);
			std::transform(msg.begin(), msg.end(), var_beliefs[vi].begin(),
				var_beliefs[vi].begin(), std::plus<double>());
		}

		// Compute normalized variable marginal (belief)
		double Z_vi = min_sum ?
			(std::accumulate(var_beliefs[vi].begin(),
				var_beliefs[vi].end(), 0.0) /
				static_cast<double>(var_beliefs[vi].size()))
			: LogSumExp::Compute(var_beliefs[vi]);
		for (unsigned int vs = 0; vs < var_beliefs[vi].size(); ++vs) {
			if (min_sum) {
				var_beliefs[vi][vs] -= Z_vi;
			} else {
				var_beliefs[vi][vs] = std::exp(var_beliefs[vi][vs] - Z_vi);
			}
			if (var_belief_vi_old.empty()) {
				max_change = std::numeric_limits<double>::infinity();
			} else {
				max_change = std::max(max_change,
					std::fabs(var_beliefs[vi][vs] - var_belief_vi_old[vs]));
			}
		}
	}
	return (max_change);
}

double BeliefPropagation::ReconstructMinimumEnergyState(
	std::vector<unsigned int>& state) const {
	const std::vector<unsigned int>& card = fg->Cardinalities();
	assert(state.size() == card.size());
	assert(var_beliefs.size() == card.size());

	for (size_t vi = 0; vi < card.size(); ++vi) {
		assert(var_beliefs[vi].size() == card[vi]);
		state[vi] = static_cast<unsigned int>(std::max_element(
			var_beliefs[vi].begin(), var_beliefs[vi].end())
			- var_beliefs[vi].begin());
	}
	return (fg->EvaluateEnergy(state));
}

// (3.45) in [Wainwright and Jordan], "A(theta) = negative free energy"
// Theorem 5 in [Yedidia, Freeman, and Weiss], "Interior stationary points of
// the constrained Bethe free energy must be BP fixed points".
// (37) in [Yedidia, Freeman, and Weiss] gives the Bethe free energy.  This is
// an approximation to the negative log partition function logZ.
double BeliefPropagation::ComputeBetheFreeEnergy() const {
	double U_Bethe = 0.0;	// Bethe average energy
	double H_Bethe = 0.0;	// Bethe entropy
	const std::vector<Factor*>& factors = fg->Factors();
	const std::vector<unsigned int>& card = fg->Cardinalities();
	std::vector<unsigned int> var_degree(card.size(), 0);
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		const Factor* factor = factors[fi];
		const std::vector<double>& energies = factor->Energies();
		size_t energies_size = energies.size();
		for (size_t ei = 0; ei < energies_size; ++ei) {
			U_Bethe += -marginals[fi][ei] * (-energies[ei]);
			H_Bethe += -marginals[fi][ei] * std::log(marginals[fi][ei]);
		}

		// Increase degrees of all variables involved in this factor
		const std::vector<unsigned int>& fac_vars = factor->Variables();
		for (unsigned int fvi = 0; fvi < fac_vars.size(); ++fvi)
			var_degree[fac_vars[fvi]] += 1;
	}

	size_t var_count = card.size();
	for (size_t vi = 0; vi < var_count; ++vi) {
		assert(var_degree[vi] >= 1);
		assert(var_beliefs[vi].size() == card[vi]);
		double corr = 0.0;
		for (unsigned int state = 0; state < card[vi]; ++state)
			corr += var_beliefs[vi][state] * std::log(var_beliefs[vi][state]);
		H_Bethe += static_cast<double>(var_degree[vi] - 1) * corr;
	}

	// Return the Bethe free energy, log Z = -Bethe = -(U-H)
	return (U_Bethe - H_Bethe);
}

}


#include "STLSceneLoader.h"
#include "SceneDrawables.h"
#include "SceneLoader.h"
#include "NullOut.h"
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <fmt/format.h>
#include <fstream>
#include "gettext.h"

using namespace std;
using namespace boost::algorithm;
using fmt::format;
using namespace cnoid;

namespace {

static const bool ENABLE_COMPACTION = true;

struct Registration {
    Registration(){
        SceneLoader::registerLoader(
            "stl",
            []() -> shared_ptr<AbstractSceneLoader> { return make_shared<STLSceneLoader>(); });
    }
} registration;

Vector3f readVector3(string text)
{
    trim(text);
    Vector3f value;
    int i = 0;
    split_iterator<string::iterator> iter = make_split_iterator(text, token_finder(is_space(), token_compress_on));
    while(iter != split_iterator<string::iterator>()){
        value[i++] = boost::lexical_cast<float>(*iter++);
        if(i == 3){
            break;
        }
    }
    return value;
}

class CompactMeshArranger
{
public:
    SgMeshPtr mesh;
    SgVertexArray& vertices;
    SgNormalArray& normals;
    SgIndexArray& triangleVertices;
    SgIndexArray& normalIndices;
    BoundingBoxf bbox;

    CompactMeshArranger();
    void addVertex(const Vector3f& vertex);
    void addNormal(const Vector3f& normal);
    SgShape* finalize();
};

}

CompactMeshArranger::CompactMeshArranger()
    : mesh(new SgMesh),
      vertices(*mesh->getOrCreateVertices()),
      normals(*mesh->getOrCreateNormals()),
      triangleVertices(mesh->triangleVertices()),
      normalIndices(mesh->normalIndices())
{

}

void CompactMeshArranger::addVertex(const Vector3f& vertex)
{
    static const int SearchLength = 27;

    bool found = false;
    int index = vertices.size() - 1;

    if(ENABLE_COMPACTION){
        int minIndex = std::max(0, index - (SearchLength - 1));
        while(index >= minIndex){
            if(vertex.isApprox(vertices[index])){
                found = true;
                break;
            }
            --index;
        }
    }

    if(found){
        triangleVertices.push_back(index);
    } else {
        triangleVertices.push_back(vertices.size());
        vertices.push_back(vertex);
        bbox.expandBy(vertex);
    }
}

void CompactMeshArranger::addNormal(const Vector3f& normal)
{
    static const int SearchLength = 12;

    bool found = false;
    int index = normals.size() - 1;

    if(ENABLE_COMPACTION){
        int minIndex = std::max(0, index - (SearchLength - 1));
        while(index >= minIndex){
            if(normal.isApprox(normals[index])){
                found = true;
                break;
            }
            --index;
        }
    }

    if(!found){
        index = normals.size();
        normals.push_back(normal);
    }
    normalIndices.push_back(index);
    normalIndices.push_back(index);
    normalIndices.push_back(index);
}

SgShape* CompactMeshArranger::finalize()
{
    if(vertices.empty()){
        return nullptr;
    }

    auto shape = new SgShape;
    shape->setMesh(mesh);
    vertices.shrink_to_fit();
    normals.shrink_to_fit();
    triangleVertices.shrink_to_fit();
    normalIndices.shrink_to_fit();
    mesh->setBoundingBox(bbox);

    return shape;
}

STLSceneLoader::STLSceneLoader()
{
    os_ = &nullout();
}

void STLSceneLoader::setMessageSink(std::ostream& os)
{
    os_ = &os;
}

SgNode* STLSceneLoader::load(const std::string& filename)
{
    std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary);
    if(!ifs.is_open()){
        os() << format(_("Unable to open file \"{}\"."), filename) << endl;
        return 0;
    }

    ifs.seekg(0, fstream::end);
    unsigned int fileSize = ifs.tellg();
    ifs.seekg(0, fstream::beg);

    CompactMeshArranger arranger;

    bool isBinary = false;
    unsigned int numFaces = 0;
    uint8_t buf[84];
    ifs.read((char*)buf, 84);
    if(ifs.gcount() == 84){
        numFaces = buf[80] + (buf[81] << 8) + (buf[82] << 16) + (buf[83] << 24);
        unsigned int expectedSize = numFaces * 50 + 84;
        if(expectedSize == fileSize){
            isBinary = true;
        }
    }

    if(isBinary){
        for(size_t i = 0; i < numFaces; ++i){
            Vector3f normal;
            for(size_t j = 0; j < 3; ++j){
                ifs.read((char*)&normal[j], 4);
            }
            arranger.addNormal(normal);

            for(size_t j = 0; j < 3; ++j){
                Vector3f vertex;
                for(size_t k = 0; k < 3; ++k){
                    ifs.read((char*)&vertex[k], 4);
                }
                arranger.addVertex(vertex);
            }
            uint16_t attrib;
            ifs.read((char *)&attrib, 2);
        }
    } else {
        // text format
        std::ifstream ifs(filename.c_str(), std::ios::in);
        std::string line;
        while(!ifs.eof() && getline(ifs, line)){
            trim(line);
            if(boost::istarts_with(line, "vertex")){
                arranger.addVertex(readVector3(line.substr(6)));
            } else if(boost::istarts_with(line, "facet normal")){
                arranger.addNormal(readVector3(line.substr(12)));
            }
        }
    }

    auto shape = arranger.finalize();

    if(!shape){
        os() << "Empty vertices." << endl;
    }

    return shape;
}


/**
 * Copyright (c) 2015 Carnegie Mellon University, Daniel Maturana <dimatura@cmu.edu>
 *
 * For License information please see the LICENSE file in the root directory.
 *
 */

#ifndef SCROLLING_STRATEGIES_HPP_ITK653BW
#define SCROLLING_STRATEGIES_HPP_ITK653BW

#include <boost/shared_ptr.hpp>

#include <ros/ros.h>

#include <pcl_util/point_types.hpp>
#include <pcl_util/pcl_util.hpp>

#include "scrollgrid/scrollgrid3.hpp"

namespace ca
{

template<class Scalar>
class ScrollForBaseFrame {
public:
 typedef Eigen::Matrix<Scalar, 3, 1> Vec3;

 typedef boost::shared_ptr<ScrollForBaseFrame> Ptr;

public:
  ScrollForBaseFrame() {
    ros::NodeHandle nh("~");

    // if you want to center box around sensor/robot, use (0,0,0)
    double x, y, z;
    nh.param<double>("/scrolling_strategies/target_sensor_to_center_x", x, 0);
    nh.param<double>("/scrolling_strategies/target_sensor_to_center_y", y, 0);
    nh.param<double>("/scrolling_strategies/target_sensor_to_center_z", z, 0);
    target_sensor_to_center_.x() = x;
    target_sensor_to_center_.y() = y;
    target_sensor_to_center_.z() = z;

    // how far do we get from "target" location before scrolling
    nh.param<double>("/scrolling_strategies/scroll_dist_thresh_x", x, 0);
    nh.param<double>("/scrolling_strategies/scroll_dist_thresh_y", y, 0);
    nh.param<double>("/scrolling_strategies/scroll_dist_thresh_z", z, 0);
    scroll_dist_thresh_.x() = x;
    scroll_dist_thresh_.y() = y;
    scroll_dist_thresh_.z() = z;
  }
  ScrollForBaseFrame(Vec3 target_sensor_to_center, Vec3 scroll_dist_thresh):
    target_sensor_to_center_(target_sensor_to_center),
    scroll_dist_thresh_(scroll_dist_thresh){
  }

  public:
  void setParameters(Vec3 target_sensor_to_center, Vec3 scroll_dist_thresh) {
    target_sensor_to_center_ = target_sensor_to_center;
    scroll_dist_thresh_ = scroll_dist_thresh;
  }

  ca::Vec3Ix compute(const tf::Transform& wv2laser,
                     const ca::ScrollGrid3<Scalar>& grid) {
    //Vec3 origin_laser;
    //origin_laser.x() = wv2laser.getOrigin().getX();
    //origin_laser.y() = wv2laser.getOrigin().getY();
    //origin_laser.z() = wv2laser.getOrigin().getZ();

    //tf::Quaternion q(wv2laser.getRotation());
    //// TODO scalar
    //Eigen::Quaternionf eq = ca::rot_cast< Eigen::Quaternionf >(q);
    //Eigen::AngleAxisf flip_around_z(.5*M_PI, Eigen::Vector3f(0, 0, 1) );
    //Eigen::AngleAxisf flip_around_x(.5*M_PI, Eigen::Vector3f(1, 0, 0) );

    // compute offset in laser frame, not world_view
    //Vec3 target_sensor_to_center_rot = eq*target_sensor_to_center_;

    // add desired baseframe offset
    //origin_laser += target_sensor_to_center_;
    //origin_laser += target_sensor_to_center_rot;

    tf::Vector3 target_sensor_to_center = wv2laser * ca::point_cast< tf::Vector3 >(target_sensor_to_center_);
    Vec3 origin_laser = ca::point_cast<Vec3>(target_sensor_to_center);

    // compute if it is too close to grid boundaries, and change scroll_cells
    // accordingly
    ca::Vec3Ix scroll_cells(0, 0, 0);
    // origin is in world_view (after cloud is preprocessed)
    // center is in world_view
    Vec3 center(grid.center());
    Vec3 radius(grid.radius());
    Vec3 deviation( (origin_laser-center) );
    Vec3 abs_deviation( deviation.cwiseAbs() );
    Scalar resolution = grid.resolution();
    scroll_cells[0] = static_cast<grid_ix_t>(round(deviation[0]/resolution));
    scroll_cells[1] = static_cast<grid_ix_t>(round(deviation[1]/resolution));
    scroll_cells[2] = static_cast<grid_ix_t>(round(deviation[2]/resolution));

    // how to interpret the scroll_dist_thresh parameter
    //
    // C = center
    // | = bounds
    // + = helicopter
    // <----- = deviation
    // |~~| = edge threshold
    //
    // --|---+-------C---------------|--
    //   |   <-------                |
    //   |~~|                        |

    // measures closeness to edge. so bigger is better
    Eigen::Matrix<bool, 3, 1> not_within_bounds(radius.array() < (abs_deviation+scroll_dist_thresh_).array());
    scroll_cells = scroll_cells.cwiseProduct(not_within_bounds.cast<grid_ix_t>());

    return scroll_cells;
  }

private:
  Vec3 target_sensor_to_center_;
  Vec3 scroll_dist_thresh_;

private:
  ScrollForBaseFrame(const ScrollForBaseFrame& other);
  ScrollForBaseFrame& operator=(const ScrollForBaseFrame& other);

};

template<class Scalar>
class ScrollDownForCoverage {
public:
 typedef Eigen::Matrix<Scalar, 3, 1> Vec3;

 typedef boost::shared_ptr<ScrollDownForCoverage> Ptr;

public:
  ScrollDownForCoverage() {
    ros::NodeHandle nh("~");

    double z;
    nh.param<double>("scrolling_strategies/scroll_cover_margin_z", z, 10);
    scroll_cover_margin_z_ = z;
  }

  virtual ~ScrollDownForCoverage() { }

public:

  ca::Vec3Ix compute(const ca::PC_XYZ& xyz,
                     const ca::ScrollGrid3<Scalar>& grid) {

    // do we need to scroll grid? find lowest relevant point in cloud.
    // we ignore points that fall outside xy boundaries of grid
    // TODO make this configurable
    ca::scrollgrid::Box<float, 2> box_xy(grid.min_pt().template head<2>().template cast<float>(),
                                         grid.max_pt().template head<2>().template cast<float>());
    int counted_pts = 0;
    float min_z = std::numeric_limits<float>::max();
    for (size_t i=0; i < xyz.size(); ++i) {
      const pcl::PointXYZ& p(xyz[i]);
      // TODO templatize on scalar
      Eigen::Vector2f pf2(p.getVector3fMap().head<2>());
      if (box_xy.contains(pf2)) {
        min_z = std::min(min_z, p.z);
        ++counted_pts;
      }
    }

    float current_min_z(grid.min_pt().z());

    ca::Vec3Ix scroll_cells(0, 0, 0);
    static const int MIN_POINTS_FOR_SCROLLING = 5;
    // scroll up, down or stay
    if (counted_pts >= MIN_POINTS_FOR_SCROLLING) {
      if (min_z < (current_min_z - scroll_cover_margin_z_)) {
        grid_ix_t num_cells = static_cast<int>(( (current_min_z-min_z)+scroll_cover_margin_z_)/grid.resolution() );
        scroll_cells = Vec3Ix(0, 0, -num_cells);
      } else if (min_z > (current_min_z + scroll_cover_margin_z_)) {
        grid_ix_t num_cells = static_cast<int>(( (min_z-current_min_z)-scroll_cover_margin_z_)/grid.resolution() );
        scroll_cells = Vec3Ix(0, 0, num_cells);
      } else {
        scroll_cells.setZero();
      }
    } else {
      scroll_cells.setZero();
    }
    ROS_INFO_STREAM("min_z " << min_z << ", current_min_z = " << current_min_z);

    return scroll_cells;
  }

private:
  Scalar scroll_cover_margin_z_;

private:
  ScrollDownForCoverage(const ScrollDownForCoverage& other);
  ScrollDownForCoverage& operator=(const ScrollDownForCoverage& other);

};

template<class Scalar>
class ScrollForBaseFrameAndDown {
public:
 typedef Eigen::Matrix<Scalar, 3, 1> Vec3;
 typedef boost::shared_ptr<ScrollForBaseFrameAndDown> Ptr;

public:
  ScrollForBaseFrameAndDown() {

  }

  virtual ~ScrollForBaseFrameAndDown() { }

public:

  ca::Vec3Ix compute(const tf::Transform& wv2laser,
                     const ca::PC_XYZ& xyz,
                     const ca::ScrollGrid3<Scalar>& grid) {

    ca::Vec3Ix scroll_bf = bf_scroller_.compute(wv2laser, grid);
    ca::Vec3Ix scroll_zcov = zcov_scroller_.compute(xyz, grid);

    ca::Vec3Ix scroll_cells(scroll_bf);
    scroll_cells[2] = scroll_zcov[2];
    return scroll_cells;
  }

private:
  ScrollForBaseFrame<Scalar> bf_scroller_;
  ScrollDownForCoverage<Scalar> zcov_scroller_;

private:
  ScrollForBaseFrameAndDown(const ScrollForBaseFrameAndDown & other);
  ScrollForBaseFrameAndDown& operator=(const ScrollForBaseFrameAndDown & other);

};

} /* ca */

#endif /* end of include guard: SCROLLING_STRATEGIES_HPP_ITK653BW */


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

#ifndef NETKET_ABSTRACT_OPERATOR_HPP
#define NETKET_ABSTRACT_OPERATOR_HPP

#include <complex>
#include <functional>
#include <memory>
#include <tuple>
#include <vector>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <nonstd/span.hpp>

#include "Hilbert/hilbert.hpp"

namespace netket {
/**
 * This struct represents a non-zero matrix-element H(v,v') of an operator for a
 * given visible state v.
 */
struct ConnectorRef {
  /// The matrix element H(v,v')
  Complex mel;
  /// The indices at which v needs to be changed to obtain v'
  nonstd::span<const int> tochange;
  /// The new values such that
  ///    v'(tochange[k]) = newconf[k]
  /// and v'(i) = v(i) for i ∉ tochange.
  nonstd::span<const double> newconf;
};

/**
      Abstract class for quantum Operators.
      This class prototypes the methods needed
      by a class satisfying the Operator concept.
      Users interested in implementing new quantum Operators should derive they
   own class from this class.
*/
class AbstractOperator {
 public:
  using VectorType = Eigen::VectorXd;
  using VectorRefType = Eigen::Ref<VectorType>;
  using VectorConstRefType = Eigen::Ref<const VectorType>;
  using MelType = std::vector<Complex>;
  using ConnectorsType = std::vector<std::vector<int>>;
  using NewconfsType = std::vector<std::vector<double>>;
  using ConnCallback = std::function<void(ConnectorRef)>;

  /**
  Member function finding the connected elements of the Operator.
  Starting from a given visible state v, it finds all other visible states v'
  such that the matrix element O(v,v') is different from zero.
  In general there will be several different connected visible units satisfying
  this condition, and they are denoted here v'(k), for k=0,1...N_connected.
  @param v a constant reference to the visible configuration.
  @param mel(k) is modified to contain matrix elements O(v,v'(k)).
  @param connector(k) for each k contains a list of sites that should be changed
  to obtain v'(k) starting from v.
  @param newconfs(k) is a vector containing the new values of the visible units
  on the affected sites, such that: v'(k,connectors(k,j))=newconfs(k,j). For the
  other sites v'(k)=v, i.e. they are equal to the starting visible
  configuration.
  */
  virtual void FindConn(VectorConstRefType v, MelType &mel,
                        ConnectorsType &connectors,
                        NewconfsType &newconfs) const = 0;

  virtual std::tuple<MelType, ConnectorsType, NewconfsType> GetConn(
      VectorConstRefType v) const;

  /**
   * Iterates over all states reachable from a given visible configuration v,
   * i.e., all states v' such that O(v,v') is non-zero.
   * @param v The visible configuration.
   * @param callback Function void callback(ConnectorRef conn) which will be
   * called once for each reachable configuration v'. The parameter conn
   * contains the value O(v,v') and the information to obtain v' from v. Note
   * that the members conn.positions and conn.values are spans that can only be
   * savely used inside the callback. They will become invalid once callback
   * returns.
   */
  virtual void ForEachConn(VectorConstRefType v, ConnCallback callback) const;

  /**
  Member function returning the hilbert space associated with this Hamiltonian.
  @return Hilbert space specifier for this Hamiltonian
  */
  const AbstractHilbert &GetHilbert() const { return *hilbert_; }

  std::shared_ptr<const AbstractHilbert> GetHilbertShared() const {
    return hilbert_;
  }

  /**
   * Returns the number of states in the underlying Hilbert space.
   *
   * @precondition `this->GetHilbert().IsIndexable()`
   */
  int Dimension() const { return GetHilbert().GetIndex().NStates(); }

  /**
   * Applies the operator to a quantum state.
   * @param state The entry state(i) corresponds to the coefficient of the basis
   * vector with quantum numbers given by StateFromNumber(i) of a HilbertIndex
   * for the original operator.
   * @precondition `this->GetHilbert().IsIndexable()`
   */
  virtual Eigen::VectorXcd Apply(const Eigen::VectorXcd &state) const {
    auto const &hilbert_index = GetHilbert().GetIndex();
    Eigen::VectorXcd result(hilbert_index.NStates());
    result.setZero();

    for (int i = 0; i < hilbert_index.NStates(); ++i) {
      // TODO: Make NumberToState return a reference so that we can avoid the
      // copy here.
      const auto v = hilbert_index.NumberToState(i);
      ForEachConn(v, [&](ConnectorRef conn) {
        const auto j = i + hilbert_index.DeltaStateToNumber(v, conn.tochange,
                                                            conn.newconf);
        result(i) += conn.mel * state(j);
      });
    }
    return result;
  }

  Eigen::MatrixXcd ToDense() const {
    const auto &hilbert_index = GetHilbert().GetIndex();
    Eigen::MatrixXcd matrix;
    matrix.resize(hilbert_index.NStates(), hilbert_index.NStates());
    matrix.setZero();
    ForEachMatrixElement([&matrix](const int i, const int j, const Complex x) {
      matrix(i, j) = x;
    });
    return matrix;
  }

  Eigen::SparseMatrix<Complex, Eigen::RowMajor> ToSparse() const {
    const auto &hilbert_index = GetHilbert().GetIndex();
    std::vector<Eigen::Triplet<Complex>> triplets;
    triplets.reserve(hilbert_index.NStates());

    ForEachMatrixElement(
        [&triplets](const int i, const int j, const Complex x) {
          triplets.emplace_back(i, j, x);
        });

    Eigen::SparseMatrix<Complex> matrix(hilbert_index.NStates(),
                                        hilbert_index.NStates());
    matrix.setFromTriplets(triplets.begin(), triplets.end());
    matrix.makeCompressed();
    return matrix;
  }

  virtual ~AbstractOperator() = default;

 protected:
  AbstractOperator(std::shared_ptr<const AbstractHilbert> hilbert)
      : hilbert_(std::move(hilbert)) {}

 private:
  template <class Function>
  void ForEachMatrixElement(Function &&function) const {
    const auto &hilbert_index = GetHilbert().GetIndex();
    for (int i = 0; i < hilbert_index.NStates(); ++i) {
      // TODO: Make NumberToState return a reference so that we can avoid the
      // copy here.
      const auto v = hilbert_index.NumberToState(i);
      ForEachConn(v, [&](ConnectorRef conn) {
        const auto j = i + hilbert_index.DeltaStateToNumber(v, conn.tochange,
                                                            conn.newconf);
        function(i, j, conn.mel);
      });
    }
  }

  std::shared_ptr<const AbstractHilbert> hilbert_;
};

template <class Operator>
Complex Mean(Operator &&apply, const Eigen::VectorXcd &state) {
  return state.adjoint() * std::forward<Operator>(apply)(state);
}

template <class Operator>
std::pair<Complex, double> MeanVariance(Operator &&apply,
                                        const Eigen::VectorXcd &state) {
  Eigen::VectorXcd state1 = apply(state);
  const Complex mean = state.adjoint() * state1;
  state1 -= mean * state;
  const double var = state1.norm();
  return {mean, var};
}

}  // namespace netket

#endif


#include <iostream>
#include <iomanip>
#include <string>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/random_generator.hpp>
#include <boost/uuid/uuid_io.hpp>

int main(int argc, char** argv){
	std::string name;
	std::string parent;
	if(argc > 1){
		name = argv[1];
	}
	if(argc > 2){
		parent = argv[2];
	}
	boost::uuids::random_generator r;
	auto u = r();

	if(argc > 1){
		std::cout << "template<class T>\n";
		std::cout << "struct " << name << "\n";
		std::cout << "\t:public cppcomponents::define_unknown_interface<T,\n";
	}
	std::cout << "\t// {" << std::uppercase << u << "}" << std::endl;

	std::cout << "\tcppcomponents::uuid<\n";
	std::cout << "\t";

	int counter = 0;
	for(auto k:u){
		if(counter==0)
			std::cout << "0x";
		if(counter==4)
			std::cout << ",0x";
		if(counter==6)
			std::cout << ",0x";
		if(counter==8)
			std::cout << ",0x";
		if(counter>8)
			std::cout << ",0x";
		++counter;
		std::cout<< std::setw(2) << std::setfill('0') << std::hex << (unsigned int)k;

	}
	std::cout << std::endl;
	std::cout << "\t>";
	if(argc > 1){
		std::cout << "\n\t";
		if(parent.size()){
			std::cout << "," << parent;
		}
		std::cout << ">\n";
		std::cout << "{\n\n\n\n\n";
		std::cout << "\t" << name << "()\n\n\t{}\n};\n";
	}

}