/*
	Lyapunov: a library for integrating nonlinear dynamical systems
	Copyright (C) 2013-2018  John Hall

	The author may be reached at jackwhall7@gmail.com.
*/

#include <cmath>
#include <iostream>
#include <vector>
#include <functional>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/numeric/odeint.hpp>
//#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
//#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
//#include <boost/numeric/odeint/stepper/runge_kutta_fehlberg78.hpp>

namespace lyapunov {

	void NotImplementedError() {
		PyErr_SetString(PyExc_NotImplementedError, "Feature not provided.");
		boost::python::throw_error_already_set();
	}
	void LengthError() {
		PyErr_SetString(PyExc_IndexError, "List lengths don't match.");
		boost::python::throw_error_already_set();
	}
	void RuntimeError(const char* error_string) {
		PyErr_SetString(PyExc_RuntimeError, error_string);
		boost::python::throw_error_already_set();
	}
	void ValueError(const char* error_string) {
		PyErr_SetString(PyExc_ValueError, error_string);
		boost::python::throw_error_already_set();
	}
	void StopIteration() {
		PyErr_SetString(PyExc_StopIteration, "Simulation finished.");
		boost::python::throw_error_already_set();
	}

	struct Interval {
		double lower, upper;
		double length() const { return upper - lower; }
		double midpoint() const { return (upper + lower) / 2.0; }
	};

	struct vector_to_python_tuple {
		vector_to_python_tuple() {
			using namespace boost::python;
			to_python_converter<const std::vector<double>, vector_to_python_tuple>();
		}

		static PyObject* convert(const std::vector<double>& x) {
			using namespace boost::python;
			list new_tuple; //is there a way around this list middle stage?
			for(auto i : x) new_tuple.append(i);
			return incref(tuple(new_tuple).ptr());
		}
	};
	struct vector_from_python_tuple {
		vector_from_python_tuple() {
			using namespace boost::python;
			converter::registry::push_back(&convertible, &construct,
										   type_id<std::vector<double>>());
		}

		static void* convertible(PyObject* obj_ptr) {
			if (!PyTuple_Check(obj_ptr)) return 0;
			return obj_ptr;
		}

		static void construct(PyObject* obj_ptr,
			    boost::python::converter::rvalue_from_python_stage1_data* data) {
			using namespace boost::python;
			assert(PyTuple_Check(obj_ptr));
			unsigned int length = PyTuple_Size(obj_ptr);
			void* storage = ((converter::rvalue_from_python_storage< std::vector<double> >*)data)->storage.bytes;
			new (storage) std::vector<double>(length);
			for(unsigned int i=0; i<length; ++i)
				static_cast< std::vector<double>* >(storage)->at(i)
					= PyFloat_AsDouble(PyTuple_GetItem(obj_ptr, i));
			data->convertible = storage;
		}
	};

    // using low-level API for iterator protocol for compatibility with both
    // python 2 and 3
    boost::python::object get_next(const boost::python::object& iter) {
        namespace bp = boost::python;
        PyObject* next_element = PyIter_Next(iter.ptr());
        if(next_element == nullptr) {
            PyErr_SetObject(PyExc_StopIteration, Py_None);
            bp::throw_error_already_set();
        }
        return bp::object(bp::handle<>(next_element));
    }

	boost::python::object pass_through(const boost::python::object& obj) { return obj; }
    double minimum(double x, double y) {
        if(x>y) return y;
        else return x;
    }
    double maximum(double x, double y) {
        if(x<y) return y;
        else return x;
    }

	//should step_across, next, and set_events be redefined for multistepper? probably
	//use bp::arg for keyword arguments
	class stepper_wrapper {
	public:
		typedef double num_type;
		typedef std::vector<num_type> state_type;

	protected: //makes derived code much easier to read
		enum stepper_state_type {NOTHING, LAST, BOUNDARY};

		boost::python::object system, steps, next_time_obj;
		state_type temporary, saved_state, event_signs;
		num_type saved_time;
		stepper_state_type saved_information;
		std::function<void(const state_type&, state_type&, num_type)> system_function;
		bool events_occurred() const {
			namespace bp = boost::python;
			if(tracking_events) {
				bp::object event, iter = system.attr("events").attr("__iter__")();
				for(auto x : event_signs) {
					event = get_next(iter);
					if(x * bp::extract<num_type>( event() ) < 0) return true;
				}
			}
			return false;
		}
		void update_signs() {
			namespace bp = boost::python;
            event_signs.clear();
            if(PyObject_HasAttrString(system.ptr(), "events")) {
                for(bp::stl_input_iterator<bp::object> iter( system.attr("events") ), end;
                      iter != end ; ++iter) {
                    event_signs.push_back(bp::extract<num_type>( (*iter)() ));
                }
            }
            if(event_signs.size() == 0) tracking_events = false;
		}
		void save_last() {
			namespace bp = boost::python;
			bp::object state_tup = system.attr("state");
			saved_time = bp::extract<num_type>(state_tup[0]);
			saved_state = bp::extract<state_type>(state_tup[1]);
			saved_information = LAST;
		}

	public:
		num_type time_tolerance;
		bool tracking_events;

		stepper_wrapper() = delete;
		stepper_wrapper(boost::python::object sys,
					    boost::python::object time)
			: system(sys),
			  steps(),
			  next_time_obj(),
			  temporary( boost::python::len(sys.attr("state")[1]) ),
			  saved_state( temporary.size() ),
			  event_signs(),
			  saved_time(0.0),
			  saved_information(NOTHING),
			  system_function([this](const state_type& x, state_type& dx, const num_type t) {
                  namespace bp = boost::python;
                  system.attr("state") = bp::make_tuple(t, x);
                  dx = bp::extract<state_type>(system()); } ),
			  time_tolerance(0.00001), //10 microseconds
			  tracking_events(true) {
			use_times(time);
			update_signs();
		}
		virtual void step(num_type next_time) = 0;
		virtual void step_back() {
			namespace bp = boost::python;
			if(saved_information != LAST) RuntimeError("Last state not saved.");
			system.attr("state") = bp::make_tuple(saved_time, saved_state);
			reset();
		}
        void reset_with_events() {
            reset();
            update_signs();
        }
		virtual void reset() {
            saved_information = NOTHING;
        }
		void step_across(boost::python::object new_state = boost::python::object()) {
			namespace bp = boost::python;
			if(saved_information != BOUNDARY) RuntimeError("No boundary to step across.");
			bp::object state_tup;
			if(new_state.is_none()) state_tup = bp::make_tuple(saved_time, saved_state);
			else state_tup = bp::make_tuple(saved_time, new_state);
			system.attr("state") = state_tup;
			reset_with_events();
		}
		num_type get_step_size() const {
			namespace bp = boost::python;
			if(saved_information != LAST) RuntimeError("Last state not saved.");
			return bp::extract<num_type>(system.attr("state")[0]) - saved_time;
		}
		num_type get_time_tolerance() const { return time_tolerance; }
		void set_time_tolerance(num_type new_tolerance) {
			if(new_tolerance >= 0) time_tolerance = new_tolerance;
			else ValueError("Positive values only.");
		}
		boost::python::object get_system() const { return system; }
		void set_system(boost::python::object new_system) { system = new_system; }
		void use_times(boost::python::object time) {
			steps = time.attr("__iter__")();
			next_time_obj = boost::python::object();
		}
		boost::python::object next() {
			namespace bp = boost::python;
            //if an event occurred, step through it
            if(saved_information == BOUNDARY) step_across();

			//get time for this next step
            // this block may throw StopIteration
			if( next_time_obj.is_none() )
                next_time_obj = get_next(steps);
			num_type next_time = bp::extract<num_type>(next_time_obj);

			//take the step
			step(next_time);

			//check for event function sign changes, if any
			if( events_occurred() ) {
				auto flagged = find_root();
				//next_time_obj is not reset!
				return bp::make_tuple(system.attr("state")[0], flagged);
			} else {
				//reset next_time_obj to NoneType so the next call
				//will continue iterating through steps
				next_time_obj = bp::object();
				return bp::make_tuple(system.attr("state")[0], bp::list());
			}
		}
		boost::python::object find_root() {
			//implements a simple bisection rootfinder
			namespace bp = boost::python;

			//initialize interval and step size goal
			Interval interval = {saved_time,
								 bp::extract<num_type>(system.attr("state")[0]) };

			//Revert (need to be able to step back across the boundary)
			step_back();

			//main rootfinding loop
			while(interval.length() > time_tolerance) {
				//step to midpoint of interval
				step(interval.midpoint());

				//check for sign changes
				if( events_occurred() ) {
					step_back();
					interval.upper = interval.midpoint();
				} else interval.lower = interval.midpoint();
			}

			//find out which events changed sign and return them
			step(interval.upper);
			num_type across_time = bp::extract<num_type>(system.attr("state")[0]);
			state_type across_state = bp::extract<state_type>(system.attr("state")[1]);
			bp::list flagged;
			bp::object event, iter = system.attr("events").attr("__iter__")();
			for(auto x : event_signs) {
				event = get_next(iter);
				if(x * bp::extract<num_type>(event()) < 0)
					flagged.append(event);
			}
			step_back();

			//save state for step_across
			saved_time = across_time;
			saved_state = std::move(across_state);
			saved_information = BOUNDARY;
			return flagged;
		}
		boost::python::str get_status() const {
			switch(saved_information) {
				case NOTHING:
					return "nothing";
				case LAST:
					return "last step";
				case BOUNDARY:
					return "boundary";
				default:
					return "status undefined!";
			}
		}
	};
	class variable_stepper_wrapper : public stepper_wrapper {
	protected:
		typedef stepper_wrapper base_type;
		num_type absolute_tolerance, relative_tolerance;
		state_type temporary, current_state, error;
		num_type step_size, final_time;
        bool final_time_reached;

	public:
		variable_stepper_wrapper() = delete;
		variable_stepper_wrapper(boost::python::object sys,
								 boost::python::object time)
			: base_type(sys, boost::python::list()),
			  absolute_tolerance(0.000001),
			  relative_tolerance(0.001),
			  temporary(saved_state.size()),
			  current_state(saved_state.size()),
			  error(saved_state.size()),
	   		  step_size(-1), //default, a flag to recompute
	   		  final_time(0.0),
              final_time_reached(false) {
			use_times(time);
		}
		virtual void step_back() {
            final_time_reached = false;
            base_type::step_back();
        }
        num_type get_step_size() const { return step_size; }
		num_type get_relative_tolerance() const { return relative_tolerance; }
		void set_relative_tolerance(num_type new_tolerance) {
			if(new_tolerance >= 0) relative_tolerance = new_tolerance;
			else ValueError("Positive values only.");
		}
		num_type get_absolute_tolerance() const { return absolute_tolerance; }
		void set_absolute_tolerance(num_type new_tolerance) {
			if(new_tolerance >= 0) absolute_tolerance = new_tolerance;
			else ValueError("Positive values only.");
		}
		void use_times(boost::python::object time) {
			namespace bp = boost::python;
            PyObject* time_ptr = time.ptr();
			if(!PySequence_Check(time_ptr) && PyNumber_Check(time_ptr)) {
				steps = boost::python::object();
				final_time = bp::extract<num_type>(time);
                if(step_size < 0) {
				    step_size = 0.001*(final_time -
							bp::extract<num_type>(system.attr("state")[0]));
                }
			} else base_type::use_times(time);
            final_time_reached = false;
		}
		virtual void try_step(num_type current_time, num_type current_step_size) = 0;
        virtual void try_free_step(num_type current_step_size) = 0;
        double compute_error_index() {
            double error_index = 0.0;
            for(int i=error.size()-1; i>=0; --i) {
				error_index = maximum(error_index, std::abs(error[i]) /
							  (absolute_tolerance + relative_tolerance*std::abs(temporary[i])));
			}
            return error_index;
        }
		virtual void increase_step(num_type error_index) = 0;
        virtual bool decrease_step(num_type error_index, num_type original_step_size) = 0;
		bool adjust_step(num_type error_index, num_type original_step_size) {
			increase_step(error_index);
			return decrease_step(error_index, original_step_size);
		}
		void free_step() {
			namespace bp = boost::python;
            if(final_time_reached) StopIteration();
			save_last();
			bool step_successful = false;
			num_type error_index, current_step_size;
			if(step_size < (final_time - saved_time)) {
				while(!step_successful) {
                    try_free_step(step_size);
				    current_step_size = step_size;
                   	error_index = compute_error_index();
					step_successful = adjust_step(error_index, step_size);
				}
			} else {
				current_step_size = final_time - saved_time;
                final_time_reached = true;
				while(!step_successful) {
                    try_free_step(current_step_size);
				    error_index = compute_error_index();
					step_successful = decrease_step(error_index, current_step_size);
                    if(!step_successful) {
                        current_step_size = step_size;
                        final_time_reached = false;
                    }
				}
			}
			system.attr("state") = bp::make_tuple(saved_time + current_step_size,
												  temporary);
		}
		virtual void step(num_type next_time) {
			namespace bp = boost::python;
			save_last();
			auto current_time = saved_time;
            current_state = saved_state;
			num_type current_step_size, error_index=0.0;
			if(step_size < 0) step_size = next_time - current_time;
            if(std::abs(step_size) < std::numeric_limits<num_type>::epsilon()) {
                step_size = -1;
                return;
            }
			bool last_step = false;
			while(true) {
				current_step_size = next_time - current_time;
				if(step_size < current_step_size) {
					current_step_size = step_size;
					last_step = false;
				} else last_step = true; //problem here?

                try_step(current_time, current_step_size);
               	error_index = compute_error_index();

				if(error_index > 1.0) {
					//calculate smaller step size
					decrease_step(error_index, current_step_size);
					continue;
				} else if(last_step) {
					break;
				} else if(error_index < 0.5) {
					//calculate larger step size
					increase_step(error_index);
				}
				current_time += current_step_size;
				std::swap(current_state, temporary);
			}
			system.attr("state") = bp::make_tuple(next_time, temporary);
		}
		boost::python::object next() {
			namespace bp = boost::python;
			if(steps.is_none()) {
                if(saved_information == BOUNDARY) step_across();
				free_step();
				if( events_occurred() ) {
					auto flagged = find_root();
					return bp::make_tuple(system.attr("state")[0], flagged);
				} else {
					return bp::make_tuple(system.attr("state")[0], bp::list());
				}
			} else {
				return base_type::next();
			}
		}
	};
	template<typename stepper_type>
	class simple_stepper_instance : public stepper_wrapper {
		stepper_type stepper;

	public:
		using stepper_wrapper::stepper_wrapper;
		void step(num_type next_time) {
			save_last();
			namespace bp = boost::python;
			stepper.do_step(system_function,
							saved_state,
							saved_time,
							temporary,
							next_time - saved_time); //(sys, xin, tin, xout, h)
			system.attr("state") = bp::make_tuple(next_time, temporary);
		}
	};
	template<typename stepper_type>
	class simple_multistepper_instance : public stepper_wrapper {
		stepper_type stepper;

	public:
		using stepper_wrapper::stepper_wrapper;
		//'step' method is duplicated in simple_stepper_instance
		void step(num_type next_time) {
			save_last();
			namespace bp = boost::python;
			stepper.do_step(system_function,
							saved_state,
							saved_time,
							temporary,
							next_time - saved_time); //(sys, xin, tin, xout, h)
			system.attr("state") = bp::make_tuple(next_time, temporary);
		}
		virtual void reset() {
			stepper.reset();
			stepper_wrapper::reset();
		}
	};
	template<typename stepper_type, unsigned int stepper_order, unsigned int error_order>
	class variable_stepper_instance : public variable_stepper_wrapper {
		stepper_type stepper;

	public:
		using variable_stepper_wrapper::variable_stepper_wrapper;
		virtual void try_step(num_type current_time, num_type current_step_size) {
			stepper.do_step(system_function,
							current_state,
							current_time,
							temporary,
							current_step_size,
							error);
		}
		virtual void try_free_step(num_type current_step_size) {
            stepper.do_step(system_function,
                            saved_state,
                            saved_time,
                            temporary,
                            current_step_size,
                            error);
        }
        virtual void increase_step(num_type error_index) {
			if(error_index < 0.5) {
				step_size *= minimum(5, 0.9*pow(error_index, -1.0/stepper_order));
            }
		}
		virtual bool decrease_step(num_type error_index, num_type original_step_size) {
			if(error_index > 1.0) { //problems here
				//decrease step size
				step_size = original_step_size * maximum(0.2, 0.9 *
							pow(error_index, -1.0/(error_order - 1.0)));
				step_size = maximum(step_size, time_tolerance);
				return false;
			}
			return true;
		}
	};
	template<typename stepper_type, unsigned int stepper_order, unsigned int error_order>
	class variable_multistepper_instance : public variable_stepper_wrapper {
		stepper_type stepper;

	public:
		using variable_stepper_wrapper::variable_stepper_wrapper;
		//'try_step' and 'adjust_step' methods are duplicated in variable_stepper_instance
		virtual void try_step(num_type current_time, num_type current_step_size) {
			stepper.do_step(system_function,
							current_state,
							current_time,
							temporary,
							current_step_size,
							error);
		}
		virtual void try_free_step(num_type current_step_size) {
            stepper.do_step(system_function,
                            saved_state,
                            saved_time,
                            temporary,
                            current_step_size,
                            error);
        }
        virtual void increase_step(num_type error_index) {
			if(error_index < 0.5) {
				step_size *= minimum(5, 0.9*pow(error_index, -1.0/stepper_order));
            }
		}
		virtual bool decrease_step(num_type error_index, num_type original_step_size) {
			if(error_index > 1.0) { //problems here
				//decrease step size
				step_size = original_step_size * maximum(0.2, 0.9 *
							pow(error_index, -1.0/(error_order - 1.0)));
				step_size = maximum(step_size, time_tolerance);
				return false;
			}
			return true;
		}
		virtual void reset() {
			stepper.reset();
			variable_stepper_wrapper::reset();
		}
	};
}

//macros for exposing wrapped steppers
//assumes 'using namespace boost::python'
//no semicolon afterwards
#define LYAPUNOV_EXPOSE_STEPPER(STEPPER, WRAPPER) { \
class_< WRAPPER< STEPPER > >(#STEPPER, init<object, object>()) \
	.def("step_across", &WRAPPER< STEPPER >::step_across, (arg("new_state")=object()) ) \
	.def("reset", &WRAPPER< STEPPER >::reset_with_events) \
	.def("__iter__", pass_through) \
	.def("next", &WRAPPER< STEPPER >::next) \
    .def("__next__", &WRAPPER< STEPPER >::next) \
	.def("use_times", &WRAPPER< STEPPER >::use_times) \
	.add_property("time_tolerance", &WRAPPER< STEPPER >::get_time_tolerance, &WRAPPER< STEPPER >::set_time_tolerance) \
	.add_property("system", &WRAPPER< STEPPER >::get_system, &WRAPPER< STEPPER >::set_system) \
	.def_readonly("tracking_events", &WRAPPER< STEPPER >::tracking_events) \
	.def_readonly("step_size", &WRAPPER< STEPPER >::get_step_size) \
	.def_readonly("status", &WRAPPER< STEPPER >::get_status); }

#define LYAPUNOV_EXPOSE_VARIABLE_STEPPER(STEPPER, WRAPPER, ORDER, ERROR) { \
class_< WRAPPER< STEPPER, ORDER, ERROR > >(#STEPPER, init<object, object>()) \
	.def("step_across", &WRAPPER< STEPPER, ORDER, ERROR >::step_across, (arg("new_state")=object()) ) \
	.def("reset", &WRAPPER< STEPPER, ORDER, ERROR >::reset_with_events) \
	.def("__iter__", pass_through) \
	.def("next", &WRAPPER< STEPPER, ORDER, ERROR >::next) \
	.def("__next__", &WRAPPER< STEPPER, ORDER, ERROR >::next) \
	.def("use_times", &WRAPPER< STEPPER, ORDER, ERROR >::use_times) \
	.add_property("time_tolerance", &WRAPPER< STEPPER, ORDER, ERROR >::get_time_tolerance, &WRAPPER< STEPPER, ORDER, ERROR >::set_time_tolerance) \
	.add_property("relative_tolerance", &WRAPPER< STEPPER, ORDER, ERROR >::get_relative_tolerance, &WRAPPER< STEPPER, ORDER, ERROR >::set_relative_tolerance) \
	.add_property("absolute_tolerance", &WRAPPER< STEPPER, ORDER, ERROR >::get_absolute_tolerance, &WRAPPER< STEPPER, ORDER, ERROR >::set_absolute_tolerance) \
	.add_property("system", &WRAPPER< STEPPER, ORDER, ERROR >::get_system, &WRAPPER< STEPPER, ORDER, ERROR >::set_system) \
	.def_readonly("tracking_events", &WRAPPER< STEPPER, ORDER, ERROR >::tracking_events) \
	.def_readonly("step_size", &WRAPPER< STEPPER, ORDER, ERROR >::get_step_size) \
	.def_readonly("status", &WRAPPER< STEPPER, ORDER, ERROR >::get_status); }

//macro to help with variable order solvers
//no semicolon afterwards
#define LYAPUNOV_EXPOSE_VARIABLE_ORDER_STEPPER(VARSTEPPER) { \
typedef ode::VARSTEPPER<1, state_type> VARSTEPPER##1; \
LYAPUNOV_EXPOSE_STEPPER(VARSTEPPER##1, simple_multistepper_instance); \
typedef ode::VARSTEPPER<2, state_type> VARSTEPPER##2; \
LYAPUNOV_EXPOSE_STEPPER(VARSTEPPER##2, simple_multistepper_instance); \
typedef ode::VARSTEPPER<3, state_type> VARSTEPPER##3; \
LYAPUNOV_EXPOSE_STEPPER(VARSTEPPER##3, simple_multistepper_instance); \
typedef ode::VARSTEPPER<4, state_type> VARSTEPPER##4; \
LYAPUNOV_EXPOSE_STEPPER(VARSTEPPER##4, simple_multistepper_instance); \
typedef ode::VARSTEPPER<5, state_type> VARSTEPPER##5; \
LYAPUNOV_EXPOSE_STEPPER(VARSTEPPER##5, simple_multistepper_instance); \
typedef ode::VARSTEPPER<6, state_type> VARSTEPPER##6; \
LYAPUNOV_EXPOSE_STEPPER(VARSTEPPER##6, simple_multistepper_instance); \
typedef ode::VARSTEPPER<7, state_type> VARSTEPPER##7; \
LYAPUNOV_EXPOSE_STEPPER(VARSTEPPER##7, simple_multistepper_instance); \
typedef ode::VARSTEPPER<8, state_type> VARSTEPPER##8; \
LYAPUNOV_EXPOSE_STEPPER(VARSTEPPER##8, simple_multistepper_instance); }

BOOST_PYTHON_MODULE(solvers) {
	using namespace boost::python;
	namespace ode = boost::numeric::odeint;
	using namespace lyapunov;

	vector_to_python_tuple vec2tup;
	vector_from_python_tuple tup2vec;
	//to_python_converter<std::vector<double>, vector_to_python_tuple>();

	typedef stepper_wrapper::state_type state_type;

	typedef ode::euler<state_type> euler;
	LYAPUNOV_EXPOSE_STEPPER(euler, simple_stepper_instance)
	typedef ode::modified_midpoint<state_type> modified_midpoint;
	LYAPUNOV_EXPOSE_STEPPER(modified_midpoint, simple_stepper_instance)
	typedef ode::runge_kutta4<state_type> runge_kutta4;
	LYAPUNOV_EXPOSE_STEPPER(runge_kutta4, simple_stepper_instance)

	typedef ode::runge_kutta_cash_karp54<state_type> cash_karp;
	LYAPUNOV_EXPOSE_VARIABLE_STEPPER(cash_karp, variable_stepper_instance, 5, 4)
	typedef ode::runge_kutta_fehlberg78<state_type> fehlberg87; //order 7 error est.
	LYAPUNOV_EXPOSE_VARIABLE_STEPPER(fehlberg87, variable_stepper_instance, 8, 7)

	typedef ode::runge_kutta_dopri5<state_type> dormand_prince;
	LYAPUNOV_EXPOSE_VARIABLE_STEPPER(dormand_prince, variable_multistepper_instance, 5, 4)

	LYAPUNOV_EXPOSE_VARIABLE_ORDER_STEPPER(adams_bashforth)
	//ode::adams_moulton has a weird extra argument for do_step (a buffer?)
	//LYAPUNOV_EXPOSE_VARIABLE_ORDER_STEPPER(adams_moulton)
	//ode::adams_bashforth_moulton lacks a reset method for some reason (a bug?)
	//LYAPUNOV_EXPOSE_VARIABLE_ORDER_STEPPER(adams_bashforth_moulton)

	//think about Bulirsch-Stoer solver too!
	//write a dense_stepper_wrapper?
}


#include "CalibTracker/SiStripLorentzAngle/interface/LA_Filler_Fitter.h"
#include "FWCore/Utilities/interface/EDMException.h"

#include <cmath>
#include <regex>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/erase.hpp>
#include <TF1.h>
#include <TGraphErrors.h>
#include <TProfile.h>

LA_Filler_Fitter::Result LA_Filler_Fitter::result(Method m, const std::string name, const Book& book) {
  Result p;
  const std::string base = boost::erase_all_copy(name, method(m));

  const TH1* const h = book[name];
  const TH1* const reco = book[base + "_reconstruction"];
  const TH1* const field = book[base + "_field"];

  if (reco)
    p.reco = std::make_pair<float, float>(reco->GetMean(), reco->GetMeanError());
  if (field)
    p.field = field->GetMean();
  if (h) {
    switch (m) {
      case WIDTH: {
        const TF1* const f = h->GetFunction("LA_profile_fit");
        if (!f)
          break;
        p.measured = std::make_pair<float, float>(f->GetParameter(0), f->GetParError(0));
        p.chi2 = f->GetChisquare();
        p.ndof = f->GetNDF();
        p.entries = (unsigned)(h->GetEntries());
        break;
      }
      case PROB1:
      case AVGV2:
      case AVGV3:
      case RMSV2:
      case RMSV3: /*case MULTI:*/ {
        const TF1* const f = h->GetFunction("SymmetryFit");
        if (!f)
          break;
        p.measured = std::make_pair<float, float>(p.reco.first + f->GetParameter(0), f->GetParameter(1));
        p.chi2 = f->GetParameter(2);
        p.ndof = (unsigned)(f->GetParameter(3));
        p.entries = (m & PROB1)             ? (unsigned)book[base + "_w1"]->GetEntries()
                    : (m & (AVGV2 | RMSV2)) ? (unsigned)book[base + method(AVGV2, false)]->GetEntries()
                    : (m & (AVGV3 | RMSV3)) ? (unsigned)book[base + method(AVGV3, false)]->GetEntries()
                                            : 0;
        break;
      }
      default:
        break;
    }
  }
  return p;
}

std::map<uint32_t, LA_Filler_Fitter::Result> LA_Filler_Fitter::module_results(const Book& book, const Method m) {
  std::map<uint32_t, Result> results;
  for (Book::const_iterator it = book.begin(".*_module\\d*" + method(m)); it != book.end(); ++it) {
    const uint32_t detid = boost::lexical_cast<uint32_t>(
        std::regex_replace(it->first, std::regex(".*_module(\\d*)_.*"), std::string("\\1")));
    results[detid] = result(m, it->first, book);
  }
  return results;
}

std::map<std::string, LA_Filler_Fitter::Result> LA_Filler_Fitter::layer_results(const Book& book, const Method m) {
  std::map<std::string, Result> results;
  for (Book::const_iterator it = book.begin(".*layer\\d.*" + method(m)); it != book.end(); ++it) {
    const std::string name = boost::erase_all_copy(it->first, method(m));
    results[name] = result(m, it->first, book);
  }
  return results;
}

std::map<std::string, std::vector<LA_Filler_Fitter::Result> > LA_Filler_Fitter::ensemble_results(const Book& book,
                                                                                const Method m) {
  std::map<std::string, std::vector<Result> > results;
  for (Book::const_iterator it = book.begin(".*_sample.*" + method(m)); it != book.end(); ++it) {
    const std::string name = std::regex_replace(it->first, std::regex("sample\\d*_"), "");
    results[name].push_back(result(m, it->first, book));
  }
  return results;
}

void LA_Filler_Fitter::summarize_ensembles(Book& book) const {
  typedef std::map<std::string, std::vector<Result> > results_t;
  results_t results;
  for (int m = FIRST_METHOD; m <= LAST_METHOD; m <<= 1)
    if (methods_ & m) {
      results_t g = ensemble_results(book, (Method)(m));
      results.insert(g.begin(), g.end());
    }

  for (auto const& group : results) {
    const std::string name = group.first;
    for (auto const& p : group.second) {
      const float pad = (ensembleUp_ - ensembleLow_) / 10;
      book.fill(p.reco.first, name + "_ensembleReco", 12 * ensembleBins_, ensembleLow_ - pad, ensembleUp_ + pad);
      book.fill(p.measured.first, name + "_measure", 12 * ensembleBins_, ensembleLow_ - pad, ensembleUp_ + pad);
      book.fill(p.measured.second, name + "_merr", 500, 0, 0.01);
      book.fill((p.measured.first - p.reco.first) / p.measured.second, name + "_pull", 500, -10, 10);
    }
    //Need our own copy for thread safety
    TF1 gaus("mygaus", "gaus");
    book[name + "_measure"]->Fit(&gaus, "LLQ");
    book[name + "_merr"]->Fit(&gaus, "LLQ");
    book[name + "_pull"]->Fit(&gaus, "LLQ");
  }
}

std::map<std::string, std::vector<LA_Filler_Fitter::EnsembleSummary> > LA_Filler_Fitter::ensemble_summary(
    const Book& book) {
  std::map<std::string, std::vector<EnsembleSummary> > summary;
  for (Book::const_iterator it = book.begin(".*_ensembleReco"); it != book.end(); ++it) {
    const std::string base = boost::erase_all_copy(it->first, "_ensembleReco");

    const TH1* const reco = it->second;
    const TH1* const measure = book[base + "_measure"];
    const TH1* const merr = book[base + "_merr"];
    const TH1* const pull = book[base + "_pull"];

    EnsembleSummary s;
    s.samples = reco->GetEntries();
    s.truth = reco->GetMean();
    s.meanMeasured = std::make_pair<float, float>(measure->GetFunction("gaus")->GetParameter(1),
                                                  measure->GetFunction("gaus")->GetParError(1));
    s.sigmaMeasured = std::make_pair<float, float>(measure->GetFunction("gaus")->GetParameter(2),
                                                   measure->GetFunction("gaus")->GetParError(2));
    s.meanUncertainty = std::make_pair<float, float>(merr->GetFunction("gaus")->GetParameter(1),
                                                     merr->GetFunction("gaus")->GetParError(1));
    s.pull = std::make_pair<float, float>(pull->GetFunction("gaus")->GetParameter(2),
                                          pull->GetFunction("gaus")->GetParError(2));

    const std::string name = std::regex_replace(base, std::regex("ensembleBin\\d*_"), "");
    summary[name].push_back(s);
  }
  return summary;
}

std::pair<std::pair<float, float>, std::pair<float, float> > LA_Filler_Fitter::offset_slope(
    const std::vector<LA_Filler_Fitter::EnsembleSummary>& ensembles) {
  try {
    std::vector<float> x, y, xerr, yerr;
    for (auto const& ensemble : ensembles) {
      x.push_back(ensemble.truth);
      xerr.push_back(0);
      y.push_back(ensemble.meanMeasured.first);
      yerr.push_back(ensemble.meanMeasured.second);
    }
    TGraphErrors graph(x.size(), &(x[0]), &(y[0]), &(xerr[0]), &(yerr[0]));
    //Need our own copy for thread safety
    TF1 pol1("mypol1", "pol1");
    graph.Fit(&pol1);
    const TF1* const fit = graph.GetFunction("pol1");

    return std::make_pair(std::make_pair(fit->GetParameter(0), fit->GetParError(0)),
                          std::make_pair(fit->GetParameter(1), fit->GetParError(1)));
  } catch (edm::Exception const& e) {
    std::cerr << "Fitting Line Failed " << std::endl << e << std::endl;
    return std::make_pair(std::make_pair(0, 0), std::make_pair(0, 0));
  }
}

float LA_Filler_Fitter::pull(const std::vector<LA_Filler_Fitter::EnsembleSummary>& ensembles) {
  float p(0), w(0);
  for (auto const& ensemble : ensembles) {
    const float unc2 = pow(ensemble.pull.second, 2);
    p += ensemble.pull.first / unc2;
    w += 1 / unc2;
  }
  return p / w;
}

std::ostream& operator<<(std::ostream& strm, const LA_Filler_Fitter::Result& r) {
  return strm << r.reco.first << "\t" << r.reco.second << "\t" << r.measured.first << "\t" << r.measured.second << "\t"
              << r.calMeasured.first << "\t" << r.calMeasured.second << "\t" << r.field << "\t" << r.chi2 << "\t"
              << r.ndof << "\t" << r.entries;
}

std::ostream& operator<<(std::ostream& strm, const LA_Filler_Fitter::EnsembleSummary& e) {
  return strm << e.truth << "\t" << e.meanMeasured.first << "\t" << e.meanMeasured.second << "\t"
              << e.sigmaMeasured.first << "\t" << e.sigmaMeasured.second << "\t" << e.meanUncertainty.first << "\t"
              << e.meanUncertainty.second << "\t" << e.pull.first << "\t" << e.pull.second << "\t" << e.samples;
}


/**
 *  testInv.cpp
 *
 *  Compare abstraction-based and interval methods on solving invariance problems.
 *
 *  Created by Yinan Li on Dec. 02, 2020.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <sys/stat.h>

#include <string>
#include <cmath>
#include <boost/numeric/odeint.hpp>

#include "src/DBAparser.h"
#include "src/abstraction.hpp"

#include "src/bsolver.hpp"

#include "src/txtfileio.h"
#include "src/hdf5io.h"

const double a = 1./3.5;
const double B = 2.0;
const double H = 0.18;
const double W = 0.25;
const double lc = 8.0;
const double cx = 1.0/lc;
const double cy = 1.0/(4*lc*B*B);
const double aH = a+H;
const double H2 = H/(2.0*W*W*W);
const double W2 = 3*W*W;

/* user defined dynamics */
struct mgode2 {

    static const int n = 2;  // system dimension
    static const int nu = 2;  // control dimension

    /* template constructor
     * @param[out] dx
     * @param[in] x
     * @param u
     */
    template<typename S>
    mgode2(S *dx, const S *x, rocs::Rn u) {
	dx[0] = cx * (aH+H2*(x[0]-W)*(W2-(x[0]-W)*(x[0]-W)) - x[1]) + u[0];
	dx[1] = cy * (x[0] - u[1]*sqrt(x[1]));
    }
};

int main(int argc, char *argv[])
{
    /* Input arguments:
     * carAbst dbafile
     */
    if (argc != 2) {
    	std::cout << "Improper number of arguments.\n";
    	std::exit(1);
    }
    std::string specfile = std::string(argv[1]);

    clock_t tb, te;
    /* set the state space */
    double e0 = 0.004;
    double xlb[]{0.5039-e0, 0.6605-e0};
    double xub[]{0.5039+e0, 0.6605+e0};

    /* set the control values */
    // double Lmu = 0.01;
    double Lu = 0.05;
    double ulb[]{-Lu, 0.5};
    double uub[]{Lu, 0.8};
    double mu[]{Lu/5, 0.01};

    /* set the sampling time and disturbance */
    double delta = 0.01;
    /* parameters for computing the flow */
    int kmax = 10;
    double tol = 0.01;
    double alpha = 0.5;
    double beta = 2;
    rocs::params controlparams(kmax, tol, alpha, beta);

    /* define the control system */
    double t= 0.1;
    rocs::CTCntlSys<mgode2> engine("Moore-Greitzer", t, mgode2::n, mgode2::nu,
				  delta, &controlparams);
    engine.init_workspace(xlb, xub);
    engine.init_inputset(mu, ulb, uub);
    engine.allocate_flows();

    /* The goal area */
    double e = 0.003;
    double goal[][2]{{0.5039-e, 0.5039+e}, {0.6605-e, 0.6605+e}};

    /**
     * Read DBA from spec *.txt file
     */
    std::cout << "Reading the specification...\n";
    rocs::UintSmall nAP = 0, nNodes = 0, q0 = 0;
    std::vector<rocs::UintSmall> acc;
    std::vector<std::vector<rocs::UintSmall>> arrayM;
    if (!rocs::read_spec(specfile, nNodes, nAP, q0, arrayM, acc))
    	std::exit(1);
    size_t nProps = arrayM.size();
    /* Mark accepting states */
    boost::dynamic_bitset<> isacc(nNodes, false);
    for (rocs::UintSmall i = 0; i < acc.size(); ++i)
    	isacc[acc[i]] = true;
    std::cout << "isacc= ";
    for (boost::dynamic_bitset<>::size_type i = 0;
         i < isacc.size(); ++i) {
        std::cout << isacc[i] << ' ';
    }
    std::cout << '\n';

    double eta[]{0.00018, 0.00018};

    /**
     * Abstraction-based control
     */
    std::string transfile = "abst_goal_0.00018.h5";
    // std::string ctlrabst = "controller_Gb_abst.txt";
    std::string h5ctlrabst = "controller_Gb_abst_0.00018.h5";

    rocs::abstraction< rocs::CTCntlSys<mgode2> > abst(&engine);
    abst.init_state(eta, xlb, xub);
    std::cout << "The number of abstraction states: " << abst._x._nv << '\n';
    abst.assign_label_outofdomain(0);
    struct stat buffer;
    float tabst;
    if(stat(transfile.c_str(), &buffer) == 0) {
    	/* Read from a file */
    	std::cout << "Transitions have been computed. Reading transitions...\n";
    	rocs::h5FileHandler transRdr(transfile, H5F_ACC_RDONLY);
    	tb = clock();
    	transRdr.read_transitions(abst._ts);
    	te = clock();
    	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
    	std::cout << "Time of reading abstraction: " << tabst << '\n';
    	std::cout << "# of transitions: " << abst._ts._ntrans << '\n';
    } else {
    	std::cout << "Transitions haven't been computed. Computing transitions...\n";
    	/* Robustness margins */
    	double e1[] = {0.0, 0.0};
    	double e2[] = {0.0, 0.0};
    	tb = clock();
    	abst.assign_transitions(e1, e2);
    	te = clock();
    	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
    	std::cout << "Time of computing abstraction: " << tabst << '\n';
    	std::cout << "# of transitions: " << abst._ts._ntrans << '\n';
    	/* Write abstraction to file */
    	rocs::h5FileHandler transWtr(transfile, H5F_ACC_TRUNC);
    	transWtr.write_transitions(abst._ts);
    }
    /* Assign labels to states: has to be consistent with the dba file */
    auto label_target = [&goal, &abst, &eta](size_t i) {
    			    std::vector<double> x(abst._x._dim);
    			    abst._x.id_to_val(x, i);
    			    double c1= eta[0]/2.0; //+1e-10;
    			    double c2= eta[1]/2.0; //+1e-10;

    			    return (goal[0][0] <= (x[0]-c1) && (x[0]+c1) <= goal[0][1] &&
    				    goal[1][0] <= (x[1]-c2) && (x[1]+c2) <= goal[1][1]) ?
    				1: abst._labels[i];
    			};
    abst.assign_labels(label_target);
    std::cout << "Specification assignment is done.\n";
    std::vector<size_t> targetIDs;
    std::vector<rocs::Rn> targetPts;   //initial invariant set
    rocs::Rn x(abst._x._dim);
    for(size_t i = 0; i < abst._labels.size(); ++i) {
    	if(abst._labels[i] > 0) {
    	    targetIDs.push_back(i);
    	    abst._x.id_to_val(x, i);
    	    targetPts.push_back(x);
    	}
    }
    /* Solve a Buchi game on the product of NTS and DBA */
    std::cout << "Start solving a Buchi game on the product of the abstraction and DBA...\n";
    rocs::BSolver solver; // memories will be allocated for psolver
    solver.construct_dba((int)nAP, (int)nNodes, (int)q0, acc, arrayM);
    tb = clock();
    solver.load_abstraction(abst);
    solver.generate_product(abst);
    solver.solve_buchigame_on_product();
    te = clock();
    float tsyn = (float)(te - tb)/CLOCKS_PER_SEC;
    std::cout << "Time of synthesizing controller: " << tsyn << '\n';
    /* Display and save memoryless controllers */
    std::cout << "Writing the controller...\n";
    // solver.write_controller_to_txt(const_cast<char*>(ctlrabst.c_str()));
    rocs::h5FileHandler ctlrWtr(h5ctlrabst, H5F_ACC_TRUNC);
    ctlrWtr.write_problem_setting< rocs::CTCntlSys<mgode2> >(engine);
    ctlrWtr.write_2d_array<double>(targetPts, "G");
    ctlrWtr.write_array<double>(eta, mgode2::n, "eta");
    ctlrWtr.write_2d_array<double>(abst._x._data, "xgrid");
    ctlrWtr.write_discrete_controller(&(solver._sol));
    std::cout << "Controller writing is done.\n";

    std::cout << "Total time of used (abstraction+synthesis): " << tabst+tsyn << '\n';

    /* Test read discrete controller */
    std::vector<long long> w_x0;
    std::vector<long long> encode3;
    std::vector<NODE_POST> nts_ctrlr;
    std::vector<CTRL> ctrl;
    std::vector<int> q_prime;
    rocs::h5FileHandler Rdr(h5ctlrabst, H5F_ACC_RDONLY);
    Rdr.read_discrete_controller(w_x0, encode3, nts_ctrlr, ctrl, q_prime);
    std::cout << "w_x0: " << w_x0.size() << '\n';
    std::cout << "encode3: " << encode3.size() << '\n';
    std::cout << "nts_ctrlr: " << nts_ctrlr.size() << '\n';
    std::cout << "ctrl: " << ctrl.size() << '\n';
    std::cout << "q_prime: " << q_prime.size() << '\n';

    /**
     * DBA control synthesis
     */
    // double glb[][2]{{0.5039-e, 0.6605-e}};
    // double gub[][2]{{0.5039+e, 0.6605+e}};
    // double glb[][2]{{0.50102, 0.65762}};
    // double gub[][2]{{0.50678, 0.66338}};
    // rocs::UintSmall nG = 1, nA = 1;
    // /* Initialize the set of S-domains */
    // std::vector<rocs::CSolver*> w(nNodes);
    // std::vector<rocs::SPtree*> sdoms(nNodes);
    // rocs::UintSmall labels[]{1}; // corresponding to goal1,2,3.
    // for (rocs::UintSmall i = 0; i < nNodes; ++ i) {
    // 	w[i] = new rocs::CSolver(&engine, nProps, rocs::RELMAX, 50);
    // 	w[i]->set_M(arrayM[i]);
    // 	std::cout << "Outgoig transitions of w" << i <<":\n";
    // 	for (auto &m : w[i]->_M)
    // 	    std::cout << m << ' ';
    // 	std::cout << '\n';
    // 	for (rocs::UintSmall j = 0; j < nG; ++j)
    // 	    w[i]->labeling(glb[j], gub[j], labels[j]);
    // 	sdoms[i] = &(w[i]->_ctlr);
    // 	std::cout << "Initial partition of w" << i <<":\n";
    // 	w[i]->print_controller();
    // }
    // // double ei[]{0.0002, 0.0002};
    // double er[]{0.00018, 0.00018};
    // rocs::dba_control< rocs::CTCntlSys<mgode2> >(w,&engine,sdoms,nNodes,isacc,er);
    // /* Save controllers to .h5 files */
    // rocs::write_csolvers_to_h5(engine, specfile, w);

    // /* Use invariance algorithm */
    // rocs::CSolver solver(&engine, 0, rocs::RELMAX);
    // solver.init(rocs::GOAL, glb[0], gub[0]);
    // solver.init_goal_area();
    // solver.invariance_control(&engine, eta);
    // solver.print_controller_info();
    // std::string filename = "controller_Gb_itvl.h5";
    // rocs::h5FileHandler wtr(filename, H5F_ACC_TRUNC);
    // wtr.write_problem_setting(engine);
    // wtr.write_sptree_controller(solver);

    // /* Test reachable set */
    // rocs::ivec G{rocs::interval(0.5039-e, 0.6605-e),
    // 		 rocs::interval(0.5039+e, 0.6605+e)};
    // // rocs::ivec G{rocs::interval(0.50102, 0.50678),
    // // 		 rocs::interval(0.65762, 0.66338)};
    // rocs::ivec g0{rocs::interval(0.50102, 0.5039),rocs::interval(0.65762, 0.6605)};
    // rocs::ivec g1{rocs::interval(0.5039, 0.50678),rocs::interval(0.65762, 0.6605)};
    // rocs::ivec g2{rocs::interval(0.50102, 0.5039),rocs::interval(0.6605, 0.66338)};
    // rocs::ivec g3{rocs::interval(0.5039, 0.50678),rocs::interval(0.6605, 0.66338)};
    // std::vector<rocs::ivec> x0;
    // x0.push_back(g0);
    // x0.push_back(g1);
    // x0.push_back(g2);
    // x0.push_back(g3);

    // std::cout << "Boxes in goal that stay in goal:\n";
    // std::vector<rocs::ivec> y(engine._ugrid._nv, rocs::ivec(2));
    // for(auto &x : x0) {
    // 	engine.get_reach_set(y, x);
    // 	for(size_t i = 0; i < y.size(); ++i) {
    // 	    if(G.isin(y[i])) {
    // 		std::cout << "x=" << x
    // 			  << ", y=" << y[i]
    // 			  << ", u=" << i << '\n';
    // 	    }
    // 	}
    // }

    engine.release_flows();
    return 0;
}


#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <boost/geometry.hpp>
#include <boost/geometry/io/io.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/fusion/adapted/boost_tuple.hpp>
#include <boost/spirit/include/karma.hpp>
#include <iostream>
#include <QPainter>
#include <sstream>
#include <QStringList>
#include <QMessageBox>

const static QString POLYGON_HEAD = "Polygon((";
const static QString POLYGON_END = "))";

using std::cout;
using std::ios;
using std::string;

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    m_paint = false;
    m_mainWidget = new QWidget(this);
    m_mainWidget = new QWidget(this);
    m_mainWidget->setLayout(ui->horizontalLayout);
    setCentralWidget(m_mainWidget);
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::GetVertical()
{
    if(m_string.empty()) {
        return;
    }
    QString qStr = QString::fromStdString(m_string);
    //split
    //remove "POLYGON((
    int leftRemove = POLYGON_HEAD.size();
    qStr.remove(0, leftRemove);

    int lastIndex = qStr.lastIndexOf(POLYGON_END);
    int rightRemove = POLYGON_END.size();
    qStr.remove(lastIndex, rightRemove + 1);
    QStringList strList = qStr.split(",");
    //
    if(strList.empty()) {
        return ;
    }
    for(int i = 0; i < strList.size(); ++i) {
        QStringList tempList = strList[i].split(" ");
        double pointX = tempList[0].toDouble();
        double pointY = tempList[1].toDouble();
        QPoint point(pointX, pointY);
        m_polygon.append(point);
    }
}

void MainWindow::paintEvent(QPaintEvent *event)
{
    if(!m_paint) {
        return ;
    }
    //重绘事件
    Q_UNUSED(event)
//    QRect rectWnd = this->rect();
//    // 绘制文字
//    QPainter painter(this);
//    // 设置画笔颜色
//    painter.setPen(QPen(Qt::black, 4, Qt::DashDotLine, Qt::RoundCap));

//    painter.drawRect(300, 300, 700, 400);

//    painter.drawEllipse(QPoint(500, 500), 50, 50);
//    //分离intersection交点
//    painter.setBrush(QBrush(QColor(0, 255, 0)));
//    if(!m_polygon.empty()) {
//        painter.drawPolygon(&m_polygon[0], m_polygon.size());
//    }
}

void MainWindow::on_btnCalc_clicked()
{
    typedef boost::geometry::model::d2::point_xy<double> point;
    typedef boost::geometry::model::polygon<point> polygon;

    const double buffer_distance = 50; // radius of circle
    const int points_per_circle = 36;
    boost::geometry::strategy::buffer::distance_symmetric<double> distance_strategy(buffer_distance);
    boost::geometry::strategy::buffer::join_round join_strategy(points_per_circle);
    boost::geometry::strategy::buffer::end_round end_strategy(points_per_circle);
    boost::geometry::strategy::buffer::point_circle circle_strategy(points_per_circle);
    boost::geometry::strategy::buffer::side_straight side_strategy;

    boost::geometry::model::multi_polygon<polygon> result;

    point pt;
    boost::geometry::read_wkt("POINT(500 500)", pt); // center of circle

    boost::geometry::buffer(pt, result,
                distance_strategy, side_strategy,
                join_strategy, end_strategy, circle_strategy);
    // first item of result is circle with 1 radius and (5,5) point as center
    // result should have 1 polygon
    polygon rect; // your rectangle
    boost::geometry::read_wkt("POLYGON((300 300,300 500,1000 700,500 300,300 300))",rect);

    std::deque<polygon> intersectionGeometry;
    boost::geometry::intersection(rect,result.front(),intersectionGeometry);
    if (intersectionGeometry.size() == 1)
        m_intersectionStream << boost::geometry::wkt(intersectionGeometry.front()) << std::endl;

    m_string = m_intersectionStream.str();
}

void MainWindow::on_btnDraw_clicked()
{
    if(m_string.empty()) {
        QMessageBox::information(this, tr("warning!"), tr("please press calculate first"));
        return;
    }
    m_paint = true;
    GetVertical();
    repaint();
    //绘制矩形
    //绘制圆形
    //绘制交集
}

void MainWindow::on_shapeComboBox_activated(int index)
{

}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_INDEG_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_INDEG_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing indeg capabilities

    convert radian to degree.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = indeg(x);
    @endcode

    is similar to:

    @code
    T r = Radindeg<T>()*x;
    @endcode

    @see  inrad, Radindeg, Radindegr, Deginrad

  **/
  const boost::dispatch::functor<tag::indeg_> indeg = {};
} }
#endif

#include <boost/simd/function/scalar/indeg.hpp>
#include <boost/simd/function/simd/indeg.hpp>

#endif


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_MODF_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MODF_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns the integral and fractional parts of the input.

    @par Header <boost/simd/function/modf.hpp>

    @par Decorators

     - std_ calls std::modf to compute the pair of outputs.

    @par Example:

      @snippet modf.cpp modf

    @par Possible output:

      @snippet modf.txt modf

  **/

  ///@{
  Value modf(Value const& x, Value const& y);
  std::pair<Value, Value> modf(Value const & x);
  ///@}
} }
#endif

#include <boost/simd/function/scalar/modf.hpp>
#include <boost/simd/function/simd/modf.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_EXPONENTBITS_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_EXPONENTBITS_HPP_INCLUDED

#include <boost/simd/ieee/functions/exponentbits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/scalar/bitwise_and.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( exponentbits_, tag::cpu_
                                    , (A0)
                                    , (scalar_< floating_<A0> >)
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return b_and((2*Maxexponent<A0>()+1)<<Nbmantissabits<A0>(), a0);
    }
  };
} } }

#endif


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

#include "PolicyGraph.h"
#include <boost/random/uniform_int.hpp>
#include "GraphUtilities.hpp"
#include "IndexSpace.hpp"

namespace pgi {

History get_history(const path_t& p, const PolicyGraph& g) {
  History h;
  h.reserve(p.size());
  for (const auto& e : p) {
    h.emplace_back(ActionObservation{g[boost::source(e, g)], g[e]});
  }
  return h;
}

std::size_t num_edge_configs(unsigned int step, std::size_t target_width,
                             const PGProperties& properties) {
  if (step == 1)
    return 1;
  else
    return std::pow(layer_width(step - 1, target_width, properties),
                    properties.num_observations_);
}

std::size_t num_local_policies(unsigned int step, std::size_t target_width,
                               const PGProperties& properties) {
  return properties.num_actions_ *
         num_edge_configs(step, target_width, properties);
}

std::size_t layer_width(unsigned int step, std::size_t target_width,
                        const PGProperties& properties) {
  if (step == properties.num_steps_)
    return 1;
  else
    return std::min(target_width,
                    num_local_policies(step, target_width, properties));
}

PolicyGraph fixed_width(unsigned int num_steps, std::size_t target_width,
                        std::size_t num_actions, std::size_t num_observations) {
  std::size_t vertex_counter(0);
  typedef std::pair<std::size_t, std::size_t> edge_pair_t;
  std::vector<edge_pair_t> edges;
  std::vector<std::size_t> edge_properties;

  PGProperties properties{num_actions, num_observations, num_steps};

  std::vector<std::size_t> src, trg;
  for (unsigned int s = 1; s <= num_steps; ++s) {
    // old sources become new targets
    std::swap(trg, src);
    // add new source vertices to graph
    src.clear();
    const std::size_t n_src = layer_width(s, target_width, properties);
    std::generate_n(std::back_inserter(src), n_src,
                    [&vertex_counter]() { return vertex_counter++; });

    // add an end node for all last action nodes
    if (s == 1) {
      std::for_each(src.begin(), src.end(),
                    [num_observations, &vertex_counter, &edges,
                     &edge_properties](vertex_t v_src) {
                      for (std::size_t e = 0; e < num_observations; ++e) {
                        edges.emplace_back(edge_pair_t(v_src, vertex_counter));
                        edge_properties.emplace_back(e);
                      }
                      ++vertex_counter;
                    });
    }

    // insert edges from each source to a target
    if (!trg.empty()) {
      auto v_trg = trg.front();
      for (std::size_t e = 0; e < num_observations; ++e) {
        std::for_each(src.begin(), src.end(),
                      [v_trg, e, &edges, &edge_properties](vertex_t v_src) {
                        edges.emplace_back(edge_pair_t(v_src, v_trg));
                        edge_properties.emplace_back(e);
                      });
      }
    }
  }

  return PolicyGraph(edges.begin(), edges.end(), edge_properties.begin(),
                     vertex_counter, edges.size(), properties);
}

unsigned int steps_remaining(vertex_t v, const PolicyGraph& g) {
  if (boost::out_degree(v, g) == 0)
    return 0;
  else {
    const edge_t e = *(boost::out_edges(v, g).first);
    return (1 + steps_remaining(boost::target(e, g), g));
  }
}

vertex_t find_root(const PolicyGraph& g) {
  vertex_t root(0);
  for (auto v : boost::make_iterator_range(boost::vertices(g))) {
    if (steps_remaining(root, g) < steps_remaining(v, g)) root = v;
  }
  return root;
}

std::pair<typename boost::filter_iterator<
              vertex_predicate_t,
              typename boost::graph_traits<PolicyGraph>::vertex_iterator>,
          typename boost::filter_iterator<
              vertex_predicate_t,
              typename boost::graph_traits<PolicyGraph>::vertex_iterator> >
vertices_with_steps_remaining(unsigned int s, const PolicyGraph& g) {
  vertex_predicate_t hp = [s, &g](vertex_t v) {
    return (steps_remaining(v, g) == s);
  };
  return filter_vertices(hp, g);
}

std::pair<typename boost::filter_iterator<
              vertex_predicate_t,
              typename boost::graph_traits<PolicyGraph>::vertex_iterator>,
          typename boost::filter_iterator<
              vertex_predicate_t,
              typename boost::graph_traits<PolicyGraph>::vertex_iterator> >
possible_successors(vertex_t v, const PolicyGraph& g) {
  const unsigned int s = steps_remaining(v, g);
  vertex_predicate_t hp = [s, &g](vertex_t v_other) {
    return (s == (steps_remaining(v_other, g) + 1));
  };
  return filter_vertices(hp, g);
}

std::size_t num_possible_successors(vertex_t v, const PolicyGraph& g) {
  auto s = possible_successors(v, g);
  return std::distance(s.first, s.second);
}

std::size_t num_out_edge_configurations(vertex_t v, const PolicyGraph& g) {
  const std::size_t s = steps_remaining(v, g);
  if (s <= 1)
    return 1;
  else
    return std::pow(num_possible_successors(v, g),
                    g[boost::graph_bundle].num_observations_);
}

std::size_t num_local_policies(vertex_t v, const PolicyGraph& g) {
  return g[boost::graph_bundle].num_actions_ *
         num_out_edge_configurations(v, g);
}

bool same_policy(vertex_t v, vertex_t u, const PolicyGraph& g) {
  if (v == u) return true;
  if (g[v] != g[u]) return false;  // different action

  if ((g[v]) == g[u] && (steps_remaining(v, g) == 1) && (steps_remaining(u, g) == 1))
    return true; // last step, and the actions are equal

  // if after same observation, the next actions are different, then the
  // policies are not locally same
  for (auto vep = boost::out_edges(v, g); vep.first != vep.second;
       ++vep.first) {
    for (auto uep = boost::out_edges(u, g); uep.first != uep.second;
         ++uep.first) {
      // check if observation same
      if (g[*vep.first] != g[*uep.first]) continue;
      // check if next nodes' policies  different
      if (!same_policy(boost::target(*vep.first, g),
                       boost::target(*uep.first, g), g))
        return false;
    }
  }
  return true;
}

void set_out_edge_configuration(vertex_t v,
                                std::size_t out_edge_configuration_idx,
                                PolicyGraph& g) {
  if ((steps_remaining(v, g) == 1) || (boost::out_degree(v, g) == 0)) return;

  IndexSpace<std::size_t> I(std::vector<std::size_t>(
      g[boost::graph_bundle].num_observations_, num_possible_successors(v, g)));

  const auto s = possible_successors(v, g);
  boost::clear_out_edges(v, g);
  for (std::size_t e = 0; e < I.local_index_size(); ++e) {
    boost::add_edge(
        v, *std::next(s.first, I.local_index(out_edge_configuration_idx, e)), e,
        g);
  }
}

void set_local_policy(vertex_t v, std::size_t local_policy_idx,
                      PolicyGraph& g) {
  IndexSpace<std::size_t> I(std::vector<std::size_t>{
      g[boost::graph_bundle].num_actions_, num_out_edge_configurations(v, g)});

  g[v] = I.local_index(local_policy_idx, 0);
  set_out_edge_configuration(v, I.local_index(local_policy_idx, 1), g);
}

std::size_t get_local_policy(vertex_t v, std::size_t action_index,
                             std::size_t out_edge_configuration_idx,
                             const PolicyGraph& g) {
  IndexSpace<std::size_t> I(std::vector<std::size_t>{
      g[boost::graph_bundle].num_actions_, num_out_edge_configurations(v, g)});
  return I.joint_index(
      std::vector<std::size_t>{action_index, out_edge_configuration_idx});
}

std::ostream& print(std::ostream& out, const PolicyGraph& g,
                    const std::vector<std::string>& actionnames,
                    const std::vector<std::string>& observationnames) {
  return pgi::print(out, g, VertexWriter<PolicyGraph>(&g, &actionnames),
                    EdgeWriter<PolicyGraph>(&g, &observationnames));
}

std::vector<vertex_t> vertices_with_n_steps(unsigned int n,
                                            const PolicyGraph& g) {
  auto vp = vertices_with_steps_remaining(n, g);
  return std::vector<vertex_t>(vp.first, vp.second);
}

bool any_same_policy(vertex_t v, const std::vector<vertex_t>& v_compare,
                     const PolicyGraph& g) {
  return std::any_of(v_compare.begin(), v_compare.end(), [&g, v](vertex_t vc) {
    return ((v != vc) && same_policy(v, vc, g));
  });
}

bool redirect_in_edges_of_same_policy(vertex_t from,
                                      const std::vector<vertex_t>& to,
                                      PolicyGraph& g, PRNG& rng) {
  bool redirected = false;
  for (auto& q_other : to) {
    if ((q_other != from) && same_policy(q_other, from, g)) {
      redirected = true;
      redirect_in_edges(from, q_other, g);
      randomize_local_policy(
          g, from, rng, vertices_with_n_steps(steps_remaining(from, g), g));
    }
  }
  return redirected;
}

void randomize(PolicyGraph& g, PRNG& rng) {
  for (unsigned int s = 0; s <= g[boost::graph_bundle].num_steps_; ++s) {
    randomize_nodes_with_steps_remaining(s, g, rng);
  }
}

void randomize_nodes_with_steps_remaining(unsigned int steps, PolicyGraph& g,
                                          PRNG& rng) {
  std::vector<vertex_t> avoid_same_policy;
  for (auto qp :
       boost::make_iterator_range(vertices_with_steps_remaining(steps, g))) {
    randomize_local_policy(g, qp, rng, avoid_same_policy);
    avoid_same_policy.push_back(qp);
  }
}

void randomize_local_policy(PolicyGraph& g, vertex_t v, PRNG& rng,
                            const std::vector<vertex_t>& avoid_same_policy) {
  using LocalPolicyDist = boost::random::uniform_int_distribution<std::size_t>;
  LocalPolicyDist d(0, num_local_policies(v, g) - 1);
  do {
    set_local_policy(v, rng(d), g);
  } while (any_same_policy(v, avoid_same_policy, g));
}

}  // namespace pgi


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2012 desire Nuentsa <desire.nuentsa_wakam@inria.fr
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "../../test/sparse_solver.h"
#include <Eigen/src/IterativeSolvers/DGMRES.h>

template<typename T> void test_dgmres_T()
{
  DGMRES<SparseMatrix<T>, DiagonalPreconditioner<T> > dgmres_colmajor_diag;
  DGMRES<SparseMatrix<T>, IdentityPreconditioner    > dgmres_colmajor_I;
  DGMRES<SparseMatrix<T>, IncompleteLUT<T> >           dgmres_colmajor_ilut;
  //GMRES<SparseMatrix<T>, SSORPreconditioner<T> >     dgmres_colmajor_ssor;

  CALL_SUBTEST( check_sparse_square_solving(dgmres_colmajor_diag)  );
//   CALL_SUBTEST( check_sparse_square_solving(dgmres_colmajor_I)     );
  CALL_SUBTEST( check_sparse_square_solving(dgmres_colmajor_ilut)     );
  //CALL_SUBTEST( check_sparse_square_solving(dgmres_colmajor_ssor)     );
}

EIGEN_DECLARE_TEST(dgmres)
{
  CALL_SUBTEST_1(test_dgmres_T<double>());
  CALL_SUBTEST_2(test_dgmres_T<std::complex<double> >());
}


//////////////////////////////////////////////////////////////////////////////////
// survival::modelss::exponential::scalar::meta::failure_time::distribution.hpp  //
//                                                                              //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_META_FAILURE_TIME_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_META_FAILURE_TIME_INCLUDE_HPP_ER_2009

#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/meta/failure_time_distribution.hpp>
#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/meta/is_survival_model.hpp>

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 *  @file testHybridGaussianFactorGraph.cpp
 *  @date Mar 11, 2022
 *  @author Fan Jiang
 */

#include <CppUnitLite/Test.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridDiscreteFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/DotWriter.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>

#include <algorithm>
#include <boost/assign/std/map.hpp>
#include <cstddef>
#include <functional>
#include <iostream>
#include <iterator>
#include <vector>

#include "Switching.h"

using namespace boost::assign;

using namespace std;
using namespace gtsam;

using gtsam::symbol_shorthand::C;
using gtsam::symbol_shorthand::D;
using gtsam::symbol_shorthand::X;
using gtsam::symbol_shorthand::Y;

/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, creation) {
  HybridConditional test;

  HybridGaussianFactorGraph hfg;

  hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));

  GaussianMixture clgc(
      {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
      GaussianMixture::Conditionals(
          C(0),
          boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
                                                  I_3x3),
          boost::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
                                                  X(1), I_3x3)));
  GTSAM_PRINT(clgc);
}

/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminate) {
  HybridGaussianFactorGraph hfg;

  hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));

  auto result = hfg.eliminatePartialSequential(KeyVector{0});

  EXPECT_LONGS_EQUAL(result.first->size(), 1);
}

/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminateMultifrontal) {
  HybridGaussianFactorGraph hfg;

  DiscreteKey c(C(1), 2);

  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));

  Ordering ordering;
  ordering.push_back(X(0));
  auto result = hfg.eliminatePartialMultifrontal(ordering);

  EXPECT_LONGS_EQUAL(result.first->size(), 1);
  EXPECT_LONGS_EQUAL(result.second->size(), 1);
}

/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
  HybridGaussianFactorGraph hfg;

  DiscreteKey c1(C(1), 2);

  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));

  DecisionTree<Key, GaussianFactor::shared_ptr> dt(
      C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
      boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));

  hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));

  auto result =
      hfg.eliminateSequential(Ordering::ColamdConstrainedLast(hfg, {C(1)}));

  auto dc = result->at(2)->asDiscreteConditional();
  DiscreteValues dv;
  dv[C(1)] = 0;
  EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3);
}

/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
  HybridGaussianFactorGraph hfg;

  DiscreteKey c1(C(1), 2);

  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));

  DecisionTree<Key, GaussianFactor::shared_ptr> dt(
      C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
      boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));

  hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
  // hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
  hfg.add(HybridDiscreteFactor(
      DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
  // 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
  // {C(1), 2}}, "1 2 2 1")));

  auto result = hfg.eliminateSequential(
      Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));

  GTSAM_PRINT(*result);
}

/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
  HybridGaussianFactorGraph hfg;

  DiscreteKey c1(C(1), 2);

  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));

  // DecisionTree<Key, GaussianFactor::shared_ptr> dt(
  //     C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
  //     boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));

  // hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
  hfg.add(GaussianMixtureFactor::FromFactors(
      {X(1)}, {{C(1), 2}},
      {boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
       boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())}));

  // hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
  hfg.add(DecisionTreeFactor(c1, {2, 8}));
  hfg.add(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"));
  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
  // 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
  // {C(1), 2}}, "1 2 2 1")));

  auto result = hfg.eliminateMultifrontal(
      Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));

  GTSAM_PRINT(*result);
  GTSAM_PRINT(*result->marginalFactor(C(2)));
}

/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
  HybridGaussianFactorGraph hfg;

  DiscreteKey c(C(1), 2);

  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));

  DecisionTree<Key, GaussianFactor::shared_ptr> dt(
      C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
      boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));

  hfg.add(GaussianMixtureFactor({X(1)}, {c}, dt));
  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
  //  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1
  //  2 3 4")));

  auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(1)});

  HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);

  GTSAM_PRINT(*hbt);
  /*
  Explanation: the Junction tree will need to reeliminate to get to the marginal
  on X(1), which is not possible because it involves eliminating discrete before
  continuous. The solution to this, however, is in Murphy02. TLDR is that this
  is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
  should do this.
  */
}

/* ************************************************************************* */
/*
 * This test is about how to assemble the Bayes Tree roots after we do partial
 * elimination
 */
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
  HybridGaussianFactorGraph hfg;

  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
  hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));

  {
    // DecisionTree<Key, GaussianFactor::shared_ptr> dt(
    //     C(0), boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
    //     boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()));

    hfg.add(GaussianMixtureFactor::FromFactors(
        {X(0)}, {{C(0), 2}},
        {boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
         boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones())}));

    DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
        C(1), boost::make_shared<JacobianFactor>(X(2), I_3x3, Z_3x1),
        boost::make_shared<JacobianFactor>(X(2), I_3x3, Vector3::Ones()));

    hfg.add(GaussianMixtureFactor({X(2)}, {{C(1), 2}}, dt1));
  }

  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
  hfg.add(HybridDiscreteFactor(
      DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));

  hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1));
  hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1));

  {
    DecisionTree<Key, GaussianFactor::shared_ptr> dt(
        C(3), boost::make_shared<JacobianFactor>(X(3), I_3x3, Z_3x1),
        boost::make_shared<JacobianFactor>(X(3), I_3x3, Vector3::Ones()));

    hfg.add(GaussianMixtureFactor({X(3)}, {{C(3), 2}}, dt));

    DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
        C(2), boost::make_shared<JacobianFactor>(X(5), I_3x3, Z_3x1),
        boost::make_shared<JacobianFactor>(X(5), I_3x3, Vector3::Ones()));

    hfg.add(GaussianMixtureFactor({X(5)}, {{C(2), 2}}, dt1));
  }

  auto ordering_full =
      Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)});

  GTSAM_PRINT(ordering_full);

  HybridBayesTree::shared_ptr hbt;
  HybridGaussianFactorGraph::shared_ptr remaining;
  std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);

  GTSAM_PRINT(*hbt);

  GTSAM_PRINT(*remaining);

  hbt->dot(std::cout);
  /*
  Explanation: the Junction tree will need to reeliminate to get to the marginal
  on X(1), which is not possible because it involves eliminating discrete before
  continuous. The solution to this, however, is in Murphy02. TLDR is that this
  is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
  should do this.
  */
}

/* ************************************************************************* */
// TODO(fan): make a graph like Varun's paper one
TEST(HybridGaussianFactorGraph, Switching) {
  auto N = 12;
  auto hfg = makeSwitchingChain(N);

  // X(5) will be the center, X(1-4), X(6-9)
  // X(3), X(7)
  // X(2), X(8)
  // X(1), X(4), X(6), X(9)
  // C(5) will be the center, C(1-4), C(6-8)
  // C(3), C(7)
  // C(1), C(4), C(2), C(6), C(8)
  // auto ordering_full =
  //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
  //     X(5),
  //                        C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)});
  KeyVector ordering;

  {
    std::vector<int> naturalX(N);
    std::iota(naturalX.begin(), naturalX.end(), 1);
    std::vector<Key> ordX;
    std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
                   [](int x) { return X(x); });

    KeyVector ndX;
    std::vector<int> lvls;
    std::tie(ndX, lvls) = makeBinaryOrdering(ordX);
    std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
    for (auto &l : lvls) {
      l = -l;
    }
    std::copy(lvls.begin(), lvls.end(),
              std::ostream_iterator<int>(std::cout, ","));
    std::cout << "\n";
  }
  {
    std::vector<int> naturalC(N - 1);
    std::iota(naturalC.begin(), naturalC.end(), 1);
    std::vector<Key> ordC;
    std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
                   [](int x) { return C(x); });
    KeyVector ndC;
    std::vector<int> lvls;

    // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
    std::tie(ndC, lvls) = makeBinaryOrdering(ordC);
    std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
    std::copy(lvls.begin(), lvls.end(),
              std::ostream_iterator<int>(std::cout, ","));
  }
  auto ordering_full = Ordering(ordering);

  // auto ordering_full =
  //     Ordering();

  // for (int i = 1; i <= 9; i++) {
  //   ordering_full.push_back(X(i));
  // }

  // for (int i = 1; i < 9; i++) {
  //   ordering_full.push_back(C(i));
  // }

  // auto ordering_full =
  //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
  //     X(5),
  //                        C(1), C(2), C(3), C(4), C(5), C(6), C(7), C(8)});

  // GTSAM_PRINT(*hfg);
  GTSAM_PRINT(ordering_full);

  HybridBayesTree::shared_ptr hbt;
  HybridGaussianFactorGraph::shared_ptr remaining;
  std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);

  // GTSAM_PRINT(*hbt);

  // GTSAM_PRINT(*remaining);

  {
    DotWriter dw;
    dw.positionHints['c'] = 2;
    dw.positionHints['x'] = 1;
    std::cout << hfg->dot(DefaultKeyFormatter, dw);
    std::cout << "\n";
    hbt->dot(std::cout);
  }

  {
    DotWriter dw;
    // dw.positionHints['c'] = 2;
    // dw.positionHints['x'] = 1;
    std::cout << "\n";
    std::cout << hfg->eliminateSequential(ordering_full)
                     ->dot(DefaultKeyFormatter, dw);
  }
  /*
  Explanation: the Junction tree will need to reeliminate to get to the marginal
  on X(1), which is not possible because it involves eliminating discrete before
  continuous. The solution to this, however, is in Murphy02. TLDR is that this
  is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
  should do this.
  */
  hbt->marginalFactor(C(11))->print("HBT: ");
}

/* ************************************************************************* */
// TODO(fan): make a graph like Varun's paper one
TEST(HybridGaussianFactorGraph, SwitchingISAM) {
  auto N = 11;
  auto hfg = makeSwitchingChain(N);

  // X(5) will be the center, X(1-4), X(6-9)
  // X(3), X(7)
  // X(2), X(8)
  // X(1), X(4), X(6), X(9)
  // C(5) will be the center, C(1-4), C(6-8)
  // C(3), C(7)
  // C(1), C(4), C(2), C(6), C(8)
  // auto ordering_full =
  //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
  //     X(5),
  //                        C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)});
  KeyVector ordering;

  {
    std::vector<int> naturalX(N);
    std::iota(naturalX.begin(), naturalX.end(), 1);
    std::vector<Key> ordX;
    std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
                   [](int x) { return X(x); });

    KeyVector ndX;
    std::vector<int> lvls;
    std::tie(ndX, lvls) = makeBinaryOrdering(ordX);
    std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
    for (auto &l : lvls) {
      l = -l;
    }
    std::copy(lvls.begin(), lvls.end(),
              std::ostream_iterator<int>(std::cout, ","));
    std::cout << "\n";
  }
  {
    std::vector<int> naturalC(N - 1);
    std::iota(naturalC.begin(), naturalC.end(), 1);
    std::vector<Key> ordC;
    std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
                   [](int x) { return C(x); });
    KeyVector ndC;
    std::vector<int> lvls;

    // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
    std::tie(ndC, lvls) = makeBinaryOrdering(ordC);
    std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
    std::copy(lvls.begin(), lvls.end(),
              std::ostream_iterator<int>(std::cout, ","));
  }
  auto ordering_full = Ordering(ordering);

  // GTSAM_PRINT(*hfg);
  GTSAM_PRINT(ordering_full);

  HybridBayesTree::shared_ptr hbt;
  HybridGaussianFactorGraph::shared_ptr remaining;
  std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);

  // GTSAM_PRINT(*hbt);

  // GTSAM_PRINT(*remaining);

  {
    DotWriter dw;
    dw.positionHints['c'] = 2;
    dw.positionHints['x'] = 1;
    std::cout << hfg->dot(DefaultKeyFormatter, dw);
    std::cout << "\n";
    hbt->dot(std::cout);
  }

  {
    DotWriter dw;
    // dw.positionHints['c'] = 2;
    // dw.positionHints['x'] = 1;
    std::cout << "\n";
    std::cout << hfg->eliminateSequential(ordering_full)
                     ->dot(DefaultKeyFormatter, dw);
  }

  auto new_fg = makeSwitchingChain(12);
  auto isam = HybridGaussianISAM(*hbt);

  {
    HybridGaussianFactorGraph factorGraph;
    factorGraph.push_back(new_fg->at(new_fg->size() - 2));
    factorGraph.push_back(new_fg->at(new_fg->size() - 1));
    isam.update(factorGraph);
    std::cout << isam.dot();
    isam.marginalFactor(C(11))->print();
  }
}

/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
  const int N = 7;
  auto hfg = makeSwitchingChain(N, X);
  hfg->push_back(*makeSwitchingChain(N, Y, D));

  for (int t = 1; t <= N; t++) {
    hfg->add(JacobianFactor(X(t), I_3x3, Y(t), -I_3x3, Vector3(1.0, 0.0, 0.0)));
  }

  KeyVector ordering;

  KeyVector naturalX(N);
  std::iota(naturalX.begin(), naturalX.end(), 1);
  KeyVector ordX;
  for (size_t i = 1; i <= N; i++) {
    ordX.emplace_back(X(i));
    ordX.emplace_back(Y(i));
  }

  // {
  //   KeyVector ndX;
  //   std::vector<int> lvls;
  //   std::tie(ndX, lvls) = makeBinaryOrdering(naturalX);
  //   std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
  //   std::copy(lvls.begin(), lvls.end(),
  //             std::ostream_iterator<int>(std::cout, ","));
  //   std::cout << "\n";

  //   for (size_t i = 0; i < N; i++) {
  //     ordX.emplace_back(X(ndX[i]));
  //     ordX.emplace_back(Y(ndX[i]));
  //   }
  // }

  for (size_t i = 1; i <= N - 1; i++) {
    ordX.emplace_back(C(i));
  }
  for (size_t i = 1; i <= N - 1; i++) {
    ordX.emplace_back(D(i));
  }

  {
    DotWriter dw;
    dw.positionHints['x'] = 1;
    dw.positionHints['c'] = 0;
    dw.positionHints['d'] = 3;
    dw.positionHints['y'] = 2;
    std::cout << hfg->dot(DefaultKeyFormatter, dw);
    std::cout << "\n";
  }

  {
    DotWriter dw;
    dw.positionHints['y'] = 9;
    // dw.positionHints['c'] = 0;
    // dw.positionHints['d'] = 3;
    dw.positionHints['x'] = 1;
    std::cout << "\n";
    // std::cout << hfg->eliminateSequential(Ordering(ordX))
    //                  ->dot(DefaultKeyFormatter, dw);
    hfg->eliminateMultifrontal(Ordering(ordX))->dot(std::cout);
  }

  Ordering ordering_partial;
  for (size_t i = 1; i <= N; i++) {
    ordering_partial.emplace_back(X(i));
    ordering_partial.emplace_back(Y(i));
  }
  {
    HybridBayesNet::shared_ptr hbn;
    HybridGaussianFactorGraph::shared_ptr remaining;
    std::tie(hbn, remaining) =
        hfg->eliminatePartialSequential(ordering_partial);

    // remaining->print();
    {
      DotWriter dw;
      dw.positionHints['x'] = 1;
      dw.positionHints['c'] = 0;
      dw.positionHints['d'] = 3;
      dw.positionHints['y'] = 2;
      std::cout << remaining->dot(DefaultKeyFormatter, dw);
      std::cout << "\n";
    }
  }
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


// ========================================================================== //
//                                                                            //
// Copyright (c) 2014-2020 The University of Texas at Austin.                 //
// All rights reserved.                                                       //
//                                                                            //
// Licensed under the Apache License, Version 2.0 (the "License");            //
// you may not use this file except in compliance with the License.           //
// A copy of the License is included with this software in the file LICENSE.  //
// If your copy does not contain the License, you may obtain a copy of the    //
// License at:                                                                //
//                                                                            //
//     https://www.apache.org/licenses/LICENSE-2.0                            //
//                                                                            //
// Unless required by applicable law or agreed to in writing, software        //
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT  //
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.           //
// See the License for the specific language governing permissions and        //
// limitations under the License.                                             //
//                                                                            //
// ========================================================================== //

#include <iostream>
#include <sstream>

#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <math.h>

#include <dtypes.h>

#include "rapidjson/document.h"

#include "Volume.h"
#include "Particles.h"
#include "Datasets.h"
#include "Filter.h"

#include <time.h>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

using namespace boost;
using namespace gxy;
using namespace std;

namespace gxy
{

enum tfer_type
{
  TF_NONE=0,
  TF_LINEAR=1,
  TF_GAUSSIAN=2
};

struct mhArgs : public filterArgs
{
  tfer_type tf_type;   // none, linear, gaussian
  int iterations;      // iteration limit - after the initial skipped iterations
  int startup;         // initial iterations to ignore
  int skip;            // only retain every skip'th successful sample
  int miss;            // max number of successive misses allowed before termination

  float radius;        // Radius for particles
  float sigma;         // type of transfer function TF_LINEAR or TF_GAUSSIAN
  float linear_tf_min; // min value of linear transfer function
  float linear_tf_max; // max value of linear transfer function
  float gaussian_mean; // gaussian transfer function mean
  float gaussian_std;  // gaussian transfer function standard deviation

  float istep;          // steps along i, j, and k axes
  float jstep;
  float kstep;
};

class MHSamplerMsg : public Work
{
public:
  MHSamplerMsg(mhArgs* a) : MHSamplerMsg(sizeof(mhArgs))
  {
    *(mhArgs *)contents->get() = *a;
  }

  WORK_CLASS(MHSamplerMsg, true);

  bool Action(int s);
};

class MHSampler : public Filter
{
  static int mhfilter_index;

// Two types of functions map volume samples to a probability distribution: a
// linear transformation that maps linearly with 0 at some data value and 1 at some
// other data value, with 0 everywhere else, and a gaussian transformation with a
// given mean and standard deviation.   Default is linear from the data value min
// to the data value max

public:

  static void
  init()
  {
    srand(GetTheApplication()->GetRank());
    MHSamplerMsg::Register();
  }

  MHSampler(KeyedDataObjectP source)
  {
    std::stringstream ss;
    ss << "MHFilter_" << mhfilter_index++;
    name = ss.str();

    result = KeyedDataObject::Cast(Particles::NewP());
    result->CopyPartitioning(source);

  }

  ~MHSampler() { std::cerr << "MHSampler dtor\n"; }

  void
  Sample(rapidjson::Document& doc, KeyedDataObjectP scalarVolume)
  {
    mhArgs args;

    args.sourceKey         = scalarVolume->getkey();
    args.destinationKey    = result->getkey();

    args.radius         = doc["mh_radius"].GetDouble();
    args.sigma          = doc["mh_sigma"].GetDouble();
    args.iterations     = doc["mh_iterations"].GetInt();
    args.startup        = doc["mh_startup"].GetInt();
    args.skip           = doc["mh_skip"].GetInt();
    args.miss           = doc["mh_miss"].GetInt();
    args.linear_tf_min  = doc["mh_linear_tf_min"].GetDouble();
    args.linear_tf_max  = doc["mh_linear_tf_max"].GetDouble();
    args.gaussian_mean  = doc["gaussian_mean"].GetDouble();
    args.gaussian_std   = doc["gaussian_std"].GetDouble();

    MHSamplerMsg msg(&args);
    msg.Broadcast(false, true);

    result->Commit();
  }

  static void normally_distributed_vector(double* v, variate_generator<mt19937, normal_distribution<> >* g)
  {
    for (size_t i = 0; i < 3; ++i)
      v[i]=(*g)();
  }

  static float gaussian(float x, float m, float s)
  {
    return ( 1 / ( s * sqrt(2*M_PI) ) ) * exp( -0.5 * pow( (x-m)/s, 2.0 ) );
  }

#define RNDM ((float)rand() / RAND_MAX)

  static vec3f get_starting_point(VolumeP v)
  {
    Box *box = v->get_local_box();
    return vec3f(box->xyz_min.x + RNDM*(box->xyz_max.x - box->xyz_min.x),
               box->xyz_min.y + RNDM*(box->xyz_max.y - box->xyz_min.y),
               box->xyz_min.z + RNDM*(box->xyz_max.z - box->xyz_min.z));
  }

  static float Q(VolumeP v, float s, mhArgs *a)
  {
    float q;

    if (a->tf_type == TF_LINEAR || a->tf_type == TF_NONE)
    {
      if (a->linear_tf_min < a->linear_tf_max)
      {
        if (s < a->linear_tf_min) q = 0.0;
        else if (s > a->linear_tf_max) q = 1.0;
        else q = (s - a->linear_tf_min) / (a->linear_tf_max - a->linear_tf_min);
      }
      else
      {
        if (s < a->linear_tf_max) q = 1.0;
        else if (s > a->linear_tf_min) q = 0.0;
        else q = (s - a->linear_tf_min) / (a->linear_tf_max - a->linear_tf_min);
      }
    }
    else
    {
      q = gaussian(s, a->gaussian_mean, a->gaussian_std);
    }
    return q;
  }

  static float sample(mhArgs *args, VolumeP v, float x, float y, float z)
  {
    float dx, dy, dz;
    v->get_deltas(dx, dy, dz);

    float ox, oy, oz;
    v->get_ghosted_local_origin(ox, oy, oz);

    x = (x - ox) / dx;
    y = (y - oy) / dy;
    z = (z - oz) / dz;

    int ix = (int)x;
    int iy = (int)y;
    int iz = (int)z;

    dx = x - ix;
    dy = y - iy;
    dz = z - iz;

    int v000 = (ix + 0) * args->istep + (iy + 0) * args->jstep + (iz + 0) * args->kstep;
    int v001 = (ix + 0) * args->istep + (iy + 0) * args->jstep + (iz + 1) * args->kstep;
    int v010 = (ix + 0) * args->istep + (iy + 1) * args->jstep + (iz + 0) * args->kstep;
    int v011 = (ix + 0) * args->istep + (iy + 1) * args->jstep + (iz + 1) * args->kstep;
    int v100 = (ix + 1) * args->istep + (iy + 0) * args->jstep + (iz + 0) * args->kstep;
    int v101 = (ix + 1) * args->istep + (iy + 0) * args->jstep + (iz + 1) * args->kstep;
    int v110 = (ix + 1) * args->istep + (iy + 1) * args->jstep + (iz + 0) * args->kstep;
    int v111 = (ix + 1) * args->istep + (iy + 1) * args->jstep + (iz + 1) * args->kstep;

    float b000 = (1.0 - dx) * (1.0 - dy) * (1.0 - dz);
    float b001 = (1.0 - dx) * (1.0 - dy) * (dz);
    float b010 = (1.0 - dx) * (dy) * (1.0 - dz);
    float b011 = (1.0 - dx) * (dy) * (dz);
    float b100 = (dx) * (1.0 - dy) * (1.0 - dz);
    float b101 = (dx) * (1.0 - dy) * (dz);
    float b110 = (dx) * (dy) * (1.0 - dz);
    float b111 = (dx) * (dy) * (dz);

    if (v->get_type() == Volume::FLOAT)
    {
      float *p = (float *)v->get_samples();
      return p[v000]*b000 + p[v001]*b001 + p[v010]*b010 + p[v011]*b011 +
             p[v100]*b100 + p[v101]*b101 + p[v110]*b110 + p[v111]*b111;
    }
    else
    {
      unsigned char *p = (unsigned char *)v->get_samples();
      return p[v000]*b000 + p[v001]*b001 + p[v010]*b010 + p[v011]*b011 +
             p[v100]*b100 + p[v101]*b101 + p[v110]*b110 + p[v111]*b111;
    }
  }

  static float sample(mhArgs *args, VolumeP v, Particle& p) { return sample(args, v, p.xyz.x, p.xyz.y, p.xyz.z); }
  static float sample(mhArgs *args, VolumeP v, vec3f xyz) { return sample(args, v, xyz.x, xyz.y, xyz.z); }

  static void
  Metropolis_Hastings(mhArgs *a)
  {
    variate_generator<mt19937, normal_distribution<> > generator(mt19937(time(0)), normal_distribution<>(0.0, a->sigma));

    VolumeP v = Volume::Cast(KeyedDataObject::GetByKey(a->sourceKey));
    ParticlesP p = Particles::Cast(KeyedDataObject::GetByKey(a->destinationKey));

    p->setModified(true);
    p->clear();
    p->CopyPartitioning(v);

    p->SetDefaultColor(1.0, 1.0, 1.0, 1.0);

    float deltaX, deltaY, deltaZ;
    v->get_deltas(deltaX, deltaY, deltaZ);

    float ox, oy, oz;
    v->get_local_origin(ox, oy, oz);

    int lioff, ljoff, lkoff;
    v->get_ghosted_local_offsets(lioff, ljoff, lkoff);

    int nli, nlj, nlk;
    v->get_local_counts(nli, nlj, nlk);

    int gnli, gnlj, gnlk;
    v->get_ghosted_local_counts(gnli, gnlj, gnlk);

    int local_count = nli*nlj*nlk;

    float max_x = ox + (nli-1) * deltaX,
          max_y = oy + (nlj-1) * deltaY,
          max_z = oz + (nlk-1) * deltaZ;

    int ngx, ngy, ngz;
    v->get_local_counts(ngx, ngy, ngz);
    int global_count = ngx*ngy*ngz;

    a->istep = 1;
    a->jstep = gnli;
    a->kstep = gnli * gnlj;

    Particle tp;
    tp.xyz = get_starting_point(v);
    tp.u.value = sample(a, v, tp.xyz);

    float tq = Q(v, tp.u.value, a);

    Box *partition_box = v->get_local_box();

    int miss_count = 0;
    for (int iteration = 0; iteration < (a->startup + a->iterations); )
    {
      double rv[3];
      normally_distributed_vector(rv, &generator);

      Particle cp(tp.xyz.x + rv[0], tp.xyz.y + rv[1], tp.xyz.z + rv[2], float(0.0));
      if (! partition_box->isIn(cp.xyz)) continue;

      iteration ++;

      cp.u.value = sample(a, v, cp);
      float cq = Q(v, cp.u.value, a);

      if ((cq > tq) || (RNDM < (cq/tq)))
      {
        tp = cp;
        tq = cq;
        if ((iteration > a->startup) && ((iteration % a->skip) == 0))
          p->push_back(tp);
        miss_count = 0;
      }
      else if (++miss_count > a->miss)
      {
        tp.xyz = get_starting_point(v);
        tp.u.value = sample(a, v, tp.xyz);
        miss_count = 0;
      }
    }

    std::cerr << "created " << p->GetNumberOfVertices() << " samples\n";
  }

private:
  variate_generator<mt19937, normal_distribution<> > *generator;
};

}


#ifndef MYMFEM_ERROR_HPP
#define MYMFEM_ERROR_HPP

#include <memory>
#include <Eigen/Core>
#include <Eigen/Dense>

#include "mfem.hpp"
#include "../mymfem/utilities.hpp"

using namespace mfem;

//! Class ComputeH1Error
//! computes the H1 Error between two grid functions
//! possible on different FE spaces.
class ComputeH1Error
{
public:
    //! Computes the H1 Error between two grid functions
    //! possible on different FE spaces.
    //! Uses PointLocator
    std::tuple <double, double, double, double>
    operator()(const GridFunction&, const GridFunction&,
               bool has_shared_vertices = false);

    //! Returns cell element ids and reference points for
    //! mesh2 in element of mesh1.
    //! Uses PointLocator
    std::pair < Array <int>, Array <IntegrationPoint> >
    get_ref_points (ElementTransformation& trans1,
                    int init_cell_id2);

    //! Computes the H1 Error between two grid functions
    //! possible on different FE spaces.
    //! Uses MFEM routines for the search
    std::tuple <double, double, double, double>
    test_slow(const GridFunction&, const GridFunction&);

    //! Returns cell element ids and reference points for
    //! mesh2 in element of mesh1.
    //! Uses MFEM routines for the search
    std::pair < Array <int>, Array <IntegrationPoint> >
    get_ref_points_slow (ElementTransformation&, Mesh&);

private:
    std::unique_ptr <const IntegrationRule> m_ir;
    std::unique_ptr <PointLocator> m_point_locator;
};

//! Class ComputeCauchyL1Error
//! computes the L1 Error between two grid functions
//! possible on different FE spaces.
class ComputeCauchyL1Error
{
public:
    //! Computes the L1 Error between two grid functions
    //! possible on different FE spaces.
    double operator()
    (const GridFunction&, const GridFunction&,
     bool has_shared_vertices = false);

    //! Returns cell element ids and reference points for
    //! mesh2 in element of mesh1.
    std::pair < Array <int>, Array <IntegrationPoint> >
    get_ref_points (ElementTransformation&,
                    int init_cell_id2);

    //! Computes the L1 Error between two grid functions
    //! defined on the same mesh
    double evalOnSameMesh
    (const GridFunction&, const GridFunction&);

private:
    std::unique_ptr <const IntegrationRule> m_ir;
    std::unique_ptr <PointLocator> m_point_locator;
};

//! Class ComputeCauchyL2Error
//! computes the L2 Error between two grid functions
//! possible on different FE spaces.
class ComputeCauchyL2Error
{
public:
    //! Computes the L2 Error between two grid functions,
    //! possible on different FE spaces.
    double operator()
    (const GridFunction&, const GridFunction&,
     bool has_shared_vertices = false);

    //! Computes the L2 Error between two grid functions,
    //! where one grid function is split into x-y components
    //! possible on different FE spaces.
    double operator()
    (const GridFunction&, const GridFunction&,
     const GridFunction&, const GridFunction&,
     bool has_shared_vertices = false);

    //! Returns cell element ids and reference points for
    //! mesh2 in element of mesh1.
    std::pair < Array <int>, Array <IntegrationPoint> >
    get_ref_points (ElementTransformation&,
                    int init_cell_id2);

    //! Computes the L2 Error between two grid functions
    //! defined on the same mesh
    double evalOnSameMesh
    (const GridFunction&, const GridFunction&);

    //! Computes the L2 Error between two grid functions,
    //! where one grid function is split into x-y components,
    //! defined on the same mesh
    double evalOnSameMesh
    (const GridFunction&, const GridFunction&,
     const GridFunction&, const GridFunction&);

private:
    std::unique_ptr <const IntegrationRule> m_ir;
    std::unique_ptr <PointLocator> m_point_locator;
};

#endif /// MYMFEM_ERROR_HPP


#include "cor_algorithm/sources/utilities.h"
#include "cor_system/sources/logger.h"
#include "cor_data_structure/sources/geometry/r_tree_pool_tmpl_impl.h"

#define BOOST_TEST_NO_LIB
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(r_tree)

BOOST_AUTO_TEST_CASE(r_tree_basic)
{
    class T
    {
    public:
        cor::type::Box2F box;

        cor::type::Box2F get_box()
        {
            return box;
        }

        T* operator->()
        {
            return this;
        }

        T()
        {
            box.set(-1, -1, 3, 3);
        }

        T(cor::RFloat px, cor::RFloat py, cor::RFloat wx, cor::RFloat wy)
        {
            box.set(px, py, wx, wy);
        }

    };

    typedef cor::data_structure::RTreePoolTmpl<cor::RFloat, cor::type::Vector2F, cor::type::Vector2I, T> TP;

    TP tp;

    auto r0 = tp.create(T(0.0f, 0.0f, 1.0f, 1.0f));
    auto r1 = tp.create(T(2.0f, 0.0f, 1.0f, 1.0f));

    tp.update_leaf_box();
    tp.construct();
    tp.fit();

    auto root_node = tp.get_root_node();

    root_node->pre_order_with_depth([&](TP::NodeRef& r, cor::RInt32 num){
        auto box = r->get()->box;
    });

    tp.clear();

    {
        cor::algorithm::times(4, [&](cor::RSize i){
            cor::algorithm::times(4, [&](cor::RSize j){
                auto r = tp.create(T(j * 2, i * 2, 1.0f, 1.0f));
            });
        });

        tp.update_leaf_box();
        tp.construct();
        tp.fit();

        auto root_node = tp.get_root_node();
        root_node->pre_order_with_depth([&](TP::NodeRef& r, cor::RInt32 num){
            auto box = r->get()->box;
            if(r->get()->leaf)
            {
                auto box = r->get()->leaf->box;
            }
        });

        TP::Box select_box = TP::Box(0.5, 0.5, 3, 3);
        cor::RInt32 ct = 0;
        tp.find(select_box, [&](TP::Ref& r){
            auto box = r->box;
            BOOST_CHECK(select_box.is_cross(box));
            ct++;
        });
        BOOST_CHECK_EQUAL(ct, 4);
    }

    {
        TP tp2;

        cor::algorithm::times(4, [&](cor::RSize i){
            cor::algorithm::times(4, [&](cor::RSize j){
                auto r = tp2.create(T(j * 2 + 0.25f, i * 2 + 0.25f, 1.0f, 1.0f));
            });
        });

        tp2.update_leaf_box();
        tp2.construct();
        tp2.fit();

        cor::RInt32 ct = 0;
        tp.find_pair(tp2, [&](TP::Ref& r0, TP::Ref& r1){
            auto box0 = r0->box;
            auto box1 = r1->box;
            BOOST_CHECK(box0.is_cross(box1));
            ct++;
        });
        BOOST_CHECK_EQUAL(ct, 16);
    }

}

BOOST_AUTO_TEST_SUITE_END()