#ifndef BG_DUMP_HANDLER_HPP
#define BG_DUMP_HANDLER_HPP

#include <iostream>
#include <utility>
#include <vector>

#include <osmium/handler.hpp>
#include <osmium/tags/tags_filter.hpp>

#include "osmium_utils/bg_factory.hpp"

#include "bg_types.hpp"
#include "builders.hpp"

#include <boost/log/trivial.hpp>

#include <iterator>

class BGDumpHandler : public osmium::handler::Handler {
private:
    osmium::geom::BGFactory m_factory;

    MultipolygonGeo search_area;
    BoxGeo search_area_box;

    template <typename Tags>
    bool fusion_test(
        Tags && tags,
        const std::vector<std::pair<std::string, osmium::StringMatcher>> &
            filter) {
        auto tags_first = tags.cbegin();
        auto tags_last = tags.cend();
        auto filter_first = filter.cbegin();
        auto filter_last = filter.cend();
        while(tags_first != tags_last && filter_first != filter_last) {
            int r_cmp = tags_first->first.compare(filter_first->first);
            if(r_cmp < 0) {
                ++tags_first;
                continue;
            }
            if(r_cmp > 0) return false;
            if(!filter_first->second(tags_first->second.data())) return false;
            ++tags_first;
            ++filter_first;
        }
        return filter_first == filter_last;
    }

    template <typename Tags>
    std::vector<std::pair<std::string_view, std::string_view>>
    get_sorted_tag_views(Tags && tags) {
        std::vector<std::pair<std::string_view, std::string_view>> tags_views;
        tags_views.reserve(tags.size());
        std::transform(tags.cbegin(), tags.cend(),
                       std::back_inserter(tags_views),
                       [](const osmium::Tag & tag) {
                           return std::make_pair(tag.key(), tag.value());
                       });
        std::sort(tags_views.begin(), tags_views.end(),
                  [](const auto & p1, const auto & p2) {
                      return p1.first < p2.first;
                  });
        return tags_views;
    }

    std::vector<
        std::pair<std::vector<std::pair<std::string, osmium::StringMatcher>>,
                  NodeBuilder>>
        node_filters;
    std::vector<std::pair<
        std::vector<std::pair<std::string, osmium::StringMatcher>>, WayBuilder>>
        way_filters;
    std::vector<
        std::pair<std::vector<std::pair<std::string, osmium::StringMatcher>>,
                  AreaBuilder>>
        area_filters;

    std::vector<Node> nodes;
    std::vector<Way> ways;
    std::vector<Area> areas;

public:
    template <typename NFilters, typename WFilters, typename AFilters>
    BGDumpHandler(NFilters && node_filters, WFilters && way_filters,
                  AFilters && area_filters)
        : osmium::handler::Handler()
        , search_area_box(boost::geometry::make_inverse<BoxGeo>())
        , node_filters(std::forward<NFilters>(node_filters))
        , way_filters(std::forward<WFilters>(way_filters))
        , area_filters(std::forward<AFilters>(area_filters)) {}

    template <typename SArea, typename NFilters, typename WFilters,
              typename AFilters>
    BGDumpHandler(SArea && p_search_area, NFilters && node_filters,
                  WFilters && way_filters, AFilters && area_filters)
        : osmium::handler::Handler()
        , search_area(p_search_area)
        , search_area_box(boost::geometry::return_envelope<BoxGeo>(search_area))
        , node_filters(std::forward<NFilters>(node_filters))
        , way_filters(std::forward<WFilters>(way_filters))
        , area_filters(std::forward<AFilters>(area_filters)) {}

    template <typename NFilter, typename... NInfoArgs>
    void add_node_rule(NFilter && filter, NInfoArgs &&... args) const noexcept {
        node_filters.emplace_back(std::piecewise_construct,
                                  std::forward_as_tuple(filter),
                                  std::forward_as_tuple(args...));
    }
    template <typename WFilter, typename... WInfoArgs>
    void add_way_rule(WFilter && filter, WInfoArgs &&... args) const noexcept {
        way_filters.emplace_back(std::piecewise_construct,
                                 std::forward_as_tuple(filter),
                                 std::forward_as_tuple(args...));
    }
    template <typename AFilter, typename... AInfoArgs>
    void add_area_rule(AFilter && filter, AInfoArgs &&... args) const noexcept {
        area_filters.emplace_back(std::piecewise_construct,
                                  std::forward_as_tuple(filter),
                                  std::forward_as_tuple(args...));
    }

    template <typename SearchArea>
    void setSearchArea(SearchArea && area) noexcept {
        search_area = std::forward<SearchArea>(area);
    }

    const BoxGeo & getSearchBox() const noexcept { return search_area_box; }
    const MultipolygonGeo & getSearchArea() const noexcept {
        return search_area;
    }
    osmium::TagsFilter getAreaEnglobingFilter() const noexcept {
        osmium::TagsFilter filter{false};
        for(const auto & rule : area_filters)
            for(const auto & [tag, value] : rule.first)
                filter.add_rule(true, tag, value);
        return filter;
    }

    const std::vector<Node> & getNodes() const noexcept { return nodes; }
    const std::vector<Way> & getWays() const noexcept { return ways; }
    const std::vector<Area> & getAreas() const noexcept { return areas; }

    void node(const osmium::Node & node) noexcept {
        try {
            if(!boost::geometry::covered_by(m_factory.create_point(node),
                                            search_area_box))
                return;

            std::vector<std::pair<std::string_view, std::string_view>> tags =
                get_sorted_tag_views(node.tags());

            for(auto & [filter, builder] : node_filters) {
                if(!fusion_test(tags, filter)) continue;

                PointGeo p = m_factory.create_point(node);
                if(!(search_area.empty() ||
                     boost::geometry::intersects(p, search_area)))
                    return;
                nodes.emplace_back(builder.build(tags, std::move(p)));
                return;
            }
        } catch(const osmium::geometry_error & e) {
            BOOST_LOG_TRIVIAL(warning)
                << "Discarded OSM entity: " << e.what() << std::endl;
        } catch(const osmium::invalid_location & e) {
            BOOST_LOG_TRIVIAL(warning)
                << "Discarded OSM entity: " << e.what() << std::endl;
        } catch(const std::runtime_error & e) {
            BOOST_LOG_TRIVIAL(warning)
                << "Discarded OSM entity: " << e.what() << std::endl;
        }
    }
    void way(const osmium::Way & way) noexcept {
        try {
            if(!search_area.empty() &&
               !boost::geometry::intersects(m_factory.envelope(way),
                                            search_area_box))
                return;

            std::vector<std::pair<std::string_view, std::string_view>> tags =
                get_sorted_tag_views(way.tags());

            for(auto & [filter, builder] : way_filters) {
                if(!fusion_test(tags, filter)) continue;

                LinestringGeo l = m_factory.create_linestring(way);
                if(!(search_area.empty() ||
                     boost::geometry::covered_by(l, search_area)))
                    return;
                ways.emplace_back(builder.build(tags, std::move(l)));
                return;
            }
        } catch(const osmium::geometry_error & e) {
            BOOST_LOG_TRIVIAL(warning)
                << "Discarded OSM entity: " << e.what() << std::endl;
        } catch(const osmium::invalid_location & e) {
            BOOST_LOG_TRIVIAL(warning)
                << "Discarded OSM entity: " << e.what() << std::endl;
        } catch(const std::runtime_error & e) {
            BOOST_LOG_TRIVIAL(warning)
                << "Discarded OSM entity: " << e.what() << std::endl;
        }
    }
    void area(const osmium::Area & area) noexcept {
        try {
            if(!search_area.empty() &&
               !boost::geometry::intersects(m_factory.envelope(area),
                                            search_area_box))
                return;

            std::vector<std::pair<std::string_view, std::string_view>> tags =
                get_sorted_tag_views(area.tags());

            for(auto & [filter, builder] : area_filters) {
                if(!fusion_test(tags, filter)) continue;

                MultipolygonGeo mp = m_factory.create_multipolygon(area);
                if(!(search_area.empty() ||
                     boost::geometry::intersects(mp, search_area)))
                    return;
                areas.emplace_back(builder.build(tags, std::move(mp)));
                return;
            }
        } catch(const osmium::geometry_error & e) {
            BOOST_LOG_TRIVIAL(warning)
                << "Discarded OSM entity: " << e.what() << std::endl;
        } catch(const osmium::invalid_location & e) {
            BOOST_LOG_TRIVIAL(warning)
                << "Discarded OSM entity: " << e.what() << std::endl;
        } catch(const std::runtime_error & e) {
            BOOST_LOG_TRIVIAL(warning)
                << "Discarded OSM entity: " << e.what() << std::endl;
        }
    }
};  // class BGDumpHandler

#endif  // BG_DUMP_HANDLER_HPP


#include <polyfem/RhsAssembler.hpp>

#include <polyfem/BoundarySampler.hpp>
#include <polyfem/LinearSolver.hpp>
// #include <polyfem/UIState.hpp>

#include <polyfem/Logger.hpp>

#include <Eigen/Sparse>

#ifdef USE_TBB
#include <tbb/parallel_for.h>
#include <tbb/task_scheduler_init.h>
#include <tbb/enumerable_thread_specific.h>
#endif

#include <iostream>
#include <map>
#include <memory>

namespace polyfem
{
	namespace
	{
		class LocalThreadScalarStorage
		{
		public:
			double val;
            ElementAssemblyValues vals;

			LocalThreadScalarStorage()
			{
				val = 0;
			}
		};
	}

	RhsAssembler::RhsAssembler(const Mesh &mesh, const int n_basis, const int size, const std::vector< ElementBases > &bases, const std::vector< ElementBases > &gbases, const std::string &formulation, const Problem &problem)
	: mesh_(mesh), n_basis_(n_basis), size_(size), bases_(bases), gbases_(gbases), formulation_(formulation), problem_(problem)
	{ }

	void RhsAssembler::assemble(Eigen::MatrixXd &rhs, const double t) const
	{
		rhs = Eigen::MatrixXd::Zero(n_basis_ * size_, 1);
		if(!problem_.is_rhs_zero())
		{
			Eigen::MatrixXd rhs_fun;

			const int n_elements = int(bases_.size());
			ElementAssemblyValues vals;
			for(int e = 0; e < n_elements; ++e)
			{
				vals.compute(e, mesh_.is_volume(), bases_[e], gbases_[e]);

				const Quadrature &quadrature = vals.quadrature;

				problem_.rhs(formulation_, vals.val, t, rhs_fun);

				for(int d = 0; d < size_; ++d)
					rhs_fun.col(d) = rhs_fun.col(d).array() * vals.det.array() * quadrature.weights.array();

				const int n_loc_bases_ = int(vals.basis_values.size());
				for(int i = 0; i < n_loc_bases_; ++i)
				{
					const AssemblyValues &v = vals.basis_values[i];

					for(int d = 0; d < size_; ++d)
					{
						const double rhs_value = (rhs_fun.col(d).array() * v.val.array()).sum();
						for(std::size_t ii = 0; ii < v.global.size(); ++ii)
							rhs(v.global[ii].index*size_+d) +=  rhs_value * v.global[ii].val;
					}
				}
			}
		}
	}

	void RhsAssembler::initial_solution(Eigen::MatrixXd &sol) const
	{
		time_bc([&](const Eigen::MatrixXd&pts, Eigen::MatrixXd&val){ problem_.initial_solution(pts, val);}, sol);
	}

	void RhsAssembler::initial_velocity(Eigen::MatrixXd &sol) const
	{
		time_bc([&](const Eigen::MatrixXd&pts, Eigen::MatrixXd&val){ problem_.initial_velocity(pts, val);}, sol);
	}

	void RhsAssembler::initial_acceleration(Eigen::MatrixXd &sol) const
	{
		time_bc([&](const Eigen::MatrixXd&pts, Eigen::MatrixXd&val){ problem_.initial_acceleration(pts, val);}, sol);
	}

	void RhsAssembler::time_bc(const std::function<void(const Eigen::MatrixXd&, Eigen::MatrixXd&)> &fun,Eigen::MatrixXd &sol) const
	{
		sol = Eigen::MatrixXd::Zero(n_basis_ * size_, 1);
		Eigen::MatrixXd loc_sol;

		const int n_elements = int(bases_.size());
        ElementAssemblyValues vals;
		for(int e = 0; e < n_elements; ++e)
		{
			vals.compute(e, mesh_.is_volume(), bases_[e], gbases_[e]);

			const Quadrature &quadrature = vals.quadrature;
			//problem_.initial_solution(vals.val, loc_sol);
			fun(vals.val, loc_sol);

			for(int d = 0; d < size_; ++d)
				loc_sol.col(d) = loc_sol.col(d).array() * vals.det.array() * quadrature.weights.array();

			const int n_loc_bases_ = int(vals.basis_values.size());
			for(int i = 0; i < n_loc_bases_; ++i)
			{
				const AssemblyValues &v = vals.basis_values[i];

				for(int d = 0; d < size_; ++d)
				{
					const double sol_value = (loc_sol.col(d).array() * v.val.array()).sum();
					for(std::size_t ii = 0; ii < v.global.size(); ++ii)
						sol(v.global[ii].index*size_+d) +=  sol_value * v.global[ii].val;
				}
			}
		}
	}

	void RhsAssembler::set_bc(
			const std::function<void(const Eigen::MatrixXi&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::MatrixXd &)> &df,
			const std::function<void(const Eigen::MatrixXi&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::MatrixXd &)> &nf,
			const std::vector< LocalBoundary > &local_boundary, const std::vector<int> &bounday_nodes, const int resolution, const std::vector< LocalBoundary > &local_neumann_boundary, Eigen::MatrixXd &rhs) const
	{
		const int n_el=int(bases_.size());

		Eigen::MatrixXd uv, samples, gtmp, rhs_fun;
		Eigen::VectorXi global_primitive_ids;

		int index = 0;
		std::vector<int> indices; indices.reserve(n_el*10);
		// std::map<int, int> global_index_to_col;

		long total_size = 0;

		Eigen::Matrix<bool, Eigen::Dynamic, 1> is_boundary(n_basis_); is_boundary.setConstant(false);
		Eigen::VectorXi global_index_to_col(n_basis_); global_index_to_col.setConstant(-1);

		const int actual_dim = problem_.is_scalar() ? 1 : mesh_.dimension();

		// assert((bounday_nodes.size()/actual_dim)*actual_dim == bounday_nodes.size());

		for(int b : bounday_nodes)
			is_boundary[b/actual_dim] = true;

		for(const auto &lb : local_boundary)
		{
			const int e = lb.element_id();
			bool has_samples = sample_boundary(lb, resolution, true, uv, samples, global_primitive_ids);

			if(!has_samples)
				continue;

			const ElementBases &bs = bases_[e];
			const int n_local_bases = int(bs.bases.size());

			total_size += samples.rows();

			for(int j = 0; j < n_local_bases; ++j)
			{
				const Basis &b=bs.bases[j];

				for(std::size_t ii = 0; ii < b.global().size(); ++ii)
				{
					//pt found
					// if(std::find(bounday_nodes.begin(), bounday_nodes.end(), size_ * b.global()[ii].index) != bounday_nodes.end())
					if(is_boundary[b.global()[ii].index])
					{
						//if(global_index_to_col.find( b.global()[ii].index ) == global_index_to_col.end())
						if(global_index_to_col(b.global()[ii].index) == -1)
						{
							// global_index_to_col[b.global()[ii].index] = index++;
							global_index_to_col(b.global()[ii].index) = index++;
							indices.push_back(b.global()[ii].index);
							assert(indices.size() == size_t(index));
						}
					}
				}
			}
		}

		// Eigen::MatrixXd global_mat = Eigen::MatrixXd::Zero(total_size, indices.size());
		Eigen::MatrixXd global_rhs = Eigen::MatrixXd::Zero(total_size, size_);

		const long buffer_size = total_size * long(indices.size());
		std::vector< Eigen::Triplet<double> > entries, entries_t;
		// entries.reserve(buffer_size);
		// entries_t.reserve(buffer_size);

		index = 0;

		int global_counter = 0;
		Eigen::MatrixXd mapped;

		std::vector<AssemblyValues> tmp_val;

		for(const auto &lb : local_boundary)
		{
			const int e = lb.element_id();
			bool has_samples = sample_boundary(lb, resolution, false, uv, samples, global_primitive_ids);

			if(!has_samples)
				continue;

			const ElementBases &bs = bases_[e];
			const ElementBases &gbs = gbases_[e];
			const int n_local_bases = int(bs.bases.size());

			gbs.eval_geom_mapping(samples, mapped);

			bs.evaluate_bases(samples, tmp_val);
			for(int j = 0; j < n_local_bases; ++j)
			{
				const Basis &b=bs.bases[j];
				const auto &tmp = tmp_val[j].val;

				for(std::size_t ii = 0; ii < b.global().size(); ++ii)
				{
					// auto item = global_index_to_col.find(b.global()[ii].index);
					// if(item != global_index_to_col.end()){
					auto item = global_index_to_col(b.global()[ii].index);
					if(item != -1){
						for(int k = 0; k < int(tmp.size()); ++k)
						{
							// entries.push_back(Eigen::Triplet<double>(global_counter+k, item->second, tmp(k, j) * b.global()[ii].val));
							// entries_t.push_back(Eigen::Triplet<double>(item->second, global_counter+k, tmp(k, j) * b.global()[ii].val));
							entries.push_back(Eigen::Triplet<double>(global_counter+k, item, tmp(k) * b.global()[ii].val));
							entries_t.push_back(Eigen::Triplet<double>(item, global_counter+k, tmp(k) * b.global()[ii].val));
						}
						// global_mat.block(global_counter, item->second, tmp.size(), 1) = tmp;
					}
				}
			}

			// problem_.bc(mesh_, global_primitive_ids, mapped, t, rhs_fun);
			df(global_primitive_ids, uv, mapped, rhs_fun);
			global_rhs.block(global_counter, 0, rhs_fun.rows(), rhs_fun.cols()) = rhs_fun;
			global_counter += rhs_fun.rows();

			//UIState::ui_state().debug_data().add_points(mapped, Eigen::MatrixXd::Constant(1, 3, 0));

			//Eigen::MatrixXd asd(mapped.rows(), 3);
			//asd.col(0)=mapped.col(0);
			//asd.col(1)=mapped.col(1);
			//asd.col(2)=rhs_fun;
			//UIState::ui_state().debug_data().add_points(asd, Eigen::MatrixXd::Constant(1, 3, 0));
		}

		assert(global_counter == total_size);

		if(total_size > 0)
		{
			const double mmin = global_rhs.minCoeff();
			const double mmax = global_rhs.maxCoeff();

			if(fabs(mmin) < 1e-8 && fabs(mmax) < 1e-8)
			{
				// std::cout<<"is all zero, skipping"<<std::endl;
				for(size_t i = 0; i < indices.size(); ++i){
					for(int d = 0; d < size_; ++d){
						if(problem_.all_dimentions_dirichelt() || std::find(bounday_nodes.begin(), bounday_nodes.end(), indices[i]*size_+d) != bounday_nodes.end())
							rhs(indices[i]*size_+d) = 0;
					}
				}
			}
			else
			{
				StiffnessMatrix mat(int(total_size), int(indices.size()));
				mat.setFromTriplets(entries.begin(), entries.end());

				StiffnessMatrix mat_t(int(indices.size()), int(total_size));
				mat_t.setFromTriplets(entries_t.begin(), entries_t.end());

				StiffnessMatrix A = mat_t * mat;
				Eigen::MatrixXd b = mat_t * global_rhs;

				Eigen::MatrixXd coeffs(b.rows(), b.cols());

				json params = {
				{"mtype", -2}, // matrix type for Pardiso (2 = SPD)
				// {"max_iter", 0}, // for iterative solvers
				// {"tolerance", 1e-9}, // for iterative solvers
				};

				// auto solver = LinearSolver::create("", "");
				auto solver = LinearSolver::create(LinearSolver::defaultSolver(), LinearSolver::defaultPrecond());
				solver->setParameters(params);
				solver->analyzePattern(A);
				solver->factorize(A);
				for(long i = 0; i < b.cols(); ++i){
					solver->solve(b.col(i), coeffs.col(i));
				}
				logger().trace("RHS solve error {}", (A*coeffs-b).norm());

				for(long i = 0; i < coeffs.rows(); ++i){
					for(int d = 0; d < size_; ++d){
						if(problem_.all_dimentions_dirichelt() || std::find(bounday_nodes.begin(), bounday_nodes.end(), indices[i]*size_+d) != bounday_nodes.end())
							rhs(indices[i]*size_+d) = coeffs(i, d);
					}
				}
			}
		}

		//Neumann
		Eigen::MatrixXd points;
		Eigen::VectorXd weights;

        ElementAssemblyValues vals;
		for(const auto &lb : local_neumann_boundary)
		{
			const int e = lb.element_id();
			bool has_samples = boundary_quadrature(lb, resolution, false, uv, points, weights, global_primitive_ids);

			if(!has_samples)
				continue;

			const ElementBases &gbs = gbases_[e];
			const ElementBases &bs = bases_[e];

			vals.compute(e, mesh_.is_volume(), points, bs, gbs);
			// problem_.neumann_bc(mesh_, global_primitive_ids, vals.val, t, rhs_fun);
			nf(global_primitive_ids, uv, vals.val, rhs_fun);

			// UIState::ui_state().debug_data().add_points(vals.val, Eigen::RowVector3d(0,1,0));

			for(int d = 0; d < size_; ++d)
				rhs_fun.col(d) = rhs_fun.col(d).array() * weights.array();

			for(int i = 0; i < lb.size(); ++i)
			{
				const int primitive_global_id = lb.global_primitive_id(i);
				const auto nodes = bs.local_nodes_for_primitive(primitive_global_id, mesh_);

				for(long n = 0; n < nodes.size(); ++n)
				{
					// const auto &b = bs.bases[nodes(n)];
					const AssemblyValues &v = vals.basis_values[nodes(n)];
					for(int d = 0; d < size_; ++d)
					{
						const double rhs_value = (rhs_fun.col(d).array() * v.val.array()).sum();

						for(size_t g = 0; g < v.global.size(); ++g)
						{
							const int g_index = v.global[g].index*size_+d;
							const bool is_neumann = std::find(bounday_nodes.begin(), bounday_nodes.end(), g_index ) == bounday_nodes.end();

							if(is_neumann){
								rhs(g_index) += rhs_value * v.global[g].val;
								// UIState::ui_state().debug_data().add_points(v.global[g].node, Eigen::RowVector3d(1,0,0));
							}
							// else
								// std::cout<<"skipping "<<g_index<<" "<<rhs_value * v.global[g].val<<std::endl;
						}
					}
				}
			}
		}
	}

	void RhsAssembler::set_bc(const std::vector< LocalBoundary > &local_boundary, const std::vector<int> &bounday_nodes, const int resolution, const std::vector< LocalBoundary > &local_neumann_boundary, Eigen::MatrixXd &rhs, const double t) const
	{
		set_bc(
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val){ problem_.bc(mesh_, global_ids, uv, pts, t, val);},
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val){ problem_.neumann_bc(mesh_, global_ids, uv, pts, t, val);},
			local_boundary, bounday_nodes, resolution, local_neumann_boundary, rhs);
	}

	void RhsAssembler::set_velocity_bc(const std::vector< LocalBoundary > &local_boundary, const std::vector<int> &bounday_nodes, const int resolution, const std::vector< LocalBoundary > &local_neumann_boundary, Eigen::MatrixXd &rhs, const double t) const
	{
		set_bc(
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val){ problem_.velocity_bc(mesh_, global_ids, uv, pts, t, val);},
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val){ problem_.neumann_velocity_bc(mesh_, global_ids, uv, pts, t, val);},
			local_boundary, bounday_nodes, resolution, local_neumann_boundary, rhs);
	}

	void RhsAssembler::set_acceleration_bc(const std::vector< LocalBoundary > &local_boundary, const std::vector<int> &bounday_nodes, const int resolution, const std::vector< LocalBoundary > &local_neumann_boundary, Eigen::MatrixXd &rhs, const double t) const
	{
		set_bc(
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val){ problem_.acceleration_bc(mesh_, global_ids, uv, pts, t, val);},
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val){ problem_.neumann_acceleration_bc(mesh_, global_ids, uv, pts, t, val);},
			local_boundary, bounday_nodes, resolution, local_neumann_boundary, rhs);
	}

	void RhsAssembler::compute_energy_grad(const std::vector< LocalBoundary > &local_boundary, const std::vector<int> &bounday_nodes, const int resolution, const std::vector< LocalBoundary > &local_neumann_boundary, const Eigen::MatrixXd &final_rhs, const double t, Eigen::MatrixXd &rhs) const
	{
		if(problem_.is_linear_in_time()){
			if(problem_.is_time_dependent())
				rhs = final_rhs;
			else
				rhs = final_rhs * t;
		}
		else
		{
			assemble(rhs, t);
			rhs *= -1;
			set_bc(local_boundary, bounday_nodes, resolution, local_neumann_boundary, rhs, t);
		}
	}

	double RhsAssembler::compute_energy(const Eigen::MatrixXd &displacement, const std::vector< LocalBoundary > &local_neumann_boundary, const int resolution, const double t) const
	{
		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> local_displacement(size_);

		double res = 0;
		Eigen::MatrixXd forces;

		if(!problem_.is_rhs_zero())
		{
#ifdef USE_TBB
		typedef tbb::enumerable_thread_specific< LocalThreadScalarStorage > LocalStorage;
		LocalStorage storages((LocalThreadScalarStorage()));
#else
		LocalThreadScalarStorage loc_storage;
#endif

		const int n_bases = int(bases_.size());

#ifdef USE_TBB
		tbb::parallel_for( tbb::blocked_range<int>(0, n_bases), [&](const tbb::blocked_range<int> &r) {
		LocalStorage::reference loc_storage = storages.local();
		for (int e = r.begin(); e != r.end(); ++e) {
#else
		for(int e=0; e < n_bases; ++e) {
#endif
			ElementAssemblyValues &vals = loc_storage.vals;
			vals.compute(e, mesh_.is_volume(), bases_[e], gbases_[e]);

			const Quadrature &quadrature = vals.quadrature;
			const Eigen::VectorXd da = vals.det.array() * quadrature.weights.array();

			problem_.rhs(formulation_, vals.val, t, forces);
			assert(forces.rows() == da.size());
			assert(forces.cols() == size_);

			for(long p = 0; p < da.size(); ++p)
			{
				local_displacement.setZero();

				for(size_t i = 0; i < vals.basis_values.size(); ++i)
				{
					const auto &bs = vals.basis_values[i];
					assert(bs.val.size() == da.size());
					const double b_val = bs.val(p);

					for(int d = 0; d < size_; ++d)
					{
						for(std::size_t ii = 0; ii < bs.global.size(); ++ii)
						{
							local_displacement(d) += (bs.global[ii].val * b_val) * displacement(bs.global[ii].index*size_ + d);
						}
					}
				}

				for(int d = 0; d < size_; ++d)
					loc_storage.val += forces(p, d) * local_displacement(d) * da(p);
					// res += forces(p, d) * local_displacement(d) * da(p);
			}
#ifdef USE_TBB
		}});
#else
		}
#endif

#ifdef USE_TBB
		for (LocalStorage::iterator i = storages.begin(); i != storages.end();  ++i)
		{
			res += i->val;
		}
#else
		res = loc_storage.val;
#endif
		}

		ElementAssemblyValues vals;
		//Neumann
		Eigen::MatrixXd points, uv;
		Eigen::VectorXd weights;
		Eigen::VectorXi global_primitive_ids;
		for(const auto &lb : local_neumann_boundary)
		{
			const int e = lb.element_id();
			bool has_samples = boundary_quadrature(lb, resolution, false, uv, points, weights, global_primitive_ids);

			if(!has_samples)
				continue;

			const ElementBases &gbs = gbases_[e];
			const ElementBases &bs = bases_[e];

			vals.compute(e, mesh_.is_volume(), points, bs, gbs);
			problem_.neumann_bc(mesh_, global_primitive_ids, uv, vals.val, t, forces);

			// UIState::ui_state().debug_data().add_points(vals.val, Eigen::RowVector3d(1,0,0));

			for(long p = 0; p < weights.size(); ++p)
			{
				local_displacement.setZero();

				for(size_t i = 0; i < vals.basis_values.size(); ++i)
				{
					const auto &vv = vals.basis_values[i];
					assert(vv.val.size() == weights.size());
					const double b_val = vv.val(p);

					for(int d = 0; d < size_; ++d)
					{
						for(std::size_t ii = 0; ii < vv.global.size(); ++ii)
						{
							local_displacement(d) += (vv.global[ii].val * b_val) * displacement(vv.global[ii].index*size_ + d);
						}
					}
				}

				for(int d = 0; d < size_; ++d)
					res -= forces(p, d) * local_displacement(d) * weights(p);
			}
		}

		return res;
	}

	bool RhsAssembler::boundary_quadrature(const LocalBoundary &local_boundary, const int order, const bool skip_computation, Eigen::MatrixXd &uv, Eigen::MatrixXd &points, Eigen::VectorXd &weights, Eigen::VectorXi &global_primitive_ids) const
	{
		uv.resize(0, 0);
		points.resize(0, 0);
		weights.resize(0);
		global_primitive_ids.resize(0);

		for(int i = 0; i < local_boundary.size(); ++i)
		{
			const int gid = local_boundary.global_primitive_id(i);
			Eigen::MatrixXd tmp_p, tmp_uv;
			Eigen::VectorXd tmp_w;
			switch(local_boundary.type())
			{
				case BoundaryType::TriLine:	 BoundarySampler::quadrature_for_tri_edge(local_boundary[i], order, gid, mesh_, tmp_uv, tmp_p, tmp_w); break;
				case BoundaryType::QuadLine: BoundarySampler::quadrature_for_quad_edge(local_boundary[i], order, gid, mesh_, tmp_uv, tmp_p, tmp_w); break;
				case BoundaryType::Quad: 	 BoundarySampler::quadrature_for_quad_face(local_boundary[i], order, gid, mesh_, tmp_uv, tmp_p, tmp_w); break;
				case BoundaryType::Tri: 	 BoundarySampler::quadrature_for_tri_face(local_boundary[i], order, gid, mesh_, tmp_uv, tmp_p, tmp_w); break;
				case BoundaryType::Invalid:  assert(false); break;
				default: assert(false);
			}

			uv.conservativeResize(uv.rows() + tmp_uv.rows(), tmp_uv.cols());
			uv.bottomRows(tmp_uv.rows()) = tmp_uv;

			points.conservativeResize(points.rows() + tmp_p.rows(), tmp_p.cols());
			points.bottomRows(tmp_p.rows()) = tmp_p;

			weights.conservativeResize(weights.rows() + tmp_w.rows(), tmp_w.cols());
			weights.bottomRows(tmp_w.rows()) = tmp_w;

			global_primitive_ids.conservativeResize(global_primitive_ids.rows() + tmp_p.rows());
			global_primitive_ids.bottomRows(tmp_p.rows()).setConstant(gid);
		}

		assert(uv.rows() == global_primitive_ids.size());
		assert(points.rows() == global_primitive_ids.size());
		assert(weights.size() == global_primitive_ids.size());

		return true;
	}

	bool RhsAssembler::sample_boundary(const LocalBoundary &local_boundary, const int n_samples, const bool skip_computation, Eigen::MatrixXd &uv, Eigen::MatrixXd &samples, Eigen::VectorXi &global_primitive_ids) const
	{
		uv.resize(0, 0);
		samples.resize(0, 0);
		global_primitive_ids.resize(0);

		for(int i = 0; i < local_boundary.size(); ++i)
		{
			Eigen::MatrixXd tmp, tmp_uv;
			switch(local_boundary.type())
			{
				case BoundaryType::TriLine:	 BoundarySampler::sample_parametric_tri_edge(local_boundary[i], n_samples, tmp_uv, tmp); break;
				case BoundaryType::QuadLine: BoundarySampler::sample_parametric_quad_edge(local_boundary[i], n_samples, tmp_uv, tmp); break;
				case BoundaryType::Quad: 	 BoundarySampler::sample_parametric_quad_face(local_boundary[i], n_samples, tmp_uv, tmp); break;
				case BoundaryType::Tri: 	 BoundarySampler::sample_parametric_tri_face(local_boundary[i], n_samples, tmp_uv, tmp); break;
				case BoundaryType::Invalid:  assert(false); break;
				default: assert(false);
			}

			uv.conservativeResize(uv.rows() + tmp_uv.rows(), tmp_uv.cols());
			uv.bottomRows(tmp_uv.rows()) = tmp_uv;

			samples.conservativeResize(samples.rows() + tmp.rows(), tmp.cols());
			samples.bottomRows(tmp.rows()) = tmp;

			global_primitive_ids.conservativeResize(global_primitive_ids.rows() + tmp.rows());
			global_primitive_ids.bottomRows(tmp.rows()).setConstant(local_boundary.global_primitive_id(i));
		}

		assert(uv.rows() == global_primitive_ids.size());
		assert(samples.rows() == global_primitive_ids.size());

		return true;
	}

}


#include "drake/solvers/fbstab/fbstab_mpc.h"

#include <memory>
#include <stdexcept>
#include <vector>

#include <Eigen/Dense>

#include "drake/solvers/fbstab/components/mpc_data.h"
#include "drake/solvers/fbstab/components/mpc_feasibility.h"
#include "drake/solvers/fbstab/components/mpc_residual.h"
#include "drake/solvers/fbstab/components/mpc_variable.h"
#include "drake/solvers/fbstab/components/riccati_linear_solver.h"

namespace drake {
namespace solvers {
namespace fbstab {

FBstabMpc::FBstabMpc(int N, int nx, int nu, int nc) {
  if (N < 1 || nx < 1 || nu < 1 || nc < 1) {
    throw std::runtime_error(
        "In FBstabMpc::FBstabMpc: problem sizes must be positive.");
  }

  N_ = N;
  nx_ = nx;
  nu_ = nu;
  nc_ = nc;
  nz_ = (nx + nu) * (N + 1);
  nl_ = nx * (N + 1);
  nv_ = nc * (N + 1);

  // create the components
  x1_ = std::make_unique<MpcVariable>(N, nx, nu, nc);
  x2_ = std::make_unique<MpcVariable>(N, nx, nu, nc);
  x3_ = std::make_unique<MpcVariable>(N, nx, nu, nc);
  x4_ = std::make_unique<MpcVariable>(N, nx, nu, nc);

  r1_ = std::make_unique<MpcResidual>(N, nx, nu, nc);
  r2_ = std::make_unique<MpcResidual>(N, nx, nu, nc);

  linear_solver_ = std::make_unique<RiccatiLinearSolver>(N, nx, nu, nc);

  feasibility_checker_ = std::make_unique<MpcFeasibility>(N, nx, nu, nc);

  algorithm_ = std::make_unique<FBstabAlgoMpc>(
      x1_.get(), x2_.get(), x3_.get(), x4_.get(), r1_.get(), r2_.get(),
      linear_solver_.get(), feasibility_checker_.get());
}

SolverOut FBstabMpc::Solve(const QPData& qp, const QPVariable* x,
                           bool use_initial_guess) {
  MpcData data(qp.Q, qp.R, qp.S, qp.q, qp.r, qp.A, qp.B, qp.c, qp.E, qp.L, qp.d,
               qp.x0);
  MpcVariable x0(x->z, x->l, x->v, x->y);

  if (data.N_ != N_ || data.nx_ != nx_ || data.nu_ != nu_ || data.nc_ != nc_) {
    throw std::runtime_error(
        "In FBstabMpc::Solve: mismatch between *this and data dimensions.");
  }
  if (x0.nz_ != nz_ || x0.nl_ != nl_ || x0.nv_ != nv_) {
    throw std::runtime_error(
        "In FBstabMpc::Solve: mismatch between *this and initial guess "
        "dimensions.");
  }

  if (!use_initial_guess) {
    x0.Fill(0.0);
  }

  return algorithm_->Solve(&data, &x0);
}

void FBstabMpc::UpdateOption(const char* option, double value) {
  algorithm_->UpdateOption(option, value);
}
void FBstabMpc::UpdateOption(const char* option, int value) {
  algorithm_->UpdateOption(option, value);
}
void FBstabMpc::UpdateOption(const char* option, bool value) {
  algorithm_->UpdateOption(option, value);
}
void FBstabMpc::SetDisplayLevel(FBstabAlgoMpc::Display level) {
  algorithm_->set_display_level(level);
}

// Explicit instantiation.
template class FBstabAlgorithm<MpcVariable, MpcResidual, MpcData,
                               RiccatiLinearSolver, MpcFeasibility>;

}  // namespace fbstab
}  // namespace solvers
}  // namespace drake


/*
 * naive.hpp
 *
 *  Created on: Apr 30, 2011
 *      Author: stewie
 */

#ifndef NAIVE_HPP_
#define NAIVE_HPP_

#include <utility>

#include <boost/multi_index_container.hpp>
#include <boost/multi_index/identity.hpp>
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/sequenced_index.hpp>

#include "../../core/BooleanDynamics.hpp"
#include "../../core/Attractor.hpp"

namespace bn {

namespace cycle_finder {

typedef boost::multi_index::multi_index_container<State,
		boost::multi_index::indexed_by<boost::multi_index::sequenced<>,
				boost::multi_index::hashed_unique<boost::multi_index::identity<
						State>, BitsetHasher> > > StateSet;

Attractor naive(BooleanDynamics& net, State s);

template<class Terminator> Attractor naive(BooleanDynamics& net, State s,
		Terminator t) {
	StateSet stateSet;
	for (size_t iter = 0; t(iter); net.update(s), ++iter) {
		const std::pair<StateSet::const_iterator, bool> p = stateSet.push_back(
				s);
		if (!p.second)
			return Attractor(std::make_pair(p.first, stateSet.end()));
	}
	return EMPTY_ATTRACTOR;
}

} // namespace cycle_finder

} // namespace bn

#endif /* NAIVE_HPP_ */


#include <memory>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <ceres/ceres.h>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <ceres-error-terms/inertial-error-term.h>
#include <ceres-error-terms/parameterization/pose-param-jpl.h>
#include <maplab-common/gravity-provider.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

using ceres_error_terms::InertialErrorTerm;

class PosegraphErrorTerms : public ::testing::Test {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 protected:
  virtual void SetUp() {
    rot0_.coeffs() << 0, 0, 0, 1;
    rot1_.coeffs() << 0, 0, 0, 1;
    pos0_ << 0, 0, 0;
    pos1_ << 1.5, 0, 0;

    accel_bias0_ << 0, 0, 0;
    accel_bias1_ << 0, 0, 0;
    gyro_bias0_ << 0, 0, 0;
    gyro_bias1_ << 0, 0, 0;

    velocity0_ << 1, 0, 0;
    velocity1_ << 2, 0, 0;

    common::GravityProvider gravity_provider(
        common::locations::kAltitudeZurichMeters,
        common::locations::kLatitudeZurichDegrees);
    gravity_magnitude_ = gravity_provider.getGravityMagnitude();

    imu_timestamps_ << 0, 0.5 * 1e9, 1.0 * 1e9;
    imu_data_ << 1, 1, 1, 0, 0, 0, gravity_magnitude_, gravity_magnitude_,
        gravity_magnitude_, 0, 0, 0, 0, 0, 0, 0, 0, 0;
  }

  void addResidual();
  void solve();

  ceres::Problem problem_;
  ceres::Solver::Summary summary_;

  Eigen::Matrix<int64_t, 1, 3> imu_timestamps_;
  Eigen::Matrix<double, 6, 3> imu_data_;

  Eigen::Quaterniond rot0_;
  Eigen::Quaterniond rot1_;
  Eigen::Vector3d pos0_;
  Eigen::Vector3d pos1_;
  Eigen::Matrix<double, 7, 1> pose0_;
  Eigen::Matrix<double, 7, 1> pose1_;

  Eigen::Vector3d accel_bias0_;
  Eigen::Vector3d accel_bias1_;
  Eigen::Vector3d gyro_bias0_;
  Eigen::Vector3d gyro_bias1_;
  Eigen::Vector3d velocity0_;
  Eigen::Vector3d velocity1_;

  double gravity_magnitude_;
};

void PosegraphErrorTerms::addResidual() {
  rot0_.normalize();
  rot1_.normalize();
  pose0_ << rot0_.coeffs(), pos0_;
  pose1_ << rot1_.coeffs(), pos1_;

  ceres::CostFunction* inertial_term_cost =
      new ceres_error_terms::InertialErrorTerm(
          imu_data_, imu_timestamps_, 1, 1, 1, 1, gravity_magnitude_);

  problem_.AddResidualBlock(
      inertial_term_cost, NULL, pose0_.data(), gyro_bias0_.data(),
      velocity0_.data(), accel_bias0_.data(), pose1_.data(), gyro_bias1_.data(),
      velocity1_.data(), accel_bias1_.data());

  ceres::LocalParameterization* pose_parameterization =
      new ceres_error_terms::JplPoseParameterization;
  problem_.SetParameterization(pose0_.data(), pose_parameterization);
  problem_.SetParameterization(pose1_.data(), pose_parameterization);
}

void PosegraphErrorTerms::solve() {
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.max_num_iterations = 500;
  options.gradient_tolerance = 1e-50;
  options.function_tolerance = 1e-50;
  options.parameter_tolerance = 1e-50;
  options.num_threads = 8;
  options.num_linear_solver_threads = 8;

  ceres::Solve(options, &problem_, &summary_);

  LOG(INFO) << summary_.message;
  LOG(INFO) << summary_.BriefReport();
}

TEST_F(PosegraphErrorTerms, InertialTermZeroCost) {
  addResidual();
  solve();

  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTerms, InertialTermFinalPositionOptimization) {
  pos1_ << 1.43, -0.2, 0.175;
  addResidual();

  problem_.SetParameterBlockConstant(gyro_bias0_.data());
  problem_.SetParameterBlockConstant(accel_bias0_.data());
  problem_.SetParameterBlockConstant(gyro_bias1_.data());
  problem_.SetParameterBlockConstant(accel_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pose0_.data());

  solve();
  EXPECT_NEAR_EIGEN(pose1_.tail(3), Eigen::Vector3d(1.5, 0, 0), 1e-15);
  EXPECT_NEAR_EIGEN(pose1_.head(4), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTerms, InertialTermFinalPositionVelocityOptimization) {
  pos1_ << 1.43, -0.2, 0.175;
  velocity1_ << 2.1, -0.2, 0.1;
  addResidual();

  problem_.SetParameterBlockConstant(gyro_bias0_.data());
  problem_.SetParameterBlockConstant(accel_bias0_.data());
  problem_.SetParameterBlockConstant(gyro_bias1_.data());
  problem_.SetParameterBlockConstant(accel_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(pose0_.data());

  solve();
  EXPECT_NEAR_EIGEN(pose1_.tail(3), Eigen::Vector3d(1.5, 0, 0), 1e-15);
  EXPECT_NEAR_EIGEN(pose1_.head(4), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_NEAR_EIGEN(velocity1_, Eigen::Vector3d(2, 0, 0), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTerms, InertialTermFinalRotationOptimization) {
  rot1_.coeffs() << 0.024225143749034013, 0.04470367401201076,
      0.04242220263937102, 0.9978051316080664;
  addResidual();

  problem_.SetParameterBlockConstant(gyro_bias0_.data());
  problem_.SetParameterBlockConstant(accel_bias0_.data());
  problem_.SetParameterBlockConstant(gyro_bias1_.data());
  problem_.SetParameterBlockConstant(accel_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pose0_.data());

  solve();
  EXPECT_NEAR_EIGEN(pose1_.tail(3), pos1_, 1e-15);
  EXPECT_NEAR_EIGEN(pose1_.head(4), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTerms, InertialTermFinalVelocityOptimization) {
  velocity1_ << 2.1, 0, 0;
  addResidual();

  problem_.SetParameterBlockConstant(gyro_bias0_.data());
  problem_.SetParameterBlockConstant(accel_bias0_.data());
  problem_.SetParameterBlockConstant(gyro_bias1_.data());
  problem_.SetParameterBlockConstant(accel_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(pose0_.data());
  problem_.SetParameterBlockConstant(pose1_.data());

  solve();
  EXPECT_NEAR_EIGEN(velocity1_, Eigen::Vector3d(2, 0, 0), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTerms, InertialTermStartPositionOptimization) {
  pos0_ << 0.13, -0.2, 0.175;
  addResidual();

  problem_.SetParameterBlockConstant(gyro_bias0_.data());
  problem_.SetParameterBlockConstant(accel_bias0_.data());
  problem_.SetParameterBlockConstant(gyro_bias1_.data());
  problem_.SetParameterBlockConstant(accel_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pose1_.data());

  solve();
  EXPECT_ZERO_EIGEN(pose0_.tail(3), 1e-15);
  EXPECT_NEAR_EIGEN(pose0_.head(4), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTerms, InertialTermStartRotationOptimization) {
  rot0_.coeffs() << 0.024225143749034013, 0.04470367401201076,
      0.04242220263937102, 0.9978051316080664;
  addResidual();

  problem_.SetParameterBlockConstant(gyro_bias0_.data());
  problem_.SetParameterBlockConstant(accel_bias0_.data());
  problem_.SetParameterBlockConstant(gyro_bias1_.data());
  problem_.SetParameterBlockConstant(accel_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(pose1_.data());

  solve();
  EXPECT_ZERO_EIGEN(pose0_.tail(3), 1e-15);
  EXPECT_NEAR_EIGEN(pose0_.head(4), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_NEAR_EIGEN(velocity1_, Eigen::Vector3d(2, 0, 0), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTerms, InertialTermStartVelocityOptimization) {
  velocity0_ << 2.1, -0.2, 0.1;
  addResidual();

  problem_.SetParameterBlockConstant(gyro_bias0_.data());
  problem_.SetParameterBlockConstant(accel_bias0_.data());
  problem_.SetParameterBlockConstant(gyro_bias1_.data());
  problem_.SetParameterBlockConstant(accel_bias1_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pose0_.data());
  problem_.SetParameterBlockConstant(pose1_.data());

  solve();
  EXPECT_NEAR_EIGEN(velocity0_, Eigen::Vector3d(1, 0, 0), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTerms, InertialTermStartGyroBiasOptimization) {
  gyro_bias0_ << 0.3, -0.2, 0.1;
  addResidual();

  problem_.SetParameterBlockConstant(accel_bias0_.data());
  problem_.SetParameterBlockConstant(gyro_bias1_.data());
  problem_.SetParameterBlockConstant(accel_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pose0_.data());
  problem_.SetParameterBlockConstant(pose1_.data());

  solve();
  EXPECT_ZERO_EIGEN(gyro_bias0_, 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTerms, InertialTermStartAccelBiasOptimization) {
  accel_bias0_ << 0.1, -0.2, 0.1;
  addResidual();

  problem_.SetParameterBlockConstant(gyro_bias0_.data());
  problem_.SetParameterBlockConstant(gyro_bias1_.data());
  problem_.SetParameterBlockConstant(accel_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pose0_.data());
  problem_.SetParameterBlockConstant(pose1_.data());

  solve();
  EXPECT_ZERO_EIGEN(accel_bias0_, 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTerms, InertialTermFinalGyroBiasOptimization) {
  gyro_bias1_ << 0.3, -0.2, 0.1;
  addResidual();

  problem_.SetParameterBlockConstant(gyro_bias0_.data());
  problem_.SetParameterBlockConstant(accel_bias0_.data());
  problem_.SetParameterBlockConstant(accel_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(pose0_.data());
  problem_.SetParameterBlockConstant(pose1_.data());

  solve();
  EXPECT_ZERO_EIGEN(gyro_bias1_, 1e-15);
  EXPECT_NEAR_EIGEN(velocity1_, Eigen::Vector3d(2, 0, 0), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTerms, InertialTermFinalAccelBiasOptimization) {
  accel_bias1_ << -0.3, -0.2, 0.1;
  addResidual();

  problem_.SetParameterBlockConstant(gyro_bias0_.data());
  problem_.SetParameterBlockConstant(accel_bias0_.data());
  problem_.SetParameterBlockConstant(gyro_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pose0_.data());
  problem_.SetParameterBlockConstant(pose1_.data());

  solve();
  EXPECT_ZERO_EIGEN(accel_bias1_, 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

MAPLAB_UNITTEST_ENTRYPOINT


/*********************************************************************************
 *
 * Inviwo - Interactive Visualization Workshop
 *
 * Copyright (c) 2015-2019 Inviwo Foundation
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *********************************************************************************/

#include <modules/vectorfieldvisualization/processors/datageneration/rbfvectorfieldgenerator2d.h>

#include <inviwo/core/datastructures/volume/volumeram.h>
#include <inviwo/core/datastructures/image/image.h>
#include <inviwo/core/datastructures/image/layerram.h>
#include <inviwo/core/datastructures/geometry/basicmesh.h>

#include <warn/push>
#include <warn/ignore/all>
#include <Eigen/Dense>
#include <warn/pop>

namespace inviwo {

const ProcessorInfo RBFVectorFieldGenerator2D::processorInfo_{
    "org.inviwo.RBFVectorFieldGenerator2D",  // Class identifier
    "RBF Based 2D Vector Field Generator",   // Display name
    "Data Creation",                         // Category
    CodeState::Stable,                       // Code state
    "CPU, Generator",                        // Tags
};
const ProcessorInfo RBFVectorFieldGenerator2D::getProcessorInfo() const { return processorInfo_; }

RBFVectorFieldGenerator2D::RBFVectorFieldGenerator2D()
    : Processor()
    , vectorField_("vectorField", DataVec2Float32::get(), false)
    , size_("size", "Volume size", ivec2(700, 700), ivec2(1, 1), ivec2(1024, 1024))
    , seeds_("seeds", "Number of seeds", 9, 1, 100)
    , randomness_("randomness", "Randomness")
    , useSameSeed_("useSameSeed", "Use same seed", true)
    , seed_("seed", "Seed", 1, 0, std::numeric_limits<int>::max())
    , shape_("shape", "Shape Parameter", 1.2f, 0.0001f, 10.0f, 0.0001f)
    , gaussian_("gaussian", "Gaussian")

    , rd_()
    , mt_(rd_())
    , theta_(0, 2 * M_PI)
    , x_(-1.0, 1.0) {
    addPort(vectorField_);

    addProperty(size_);
    addProperty(seeds_);
    addProperty(shape_);
    addProperty(gaussian_);

    addProperty(randomness_);
    randomness_.addProperty(useSameSeed_);
    randomness_.addProperty(seed_);
    useSameSeed_.onChange([&]() {
        seed_.setVisible(useSameSeed_.get());
        samples_.clear();
    });
    seed_.onChange([&]() { samples_.clear(); });
}

RBFVectorFieldGenerator2D::~RBFVectorFieldGenerator2D() {}

void RBFVectorFieldGenerator2D::process() {
    if (samples_.size() != static_cast<size_t>(seeds_.get())) {
        createSamples();
    }

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(seeds_.get(), seeds_.get());
    Eigen::VectorXd bx = Eigen::VectorXd::Zero(seeds_.get()), by(seeds_.get());
    Eigen::VectorXd xx = Eigen::VectorXd::Zero(seeds_.get()), xy(seeds_.get());

    int row = 0;
    for (auto &a : samples_) {
        int col = 0;
        for (auto &b : samples_) {
            auto r = glm::distance(a.first, b.first);
            A(row, col++) = shape_.get() + gaussian_.evaluate(r);
        }
        bx(row) = a.second.x;
        by(row++) = a.second.y;
    }

    auto solverX = A.llt();
    auto solverY = A.llt();

    xx = solverX.solve(bx);
    xy = solverY.solve(by);

    auto img = std::make_shared<Image>(size_.get(), DataVec2Float32::get());
    img->getColorLayer()->setSwizzleMask(
        {ImageChannel::Red, ImageChannel::Green, ImageChannel::Zero, ImageChannel::One});
    auto data =
        static_cast<vec2 *>(img->getColorLayer()->getEditableRepresentation<LayerRAM>()->getData());

    int i = 0;
    for (int y = 0; y < size_.get().y; y++) {
        for (int x = 0; x < size_.get().x; x++) {
            dvec2 p(x, y);
            p /= size_.get();
            p *= 2;
            p -= 1;

            vec2 v(0, 0);
            int s = 0;
            for (; s < seeds_.get(); s++) {
                double r = glm::distance(p, samples_[s].first);
                auto w = gaussian_.evaluate(r);
                v.x += static_cast<float>(xx(s) * w);
                v.y += static_cast<float>(xy(s) * w);
            }
            data[i++] = v;
        }
    }
    vectorField_.setData(img);
}

dvec2 RBFVectorFieldGenerator2D::randomVector() {
    dvec2 v(1, 0);

    auto d = theta_(mt_);
    auto c = std::cos(d);
    auto s = std::sin(d);
    return (mat2(c, s, -s, c) * v) * static_cast<float>((x_(mt_) * 2 - 1));
}

void RBFVectorFieldGenerator2D::createSamples() {
    samples_.clear();

    if (useSameSeed_.get()) {
        mt_.seed(seed_.get());
    }

    samples_.resize(seeds_.get());

    std::generate(samples_.begin(), samples_.end(), [&]() {
        auto x = x_(mt_);
        auto y = x_(mt_);
        return std::make_pair(dvec2(x, y), randomVector());
    });
}

void RBFVectorFieldGenerator2D::serialize(Serializer &s) const {
    std::vector<dvec2> sx;
    std::vector<dvec2> sy;

    for (auto sample : samples_) {
        sx.push_back(sample.first);
        sy.push_back(sample.second);
    }

    s.serialize("samplesx", sx, "samplex");
    s.serialize("samplesy", sy, "sampley");

    Processor::serialize(s);
}

void RBFVectorFieldGenerator2D::deserialize(Deserializer &d) {
    std::vector<dvec2> sx;
    std::vector<dvec2> sy;
    d.deserialize("samplesx", sx, "samplex");
    d.deserialize("samplesy", sy, "sampley");

    Processor::deserialize(d);

    for (size_t i = 0; i < sx.size(); i++) {
        samples_.emplace_back(sx[i], sy[i]);
    }
}

}  // namespace inviwo


//==================================================================================================
/*!
  @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_SIGNNZ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIGNNZ_HPP_INCLUDED

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

 /*!

    @ingroup group-ieee
    Function object implementing signnz capabilities

    Returns the sign of x. I.e. -1 or 1, according
    x is negative or positive.

    This function never returns @ref Zero (zero is considered positive for integers).

    For floating point numbers the bit of sign is taken into account and so
    we always have signnz(-z) == -signnz(z)).

    The result for @ref Nan entry is undefined.

    @par Semantic:

    @code
    auto r = signnz(x);
    @endcode

    @see Mzero, sign, is_negative, is_positive

  **/
  Value signnz(Value const & v0);
} }
#endif

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

#endif


/*
 * MIT License
 *
 * Copyright (c) 2015 Alexis LE GOADEC
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#include "include/Helly.hh"
#include "../model/include/Hypergraphe.hh"
#include "../model/include/HyperVertex.hh"
#include "../model/include/HyperEdge.hh"
#include <boost/foreach.hpp>

Helly::Helly(const boost::shared_ptr<HypergrapheAbstrait>& ptrHypergrapheAbstrait) :
				_ptrHypergrapheAbstrait( ptrHypergrapheAbstrait ) {

}

void
Helly::runAlgorithme() {

	_result.setBooleanResult(true);

	BOOST_FOREACH(auto& x, _ptrHypergrapheAbstrait->getHyperVertexList()) {
		BOOST_FOREACH(auto& y, _ptrHypergrapheAbstrait->getHyperVertexList()) {

			LibType::ListHyperEdge X_xy( allContainXY(x, y) );
			BOOST_FOREACH(auto& v, _ptrHypergrapheAbstrait->getHyperVertexList()) {

				if( voisin(x, v) && voisin(y, v) ) {
					LibType::ListHyperEdge X_xv( allContainXY(x, v) );
					LibType::ListHyperEdge X_yv( allContainXY(y, v) );

					LibType::ListHyperEdge X;
					concatenate(X, X_xy);
					concatenate(X, X_xv);
					concatenate(X, X_yv);

					if( !nonEmptyIntersection(X) ) {
						_result.setBooleanResult(false);
						return;
					}
				}
			}

		}
	}
}

bool
Helly::voisin(boost::shared_ptr<HyperVertex>& v1, boost::shared_ptr<HyperVertex>& v2) {
	BOOST_FOREACH(auto& element1, v1->getHyperEdgeList() ) {
		BOOST_FOREACH(auto& element2, v2->getHyperEdgeList() ) {
			if( element1==element2 ) {
				return true;
			}
		}
	}
	return false;
}

void
Helly::concatenate(LibType::ListHyperEdge& dest, LibType::ListHyperEdge& src) {
	BOOST_FOREACH(auto& e, src) {
		dest.push_back(e);
	}
}

bool
Helly::nonEmptyIntersection(LibType::ListHyperEdge& ensemble) {
	for(unsigned int i=0; i<ensemble.size(); i++) {
		for(unsigned int j=i+1; j<ensemble.size(); j++) {
			if( !nonEmptyBetween(ensemble.at(i), ensemble.at(j)) )
				return false;
		}
	}
	return true;
}

bool
Helly::nonEmptyBetween(boost::shared_ptr<HyperEdge>& e1, boost::shared_ptr<HyperEdge>& e2) {
	BOOST_FOREACH(auto& a, e1->getHyperVertexList()) {
		BOOST_FOREACH(auto& b, e2->getHyperVertexList()) {
			if(a==b)return true;
		}
	}
	return false;
}

LibType::ListHyperEdge&
Helly::allContainXY(boost::shared_ptr<HyperVertex>& v1, boost::shared_ptr<HyperVertex>& v2) {
	LibType::ListHyperEdge * elist = new LibType::ListHyperEdge();
	BOOST_FOREACH(auto& e, _ptrHypergrapheAbstrait->getHyperEdgeList()) {
		if( e->containVertex(v1) && e->containVertex(v2) ) {
			elist->push_back(e);
		}
	}
	return *elist;
}

RStructure
Helly::getResult() const {
	return _result;
}

Helly::~Helly() {

}


// -----------------------------------------------------------------------------
// Copyright (c) 2015-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE mitrax flat_iterator
#include <boost/test/unit_test.hpp>

#include <mitrax/iterator/flat.hpp>

#include <complex>
#include <iostream>

using namespace mitrax;

template < typename T >
auto rt_id(T&& v){
	return boost::typeindex::type_id_runtime(static_cast< T&& >(v));
}

template < typename T >
auto const id = boost::typeindex::type_id< T >();

BOOST_AUTO_TEST_SUITE(suite_flat_iterator)

BOOST_AUTO_TEST_CASE(test_1D){
	int array[2] = {1, 2};

	auto iter = flat_iterator< int, 2 >(&array);

	BOOST_TEST(iter[0] == 1);
	BOOST_TEST(iter[1] == 2);

	BOOST_TEST(*(iter + 0) == 1);
	BOOST_TEST(*(iter + 1) == 2);

	BOOST_TEST(*(iter -  0) == 1);
	BOOST_TEST(*(iter - -1) == 2);

	BOOST_TEST(*iter++ == 1);
	BOOST_TEST(*iter++ == 2);

	BOOST_TEST(*--iter == 2);
	BOOST_TEST(*--iter == 1);
	BOOST_TEST(*++iter == 2);
	BOOST_TEST(*iter-- == 2);
	BOOST_TEST(*iter == 1);

	BOOST_TEST(*(0 + iter) == 1);
	BOOST_TEST(*(1 + iter) == 2);

	BOOST_TEST((iter + 1 - iter) == 1);
}

BOOST_AUTO_TEST_CASE(test_2D){
	int array[2][3] = {{1, 2, 3}, {4, 5, 6}};

	auto iter = flat_iterator< int, 2, 3 >(&array);

	BOOST_TEST(iter[0] == 1);
	BOOST_TEST(iter[1] == 2);
	BOOST_TEST(iter[2] == 3);
	BOOST_TEST(iter[3] == 4);
	BOOST_TEST(iter[4] == 5);
	BOOST_TEST(iter[5] == 6);

	BOOST_TEST(iter[0] == 1);
	BOOST_TEST(iter[1] == 2);

	BOOST_TEST(*(iter + 0) == 1);
	BOOST_TEST(*(iter + 1) == 2);

	BOOST_TEST(*(iter -  0) == 1);
	BOOST_TEST(*(iter - -1) == 2);

	BOOST_TEST(*iter++ == 1);
	BOOST_TEST(*iter++ == 2);

	BOOST_TEST(*--iter == 2);
	BOOST_TEST(*--iter == 1);
	BOOST_TEST(*++iter == 2);
	BOOST_TEST(*iter-- == 2);
	BOOST_TEST(*iter == 1);

	BOOST_TEST(*(0 + iter) == 1);
	BOOST_TEST(*(1 + iter) == 2);

	BOOST_TEST((iter + 1 - iter) == 1);
}

BOOST_AUTO_TEST_CASE(test_3D){
	int array[2][3][4] = {
		{{1, 2, 3, 4}, {5, 6, 7, 8}, {9, 10, 11, 12}},
		{{13, 14, 15, 16}, {17, 18, 19, 20}, {21, 22, 23, 24}}
	};

	auto iter = flat_iterator< int, 2, 3, 4 >(&array);

	BOOST_TEST(iter[0] == 1);
	BOOST_TEST(iter[1] == 2);
	BOOST_TEST(iter[2] == 3);
	BOOST_TEST(iter[3] == 4);
	BOOST_TEST(iter[4] == 5);
	BOOST_TEST(iter[5] == 6);
	BOOST_TEST(iter[6] == 7);
	BOOST_TEST(iter[7] == 8);
	BOOST_TEST(iter[8] == 9);
	BOOST_TEST(iter[9] == 10);
	BOOST_TEST(iter[10] == 11);
	BOOST_TEST(iter[11] == 12);
	BOOST_TEST(iter[12] == 13);
	BOOST_TEST(iter[13] == 14);
	BOOST_TEST(iter[14] == 15);
	BOOST_TEST(iter[15] == 16);
	BOOST_TEST(iter[16] == 17);
	BOOST_TEST(iter[17] == 18);
	BOOST_TEST(iter[18] == 19);
	BOOST_TEST(iter[19] == 20);
	BOOST_TEST(iter[20] == 21);
	BOOST_TEST(iter[21] == 22);
	BOOST_TEST(iter[22] == 23);
	BOOST_TEST(iter[23] == 24);

	BOOST_TEST(iter[0] == 1);
	BOOST_TEST(iter[1] == 2);

	BOOST_TEST(*(iter + 0) == 1);
	BOOST_TEST(*(iter + 1) == 2);

	BOOST_TEST(*(iter -  0) == 1);
	BOOST_TEST(*(iter - -1) == 2);

	BOOST_TEST(*iter++ == 1);
	BOOST_TEST(*iter++ == 2);

	BOOST_TEST(*--iter == 2);
	BOOST_TEST(*--iter == 1);
	BOOST_TEST(*++iter == 2);
	BOOST_TEST(*iter-- == 2);
	BOOST_TEST(*iter == 1);

	BOOST_TEST(*(0 + iter) == 1);
	BOOST_TEST(*(1 + iter) == 2);

	BOOST_TEST((iter + 1 - iter) == 1);
}

BOOST_AUTO_TEST_SUITE_END()


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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_GEOMETRY_ALGORITHMS_DETAIL_EQUALS_COLLECT_VECTORS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_EQUALS_COLLECT_VECTORS_HPP

#include <boost/numeric/conversion/cast.hpp>
#include <boost/range.hpp>
#include <boost/typeof/typeof.hpp>

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

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

// TODO: if Boost.LA of Emil Dotchevski is accepted, adapt this
template <typename T>
struct collected_vector
{
    typedef T type;

    inline collected_vector()
    {}

    inline collected_vector(T const& px, T const& py,
            T const& pdx, T const& pdy)
        : x(px)
        , y(py)
        , dx(pdx)
        , dy(pdy)
        , dx_0(T())
        , dy_0(T())
    {}

    T x, y;
    T dx, dy;
    T dx_0, dy_0;

    // For sorting
    inline bool operator<(collected_vector<T> const& other) const
    {
        if (math::equals(x, other.x))
        {
            if (math::equals(y, other.y))
            {
                if (math::equals(dx, other.dx))
                {
                    return dy < other.dy;
                }
                return dx < other.dx;
            }
            return y < other.y;
        }
        return x < other.x;
    }

    inline bool same_direction(collected_vector<T> const& other) const
    {
        // For high precision arithmetic, we have to be
        // more relaxed then using ==
        // Because 2/sqrt( (0,0)<->(2,2) ) == 1/sqrt( (0,0)<->(1,1) )
        // is not always true (at least, it is not for ttmath)
        return math::equals_with_epsilon(dx, other.dx)
            && math::equals_with_epsilon(dy, other.dy);
    }

    // For std::equals
    inline bool operator==(collected_vector<T> const& other) const
    {
        return math::equals(x, other.x)
            && math::equals(y, other.y)
            && same_direction(other);
    }
};

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace collect_vectors
{

template <typename Range, typename Collection>
struct range_collect_vectors
{
    typedef typename boost::range_value<Collection>::type item_type;
    typedef typename item_type::type calculation_type;

    static inline void apply(Collection& collection, Range const& range)
    {
        if (boost::size(range) < 2)
        {
            return;
        }

        typedef typename boost::range_iterator<Range const>::type iterator;

        bool first = true;
        iterator it = boost::begin(range);

        for (iterator prev = it++;
            it != boost::end(range);
            prev = it++)
        {
            typename boost::range_value<Collection>::type v;

            v.x = get<0>(*prev);
            v.y = get<1>(*prev);
            v.dx = get<0>(*it) - v.x;
            v.dy = get<1>(*it) - v.y;
            v.dx_0 = v.dx;
            v.dy_0 = v.dy;

            // Normalize the vector -> this results in points+direction
            // and is comparible between geometries
            calculation_type magnitude = sqrt(
                boost::numeric_cast<calculation_type>(v.dx * v.dx + v.dy * v.dy));

            // Avoid non-duplicate points (AND division by zero)
            if (magnitude > 0)
            {
                v.dx /= magnitude;
                v.dy /= magnitude;

                // Avoid non-direction changing points
                if (first || ! v.same_direction(collection.back()))
                {
                    collection.push_back(v);
                }
                first = false;
            }
        }

        // If first one has same direction as last one, remove first one
        if (boost::size(collection) > 1
            && collection.front().same_direction(collection.back()))
        {
            collection.erase(collection.begin());
        }
    }
};

template <typename Box, typename Collection>
struct box_collect_vectors
{
    // Calculate on coordinate type, but if it is integer,
    // then use double
    typedef typename boost::range_value<Collection>::type item_type;
    typedef typename item_type::type calculation_type;

    static inline void apply(Collection& collection, Box const& box)
    {
        typename point_type<Box>::type lower_left, lower_right,
                upper_left, upper_right;
        geometry::detail::assign_box_corners(box, lower_left, lower_right,
                upper_left, upper_right);

        typedef typename boost::range_value<Collection>::type item;

        collection.push_back(item(get<0>(lower_left), get<1>(lower_left), 0, 1));
        collection.push_back(item(get<0>(upper_left), get<1>(upper_left), 1, 0));
        collection.push_back(item(get<0>(upper_right), get<1>(upper_right), 0, -1));
        collection.push_back(item(get<0>(lower_right), get<1>(lower_right), -1, 0));
    }
};

template <typename Polygon, typename Collection>
struct polygon_collect_vectors
{
    static inline void apply(Collection& collection, Polygon const& polygon)
    {
        typedef typename geometry::ring_type<Polygon>::type ring_type;

        typedef range_collect_vectors<ring_type, Collection> per_range;
        per_range::apply(collection, exterior_ring(polygon));

        typename interior_return_type<Polygon const>::type rings
                    = interior_rings(polygon);
        for (BOOST_AUTO_TPL(it, boost::begin(rings)); it != boost::end(rings); ++it)
        {
            per_range::apply(collection, *it);
        }
    }
};

template <typename MultiGeometry, typename Collection, typename SinglePolicy>
struct multi_collect_vectors
{
    static inline void apply(Collection& collection, MultiGeometry const& multi)
    {
        for (typename boost::range_iterator<MultiGeometry const>::type
                it = boost::begin(multi);
            it != boost::end(multi);
            ++it)
        {
            SinglePolicy::apply(collection, *it);
        }
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Tag,
    typename Collection,
    typename Geometry
>
struct collect_vectors
{
    static inline void apply(Collection&, Geometry const&)
    {}
};

template <typename Collection, typename Box>
struct collect_vectors<box_tag, Collection, Box>
    : detail::collect_vectors::box_collect_vectors<Box, Collection>
{};

template <typename Collection, typename Ring>
struct collect_vectors<ring_tag, Collection, Ring>
    : detail::collect_vectors::range_collect_vectors<Ring, Collection>
{};

template <typename Collection, typename LineString>
struct collect_vectors<linestring_tag, Collection, LineString>
    : detail::collect_vectors::range_collect_vectors<LineString, Collection>
{};

template <typename Collection, typename Polygon>
struct collect_vectors<polygon_tag, Collection, Polygon>
    : detail::collect_vectors::polygon_collect_vectors<Polygon, Collection>
{};

template <typename Collection, typename MultiPolygon>
struct collect_vectors<multi_polygon_tag, Collection, MultiPolygon>
    : detail::collect_vectors::multi_collect_vectors
        <
            MultiPolygon,
            Collection,
            detail::collect_vectors::polygon_collect_vectors
            <
                typename boost::range_value<MultiPolygon>::type,
                Collection
            >
        >
{};

} // namespace dispatch
#endif

/*!
    \ingroup collect_vectors
    \tparam Collection Collection type, should be e.g. std::vector<>
    \tparam Geometry geometry type
    \param collection the collection of vectors
    \param geometry the geometry to make collect_vectors
*/
template <typename Collection, typename Geometry>
inline void collect_vectors(Collection& collection, Geometry const& geometry)
{
    concept::check<Geometry const>();

    dispatch::collect_vectors
        <
            typename tag<Geometry>::type,
            Collection,
            Geometry
        >::apply(collection, geometry);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_EQUALS_COLLECT_VECTORS_HPP


#include <iostream>
#include <armadillo>
#include "RkFehl.h"
#include "DuffingOscil.h"
/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
using namespace std;
using namespace arma;

/**
 * Pars has 5 fields:
 *
 * gamma - index 0.
 * h - index 1.
 * tf - index 2.
 * x0 - index 3.
 * y0 - index 4.
 *
 *
 * @param pars
 * @return
 */
JNIEXPORT jdoubleArray JNICALL Java_net_tedkwan_javafem_jni_DuffingOscil_rk4(JNIEnv *env, jobject jobj, jdoubleArray jarray) {
    unsigned int i;
    jboolean isCopy1;

    jdouble* srcArrayElems =
            env -> GetDoubleArrayElements(jarray, &isCopy1);
    jint n = env -> GetArrayLength(jarray);
    vector<double> pars(4);
    pars[2]= srcArrayElems[1];
    pars[3]= srcArrayElems[4];
    double h = srcArrayElems[0];
    pars[0]= srcArrayElems[2];
    pars[1]= srcArrayElems[3];
    double tf = srcArrayElems[5];
    vec x = zeros<vec>(2);
    x(0) = srcArrayElems[6];
    x(1) = srcArrayElems[7];
    //x(2) = 0.75;
    //Rkfun testrk(gamma,h,tf,x);
    RkFehl testrk(pars,"vanderpol",tf,h,x);
    vector<vec> y=testrk.y;
    vector<double> t=testrk.t;

    vector<double> res;
    //jboolean isCopy2;
    unsigned int k=t.size();

    for(int j=0;j<3;j++){
        for (i = 0; i < k; i++) {
        if(j<2){
            res.push_back(y[i](j));
        }
        else{
            res.push_back(t[i]);
        }
    }
//        for(int j=0;j<4;j++){
//        for (i = 0; i < k; i++) {
//        if(j<3){
//            res.push_back(y[i](j));
//        }
//        else{
//            res.push_back(t[i]);
//        }
//    }

    }

    cout << n << endl;

    if (isCopy1 == JNI_TRUE) {
        env -> ReleaseDoubleArrayElements(jarray, srcArrayElems, JNI_ABORT);
    }
    int lenres=res.size();
    jdoubleArray result = env -> NewDoubleArray(lenres);
    env->SetDoubleArrayRegion(result, 0, lenres, &res[0]);
//    if (isCopy2 == JNI_TRUE) {
//        env -> ReleaseDoubleArrayElements(jarray, destArrayElems, 0);
//    }

    return result;
}


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
#include <stack>
//using namespace std;

#include <boost/functional/hash.hpp>
using namespace boost;

///////////////////////////////////////

void case1()
{
    std::cout << std::hex;

    std::cout << hash<int>()(0x2000) << std::endl;
    std::cout << hash<double>()(1.732) << std::endl;
    std::cout << hash<const char*>()("string") << std::endl;

    std::complex<double> c(1.0, 2.0);
    std::cout << hash<decltype(c)>()(c) << std::endl;

    std::cout << hash<std::string>()("string") << std::endl;

    std::vector<int> v(12);
    std::cout << hash<decltype(v)>()(v) << std::endl;

    std::map<int, std::string> m;
    std::cout << hash<decltype(m)>()(m);

    std::array<int, 5> ar;
    std::cout << hash<decltype(ar)>()(ar) << std::endl;

    //std::stack<int> s;
    //std::cout << hash<decltype(s)>()(s) << std::endl;
}

///////////////////////////////////////

#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>

void case2()
{
    boost::unordered_set<int, boost::hash<int> > us;
    boost::unordered_map<int, std::string, boost::hash<int> > um;
}

///////////////////////////////////////

class person final
{
private:
    int id;
    std::string name;
    unsigned int age;
public:
    person(int a, const char* b, unsigned int c):
      id(a),name(b), age(c)
    {}

    size_t hash_value() const
    {
        //return hash<int>()(id);

        size_t seed = 1984;
        hash_combine(seed, id);
        hash_combine(seed, name);
        hash_combine(seed, age);

        return seed;
    }

    friend bool operator==(person const & l, person const & r)
    {   return l.id == r.id;}

};

size_t hash_value(person const & p)
{   return p.hash_value();}

void case3()
{
    person p(1, "adam", 20);
    std::cout << hash<person>()(p) << std::endl;

    unordered_set<person> us;                   //无序集合容器

    us.insert(person(1, "adam", 20));
    us.insert(person(2, "eva", 20));
    assert(us.size() == 2);

}

///////////////////////////////////////
class demo_class
{
private:
    std::vector<std::string> v;
    int x;
public:
    size_t hash_value()
    {
        size_t seed = 0;
        hash_combine(seed, x);
        hash_range(seed, v.rbegin(), v.rend());

        return seed;
    }
};

void case4()
{
    std::vector<int> v {1,2,5,8,15};
    auto hv = hash_range(v.begin(), v.end());

    unordered_set<int> us {1,2,5,8,15};
    hv = hash_range(us.begin(), us.end());

    (void)hv;
}

///////////////////////////////////////

int main()
{
    std::cout << "hello hash" << std::endl;

    case1();
    case2();
    case3();
    case4();
}


#define BOOST_MATH_DOMAIN_ERROR_POLICY ignore_error
#include <boost/math/distributions/fisher_f.hpp>

#include "statsutil.h"

using std::size_t;

double fpval(double x, double df1, double df2)
{
    boost::math::fisher_f f(df1, df2);
    return boost::math::cdf(boost::math::complement(f, x));
}

int idummy1(const std::vector<int> &g, std::vector< std::vector<double> > &x)
{
    if ( g.empty() ) {
        x.clear();
        return 0;
    }

    auto p = std::minmax_element(g.begin(), g.end());

    if (*p.first != 0) {
        x.clear();
        return 1;
    }

    if (*p.second == 0) {
        x.clear();
        return 0;
    }

    auto n = g.size();
    auto m = static_cast<size_t>( * p.second );

    x.assign(m, std::vector<double>(n, 0.0));

    for (size_t i = 0; i < n; ++i) {
        auto k = static_cast<size_t>(g[i]);
        if (k != m)
            x[k][i] = 1.0;
        else
            for (size_t j = 0; j < m; ++j)
                x[j][i] = -1.0;
    }

    return 0;
}

int idummy2(const std::vector<int> &g, std::vector< std::vector<double> > &x)
{
    if ( g.empty() ) {
        x.clear();
        return 1;
    }

    auto p = std::minmax_element(g.begin(), g.end());

    if (*p.first != 0) {
        x.clear();
        return 1;
    }

    if (*p.second == 0) {
        x.clear();
        return 0;
    }

    auto n = g.size();
    auto m = static_cast<size_t>( * p.second );

    x.assign(m, std::vector<double>(n, 0.0));

    for (size_t i = 0; i < n; ++i) {
        auto k = static_cast<size_t>(g[i]);
        if (k != m)
            x[k][i] = 1.0;
    }

    return 0;
}

int idummy3(const std::vector<int> &g, std::vector< std::vector<double> > &x)
{
    if ( g.empty() ) {
        x.clear();
        return 0;
    }

    auto p = std::minmax_element(g.begin(), g.end());

    if (*p.first != 0) {
        x.clear();
        return 1;
    }

    if (*p.second == 0) {
        x.assign(1, std::vector<double>(g.size(), 1.0));
        return 0;
    }

    auto n = g.size();
    auto m = static_cast<size_t>( * p.second );

    x.assign(m + 1, std::vector<double>(n, 0.0));

    for (size_t i = 0; i < n; ++i) {
        auto k = static_cast<size_t>(g[i]);
        x[k][i] = 1.0;
    }

    return 0;
}


#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <boost/algorithm/string.hpp>

#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>

// additional pcl headers
#include <pcl/ModelCoefficients.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>

#define KEYSYM_ARROW_LEFT "Left"
#define KEYSYM_ARROW_RIGHT "Right"

// global variables
static int counter_ = 0;
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr_(new pcl::PointCloud<pcl::PointXYZRGB>);
static pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_(new pcl::PointCloud<pcl::Normal>);
static boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_(new pcl::visualization::PCLVisualizer ("3D Viewer"));
static std::ifstream coefficientsFile_;
static std::vector<pcl::ModelCoefficients::Ptr> coefficients_;

// prototypes
void calculateNormals(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_ptr, pcl::PointCloud<pcl::Normal>::Ptr normals_cloud_ptr, float radius=0.1f);
void loadPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_ptr, pcl::PointCloud<pcl::Normal>::Ptr normals_cloud_ptr, int idx);
void update();

void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void* viewer_void)
{
    if(event.keyDown()) {
        if(event.getKeySym() == KEYSYM_ARROW_LEFT)
            counter_ = (counter_-1) >= 0 ? (counter_-1) : 0;
        else if(event.getKeySym() == KEYSYM_ARROW_RIGHT)
            counter_++;
        else if(event.getKeySym() == "r")
            counter_ = 0;

        if(counter_ >= coefficients_.size()) \
            counter_ = coefficients_.size()-1;

        std::cout << "Index: " << counter_ << std::endl;
        loadPointCloud(point_cloud_ptr_, cloud_normals_, counter_);
        update();
    }
}

void mouseEventOccurred(const pcl::visualization::MouseEvent &event, void* viewer_void)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *> (viewer_void);
    if (event.getButton () == pcl::visualization::MouseEvent::LeftButton &&
        event.getType () == pcl::visualization::MouseEvent::MouseButtonRelease) {
        // TODO
    }
}

void calculateNormals(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_ptr, pcl::PointCloud<pcl::Normal>::Ptr normals_cloud_ptr, float radius)
{
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
    ne.setInputCloud(cloud_ptr);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB> ());
    ne.setSearchMethod(tree);
    ne.setRadiusSearch(radius);
    ne.compute(*normals_cloud_ptr);
}

void loadPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_ptr, pcl::PointCloud<pcl::Normal>::Ptr normals_cloud_ptr, int idx)
{
    std::stringstream filename;
    filename << "/usr/stud/mueller/test_pcd" << idx << ".pcd";
    if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename.str().c_str(), *cloud_ptr) == -1) {
        std::cerr << "Couldn't read file " << filename.str().c_str() << std::endl;
        return;
    }
    calculateNormals(cloud_ptr, normals_cloud_ptr);
}

void update()
{
    viewer_->updatePointCloud(point_cloud_ptr_, "cloud");
//    viewer_->removePointCloud("normals");
//    viewer_->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (point_cloud_ptr_, cloud_normals_, 10, 0.05, "normals");
    viewer_->removeShape("cylinder");
    viewer_->addCylinder(*coefficients_[counter_]);
}

int main(int argc, char** argv)
{
    // Init viewer
    viewer_->setBackgroundColor (45.0/255.0,45.0/255.0,45.0/255.0);
    viewer_->addCoordinateSystem (0.1);
    viewer_->initCameraParameters ();

    // Register callbacks
    viewer_->registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer_);
    viewer_->registerMouseCallback (mouseEventOccurred, (void*)&viewer_);

    // Load files
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr initial_pc_ptr_(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::Normal>::Ptr initial_normals_(new pcl::PointCloud<pcl::Normal>);
    loadPointCloud(initial_pc_ptr_, initial_normals_, counter_);
    loadPointCloud(point_cloud_ptr_, cloud_normals_, counter_);
    coefficientsFile_.open("/usr/stud/mueller/cylinderCoefficients.txt");
    std::string line;
    if(coefficientsFile_.is_open()) {
        while(std::getline(coefficientsFile_, line)) {
            std::vector<std::string> strs;
            boost::split(strs, line, boost::is_any_of(" "));

            std::vector<float> c(strs.size());
            for(int i=0; i<strs.size(); i++) {
                c[i] = std::atof(strs[i].c_str());
            }

            pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
            coeff->values = c;
            coefficients_.push_back(coeff);
        }
        coefficientsFile_.close();
    } else {
        std::cerr << "Could not open cylinder coefficient file." << std::endl;
        return 0;
    }

    // Add from file
//    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorH(initial_pc_ptr_);
//    viewer_->addPointCloud<pcl::PointXYZRGB> (initial_pc_ptr_, colorH, "initialCloud");
//    viewer_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "initialCloud");
//    viewer_->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (initial_pc_ptr_, initial_normals_, 10, 0.05, "initialNormals");
    viewer_->addCylinder(*coefficients_[0]);

    // Add pointclouds
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(point_cloud_ptr_);
    viewer_->addPointCloud<pcl::PointXYZRGB> (point_cloud_ptr_, rgb, "cloud");
    viewer_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
//    viewer_->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (point_cloud_ptr_, cloud_normals_, 10, 0.05, "normals");

    // Main loop
    while(!viewer_->wasStopped()) {
        viewer_->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }

    // Clean up
    coefficients_.clear();

    return 0;
}