/*
  [auto_generated]
  boost/numeric/odeint/external/vexcl/vexcl_algebra_dispatcher.hpp

  [begin_description]
  algebra_dispatcher specialization for vexcl
  [end_description]

  Copyright 2009-2013 Karsten Ahnert
  Copyright 2009-2013 Mario Mulansky

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE_1_0.txt or
  copy at http://www.boost.org/LICENSE_1_0.txt)
*/

#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_ALGEBRA_DISPATCHER_HPP_DEFINED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_ALGEBRA_DISPATCHER_HPP_DEFINED

#include <vexcl/vector.hpp>

#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>

namespace boost {
namespace numeric {
namespace odeint {

// specialization for vexcl vector
template< typename T >
struct algebra_dispatcher< vex::vector< T > >
{
    typedef vector_space_algebra algebra_type;
};

// specialization for vexcl multivector
template< typename T , size_t N >
struct algebra_dispatcher< vex::multivector< T , N > >
{
    typedef vector_space_algebra algebra_type;
};

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_ALGEBRA_DISPATCHER_HPP_DEFINED


// Wrapper for UAI competition code  -- For qlou solvers (?)
//

#include <cstdio>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <queue>
#include <boost/program_options.hpp>

#include "graphmodel.h"
#include "wmbe.h"
#include "mcts.h"
#include "wmbsearch.h"
#include "searchsample.h"
#include "mmap.h"
#include "randheur.h"
#include "mmapIS.h"

using namespace std;
using mex::mxObject;
using mex::Var;
using mex::VarSet;
using mex::Factor;
using mex::vector;
using mex::graphModel;

using mex::timeSystem;

namespace po = boost::program_options;

#define c_log10 std::log(10)

double wmbMemLimit;
double dt;
int nOrders, nExtra;
size_t iBound;
double timeOrder;
double stopIter, stopTime;
int nSearch;

double dampTheta, stepWeights;
double addEpsilon;
int isAdaptive;
double memUseRandom = mex::infty(); // if memory *way* out of reach, just use random ordering...
int verbose; // output messages
//int max_nodes = 1;
// memory limit for search
double searchMemLimit;
// log ratio between upper and lower bounds
//double searchBoundRatio;
// output time unit
double timeUnit;
// output time ratio
// timeUnit * timeRatio^t is the actual timestamp
// if timeRatio > 1, its output is in exponential
double timeRatio;
// no. of samples
int nSample;
double DELTA;
// overall time budget
double timeBudget;
double searchTime;
// overall memory budget including wmb construction and search
double memoryBudget;
// overall memory control, let wmb construction use as memory as it can (within memory budget)
// the left memory is for other task
int isAllMemControl = 0;
// overall time control
int isAllTimeControl = 0;
// for generating a tree
int NSP;
// no. of nodes to expand in each iteration for searchsample
int NND;
// no. of samples to draw in each expansion of a frontier node
int KSP;
//
int lookahead_depth;
//
int sumVarCopies; // no. of copies of SUM variables, used in "MMAPIS"

// Caveat: MEX_ENUM seems buggy: it can not deal with space between the second last and the last options
MEX_ENUM( Task, MPE,PR,MAR,MMAP );
MEX_ENUM( Method, AOBFS,UBFS,MCTS,SESA,RDHR,MMAPIS );
//MEX_ENUM( Mode , DFS,SMA ); defined in wmbsearch.h
MEX_ENUM( Heur, UB,BOTH ); // upper bound heuristics, or both upper and lower heuristics, default BOTH

bool isAndOr = true; // whether using AND/OR structure
// whether we need lower heuristics
bool isLbHeur = true;
bool isSolved = false;

int main(int argc, char* argv[]) {
//  double timeStart = timeSystem();
	//if (argc < 4) { cout<<"Usage: "<<argv[0]<<" <file.uai> <seed> <PR|MPE|MAR>\n"; return 0; }
	const char* probName; // = argv[1];
	const char* taskName; //  = argv[3];
	const char* methodName;
	const char* pstName; // pseudo tree file
	const char* priorityName; // type of priority
	const char* searchModeName; // SMA or DFS
	const char* heurName;

	mex::vector<Factor> bel;

	Task task;
	Method method;
	Mode searchMode; // SMA or DFS
	Heur heur;

	po::options_description desc("Available options");
	desc.add_options()
			("help", "print help message")
			("file,f", po::value<std::string>(), "input problem filename")
			("evidence,e", po::value<std::string>(), "input evidence filename")
			("seed,S", po::value<int>(), "random number initial seed")
			("task,T", po::value<std::string>(), "inference task string: PR, MMAP")
			("heur,H", po::value<std::string>(), "heuristics, UB (upper bound) or BOTH (default)")
			("method", po::value<std::string>(), "inference methods: AOBFS, UBFS, MCTS, SESA, RDHR, MMAPIS")
			("orders,o", po::value<int>(&nOrders)->default_value(1), "number of variable orderings to try")
			("order-time,t", po::value<double>(&timeOrder)->default_value(1), "max time spend on variable orderings")
			("order-rand", po::value<int>(&nExtra)->default_value(0), "var order randomness; n=-1 (none), or among best+n")
			("order-file", po::value<std::string>(), "problem elimination ordering filename")
			("query-file", po::value<std::string>(), "query (MAX) variables, see uai-14 query file format")
			("ibound,i", po::value<size_t>(&iBound)->default_value(50), "initial i-bound")
			("memory,m", po::value<double>(&wmbMemLimit)->default_value(2 * 1024.0), "memory bound for WMB construction (MB)")
			("stopiter,n", po::value<double>(&stopIter)->default_value(1.0), "maximum number of iterations")
			("stoptime", po::value<double>(&stopTime)->default_value(20.0), "maximum iteration time (sec)")
			("damp", po::value<double>(&dampTheta)->default_value(1.0), "damping of factor reparameterization, real[0,1]: 0.0=no update, 1.0=no damping")
			("stepw", po::value<double>(&stepWeights)->default_value(0.1), "step size of weight update; 0.0=no update")
			("adaptive", po::value<int>(&isAdaptive)->default_value(1), "Adaptive step sizes; 0=no, 1=yes (default)")
			("eps", po::value<double>(&addEpsilon)->default_value(0.0), "constant to add to all factors (to avoid hard zeros). require for mmapIS sampling!")
			("nsearch,s", po::value<int>(&nSearch)->default_value(0), "max number of search nodes")
			("nsample", po::value<int>(&nSample)->default_value(1e8), "max number of samples")
			("delta", po::value<double>(&DELTA)->default_value(0.025), "delta  (1-confidence) for Empirical Bernstein bound")
			("pseudo-tree", po::value<bool>(&isAndOr)->default_value(1), "non-chain pseudo tree? default 1 (yes)")
			("timebudget", po::value<double>(&timeBudget)->default_value(60.0), "overall time budget for all processes")
			("priority", po::value<std::string>(), "priority type")
			("searchmemory", po::value<double>(&searchMemLimit)->default_value(2 * 1024.0), "memory limit for wmbsearch (MB)")
			("memorybudget", po::value<double>(&memoryBudget)->default_value(4 * 1024.0), "overall memory control")
			("timeunit", po::value<double>(&timeUnit)->default_value(60.0), "output period (second)")
			("searchtime", po::value<double>(&searchTime)->default_value(60.0), "time limit for wmbsearch (sec)")
			("pst_file", po::value<std::string>(), "output pseudo tree filename")
			("verbose,v", po::value<int>(&verbose)->default_value(0), "verbose, 0,1,2,3")
			("searchmode", po::value<std::string>(), "search mode in wmbsearch: SMA (default), DFS")
			("nsp", po::value<int>(&NSP)->default_value(-1), "no. of samples to draw in each iteration")
			("nnd", po::value<int>(&NND)->default_value(1), "no. of nodes to expand in each iteration")
			("ksp", po::value<int>(&KSP)->default_value(1), "no. of samples to draw in each expansion")
			("sumCopies,K", po::value<int>(&sumVarCopies)->default_value(1), "no. of copies of SUM variables")
			("lookahead", po::value<int>(&lookahead_depth)->default_value(std::numeric_limits<int>::infinity()),
					"depth of stochastic lookahead, 0 - adaptive")
			("timecontrol", po::value<int>(&isAllTimeControl)->default_value(1), "set 0 to disable overall time control")
			("memorycontrol", po::value<int>(&isAllMemControl)->default_value(0), "set > 0 to enable overall memory control")
			;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		std::cout << desc << "\n";
		return 1;
	}
	if (vm.count("file")) {
		probName = vm["file"].as<std::string>().c_str();
	} else {
		std::cout << "Missing input problem file!\n";
		return 1;
	}
	if (vm.count("seed")) {
		mex::randSeed(vm["seed"].as<int>());
	}
	// task
	if (vm.count("task")) {
		taskName = vm["task"].as<std::string>().c_str();
		task = Task(taskName);
	} else {
		std::cout << "Missing task!\n";
		return 1;
	}
	if (task != Task::PR && task != Task::MMAP) {
		std::cout << "Task should be PR or MMAP" << std::endl;
		return 1;
	}
	// method
	if (vm.count("method")) {
		methodName = vm["method"].as<std::string>().c_str();
		method = Method(methodName);
	} else {
		std::cout << "Missing method!\n";
		return 1;
	}
	/*	if (method == Method::AND_OR) {
	 isAndOr = true;
	 }
	 if (method == Method::OR) {
	 isAndOr = false;
	 }*/
	// heuristics
	if (vm.count("heur")) {
		heurName = vm["heur"].as<std::string>().c_str();
		heur = Heur(heurName);
	} else {
		heur = Heur::BOTH; // default
	}
	if (heur == Heur::UB) {
		std::cout << "Only upper (bound) heuristics applied!\n";
		isLbHeur = false;
	} else {
//		assert(heur == Heur::BOTH);
		std::cout << "Both upper and Lower (bound) heuristics applied!\n";
		isLbHeur = true;
	}
	// file to output pseudotree
	if (vm.count("pst_file")) {
		pstName = vm["pst_file"].as<std::string>().c_str();
	} else {
		pstName = "";
	}
	// priority type
	if (vm.count("priority")) {
		priorityName = vm["priority"].as<std::string>().c_str();
	} else {
		priorityName = "";
	}

	if (vm.count("searchmode")) {
		searchModeName = vm["searchmode"].as<std::string>().c_str();
		searchMode = Mode(searchModeName);
	} else {
//		std::cout << "Default search mode (SMA) in wmbsearch applied!\n";
		searchMode = Mode::SMA;
	}

	if (isAllMemControl > 0) {
		wmbMemLimit = memoryBudget;
	}

	double processEnteringTime = timeSystem();

	/*** READ IN PROBLEM FILE **********************************************************/
	std::cout << "Reading model file: " << probName << "\n";
	ifstream is;
	is.open(probName);
	if (!is.is_open())
		throw std::runtime_error("Failed to open problem file");
	mex::vector<Factor> forig = Factor::readUai10(is);
	if (addEpsilon > 0.0)
		for (size_t f = 0; f < forig.size(); ++f)
			forig[f] += addEpsilon;
	//
	mex::wmbe wmbUB(forig);
//	mex::wmbe wmbLB(forig);
	mex::wmbe wmbLB;
	if (isLbHeur) {
		wmbLB = mex::wmbe(forig);
	}

	size_t nvar = wmbUB.nvar();
	bel.resize(nvar);
	mex::vector<size_t> evid(nvar, 0);
	std::cout << "Graphical model over " << wmbUB.nvar() << " variables with "
			<< forig.size() << " factors\n";

	/*** READ IN EVIDENCE FILE *********************************************************/
	VarSet evVar;
	ifstream is2;
	if (vm.count("evidence")) {
		is2.open(vm["evidence"].as<std::string>().c_str());
	}
	if (is2.is_open()) {
		std::cout << "Got evidence file\n";
		//int nEvid; is2 >> nEvid;
		int nEvid = 1;  // 2014 format: single evidence, no # of evidences entry
		std::cout << "Got " << nEvid << " evidences?\n";
		if (nEvid > 0) {
			int nEvidVar;
			is2 >> nEvidVar;
			for (size_t i = 0; i < nEvidVar; i++) {
				uint32_t vid;
				size_t vval;
				is2 >> vid >> vval;
				evid[vid] = vval;
				evVar |= wmbUB.var(vid);
				bel[vid] = Factor::delta(wmbUB.var(vid), vval);
			}
			std::cout << "Evidence on variables " << evVar << "\n";
			wmbUB.condition(evVar, evid);
			if (isLbHeur) {
				wmbLB.condition(evVar, evid);
			}
		}
	} else
		std::cout << "Evidence file not specified or not found\n";

	/*** Read in query (MAX) variables ************************************************************/
	VarSet maxVars;
	ifstream ismap;
//	auto queryFile = vm["query-file"].as<std::string>().c_str();
	if (vm.count("query-file")) {
//		ismap.open(queryFile);
		ismap.open(vm["query-file"].as<std::string>().c_str());
	}
	if (ismap.is_open()) {
		auto queryFile = vm["query-file"].as<std::string>().c_str();
		std::cout << "Reading query file: " << queryFile << std::endl;
//		std::cout << "Got query file" << std::endl;
		// the first entry is # of query (MAX) variables
		int nMax = 0;
		ismap >> nMax;
		std::cout << nMax << " MAX variables specified!" << std::endl;
		if (nMax > 0) {
			for (int i = 0; i < nMax; i++) {
				uint32_t vid;
				ismap >> vid;
				maxVars |= wmbUB.var(vid);
			}
			// Set MAX variables
			// this should not automatically set weights, thus safe for mmapIS
			wmbUB.setMaxVars(maxVars);
		}
		ismap.close();
	} else {
		std::cout << "Query (MAP) file not specified or not found" << std::endl;
	}

	/*** PREPARE REQUESTED TASK ********************************************************/
	std::cout << "Task is " << task << "\n";
	//if (task==Task::MPE) { std::cout<<"MPE task not supported\n"; return 1; }
	std::cout << "Method is " << method << "\n";

	/*** ELIMINATION ORDERS ************************************************************/

//    double startOrder = timeSystem();
	mex::VarOrder order;
	size_t InducedWidth, iOrder = 1;
	double score = wmbUB.order(mex::graphModel::OrderMethod::Random, order, 0,
			memUseRandom);
	const char *orderFile = NULL; // Check for pre-specified elimination order file
	if (vm.count("order-file")) {
		orderFile = vm["order-file"].as<std::string>().c_str();
	}
	ifstream orderIStream;
	if (orderFile != NULL)
		orderIStream.open(orderFile);
	if (orderIStream.is_open()) {
		// If we were given an input file with an elimination ordering, just use that
		std::cout << "Reading ordering file: " << orderFile << "\n";
		size_t ordersize;
		orderIStream >> ordersize;
		assert(ordersize == order.size());
		for (size_t i = 0; i < order.size(); ++i) {
			size_t tmp;
			orderIStream >> tmp;
			order[i] = tmp;
		};
		orderIStream.close();
		// double check, whether the order is valid  (SUM variables are eliminated first)
		if (maxVars.size() > 0) {
			bool maxStart = false;
			for (size_t i = 0; i < ordersize; ++i) {
				if (maxVars.contains(wmbUB.var(order[i]))) {
					maxStart = true;
				} else {
					if (maxStart) {
						std::cout
								<< "Invalid order: a SUM variable eliminated later than some MAX variable!"
								<< std::endl;
						return 1;
					}
				}
			}
		}
	} else {
		// current implementation of MMAP: input order file should be given
		if (task == Task::MMAP) {
			std::cout << "order file should be specified for MMAP task!"
					<< std::endl;
			return 1;
		}

		// Otherwise, calculate elimination order(s) ////////////////////////////////////
		double startOrder = timeSystem();
		iOrder = 0;
		// Try to build new orders until time or count limit reached ////////////////////
		while (iOrder < nOrders && (timeSystem() - startOrder < timeOrder)) {
			score = wmbUB.order(mex::graphModel::OrderMethod::WtMinFill, order,
					nExtra, score);
			++iOrder;
		}

		// If we were given an ordering file name but no file, write our order out
		ofstream orderOStream;
		if (orderFile != NULL)
			orderOStream.open(orderFile);
		if (orderOStream.is_open()) {
			std::cout << "Writing elimination order to " << orderFile << "\n";
			orderOStream << order.size();
			for (size_t i = 0; i < order.size(); ++i)
				orderOStream << " " << order[i];
			orderOStream << "\n";
			orderOStream.close();
		}
	}

	InducedWidth = wmbUB.inducedWidth(order);
	std::cout << "Best order of " << iOrder << " has induced width "
			<< InducedWidth << ", score " << score << "\n";
	wmbUB.setOrder(order);
	if (isLbHeur) {
		wmbLB.setOrder(order);
	}
	/*** SET IBOUND ********************************************************************/
	if (isAndOr) {
		std::cout << "non-chain structured pseudo tree used!" << std::endl;
	} else {
		std::cout << "chain structured pseudo tree used!" << std::endl;
	}
	double mem = 0.0;
	/*	if (isAllMemControl > 0) {
	 iBound = 50;
	 }*/
	if (iBound >= InducedWidth) {
		iBound = InducedWidth;
	}

	wmbUB.setIBound(iBound);
	if (isLbHeur) {
		wmbLB.setIBound(iBound);
	}
	for (;; --iBound) {
		wmbUB.init(isAndOr);
		if (isLbHeur) {
			wmbLB.init(isAndOr);
		}
		wmbUB.setIBound(iBound);
		if (isLbHeur) {
			wmbLB.setIBound(iBound);
		}
		wmbUB.build();
		if (isLbHeur) {
			wmbLB.build();
		}
		if (heur == Heur::BOTH) {
			mem = wmbUB.memory() + wmbLB.memory();
		} else {
			mem = wmbUB.memory();
		}
		if (mem <= wmbMemLimit || iBound == 1)
			break;
		if (verbose > 1) {
			std::cout << "Projected memory too high (" << iBound << "=> " << mem
					<< "MB)\n";
		}
	}
	std::cout << "The actual ibound used is: " << iBound << " => " << mem
			<< " MB" << std::endl;
	isSolved = iBound >= InducedWidth;
	if (isAllMemControl > 0) {
		searchMemLimit = memoryBudget - mem;
	}

	wmbUB.setTheta();
	if (method == Method::MMAPIS) {
		wmbUB.setMsgFwdRep(sumVarCopies);
	}

	if (isLbHeur) {
		wmbLB.setTheta();
		if (method == Method::MMAPIS)
			wmbLB.setMsgFwdRep(sumVarCopies);
	}
	switch (task) {
	case Task::PR:
		wmbUB.setElimType(mex::wmbe::ElimType::SumUpper);
		if (isLbHeur) {
			wmbLB.setElimType(mex::wmbe::ElimType::SumLower);
		}
		break;
	case Task::MMAP:
//		wmbUB.setElimType(mex::wmbe::ElimType::MaxSumUpper);
		// set elimination type for each variable
		for (size_t v = 0; v < wmbUB.nvar(); ++v) {
			auto var = wmbUB.var(v);
//			if (maxVars.contains(var))
			// note that for MMAPIS, weights for MAX variables still sums to 1.
			if ((method != Method::MMAPIS) && maxVars.contains(var)) {
				// MAX variable
				wmbUB.setElimType(var, mex::wmbe::ElimType::MaxUpper);
			} else {
				// SUM variable
				wmbUB.setElimType(var, mex::wmbe::ElimType::SumUpper);
			}
		}
		break;
	default:
		std::cout << "Task not supported\n";
		return 1;
	}

	//wmb.dump(std::cout);

	/*** RUN ITERATIONS ****************************************************************/

	double startIter = timeSystem();
	double UB = 0.0, LB = 0.0;
	size_t it = 0;
	assert(it < stopIter);

	//Caveat: we cannot do backward message passing in my current implementation
	if (method == Method::MMAPIS)
		stopIter = 1;

	while (true) {
		++it;
//	for (size_t it = 0; it < stopIter && (timeSystem() - startIter < stopTime); ++it)
		UB = 0.0;
		LB = 0.0;
		for (size_t b = 0; b < wmbUB.getOrder().size(); ++b) {
			if (method == Method::MMAPIS)
				UB += wmbUB.msgForwardAug(b, dampTheta, stepWeights,
						sumVarCopies);
			else
				UB += wmbUB.msgForward(b, dampTheta, stepWeights);
		}
		if (isLbHeur) {
			for (size_t b = 0; b < wmbLB.getOrder().size(); ++b) {
				if (method == Method::MMAPIS)
					LB += wmbLB.msgForwardAug(b, dampTheta, stepWeights,
							sumVarCopies);
				else
					LB += wmbLB.msgForward(b, dampTheta, stepWeights);
			}
		}

		//if (isAdaptive && obj > last) { dampTheta /= 2; stepWeights /= 2; }
		//last = obj;
		std::cout.precision(10);
		std::cout << "[" << std::setw(10) << timeSystem() - startIter << "] : "
				<< std::setw(10) << UB << " > ln Z > " << std::setw(10) << LB
				<< std::endl;
		// make sure we end up with forward message passing
		if (it == stopIter || timeSystem() - startIter > stopTime) {
//			std::cout << "Done " << it << " iterations of forward message passing!" << std::endl;
			break;
		}
		/*		if (timeSystem() - startIter > stopTime)
		 break;*/

		// I have not implemented "msgBackward" for "mmapIS"
		for (int b = wmbUB.getOrder().size() - 1; b >= 0; --b)
			wmbUB.msgBackward(b, dampTheta, stepWeights);
		if (isLbHeur) {
			for (int b = wmbLB.getOrder().size() - 1; b >= 0; --b)
				wmbLB.msgBackward(b, dampTheta, stepWeights);
		}
	}

	//wmbUB.dump(std::cout);
	//
	std::cout << "memory limit for WMB is: " << wmbMemLimit
			<< " (MB), actual used: " << mem << " (MB)!" << std::endl;
	startIter = timeSystem(); // just for search

	/*** Run Algorithms ****************************************************************/

	// task
	if (task == Task::MMAP) {
		searchTime = timeBudget - (timeSystem() - processEnteringTime);
		if (method == Method::MMAPIS) {
			std::cout << "[" << std::setw(10)
					<< timeSystem() - processEnteringTime
					<< "] : Run into MMAPIS..." << std::endl;
//			mmapIS(mex::wmbe& wmbeub, double ub, double delta, int vb, double tbgt, double mbgt,
//					int nv, bool isSolved, int K, const mex::VarSet& maxVars, int nmax);
			mmapIS mmapisObj(wmbUB, UB, DELTA, verbose, searchTime,
					searchMemLimit, wmbUB.nvar(), isSolved, sumVarCopies,
					maxVars, maxVars.size());
			mmapisObj.start(NSP, NND);
		} else {
			// default: UBFS
			mmap mmapObj(wmbUB, verbose, wmbUB.nvar(), wmbUB.nMaxVar(), UB,
					searchMemLimit, searchTime);
			mmapObj.start();
		}
		return 0;
	}
	//

//	if ((method == Method::AND_OR) || (method == Method::OR))
	if (method == Method::AOBFS) {
		if (isAllTimeControl > 0) {
			searchTime = timeBudget - (timeSystem() - processEnteringTime);
			if (searchTime < 0) {
				std::cout << "no time for search!" << std::endl;
				return -1;
			}
		}
		if (searchMemLimit < 0) {
			std::cout << "no memory for search!" << std::endl;
			return -1;
		}
		if (isAndOr) {
			std::cout << "[" << std::setw(10)
					<< timeSystem() - processEnteringTime
					<< "] : Entering AND/OR search..." << std::endl;
		} else {
			std::cout << "[" << std::setw(10)
					<< timeSystem() - processEnteringTime
					<< "] : Entering OR search..." << std::endl;
		}
		wmbsearch andOrSearch(wmbLB, wmbUB, verbose, priorityName, startIter,
				timeUnit, timeRatio, LB, UB, wmbUB.nvar(), searchMemLimit,
				searchTime);
//		andOrSearch.start(searchTime, nSearch, pstName,
//				addEpsilon, probName, searchMemLimit);
		andOrSearch.startSearch(searchMode);
	} else if (method == Method::MCTS) {
		// mcts(const mex::wmbe& wmbeub, const mex::wmbe& wmbelb, int ns, double DELTA, double ub, double lb)
		std::cout << "run into mcts..." << endl;
		mcts mctsOR(wmbUB, wmbLB, nSample, DELTA, UB, LB);
//		mctsOR.start();
//		mctsOR.startTwoStageSampling();
		if (NSP < 0) {
//			mctsOR.startTwoStageSamplingWithFixedTree(searchTime, searchMemLimit, timeBudget);
			mctsOR.startTwoStageSamplingWithFixedTree(searchTime, memoryBudget,
					timeBudget);
		} else
			mctsOR.startTwoStageSamplingWithFixedTree(searchTime,
					searchMemLimit, timeBudget, NSP);
//			mctsOR.startGBFSWithNewGap(searchTime, searchMemLimit, timeBudget, NSP); // temporary use
	} else if (method == Method::SESA) {
		std::cout << "[" << std::setw(10) << timeSystem() - processEnteringTime
				<< "] : Run into searchsample..." << std::endl;
		searchsample SS(wmbUB, UB, nSample, DELTA, verbose,
				timeBudget - (timeSystem() - processEnteringTime),
				searchMemLimit, wmbUB.nvar(), isSolved);
		/*		if (nSearch > 0) {
		 SS.start(nSearch);
		 } else {
		 SS.start();
		 }*/
		SS.start(NSP, NND);
	} else if (method == Method::RDHR) {
		std::cout << "[" << std::setw(10) << timeSystem() - processEnteringTime
				<< "] : Run into RDHR..." << std::endl;
		//randheur(mex::wmbe& wmbeub, double ub, double delta, int vb, double tbgt, double mbgt, int nv, bool isSolved, int ksp)
		if (lookahead_depth > nvar || lookahead_depth < 0) {
			// 0 for adaptive lookahead depth
			lookahead_depth = nvar;
		}
		randheur SS(wmbUB, UB, DELTA, verbose,
				timeBudget - (timeSystem() - processEnteringTime),
				searchMemLimit, wmbUB.nvar(), isSolved, KSP, lookahead_depth);
		SS.start(NSP, NND);
	} else {
		std::cout << "Method not supported" << endl;
		return 1;
	}
	//
	return 0;
}
// EOF


//
// Copyright (c) 2016,2018 CNRS
//

#ifndef __pinocchio_joint_generic_hpp__
#define __pinocchio_joint_generic_hpp__

#include "pinocchio/multibody/joint/joint-collection.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
#include "pinocchio/container/aligned-vector.hpp"

#include <boost/mpl/contains.hpp>

namespace pinocchio
{

  template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
  struct JointTpl;
  typedef JointTpl<double> Joint;

  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointTpl<_Scalar,_Options,JointCollectionTpl> >
  {
    enum {
      Options = _Options,
      NQ = Eigen::Dynamic, // Dynamic because unknown at compile time
      NV = Eigen::Dynamic
    };

    typedef _Scalar Scalar;
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;

    typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionTpl<Scalar,Options>  Motion_t;
    typedef MotionTpl<Scalar,Options>  Bias_t;

    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> F_t;
    // [ABA]
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;

    typedef Constraint_t ConstraintTypeConstRef;
    typedef Transformation_t TansformTypeConstRef;
    typedef Motion_t MotionTypeConstRef;
    typedef Bias_t BiasTypeConstRef;
    typedef U_t UTypeConstRef;
    typedef U_t UTypeRef;
    typedef D_t DTypeConstRef;
    typedef UD_t UDTypeConstRef;

    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
  };

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointDataTpl<Scalar,Options,JointCollectionTpl> >
  { typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointModelTpl<Scalar,Options,JointCollectionTpl> >
  { typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };

  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct JointDataTpl
  : public JointDataBase< JointDataTpl<_Scalar,_Options,JointCollectionTpl> >
  , JointCollectionTpl<_Scalar,_Options>::JointDataVariant
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
    typedef JointDataBase<JointDataTpl> Base;

    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE;

    typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
    typedef typename JointCollection::JointDataVariant JointDataVariant;

    JointDataVariant & toVariant() { return *static_cast<JointDataVariant*>(this); }
    const JointDataVariant & toVariant() const { return *static_cast<const JointDataVariant*>(this); }

    Constraint_t      S() const  { return constraint_xd(*this); }
    Transformation_t  M() const  { return joint_transform(*this); }
    Motion_t          v() const  { return motion(*this); }
    Bias_t            c() const  { return bias(*this); }

    // [ABA CCRBA]
    U_t               U()     const { return u_inertia(*this); }
    D_t               Dinv()  const { return dinv_inertia(*this); }
    UD_t              UDinv() const { return udinv_inertia(*this); }

    JointDataTpl() : JointDataVariant() {}

    JointDataTpl(const JointDataVariant & jdata_variant)
    : JointDataVariant(jdata_variant)
    {}

    template<typename JointDataDerived>
    JointDataTpl(const JointDataBase<JointDataDerived> & jdata)
    : JointCollection::JointDataVariant((JointDataVariant)jdata.derived())
    {
      BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types,JointDataDerived>));
    }

    /// Define all the standard accessors
    Constraint_t S_accessor() const { return S(); }
    Transformation_t M_accessor() const { return M(); }
    Motion_t v_accessor() const { return v(); }
    Bias_t c_accessor() const { return c(); }
    U_t U_accessor() const { return U(); }
    D_t Dinv_accessor() const { return Dinv(); }
    UD_t UDinv_accessor() const { return UDinv(); }

    static std::string classname() { return "JointData"; }
    std::string shortname() const { return ::pinocchio::shortname(*this); }

  };

  template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct CastType< NewScalar, JointModelTpl<Scalar,Options,JointCollectionTpl> >
  {
    typedef JointModelTpl<NewScalar,Options,JointCollectionTpl> type;
  };

  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct JointModelTpl
  : JointModelBase< JointModelTpl<_Scalar,_Options,JointCollectionTpl> >
  , JointCollectionTpl<_Scalar,_Options>::JointModelVariant
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;

    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE;
    PINOCCHIO_JOINT_USE_INDEXES;

    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    typedef typename JointCollection::JointDataVariant JointDataVariant;
    typedef typename JointCollection::JointModelVariant JointModelVariant;

    using Base::id;
    using Base::setIndexes;
    using Base::operator==;
    using Base::operator!=;

    JointModelTpl() : JointModelVariant() {}

    JointModelTpl(const JointModelVariant & jmodel_variant)
    : JointCollection::JointModelVariant(jmodel_variant)
    {}

    template<typename JointModelDerived>
    JointModelTpl(const JointModelBase<JointModelDerived> & jmodel)
    : JointModelVariant((JointModelVariant)jmodel.derived())
    {
      BOOST_MPL_ASSERT((boost::mpl::contains<typename JointModelVariant::types,JointModelDerived>));
    }

    JointModelVariant & toVariant()
    { return *static_cast<JointModelVariant*>(this); }

    const JointModelVariant & toVariant() const
    { return *static_cast<const JointModelVariant*>(this); }

    JointDataDerived createData() const
    { return ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(*this); }

    using Base::isEqual;
    bool isEqual(const JointModelTpl & other) const
    {
      return Base::isEqual(other)
      && toVariant() == other.toVariant();
    }

    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const Eigen::MatrixBase<ConfigVector> & q) const
    { calc_zero_order(*this,data,q); }

    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const Eigen::MatrixBase<ConfigVector> & q,
              const Eigen::MatrixBase<TangentVector> & v) const
    { calc_first_order(*this,data,q,v); }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
    { ::pinocchio::calc_aba(*this,data,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),update_I); }

    std::string shortname() const { return ::pinocchio::shortname(*this); }
    static std::string classname() { return "JointModel"; }

    int     nq_impl() const { return ::pinocchio::nq(*this); }
    int     nv_impl() const { return ::pinocchio::nv(*this); }

    int     idx_q()   const { return ::pinocchio::idx_q(*this); }
    int     idx_v()   const { return ::pinocchio::idx_v(*this); }

    JointIndex     id()      const { return ::pinocchio::id(*this); }

    void setIndexes(JointIndex id, int nq, int nv)
    { ::pinocchio::setIndexes(*this,id, nq, nv); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelTpl<NewScalar,Options,JointCollectionTpl> cast() const
    {
      return cast_joint<NewScalar,Scalar,Options,JointCollectionTpl>(*this);
    }
  };

  typedef container::aligned_vector<JointData> JointDataVector;
  typedef container::aligned_vector<JointModel> JointModelVector;

} // namespace pinocchio

#endif // ifndef __pinocchio_joint_generic_hpp__


/*****************************************************************************
 * Copyright (c) 2018, Toyota Research Institute.
 * 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 copyright holder 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 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.
 *****************************************************************************/

/**
 * @file
 *
 * Test a Drake camera simulation that has been linked with PCL.
 */

#include <cmath>
#include <iostream>
#include <memory>
#include <utility>

#include <Eigen/Core>

// Test Eigen header presence (fails if Drake's Eigen w/ additional Autodiff
// support is not utilised).
#include <drake/common/autodiff.h>  // IWYU pragma: keep
#include <drake/multibody/rigid_body_plant/rigid_body_plant.h>
#include <drake/multibody/rigid_body_tree.h>
#include <drake/multibody/rigid_body_tree_construction.h>
#include <drake/systems/analysis/simulator.h>
#include <drake/systems/framework/diagram_builder.h>
#include <drake/systems/sensors/rgbd_camera.h>

namespace shambhala {

int main() {
  // Creates a simple discrete RGB-D camera simulation.
  drake::systems::DiagramBuilder<double> builder;

  auto tree = std::make_unique<RigidBodyTree<double>>();
  drake::multibody::AddFlatTerrainToWorld(tree.get());
  auto plant = builder.AddSystem<drake::systems::RigidBodyPlant<double>>(
      std::move(tree));

  auto rgbd_camera =
      builder.AddSystem<drake::systems::sensors::RgbdCameraDiscrete>(
          std::make_unique<drake::systems::sensors::RgbdCamera>(
              "rgbd_camera", plant->get_rigid_body_tree(),
              Eigen::Vector3d(-1., 0., 1.), Eigen::Vector3d(0., M_PI_4, 0.),
              0.5, 5., M_PI_4, false),
          0.03);

  builder.Connect(plant->get_output_port(0), rgbd_camera->state_input_port());
  auto diagram = builder.Build();
  auto simulator =
      std::make_unique<drake::systems::Simulator<double>>(*diagram);
  simulator->StepTo(0.1);

  // TODO(eric.cousineau): Add in example of converting depth image to PCL
  // point cloud.

  std::cout << "Finished camera simulation." << std::endl;
  return 0;
}

}  // namespace shambhala

int main() { return shambhala::main(); }


/**
 * @file shader_object.cc
 * @author Daeyun Shin <daeyun@dshin.org>
 * @version 0.1
 * @date 2015-01-02
 * @copyright librender is free software released under the BSD 2-Clause
 * license.
 */
#include "shader_object.h"

#include <exception>
#include <armadillo>
#include <vector>
#include <boost/algorithm/string.hpp>
#include "shader.h"
#include "graphics.h"
#include "databuffer.h"
#include "matrix_util.h"
#include "debug.h"

namespace librender {
namespace shader {

/**
 * @brief Copy data to data buffers and attach them to the vertex array object.
 */
void ShaderObject::SetupVAO(const Shape* shape) {
  glBindVertexArray(vertex_array_id);

  position_buffer = new VertexAttribBuffer(
      GL_ARRAY_BUFFER, DataBufferLocation::kVertex, shape->v.n_rows, GL_FLOAT,
      &shape->v[0], shape->v.n_elem * sizeof(float));

  if (!shape->vn.empty()) {
    normal_buffer = new VertexAttribBuffer(
        GL_ARRAY_BUFFER, DataBufferLocation::kVertexNormal, shape->vn.n_rows,
        GL_FLOAT, &shape->vn[0], shape->vn.n_elem * sizeof(float));
  }

  if (!shape->vc.empty()) {
    color_buffer = new VertexAttribBuffer(
        GL_ARRAY_BUFFER, DataBufferLocation::kVertexColor, shape->vc.n_rows,
        GL_FLOAT, &shape->vc[0], shape->vc.n_elem * sizeof(float));
  }

  if (!shape->uv.empty()) {
    texture_buffer = new VertexAttribBuffer(
        GL_ARRAY_BUFFER, DataBufferLocation::kVertexTexCoord, shape->uv.n_rows,
        GL_FLOAT, &shape->uv[0], shape->uv.n_elem * sizeof(float));
  }

  GLenum draw_mode;
  switch (shape->type) {
    case ShapeType::kTriangles:
      draw_mode = GL_TRIANGLES;
      break;
    case ShapeType::kLines:
      draw_mode = GL_LINES;
      break;
    case ShapeType::kPoints:
      draw_mode = GL_POINTS;
      break;
  }

  index_buffer = new IndexBuffer(
      GL_ELEMENT_ARRAY_BUFFER, draw_mode, shape->ind.n_elem, GL_UNSIGNED_INT,
      &shape->ind[0], shape->ind.n_elem * sizeof(arma::uword));
}

/**
 * @brief Updates the vertex positions and normals.
 */
void ShaderObject::UpdateVAO(const glm::mat4& mv, const glm::mat3& normal_mat) {
  if (!shape->v.empty()) {
    auto vertex_view = librender::util::MatMult(mv, shape->v);
    void* data_ptr = vertex_view.memptr();
    size_t data_size = vertex_view.n_elem * sizeof(vertex_view[0]);
    position_buffer->SetBufferData(data_ptr, data_size);
  }

  if (!shape->vn.empty()) {
    auto vertex_view = librender::util::MatMult(normal_mat, shape->v);
    void* data_ptr = vertex_view.memptr();
    size_t data_size = vertex_view.n_elem * sizeof(vertex_view[0]);
    normal_buffer->SetBufferData(data_ptr, data_size);
  }
}

/**
 * @brief
 * @param[in] shader_id
 * @param[in,out] shader_globals
 */
void ShaderObject::FillUniformLocations(
    GLuint shader_id,
    std::unordered_map<string, ShaderAttribute>& shader_globals) {
  this->matrices.model_view_projection =
      glGetUniformLocation(shader_id, "iModelViewProjectionMatrix");
  this->matrices.model_view =
      glGetUniformLocation(shader_id, "iModelViewMatrix");
  this->matrices.vector_model_view =
      glGetUniformLocation(shader_id, "iVectorModelViewMatrix");
  this->matrices.projection =
      glGetUniformLocation(shader_id, "iProjectionMatrix");

  for (auto& kv : shader_globals) {
    kv.second.uniform_location =
        glGetUniformLocation(shader_id, kv.first.c_str());
  }
}

ShaderObject::~ShaderObject() {
  if (position_buffer) {
    delete position_buffer;
  }
  if (normal_buffer) {
    delete normal_buffer;
  }
  if (color_buffer) {
    delete color_buffer;
  }
  if (index_buffer) {
    delete index_buffer;
  }

  glDeleteVertexArrays(1, &this->vertex_array_id);
}

void ShaderObject::Draw(const RenderParams& render_params, bool is_view_space) {
  glBindVertexArray(this->vertex_array_id);
  glUseProgram(this->shader_id);

  GLint is_linked;
  int log_length;
  glGetProgramiv(this->shader_id, GL_LINK_STATUS, &is_linked);
  glGetProgramiv(this->shader_id, GL_INFO_LOG_LENGTH, &log_length);
  if (log_length > 0 || !is_linked) {
    std::vector<char> error_message(log_length + 1);
    glGetProgramInfoLog(this->shader_id, log_length, nullptr,
                        &error_message[0]);
    throw std::runtime_error(&error_message[0]);
  }

  mat4 mv = render_params.shader_params.view_mat *
            render_params.shader_params.model_mat;
  mat4 mvp = render_params.shader_params.projection_mat * mv;
  // inverse transpose of the upper left 3x3 submatrix of the modelview matrix
  mat3 normal_mat = mat3(glm::transpose(glm::inverse(mv)));

  if (is_view_space) {
    UpdateVAO(mv, normal_mat);
    for (size_t i = 0; i < render_params.shader_params.lights.size(); ++i) {
      this->light_positions[i] = util::MatMult(
          mv, render_params.shader_params.lights[i].light_position);
    }
  }

  glUniformMatrix4fv(this->matrices.model_view_projection, 1, GL_FALSE,
                     &mvp[0][0]);
  glUniformMatrix4fv(this->matrices.model_view, 1, GL_FALSE, &mv[0][0]);
  glUniformMatrix4fv(this->matrices.projection, 1, GL_FALSE,
                     &render_params.shader_params.projection_mat[0][0]);
  glUniformMatrix3fv(this->matrices.vector_model_view, 1, GL_FALSE,
                     &normal_mat[0][0]);

  for (auto& kv : this->attribs) {
    switch (kv.second.type) {
      case AttribType::kInt:
        glUniform1i(kv.second.uniform_location, kv.second.data_int);
        break;
      case AttribType::kFloat:
        glUniform1f(kv.second.uniform_location, kv.second.data_float);
        break;
      case AttribType::kVec3:
        glUniform3fv(kv.second.uniform_location, 1,
                     (const float*)kv.second.data_pointer);
        break;
      case AttribType::kVec4:
        glUniform4fv(kv.second.uniform_location, 1,
                     (const float*)kv.second.data_pointer);
        break;
      default:
        throw std::runtime_error("Unreconized shader attribute type");
        break;
    }
  }

  this->index_buffer->draw();
}
}
}


/* Author: Mincheul Kang */

#include <boost/chrono.hpp>
#include <boost/foreach.hpp>
#include <ompl/base/SpaceInformation.h>

#include <harmonious_sampling/ManipulationRegion.h>

MainpulationRegion::MainpulationRegion(planning_scene::PlanningScenePtr& planning_scene,
                                       std::string planning_group,
                                       std::string collision_check_group,
                                       HarmoniousParameters &params,
                                       std::vector<harmonious_msgs::BaseConf> &base_confs,
                                       std::vector<harmonious_msgs::JointConf> &joint_confs):
        base_confs_(base_confs),
        joint_confs_(joint_confs),
        params_(params)
{
    planning_scene_ = planning_scene;
    planning_group_ = planning_group; // base_group

    collision_request_.group_name = collision_check_group;
    collision_request_.contacts = true;
    collision_request_.max_contacts = 100;
    collision_request_.max_contacts_per_pair = 1;
    collision_request_.verbose = false;

    regions_ = new bool**[params_.reg_index_[0]];
    for (uint i = 0; i < params_.reg_index_[0]; i++){
        regions_[i] = new bool*[params_.reg_index_[1]];
        for (uint j = 0; j < params_.reg_index_[1]; j++){
            regions_[i][j] = new bool[params_.reg_index_[2]];
        }
    }

    for(uint i = 0; i < params_.reg_index_[0]; i++){
        for(uint j = 0; j < params_.reg_index_[1]; j++) {
            for (uint k = 0; k < params_.reg_index_[2]; k++) {
                regions_[i][j][k] = false;
            }
        }
    }
}

MainpulationRegion::~MainpulationRegion(){
    for(uint i=0; i<params_.reg_index_[0]; i++) {
        for(uint j=0; j<params_.reg_index_[1]; j++)
            delete [] regions_[i][j];
        delete [] regions_[i];
    }
    delete [] regions_;
}

bool MainpulationRegion::isValid(harmonious_msgs::BaseConf &bp) {
    std::map<std::string, double> configuration;
    collision_detection::CollisionResult collision_result;
    std::vector<double> values;

    values.push_back(bp.b_c[0]);
    values.push_back(bp.b_c[1]);
    values.push_back(bp.b_c[2]);

    collision_result.clear();

    robot_state::RobotState robot_state = planning_scene_->getCurrentStateNonConst();
    robot_state.setJointGroupPositions(planning_group_, values);
    planning_scene_->checkCollision(collision_request_, collision_result, robot_state);

    return !collision_result.collision;
}

void MainpulationRegion::generate_regions(ompl::base::RealVectorBounds &bounds){
    for (uint i = 0; i < base_confs_.size(); i++){
        if(base_confs_[i].b_c[0] < bounds.low[0] || base_confs_[i].b_c[0] > bounds.high[0]){
            joint_confs_[i].j_c.clear();
            continue;
        }
        if(base_confs_[i].b_c[1] < bounds.low[1] || base_confs_[i].b_c[1] > bounds.high[1]){
            joint_confs_[i].j_c.clear();
            continue;
        }

        if(isValid(base_confs_[i])){
            regions_[int(((base_confs_[i]).b_c[0]-params_.bounds_.low[0]) * params_.resolution_)]
            [int(((base_confs_[i]).b_c[1]-params_.bounds_.low[1]) * params_.resolution_)]
            [int(((base_confs_[i]).b_c[2]-params_.bounds_.low[2]) * params_.res_yaw_)] = true;
        }
    }
}


/*
 * MCSat.cpp
 *
 *  Created on: Feb 8, 2012
 *      Author: selman.joe@gmail.com
 */

#include <cmath>
#include <boost/random/bernoulli_distribution.hpp>
#include "MCSat.h"
#include "MaxWalkSat.h"
#include "../logic/UnitProp.h"
#include "../logic/Domain.h"
#include "../logic/syntax/ELSentence.h"

const unsigned int MCSat::defNumSamples = 1000;
const unsigned int MCSat::defBurnInIterations = 1000;
const unsigned int MCSat::defWalksatIterations = 1000;
const double MCSat::defWalksatRandomMoveProb = 0.2;
const unsigned int MCSat::defWalksatNumRandomRestarts = 4;
const bool MCSat::defUseRandomInitialModels = true;
const bool MCSat::defUseUnitPropagation = true;

void MCSat::run(boost::mt19937& rng) { // TODO: setup using random initial models
    if (d_ == 0) {
        throw std::logic_error("MCSat::run() - Domain not set");
    }
    if (sampleStrategy_ == 0) {
        throw std::logic_error("MCSat::run() - SampleStrategy not set");
    }
    samples_.clear();
    samples_.reserve(numSamples_);

    //std::cout << "initial domain: ";
    //d_->printDebugDescription(std::cout);

    // first, run unit propagation on our domain to get a new reduced one.
    Domain reduced;
    if (useUnitPropagation_) {
        reduced = MCSat::applyUP(*d_);
    } else {
        reduced = *d_;
    }
    //std::cout << "reduced domain: ";
    //reduced.printDebugDescription(std::cout);

    Model prevModel = (useRandomInitialModels_ ? reduced.randomModel(rng) : reduced.defaultModel());

    // do a starting run on the whole problem as our initial sample
    //boost::unordered_set<Model> initModels = sampleSat(prevModel, reduced);
    //prevModel = *initModels.begin();
    Domain prevDomain = reduced;

    if (burnInIterations_ == 0) samples_.push_back(prevModel);
    unsigned int totalIterations = numSamples_+burnInIterations_;

    for (unsigned int iteration = 1; iteration < totalIterations; iteration++) {
        std::vector<ELSentence> newSentences;

        if ( totalIterations < 20 ||
                iteration % (totalIterations / 20) == 0) {
            std::cout << (((double)iteration) / ((double) totalIterations))*100 << "% done." << std::endl;
        }

        sampleStrategy_->sampleSentences(prevModel, reduced, rng, newSentences);
//        if (iteration == 1) {
//            std::cout << "initial sampled sentences: ";
//            std::copy(newSentences.begin(), newSentences.end(), std::ostream_iterator<ELSentence>(std::cout, "\n"));
//            std::cout << "initial model:";
//            std::cout << prevModel;
//            for (Domain::formula_const_iterator it = prevDomain.formulas_begin();
//                    it != prevDomain.formulas_end();
//                    it++) {
//                std::cout << "formula " << *it << " is satisfied at: " << it->dSatisfied(prevModel, prevDomain) << std::endl;
//            }
//            std::cin.get();
//        }

        // make a new domain using new Sentences
        Domain curDomain;
        curDomain.setMaxInterval(prevDomain.maxInterval());
        for (Domain::fact_const_iterator it = prevDomain.facts_begin(); it != prevDomain.facts_end(); it++) {
            curDomain.addFact(*it);
        }
        for (std::vector<ELSentence>::const_iterator it = newSentences.begin(); it != newSentences.end(); it++) {
            curDomain.addFormula(*it);
        }
        curDomain.addAtoms(prevDomain.atoms_begin(), prevDomain.atoms_end());
//
//        if (iteration == burnInIterations_ + numSamples_/2) {
//            std::cout << "ITERATION: " << iteration << std::endl;
//            std::cout << "curDomain";
//            curDomain.printDebugDescription(std::cout);
//            std::cout << "sampled sentences: ";
//            std::copy(newSentences.begin(), newSentences.end(), std::ostream_iterator<ELSentence>(std::cout, "\n"));
//        }

        boost::unordered_set<Model> curModels = sampleSat(prevModel, curDomain, rng);
        assert(!curModels.empty());
        // choose a random model
        boost::uniform_int<std::size_t> pickModel(0, curModels.size()-1);
        boost::unordered_set<Model>::size_type index = pickModel(rng);
        boost::unordered_set<Model>::const_iterator it = curModels.begin();
        while (index > 0) {
            it++;
            index--;
        }
        // add the model
        if (iteration >= burnInIterations_) samples_.push_back(*it);
        prevModel = *it;
        prevDomain = curDomain;
    }
}

Domain MCSat::applyUP(const Domain& d) {
    Domain reduced;
    try {
        reduced = performUnitPropagation(d);
    } catch (contradiction& c) {
        // rewrite error message
        throw contradiction("Contradiction found in MCSat::run() when running unit prop()");
    }

    // default model is guaranteed to satisfy the facts
    Model m = reduced.defaultModel();
    // check to make sure hard clauses are satisfied
    std::vector<ELSentence> hardClauses;
    std::remove_copy_if(reduced.formulas_begin(), reduced.formulas_end(), std::back_inserter(hardClauses), std::not1(IsHardClausePred()));
    for (std::vector<ELSentence>::const_iterator it = hardClauses.begin(); it != hardClauses.end(); it++) {
        if (!it->fullySatisfied(m, reduced)) {
            throw contradiction("Contradiction found in MCSat::run() when verifying hard clauses are satisfied");
        }
    }
    return reduced;
}

/*

void MCSatSampleSegmentsStrategy::sampleSentences(const Model& m, const Domain& d, boost::mt19937& rng, std::vector<ELSentence>& sampled) {
    for (Domain::formula_const_iterator it = d.formulas_begin(); it != d.formulas_end(); it++) {
        ELSentence currentSentence = *it;

        // if it has hard weight, it must be included
        if (currentSentence.hasInfWeight()) {
            sampled.push_back(currentSentence);
            continue;
        }
        SISet satisfied = currentSentence.dSatisfied(m, d);
        if (satisfied.empty()) continue;

        // find the segments for this sentence
        boost::unordered_set<SpanInterval> segments = formulaToSegment_.at(currentSentence.sentence().get());
        SISet toEnforceAt(false, d.maxInterval());
        for (boost::unordered_set<SpanInterval>::const_iterator sIt = segments.begin(); sIt != segments.end(); sIt++) {
            // first, require that the sentence be fully satisfied at the spanning interval
            if (!satisfied.includes(*sIt)) continue;
            // sample it.  with probability 1 - e^-score, add it to be enforced
            double prob = 1.0 - exp((double)-(currentSentence.weight()*sIt->size()));
            boost::bernoulli_distribution<double> flip(prob);
            bool addit = flip(rng);
            //bool addit = (((double)rand()) / ((double)RAND_MAX)) <= prob;
            if (addit) toEnforceAt.add(*sIt);
        }
        if (!toEnforceAt.empty()) {
            currentSentence.setQuantification(toEnforceAt);
            sampled.push_back(currentSentence);
        }
    }
}
*/

boost::unordered_set<Model> MCSat::sampleSat(const Model& initialModel, const Domain& d, boost::mt19937& rng) {
    boost::unordered_set<Model> models;
    models.insert(initialModel); // always include the initial model

    // transform domain into a SAT problem
    Domain dSat;
    for (Domain::fact_const_iterator it = d.facts_begin(); it != d.facts_end(); it++) {
        dSat.addFact(*it);
    }
    for (Domain::formula_const_iterator it = d.formulas_begin(); it != d.formulas_end(); it++) {
        ELSentence s = *it;
        s.setHasInfWeight(true);
        dSat.addFormula(s);
    }
    dSat.addAtoms(d.atoms_begin(), d.atoms_end());
    // perform UP (if possible)
    Domain reduced;
    if (useUnitPropagation_) {
        try {
            reduced = MCSat::applyUP(dSat);
        } catch (contradiction& c) {
            return models;  // can't continue, just return our models which has only one item.
        }
    } else {
        reduced = dSat;
    }

    // rewrite infinite weighted formulas so they have singular weight
    reduced = reduced.replaceInfForms();

    // do some random restarts and hope for different models
    MWSSolver walksatSolver(walksatIterations_, walksatRandomMoveProb_, &reduced);
    for (unsigned int i = 1; i <= walksatNumRandomRestarts_; i++) {
        Model iterInitModel = reduced.randomModel(rng);   // TODO: better way to make random models
        if (reduced.formulas_size() == 0) {
            // just add the random model and continue
            models.insert(reduced.randomModel(rng));
            continue;
        }

        //std::cout << "--\nFormulas for maxwalksat: ";
        //std::copy(reduced.formulas_begin(), reduced.formulas_end(), std::ostream_iterator<ELSentence>(std::cout, ", "));
        //std::cout << std::endl;

        Model iterModel = walksatSolver.run(rng, iterInitModel);
        if (reduced.isFullySatisfied(iterModel)) models.insert(iterModel);
    }
    return models;
}

double MCSat::estimateProbability(const Proposition& prop, const Interval& where) const {
    return ((double)countProps(prop, where)) / ((double) numSamples_);
}

unsigned int MCSat::countProps(const Proposition& prop, const Interval& where) const {
    assert(samples_.size() == numSamples_);
    unsigned int count = 0;

    for (std::vector<Model>::const_iterator it = samples_.begin(); it != samples_.end(); it++) {
        SISet trueAt = it->getAtom(prop.atom());
        if ((trueAt.includes(where) && prop.sign())
                || (!trueAt.includes(where) && !prop.sign())) {
            count++;
        }
    }

    return count;
}

/*
MCSatSampleSegmentsStrategy::MCSatSampleSegmentsStrategy(const Domain& d)
    : formulaToSegment_() {
    for (Domain::fact_const_iterator it = d.facts_begin(); it != d.facts_end(); it++) {
        Proposition p = it->first;
        Sentence *a = new Atom(p.atom()); // this should work for negation too,
                                        // since the segment collection is the same

        SISet sisetWhere = it->second;
        boost::unordered_set<SpanInterval> where;
        sisetWhere.collectSegments(std::inserter(where, where.end()));

        formulaToSegment_[a] = where;
    }
    for (Domain::formula_const_iterator it = d.formulas_begin(); it != d.formulas_end(); it++) {
        ELSentence s = *it;
        // collect all the atoms, union their sets and add it to the map
        AtomCollector atomCollect;

        s.sentence()->visit(atomCollect);
        assert(!atomCollect.atoms.empty());
        boost::unordered_set<SpanInterval> segments;
        for (boost::unordered_set<Atom>::const_iterator it2 = atomCollect.atoms.begin();
                it2 != atomCollect.atoms.end();
                it2++) {
            Atom atom = *it2;
            if (formulaToSegment_.count(&atom) == 0) continue;
            boost::unordered_set<SpanInterval> toMerge = formulaToSegment_.at(&atom);
            std::copy(toMerge.begin(), toMerge.end(), std::inserter(segments, segments.end()));
        }
        Sentence *sPtr = s.sentence()->clone();
        formulaToSegment_[sPtr] = segments;
    }
}

MCSatSampleSegmentsStrategy::MCSatSampleSegmentsStrategy(const MCSatSampleSegmentsStrategy& s)
    : formulaToSegment_() {
    for (boost::unordered_map<Sentence*, boost::unordered_set<SpanInterval> >::const_iterator it = s.formulaToSegment_.begin();
            it != s.formulaToSegment_.end();
            it++) {
        Sentence *sPtr = (it->first == 0 ? 0 : it->first->clone());
        formulaToSegment_.insert(std::make_pair(sPtr, it->second));
    }
}

MCSatSampleSegmentsStrategy::~MCSatSampleSegmentsStrategy() {
    std::vector<Sentence*> toDelete;
    for (boost::unordered_map<Sentence*, boost::unordered_set<SpanInterval> >::iterator it = formulaToSegment_.begin();
            it != formulaToSegment_.end();
            it++) {
        toDelete.push_back(it->first);
    }
    formulaToSegment_.clear();
    for (std::vector<Sentence*>::iterator it = toDelete.begin(); it != toDelete.end(); it++) {
        delete *it;
    }
    toDelete.clear();
}
*/

bool operator==(const MCSat& l, const MCSat& r) {
    return (
            (l.d_ == r.d_ || (l.d_ != NULL && r.d_ != NULL && *l.d_ == *r.d_) ) &&
            l.numSamples_ == r.numSamples_ &&
            l.burnInIterations_ == r.burnInIterations_ &&
            l.walksatIterations_ == r.walksatIterations_ &&
            l.walksatRandomMoveProb_ == r.walksatRandomMoveProb_ &&
            l.walksatNumRandomRestarts_ == r.walksatNumRandomRestarts_ &&
            l.useRandomInitialModels_ == r.useRandomInitialModels_ &&
            l.useUnitPropagation_ == r.useUnitPropagation_ &&
            l.samples_ == r.samples_ &&
            (l.sampleStrategy_ == r.sampleStrategy_ || (l.sampleStrategy_ != NULL && r.sampleStrategy_ != NULL && *l.sampleStrategy_ == *r.sampleStrategy_))
            );
//    ar & d_;
//    ar & numSamples_;
//    ar & burnInIterations_;
//    ar & walksatIterations_;
//    ar & walksatRandomMoveProb_;
//    ar & walksatNumRandomRestarts_;
//    ar & useRandomInitialModels_;
//    ar & samples_;
//    ar & sampleStrategy_;
}


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

/*
 Code adapted from:
 https://gist.githubusercontent.com/mtao/5798888/raw/5be9fa9b66336c166dba3a92c0e5b69ffdb81501/eigen_boost_serialization.hpp
 Copyright (c) 2015 Michael Tao

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

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

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

#ifndef EIGEN_BOOST_SERIALIZATION
#define EIGEN_BOOST_SERIALIZATION

#include <Eigen/Dense>
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>

namespace boost {
namespace serialization {
template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
void save(Archive& ar, const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m,
          const unsigned int version) {
  if (version) {
    // Do something
  }
  Eigen::DenseIndex rows(m.rows()), cols(m.cols());
  ar& BOOST_SERIALIZATION_NVP(rows);
  ar& BOOST_SERIALIZATION_NVP(cols);
  ar& make_nvp("data", make_array(m.data(), (size_t)m.size()));
}

template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
void load(Archive& ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m,
          const unsigned int version) {
  if (version) {
    // Do something
  }
  Eigen::DenseIndex rows, cols;
  ar >> BOOST_SERIALIZATION_NVP(rows);
  ar >> BOOST_SERIALIZATION_NVP(cols);
  m.resize(rows, cols);
  // if(m.size() > 0)
  ar >> make_nvp("data", make_array(m.data(), (size_t)m.size()));
}

template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
void serialize(Archive& ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m,
               const unsigned int version) {
  split_free(ar, m, version);
}

}  // namespace serialization
}  // namespace boost

#endif  // ifndef __multicontact_api_serialization_eigen_matrix_hpp__


//  Copyright John Maddock 2012.
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/integer.hpp>

typedef boost::uint_t<sizeof(boost::intmax_t)*CHAR_BIT + 1>::fast fail_uint_fast;


#include "covreg/ClipOptimizer.h"
#include "covreg/CovarianceEstimator.h"
#include "covreg/CovarianceEstimatorInfo.h"
#include "covreg/EstimatorInfoParsers.h"
#include "covreg/OptimizerStatus.h"

#include "argus_msgs/FilterStepInfo.h"

#include "argus_utils/utils/YamlUtils.h"
#include "argus_utils/utils/MatrixUtils.h"
#include "argus_utils/utils/ParamUtils.h"
#include "argus_utils/filters/FilterUtils.h"

#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_sequencer.h>
#include <signal.h>

#include "argus_utils/synchronization/SynchronizationTypes.h"
#include "argus_utils/synchronization/ThreadsafeQueue.hpp"

#include "broadcast/BroadcastReceiver.h"

#include <boost/bind.hpp>
#include <boost/thread/thread.hpp>

#include <fstream>

using namespace argus_msgs;
using namespace argus;
using namespace covreg;

class Optimizer
{
public:

	static const unsigned int qInputDim = 6;
	static const unsigned int qOutputDim = 18;
	static const unsigned int qHiddenLayers = 1;
	static const unsigned int qLayerWidth = 50;

	static const unsigned int gInputDim = 6;
	static const unsigned int gOutputDim = 3;
	static const unsigned int gHiddenLayers = 1;
	static const unsigned int gLayerWidth = 50;

	static const unsigned int voInputDim = 6;
	static const unsigned int voOutputDim = 3;
	static const unsigned int voHiddenLayers = 1;
	static const unsigned int voLayerWidth = 50;

	Optimizer( ros::NodeHandle& nodeHandle, ros::NodeHandle& ph )
	: privHandle( ph ),
	rxInitialized( false ),
	epsInitialized( false ),
	lastSeq( 0 )
	{
		GetParam<double>( privHandle, "feature_cache_time",
		                  _featureCacheTime, 1.0 );

		// Number of info updates to store max
		unsigned int infoBufferSize;
		GetParam<unsigned int>( privHandle, "info_buffer_size", infoBufferSize, 0 );
		_dataBuffer.SetMaxSize( infoBufferSize );

		std::string logPath;
		if( GetParam( privHandle, "output_log_path", logPath ) )
		{
			_logOutFile.open( logPath );
		}

		// Iterate over all models
		WriteLock lock( _initMutex );
		XmlRpc::XmlRpcValue modelsXml;
		GetParam( privHandle, "models", modelsXml );
		YAML::Node modelsYaml = XmlToYaml( modelsXml );
		YAML::Node::const_iterator iter;
		for( iter = modelsYaml.begin(); iter != modelsYaml.end(); ++iter )
		{
			const std::string& modelName = iter->first.as<std::string>();
			YAML::Node modelYaml = iter->second;
			RegisterModel( modelName, modelYaml );
		}

		if( _estRegistry.count( "transition" ) == 0 )
		{
			ROS_ERROR_STREAM( "Must specify transition model." );
			exit( -1 );
		}

		InnovationClipParameters clipParams;
		GetParam<unsigned int>( privHandle, "max_eps_length", clipParams.maxEpisodeLength, 100 );
		// GetParam<unsigned int>( privHandle, "max_eps_to_keep", clipParams.numEpisodesToKeep, 50 );
		GetParam<double>( privHandle, "l2_weight", clipParams.l2Weight, 1E-6 );
		GetParam<unsigned int>( privHandle, "batch_size", clipParams.batchSize, 30 );

		CovarianceEstimator& transReg = *(_estRegistry["transition"].estimator);
		_clipOptimizer = std::make_shared<InnovationClipOptimizer>( transReg, clipParams );
		// _clipOptimizer = std::make_shared<InnovationClipOptimizer>( clipParams );

		// TODO Iterate and add all non-transition regs to the optimizer
		typedef EstimatorRegistry::value_type Item;
		BOOST_FOREACH( Item& item, _estRegistry )
		{
			_clipOptimizer->AddObservationReg( *item.second.estimator, item.first );
		}

		ros::NodeHandle optHandle( ph.resolveName( "optimization" ) );
		_clipOptimizer->InitializeOptimization( optHandle );

		double bufferProcessRate;
		GetParam<double>( privHandle, "buffer_process_rate", bufferProcessRate, 10.0 );
		_printPeriod = ros::Duration( bufferProcessRate );

		double statusPublishRate;
		GetParam<double>( privHandle, "status_publish_rate", statusPublishRate, 1.0 );
		_statusPeriod = ros::Duration( 1.0/statusPublishRate );
		_statusPub = privHandle.advertise<OptimizerStatus>( "optimizer_status", 10 );
		_startTime = ros::Time::now();

		double printRate;
		GetParam<double>( privHandle, "print_rate", printRate, 1.0 );
		_printPeriod = ros::Duration( 1.0/printRate );
		_lastPrintTime = ros::Time::now();

		double paramPublishRate;
		GetParam<double>( privHandle, "param_publish_rate", paramPublishRate, 1.0 );
		_publishPeriod = ros::Duration( 1.0/paramPublishRate );
		_lastPublishTime = ros::Time::now();

		_infoSub = nodeHandle.subscribe( "filter_info", infoBufferSize, &Optimizer::FilterCallback, this );

		_waitTimer = nodeHandle.createTimer( ros::Duration( 1.0 ),
		                                     &Optimizer::ReceiveWaitTimerCallback,
		                                     this, true );

		_optimizerThread = boost::thread( boost::bind( &Optimizer::OptimizerCallback, this ) );
	}

	void RegisterModel( const std::string& name, const YAML::Node& info )
	{
		if( _estRegistry.count( name ) > 0 )
		{
			ROS_ERROR_STREAM( "Already registered estimator: " << name );
			exit( -1 );
		}
		ROS_INFO_STREAM( "Registering model: " << name );

		EstimatorRegistration& reg = _estRegistry[ name ];
		std::string paramTopic = name + "/param_updates";
		reg.estimator = std::make_shared<CovarianceEstimator>( name, info );
		reg.paramPublisher = privHandle.advertise<CovarianceEstimatorInfo>( paramTopic, 10 );
		if( name != "transition" )
		{
			reg.weight = TryYamlField<double>( info, "weight" );
		}

		// Register all features this model needs
		//reg.features = TryYamlField<std::vector<std::string>>( info, "features" );
		unsigned int fDim = 0;
		YAML::Node inputStreams = info["features"];
		YAML::const_iterator iter;
		for( iter = inputStreams.begin(); iter != inputStreams.end(); ++iter )
		{
			const std::string& featureName = iter->first.as<std::string>();
			RegisterReceiver( featureName, iter->second );
			fDim += _receivers.at( featureName ).GetDim();
		}

		if( fDim != reg.estimator->InputDim() )
		{
			ROS_ERROR_STREAM( "Model: " << name << " has input dim: " <<
			                  reg.estimator->InputDim() << " but features have total dim: " <<
			                  fDim );
			exit( -1 );
		}

		// Load parameters if we have them
		bool loaded = false;
		if( info["load_path"] )
		{
			CovarianceEstimatorInfo calib;
			std::string infoPath = info["load_path"].as<std::string>();
			if( ReadInfo( infoPath, calib ) )
			{
				ROS_INFO_STREAM( "Loading params for: " << name <<
				                 " from: " << infoPath );
				reg.estimator->SetParamsMsg( calib );
				loaded = true;
			}
			else
			{
				ROS_WARN_STREAM( "Could not load parameters for: " << name <<
			                     " from: " << infoPath );
			}
		}
		if( !loaded )
		{
			ROS_INFO_STREAM( "Initializing: " << name << " to default params." );
			reg.estimator->RandomizeVarianceParams();
			reg.estimator->ZeroCorrelationParams();
			// reg.estimator->ZeroParams();
			if( info["offsets"] )
			{
				ROS_INFO_STREAM( "Setting offsets." );
				std::vector<double> off = info["offsets"].as<std::vector<double>>();
				reg.estimator->SetVarianceOffsets( GetVectorView( off ) );
			}
		}

		// Verify the output path is OK
		if( info["save_path"] )
		{
			reg.outputPath = info["save_path"].as<std::string>();
			std::ofstream outFile( reg.outputPath );
			if( !outFile.is_open() )
			{
				ROS_ERROR_STREAM( "Could not open output file for: " << name <<
				                  " at: " << reg.outputPath );
				exit( -1 );
			}
		}
	}

	void RegisterReceiver( const std::string& streamName, const YAML::Node& props )
	{
		if( _receivers.count( streamName ) > 0 ) { return; }
		_receivers[streamName].Initialize( streamName, props );
	}

	void SaveLog()
	{
		if( _logOutFile.is_open() )
		{
			// _clipOptimizer->CalculateCost();
			_logOutFile << *_clipOptimizer << std::endl;
			typedef EstimatorRegistry::value_type Item;
			BOOST_FOREACH( Item& item, _estRegistry )
			{
				_logOutFile << item.first << " params: " <<
				               item.second.estimator->GetParamSet()->GetParamsVec().transpose() <<
				               std::endl;
			}
		}
		_logOutFile.close();
	}

	void SaveParameters()
	{
		typedef EstimatorRegistry::value_type Item;
		BOOST_FOREACH( Item& item, _estRegistry )
		{
			const std::string& outputPath = item.second.outputPath;
			if( outputPath.empty() ) { continue; }
			ROS_INFO_STREAM( "Writing parameters for " << item.first <<
			                 " to: " << outputPath );
			if( !WriteInfo( outputPath, item.second.estimator->GetParamsMsg() ) )
			{
				ROS_WARN_STREAM( "Failed to write parameters for: " << item.first <<
				                " to: " << outputPath );
			}
		}
	}

	// Destructor only gets called as the program is exiting, but it only exits
	// once all threads have terminated, so we have to tell the thread to stop
	void Terminate()
	{
		_optimizerThread.interrupt();
		_optimizerThread.join();
	}

	void ReceiveWaitTimerCallback( const ros::TimerEvent& event )
	{
		WriteLock lock( _initMutex );
		typedef ReceiverRegistry::value_type Item;
		BOOST_FOREACH( Item& item, _receivers )
		{
			if( !item.second.IsReady() )
			{
				ROS_WARN_STREAM( "Stream: " << item.second.GetStreamName() <<
				                 " has not received data yet." );
				_waitTimer.stop();
				_waitTimer.setPeriod( ros::Duration( 1.0 ) );
				_waitTimer.start();
				return;
			}
		}
		ROS_INFO_STREAM( "All streams have received. Optimizer is ready." );
		rxInitialized = true;
	}

private:

	ros::NodeHandle privHandle;

	struct EstimatorRegistration
	{
		ros::Publisher paramPublisher;
		std::shared_ptr<CovarianceEstimator> estimator;
		std::vector<std::string> features;
		std::string outputPath;
		double weight;
	};
	typedef std::unordered_map<std::string, EstimatorRegistration> EstimatorRegistry;
	EstimatorRegistry _estRegistry;

	typedef std::unordered_map<std::string, BroadcastReceiver> ReceiverRegistry;
	ReceiverRegistry _receivers;

	std::shared_ptr<InnovationClipOptimizer> _clipOptimizer;

	typedef std::pair<VectorType,argus_msgs::FilterStepInfo> InfoData;
	ThreadsafeQueue<InfoData> _dataBuffer;

	double _featureCacheTime;

	ros::Time _startTime;

	unsigned int _numOptimizerSteps;
	ros::Subscriber _infoSub;

	ros::Publisher _statusPub;
	ros::Time _lastStatusTime;
	ros::Duration _statusPeriod;

	ros::Time _lastPrintTime;
	ros::Duration _printPeriod;

	ros::Time _lastPublishTime;
	ros::Duration _publishPeriod;
	ros::Duration _processPeriod;

	unsigned _minOptimizeSize;
	double _minOptimizeTime;
	double _maxEpisodeSpan;
	unsigned _ministeps;
	bool _clearAfterOptimize;

	Mutex _optimizerMutex;
	boost::thread _optimizerThread;

	std::ofstream _logOutFile;

	ros::Timer _waitTimer;
	Mutex _initMutex;
	bool rxInitialized;
	bool epsInitialized;

	unsigned int lastSeq;

	void FilterCallback( const argus_msgs::FilterStepInfo::ConstPtr& msg )
	{
		WriteLock lock( _initMutex );
		if( !rxInitialized ) { return; }

		const std::string& sourceName = msg->header.frame_id;
		if( sourceName != "transition" && _estRegistry.count( sourceName ) == 0 )
		{
			ROS_WARN_STREAM( "Received step info from unregistered model: " << sourceName );
			return;
		}

		const EstimatorRegistration& reg = _estRegistry.at( msg->header.frame_id );
		VectorType feat( reg.estimator->InputDim() );
		unsigned int featInd = 0;

		try{
			BOOST_FOREACH( const std::string& featureName, reg.features )
			{
				BroadcastReceiver& rx = _receivers.at( featureName );
				StampedFeatures f;
				rx.ReadStream( msg->header.stamp, f );
				feat.segment( featInd, rx.GetDim() ) = f.features;
				featInd += rx.GetDim();
			}
		}
		catch( std::runtime_error e )
		{
			ROS_WARN_STREAM( "Could not get features for timestamp: " << msg->header.stamp );
			return;
		}

		_dataBuffer.EmplaceBack( feat, *msg );
	}

	void OptimizerCallback()
	{
		boost::this_thread::interruption_enabled();
		try
		{
			while( !ros::isShuttingDown() )
			{
				boost::this_thread::interruption_point();
				PublishParams();

				ProcessBuffer();
				ros::Time now = ros::Time::now();
				ros::Time earliestTime = _clipOptimizer->GetEarliestTime();
				double span = (now - earliestTime).toSec();

				if( !epsInitialized )
				{
					if( span > _minOptimizeTime )
					{
						epsInitialized = true;
						// ROS_INFO_STREAM( "Episode buffer filled. Starting optimization." );
					}
					else
					{
						// ROS_INFO_STREAM( "Span: " << span );
						_processPeriod.sleep();
						continue;
					}
				}

				while( _clipOptimizer->NumEpisodes() > 0 && span > _maxEpisodeSpan )
				{
					// ROS_INFO_STREAM( "Span: " << span << " greater than max. Trimming oldest episode." );
					_clipOptimizer->RemoveEarliestEpisode();
					earliestTime = _clipOptimizer->GetEarliestTime();
					span = (now - earliestTime).toSec();
				}

				if( _clipOptimizer->NumEpisodes() == 0 )
				{
					ros::Duration( 1.0 ).sleep();
					continue;
				}

				// if( _clipOptimizer->NumEpisodes() >= _minOptimizeSize )

				bool converged = false;
				for( unsigned int i = 0; i < _numOptimizerSteps; i++ )
				{
					converged = _clipOptimizer->Optimize();
				}
				if( converged )
				{
					ROS_INFO_STREAM( "Optimizer has converged. Saving..." );
					break;
				}
				PrintStatus();
				PublishStatus();
				if( _clearAfterOptimize )
				{
					epsInitialized = false;
					ROS_INFO_STREAM( "Clearing episodes..." );
					while( _clipOptimizer->NumEpisodes() > 0 )
					{
						_clipOptimizer->RemoveEarliestEpisode();
					}
					_dataBuffer.Clear();
				}

			}
			SaveParameters();
			SaveLog();
			exit( 0 );
		}
		catch( std::exception e )
		{
			ROS_INFO_STREAM( "Optimizer thread: " << e.what() );
			SaveParameters();
			SaveLog();
			return;
		}
	}

	void PublishStatus()
	{
		ros::Time now = ros::Time::now();
		if( now - _lastStatusTime < _statusPeriod ) { return; }
		_lastStatusTime = now;

		if( _clipOptimizer->NumEpisodes() == 0 ) { return; }

		OptimizerStatus msg;
		msg.header.stamp = now;
		msg.header.frame_id = "covariance_optimizer";
		msg.current_objective = _clipOptimizer->CalculateCost();
		msg.secs_since_start = ( now - _startTime ).toSec();
		_statusPub.publish( msg );
	}

	void PrintStatus()
	{
		ros::Time now = ros::Time::now();
		if( now - _lastPrintTime < _printPeriod ) { return; }
		_lastPrintTime = now;

		if( _clipOptimizer->NumEpisodes() == 0 ) { return; }

		double cost = _clipOptimizer->CalculateCost();
		ROS_INFO_STREAM( "Has " << _clipOptimizer->NumEpisodes() << " episodes." );
		ROS_INFO_STREAM( "Total cost: " << cost << std::endl );
		ROS_INFO_STREAM( "Message buffer size: " << _dataBuffer.Size() );
		typedef EstimatorRegistry::value_type Item;
		BOOST_FOREACH( const Item& item, _estRegistry )
		{
			ROS_INFO_STREAM( "Model: " << item.first << std::endl << item.second.estimator->GetModule().dReg );
		}
		// ROS_INFO_STREAM( "vo: " << std::endl << _estRegistry.at("vo").estimator->GetModule().dReg );
	}

	void ProcessBuffer()
	{
		size_t bsize = _dataBuffer.Size();
		for( size_t i = 0; i < bsize; ++i )
		{
			InfoData data;
			_dataBuffer.WaitPopFront( data );
			VectorType& features = data.first;
			argus_msgs::FilterStepInfo& msg = data.second;

			if( msg.step_num != lastSeq + 1 )
			{
				ROS_WARN_STREAM( "Got sequence " << msg.step_num <<
				                 " but expected " << lastSeq + 1 );
				_clipOptimizer->BreakCurrentEpisode();
			}

			lastSeq = msg.step_num;
			if( msg.isPredict )
			{
				PredictInfo info = MsgToPredict( msg );
				_clipOptimizer->AddPredict( info, features );
			}
			else
			{
				const std::string& sourceName = msg.header.frame_id;
				UpdateInfo info = MsgToUpdate( msg );
				_clipOptimizer->AddUpdate( info,
				                           features,
				                           sourceName,
				                           _estRegistry[sourceName].weight,
				                           msg.header.stamp );
			}
		}
	}

	void PublishParams()
	{
		ros::Time now = ros::Time::now();
		if( now - _lastPublishTime < _publishPeriod ) { return; }
		_lastPublishTime = now;

		typedef EstimatorRegistry::value_type Item;
		BOOST_FOREACH( Item& item, _estRegistry )
		{
			item.second.paramPublisher.publish( item.second.estimator->GetParamsMsg() );
		}
	}

};

int main( int argc, char** argv )
{

	ros::init( argc, argv, "covariance_optimizer" );

	ros::NodeHandle nh, ph("~");

	Optimizer optimizer( nh, ph );

	try
	{
		ros::spin();
	}
	catch( std::runtime_error e )
	{
		ROS_ERROR_STREAM( "Error: " << e.what() );
	}

	optimizer.Terminate();

	return 0;
}


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

// Copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_GEOMETRY_TO_GEOMETRY_RTREE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_GEOMETRY_TO_GEOMETRY_RTREE_HPP

#include <cstddef>
#include <algorithm>
#include <iterator>

#include <boost/assert.hpp>
#include <boost/range.hpp>

#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/iterators/point_iterator.hpp>
#include <boost/geometry/iterators/has_one_element.hpp>

#include <boost/geometry/algorithms/for_each.hpp>
#include <boost/geometry/algorithms/intersects.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/distance_comparable_to_regular.hpp>
#include <boost/geometry/strategies/tags.hpp>

#include <boost/geometry/algorithms/dispatch/distance.hpp>

#include <boost/geometry/index/rtree.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace distance
{

template
<
    typename RTreePoint,
    typename Geometry,
    typename Strategy
>
class point_range_to_geometry_rtree
{
private:
    typedef typename strategy::distance::services::comparable_type
        <
            Strategy
        >::type comparable_strategy;

    typedef typename strategy::distance::services::return_type
        <
            comparable_strategy,
            RTreePoint,
            typename point_type<Geometry>::type
        >::type comparable_return_type;

    typedef index::rtree<RTreePoint, index::linear<8> > r_tree;

    // functor to evaluate minimum comparable distance
    struct minimum_comparable_distance_evaluator
    {
        r_tree const& m_r_tree;
        comparable_strategy const& m_cstrategy;
        bool m_first;
        comparable_return_type m_min_cd;

        minimum_comparable_distance_evaluator
        (r_tree const& r_tree, comparable_strategy const& cstrategy)
            : m_r_tree(r_tree)
            , m_cstrategy(cstrategy)
            , m_first(true)
            , m_min_cd()
        {}

        template <typename QueryGeometry>
        inline void operator()(QueryGeometry const& query_geometry)
        {
            typename r_tree::value_type t_v;
            std::size_t n =
                m_r_tree.query(index::nearest(query_geometry, 1), &t_v);

            BOOST_ASSERT( n > 0 );

            comparable_return_type cd = dispatch::distance
                <
                    typename r_tree::value_type,
                    QueryGeometry,
                    comparable_strategy
                >::apply(t_v, query_geometry, m_cstrategy);

            if ( m_first || cd < m_min_cd )
            {
                m_first = false;
                m_min_cd = cd;
            }
        }
    };

    // class to choose between for_each_point and for_each_segment
    template <typename G, typename Tag = typename tag<G>::type>
    struct for_each_selector
    {
        typedef dispatch::for_each_segment<G> type;
    };

    template <typename MultiPoint>
    struct for_each_selector<MultiPoint, multi_point_tag>
    {
        typedef dispatch::for_each_point<MultiPoint> type;
    };

public:
    typedef typename strategy::distance::services::return_type
        <
            Strategy,
            RTreePoint,
            typename point_type<Geometry>::type
        >::type return_type;

    template <typename PointIterator>
    static inline return_type apply(PointIterator points_first,
                                    PointIterator points_beyond,
                                    Geometry const& geometry,
                                    Strategy const& strategy)
    {
        BOOST_ASSERT( points_first != points_beyond );

        if ( geometry::has_one_element(points_first, points_beyond) )
        {
            return dispatch::distance
                <
                    typename std::iterator_traits<PointIterator>::value_type,
                    Geometry,
                    Strategy
                >::apply(*points_first, geometry, strategy);
        }

        // create -- packing algorithm
        r_tree rt(points_first, points_beyond);

        minimum_comparable_distance_evaluator
            functor(rt,
                    strategy::distance::services::get_comparable
                        <
                            Strategy
                        >::apply(strategy));

        for_each_selector<Geometry>::type::apply(geometry, functor);

        return strategy::distance::services::comparable_to_regular
            <
                comparable_strategy, Strategy, RTreePoint, Geometry
            >::apply(functor.m_min_cd);
    }
};

template
<
    typename Geometry1,
    typename Geometry2,
    typename Strategy
>
class geometry_to_geometry_rtree
{
    // the following works with linear geometries seen as ranges of points
    //
    // we compute the r-tree for the points of one range and then,
    // compute nearest points for the segments of the other,
    // ... and ...
    // vice versa.

private:
    typedef typename strategy::distance::services::comparable_type
        <
            Strategy
        >::type comparable_strategy;

    typedef typename strategy::distance::services::return_type
        <
            comparable_strategy,
            typename point_type<Geometry1>::type,
            typename point_type<Geometry2>::type
        >::type comparable_return_type;

public:
    typedef typename strategy::distance::services::return_type
        <
            Strategy,
            typename point_type<Geometry1>::type,
            typename point_type<Geometry2>::type
        >::type return_type;

    static inline return_type apply(Geometry1 const& geometry1,
                                    Geometry2 const& geometry2,
                                    Strategy const& strategy,
                                    bool check_intersection = true)
    {
        point_iterator<Geometry1 const> first1 = points_begin(geometry1);
        point_iterator<Geometry1 const> beyond1 = points_end(geometry1);
        point_iterator<Geometry2 const> first2 = points_begin(geometry2);
        point_iterator<Geometry2 const> beyond2 = points_end(geometry2);

        if ( geometry::has_one_element(first1, beyond1) )
        {
            return dispatch::distance
                <
                    typename point_type<Geometry1>::type,
                    Geometry2,
                    Strategy
                >::apply(*first1, geometry2, strategy);
        }

        if ( geometry::has_one_element(first2, beyond2) )
        {
            return dispatch::distance
                <
                    typename point_type<Geometry2>::type,
                    Geometry1,
                    Strategy
                >::apply(*first2, geometry1, strategy);
        }

        if ( check_intersection && geometry::intersects(geometry1, geometry2) )
        {
            return return_type(0);
        }

        comparable_strategy cstrategy =
            strategy::distance::services::get_comparable
                <
                    Strategy
                >::apply(strategy);

        comparable_return_type cdist1 = point_range_to_geometry_rtree
            <
                typename point_type<Geometry1>::type,
                Geometry2,
                comparable_strategy
            >::apply(first1, beyond1, geometry2, cstrategy);

        comparable_return_type cdist2 = point_range_to_geometry_rtree
            <
                typename point_type<Geometry2>::type,
                Geometry1,
                comparable_strategy
            >::apply(first2, beyond2, geometry1, cstrategy);

        return strategy::distance::services::comparable_to_regular
            <
                comparable_strategy, Strategy, Geometry1, Geometry2
            >::apply( (std::min)(cdist1, cdist2) );
    }
};

}} // namespace detail::distance
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Linestring1, typename Linestring2, typename Strategy>
struct distance
    <
        Linestring1, Linestring2, Strategy,
        linestring_tag, linestring_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::geometry_to_geometry_rtree
        <
            Linestring1, Linestring2, Strategy
        >
{};

template <typename Linestring, typename Polygon, typename Strategy>
struct distance
    <
        Linestring, Polygon, Strategy,
        linestring_tag, polygon_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::geometry_to_geometry_rtree
        <
            Linestring, Polygon, Strategy
        >
{};

template <typename Linestring, typename Ring, typename Strategy>
struct distance
    <
        Linestring, Ring, Strategy,
        linestring_tag, ring_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::geometry_to_geometry_rtree
        <
            Linestring, Ring, Strategy
        >
{};

template <typename Polygon1, typename Polygon2, typename Strategy>
struct distance
    <
        Polygon1, Polygon2, Strategy,
        polygon_tag, polygon_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::geometry_to_geometry_rtree
        <
            Polygon1, Polygon2, Strategy
        >
{};

template <typename Polygon, typename Ring, typename Strategy>
struct distance
    <
        Polygon, Ring, Strategy,
        polygon_tag, ring_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::geometry_to_geometry_rtree
        <
            Polygon, Ring, Strategy
        >
{};

template <typename Ring1, typename Ring2, typename Strategy>
struct distance
    <
        Ring1, Ring2, Strategy,
        ring_tag, ring_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::geometry_to_geometry_rtree
        <
            Ring1, Ring2, Strategy
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_GEOMETRY_TO_GEOMETRY_RTREE_HPP


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_DISTANCE_GEOGRAPHIC_HPP
#define BOOST_GEOMETRY_STRATEGIES_DISTANCE_GEOGRAPHIC_HPP

#include <boost/geometry/strategies/distance/comparable.hpp>
#include <boost/geometry/strategies/distance/detail.hpp>
#include <boost/geometry/strategies/distance/services.hpp>
#include <boost/geometry/strategies/detail.hpp>

#include <boost/geometry/strategies/geographic/azimuth.hpp>

#include <boost/geometry/strategies/geographic/distance.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track_box_box.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track_point_box.hpp>
#include <boost/geometry/strategies/geographic/distance_segment_box.hpp>
// TODO - for backwards compatibility, remove?
#include <boost/geometry/strategies/geographic/distance_andoyer.hpp>
#include <boost/geometry/strategies/geographic/distance_thomas.hpp>
#include <boost/geometry/strategies/geographic/distance_vincenty.hpp>

#include <boost/geometry/strategies/normalize.hpp>
#include <boost/geometry/strategies/relate/geographic.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace distance
{

// TODO: azimuth and normalize getters would not be needed if distance_segment_box was implemented differently
//       right now it calls disjoint algorithm details.

template
<
    typename FormulaPolicy = strategy::andoyer,
    std::size_t SeriesOrder = strategy::default_order<FormulaPolicy>::value,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
    : public strategies::relate::geographic<FormulaPolicy, SeriesOrder, Spheroid, CalculationType>
{
    using base_t = strategies::relate::geographic<FormulaPolicy, SeriesOrder, Spheroid, CalculationType>;

public:
    geographic() = default;

    explicit geographic(Spheroid const& spheroid)
        : base_t(spheroid)
    {}

    // azimuth

    auto azimuth() const
    {
        return strategy::azimuth::geographic
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    // distance

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_pp_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::geographic
                <
                    FormulaPolicy, Spheroid, CalculationType
                >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_ps_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::geographic_cross_track
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_pb_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::geographic_cross_track_point_box
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_sb_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::geographic_segment_box
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_bb_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::geographic_cross_track_box_box
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    // normalize

    template <typename Geometry>
    static auto normalize(Geometry const&,
                          std::enable_if_t
                            <
                                util::is_point<Geometry>::value
                            > * = nullptr)
    {
        return strategy::normalize::spherical_point();
    }
};

namespace services
{

template <typename Geometry1, typename Geometry2>
struct default_strategy<Geometry1, Geometry2, geographic_tag, geographic_tag>
{
    using type = strategies::distance::geographic<>;
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::distance::geographic<FP, S, CT> >
{
    static auto get(strategy::distance::geographic<FP, S, CT> const& s)
    {
        return strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>(s.model());
    }
};
// TODO - for backwards compatibility, remove?
template <typename S, typename CT>
struct strategy_converter<strategy::distance::andoyer<S, CT> >
{
    static auto get(strategy::distance::andoyer<S, CT> const& s)
    {
        return strategies::distance::geographic<strategy::andoyer, strategy::default_order<strategy::andoyer>::value, S, CT>(s.model());
    }
};
// TODO - for backwards compatibility, remove?
template <typename S, typename CT>
struct strategy_converter<strategy::distance::thomas<S, CT> >
{
    static auto get(strategy::distance::thomas<S, CT> const& s)
    {
        return strategies::distance::geographic<strategy::thomas, strategy::default_order<strategy::thomas>::value, S, CT>(s.model());
    }
};
// TODO - for backwards compatibility, remove?
template <typename S, typename CT>
struct strategy_converter<strategy::distance::vincenty<S, CT> >
{
    static auto get(strategy::distance::vincenty<S, CT> const& s)
    {
        return strategies::distance::geographic<strategy::vincenty, strategy::default_order<strategy::vincenty>::value, S, CT>(s.model());
    }
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::distance::geographic_cross_track<FP, S, CT> >
{
    static auto get(strategy::distance::geographic_cross_track<FP, S, CT> const& s)
    {
        return strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>(s.model());
    }
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::distance::geographic_cross_track_point_box<FP, S, CT> >
{
    static auto get(strategy::distance::geographic_cross_track_point_box<FP, S, CT> const& s)
    {
        return strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>(s.model());
    }
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::distance::geographic_segment_box<FP, S, CT> >
{
    static auto get(strategy::distance::geographic_segment_box<FP, S, CT> const& s)
    {
        return strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>(s.model());
    }
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::distance::geographic_cross_track_box_box<FP, S, CT> >
{
    static auto get(strategy::distance::geographic_cross_track_box_box<FP, S, CT> const& s)
    {
        return strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>(s.model());
    }
};

// details

// TODO: This specialization wouldn't be needed if strategy::distance::geographic_cross_track was implemented as an alias
template <typename FP, typename S, typename CT, bool B, bool ECP>
struct strategy_converter<strategy::distance::detail::geographic_cross_track<FP, S, CT, B, ECP> >
{
    struct altered_strategy
        : strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>
    {
        typedef strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT> base_t;

        explicit altered_strategy(S const& s) : base_t(s) {}

        using base_t::distance;

        template <typename Geometry1, typename Geometry2>
        auto distance(Geometry1 const&, Geometry2 const&,
                      std::enable_if_t
                      <
                            util::is_pointlike<Geometry1>::value
                                && util::is_segmental<Geometry2>::value
                         || util::is_segmental<Geometry1>::value
                                && util::is_pointlike<Geometry2>::value
                         || util::is_segmental<Geometry1>::value
                                && util::is_segmental<Geometry2>::value
                      > * = nullptr) const
        {
            return strategy::distance::detail::geographic_cross_track
                <
                    FP, S, CT, B, ECP
                >(base_t::m_spheroid);
        }
    };

    static auto get(strategy::distance::detail::geographic_cross_track<FP, S, CT, B, ECP> const& s)
    {
        return altered_strategy(s.model());
    }
};

} // namespace services

}} // namespace strategies::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_DISTANCE_GEOGRAPHIC_HPP