#ifndef FIRST_ORDER_VISIBILITY_GRAPH_HPP
#define FIRST_ORDER_VISIBILITY_GRAPH_HPP

#include <memory>
#include <functional>
#include <Eigen/Core>
#include <omp.h>
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include "arc_utilities/arc_helpers.hpp"

namespace arc_utilities
{
    class FirstOrderVisibilityGraph
    {
        public:
            typedef std::pair<ssize_t, ssize_t> ConfigType;
            typedef std::pair<ConfigType, double> ConfigAndDistType;
            typedef std::function<bool(const ssize_t row, const ssize_t col)> ValidityCheckFnType;

            static double ConfigTypeDistance(const ConfigType& c1, const ConfigType& c2)
            {
                return Eigen::Vector2d((double)(c1.first - c2.first), (double)(c1.second - c2.second)).norm();
            }

            struct BestFirstSearchComparator
            {
                public:
                    // Defines a "less" operation"; by using "greater" then the smallest element will appear at the top of the priority queue
                    bool operator()(const ConfigAndDistType& c1, const ConfigAndDistType& c2) const
                    {
                        // If expected distances are different, we want to explore the one with the smaller expected distance
                        return (c1.second > c2.second);
                    }
            };

            static bool CheckFirstOrderVisibility(const ssize_t rows, const ssize_t cols, const ValidityCheckFnType& validity_check_fn, const bool visualization_enabled = true)
            {
                assert(rows > 0 && cols > 0);
                typedef Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> ArrayXb;

                ros::NodeHandle nh;
                ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 1000, true);

                visualization_msgs::Marker marker;
                {
                    marker.header.frame_id = "mocap_world";
                    marker.type = visualization_msgs::Marker::POINTS;
                    marker.action = visualization_msgs::Marker::ADD;
                    marker.ns = "explored_states";
                    marker.id = 1;
                    marker.scale.x = 1.0;
                    marker.scale.y = 1.0;
                    marker.header.stamp = ros::Time::now();
                    marker_pub.publish(marker);
                    marker.color = arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(0.0, 0.0, 1.0, 1.0);
                }

                const ConfigType start(0, 0), goal(rows - 1, cols - 1);
                const auto heuristic_distance_fn = [&goal] (const ConfigType& config) { return ConfigTypeDistance(config, goal); };

                std::priority_queue<ConfigAndDistType, std::vector<ConfigAndDistType>, BestFirstSearchComparator> frontier;
                ArrayXb explored = ArrayXb::Constant(rows, cols, false);

                frontier.push(ConfigAndDistType(start, heuristic_distance_fn(start)));

                bool path_found = false;
                std::cout << "Entering explore loop\n\n";
                while (!path_found && ros::ok() && frontier.size() > 0)
                {
                    const ConfigAndDistType current = frontier.top();
                    frontier.pop();
                    const ConfigType& current_config = current.first;

                    // Visualization code
                    if (visualization_enabled)
                    {
                        geometry_msgs::Point p;
                        p.x = current_config.first;
                        p.y = current_config.second;
                        p.z = 0;
                        ++marker.id;
                        marker.points.push_back(p);

                        if (marker.id % 1000 == 0)
                        {
                            marker.header.stamp = ros::Time::now();
                            marker_pub.publish(marker);
                            marker.points.clear();
                            usleep(10);
                        }
                    }

                    if (current_config.first == goal.first && current_config.second == goal.second)
                    {
                        if (visualization_enabled)
                        {
                            std::cout << "Reached goal!\n";
                            std::cout << PrettyPrint::PrettyPrint(current_config, true, " ") << std::endl << std::flush;
                            std::cout << std::endl;
                            marker.header.stamp = ros::Time::now();
                            marker_pub.publish(marker);
                        }
                        path_found = true;
                    }
                    // Double check if we've already explored this node:
                    //    a single node can be inserted into the frontier multiple times at the same or different priorities
                    //    so we want to avoid the expense of re-exploring it, and just discard this one once we pop it
                    else if (explored(current_config.first, current_config.second) == false)
                    {
                        explored(current_config.first, current_config.second) = true;

                        // Expand the node to find all neighbours, adding them to the frontier if we have not already explored them
                        const auto neighbours = GetNeighbours(current_config, rows, cols, validity_check_fn);
                        for (const auto neighbour : neighbours)
                        {
                            // Check if we've already explored this neighbour to avoid re-adding it to the frontier
                            if (explored(neighbour.first, neighbour.second) == false)
                            {
                                frontier.push(ConfigAndDistType(neighbour, heuristic_distance_fn(neighbour)));
                            }
                        }
                    }
                }

                if (visualization_enabled)
                {
                    marker.header.stamp = ros::Time::now();
                    marker_pub.publish(marker);
                }

                return path_found;
            }

        private:
            FirstOrderVisibilityGraph() {}

            static std::vector<ConfigType> GetNeighbours(const ConfigType& config, const ssize_t rows, const ssize_t cols, const ValidityCheckFnType& validity_check_fn)
            {
                std::vector<ConfigType> neighbours;
                neighbours.reserve(8);

                const ssize_t row_min = std::max(0L, config.first - 1);
                const ssize_t row_max = std::min(rows - 1, config.first + 1);

                const ssize_t col_min = std::max(0L, config.second - 1);
                const ssize_t col_max = std::min(cols - 1, config.second + 1);

                for (ssize_t col = col_min; col <= col_max; col++)
                {
                    for (ssize_t row = row_min; row <= row_max; row++)
                    {
                        if (!(row == config. first && col == config.second) && validity_check_fn(row, col) == true)
                        {
                            neighbours.push_back(ConfigType(row, col));
                        }
                    }
                }

                return neighbours;
            }

    };
}

#endif // FIRST_ORDER_VISIBILITY_GRAPH_HPP


/*Deep euler implementation*/

#include <iostream>
#include <fstream>
#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <string>
#include <chrono>
#include <regex>

#include <boost/program_options.hpp>
#include <torch/script.h>

using namespace std;

//const int N = 16;
//const int N_z = N / 2 - 1;
const int nn_inputs = 4;
const int nn_outputs = 2; // output dimension
c10::TensorOptions global_tensor_op;

string file_name = "../lotka_dem.txt";
string file_name_normal_euler = "../lotka_euler.txt";
string time_counting_file_name = "../clock.txt";
string model_file = "../../training/traced_model_e20_2021_11_04.pt";
string range_model_file = "../../training/traced_range_model_e20_2021_11_04.pt";
string embedded_model_file = "../../training/traced_range_embedded_model_e22_2021_11_10.pt";
static bool use_embedded = false;
static bool use_generalized = false;

typedef double value_type;
typedef vector<value_type> state_type;

//ode function of bubble dynamic
class lotka {
//	std::ofstream outputs_out;
public:
	torch::jit::script::Module model;
	torch::Tensor inputs; //reused tensor of inputs

	lotka(std::vector<double> inital_values) {

		//metamodel initializations
		int _size = inital_values.size();
		inputs = torch::ones({ 1, _size }, global_tensor_op);
		//model inputs: dt x1 x2 x3 z... x1now x2now x3now
		for (int i = 0; i < _size; i++) inputs[0][i] = inital_values[i];
		//outputs: z... grad_z(wall)
		try {
			if (use_embedded) {
				model = torch::jit::load(embedded_model_file);
			}
			else if(use_generalized) {
				model = torch::jit::load(range_model_file);
			}
			else {
				model = torch::jit::load(model_file);
			}
			std::vector<torch::jit::IValue> inp;
			inp.push_back(torch::ones({ 1, _size }, global_tensor_op));
			std::cout << inp << endl;
			// Execute the model and turn its output into a tensor.
			at::Tensor output = model.forward(inp).toTensor().detach();
			std::cout << output << endl;
		}
		catch (const c10::Error& e) {
			std::cerr << "Error loading the model: " << e.what() << endl;
			//	exit(-1);
		}
	}

	/*Rewrites the errors array with the predicted local truncation errors*/
	void local_error(double t, double t_next, const double* x, double* errors) {
		//updating inputs
		inputs[0][0] = t_next;
		inputs[0][1] = t;
		for (int i = 0; i < nn_inputs - 2; i++) {
			inputs[0][i + 2] = x[i];
		}
		std::vector<torch::jit::IValue> inps;
		inps.push_back(inputs);
		//evaluating
		torch::Tensor loc_trun_err = model.forward(inps).toTensor().detach();

		for (int i = 0; i < nn_outputs; i++) {
			errors[i] = loc_trun_err[0][i].item<double>();
		}
	}

	/*ODE function*/
	inline void operator()(double t, double* x, double* dx) {
		dx[0] = x[0] - x[0] * x[1];
		dx[1] = -x[1] + x[0] * x[1];
	}
};

class ODESolver
{
public:
	// order is the number of the output
	ODESolver(int order): order(order) {
	// initialize necessary data structures
		derivative = (double*)malloc(sizeof(double) * order);
		local_error = (double*)malloc(sizeof(double) * order);
	};

	bool setInitialCondition(double* conds, double at) {
		begin_t = at;
		init_conds = (double*)malloc(sizeof(double) * order);
		for (int u = 0; u < order; u++) {
			init_conds[u] = conds[u];
		}
		return true;
	}

	void setTimeStep(double dt) {
		delta_t = dt;
	}

	bool setStepNumber(int steps) {
		max_l = steps;
		sol_t = (double*)malloc(sizeof(double) * (max_l + 1));
		sol = new double* [max_l + 1];
		for (int i = 0; i < max_l + 1; i++)
			sol[i] = new double[order];
		return true;
	}

	void solve(lotka& lot) {

		for (int u = 0; u < order; u++) {
			sol[0][u] = init_conds[u];
		}

		double t = begin_t;
		sol_t[0] = t;
		int l = 0;
		while (l < max_l) {
			lot.local_error(t, t + delta_t, sol[l], local_error); // fill the local_error
			lot(t, sol[l], derivative); // fill the derivative
			for (int j = 0; j < order; j++) {
				sol[l + 1][j] = sol[l][j] + delta_t * derivative[j] + delta_t * delta_t * local_error[j];
			}
			l++;
			t += delta_t;
			sol_t[l] = t;
		}
	}

	// disable residue approximation
	void solve_normal(lotka& lot) {

		for (int u = 0; u < order; u++) {
			sol[0][u] = init_conds[u];
		}

		double t = begin_t;
		sol_t[0] = t;
		int l = 0;
		while (l < max_l) {
			lot(t, sol[l], derivative); // fill the derivative
			for (int j = 0; j < order; j++) {
				sol[l + 1][j] = sol[l][j] + delta_t * derivative[j];
			}
			l++;
			t += delta_t;
			sol_t[l] = t;
		}
	}

	// output sol_t and sol
	void output(ofstream& out) {
		for (int i = 0; i <= max_l; i++) {
			out << sol_t[i];
			for (int j = 0; j < order; j++) {
				out << ' ' << sol[i][j];
			}
			out << '\n';
		}
	}

	~ODESolver(){
		free(init_conds);
		free(derivative);
		free(sol_t);
		for (int indx = 0; indx <= max_l; ++indx)
		{
			delete sol[indx];
		}
		delete[] sol;
	}
private:
	int order = 1;
	double* local_error;
	double* init_conds;
	double* sol_t;
	double** sol;
	double* derivative;
	double begin_t = 0;
	double delta_t = 0.1;
	int max_l = 10;
};

void setup_ofstream(ofstream& ofs) {
	if(!ofs.is_open())exit(-1);
	ofs.precision(17);
	ofs.flags(ios::scientific);
}

int main(int argc, const char* argv[]) {
	boost::program_options::options_description desc;
	desc.add_options()
		("help,h", "Show this help screen")
		("embedded", boost::program_options::value<bool>()->implicit_value(true)->default_value(false), "whether to use embedded model")
		("generalized", boost::program_options::value<bool>()->implicit_value(true)->default_value(false), "whether to use embedded model");

	boost::program_options::variables_map vm;
	boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
	boost::program_options::notify(vm);
	if (vm.count("help")) {
		std::cout << desc << '\n';
		return 0;
	}
	use_embedded = vm["embedded"].as<bool>();
	use_generalized = vm["generalized"].as<bool>();
	global_tensor_op = torch::TensorOptions().dtype(torch::kFloat64);
	std::cout << "Lotka Volterra with meta-model started\n" << setprecision(17) << endl;

	double* x = new double[nn_outputs]{ 2.0, 1.0 };

	double dem_step_list[] = {0.1, 0.05, 0.01, 0.005};
	double euler_step_list[] = { 0.002, 0.001, 0.0005, 0.0002};
	int step_list_length = (sizeof(dem_step_list) / sizeof(*dem_step_list));
	std::regex _regex(".txt");
	std::string replace_base;
	if (use_embedded) {
		replace_base = "_embedded.txt";
	}
	else if (use_generalized) {
		replace_base = "_generalized.txt";
	}
	else {
		replace_base = ".txt";
	}
	std::string time_counting_file_name_special = std::regex_replace(time_counting_file_name, _regex, replace_base);
	ofstream clock_of(time_counting_file_name_special);
	double t_stop = 15.0;
	//initial conditions
	std::vector<double> initial_inputs;
	if (use_generalized) {
		initial_inputs = { 0.0, 0.1, 2.0, 1.0, 1.0, 1.0, 1.0, 1.0, 2.0, 1.0 };
	}
	else {
		initial_inputs = { 1e-5, 0.0, 2.0, 1.0 };
	}
	for (int i = 0; i < step_list_length; i++) {

		std::string file_name_i = std::regex_replace(file_name, _regex, std::to_string(i) + replace_base);
		std::string file_name_normal_euler_i = std::regex_replace(file_name_normal_euler, _regex, std::to_string(i) + ".txt");

		ofstream ofs(file_name_i);
		setup_ofstream(ofs);
		std::cout << "Writing file: " << file_name_i << endl;
		ofstream ofs_2(file_name_normal_euler_i);
		setup_ofstream(ofs_2);

		double t_start = 0.0;
		lotka bubi(initial_inputs);

		ODESolver solver(nn_outputs), solver_2(nn_outputs);
		solver.setInitialCondition(x, 0.0);
		solver.setTimeStep(dem_step_list[i]);
		solver.setStepNumber(int(t_stop/ dem_step_list[i]));

		std::cout << "Solving..." << endl;
		auto t1 = chrono::high_resolution_clock::now();
		solver.solve(bubi);
		auto t2 = chrono::high_resolution_clock::now();
		// std::cout << "DEM Time (ms):" << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count() << endl;
		clock_of << std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
		solver.output(ofs);

		solver_2.setInitialCondition(x, 0.0);
		solver_2.setTimeStep(euler_step_list[i]);
		solver_2.setStepNumber(int(t_stop / euler_step_list[i]));
		t1 = chrono::high_resolution_clock::now();
		solver_2.solve_normal(bubi);
		t2 = chrono::high_resolution_clock::now();
		// std::cout << "EM Time (ms):" << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count() << endl;
		clock_of << ' ' << std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count() << endl;
		solver_2.output(ofs_2);
		ofs.flush();
		ofs.close();
		ofs_2.flush();
		ofs_2.close();
	}
	clock_of.close();
	std::cout << "Ready"<< endl;
	return 0;
}


#include <iostream>

//--- SpatialOps includes ---//
#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/structured/IntVec.h>
#include <spatialops/structured/MemoryWindow.h>
#include <spatialops/structured/stencil/FVStaggeredOperatorTypes.h>
#include <spatialops/OperatorDatabase.h>
#include <spatialops/structured/stencil/StencilBuilder.h>
#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/Nebo.h>

//-- boost includes ---//
#include <boost/program_options.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

namespace po = boost::program_options;

using namespace SpatialOps;

#define RUN_TEST(TEST,							\
		 TYPE)							\
  boost::posix_time::ptime start( boost::posix_time::microsec_clock::universal_time() ); \
  boost::posix_time::ptime end( boost::posix_time::microsec_clock::universal_time() ); \
  int ii = 0;								\
  start = boost::posix_time::microsec_clock::universal_time();		\
									\
  for(; ii < number_of_runs; ii++) {					\
    TEST;								\
  };									\
									\
  end = boost::posix_time::microsec_clock::universal_time();		\
  std::cout << TYPE;							\
  std::cout << " runs: ";						\
  std::cout << number_of_runs;						\
  std::cout << " result: ";						\
  std::cout << (end - start).total_microseconds()*1e-6;			\
  std::cout << std::endl;

#define build_stencil_point(f, stencil_offset)                                  \
    (FieldType(MemoryWindow(f.window_without_ghost().glob_dim(),                \
                            f.window_without_ghost().offset() + stencil_offset, \
                            f.window_without_ghost().extent()),                 \
	       f) )

template<typename FieldType>
inline void evaluate_serial_example(FieldType & result,
				    FieldType const & phi,
				    FieldType const & dCoef,
				    IntVec const npts,
				    double const Lx,
				    double const Ly,
				    double const Lz,
				    int number_of_runs) {

    SpatialOps::OperatorDatabase opDB;
    SpatialOps::build_stencils(npts[0],
                                           npts[1],
                                           npts[2],
                                           Lx,
                                           Ly,
                                           Lz,
                                           opDB);

    typename BasicOpTypes<FieldType>::GradX* const gradXOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradX>();
    typename BasicOpTypes<FieldType>::GradY* const gradYOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradY>();
    typename BasicOpTypes<FieldType>::GradZ* const gradZOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradZ>();
    typename BasicOpTypes<FieldType>::InterpC2FX* const interpXOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FX>();
    typename BasicOpTypes<FieldType>::InterpC2FY* const interpYOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FY>();
    typename BasicOpTypes<FieldType>::InterpC2FZ* const interpZOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FZ>();
    typename BasicOpTypes<FieldType>::DivX* const divXOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivX>();
    typename BasicOpTypes<FieldType>::DivY* const divYOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivY>();
    typename BasicOpTypes<FieldType>::DivZ* const divZOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivZ>();

    MemoryWindow const w = phi.window_with_ghost();
    const GhostData& g = phi.get_ghost_data();
    const BoundaryCellInfo& bc = phi.boundary_info();
    typename FaceTypes<FieldType>::XFace  tmpFaceX( w, bc, g, NULL );
    typename FaceTypes<FieldType>::XFace tmpFaceX2( w, bc, g, NULL );
    FieldType tmpX( w, bc, g, NULL );
    typename FaceTypes<FieldType>::YFace  tmpFaceY( w, bc, g, NULL );
    typename FaceTypes<FieldType>::YFace tmpFaceY2( w, bc, g, NULL );
    FieldType tmpY( w, bc, g, NULL );
    typename FaceTypes<FieldType>::ZFace  tmpFaceZ( w, bc, g, NULL );
    typename FaceTypes<FieldType>::ZFace tmpFaceZ2( w, bc, g, NULL );
    FieldType tmpZ( w, bc, g, NULL );

    RUN_TEST(// X - direction
	     gradXOp_  ->apply_to_field( phi,    tmpFaceX  );
	     interpXOp_->apply_to_field( dCoef, tmpFaceX2 );
	     tmpFaceX <<= tmpFaceX * tmpFaceX2;
	     divXOp_->apply_to_field( tmpFaceX, tmpX );

	     // Y - direction
	     gradYOp_  ->apply_to_field( phi,    tmpFaceY  );
	     interpYOp_->apply_to_field( dCoef, tmpFaceY2 );
	     tmpFaceY <<= tmpFaceY * tmpFaceY2;
	     divYOp_->apply_to_field( tmpFaceY, tmpY );

	     // Z - direction
	     gradZOp_  ->apply_to_field( phi,    tmpFaceZ  );
	     interpZOp_->apply_to_field( dCoef, tmpFaceZ2 );
	     tmpFaceZ <<= tmpFaceZ * tmpFaceZ2;
	     divZOp_->apply_to_field( tmpFaceZ, tmpZ );

	     result <<= - tmpX - tmpY - tmpZ,
	     "old");

};

template<typename FieldType>
inline void evaluate_chaining_example(FieldType & result,
				      FieldType const & phi,
				      FieldType const & dCoef,
				      IntVec const npts,
				      double const Lx,
				      double const Ly,
				      double const Lz,
				      int number_of_runs) {

    SpatialOps::OperatorDatabase opDB;
    SpatialOps::build_stencils(npts[0],
                                           npts[1],
                                           npts[2],
                                           Lx,
					   Ly,
					   Lz,
                                           opDB);

    typename BasicOpTypes<FieldType>::GradX* const gradXOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradX>();
    typename BasicOpTypes<FieldType>::GradY* const gradYOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradY>();
    typename BasicOpTypes<FieldType>::GradZ* const gradZOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradZ>();
    typename BasicOpTypes<FieldType>::InterpC2FX* const interpXOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FX>();
    typename BasicOpTypes<FieldType>::InterpC2FY* const interpYOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FY>();
    typename BasicOpTypes<FieldType>::InterpC2FZ* const interpZOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FZ>();
    typename BasicOpTypes<FieldType>::DivX* const divXOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivX>();
    typename BasicOpTypes<FieldType>::DivY* const divYOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivY>();
    typename BasicOpTypes<FieldType>::DivZ* const divZOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivZ>();

    IntVec const neutral = IntVec(0,0,0);
    IntVec const neg_X = IntVec(-1,0,0);
    IntVec const pos_X = IntVec(1,0,0);
    IntVec const neg_Y = IntVec(0,-1,0);
    IntVec const pos_Y = IntVec(0,1,0);
    IntVec const neg_Z = IntVec(0,0,-1);
    IntVec const pos_Z = IntVec(0,0,1);

    RUN_TEST(FieldType r = build_stencil_point(result, neutral);

	     FieldType const phi_xlxl = build_stencil_point(phi, neg_X);
	     FieldType const phi_xlxh = build_stencil_point(phi, neutral);
	     FieldType const phi_xhxl = build_stencil_point(phi, neutral);
	     FieldType const phi_xhxh = build_stencil_point(phi, pos_X);
	     FieldType const phi_ylyl = build_stencil_point(phi, neg_Y);
	     FieldType const phi_ylyh = build_stencil_point(phi, neutral);
	     FieldType const phi_yhyl = build_stencil_point(phi, neutral);
	     FieldType const phi_yhyh = build_stencil_point(phi, pos_Y);
	     FieldType const phi_zlzl = build_stencil_point(phi, neg_Z);
	     FieldType const phi_zlzh = build_stencil_point(phi, neutral);
	     FieldType const phi_zhzl = build_stencil_point(phi, neutral);
	     FieldType const phi_zhzh = build_stencil_point(phi, pos_Z);

	     FieldType const dCoef_xlxl = build_stencil_point(dCoef, neg_X);
	     FieldType const dCoef_xlxh = build_stencil_point(dCoef, neutral);
	     FieldType const dCoef_xhxl = build_stencil_point(dCoef, neutral);
	     FieldType const dCoef_xhxh = build_stencil_point(dCoef, pos_X);
	     FieldType const dCoef_ylyl = build_stencil_point(dCoef, neg_Y);
	     FieldType const dCoef_ylyh = build_stencil_point(dCoef, neutral);
	     FieldType const dCoef_yhyl = build_stencil_point(dCoef, neutral);
	     FieldType const dCoef_yhyh = build_stencil_point(dCoef, pos_Y);
	     FieldType const dCoef_zlzl = build_stencil_point(dCoef, neg_Z);
	     FieldType const dCoef_zlzh = build_stencil_point(dCoef, neutral);
	     FieldType const dCoef_zhzl = build_stencil_point(dCoef, neutral);
	     FieldType const dCoef_zhzh = build_stencil_point(dCoef, pos_Z);

	     double gXcl = gradXOp_->get_minus_coef();
	     double gXch = gradXOp_->get_plus_coef();
	     double iXcl = interpXOp_->get_minus_coef();
	     double iXch = interpXOp_->get_plus_coef();
	     double dXcl = divXOp_->get_minus_coef();
	     double dXch = divXOp_->get_plus_coef();
	     double gYcl = gradYOp_->get_minus_coef();
	     double gYch = gradYOp_->get_plus_coef();
	     double iYcl = interpYOp_->get_minus_coef();
	     double iYch = interpYOp_->get_plus_coef();
	     double dYcl = divYOp_->get_minus_coef();
	     double dYch = divYOp_->get_plus_coef();
	     double gZcl = gradZOp_->get_minus_coef();
	     double gZch = gradZOp_->get_plus_coef();
	     double iZcl = interpZOp_->get_minus_coef();
	     double iZch = interpZOp_->get_plus_coef();
	     double dZcl = divZOp_->get_minus_coef();
	     double dZch = divZOp_->get_plus_coef();

	     r <<= (- (dXcl * ((gXcl * phi_xlxl + gXch * phi_xhxl) * (iXcl * dCoef_xlxl + iXch * dCoef_xhxl)) +
		       dXch * ((gXcl * phi_xlxh + gXch * phi_xhxh) * (iXcl * dCoef_xlxh + iXch * dCoef_xhxh)))
		    - (dYcl * ((gYcl * phi_ylyl + gYch * phi_yhyl) * (iYcl * dCoef_ylyl + iYch * dCoef_yhyl)) +
		       dYch * ((gYcl * phi_ylyh + gYch * phi_yhyh) * (iYcl * dCoef_ylyh + iYch * dCoef_yhyh)))
		    - (dZcl * ((gZcl * phi_zlzl + gZch * phi_zhzl) * (iZcl * dCoef_zlzl + iZch * dCoef_zhzl)) +
		       dZch * ((gZcl * phi_zlzh + gZch * phi_zhzh) * (iZcl * dCoef_zlzh + iZch * dCoef_zhzh)))),
	     "new");

};

int main(int iarg, char* carg[]) {
    typedef SVolField Field;

    int nx, ny, nz;
    int number_of_runs;
    double Lx, Ly, Lz;
#ifdef ENABLE_THREADS
  int thread_count;
#endif

    // parse the command line options input describing the problem
    {
        po::options_description desc("Supported Options");
	desc.add_options()
	  ( "help", "print help message" )
	  ( "nx", po::value<int>(&nx)->default_value(10), "Grid in x" )
	  ( "ny", po::value<int>(&ny)->default_value(10), "Grid in y" )
	  ( "nz", po::value<int>(&nz)->default_value(10), "Grid in z" )
	  ( "Lx", po::value<double>(&Lx)->default_value(1.0),"Length in x")
	  ( "Ly", po::value<double>(&Ly)->default_value(1.0),"Length in y")
	  ( "Lz", po::value<double>(&Lz)->default_value(1.0),"Length in z")
#ifdef ENABLE_THREADS
      ( "tc", po::value<int>(&thread_count)->default_value(NTHREADS), "Number of threads for Nebo")
#endif
	  ( "runs", po::value<int>(&number_of_runs)->default_value(1), "Number of iterations of each test");

	po::variables_map args;
	po::store( po::parse_command_line(iarg,carg,desc), args );
	po::notify(args);

	if (args.count("help")) {
	    std::cout << desc << "\n";
	    return 1;
	}

#ifdef ENABLE_THREADS
    set_hard_thread_count(thread_count);
#endif
    }

    const GhostData ghost(1);
    const BoundaryCellInfo bc = BoundaryCellInfo::build<Field>(false,false,false);
    const MemoryWindow window( get_window_with_ghost(IntVec(nx,ny,nz),ghost,bc) );

    Field  a( window, bc, ghost, NULL );
    Field  b( window, bc, ghost, NULL );
    Field cr( window, bc, ghost, NULL );
    Field sr( window, bc, ghost, NULL );

    Field::iterator ia = a.begin();
    Field::iterator ib = b.begin();
    for(size_t kk = 0; kk < window.glob_dim(2); kk++) {
        for(size_t jj = 0; jj < window.glob_dim(1); jj++) {
            for(size_t ii = 0; ii < window.glob_dim(0); ii++, ++ia, ++ib) {
	      *ia = ii + jj * 2 + kk * 4;
	      *ib = ii + jj * 3 + kk * 5;
            }
        }
    };

    evaluate_serial_example(sr,
			    a,
			    b,
			    IntVec(nx,ny,nz),
			    Lx,
			    Ly,
			    Lz,
			    number_of_runs);

    evaluate_chaining_example(cr,
			      a,
			      b,
			      IntVec(nx,ny,nz),
			      Lx,
			      Ly,
			      Lz,
			      number_of_runs);
    return 0;
};


#include <armadillo>
#include <boost/program_options.hpp>
#include <HSMM.hpp>
#include <iostream>
#include <json.hpp>
#include <memory>
#include <ProMPs_emission.hpp>

using namespace arma;
using namespace hsmm;
using namespace std;
using json = nlohmann::json;
namespace po = boost::program_options;

mat fieldToMat(int njoints, field<mat> &samples) {
    mat ret(njoints, samples.n_elem);
    for(int i = 0; i < samples.n_elem; i++)
        ret.col(i) = samples(i);
    return ret;
}

vec pmfFromGaussian(double mean, double var, int size, int min_duration) {
    vec pmf(size);
    for (int t = min_duration; t < min_duration + size; t++) {
        int idx = t - min_duration;
        double tmp = ((t - mean) * (t - mean)) / var;
        pmf(idx) = exp(-0.5 * tmp);
    }
    pmf = pmf * (1.0 / sum(pmf));
    return pmf;
}

int main(int argc, char *argv[]) {
    po::options_description desc("Options");
    desc.add_options()
        ("help,h", "Produce help message")
        ("params,p", po::value<string>(), "JSON input params (optional)")
        ("output,o", po::value<string>(), "Filename to store the obs")
        ("vit,v", po::value<string>(), "Filename to store the viterbi output")
        ("ms", po::value<string>(), "state marginals file name")
        ("mr", po::value<string>(), "runlength marginals file name")
        ("md", po::value<string>(), "duration marginals file name ")
        ("imd", po::value<string>(), "implicit duration marginals file name."
                " This means it is computed from the runlength and state")
        ("polybasisfun", po::value<int>()->default_value(1), "Order of the"
                " poly basis")
        ("rbfbasisfun", po::value<int>()->default_value(3), "Number of radial"
                " basis functions to use between 0 and 1. 0,1 are excluded.")
        ("delta", po::value<double>(), "delta between sample locations")
        ("print_ll_vit", "If set then the ll from each hs for"
                " every ground truth segment is printed. Intended for "
                "debugging purposes");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    int min_duration = 30;
    mat transition = ones<mat>(2, 2);
    transition.diag().zeros();
    int nstates = transition.n_rows;
    int ndurations = 30;
    vec pi = {0.5, 0.5};
    mat durations(nstates, ndurations);
    durations.row(0) = conv_to<rowvec>::from(pmfFromGaussian(
                40, 16, ndurations, min_duration));
    durations.row(1) = conv_to<rowvec>::from(pmfFromGaussian(
                50, 16, ndurations, min_duration));
    int njoints = 1;
    json input_params;
    if (vm.count("params")) {
        string params = vm["params"].as<string>();
        ifstream input_params_file(params);
        input_params_file >> input_params;
        min_duration = input_params["min_duration"];
        nstates = input_params["nstates"];
        ndurations = input_params["ndurations"];
        transition = eye<mat>(nstates, nstates);
        pi = eye<vec>(nstates, 1);
        durations = eye<mat>(nstates, ndurations);
        njoints = input_params["emission_params"][0]["num_joints"];
    }

    // Setting a combination of polynomial and rbf basis functions.
    auto poly = make_shared<ScalarPolyBasis>(vm["polybasisfun"].as<int>());
    auto comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({poly}));
    int nrbf = vm["rbfbasisfun"].as<int>();
    if (nrbf > 0) {
        vec centers = linspace<vec>(0, 1.0, nrbf + 2);
        centers = centers.subvec(1, centers.n_elem - 2);
        auto rbf = shared_ptr<ScalarGaussBasis>(new ScalarGaussBasis(centers,
                    0.25));
        comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({rbf, poly}));
    }
    int n_basis_functions = comb->dim();
    int nparameters = n_basis_functions * njoints;

    // Instantiating as many ProMPs as hidden states.
    vector<FullProMP> promps;
    for(int i = 0; i < nstates; i++) {
        vec mu_w = zeros<vec>(nparameters);
        mat Sigma_w = eye<mat>(nparameters, nparameters);
        mat Sigma_y = 0.00001*eye<mat>(njoints, njoints);
        ProMP promp(mu_w, Sigma_w, Sigma_y);
        FullProMP poly(comb, promp, njoints);
        promps.push_back(poly);
    }

    // Creating the ProMP emission.
    shared_ptr<AbstractEmissionOnlineSetting> ptr_emission(new ProMPsEmission(
                promps));

    OnlineHSMM promp_hsmm(ptr_emission, transition, pi, durations,
            min_duration);
    if (vm.count("params"))
        promp_hsmm.from_stream(input_params);

    int nseq = 1;
    int nsegments = 10;
    field<ivec> hidden_states, hidden_durations;
    field<field<mat>> multiple_toy_obs = promp_hsmm.sampleMultipleSequences(
            nseq, nsegments, hidden_states, hidden_durations);
    cout << "Generated states and durations for the first sequence" << endl;
    imat viterbi = join_horiz(hidden_states(0), hidden_durations(0));
    cout << viterbi << endl;

    cout << "Model Parameters" << endl;
    json params = promp_hsmm.to_stream();
    cout << params.dump(4) << endl;

    mat obs = fieldToMat(njoints, multiple_toy_obs(0));
    if (vm.count("output"))
        obs.save(vm["output"].as<string>(), raw_ascii);
    if (vm.count("vit"))
        viterbi.save(vm["vit"].as<string>(), raw_ascii);

    // Note that delta only affects the online filtering, not the generation.
    if (vm.count("delta"))
        std::static_pointer_cast<ProMPsEmission>(
                ptr_emission)->setDelta(vm["delta"].as<double>());

    if (vm.count("print_ll_vit")) {
        int idx = 0;
        for(int i = 0; i < viterbi.n_rows; i++) {
            int dur = viterbi(i, 1);
            int hs = viterbi(i, 0);
            cout << "Segment #" << i << ": (" << hs << "," << dur << ")" <<
                    endl;
            const field<mat>& segment = multiple_toy_obs(0).rows(idx,
                    idx + dur - 1);
            idx += dur;
            for(int i = 0; i < nstates; i++) {
                double ll = promp_hsmm.emission_->loglikelihood(i, segment);
                double lld = ll + log(promp_hsmm.duration_(i, dur - min_duration));
                cout << "State " << i << ": " << ll << " with dur: " << lld <<
                        endl;
            }
        }
    }

    mat state_marginals_over_time(nstates, obs.n_cols);
    mat runlength_marginals_over_time(min_duration + ndurations,
            obs.n_cols);
    mat duration_marginals_over_time(ndurations, obs.n_cols);
    mat implicit_duration_marginals_over_time(ndurations, obs.n_cols);
    for(int c = 0; c < obs.n_cols; c++) {
        promp_hsmm.addNewObservation(obs.col(c));
        vec s_marginal = promp_hsmm.getStateMarginal();
        state_marginals_over_time.col(c) = s_marginal;
        vec r_marginal = promp_hsmm.getRunlengthMarginal();
        runlength_marginals_over_time.col(c) = r_marginal;
        vec d_marginal = promp_hsmm.getDurationMarginal();
        duration_marginals_over_time.col(c) = d_marginal;
        if (vm.count("imd")) {
            vec indirect_d_marginal = promp_hsmm.getImplicitDurationMarginal();
            implicit_duration_marginals_over_time.col(c) = indirect_d_marginal;
        }
        if (vm.count("print_ll_vit")) {
            cout << "Obs idx: " << c << endl;
            promp_hsmm.printTopKFromPosterior(40);
        }
    }

    // Saving the marginals if required.
    if (vm.count("ms"))
        state_marginals_over_time.save(vm["ms"].as<string>(), raw_ascii);
    if (vm.count("mr"))
        runlength_marginals_over_time.save(vm["mr"].as<string>(), raw_ascii);
    if (vm.count("md"))
        duration_marginals_over_time.save(vm["md"].as<string>(), raw_ascii);
    if (vm.count("imd"))
        implicit_duration_marginals_over_time.save(vm["imd"].as<string>(),
                raw_ascii);
    return 0;
}


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

// Copyright (c) 2016-2017 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_STRATEGIES_SPHERICAL_AZIMUTH_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_AZIMUTH_HPP

#include <boost/geometry/strategies/azimuth.hpp>
#include <boost/geometry/formulas/spherical.hpp>

#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_void.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace azimuth
{

template
<
    typename CalculationType = void
>
class spherical
{
public :

    inline spherical()
    {}

    template <typename T>
    static inline void apply(T const& lon1_rad, T const& lat1_rad,
                             T const& lon2_rad, T const& lat2_rad,
                             T& a1, T& a2)
    {
        typedef typename boost::mpl::if_
            <
                boost::is_void<CalculationType>, T, CalculationType
            >::type calc_t;

        geometry::formula::result_spherical<calc_t>
            result = geometry::formula::spherical_azimuth<calc_t, true>(
                        calc_t(lon1_rad), calc_t(lat1_rad),
                        calc_t(lon2_rad), calc_t(lat2_rad));

        a1 = result.azimuth;
        a2 = result.reverse_azimuth;
    }

};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename CalculationType>
struct default_strategy<spherical_equatorial_tag, CalculationType>
{
    typedef strategy::azimuth::spherical<CalculationType> type;
};

/*
template <typename CalculationType>
struct default_strategy<spherical_polar_tag, CalculationType>
{
    typedef strategy::azimuth::spherical<CalculationType> type;
};
*/
}

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::azimuth

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_AZIMUTH_HPP


// Copyright (c) 2021 Franka Emika GmbH
//
// 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 <franka_example_controllers/motion_generator.hpp>

#include <algorithm>
#include <array>
#include <cassert>
#include <cmath>
#include <utility>

#include <Eigen/Core>

MotionGenerator::MotionGenerator(double speed_factor,
                                 const Vector7d& q_start,
                                 const Vector7d& q_goal)
    : q_start_(q_start) {
  assert(speed_factor > 0);
  assert(speed_factor <= 1);
  delta_q_ = q_goal - q_start;
  dq_max_ *= speed_factor;
  ddq_max_start_ *= speed_factor;
  ddq_max_goal_ *= speed_factor;
  calculateSynchronizedValues();
}

bool MotionGenerator::calculateDesiredValues(double t, Vector7d* delta_q_d) const {
  Vector7i sign_delta_q;
  sign_delta_q << delta_q_.cwiseSign().cast<int>();
  Vector7d t_d = t_2_sync_ - t_1_sync_;
  Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_;
  std::array<bool, kJoints> joint_motion_finished{};

  for (auto i = 0; i < kJoints; i++) {
    if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) {
      (*delta_q_d)[i] = 0;
      joint_motion_finished.at(i) = true;
    } else {
      if (t < t_1_sync_[i]) {
        (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] *
                          (0.5 * t - t_1_sync_[i]) * std::pow(t, 3.0);
      } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) {
        (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i];
      } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) {
        (*delta_q_d)[i] =
            delta_q_[i] + 0.5 *
                              (1.0 / std::pow(delta_t_2_sync[i], 3.0) *
                                   (t - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) *
                                   std::pow((t - t_1_sync_[i] - t_d[i]), 3.0) +
                               (2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) *
                              dq_max_sync_[i] * sign_delta_q[i];
      } else {
        (*delta_q_d)[i] = delta_q_[i];
        joint_motion_finished.at(i) = true;
      }
    }
  }
  return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(),
                     [](bool x) { return x; });
}

void MotionGenerator::calculateSynchronizedValues() {
  Vector7d dq_max_reach(dq_max_);
  Vector7d t_f = Vector7d::Zero();
  Vector7d delta_t_2 = Vector7d::Zero();
  Vector7d t_1 = Vector7d::Zero();
  Vector7d delta_t_2_sync = Vector7d::Zero();
  Vector7i sign_delta_q;
  sign_delta_q << delta_q_.cwiseSign().cast<int>();

  for (auto i = 0; i < kJoints; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) +
                                   3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) {
        dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] *
                                    (ddq_max_start_[i] * ddq_max_goal_[i]) /
                                    (ddq_max_start_[i] + ddq_max_goal_[i]));
      }
      t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i];
      delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i];
      t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i];
    }
  }
  double max_t_f = t_f.maxCoeff();
  for (auto i = 0; i < kJoints; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]);
      double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i];
      double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i];
      double delta = b * b - 4.0 * a * c;
      if (delta < 0.0) {
        delta = 0.0;
      }
      dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a);
      t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i];
      delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i];
      t_f_sync_[i] =
          (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]);
      t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i];
      q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]);
    }
  }
}

std::pair<MotionGenerator::Vector7d, bool> MotionGenerator::getDesiredJointPositions(
    const rclcpp::Duration& trajectory_time) {
  time_ = trajectory_time.seconds();

  Vector7d delta_q_d;
  bool motion_finished = calculateDesiredValues(time_, &delta_q_d);

  std::array<double, kJoints> joint_positions{};
  Eigen::VectorXd::Map(&joint_positions[0], kJoints) = (q_start_ + delta_q_d);
  return std::make_pair(q_start_ + delta_q_d, motion_finished);
}


//
// Created by Vasiliy Ershov on 08/11/2016.
//

#ifndef PROJECT_NORMAL_QUALITY_MODEL_HPP
#define PROJECT_NORMAL_QUALITY_MODEL_HPP

#include <common/utils/parallel/openmp_wrapper.h>
#include <array>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/trigamma.hpp>
#include <vector>
#include "config_struct.hpp"
#include "kmer_data.hpp"
#include "quality_thresholds_estimator.h"
#include "thread_utils.h"
#include "valid_hkmer_generator.hpp"
//

namespace n_normal_model {

struct QualityTransform {
  double bias_;

  QualityTransform(double bias = 60.0) : bias_(bias) {}

  double Apply(double quality, double count) const {
    return quality / (count + 60);
  }
};

class NormalDistribution {
 private:
  double mean_;
  double sigma_sqr_;

 public:
  NormalDistribution(const NormalDistribution&) = default;

  NormalDistribution& operator=(const NormalDistribution&) = default;

  NormalDistribution(const double mean = 0, const double sigma = 1)
      : mean_(mean), sigma_sqr_(sigma) {}

  inline double GetMean() const { return mean_; }

  inline double GetSigmaSqr() const { return sigma_sqr_; }

  double LogLikelihood(double x) const {
    return -0.5 *
           ((x - mean_) * (x - mean_) / sigma_sqr_ + log(2 * M_PI * sigma_sqr_));
  }

  double LogLikelihoodFromStats(const double sum,
                                const double sum2,
                                const double weight) const {
    return -0.5 * ((sum2 - 2 * sum * mean_ + weight * mean_ * mean_) / sigma_sqr_ +
                   weight * log(2 * M_PI * sigma_sqr_));
  }

  static NormalDistribution FromStats(const double sum,
                                      const double sum2,
                                      const double weight) {
    const double mu = sum / weight;
    const double var = sum2 / weight - mu * mu;
    return NormalDistribution(mu, var);
  }
};

class NormalMixture {
 private:
  NormalDistribution first_;
  NormalDistribution second_;
  double first_weight_;

 public:
  NormalMixture() : first_weight_(0) {}

  NormalMixture(const NormalDistribution& first,
                const NormalDistribution& second,
                double weight)
      : first_(first), second_(second), first_weight_(weight) {}

  const NormalDistribution& GetFirst() const { return first_; }

  const NormalDistribution& GetSecond() const { return second_; }

  double GetFirstWeight() const { return first_weight_; }

  double LogLikelihood(double x) const {
    return log(first_weight_ * exp(first_.LogLikelihood(x)) +
               (1 - first_weight_) * exp(second_.LogLikelihood(x)));
  }

  double FirstComponentPosterior(double x) const {
    double firstLL = first_.LogLikelihood(x) + log(first_weight_);
    double secondLL = second_.LogLikelihood(x) + log(1.0 - first_weight_);
    const double expDiff = exp(secondLL - firstLL);

    return std::isfinite(expDiff) ? -log(1.0 + exp(secondLL - firstLL))
                                  : firstLL - secondLL;
  }
};

class Binarizer {
 private:
  std::vector<double> borders_;

 public:
  Binarizer() {
    for (int i = 17; i < 30; ++i) {
      borders_.push_back(i);
    }
  }

  Binarizer(const std::vector<double> &borders) : borders_(borders) {}

  int GetBin(double value) const {
    uint index = 0;
    while (index < borders_.size() && value > borders_[index]) {
      ++index;
    }
    return index;
  }

  size_t GetBinCount() const { return borders_.size() + 1; }

  double GetBorder(int bin) {
    --bin;
    bin = std::min(bin, (int)(borders_.size() - 1));
    if (bin < 0) {
      return 0;
    }
    return borders_[bin];
  }
};

class NormalClusterModel {
 private:
  std::vector<NormalMixture> mixtures_;
  Binarizer binarizer_;
  std::vector<double> median_qualities_;
  QualityTransform trans_;
  double lower_quality_threshold_;

  static std::vector<double> left_likelihoods_;
  static std::vector<double> equal_likelihoods_;
  static std::vector<double> right_likelihoods_;

 public:
  NormalClusterModel() {}

  NormalClusterModel(const std::vector<NormalMixture>& mixtures,
                      const Binarizer& binarizer,
                      const std::vector<double>& medianQualities,
                      const QualityTransform& trans)
      : mixtures_(mixtures),
        binarizer_(binarizer),
        median_qualities_(medianQualities),
        trans_(trans) {
    lower_quality_threshold_ = cfg::get().noise_filter_count_threshold;  // threshold >= 10 ? 1 : 0;
  }

  NormalClusterModel(const NormalClusterModel& other) = default;

  NormalClusterModel& operator=(const NormalClusterModel&) = default;

  bool NeedSubcluster(const hammer::KMerStat& stat) const {
    return stat.count > 15 && GenomicLogLikelihood(stat) > -0.0001;
  }

  double StatTransform(const hammer::KMerStat& stat) const {
    return trans_.Apply(stat.qual, stat.count);
  }

  double GenomicLogLikelihood(const hammer::KMerStat& stat) const {
    return GenomicLogLikelihood(binarizer_.GetBin((double)GetKmerBinIdx(stat.kmer)),
                                stat.qual, stat.count);
  }

  bool IsHighQuality(const hammer::KMerStat& stat) const {
    const auto bin = binarizer_.GetBin((double)GetKmerBinIdx(stat.kmer));
    return trans_.Apply(stat.qual, stat.count) <= median_qualities_[bin];
  }

  double GenomicLogLikelihood(int bin, double quality, double count) const {
    if (count <= lower_quality_threshold_) {
      return -1e5;
    }
    const double x = trans_.Apply(quality, count);
    return mixtures_[bin].FirstComponentPosterior(x);
  }

  static size_t GetKmerBinIdx(const hammer::HKMer& kmer) {
    if (kmer.size() > 21) {
      return 1 + kmer.max_run_length();
    } else {
      return 0;
    }
  }

  static double ErrorLogLikelihood(int from, int to) {
    int diff = std::abs(from - to);
    from = std::max(from, 0);
    --from;
    int sign = from > to ? -1 : 1;
    from = std::min((int)equal_likelihoods_.size() - 1, from);
    if (diff == 0) {
      return equal_likelihoods_[from];
    }
    if (sign == -1) {
      return left_likelihoods_[from] * diff;
    }
    return right_likelihoods_[from] * diff;
  }
};

class NormalMixtureEstimator {
 private:
  uint num_threads_;
  size_t max_iterations_;
  bool calc_likelihoods_;

 private:
  std::vector<double> BuildPriors(const std::vector<double>& observations) const {
    double threshold = SimpleTwoClassClustering::SimpleThresholdEstimation(
                           observations.begin(), observations.end())
                           .split_;

    std::vector<double> priors(observations.size());

#pragma omp parallel for num_threads(num_threads_)
    for (size_t i = 0; i < observations.size(); ++i) {
      priors[i] = observations[i] <= threshold ? 1 : 0;
    }

    return priors;
  }

  struct Stats {
    double sum_left_ = 0;
    double sum2_left_ = 0;
    double weight_left_ = 0;
    double sum_right_ = 0;
    double sum2_right_ = 0;

    Stats& operator+=(const Stats& other) {
      if (this != &other) {
        sum_left_ += other.sum_left_;
        sum2_left_ += other.sum2_left_;
        sum_right_ += other.sum_right_;
        sum2_right_ += other.sum2_right_;
        weight_left_ += other.weight_left_;
      }
      return *this;
    }
  };

 public:
  NormalMixtureEstimator(uint num_threads,
                         size_t max_iterations,
                          bool calc_likelihood)
      : num_threads_(num_threads),
        max_iterations_(max_iterations),
        calc_likelihoods_(calc_likelihood) {}

  NormalMixture Estimate(std::vector<double>& observations) const {
    std::sort(observations.begin(), observations.end(), std::greater<double>());
    observations.resize(observations.size());
    std::reverse(observations.begin(), observations.end());

    std::vector<double> priors = BuildPriors(observations);

    NormalMixture mixture;

    for (size_t iter = 0; iter < max_iterations_; ++iter) {
      auto stats =
          n_computation_utils::ParallelStatisticsCalcer<Stats>(num_threads_)
              .Calculate(observations.size(),
                         [&]() -> Stats { return Stats(); },
                         [&](Stats& stat, size_t k) {
                           const double x = observations[k];
                           const double w = priors[k];
                           stat.sum2_left_ += w * x * x;
                           stat.sum_left_ += w * x;
                           stat.weight_left_ += w;
                           stat.sum2_right_ += (1 - w) * x * x;
                           stat.sum_right_ += (1 - w) * x;
                         });

      mixture =
          NormalMixture(NormalDistribution::FromStats(
                             stats.sum_left_, stats.sum2_left_, stats.weight_left_),
                         NormalDistribution::FromStats(
                             stats.sum_right_, stats.sum2_right_,
                             (double)observations.size() - stats.weight_left_),
                         stats.weight_left_ / (double)observations.size());

// expectation
#pragma omp parallel for num_threads(num_threads_)
      for (size_t i = 0; i < observations.size(); ++i) {
        priors[i] = exp(mixture.FirstComponentPosterior(observations[i]));
      }

      if (calc_likelihoods_) {
        double ll = 0;
        for (size_t i = 0; i < observations.size(); ++i) {
          const double x = observations[i];
          ll += mixture.LogLikelihood(x);
        }
        INFO("LogLikelihood: " << ll);
      }

      if (iter == 0 || iter == (max_iterations_ - 1)) {
        const double llFirst = mixture.GetFirst().LogLikelihoodFromStats(
            stats.sum_left_, stats.sum2_left_, stats.weight_left_);
        INFO("Likelihood first: " << llFirst);
        const double llSecond = mixture.GetSecond().LogLikelihoodFromStats(
            stats.sum_right_, stats.sum2_right_,
            (double)observations.size() - stats.weight_left_);
        INFO("Likelihood second: " << llSecond);
        INFO("First weights: " << mixture.GetFirstWeight());
      }
    }
    return mixture;
  };
};

// this class estimate prior distribution.
class ModelEstimator {
 private:
  const KMerData& data_;
  uint num_threads_;
  size_t max_iterations_;
  bool is_calc_likelihood_;

 public:
  ModelEstimator(const KMerData& data,
                 uint num_threads = 16,
                 size_t maxIterations = 40,
                 bool calc_likelihood = false)
      : data_(data),
        num_threads_(num_threads),
        max_iterations_(maxIterations),
        is_calc_likelihood_(calc_likelihood) {}

  NormalClusterModel Estimate(
      const std::vector<std::vector<size_t> >& clusters) {
    QualityTransform trans;

    std::vector<size_t> cluster_center;
    {
      cluster_center.resize(clusters.size());
#pragma omp parallel for num_threads(num_threads_)
      for (size_t i = 0; i < clusters.size(); ++i) {
        auto& cluster = clusters[i];

        double best_qual =
            trans.Apply(data_[cluster[0]].qual, data_[cluster[0]].count);
        size_t bestIdx = cluster[0];

        for (auto idx : cluster) {
          const auto qual = trans.Apply(data_[idx].qual, data_[idx].count);
          if (qual < best_qual ||
              (qual == best_qual &&
               data_[idx].kmer.size() < data_[bestIdx].kmer.size())) {
            best_qual = qual;
            bestIdx = idx;
          }
          cluster_center[i] = bestIdx;
        }
      }
    }

    std::vector<std::vector<double> > qualities;
    qualities.reserve(16);
    const size_t sampleMaxThreshold = (size_t)1e9;
    const size_t min_sample_size = (size_t)1e4;

    {
      double skip_threshold = cfg::get().noise_filter_count_threshold;  // threshold >= 10 ? 1 : 0;

      for (size_t i = 0; i < cluster_center.size(); ++i) {
        const auto& stat = data_[cluster_center[i]];

        if (stat.count <= skip_threshold) {
          continue;
        }
        const size_t bin = NormalClusterModel::GetKmerBinIdx(stat.kmer);

        if (bin >= qualities.size()) {
          qualities.resize(bin + 1);
        }

        if (qualities[bin].size() > sampleMaxThreshold) {
          continue;
        }
        auto trans_qual = trans.Apply(stat.qual, stat.count);
        qualities[bin].push_back(trans_qual);
      }
    }

    std::vector<NormalMixture> models;
    std::vector<double> borders;
    std::vector<double> median_qualities;

    size_t total_count = 0;
    for (const auto& qual : qualities) {
      total_count += qual.size();
    }
    assert(qualities[1].size() == 0);

    {
      auto model = NormalMixtureEstimator(num_threads_, max_iterations_, is_calc_likelihood_).Estimate(qualities[0]);

      const double median_quality = FindHighQualityThreshold(qualities[0], model);
      INFO("For kmer length <= 21");
      INFO("Median quality " << median_quality);
      INFO("Sample size " << qualities[0].size());
      INFO("Genomic dist: " << model.GetFirst().GetMean() << " "
                            << model.GetFirst().GetSigmaSqr());
      INFO("NonGenomic dist: " << model.GetSecond().GetMean() << " "
                               << model.GetSecond().GetSigmaSqr());
      models.push_back(model);
      median_qualities.push_back(median_quality);
      borders.push_back(0);
      total_count -= qualities[0].size();
    }

    const auto len_limit = std::min(qualities.size(), 7UL);
    for (uint max_run_len = 2; max_run_len < len_limit; ++max_run_len) {
      if (total_count < min_sample_size) {
        break;
      }

      const size_t bin = max_run_len + 1;
      auto bin_qualities = qualities[bin];
      total_count -= bin_qualities.size();

      if (bin_qualities.size() < min_sample_size) {
        if (bin + 1 < qualities.size()) {
          qualities[bin + 1].insert(qualities[bin + 1].end(),
                                    bin_qualities.begin(),
                                    bin_qualities.end());
        }
        continue;
      }

      auto model = NormalMixtureEstimator(num_threads_, max_iterations_, is_calc_likelihood_).Estimate(bin_qualities);

      const double median_quality = FindHighQualityThreshold(bin_qualities, model);

      INFO("Sample size " << bin_qualities.size());
      INFO("Median quality " << median_quality);
      INFO("For max run length >= " << max_run_len);
      INFO("Genomic dist: " << model.GetFirst().GetMean() << " "
                            << model.GetFirst().GetSigmaSqr());
      INFO("NonGenomic dist: " << model.GetSecond().GetMean() << " "
                               << model.GetSecond().GetSigmaSqr());
      median_qualities.push_back(median_quality);
      models.push_back(model);
      borders.push_back((double)bin);
    }
    borders.resize(borders.size() - 1);

    return NormalClusterModel(models, Binarizer(borders), median_qualities,
                               trans);
  }

  double FindHighQualityThreshold(const std::vector<double>& bin_quality,
                                  const NormalMixture& model) const {
    std::vector<double> good_samples;
    good_samples.reserve(bin_quality.size());
    for (size_t i = 0; i < bin_quality.size(); ++i) {
      if (model.FirstComponentPosterior(bin_quality[i]) > -0.69) {
        good_samples.push_back(bin_quality[i]);
      }
    }

    const size_t quantile = (size_t)((double)good_samples.size() * cfg::get().dist_one_subcluster_alpha);
    std::nth_element(good_samples.begin(), good_samples.begin() + quantile,
                     good_samples.end());
    return good_samples[quantile];
  }
};

}  // namespace NNormalModel

#endif  // PROJECT_NORMAL_QUALITY_MODEL_HPP


#include <stan/math/rev.hpp>
#include <test/unit/math/test_ad.hpp>
#include <gtest/gtest.h>
#include <boost/math/differentiation/finite_difference.hpp>

TEST(mathMixScalFun, neg_binomial_2_log_lpmf_derivatives) {
  auto f1 = [](const auto& eta, const auto& phi) {
    return stan::math::neg_binomial_2_log_lpmf(0, eta, phi);
  };
  auto f2 = [](const auto& eta, const auto& phi) {
    return stan::math::neg_binomial_2_log_lpmf(6, eta, phi);
  };

  stan::test::expect_ad(f1, -1.5, 4.1);
  stan::test::expect_ad(f1, 2.0, 1.1);
  stan::test::expect_ad(f2, -1.5, 4.1);
  stan::test::expect_ad(f2, 2.0, 1.1);
}


#include <ctime>

#include <boost/random.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include "NonlinearRBFFactorType.h"

namespace Grante {

NonlinearRBFFactorType::NonlinearRBFFactorType(const std::string& name,
	const std::vector<unsigned int>& card,
	unsigned int data_size, unsigned int rbf_basis_count, double log_beta)
	: FactorType(name, card, data_size), rbfnet(rbf_basis_count, data_size),
		rbf_basis_count(rbf_basis_count) {
	InitializeProdCard();
	assert(rbf_basis_count > 0);

	assert((boost::math::isnan)(log_beta) == false);
	rbfnet.FixBeta(log_beta);
	size_t wdim = prod_card * rbfnet.ParameterDimension();

	// Initialize weight vector randomly
	boost::mt19937 rgen(static_cast<const boost::uint32_t>(std::time(0))+1);
	boost::uniform_real<double> rdestu;	// range [0,1]
	boost::variate_generator<boost::mt19937,
		boost::uniform_real<double> > randu(rgen, rdestu);

	// FIXME: better initialization concepts
	w.resize(wdim);
	std::fill(w.begin(), w.end(), 0.0);
	size_t wbase = 0;
	for (unsigned int ri = 0; ri < prod_card; ++ri) {
		// Initialize alpha_n
		for (unsigned int wi = 0; wi < rbf_basis_count; ++wi)
			w[wbase + wi] = randu() - 0.5;

		// Initialize c_n
		for (unsigned int wi = 0; wi < (data_size*rbf_basis_count); ++wi)
			w[wbase + rbf_basis_count + wi] = randu() - 0.5;

		wbase += rbfnet.ParameterDimension();
	}
}

NonlinearRBFFactorType::~NonlinearRBFFactorType() {
}

void NonlinearRBFFactorType::InitializeUsingTrainingData(const std::vector<
	ParameterEstimationMethod::labeled_instance_type>& training_data) {
	boost::mt19937 rgen(static_cast<const boost::uint32_t>(std::time(0))+1);
	boost::uniform_real<double> rdestu;	// range [0,1]
	boost::variate_generator<boost::mt19937,
		boost::uniform_real<double> > randu(rgen, rdestu);

	size_t wbase = 0;
	for (unsigned int ri = 0; ri < prod_card; ++ri) {
		// 1. Collect all factors that are labeled with the corresponding
		// ground truth label
		std::vector<Factor*> m_factors;
		for (unsigned int n = 0; n < training_data.size(); ++n) {
			const FactorGraph* fg = training_data[n].first;
			const FactorGraphObservation* obs = training_data[n].second;
			assert(obs->Type() == FactorGraphObservation::DiscreteLabelingType);
			const std::vector<Factor*>& factors = fg->Factors();
			for (unsigned int fi = 0; fi < factors.size(); ++fi) {
				if (factors[fi]->Type()->Name() != Name())
					continue;

				unsigned int ei_obs =
					factors[fi]->ComputeAbsoluteIndex(obs->State());
				if (ei_obs != ri)
					continue;

				m_factors.push_back(factors[fi]);
			}
		}
		// Need to be sure there is at least one observation
		assert(m_factors.size() >= 1);
		std::cout << m_factors.size() << " samples for statepair "
			<< ri << std::endl;

		// Initialize alpha_n
		for (unsigned int wi = 0; wi < rbf_basis_count; ++wi)
			w[wbase + wi] = 1.0;

		// Initialize c_n as sample from the training set
		size_t wbi_base = 0;
		for (unsigned int bi = 0; bi < rbf_basis_count; ++bi) {
			unsigned int mi = static_cast<unsigned int>(
				randu() * static_cast<double>(m_factors.size()));
			assert(mi < m_factors.size());
			const std::vector<double>& H = m_factors[mi]->Data();

			// Copy selected training instance, perturbed
			assert(H.size() == data_size);
			for (unsigned int wi = 0; wi < data_size; ++wi) {
				w[wbase + rbf_basis_count + wbi_base + wi] =
					H[wi] + randu()*1.0e-8;
			}
			wbi_base += data_size;
		}
		wbase += rbfnet.ParameterDimension();
	}
	assert(wbase == w.size());
}

void NonlinearRBFFactorType::InitializeWeights(
	const std::vector<double>& weights) {
	assert(weights.size() == (prod_card * rbfnet.ParameterDimension()));
	this->w = weights;
}

bool NonlinearRBFFactorType::IsDataDependent() const {
	return (true);
}

void NonlinearRBFFactorType::ForwardMap(const Factor* factor,
	std::vector<double>& energies) const {
	const std::vector<double>& H = factor->Data();
	assert(H.size() == data_size);
	assert(energies.size() == prod_card);
	size_t wbase = 0;
	for (size_t ei = 0; ei < prod_card; ++ei) {
		energies[ei] = rbfnet.Evaluate(H, w, wbase);
		wbase += rbfnet.ParameterDimension();
	}
}

void NonlinearRBFFactorType::BackwardMap(const Factor* factor,
	const std::vector<double>& marginals,
	std::vector<double>& parameter_gradient, double mult) const {
	const std::vector<double>& H = factor->Data();
	assert(H.size() == data_size);
	size_t wbase = 0;
	for (size_t ei = 0; ei < prod_card; ++ei) {
		rbfnet.EvaluateGradient(H, w, parameter_gradient, wbase,
			mult * marginals[ei]);
		wbase += rbfnet.ParameterDimension();
	}
}

const RBFNetwork& NonlinearRBFFactorType::Net() const {
	return (rbfnet);
}

}


/*M///////////////////////////////////////////////////////////////////////////////////////
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2008-2011, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistributions of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistributions in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation 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.
//
// C by Benjamin Wassermann
//M*/

#ifndef _GEOMETRY_BASE_HPP_
#define _GEOMETRY_BASE_HPP_
#ifdef __cplusplus

//#define EIGEN_MATRIXBASE_PLUGIN "MatrixBaseAddons.h"
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include "../utility/limit.hpp"
#include <cstdlib>

namespace lsfm {
    namespace detail {
        using std::round;
        using std::abs;
        using std::log;
        using std::exp;
        using std::sqrt;
        using std::cos;
        using std::acos;
        using std::sin;
        using std::asin;
        using std::tan;
        using std::atan;
        using std::sinh;
        using std::cosh;
        using std::tanh;
        using std::pow;
        using std::atan2;
        using std::hypot;
    }

    template<class FT>
    FT getScalar(FT val){
        return val;
    }

    // note -> use Eigen::DontAlign for eigen matrix to prevent align problems, eg. transpose etc.
    // eigen seems to have problems wiht stl vector -> Eigen::aligned_allocator has to be used or
    // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION or best swith off alignment?!
    template<int rows, int _cols>
    struct Major {
        enum { type = ((rows > 1 || rows == Eigen::Dynamic) * (_cols > 1 || _cols == Eigen::Dynamic) * Eigen::RowMajor) | Eigen::DontAlign};
		//static constexpr int type = ((rows > 1 || rows == Eigen::Dynamic) * (_cols > 1 || _cols == Eigen::Dynamic) * Eigen::RowMajor) | Eigen::DontAlign ;
    };

    // matrix base, up to 4x4
    template<class FT, int _rows, int _cols>
    class Matx : public Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type > {
    public:
        typedef FT float_type;

        typedef Eigen::Matrix<FT,_rows,_cols, Major<_rows, _cols>::type> MatrixBase;
        typedef typename MatrixBase::Base Base;
        typedef typename MatrixBase::Index Index;

        //typedef typename Base::PlainObject PlainObject;
        //using Base::base;
        //using Base::coeffRef;

        //EIGEN_DENSE_PUBLIC_INTERFACE(MyMatrixBase)

        //Matx(const MyMatrixBase& other) : Eigen::Matrix<FT,_rows,_cols>(other) {}

        template<typename OtherDerived>
        Matx(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(other) {}

        template<typename OtherDerived>
        Matx(const Eigen::ReturnByValue<OtherDerived>& other) : Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(other) {}

        template<typename OtherDerived>
        Matx(const Eigen::EigenBase<OtherDerived> &other) : Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(other) {}

        Matx() : Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() {}
        Matx(const FT &v0, const FT &v1): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(v0,v1) {}
        Matx(const FT &v0, const FT &v1, const FT &v2): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(v0,v1,v2) {}
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() {
            if (_rows == 4 || _cols == 4) {
                this->m_storage.data()[0] = v0;
                this->m_storage.data()[1] = v1;
                this->m_storage.data()[2] = v2;
                this->m_storage.data()[3] = v3;
                return;
            }
            *this << v0,v1,v2,v3;
        }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3, const FT &v4): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4; }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3, const FT &v4, const FT &v5): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4,v5; }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3, const FT &v4, const FT &v5, const FT &v6): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4,v5, v6; }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3, const FT &v4, const FT &v5, const FT &v6, const FT &v7): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4,v5,v6,v7; }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3, const FT &v4, const FT &v5, const FT &v6, const FT &v7, const FT &v8): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() {
            *this << v0,v1,v2,v3,v4,v5,v6,v7,v8;
        }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3,
             const FT &v4, const FT &v5, const FT &v6, const FT &v7,
             const FT &v8, const FT &v9, const FT &v10, const FT &v11): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4,v5,v6,v7,v8,v9,v10,v11; }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3,
             const FT &v4, const FT &v5, const FT &v6, const FT &v7,
             const FT &v8, const FT &v9, const FT &v10, const FT &v11,
             const FT &v12, const FT &v13, const FT &v14, const FT &v15): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4,v5,v6,v7,v8,v9,v10,v11,v12,v13,v14,v15; }

        explicit Matx(const FT* vals) : Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(vals) {}

        /*Matx& operator=(const MyMatrixBase& other) {
            this->Base::_set(other);
            return *this;
        }*/

        template<typename OtherDerived>
        Matx& operator=(const Eigen::MatrixBase<OtherDerived>& other) {
            this->Base::operator=(other);
            return *this;
        }

        template<typename OtherDerived>
        Matx& operator=(const Eigen::EigenBase<OtherDerived> &other) {
            this->Base::operator=(other);
            return *this;
        }

        template<typename OtherDerived>
        Matx& operator=(const Eigen::ReturnByValue<OtherDerived>& func) {
            this->Base::operator=(func);
            return *this;
        }

        // linear access
        inline FT& operator[](Index idx) { return this->m_storage.data()[idx]; }
        inline const FT& operator[](Index idx) const { return this->m_storage.data()[idx]; }

        // point access
        inline FT& x() { return this->m_storage.data()[0]; }
        inline const FT& x() const { return this->m_storage.data()[0]; }

        inline FT& y() { return this->m_storage.data()[1]; }
        inline const FT& y() const { return this->m_storage.data()[1]; }

        inline FT& z() { return this->m_storage.data()[2]; }
        inline const FT& z() const { return this->m_storage.data()[2]; }

        inline FT& w() { return this->m_storage.data()[3]; }
        inline const FT& w() const { return this->m_storage.data()[3]; }
    };

    template<class FT>
    using Matx22 = Matx<FT,2,2>;

    template<class FT>
    using Matx23 = Matx<FT,2,3>;

    template<class FT>
    using Matx32 = Matx<FT,3,2>;

    template<class FT>
    using Matx33 = Matx<FT,3,3>;

    template<class FT>
    using Matx34 = Matx<FT,3,4>;

    template<class FT>
    using Matx43 = Matx<FT,4,3>;

    template<class FT>
    using Matx44 = Matx<FT,4,4>;

    typedef Matx22<float> Matx22f;
    typedef Matx22<double> Matx22d;

    typedef Matx23<float> Matx23f;
    typedef Matx23<double> Matx23d;

    typedef Matx32<float> Matx32f;
    typedef Matx32<double> Matx32d;

    typedef Matx33<float> Matx33f;
    typedef Matx33<double> Matx33d;

    typedef Matx34<float> Matx34f;
    typedef Matx34<double> Matx34d;

    typedef Matx43<float> Matx43f;
    typedef Matx43<double> Matx43d;

    typedef Matx44<float> Matx44f;
    typedef Matx44<double> Matx44d;

    template<class FT, int rows>
    using Vec = Matx<FT,rows,1>;

    template<class FT>
    using Vec2 = Vec<FT,2>;

    template<class FT>
    using Vec3 = Vec<FT,3>;

    template<class FT>
    using Vec4 = Vec<FT,4>;

    typedef Vec2<float> Vec2f;
    typedef Vec2<double> Vec2d;
    typedef Vec2<int> Vec2i;
    typedef Vec3<float> Vec3f;
    typedef Vec3<double> Vec3d;
    typedef Vec3<int> Vec3i;
    typedef Vec4<float> Vec4f;
    typedef Vec4<double> Vec4d;
    typedef Vec4<int> Vec4i;

    template<class FT, int cols>
    using RowVec = Matx<FT,1,cols>;

    template<class FT>
    using RowVec2 = RowVec<FT,2>;

    template<class FT>
    using RowVec3 = RowVec<FT,3>;

    template<class FT>
    using RowVec4 = RowVec<FT,4>;

    typedef RowVec2<float> RowVec2f;
    typedef RowVec2<double> RowVec2d;
    typedef RowVec3<float> RowVec3f;
    typedef RowVec3<double> RowVec3d;
    typedef RowVec4<float> RowVec4f;
    typedef RowVec4<double> RowVec4d;

    //! convert rot vec to rot matrix
    template<class FT>
    inline Matx33<FT> rodrigues(const Vec3<FT> &r) {
        Vec3<FT> axis;
        FT angle = rodrigues(r,axis);
        return Eigen::AngleAxis<FT>(angle,axis).matrix();

    }

    //! convert rot matrix to rot vec
    template<class FT>
    inline Vec3<FT> rodrigues(const Matx33<FT> &r) {
        Eigen::AngleAxis<FT> axis(r);
        return axis.axis() * axis.angle();
    }

    //! convert rot point to rot axis and angle
    template<class FT>
    inline FT rodrigues(const Vec3<FT> r, Vec3<FT> &axis) {
        FT n = r.norm();
        if (n < LIMITS<FT>::tau()) {
            axis = Vec3<FT>(FT(1),FT(0),FT(0));
            return FT(0);
        }
        axis = r / n;
        return n;
    }

    //! compose homogeneouse matrix from trans vector and rot vector
    template<class FT>
    Matx44<FT> composeHom(const Vec3<FT> &trans, const Vec3<FT> &rot = Vec3<FT>(FT(0),FT(0),FT(0))) {
        return composeHom(trans,rodrigues(rot));
    }

    //! compose homogeneouse matrix from trans vector and rot matrix
    template<class FT>
    Matx44<FT> composeHom(const Vec3<FT> &trans, const Matx33<FT> &rot) {
        return Matx44<FT>(rot(0,0),rot(0,1),rot(0,2),trans.x(),
                          rot(1,0),rot(1,1),rot(1,2),trans.y(),
                          rot(2,0),rot(2,1),rot(2,2),trans.z(),
                          FT(0) , FT(0),  FT(0) , FT(1));
    }

    //! compose homogeneouse matrix from other matrix
    template<class FT>
    Matx44<FT> composeHom(const Matx33<FT> &m) {
        return Matx44<FT>(m(0,0),m(0,1),m(0,2),FT(0),
                          m(1,0),m(1,1),m(1,2),FT(0),
                          m(2,0),m(2,1),m(2,2),FT(0),
                          FT(0) , FT(0),  FT(0) , FT(1));
    }

    //! decompose homogeneouse matrix to trans vector and rot vector
    template<class FT>
    void decomposeHom(const Matx44<FT> &m, Vec3<FT> &trans, Vec3<FT> &rot) {
        Matx33<FT> r;
        decomposeHom(m,trans,r);
        rot = rodrigues(r);
    }

    //! decompose homogeneouse matrix to trans vector and rot matrix
    template<class FT>
    void decomposeHom(const Matx44<FT> &m, Vec3<FT> &trans, Matx33<FT> &rot) {
        trans = Vec3<FT>(m(0,3),m(1,3),m(2,3));
        decomposeHom(m,rot);
    }

    //! decompose homogeneouse matrix and  matrix
    template<class FT>
    void decomposeHom(const Matx44<FT> &m,  Matx33<FT> &m33) {
        m33 = Matx33<FT>(m(0,0),m(0,1),m(0,2),
                         m(1,0),m(1,1),m(1,2),
                         m(2,0),m(2,1),m(2,2));
    }

    //! compose homogeneouse matrix from trans vector and rot matrix
    template<class FT>
    Matx33<FT> composeHom(const Vec2<FT> &trans, const Matx22<FT> &rot) {
        return Matx33<FT>(rot(0,0),rot(0,1),trans.x(),
                          rot(1,0),rot(1,1),trans.y(),
                          FT(0) ,  FT(0),   FT(1));
    }

    //! compose homogeneouse matrix from other matrix
    template<class FT>
    Matx33<FT> composeHom(const Matx22<FT> &m) {
        return Matx33<FT>(m(0,0),m(0,1),FT(0),
                          m(1,0),m(1,1),FT(0),
                          FT(0) , FT(0),FT(1));
    }

    //! decompose homogeneouse matrix to trans vector and rot matrix
    template<class FT>
    void decomposeHom(const Matx33<FT> &m, Vec2<FT> &trans, Matx22<FT> &rot) {
        trans = Vec3<FT>(m(0,2),m(1,2));
        decomposeHom(m,rot);
    }

    //! decompose homogeneouse matrix to matrix
    template<class FT>
    void decomposeHom(const Matx33<FT> &m, Matx22<FT> &m22) {
        m22 = Matx33<FT>(m(0,0),m(0,1),
                         m(1,0),m(1,1));
    }
}

namespace Eigen {
    namespace internal {
        template<typename _Scalar, int _Rows, int _Cols>
        struct traits<lsfm::Matx<_Scalar, _Rows, _Cols> >
        {
          typedef _Scalar Scalar;
          typedef Dense StorageKind;
          typedef DenseIndex StorageIndex;
          typedef MatrixXpr XprKind;
          enum {
            RowsAtCompileTime = _Rows,
            ColsAtCompileTime = _Cols,
            MaxRowsAtCompileTime = Dynamic,
            MaxColsAtCompileTime = Dynamic,
            Flags = compute_matrix_flags<_Scalar, _Rows, _Cols, lsfm::Major<_Rows,_Cols>::type, Dynamic, Dynamic>::ret,
            CoeffReadCost = NumTraits<Scalar>::ReadCost,
            Options = lsfm::Major<_Rows,_Cols>::type,
            InnerStrideAtCompileTime = 1,
            OuterStrideAtCompileTime = (Options&RowMajor) ? ColsAtCompileTime : RowsAtCompileTime
          };
        };
    }
}

#endif
#endif


///////////////////////////////////////////////////////////////////////////////
// kernel::functional::rp_visitor.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_KERNEL_ESTIMATION_RP_VISITOR_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_RP_VISITOR_HPP_ER_2009
#include <boost/type_traits/is_reference.hpp>
#include <boost/mpl/not.hpp>
#include <boost/call_traits.hpp>
#include <boost/statistics/detail/kernel/estimation/detail/mean_accumulator.hpp>
//#include <boost/statistics/detail/kernel/estimation/detail/return_if.hpp>
//#include <boost/statistics/detail/kernel/estimation/detail/range_difference.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace kernel{

// This visitor, f, keeps a data point, x0, and each time f(x) is called, it
// updates an estimate of the density at x0, p(x0), by the Rosenblatt-Parzen
// method, using a given kernel k.
//
// The estimate is the average of the kernel evaluations over the traversed
// dataset. That average is implemented by an accumulator of type A.
//
// X can be a reference which is only recommended if the x object is expensive
// to copy
template<
    typename K,
    typename X,
    typename A = typename
        statistics::detail::kernel::detail::mean_accumulator<typename K::result_type>::type
>
class rp_visitor : K{ //, addable<rp_visitor<K,X,A> >{ //
    typedef is_reference<X> is_ref_;
    public:
    typedef K kernel_type;
    typedef A accumulator_type;
    typedef typename K::result_type result_type;

    // Construct
    rp_visitor();
    rp_visitor(typename call_traits<X>::param_type);
    rp_visitor(
        K k, // passing radius calls implicit conversion
        typename call_traits<X>::param_type x
    );
    rp_visitor(
        K k,
        typename call_traits<X>::param_type,
        const accumulator_type&
    );
    rp_visitor(const rp_visitor&);
    rp_visitor& operator=(const rp_visitor&);

    // Update
    // Passing the training data x1 updates the estimator
    template<typename X1> result_type operator()(const X1& x1);

    // Access
    typename call_traits<X>::const_reference x()const;
    const A& accumulator()const;
    const result_type& normalizing_constant()const;

    result_type estimate()const;

    private:
    typename call_traits<X>::value_type x_;
    A acc_;
};

//Construction
template<typename K,typename X,typename A>
rp_visitor<K,X,A>::rp_visitor(){
    BOOST_MPL_ASSERT((
        mpl::not_<is_ref_>
    ));
}

template<typename K,typename X,typename A>
rp_visitor<K,X,A>::rp_visitor(K k,typename call_traits<X>::param_type x)
:K(k),x_(x),acc_(){}

template<typename K,typename X,typename A>
rp_visitor<K,X,A>::rp_visitor(
    K k,
    typename call_traits<X>::param_type x,
    const A& a
):K(k),x_(x),acc_(a){}

template<typename K,typename X,typename A>
rp_visitor<K,X,A>::rp_visitor(const rp_visitor& that)
:K(static_cast<const K&>(that)),x_(that.x_),acc_(that.acc_){}

template<typename K,typename X,typename A>
typename rp_visitor<K,X,A>::rp_visitor&
rp_visitor<K,X,A>::operator=(const rp_visitor& that){
    if(&that!=this){
        BOOST_MPL_ASSERT((mpl::not_<is_ref_>));
        K::operator=(static_cast<const K&>(*that));
        x_ = that.x_;
        acc_ = that.acc_;
    }
    return *this;
}

// Evaluate
template<typename K,typename X,typename A>
template<typename X1>
typename rp_visitor<K,X,A>::result_type
rp_visitor<K,X,A>::operator()(const X1& x1){
    const K& kernel = static_cast<const K&>(*this);
    result_type t = kernel(x(),x1);
    this->acc_(t);
    return t;
}

// Access
template<typename K,typename X,typename A>
typename rp_visitor<K,X,A>::result_type
rp_visitor<K,X,A>::estimate()const{
    return accumulators::mean(
        this->accumulator()
    );
}

template<typename K,typename X,typename A>
const A&
rp_visitor<K,X,A>::accumulator()const{
    return  this->acc_;
}

template<typename K,typename X,typename A>
const typename rp_visitor<K,X,A>::result_type&
rp_visitor<K,X,A>::normalizing_constant()const{
    const K& k = static_cast<const K&>(*this);
    return k.normalizing_constant();
}

template<typename K,typename X,typename A>
typename call_traits<X>::const_reference
rp_visitor<K,X,A>::x()const{
    return this->x_;
}

}// kernel
}// detail
}// statistics
}// boost

#endif


#ifndef CAPPA_CLUSTER_DISTANCE_HPP
#define CAPPA_CLUSTER_DISTANCE_HPP

#include <Eigen/Dense>

namespace cluster {

double euclidean_distance(Eigen::VectorXd const &vector1, Eigen::VectorXd const &vector2);

Eigen::MatrixXd calculate_distance_matrix(Eigen::MatrixXd const &matrix);
}

#endif //CAPPA_CLUSTER_DISTANCE_HPP


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/25/problem25.hpp"

BOOST_AUTO_TEST_SUITE( Problem25 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem25::solve(3);
        BOOST_CHECK_EQUAL(res, 12);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem25::solve();
        BOOST_CHECK_EQUAL(res, 4782);
    }

BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// 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)
//=======================================================================
/**
 * @file scheduling_jobs_with_deadlines_on_a_single_machine.hpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2013-09-09
 */
#ifndef PAAL_SCHEDULING_JOBS_WITH_DEADLINES_ON_A_SINGLE_MACHINE_HPP
#define PAAL_SCHEDULING_JOBS_WITH_DEADLINES_ON_A_SINGLE_MACHINE_HPP

#include "paal/utils/functors.hpp"
#include "paal/utils/type_functions.hpp"
#include "paal/utils/assign_updates.hpp"

#include <boost/iterator/counting_iterator.hpp>
#include <boost/range/algorithm/sort.hpp>

#include <queue>
#include <vector>
#include <algorithm>
#include <utility>

namespace paal {
namespace greedy {

/**
 * @brief solve scheduling jobs on identical parallel machines problem
 * and fill start time of all jobs
 * example:
 * \snippet scheduling_jobs_with_deadlines_on_a_single_machine_example.cpp Scheduling Jobs On Single Machine Example
 * example file is
 * scheduling_jobs_with_deadlines_on_a_single_machine_example.cpp
 * @param first - jobs begin
 * @param last - jobs end
 * @param get_time
 * @param get_release_date
 * @param get_due_date
 * @param result
 * @tparam Time
 * @tparam InputIterator
 * @tparam OutputIterator
 * @tparam GetTime
 * @tparam GetDueDate
 * @tparam GetReleaseDate
 */
template <class InputIterator, class OutputIterator, class GetTime,
          class GetDueDate, class GetReleaseDate>
auto scheduling_jobs_with_deadlines_on_a_single_machine(
    const InputIterator first, const InputIterator last, GetTime get_time,
    GetReleaseDate get_release_date, GetDueDate get_due_date,
    OutputIterator result) {
    using Time = puretype(get_time(*first));
    std::vector<InputIterator> jobs;
    std::copy(boost::make_counting_iterator(first),
              boost::make_counting_iterator(last), std::back_inserter(jobs));

    auto get_due_date_from_iterator =
        utils::make_lift_iterator_functor(get_due_date);
    auto due_date_compatator = utils::make_functor_to_comparator(
        get_due_date_from_iterator, utils::greater{});
    using QueueType = std::priority_queue<
        InputIterator, std::vector<InputIterator>, decltype(due_date_compatator)>;
    QueueType active_jobs_iters(due_date_compatator);

    auto get_release_date_from_iterator =
        utils::make_lift_iterator_functor(get_release_date);
    boost::sort(jobs,
              utils::make_functor_to_comparator(get_release_date_from_iterator));
    Time start_idle = Time();
    Time longest_delay = Time();
    auto do_job = [&]() {
        auto job_iter = active_jobs_iters.top();
        active_jobs_iters.pop();
        Time start_time = std::max(start_idle, get_release_date(*job_iter));
        start_idle = start_time + get_time(*job_iter);
        assign_max(longest_delay, start_idle - get_due_date(*job_iter));
        *result = std::make_pair(job_iter, start_time);
        ++result;
    };
    for (auto job_iter : jobs) {
        while (!active_jobs_iters.empty() &&
               get_release_date(*job_iter) > start_idle)
            do_job();
        active_jobs_iters.push(job_iter);
    }
    while (!active_jobs_iters.empty()) {
        do_job();
    }

    return longest_delay;
}

} //!greedy
} //!paal

#endif // PAAL_SCHEDULING_JOBS_WITH_DEADLINES_ON_A_SINGLE_MACHINE_HPP


#ifndef GEM_TENSOR_BASE_HPP_INCLUDED
#define GEM_TENSOR_BASE_HPP_INCLUDED

#include <ostream>
#include <string>

#include <boost/hana/type.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/reverse.hpp>
#include <boost/hana/back.hpp>
#include <boost/hana/drop_back.hpp>
#include <boost/hana/for_each.hpp>
#include <boost/hana/greater.hpp>
#include <boost/hana/assert.hpp>

#include <cereal/cereal.hpp>
#include <cereal/access.hpp>

#include <gem/type.hpp>
#include <gem/dimensions_tuple.hpp>
#include <gem/cereal/hana_binder.hpp>

namespace gem {

template <typename dtype, long long n_con, long long n_cov,
          gem::concepts::Dimension... dims>
class TensorBase
{

public:
    using type_t = TensorBase;
    using data_t = dtype;

    using dimension_tuple_t = DimensionTuple<false, n_con, n_cov, dims...>;
    using dimension_tuple_common_t = typename dimension_tuple_t::common_t;

    template<typename n_dtype, long long n_n_con, long long n_n_cov,
             gem::concepts::Dimension... n_dims>
    friend class TensorBase;

    friend class cereal::access;

public:
    static constexpr
    boost::hana::type<type_t> type {};

    static constexpr
    boost::hana::type<data_t> data_type {};

    static constexpr
    boost::hana::ullong<sizeof...(dims)> order {};

    static constexpr
    boost::hana::ullong<n_con> contravariants {};

    static constexpr
    boost::hana::ullong<n_cov> covariants {};

    dimension_tuple_t dim;

    static constexpr BOOST_HANA_CONSTANT_CHECK_MSG(
        contravariants + covariants == order,
        "Contravariants and covariants must add up to the order "
        "(number of dimensions)");
    static constexpr BOOST_HANA_CONSTANT_CHECK_MSG(
        0_u <= contravariants,
        "The number of contravariants must be positive");
    static constexpr  BOOST_HANA_CONSTANT_CHECK_MSG(
        0_u <= covariants,
        "The number of covariants must be positive");

public:

    TensorBase(void) = delete;

    constexpr inline TensorBase(const dims&... d) noexcept :
        dim(d...)
    {

    }

    constexpr inline TensorBase(dims&&... d) noexcept :
        dim(std::forward<dims>(d)...)
    {

    }

    constexpr inline TensorBase(const TensorBase & tensor) noexcept :
        dim(tensor.dim)
    {

    }

    constexpr inline TensorBase(TensorBase && tensor) noexcept :
        dim(std::move(tensor.dim))
    {

    }

    constexpr inline auto rows(void) const noexcept
        -> const std::enable_if_t<(order > 0_u),
                                  decltype(dim[GEM_START_IDX])> &
    {
        return dim[GEM_START_IDX];
    }

    constexpr inline auto cols(void) const noexcept
        -> const std::enable_if_t<(order > 1_u),
                                  decltype(dim[GEM_START_IDX + 1_u])> &
    {
        return dim[GEM_START_IDX + 1_u];
    }

    constexpr inline auto size(void) const noexcept
    {
        return dim.size();
    }

    constexpr inline auto operator =(const TensorBase & tensor) noexcept
    {
        dim = tensor.dim;
        return *this;
    }

    constexpr inline auto operator =(TensorBase && tensor) noexcept
    {
        dim = std::move(tensor.dim);
        return *this;
    }

    constexpr inline auto operator ==(const TensorBase & tensor) noexcept
    {
        return dim == tensor.dim;
    }

    constexpr inline auto operator !=(const TensorBase & tensor) noexcept
    {
        return dim != tensor.dim;
    }

    constexpr inline auto operator ()(void) const noexcept
        -> const type_t &
    {
        return *this;
    }

    constexpr inline auto operator ()(void) noexcept
        -> type_t &
    {
        return *this;
    }

    constexpr inline auto transpose(void) const noexcept
    {
        constexpr auto construct_ = [](auto... args) {
            return TensorBase<dtype, n_cov, n_con, decltype(args)...>(args...);
        };
        return boost::hana::unpack(boost::hana::reverse(dim), construct_);
    }

    friend inline auto
    operator <<(std::ostream& os, const TensorBase & tensor)
        -> std::ostream&
    {
        os << "covariants: " << covariants << ", ";
        os << "contravariants: " << contravariants << ", ";
        os << "dimensions: {";
        boost::hana::for_each(
            boost::hana::drop_back(tensor.dim), [&](auto const& d){
                os << d << ", ";
            }
        );
        os << boost::hana::back(tensor.dim) << '}';
        return os;
    }

private:

    template<class Archive>
    auto save(Archive & archive) const -> void
    {
        // archive(cereal::make_nvp("data_type",
        //                          gem::demangle(typeid(data_t).name())),
        //         CEREAL_NVP(order),
        //         CEREAL_NVP(contravariants), CEREAL_NVP(covariants),
        //         cereal::make_nvp("size", size()),
        //         cereal::make_nvp("dimensions", dim));
        archive(cereal::make_nvp("data_type",
                                 gem::demangle(typeid(data_t).name())),
                CEREAL_NVP(order),
                CEREAL_NVP(contravariants), CEREAL_NVP(covariants),
                cereal::make_nvp("size", size())
                // cereal::make_nvp("dimensions", dim)
                );
// archive(cereal::make_size_tag(n_con + n_cov));
        // archive(cereal::make_nvp("dimensions", dim));
    }

    template<class Archive>
    auto load(Archive & archive) -> void
    {

    }

};

template <typename dtype, long long n_con, long long n_cov,
          gem::concepts::Dimension... dims>
constexpr
boost::hana::ullong<sizeof...(dims)>
TensorBase<dtype, n_con, n_cov, dims...>::order;

template <typename dtype, long long n_con, long long n_cov,
          gem::concepts::Dimension... dims>
constexpr
boost::hana::ullong<n_con>
TensorBase<dtype, n_con, n_cov, dims...>::contravariants;

template <typename dtype, long long n_con, long long n_cov,
          gem::concepts::Dimension... dims>
constexpr
boost::hana::ullong<n_cov>
TensorBase<dtype, n_con, n_cov, dims...>::covariants;

template <typename dtype, long long n_con, long long n_cov,
          gem::concepts::Dimension... dims>
constexpr boost::hana::type<typename TensorBase<dtype, n_con, n_cov,
                            dims...>::type_t>
TensorBase<dtype, n_con, n_cov, dims...>::type;

template <typename dtype, long long n_con, long long n_cov,
          gem::concepts::Dimension... dims>
constexpr boost::hana::type<typename TensorBase<dtype, n_con, n_cov,
                                                dims...>::data_t>
TensorBase<dtype, n_con, n_cov, dims...>::data_type;

}  // namespace gem

#endif  // !GEM_TENSOR_BASE_HPP_INCLUDED


//
// Created by Zhongshi Jiang on 2/9/17.
//

#include <igl/igl_inline.h>
#include <Eigen/Dense>
namespace igl {

template <
    typename DerivedV,
    typename DerivedT,
    typename DerivedF,
    typename DerivedSV,
    typename DerivedSVI,
    typename DerivedSVJ,
    typename DerivedST,
    typename DerivedSF>
IGL_INLINE void remove_duplicate_vertices(
    const Eigen::MatrixBase<DerivedV>& V,
    const Eigen::MatrixBase<DerivedT>& T,
    const Eigen::MatrixBase<DerivedF>& F,
    const double epsilon,
    Eigen::PlainObjectBase<DerivedSV>& SV,
    Eigen::PlainObjectBase<DerivedSVI>& SVI,
    Eigen::PlainObjectBase<DerivedSVJ>& SVJ,
    Eigen::PlainObjectBase<DerivedST>& ST,
    Eigen::PlainObjectBase<DerivedSF>& SF) {
  using namespace Eigen;
  using namespace std;
  remove_duplicate_vertices(V, epsilon, SV, SVI, SVJ);
  SF.resizeLike(F);
  ST.resizeLike(T);
  for (int f = 0; f < F.rows(); f++) {
    for (int c = 0; c < F.cols(); c++) {
      SF(f, c) = SVJ(F(f, c));
    }
  }
  for (int f = 0; f < T.rows(); f++) {
    for (int c = 0; c < T.cols(); c++) {
      ST(f, c) = SVJ(T(f, c));
    }
  }

};
}
/*
template void igl::remove_duplicate_vertices<Eigen::Matrix<double, -1, -1, 0,
                                                          -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, double, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&);
*/