#include "contour_optimization.h"

#include "session_data.h"
#include "frame_data.h"
#include "focal_grid.h"
#include "image_segmentation.h"
#include "model_class.h"

#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <string>
#include <stdint.h>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>
#include <list>
#include <thread>
#include <chrono>
#include <algorithm>
#include <sys/types.h>
#include <sys/stat.h>
#include <dirent.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <armadillo>

#include <nlopt.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Boolean_set_operations_2.h>
#include <CGAL/algorithm.h>
#include <CGAL/assertions.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/Polygon_set_2.h>

#define PI 3.14159265

using namespace std;

namespace p = boost::python;
namespace np = boost::python::numpy;

typedef CGAL::Exact_predicates_inexact_constructions_kernel KI;
//typedef CGAL::Exact_predicates_exact_constructions_kernel KE;

typedef KI::Point_2 				   Point_2;
typedef CGAL::Polygon_2<KI>            Polygon_2;
typedef CGAL::Polygon_with_holes_2<KI> Polygon_with_holes_2;
typedef CGAL::Polygon_set_2<KI> 	   Polygon_set_2;

typedef struct {
	FocalGrid* fg;
	ContourOpt* opt;
	ModelClass* mdl;
	int N_cam;
	const vector<list<Polygon_with_holes_2>> body_dest;
	const vector<list<Polygon_with_holes_2>> wing_L_dest;
	const vector<list<Polygon_with_holes_2>> wing_R_dest;
} cost_func_data;

double cost_function(const vector<double> &x, vector<double> &grad, void *data) {

	clock_t start;
	double duration;

	start = clock();

	cost_func_data *d = reinterpret_cast<cost_func_data*>(data);
	FocalGrid* fg = d->fg;
	ContourOpt* opt = d->opt;
	ModelClass* mdl = d->mdl;
	int N_cam = d->N_cam;
	//vector<list<Polygon_with_holes_2>> body_dest = d->body_dest;
	//vector<list<Polygon_with_holes_2>> wing_L_dest = d->wing_L_dest;
	//vector<list<Polygon_with_holes_2>> wing_R_dest = d->wing_R_dest;

	vector<double> state_now;
	state_now.push_back(x[0]);
	state_now.push_back(x[1]);
	state_now.push_back(x[2]);
	state_now.push_back(x[3]);
	state_now.push_back(x[4]);
	state_now.push_back(x[5]);
	state_now.push_back(x[6]);
	state_now.push_back(x[7]);
	state_now.push_back(x[8]);
	state_now.push_back(x[9]);
	state_now.push_back(x[10]);
	state_now.push_back(x[11]);
	state_now.push_back(x[12]);
	state_now.push_back(x[13]);
	state_now.push_back(x[14]);

	vector<list<Polygon_with_holes_2>> body_src;
	vector<list<Polygon_with_holes_2>> wing_L_src;
	vector<list<Polygon_with_holes_2>> wing_R_src;

	vector<list<Polygon_with_holes_2>> body_dest;
	vector<list<Polygon_with_holes_2>> wing_L_dest;
	vector<list<Polygon_with_holes_2>> wing_R_dest;

	// Convert state x to affine matrix transforms:
	cout << "return src state" << endl;
	vector<double> src_state = mdl->ReturnSRCState(state_now);
	cout << "size src state" << src_state.size() << endl;

	cout << "get src contours" << endl;
	for (int i=0; i<N_cam; i++) {
		cout << "get silhouette" << endl;
		vector<Polygon_2> m_silhouette = mdl->ReturnSilhouette2(*fg,src_state,i);
		cout << "size silhouette" << m_silhouette.size() << endl;
		vector<Polygon_2> m_body;
		for (int j=0; j<3; j++) {
			m_body.push_back(m_silhouette[j]);
		}
		list<Polygon_with_holes_2> body_contour_poly = opt->CalcUnion(m_body);
		body_src.push_back(body_contour_poly);
		body_dest.push_back(d->body_dest[i]);
		vector<Polygon_2> m_wing_L;
		m_wing_L.push_back(m_silhouette[3]);
		list<Polygon_with_holes_2> wing_L_contour_poly = opt->CalcUnion(m_wing_L);
		list<Polygon_with_holes_2> wing_L_complement = opt->CalcComplement(body_contour_poly, wing_L_contour_poly);
		wing_L_src.push_back(wing_L_complement);
		wing_L_dest.push_back(d->wing_L_dest[i]);
		vector<Polygon_2> m_wing_R;
		m_wing_R.push_back(m_silhouette[4]);
		list<Polygon_with_holes_2> wing_R_contour_poly = opt->CalcUnion(m_wing_R);
		list<Polygon_with_holes_2> wing_R_complement = opt->CalcComplement(body_contour_poly, wing_R_contour_poly);
		wing_R_src.push_back(wing_R_complement);
		wing_R_dest.push_back(d->wing_R_dest[i]);
	}

	// Calculate the cost by calculating the symmetric difference per view per polygon list:
	double cost = 0.0;

	list<Polygon_with_holes_2> body_diff;
	list<Polygon_with_holes_2> wing_L_diff;
	list<Polygon_with_holes_2> wing_R_diff;

	cout << "body src vec size: " << body_src.size() << endl;
	cout << "body dest vec size: " << body_dest.size() << endl;

	for (int i=0; i<N_cam; i++) {
		cout << "i " << i << endl;
		if (body_dest[i].size()>0 && body_src[i].size()>0) {
			//body_diff.clear();
			cout << "Calc symmetric difference body " << endl;
			list<Polygon_with_holes_2> body_diff = opt->CalcSymmetricDifference(body_dest[i],body_src[i]);
			if (body_diff.size() > 0) {
				for (list<Polygon_with_holes_2>::iterator it1 = body_diff.begin(); it1 != body_diff.end(); ++it1) {
					cout << "area: " << it1->outer_boundary().area() << endl;
					//cost += it->outer_boundary().area();
				}
			}
		}
		/*
		if (wing_L_dest[i].size()>0 && wing_L_src[i].size()>0) {
			//wing_L_diff.clear();
			cout << "Calc symmetric difference wing_L " << endl;
			list<Polygon_with_holes_2> wing_L_diff = opt->CalcSymmetricDifference(wing_L_dest[i],wing_L_src[i]);
			if (wing_L_diff.size() > 0) {
				for (list<Polygon_with_holes_2>::iterator it2 = wing_L_diff.begin(); it2 != wing_L_diff.end(); ++it2) {
					cout << "area: " << it2->outer_boundary().area() << endl;
					//cost += it->outer_boundary().area();
				}
			}
		}
		cout << "dest polygons size " << wing_R_dest[i].size() << endl;
		cout << "src polygons size "<< wing_R_src[i].size() << endl;
		if (wing_R_dest[i].size()>0 && wing_R_src[i].size()>0) {
			//wing_R_diff.clear();
			cout << "wing_R_dest" << endl;
			cout << wing_R_dest[i].front() << endl;
			cout << "wing_R_src" << endl;
			cout << wing_R_src[i].front() << endl;
			cout << "Calc symmetric difference wing_R " << endl;
			list<Polygon_with_holes_2> wing_R_diff = opt->CalcSymmetricDifference(wing_R_dest[i],wing_R_src[i]);
			cout << "wing_R_diff size " << wing_R_diff.size() << endl;
			if (wing_R_diff.size() > 0) {
				for (list<Polygon_with_holes_2>::iterator it3 = wing_R_diff.begin(); it3 != wing_R_diff.end(); ++it3) {
					cout << "area: " << it3->outer_boundary().area() << endl;
					//cost += it->outer_boundary().area();
				}
			}
		}
		*/
	}

	cout << "cost: " << cost << endl;
	duration = ( clock() - start ) / (double) CLOCKS_PER_SEC;
    cout<<"printf: "<< duration <<'\n';

	return cost;
}

double quat_constraint_body(const std::vector<double> &x, std::vector<double> &grad, void *data) {
	double q_norm = sqrt(pow(x[0],2)+pow(x[1],2)+pow(x[2],2)+pow(x[3],2));
	return (1.0-q_norm);
}

double quat_constraint_wing_L(const std::vector<double> &x, std::vector<double> &grad, void *data) {
	double q_norm = sqrt(pow(x[7],2)+pow(x[8],2)+pow(x[9],2)+pow(x[10],2));
	return (1.0-q_norm);
}

double quat_constraint_wing_R(const std::vector<double> &x, std::vector<double> &grad, void *data) {
	double q_norm = sqrt(pow(x[11],2)+pow(x[12],2)+pow(x[13],2)+pow(x[14],2));
	return (1.0-q_norm);
}

/*
void Optimize(FocalGrid &fg, frame_data &frame_in, ModelClass &mdl, ContourOpt &opt) {

	int N_cam = frame_in.N_cam;

	cost_func_data cost_data = {fg, opt, mdl, N_cam, opt.body_dest_polygons, opt.wing_L_dest_polygons, opt.wing_R_dest_polygons};

    vector<double> lb(15);
    lb[0] = -1.0;
	lb[1] = -1.0;
	lb[2] = -1.0;
	lb[3] = -1.0;
	lb[4] = -10.0;
	lb[5] = -10.0;
	lb[6] = -10.0;
	lb[7] = -1.0;
	lb[8] = -1.0;
	lb[9] = -1.0;
	lb[10] = -1.0;
	lb[11] = -1.0;
	lb[12] = -1.0;
	lb[13] = -1.0;
	lb[14] = -1.0;

	vector<double> ub(15);
	ub[0] = -1.0;
	ub[1] = -1.0;
	ub[2] = -1.0;
	ub[3] = -1.0;
	ub[4] = -10.0;
	ub[5] = -10.0;
	ub[6] = -10.0;
	ub[7] = -1.0;
	ub[8] = -1.0;
	ub[9] = -1.0;
	ub[10] = -1.0;
	ub[11] = -1.0;
	ub[12] = -1.0;
	ub[13] = -1.0;
	ub[14] = -1.0;

	vector<double> x(15);
	x[0] = frame_in.init_state(0,0);
	x[1] = frame_in.init_state(1,0);
	x[2] = frame_in.init_state(2,0);
	x[3] = frame_in.init_state(3,0);
	x[4] = frame_in.init_state(4,0);
	x[5] = frame_in.init_state(5,0);
	x[6] = frame_in.init_state(6,0);
	x[7] = frame_in.init_state(0,1);
	x[8] = frame_in.init_state(1,1);
	x[9] = frame_in.init_state(2,1);
	x[10] = frame_in.init_state(3,1);
	x[11] = frame_in.init_state(0,2);
	x[12] = frame_in.init_state(1,2);
	x[13] = frame_in.init_state(2,2);
	x[14] = frame_in.init_state(3,2);

	nlopt::opt opt_alg(nlopt::LN_COBYLA, 15);
	opt_alg.set_lower_bounds(lb);
	opt_alg.set_upper_bounds(ub);
	opt_alg.set_min_objective(cost_function, &cost_data);
	opt_alg.add_equality_constraint(quat_constraint_body, NULL, 1e-6);
	opt_alg.add_equality_constraint(quat_constraint_wing_L, NULL, 1e-6);
	opt_alg.add_equality_constraint(quat_constraint_wing_R, NULL, 1e-6);
	opt_alg.set_xtol_rel(1e-5);

	cout << " -------------------- " << endl;
	cout << "x start: " << x[0] << ", " << x[1] << ", " << x[2] << ", " << x[3] << ", " << x[4]
		<< ", " << x[5] << ", " << x[6] << endl;
	cout << x[7] << ", " << x[8] << ", " << x[9] << ", " << x[10] << endl;
	cout << x[11] << ", " << x[12] << ", " << x[13] << ", " << x[14] << endl;

	double minf;

	nlopt::result result = opt_alg.optimize(x, minf);

	cout << "x opt: " << x[0] << ", " << x[1] << ", " << x[2] << ", " << x[3] << ", " << x[4]
		<< ", " << x[5] << ", " << x[6] << endl;
	cout << x[7] << ", " << x[8] << ", " << x[9] << ", " << x[10] << endl;
	cout << x[11] << ", " << x[12] << ", " << x[13] << ", " << x[14] << endl;

	cout << "quaternion norm body: " << sqrt(pow(x[0],2)+pow(x[1],2)+pow(x[2],2)+pow(x[3],2)) << endl;
	cout << "quaternion norm wing L: " << sqrt(pow(x[7],2)+pow(x[8],2)+pow(x[9],2)+pow(x[10],2)) << endl;
	cout << "quaternion norm wing R: " << sqrt(pow(x[11],2)+pow(x[12],2)+pow(x[13],2)+pow(x[14],2)) << endl;
	cout << " -------------------- " << endl;
	cout << endl;
}
*/

ContourOpt::ContourOpt() {
	// Empty
}

void ContourOpt::OptimizeState(FocalGrid &fg, frame_data &frame_in, ModelClass &mdl, ImagSegm &seg) {
	ContourOpt::FindDestinationContour(fg, frame_in, seg);
	ContourOpt::FindInitContour(fg, frame_in, mdl, seg);
	ContourOpt::Optimize(fg, frame_in, mdl);
}

void ContourOpt::Optimize(FocalGrid &fg, frame_data &frame_in, ModelClass &mdl) {

	int N_cam = frame_in.N_cam;

	cost_func_data cost_data = {&fg, this, &mdl, N_cam, body_dest_polygons, wing_L_dest_polygons, wing_R_dest_polygons};

    vector<double> lb(15);
    lb[0] = -1.0;
	lb[1] = -1.0;
	lb[2] = -1.0;
	lb[3] = -1.0;
	lb[4] = -10.0;
	lb[5] = -10.0;
	lb[6] = -10.0;
	lb[7] = -1.0;
	lb[8] = -1.0;
	lb[9] = -1.0;
	lb[10] = -1.0;
	lb[11] = -1.0;
	lb[12] = -1.0;
	lb[13] = -1.0;
	lb[14] = -1.0;

	vector<double> ub(15);
	ub[0] = 1.0;
	ub[1] = 1.0;
	ub[2] = 1.0;
	ub[3] = 1.0;
	ub[4] = 10.0;
	ub[5] = 10.0;
	ub[6] = 10.0;
	ub[7] = 1.0;
	ub[8] = 1.0;
	ub[9] = 1.0;
	ub[10] = 1.0;
	ub[11] = 1.0;
	ub[12] = 1.0;
	ub[13] = 1.0;
	ub[14] = 1.0;

	vector<double> x(15);
	x[0] = frame_in.init_state(0,0);
	x[1] = frame_in.init_state(1,0);
	x[2] = frame_in.init_state(2,0);
	x[3] = frame_in.init_state(3,0);
	x[4] = frame_in.init_state(4,0);
	x[5] = frame_in.init_state(5,0);
	x[6] = frame_in.init_state(6,0);
	x[7] = frame_in.init_state(0,1);
	x[8] = frame_in.init_state(1,1);
	x[9] = frame_in.init_state(2,1);
	x[10] = frame_in.init_state(3,1);
	x[11] = frame_in.init_state(0,2);
	x[12] = frame_in.init_state(1,2);
	x[13] = frame_in.init_state(2,2);
	x[14] = frame_in.init_state(3,2);

	nlopt::opt opt_alg(nlopt::LN_COBYLA, 15);
	opt_alg.set_lower_bounds(lb);
	opt_alg.set_upper_bounds(ub);
	opt_alg.set_min_objective(cost_function, &cost_data);
	opt_alg.add_equality_constraint(quat_constraint_body, NULL, 1e-6);
	opt_alg.add_equality_constraint(quat_constraint_wing_L, NULL, 1e-6);
	opt_alg.add_equality_constraint(quat_constraint_wing_R, NULL, 1e-6);
	opt_alg.set_xtol_rel(1e-5);

	cout << " -------------------- " << endl;
	cout << "x start: " << x[0] << ", " << x[1] << ", " << x[2] << ", " << x[3] << ", " << x[4]
		<< ", " << x[5] << ", " << x[6] << endl;
	cout << x[7] << ", " << x[8] << ", " << x[9] << ", " << x[10] << endl;
	cout << x[11] << ", " << x[12] << ", " << x[13] << ", " << x[14] << endl;

	double minf;

	nlopt::result result = opt_alg.optimize(x, minf);

	cout << "x opt: " << x[0] << ", " << x[1] << ", " << x[2] << ", " << x[3] << ", " << x[4]
		<< ", " << x[5] << ", " << x[6] << endl;
	cout << x[7] << ", " << x[8] << ", " << x[9] << ", " << x[10] << endl;
	cout << x[11] << ", " << x[12] << ", " << x[13] << ", " << x[14] << endl;

	cout << "quaternion norm body: " << sqrt(pow(x[0],2)+pow(x[1],2)+pow(x[2],2)+pow(x[3],2)) << endl;
	cout << "quaternion norm wing L: " << sqrt(pow(x[7],2)+pow(x[8],2)+pow(x[9],2)+pow(x[10],2)) << endl;
	cout << "quaternion norm wing R: " << sqrt(pow(x[11],2)+pow(x[12],2)+pow(x[13],2)+pow(x[14],2)) << endl;
	cout << " -------------------- " << endl;
	cout << endl;
}

void ContourOpt::FindDestinationContour(FocalGrid &fg, frame_data &frame_in, ImagSegm &seg) {

	clock_t start;
	double duration;

	start = clock();

	body_dest_polygons.clear();
	wing_L_dest_polygons.clear();
	wing_R_dest_polygons.clear();
	body_dest_contours.clear();
	wing_L_dest_contours.clear();
	wing_R_dest_contours.clear();

	int N_cam = frame_in.N_cam;

	// Get the body and wing pointclouds
	arma::uvec body_pt_ids   = arma::find(frame_in.body_and_wing_pcls.row(3) == 1.0);
	arma::uvec wing_L_pt_ids = arma::find(frame_in.body_and_wing_pcls.row(3) == 2.0);
	arma::uvec wing_R_pt_ids = arma::find(frame_in.body_and_wing_pcls.row(3) == 3.0);

	arma::Mat<double> body_pcl = frame_in.body_and_wing_pcls.cols(body_pt_ids);
	arma::Mat<double> wing_L_pcl = frame_in.body_and_wing_pcls.cols(wing_L_pt_ids);
	arma::Mat<double> wing_R_pcl = frame_in.body_and_wing_pcls.cols(wing_R_pt_ids);

	//arma::Mat<double> means = ContourOpt::KMeans(body_pcl, 3, 10);

	//cout << means << endl;

	vector<arma::Col<int>> body_proj = fg.FocalGrid::ProjectCloud2Frames(body_pcl);
	vector<arma::Col<int>> wing_L_proj = fg.FocalGrid::ProjectCloud2Frames(wing_L_pcl);
	vector<arma::Col<int>> wing_R_proj = fg.FocalGrid::ProjectCloud2Frames(wing_R_pcl);

	vector<vector<arma::Mat<double>>> contour_vec = ContourOpt::FindContours(body_proj, wing_L_proj, wing_R_proj, frame_in, seg);

	for (int i=0; i<N_cam; i++) {
		body_dest_polygons.push_back(ContourOpt::Convert2PolygonWithHoles(contour_vec[i][0]));
		wing_L_dest_polygons.push_back(ContourOpt::Convert2PolygonWithHoles(contour_vec[i][1]));
		wing_R_dest_polygons.push_back(ContourOpt::Convert2PolygonWithHoles(contour_vec[i][2]));
		body_dest_contours.push_back(contour_vec[i][0]);
		wing_L_dest_contours.push_back(contour_vec[i][1]);
		wing_R_dest_contours.push_back(contour_vec[i][2]);
	}

	duration = ( clock() - start ) / (double) CLOCKS_PER_SEC;
    cout<<"printf: "<< duration <<'\n';

}

arma::Mat<double> ContourOpt::KMeans(arma::Mat<double> &pcl_in, int N_clusters, int N_iter) {

	arma::Mat<double> data = pcl_in.rows(0,2);

	arma::Mat<double> means;

	bool status =arma::kmeans(means,data,N_clusters,arma::random_subset,N_iter,false);

	if (status==false) {
		cout << "clustering failed" << endl;
	}

	return means;
}

void ContourOpt::FindInitContour(FocalGrid &fg, frame_data &frame_in, ModelClass &mdl, ImagSegm &seg) {

	clock_t start;
	double duration;

	start = clock();

	body_init_contours.clear();
	wing_L_init_contours.clear();
	wing_R_init_contours.clear();

	int N_cam = frame_in.N_cam;

	// Get model pcl's in initial state:

	vector<arma::Mat<double>> M_init_vec = mdl.ReturnInitState(frame_in);

	for (int i=0; i<N_cam; i++) {
		vector<Polygon_2> m_silhouette = mdl.ReturnSilhouette(fg, M_init_vec,fg.CalculateViewVector(i),i);
		vector<Polygon_2> m_body;
		for (int j=0; j<3; j++) {
			m_body.push_back(m_silhouette[j]);
		}
		list<Polygon_with_holes_2> body_contour_poly = ContourOpt::CalcUnion(m_body);
		body_init_contours.push_back(ContourOpt::ConvertPolygonWithHoles2Mat(body_contour_poly));
		vector<Polygon_2> m_wing_L;
		m_wing_L.push_back(m_silhouette[3]);
		list<Polygon_with_holes_2> wing_L_contour_poly = ContourOpt::CalcUnion(m_wing_L);
		list<Polygon_with_holes_2> wing_L_complement = ContourOpt::CalcComplement(body_contour_poly, wing_L_contour_poly);
		wing_L_init_contours.push_back(ContourOpt::ConvertPolygonWithHoles2Mat(wing_L_complement));
		vector<Polygon_2> m_wing_R;
		m_wing_R.push_back(m_silhouette[4]);
		list<Polygon_with_holes_2> wing_R_contour_poly = ContourOpt::CalcUnion(m_wing_R);
		list<Polygon_with_holes_2> wing_R_complement = ContourOpt::CalcComplement(body_contour_poly, wing_R_contour_poly);
		wing_R_init_contours.push_back(ContourOpt::ConvertPolygonWithHoles2Mat(wing_R_complement));
	}

	duration = ( clock() - start ) / (double) CLOCKS_PER_SEC;
    cout<<"printf: "<< duration <<'\n';
}

vector<vector<arma::Mat<double>>> ContourOpt::FindContours(vector<arma::Col<int>> &body_proj, vector<arma::Col<int>> &wing_L_proj, vector<arma::Col<int>> &wing_R_proj, frame_data &frame_in, ImagSegm &seg) {

	int N_cam = frame_in.N_cam;

	vector<vector<arma::Mat<double>>> contour_vec_out;

	vector<arma::Mat<double>> contour_vec_now;

	arma::Mat<double> contour_now;

	arma::Col<int> proj_body_img;
	arma::Col<int> proj_wing_L_img;
	arma::Col<int> proj_wing_R_img;

	for (int i=0; i<N_cam; i++) {

		contour_vec_now.clear();

		int N_row = get<0>(frame_in.image_size[i]);
		int N_col = get<1>(frame_in.image_size[i]);

		proj_body_img = body_proj[i];
		proj_wing_L_img = wing_L_proj[i]%(1-body_proj[i]);
		proj_wing_R_img = wing_R_proj[i]%(1-body_proj[i]);

		contour_vec_now.push_back(seg.FindContours(proj_body_img,N_row,N_col));
		contour_vec_now.push_back(seg.FindContours(proj_wing_L_img,N_row,N_col));
		contour_vec_now.push_back(seg.FindContours(proj_wing_R_img,N_row,N_col));

		contour_vec_out.push_back(contour_vec_now);
	}
	return contour_vec_out;
}

arma::Mat<double> ContourOpt::CalculateContour(arma::Mat<double> &pts_in, double d_u, double d_v) {

	int N_pts = pts_in.n_cols;

	// find starting point (minimum u index):

	arma::Col<double> uv_start = pts_in.col(arma::index_min(pts_in.row(0)));

	arma::Col<double> uv_now = uv_start;

	arma::Col<double> uv_prev = uv_start;

	arma::uvec inds_u_plus;
	arma::uvec inds_u_min;
	arma::uvec inds_v_plus;
	arma::uvec inds_v_min;

	arma::uvec inds_neighbors;

	double theta_now;

	double theta_min;

	int theta_min_ind;

	arma::Mat<double> contour_pts;

	contour_pts = uv_start;

	int iter = 0;

	int N_contour_pts = 0;

	bool contour_closed = false;

	while (iter<N_pts && contour_closed==false) {

		inds_u_plus = arma::find(pts_in.row(0)>(uv_now(0)-d_u));
		inds_u_min = arma::find(pts_in.row(0)<(uv_now(0)+d_u));
		inds_v_plus = arma::find(pts_in.row(1)>(uv_now(1)-d_v));
		inds_v_min = arma::find(pts_in.row(1)<(uv_now(1)+d_v));

		inds_neighbors = arma::intersect(arma::intersect(inds_u_plus,inds_u_min),arma::intersect(inds_v_plus,inds_v_min));

		if (inds_neighbors.n_rows>0) {

			theta_min = 2*PI;
			theta_min_ind = -1;

			for (int i=0; i<inds_neighbors.n_rows; i++) {
				theta_now = atan2(pts_in(1,inds_neighbors(i))-uv_prev(1),pts_in(0,inds_neighbors(i))-uv_prev(0));
				if (isnan(theta_now)==0) {
					if (theta_now<theta_min) {
						theta_min = theta_now;
						theta_min_ind = i;
					}
				}
			}

			uv_prev = uv_now;

			uv_now = pts_in.col(inds_neighbors(theta_min_ind));

			contour_pts.insert_cols(N_contour_pts,uv_now);
			N_contour_pts++;
			if (uv_now(0)==uv_start(0) && uv_now(1)==uv_start(1)) {
				contour_closed = true;
			}
		}
		else {
			iter = N_pts;
			cout << "could not close contour" << endl;
		}
		iter++;
	}

	return contour_pts;
}

list<Polygon_with_holes_2> ContourOpt::CalcUnion(vector<Polygon_2> &polygons_in) {

	int N_c = polygons_in.size();

	Polygon_set_2 S;

	for (int i=0; i<N_c; i++) {
		if (i==0) {
			S.insert(polygons_in[i]);
		}
		else {
			S.join(polygons_in[i]);
		}
	}

	//S.union()

	list<Polygon_with_holes_2> res;
	S.polygons_with_holes (back_inserter (res));

	return res;
}

list<Polygon_with_holes_2> ContourOpt::CalcComplement(list<Polygon_with_holes_2> &pol_A, list<Polygon_with_holes_2> &pol_B) {

	Polygon_set_2 S;

	int iter_1 =0;
	for (list<Polygon_with_holes_2>::iterator it_1=pol_B.begin(); it_1 !=pol_B.end(); ++it_1) {
		//cout << "pol B area " << it_1->outer_boundary().area() << endl;
		if (iter_1==0) {
			S.insert(*it_1);
		}
		else {
			S.join(*it_1);
		}
		iter_1++;
	}

	int iter_2 = 0;
	for (list<Polygon_with_holes_2>::iterator it_2=pol_A.begin(); it_2 !=pol_A.end(); ++it_2) {
		//cout << "pol A area " << it_2->outer_boundary().area() << endl;
		S.difference(*it_2);
	}

	list<Polygon_with_holes_2> res;
	S.polygons_with_holes (back_inserter (res));

	return res;
}

list<Polygon_with_holes_2> ContourOpt::CalcSymmetricDifference(list<Polygon_with_holes_2> &pol_A, list<Polygon_with_holes_2> &pol_B) {

	/*
	Polygon_set_2 S;

	int iter_1 =0;
	for (list<Polygon_with_holes_2>::iterator it_1=pol_B.begin(); it_1 !=pol_B.end(); ++it_1) {
		if (iter_1==0) {
			S.insert(*it_1);
		}
		else {
			S.join(*it_1);
		}
		iter_1++;
	}

	int iter_2 = 0;
	for (list<Polygon_with_holes_2>::iterator it_2=pol_A.begin(); it_2 !=pol_A.end(); ++it_2) {
		S.symmetric_difference(*it_2);
	}

	list<Polygon_with_holes_2> res;
	S.polygons_with_holes (back_inserter (res));
	*/
	//cout << "caclulating symmetric difference" << endl;
	list<Polygon_with_holes_2> res_out;

	//if (!pol_A.empty() && !pol_B.empty()){
  	for (list<Polygon_with_holes_2>::iterator it_1=pol_B.begin(); it_1 !=pol_B.end(); ++it_1) {
  		for (list<Polygon_with_holes_2>::iterator it_2=pol_A.begin(); it_2 !=pol_A.end(); ++it_2) {
  			//if (CGAL::do_intersect(*it_1, *it_2)) {
  			//cout << "do intersect " << CGAL::do_intersect(*it_1, *it_2) << endl;
			//cout << "area pol_A " << it_2->outer_boundary().area() << endl;
			//cout << "area pol_B " << it_1->outer_boundary().area() << endl;
			CGAL::symmetric_difference (*it_1, *it_2, back_inserter(res_out));
			//}
			//else {
			//	cout << "do not intersect " << endl;
			//}
		}
	}
	//}

	cout << "Res size: " << res_out.size() << endl;

	return res_out;
}

arma::Mat<double> ContourOpt::ConvertPolygonWithHoles2Mat(list<Polygon_with_holes_2> &Pol_list_in) {

	int N_poly = Pol_list_in.size();
	int N_v;
	arma::Mat<double> mat_out;

	if (N_poly>0) {
		int iter = 0;
		vector<double> output_iterator;
		for (list<Polygon_with_holes_2>::iterator it=Pol_list_in.begin(); it !=Pol_list_in.end(); ++it) {
			//cout << it->is_unbounded() << endl;
			N_v = it->outer_boundary().size();
			arma::Mat<double> curr_pol_mat(3,N_v);
			for (int j=0; j<N_v; j++) {
				curr_pol_mat(0,j) = static_cast<double>(it->outer_boundary().vertex(j).x());
				curr_pol_mat(1,j) = static_cast<double>(it->outer_boundary().vertex(j).y());
				curr_pol_mat(2,j) = iter+1;
			}
			if (iter==0) {
				mat_out = curr_pol_mat;
			}
			else {
				mat_out = arma::join_rows(mat_out, curr_pol_mat);
			}
			iter++;
		}
	}
	else {
		mat_out.zeros(3,1);
	}

	return mat_out;
}

arma::Mat<double> ContourOpt::ConvertPolygon2Mat(Polygon_2 &Pol_in) {

	int N_v = Pol_in.size();

	arma::Mat<double> mat_out;
	mat_out.zeros(3,N_v);

	//int k=0;

	//for (VertexIterator vi = Pol_in.vertices_begin(); vi != Pol_in.vertices_end(); ++vi) {
	for (int i=0; i<N_v; i++) {

		mat_out(0,i) = static_cast<double>(Pol_in.vertex(i).x());
		mat_out(1,i) = static_cast<double>(Pol_in.vertex(i).y());
		mat_out(2,i) = 1.0;

		//k++;
	}

	return mat_out;
}

list<Polygon_with_holes_2> ContourOpt::Convert2PolygonWithHoles(arma::Mat<double> &contour_mat) {

	int N_contours = contour_mat.row(2).max();

	arma::uvec temp_inds;
	arma::Mat<double> temp_mat;

	list<Polygon_with_holes_2> poly_list_out;

	for (int i=0; i<N_contours; i++) {
		temp_inds = arma::find(contour_mat.row(2)==i+1);
		temp_mat = contour_mat.cols(temp_inds);
		Polygon_with_holes_2 t_poly(ContourOpt::Convert2Polygon(temp_mat));
		poly_list_out.push_back(t_poly);
	}

	return poly_list_out;
}

Polygon_2 ContourOpt::Convert2Polygon(arma::Mat<double> &contour_mat) {

	// Run through the points and construct a Polygon_2 object:
	Polygon_2 P;

	int N_pts = contour_mat.n_cols;

	//for (int i=0; i<N_pts; i++) {
	for (int i=(N_pts-1); i>=0; i--) {
		P.push_back(Point_2 (contour_mat(0,i),contour_mat(1,i)));
	}

	return P;
}

p::list ContourOpt::ReturnInitContour(int cam_nr) {

	p::list init_contour_list;

	arma::Mat<double> body_c = body_init_contours[cam_nr];
	arma::Mat<double> wing_L_c = wing_L_init_contours[cam_nr];
	arma::Mat<double> wing_R_c = wing_R_init_contours[cam_nr];

	arma::uvec found_ids;
	int N_found;

	for (int i=0; i<body_c.row(2).max(); i++) {
		found_ids = arma::find(body_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = body_c(0,found_ids(j));
				c_array[1][j] = body_c(1,found_ids(j));
				c_array[2][j] = 1;
			}
			init_contour_list.append(c_array);
		}
	}

	for (int i=0; i<wing_L_c.row(2).max(); i++) {
		found_ids = arma::find(wing_L_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = wing_L_c(0,found_ids(j));
				c_array[1][j] = wing_L_c(1,found_ids(j));
				c_array[2][j] = 2;
			}
			init_contour_list.append(c_array);
		}
	}

	for (int i=0; i<wing_R_c.row(2).max(); i++) {
		found_ids = arma::find(wing_R_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = wing_R_c(0,found_ids(j));
				c_array[1][j] = wing_R_c(1,found_ids(j));
				c_array[2][j] = 3;
			}
			init_contour_list.append(c_array);
		}
	}

	return init_contour_list;
}

p::list ContourOpt::ReturnDestContour(int cam_nr) {

	p::list dest_contour_list;

	arma::Mat<double> body_c = body_dest_contours[cam_nr];
	arma::Mat<double> wing_L_c = wing_L_dest_contours[cam_nr];
	arma::Mat<double> wing_R_c = wing_R_dest_contours[cam_nr];

	arma::uvec found_ids;
	int N_found;

	for (int i=0; i<body_c.row(2).max(); i++) {
		found_ids = arma::find(body_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = body_c(0,found_ids(j));
				c_array[1][j] = body_c(1,found_ids(j));
				c_array[2][j] = 1;
			}
			dest_contour_list.append(c_array);
		}
	}

	for (int i=0; i<wing_L_c.row(2).max(); i++) {
		found_ids = arma::find(wing_L_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = wing_L_c(0,found_ids(j));
				c_array[1][j] = wing_L_c(1,found_ids(j));
				c_array[2][j] = 2;
			}
			dest_contour_list.append(c_array);
		}
	}

	for (int i=0; i<wing_R_c.row(2).max(); i++) {
		found_ids = arma::find(wing_R_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = wing_R_c(0,found_ids(j));
				c_array[1][j] = wing_R_c(1,found_ids(j));
				c_array[2][j] = 3;
			}
			dest_contour_list.append(c_array);
		}
	}

	return dest_contour_list;
}


#include "operators_c.h"
#include "operators.h"
#include <Eigen/Dense>
#include <iostream>

using namespace std;

static dfloat_t *to_c(VectorXd &v)
{
  dfloat_t *vdata = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * v.size());
#if USE_DFLOAT_DOUBLE == 1
  Eigen::Map<Eigen::VectorXd>(vdata, v.size()) = v.cast<dfloat_t>();
#else
  Eigen::Map<Eigen::VectorXf>(vdata, v.size()) = v.cast<dfloat_t>();
#endif
  return vdata;
}

static dfloat_t *to_c(MatrixXd &m)
{
  dfloat_t *mdata = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * m.size());
#if USE_DFLOAT_DOUBLE == 1
  Eigen::Map<Eigen::MatrixXd>(mdata, m.rows(), m.cols()) = m.cast<dfloat_t>();
#else
  Eigen::Map<Eigen::MatrixXf>(mdata, m.rows(), m.cols()) = m.cast<dfloat_t>();
#endif
  return mdata;
}

static uintloc_t *to_c(MatrixXu32 &m)
{
  uintloc_t *mdata =
      (uintloc_t *)asd_malloc_aligned(sizeof(uintloc_t) * m.size());
  Eigen::Map<Eigen::Matrix<uintloc_t, Eigen::Dynamic, Eigen::Dynamic>>(
      mdata, m.rows(), m.cols()) = m.cast<uintloc_t>();
  return mdata;
}

host_operators_t *host_operators_new_2D(int N, int M, uintloc_t E,
                                        uintloc_t *EToE, uint8_t *EToF,
                                        uint8_t *EToO, double *EToVX)
{
  host_operators_t *ops =
      (host_operators_t *)asd_malloc(sizeof(host_operators_t));

  ref_elem_data *ref_data = build_ref_ops_2D(N, M, M);

  VectorXd wq = ref_data->wq;

  // nodal
  MatrixXd Dr = ref_data->Dr;
  MatrixXd Ds = ref_data->Ds;
  MatrixXd Vq = ref_data->Vq;

  VectorXd wfq = ref_data->wfq;
  VectorXd nrJ = ref_data->nrJ;
  VectorXd nsJ = ref_data->nsJ;
  MatrixXd Vfqf = ref_data->Vfqf;
  MatrixXd Vfq = ref_data->Vfq;
  MatrixXd Pq = ref_data->Pq;

  MatrixXd MM = Vq.transpose() * wq.asDiagonal() * Vq;
  MatrixXd MMfq = Vfq.transpose() * wfq.asDiagonal();
  MatrixXd Lq = mldivide(MM, MMfq);
  MatrixXd VqLq = Vq * Lq;
  MatrixXd VqPq = Vq * Pq;
  MatrixXd VfPq = Vfq * Pq;
  MatrixXd Drq = Vq * Dr * Pq - .5 * Vq * Lq * nrJ.asDiagonal() * Vfq * Pq;
  MatrixXd Dsq = Vq * Ds * Pq - .5 * Vq * Lq * nsJ.asDiagonal() * Vfq * Pq;

  ops->dim = 2;

  ops->N = N;
  ops->M = M;

  ops->Np = (int)ref_data->r.size();
  ops->Nq = (int)ref_data->rq.size();

  // printf("Num cubature points Nq = %d\n",ops->Nq);

  ops->Nfp = N + 1;
  ops->Nfq = (int)ref_data->ref_rfq.size();

  ops->Nfaces = ref_data->Nfaces;
  ops->Nvgeo = 4;
  ops->Nfgeo = 3;

  ops->wq = to_c(wq);
  ops->nrJ = to_c(nrJ);
  ops->nsJ = to_c(nsJ);

  ops->Drq = to_c(Drq);
  ops->Dsq = to_c(Dsq);

  ops->Vq = to_c(Vq);
  ops->Pq = to_c(Pq);

  ops->VqLq = to_c(VqLq);
  ops->VqPq = to_c(VqPq);
  ops->VfPq = to_c(VfPq);
  ops->Vfqf = to_c(Vfqf);

  Map<MatrixXd> EToVXmat(EToVX, 2 * 3, E);

  if (sizeof(uintloc_t) != sizeof(uint32_t))
  {
    cerr << "Need to update build maps to support different integer types"
         << endl;
    std::abort();
  }
  Map<MatrixXu32> mapEToE(EToE, 3, E);
  Map<MatrixXu8> mapEToF(EToF, 3, E);
  Map<MatrixXu8> mapEToO(EToO, 3, E);

  geo_elem_data *geo_data = build_geofacs_2D(ref_data, EToVXmat);
  map_elem_data *map_data = build_maps_2D(ref_data, mapEToE, mapEToF, mapEToO);

  const int Nvgeo = ops->Nvgeo;
  const int Nfgeo = ops->Nfgeo;
  const int Nfaces = ref_data->Nfaces;
  const int Nq = ops->Nq;
  const int Nfq = ops->Nfq;

  ops->xyzq =
      (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nq * 3 * E);
  ops->xyzf = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                             ops->Nfaces * 3 * E);
  ops->vgeo =
      (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nq * Nvgeo * E);
  ops->vfgeo = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                              Nfaces * Nvgeo * E);
  ops->fgeo = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                             Nfgeo * Nfaces * E);

  for (uintloc_t e = 0; e < E; ++e)
  {
    for (int n = 0; n < Nq; ++n)
    {
      ops->xyzq[n + 0 * Nq + e * Nq * 3] = (dfloat_t)geo_data->xq(n, e);
      ops->xyzq[n + 1 * Nq + e * Nq * 3] = (dfloat_t)geo_data->yq(n, e);
      // ops->xyzq[n + 2*Nq + e*Nq*3] = (dfloat_t)geo_data->zq(n,e);

      ops->vgeo[e * Nq * Nvgeo + 0 * Nq + n] = (dfloat_t)geo_data->rxJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 1 * Nq + n] = (dfloat_t)geo_data->ryJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 2 * Nq + n] = (dfloat_t)geo_data->sxJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 3 * Nq + n] = (dfloat_t)geo_data->syJ(n, e);
    }
  }

  for (uintloc_t e = 0; e < E; ++e)
  {
    for (int n = 0; n < Nfq * Nfaces; ++n)
    {
      ops->xyzf[n + 0 * Nfq * Nfaces + e * Nfq * Nfaces * 3] =
          (dfloat_t)geo_data->xf(n, e);
      ops->xyzf[n + 1 * Nfq * Nfaces + e * Nfq * Nfaces * 3] =
          (dfloat_t)geo_data->yf(n, e);

      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 0 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->rxJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 1 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->ryJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 2 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->sxJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 3 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->syJf(n, e);

      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 0 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->nxJ(n, e);
      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 1 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->nyJ(n, e);
      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 2 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->sJ(n, e);
    }
  }

  ops->Jq = to_c(geo_data->J);
  ops->mapPq = to_c(map_data->mapPq);
  //  ops->mapPqNoFields = to_c(map_data->mapPq);  // save

  /*
  // JC: FIX LATER - only valid for tri2.msh
  ops->mapPq[0] = 7;
  ops->mapPq[1] = 6;
  ops->mapPq[2] = 9;
  ops->mapPq[3] = 8;
  ops->mapPq[6] = 1;
  ops->mapPq[7] = 0;
  ops->mapPq[8] = 3;
  ops->mapPq[9] = 2;
  */

  //  for(int i = 0; i < Nfq*Nfaces*E; ++i){
  //    printf("mapPq(%d) = %d\n",i,ops->mapPq[i]);
  //  }
  ops->Fmask = to_c(map_data->fmask);

  delete ref_data;
  delete geo_data;
  delete map_data;

  return ops;
}

host_operators_t *host_operators_new_3D(int N, int M, uintloc_t E,
                                        uintloc_t *EToE, uint8_t *EToF,
                                        uint8_t *EToO, double *EToVX)
{
  host_operators_t *ops =
      (host_operators_t *)asd_malloc(sizeof(host_operators_t));

  ref_elem_data *ref_data = build_ref_ops_3D(N, M, M);

  VectorXd wq = ref_data->wq;

  // nodal
  MatrixXd Dr = ref_data->Dr;
  MatrixXd Ds = ref_data->Ds;
  MatrixXd Dt = ref_data->Dt;
  MatrixXd Vq = ref_data->Vq;
  MatrixXd Pq = ref_data->Pq;
  MatrixXd Vfqf = ref_data->Vfqf;
  MatrixXd Vfq = ref_data->Vfq;

  VectorXd wfq = ref_data->wfq;
  VectorXd nrJ = ref_data->nrJ;
  VectorXd nsJ = ref_data->nsJ;
  VectorXd ntJ = ref_data->ntJ;

  MatrixXd MM = Vq.transpose() * wq.asDiagonal() * Vq;
  MatrixXd MMfq = Vfq.transpose() * wfq.asDiagonal();
  MatrixXd Lq = mldivide(MM, MMfq);
  MatrixXd VqLq = Vq * Lq;
  MatrixXd VqPq = Vq * Pq;
  MatrixXd VfPq = Vfq * Pq;
  MatrixXd Drq = Vq * Dr * Pq - .5 * Vq * Lq * nrJ.asDiagonal() * Vfq * Pq;
  MatrixXd Dsq = Vq * Ds * Pq - .5 * Vq * Lq * nsJ.asDiagonal() * Vfq * Pq;
  MatrixXd Dtq = Vq * Dt * Pq - .5 * Vq * Lq * ntJ.asDiagonal() * Vfq * Pq;

  MatrixXd Drstq(Drq.rows(),3*Drq.cols());
  Drstq << Drq,Dsq,Dtq;

  /*
  cout << "VqPq = " << endl << VqPq << endl;
  cout << "VqLq = " << endl << VqLq << endl;
  cout << "VfPq = " << endl << VfPq << endl;

  cout << "Drq = " << endl << Drq << endl;
  cout << "nrJ = " << endl << nrJ << endl;
  cout << "Dsq = " << endl << Dsq << endl;
  cout << "nsJ = " << endl << nsJ << endl;
  cout << "Dtq = " << endl << Dtq << endl;
  cout << "ntJ = " << endl << ntJ << endl;
  */

  ops->dim = 3;

  ops->N = N;
  ops->M = M;

  ops->Np = (int)ref_data->r.size();
  ops->Nq = (int)ref_data->rq.size();

  ops->Nfp = (N + 1) * (N + 2) / 2;
  ops->Nfq = (int)ref_data->ref_rfq.size();

  ops->Nfaces = ref_data->Nfaces;
  ops->Nvgeo = 9;
  ops->Nfgeo = 4;

  ops->wq = to_c(wq);
  ops->nrJ = to_c(nrJ);
  ops->nsJ = to_c(nsJ);
  ops->ntJ = to_c(ntJ);

  ops->Drq = to_c(Drq);
  ops->Dsq = to_c(Dsq);
  ops->Dtq = to_c(Dtq);

  ops->Drstq = to_c(Drstq);

  ops->Vq = to_c(Vq);
  ops->Pq = to_c(Pq);

  ops->VqLq = to_c(VqLq);
  ops->VqPq = to_c(VqPq);
  ops->VfPq = to_c(VfPq);
  ops->Vfqf = to_c(Vfqf);

  Map<MatrixXd> EToVXmat(EToVX, 3 * 4, E);

  if (sizeof(uintloc_t) != sizeof(uint32_t))
  {
    cerr << "Need to update build maps to support different integer types"
         << endl;
    std::abort();
  }
  Map<MatrixXu32> mapEToE(EToE, 4, E);
  Map<MatrixXu8> mapEToF(EToF, 4, E);
  Map<MatrixXu8> mapEToO(EToO, 4, E);

  geo_elem_data *geo_data = build_geofacs_3D(ref_data, EToVXmat);
  map_elem_data *map_data = build_maps_3D(ref_data, mapEToE, mapEToF, mapEToO);

  const int Nvgeo = ops->Nvgeo;
  const int Nfgeo = ops->Nfgeo;
  const int Nfaces = ref_data->Nfaces;
  const int Nq = ops->Nq;
  const int Nfq = ops->Nfq;

  ops->xyzq =
      (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nq * 3 * E);
  ops->xyzf = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                             ops->Nfaces * 3 * E);
  ops->vgeo =
      (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nq * Nvgeo * E);
  ops->vfgeo = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                              Nfaces * Nvgeo * E);
  ops->fgeo = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                             Nfgeo * Nfaces * E);

  for (uintloc_t e = 0; e < E; ++e)
  {
    for (int n = 0; n < Nq; ++n)
    {
      ops->xyzq[n + 0 * Nq + e * Nq * 3] = (dfloat_t)geo_data->xq(n, e);
      ops->xyzq[n + 1 * Nq + e * Nq * 3] = (dfloat_t)geo_data->yq(n, e);
      ops->xyzq[n + 2 * Nq + e * Nq * 3] = (dfloat_t)geo_data->zq(n, e);

      ops->vgeo[e * Nq * Nvgeo + 0 * Nq + n] = (dfloat_t)geo_data->rxJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 1 * Nq + n] = (dfloat_t)geo_data->ryJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 2 * Nq + n] = (dfloat_t)geo_data->rzJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 3 * Nq + n] = (dfloat_t)geo_data->sxJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 4 * Nq + n] = (dfloat_t)geo_data->syJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 5 * Nq + n] = (dfloat_t)geo_data->szJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 6 * Nq + n] = (dfloat_t)geo_data->txJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 7 * Nq + n] = (dfloat_t)geo_data->tyJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 8 * Nq + n] = (dfloat_t)geo_data->tzJ(n, e);
    }
  }

  for (uintloc_t e = 0; e < E; ++e)
  {
    for (int n = 0; n < Nfq * Nfaces; ++n)
    {
      ops->xyzf[n + 0 * Nfq * Nfaces + e * Nfq * Nfaces * 3] =
          (dfloat_t)geo_data->xf(n, e);
      ops->xyzf[n + 1 * Nfq * Nfaces + e * Nfq * Nfaces * 3] =
          (dfloat_t)geo_data->yf(n, e);
      ops->xyzf[n + 2 * Nfq * Nfaces + e * Nfq * Nfaces * 3] =
          (dfloat_t)geo_data->zf(n, e);

      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 0 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->rxJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 1 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->ryJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 2 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->rzJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 3 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->sxJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 4 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->syJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 5 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->szJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 6 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->txJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 7 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->tyJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 8 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->tzJf(n, e);

      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 0 * Nfq * Nfaces + n] =
        (dfloat_t)geo_data->nxJ(n, e);
      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 1 * Nfq * Nfaces + n] =
        (dfloat_t)geo_data->nyJ(n, e);
      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 2 * Nfq * Nfaces + n] =
        (dfloat_t)geo_data->nzJ(n, e);
      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 3 * Nfq * Nfaces + n] =
        (dfloat_t)geo_data->sJ(n, e);
      //      if (e==99){
      //        printf("n = %d, nxJ = %f\n", n, (dfloat_t) geo_data->nxJ(n,e));
      //      }
    }
  }

  ops->Jq = to_c(geo_data->J);

  ops->mapPq = to_c(map_data->mapPq);
  ops->Fmask = to_c(map_data->fmask);

  delete ref_data;
  delete geo_data;
  delete map_data;

  return ops;
}

void host_operators_free(host_operators_t *ops)
{
  asd_free_aligned(ops->vgeo);
  asd_free_aligned(ops->fgeo);
  asd_free_aligned(ops->Jq);

  asd_free_aligned(ops->mapPq);
  asd_free_aligned(ops->Fmask);

  asd_free_aligned(ops->nrJ);
  asd_free_aligned(ops->nsJ);

  asd_free_aligned(ops->Drq);
  asd_free_aligned(ops->Dsq);

  if (ops->dim == 3)
  {
    asd_free_aligned(ops->ntJ);
    asd_free_aligned(ops->Dtq);
  }

  asd_free_aligned(ops->Vq);
  asd_free_aligned(ops->Pq);

  asd_free_aligned(ops->VqLq);
  asd_free_aligned(ops->VqPq);
  asd_free_aligned(ops->VfPq);
  asd_free_aligned(ops->Vfqf);
}


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

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2019 program.

// This file was modified by Oracle on 2021.
// Modifications copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, 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_STRATEGY_CARTESIAN_SIDE_ROBUST_HPP
#define BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_ROBUST_HPP

#include <boost/geometry/core/config.hpp>
#include <boost/geometry/strategy/cartesian/side_non_robust.hpp>

#include <boost/geometry/strategies/side.hpp>

#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/precise_math.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

struct epsilon_equals_policy
{
public:
    template <typename Policy, typename T1, typename T2>
    static bool apply(T1 const& a, T2 const& b, Policy const& policy)
    {
        return boost::geometry::math::detail::equals_by_policy(a, b, policy);
    }
};

struct fp_equals_policy
{
public:
    template <typename Policy, typename T1, typename T2>
    static bool apply(T1 const& a, T2 const& b, Policy const&)
    {
        return a == b;
    }
};

/*!
\brief Adaptive precision predicate to check at which side of a segment a point lies:
    left of segment (>0), right of segment (< 0), on segment (0).
\ingroup strategies
\tparam CalculationType \tparam_calculation (numeric_limits<ct>::epsilon() and numeric_limits<ct>::digits must be supported for calculation type ct)
\tparam Robustness std::size_t value from 0 (fastest) to 3 (default, guarantees correct results).
\details This predicate determines at which side of a segment a point lies using an algorithm that is adapted from orient2d as described in "Adaptive Precision Floating-Point Arithmetic and Fast Robust Geometric Predicates" by Jonathan Richard Shewchuk ( https://dl.acm.org/citation.cfm?doid=237218.237337 ). More information and copies of the paper can also be found at https://www.cs.cmu.edu/~quake/robust.html . It is designed to be adaptive in the sense that it should be fast for inputs that lead to correct results with plain float operations but robust for inputs that require higher precision arithmetics.
 */
template
<
    typename CalculationType = void,
    typename EqualsPolicy = epsilon_equals_policy,
    std::size_t Robustness = 3
>
struct side_robust
{

    template <typename CT>
    struct epsilon_policy
    {
        using Policy = boost::geometry::math::detail::equals_factor_policy<CT>;

        epsilon_policy() {}

        template <typename Type>
        epsilon_policy(Type const& a, Type const& b, Type const& c, Type const& d)
            : m_policy(a, b, c, d)
        {}
        Policy m_policy;

    public:

        template <typename T1, typename T2>
        bool apply(T1 a, T2 b) const
        {
            return EqualsPolicy::apply(a, b, m_policy);
        }
    };

public:

    typedef cartesian_tag cs_tag;

    //! \brief Computes the sign of the CCW triangle p1, p2, p
    template
    <
        typename PromotedType,
        typename P1,
        typename P2,
        typename P,
        typename EpsPolicyInternal,
        std::enable_if_t<std::is_fundamental<PromotedType>::value, int> = 0
    >
    static inline PromotedType side_value(P1 const& p1,
                                          P2 const& p2,
                                          P const& p,
                                          EpsPolicyInternal& eps_policy)
    {
        using vec2d = ::boost::geometry::detail::precise_math::vec2d<PromotedType>;
        vec2d pa;
        pa.x = get<0>(p1);
        pa.y = get<1>(p1);
        vec2d pb;
        pb.x = get<0>(p2);
        pb.y = get<1>(p2);
        vec2d pc;
        pc.x = get<0>(p);
        pc.y = get<1>(p);
        return ::boost::geometry::detail::precise_math::orient2d
            <PromotedType, Robustness>(pa, pb, pc, eps_policy);
    }

    template
    <
        typename PromotedType,
        typename P1,
        typename P2,
        typename P,
        typename EpsPolicyInternal,
        std::enable_if_t<!std::is_fundamental<PromotedType>::value, int> = 0
    >
    static inline auto side_value(P1 const& p1, P2 const& p2, P const& p,
                                  EpsPolicyInternal&)
    {
        return side_non_robust<>::apply(p1, p2, p);
    }

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    template
    <
        typename P1,
        typename P2,
        typename P
    >
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        using coordinate_type = typename select_calculation_type_alt
            <
                CalculationType,
                P1,
                P2,
                P
            >::type;

        using promoted_type = typename select_most_precise
            <
                coordinate_type,
                double
            >::type;

        epsilon_policy<promoted_type> epsp;
        promoted_type sv = side_value<promoted_type>(p1, p2, p, epsp);
        promoted_type const zero = promoted_type();

        return epsp.apply(sv, zero) ? 0
            : sv > zero ? 1
            : -1;
    }

#endif

};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_ROBUST_HPP


//---------------------------------------------------------------------------//
//!
//! \file   Utility_AtomUnit.hpp
//! \author Alex Robinson
//! \brief  The atom unit declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_ATOM_UNIT_HPP
#define UTILITY_ATOM_UNIT_HPP

// Boost Includes
#include <boost/units/systems/si/amount.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/conversion.hpp>
#include <boost/units/io.hpp>

// FRENSIE Includes
#include "Utility_RawPhysicalConstants.hpp"

namespace Utility{

namespace Units{

//! The atom base unit
struct AtomBaseUnit : public boost::units::base_unit<AtomBaseUnit, boost::units::amount_dimension,2>
{
  static const char* name() { return "atom"; }
  static const char* symbol() { return "atom"; }
};

//! The atom unit
typedef AtomBaseUnit::unit_type Atom;

BOOST_UNITS_STATIC_CONSTANT( atom, Atom );

} // end Units namespace

} // end Utility namespace

BOOST_UNITS_DEFINE_CONVERSION_FACTOR( Utility::Units::AtomBaseUnit, boost::units::si::amount, double, 1.0/Utility::RawPhysicalConstants::avogadro_constant );
BOOST_UNITS_DEFINE_CONVERSION_FACTOR( boost::units::si::amount, Utility::Units::AtomBaseUnit, double, Utility::RawPhysicalConstants::avogadro_constant );

BOOST_UNITS_DEFAULT_CONVERSION( Utility::Units::AtomBaseUnit, boost::units::si::amount );

#endif // end UTILITY_ATOM_UNIT_HPP

//---------------------------------------------------------------------------//
// end Utility_AtomUnit.hpp
//---------------------------------------------------------------------------//


// Copyright 2015 The ALMA Project Developers
//
// 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.

/// @file
/// Writes the distribution in files.

#include <iostream>
#include <fstream>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/mpi.hpp>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cmakevars.hpp>
#include <constants.hpp>
#include <utilities.hpp>
#include <structures.hpp>
#include <bulk_hdf5.hpp>
#include <isotopic_scattering.hpp>
#include <io_utils.hpp>
#include <msgpack.hpp>
#include <geometry_2d.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>

///Some alias
using gridData = std::unordered_map<std::string,
        std::unique_ptr<alma::Gamma_grid>>;
using cellData = std::unordered_map<std::string,
        std::unique_ptr<alma::Crystal_structure>>;

/// Probability density function for a Gamma distribution.
///
/// @param[in] k - shape parameter
/// @param[in] theta - scale parameter
/// @param[in] x - point at which to evaluate the function
/// @return the value of the pdf
inline double gamma_pdf(double k, double theta, double x) {
    return boost::math::gamma_p_derivative(k, x / theta) / theta;
}
///To store geometric and input data:
///Parameters of input file
struct input_parameters {

    ///Geometry
    std::vector<alma::geometry_2d>
        system;
    ///Material data
    cellData system_cell;
    gridData system_grid;

    ///Vector of thickness
    std::vector<double>
        thicknesses;
    ///Where to print data
    std::vector<std::size_t> boxesID;
    std::vector<double>      dumptimes;
    ///nbins
    std::size_t nbins;
};

///To read input file:
///This reads the input file
///@param[in] filename - input filename with path
///@param[in] world    - mpi communicator
///@return structure with input parameters
input_parameters
process_input(std::string& filename,
              boost::mpi::communicator& world
){
    // Create empty property tree object
    boost::property_tree::ptree tree;

    // Parse XML input file into the tree
    boost::property_tree::read_xml(filename, tree);

    input_parameters inpars;
    ///Map to store thickness
    std::map<std::string,double> zmap;

    for (const auto& v : tree.get_child("beRTAMC2D")) {
        ///Reading geometry
        if (v.first == "geometry") {
            std::string gfname =
                alma::parseXMLfield<std::string>(v, "file");
            inpars.system =
                alma::read_geometry_XML(gfname);
        }
        if (v.first == "material") {
            std::string name =
                alma::parseXMLfield<std::string>(v, "name");
            std::string hdf5file =
                alma::parseXMLfield<std::string>(v, "database");
            double zsize =
                alma::parseXMLfield<double>(v, "thickness");
            auto data =
                alma::load_bulk_hdf5(hdf5file.c_str(), world);
            inpars.system_cell[name] =
                std::move(std::get<1>(data));
            inpars.system_grid[name] =
                std::move(std::get<3>(data));

            zmap[name] = zsize;
        }

        if (v.first == "spectral") {
            for (auto it = v.second.begin(); it != v.second.end(); it++) {
                if (it->first == "resolution") {
                    inpars.nbins =
                        alma::parseXMLfield<std::size_t>(*it, "ticks");
                }
                if (it->first == "location") {
                    inpars.boxesID.push_back(
                        alma::parseXMLfield<std::size_t>(*it, "bin"));
                }
                if (it->first == "time") {
                    inpars.dumptimes.push_back(
                        alma::parseXMLfield<double>(*it, "t"));
                }
            }
        }

    }

    if (inpars.system_cell.size()>1){
        std::cout << "This is currently not supported\n";
        world.abort(1);
    }

    /// Fill thicknesses
    for (std::size_t i=0;i<inpars.system.size();i++) {
        auto mat = inpars.system[i].material;
        inpars.thicknesses.push_back(
            zmap[mat]);
    }

    /// Order the times
    std::sort(inpars.dumptimes.begin(),inpars.dumptimes.end());

    return inpars;
}

class data {
public:
    double time;
    double Eeff;
    std::vector<double> temperatures;
    std::vector<Eigen::VectorXi> histograms;
    std::vector<std::size_t> nbands;
    std::vector<std::ofstream> outf;
    std::vector<std::ofstream> outfvx;
    std::vector<std::ofstream> outfvy;
    std::vector<std::ofstream> outfd;

    data(input_parameters& inpars){
        temperatures.resize(inpars.system.size());
        histograms.resize(temperatures.size());
        nbands.resize(temperatures.size(),0);
        outf.resize(nbands.size());
        outfvx.resize(nbands.size());
        outfvy.resize(nbands.size());
        outfd.resize(nbands.size());

        for (auto s : inpars.system) {

            auto id = s.get_id();

            if (std::find(inpars.boxesID.begin(),
                inpars.boxesID.end(),id)==inpars.boxesID.end())
                continue;

            outf[id].open("deltaT_omega_"+std::to_string(id)+".csv");
            outfvx[id].open("jx_omega_"+std::to_string(id)+".csv");
            outfvy[id].open("jy_omega_"+std::to_string(id)+".csv");
            outfd[id].open("fd_"+std::to_string(id)+".csv");

            auto mat = inpars.system[id].material;

            auto nqpoints = inpars.system_grid[mat]->nqpoints;

            auto nbands_   = inpars.system_grid[mat
                ]->get_spectrum_at_q(0).omega.size();

            histograms[id].resize(nbands_*nqpoints);
            histograms[id].setZero();
            nbands[id] = nbands_;
        }
    }

    ~data(){
        for (auto& o : outf) {
            if(o.is_open())
                o.close();
        }
        for (auto& o : outfvx) {
            if(o.is_open())
                o.close();
        }
        for (auto& o : outfvy) {
            if(o.is_open())
                o.close();
        }
        for (auto& o : outfd) {
            if(o.is_open())
                o.close();
        }

    }

  // this function is looks like de-serializer, taking an msgpack object
  // and extracting data from it to the current class fields
  void msgpack_unpack(msgpack::object o) {

    // check if received structure is an array
    if(o.type != msgpack::type::ARRAY) { throw msgpack::type_error(); }

    o.via.array.ptr[0].convert(time);
    o.via.array.ptr[1].convert(Eeff);

    std::cout << "time " << time << std::endl;
    std::cout << "Eeff " << Eeff << std::endl;

    // extract value of second array entry which is array itself:
    for (std::size_t i = 0; i < temperatures.size() ; i++) {
      o.via.array.ptr[2].via.array.ptr[i].convert((temperatures[i]));
    }
    for (std::size_t i = 0; i < temperatures.size(); i++) {
        if (nbands[i] == 0)
            continue;
        std::size_t nelements;
        o.via.array.ptr[3+2*i].convert(nelements);

        std::cout << "nelements " << nelements << std::endl;

        histograms[i].setZero();

        for (std::size_t j = 0; j < nelements/2; j++) {
            std::size_t imode;
            o.via.array.ptr[4+2*i].via.array.ptr[2*j].convert(imode);
            o.via.array.ptr[4+2*i].via.array.ptr[2*j+1].convert((histograms[i](imode)));
        }
    }

  }

  // destination of this function is unknown - i've never ran into scenary
  // what it was called. some explaination/documentation needed.
  template <typename MSGPACK_OBJECT>
  void msgpack_object(MSGPACK_OBJECT* o, msgpack::zone* z) const {

  }

  /// This prints the distribution
  ///@param[in] inpar - input parameters
  double print_distribution(input_parameters& inpars){
      static std::map<std::string,Eigen::ArrayXXd> sigmas;  //Broadening
      static std::map<std::string,Eigen::ArrayXXd> vx;
      static std::map<std::string,Eigen::ArrayXXd> vy;
      static std::map<std::string,Eigen::MatrixXd> qpoints1stBZ; //Q points in the first BZ
      static std::map<std::string,double> Cvtot;      //specific heat of each material [J/(nm**3 * K)]
      static std::map<std::string,Eigen::VectorXd> mesh;
      static std::vector<bool> first(temperatures.size(),true);

      /// If not in list ignore
      if (std::find_if(inpars.dumptimes.begin(),inpars.dumptimes.end(),[&](double &t_){
          return alma::almost_equal(time,t_);
      })==inpars.dumptimes.end()){
          return -1.0;
      }

      /// Filling tables

      if (sigmas.empty()) {

        for (std::size_t ibox = 0; ibox < temperatures.size(); ibox++) {
            if (nbands[ibox]==0) {
                continue;
            }
            //std::cout << "Printing " << ibox << std::endl;
            auto mat = inpars.system[ibox].material;
            if (sigmas.count(mat) !=0)
                    continue;
            auto tbox = temperatures[ibox];
            sigmas[mat].resize(nbands[ibox],inpars.system_grid[mat]->nqpoints);
            vx[mat].resize(nbands[ibox],inpars.system_grid[mat]->nqpoints);
            vy[mat].resize(nbands[ibox],inpars.system_grid[mat]->nqpoints);
            qpoints1stBZ[mat].resize(inpars.system_grid[mat]->nqpoints,2);
            sigmas[mat].setZero();
            vx[mat].setZero();
            vy[mat].setZero();
            qpoints1stBZ[mat].setZero();
            double maxf = 0.;

            double prefactor =
                alma::constants::kB / (inpars.system_cell[mat]->V * inpars.thicknesses[ibox]
                /inpars.system_cell[mat]->lattvec(2,2)) / inpars.system_grid[mat]->nqpoints;

            for (std::size_t iq = 0; iq < inpars.system_grid[mat]->nqpoints; ++iq) {
                    auto spectrum = inpars.system_grid[mat]->get_spectrum_at_q(iq);

                    auto q1BZ = inpars.system_cell[mat]->map_to_firstbz(
                        inpars.system_grid[mat]->get_q(iq));

                    qpoints1stBZ[mat](iq,0) = q1BZ.row(0).mean();
                    qpoints1stBZ[mat](iq,1) = q1BZ.row(1).mean();

                    auto maxomega_mode =
                        spectrum.omega.maxCoeff();
                    if (maxomega_mode > maxf)
                        maxf = maxomega_mode;

                    for (std::size_t im = 0; im < nbands[ibox]; ++im) {
                        auto omega_ = spectrum.omega[im];

                        if (alma::almost_equal(omega_,0.))
                            continue;

                        sigmas[mat](im, iq) = inpars.system_grid[
                            mat]->base_sigma(spectrum.vg.col(im));
                        ///Get group velocities in m/s
                        vx[mat](im, iq) = 1.0e+3 * spectrum.vg(0,im);
                        vy[mat](im, iq) = 1.0e+3 * spectrum.vg(1,im);
                    }
            }
            // And refine them by removing outliers.
            auto percent = alma::calc_percentiles_log(sigmas[mat]);
            double lbound = std::exp(percent[0] - 1.5 * (percent[1] - percent[0]));
            sigmas[mat] = (sigmas[mat] < lbound).select(lbound, sigmas[mat]);

            mesh[mat] = Eigen::VectorXd::LinSpaced(inpars.nbins,0.,maxf);
            mesh[mat] = (mesh[mat].array() + mesh[mat](1)/2.0).matrix();

            /// Filling volumetric heat capacity
            Cvtot[mat] = 0.;

            for (std::size_t iq = 0; iq < inpars.system_grid[mat]->nqpoints; ++iq) {
                auto spectrum = inpars.system_grid[mat]->get_spectrum_at_q(iq);
                for (std::size_t im = 0; im < nbands[ibox]; ++im) {
                    auto omega_ = spectrum.omega[im];

                    if (alma::almost_equal(0.,omega_))
                        continue;

                    Cvtot[mat] += alma::bose_einstein_kernel(omega_, tbox);
                }
            }
            Cvtot[mat] *= prefactor;
        }
      }

      for (std::size_t ibox = 0; ibox < temperatures.size(); ibox++) {

          ///If not in print list ignore
          if (std::find(inpars.boxesID.begin(),
              inpars.boxesID.end(),ibox)==inpars.boxesID.end())
              continue;

          auto mat = inpars.system[ibox].material;
          if (inpars.system[ibox].reservoir
              or inpars.system[ibox].reservoir)
            continue;

          Eigen::VectorXd d(mesh[mat]), dvx(mesh[mat]), dvy(mesh[mat]);
          Eigen::VectorXd dfd(inpars.system_grid[mat]->nqpoints);

          d.setZero();
          dvx.setZero();
          dvy.setZero();
          dfd.setZero();

          auto vol  = inpars.system[ibox].get_area()*inpars.thicknesses[ibox];
          double Ebox = 0.;

          for (std::size_t imode = 0; imode < static_cast<std::size_t>(
                histograms[ibox].rows()); imode++){
              auto value = histograms[ibox](imode);
              if (value == 0) {
                  continue;
              }
              auto ib = imode % nbands[ibox];
              auto iq = imode / nbands[ibox];
              auto omega = inpars.system_grid[mat]->get_spectrum_at_q(iq).omega(ib);
              dfd(iq) += value/omega;
              auto sigma = sigmas[mat](ib,iq);
              Eigen::VectorXd v = inpars.system_grid[mat]->get_spectrum_at_q(iq).vg.col(ib).matrix();
              Ebox  += value;
              ///If gamma they do not contribute
              if (alma::almost_equal(omega,0) or alma::almost_equal(sigma,0))
                  continue;
              for (int ii = 0; ii < d.rows(); ii++ ) {
                  double k = omega * omega / sigma;
                  double theta = sigma / omega;
                  double valati = value*gamma_pdf(k,theta,mesh[mat](ii));
                  d(ii)   += valati;
                  dvx(ii) += valati * 1.0e+3 * v(0);
                  dvy(ii) += valati * 1.0e+3 * v(1);
              }
          }

          ///Recover the distribution function
          dfd   *=   Eeff*(inpars.system_cell[mat]->V * inpars.thicknesses[ibox]
                 / inpars.system_cell[mat]->lattvec(2,2)) *
                inpars.system_grid[mat]->nqpoints / (1.0e+12*vol*alma::constants::hbar);
          dvx      *= Eeff * 1.0e+27/vol  ;
          dvy      *= Eeff * 1.0e+27/vol  ;
          d        *= Eeff/(vol*Cvtot[mat]);
          Ebox     *= Eeff/vol;

          std::cout <<"Energy density of box " << ibox << " is " << Ebox << std::endl;
          std::cout <<"Specific heat " << Cvtot[mat] * 1.0e+27 << " J/(m**3 * K)" << std::endl;

          auto &myo   = outf[ibox];
          auto &myox  = outfvx[ibox];
          auto &myoy  = outfvy[ibox];
          auto &myofd = outfd[ibox];

          if (first[ibox]) {
              myo << -1 << ',';
              myox << -1 << ',';
              myoy << -1 << ',';
              for (int ii = 0; ii < d.rows(); ii++ ) {
                  myo << mesh[mat](ii);
                  myox << mesh[mat](ii);
                  myoy << mesh[mat](ii);
                  if (ii != d.rows()-1) {
                    myo  << ',';
                    myox << ',';
                    myoy << ',';
                  }
              }
              myo << std::endl;
              myox << std::endl;
              myoy << std::endl;

              ///Print coordinates

              myofd << "# ";
              for (int ii = 0; ii < dfd.rows();ii++) {
                 myofd << qpoints1stBZ[mat](ii,0);
                 if (ii != dfd.rows()-1) {
		    myofd << ",";
                 }
              }
              myofd << std::endl;
              myofd << "# ";
              for (int ii = 0; ii < dfd.rows();ii++) {
                  myofd << qpoints1stBZ[mat](ii,1);
                  if (ii != dfd.rows()-1) {
                    myofd << ",";
                 }
              }
              myofd << std::endl;

              first[ibox] = false;
          }

          myo  << time << ',';
          myox << time << ',';
          myoy << time << ',';
          myofd << time << ',';
          for (int ii = 0; ii < d.rows(); ii++ ) {
            myo  << d(ii);
            myox << dvx(ii);
            myoy << dvy(ii);

            if (ii != d.rows()-1) {
                myo  << ',';
                myox << ',';
                myoy << ',';
            }
          }

          for (int ii = 0; ii < dfd.rows();ii++) {
            myofd << dfd(ii);
            if (ii != dfd.rows()-1) {
                myofd << ',';
            }
          }

          myo   << std::endl;
          myox  << std::endl;
          myoy  << std::endl;
          myofd << std::endl;

      }

    return time;
  }

};

int main(int argc, char** argv) {
    boost::mpi::environment env;
    boost::mpi::communicator world;

    if (world.size()!=1) {
        std::cout << "Error: dist_reader cannot be "
                     "run in more than one MPI-process"
                    << std::endl;
        world.abort(1);
    }

    // Reference temperature at which to compute
    // heat capacities and scattering rates.

    // Path to the HDF5 file.
    std::string h5filename;
    std::string inputfilename;
    std::string msgpackfilename;
    std::string reader;

    if (argc != 3) {
        std::cout << "USAGE: dist_reader <input.xml> <histogram.msgpack.bin> "
                  << std::endl;
        world.abort(1);
    }

    inputfilename = std::string(argv[1]);
    msgpackfilename = std::string(argv[2]);

    std::cout << "***********************************" << std::endl;
    std::cout << "This is ALMA/dist_reader version " << ALMA_VERSION_MAJOR << "."
              << ALMA_VERSION_MINOR << std::endl;
    std::cout << "***********************************" << std::endl;

    ///Reading geometry and other things
    std::cout << "Reading " << inputfilename << std::endl;

    auto inpars = process_input(inputfilename,world);

    ///Processing:
    std::cout << "Init data storing" << std::endl;
    data simdata(inpars);

    std::ifstream histdat;

    boost::filesystem::path msgpack_path{msgpackfilename};

    if (!(boost::filesystem::exists(msgpack_path))) {
        std::cout << "ERROR:" << std::endl;
        std::cout << "msgpack file " << msgpack_path << " does not exist."
                  << std::endl;
        world.abort(1);
    }

    histdat.open(msgpackfilename.c_str());

    std::string sizeline;
    ///Each
    std::size_t istep = 0;
    while(true){
        std::getline(histdat,sizeline,'#');
        auto length_block = boost::lexical_cast<std::size_t>(sizeline);
        if (length_block == 0)
            break;

        std::vector<char> dataline(length_block);
        histdat.read(dataline.data(),length_block);
        std::cout << "*Read line " << istep << std::endl;
        msgpack::object_handle oh =
            msgpack::unpack(dataline.data(), dataline.size());
        simdata.msgpack_unpack(oh.get());
        double that_time = simdata.print_distribution(inpars);

        /// Stop if last time has been read
        if (alma::almost_equal(that_time,inpars.dumptimes.back())){
            break;
        }

        istep++;
    }

    histdat.close();

    return 0;
}


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <orea/engine/parametricvar.hpp>

#include <orea/engine/riskfilter.hpp>
#include <orea/scenario/shiftscenariogenerator.hpp>
#include <ored/utilities/csvfilereader.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>

#include <qle/math/deltagammavar.hpp>

#include <ql/math/matrixutilities/pseudosqrt.hpp>
#include <ql/math/matrixutilities/symmetricschurdecomposition.hpp>

#include <boost/regex.hpp>

using namespace QuantLib;

namespace ore {
namespace analytics {

ParametricVarCalculator::ParametricVarCalculator(
    const std::map<std::string, std::set<string>>& tradePortfolios, const std::string& portfolioFilter,
    const boost::shared_ptr<SensitivityStream>& sensitivities,
    const std::map<std::pair<RiskFactorKey, RiskFactorKey>, Real> covariance, const std::vector<Real>& p,
    const std::string& method, const Size mcSamples, const Size mcSeed, const bool breakdown,
    const bool salvageCovarianceMatrix)
    : tradePortfolios_(tradePortfolios), portfolioFilter_(portfolioFilter), sensitivities_(sensitivities),
      covariance_(covariance), p_(p), method_(method), mcSamples_(mcSamples), mcSeed_(mcSeed), breakdown_(breakdown),
      salvageCovarianceMatrix_(salvageCovarianceMatrix) {}

void ParametricVarCalculator::calculate(ore::data::Report& report) {
    LOG("Parametric VaR calculation started...");

    // prepare report
    report.addColumn("Portfolio", string()).addColumn("RiskClass", string()).addColumn("RiskType", string());
    for (Size i = 0; i < p_.size(); ++i)
        report.addColumn("Quantile_" + std::to_string(p_[i]), double(), 6);

    // build portfolio filter, if given
    bool hasFilter = false;
    boost::regex filter;
    if (portfolioFilter_ != "") {
        hasFilter = true;
        filter = boost::regex(portfolioFilter_);
        LOG("Portfolio filter: " << portfolioFilter_);
    } else {
        LOG("No portfolio filter will be applied.");
    }

    // read sensitivities and preaggregate them per prtfolio
    LOG("Preaggregate sensitivities per portfolio");
    std::set<RiskFactorKey> sensiKeysTmp;
    std::set<std::string> portfoliosTmp;
    std::map<std::string, std::map<std::pair<RiskFactorKey, RiskFactorKey>, Real>> value1, value2;
    std::map<std::pair<RiskFactorKey, RiskFactorKey>, Real> value1All, value2All;
    while (SensitivityRecord sr = sensitivities_->next()) {
        std::set<std::string> portfolios;
        auto pn = tradePortfolios_.find(sr.tradeId);
        if (pn != tradePortfolios_.end()) {
            if (pn->second.empty())
                portfolios = {"(empty)"};
            else
                portfolios = pn->second;
        } else
            portfolios = {"(unknown)"};
        RiskFactorKey k1 = sr.key_1;
        RiskFactorKey k2 = sr.key_2;
        auto key = std::make_pair(k1, k2);
        if (k1 != RiskFactorKey())
            sensiKeysTmp.insert(k1);
        if (k2 != RiskFactorKey())
            sensiKeysTmp.insert(k2);
        bool relevant = false;
        for (auto const& p : portfolios) {
            if (!hasFilter || boost::regex_match(p, filter)) {
                relevant = true;
                portfoliosTmp.insert(p);
                if (sr.isCrossGamma()) {
                    value1[p][key] += sr.gamma;
                } else {
                    value1[p][key] += sr.delta;
                }
                if (!sr.isCrossGamma()) {
                    value2[p][key] += sr.gamma;
                }
            }
        }
        if (relevant) {
            if (sr.isCrossGamma()) {
                value1All[key] += sr.gamma;
            } else {
                value1All[key] += sr.delta;
            }
            if (!sr.isCrossGamma()) {
                value2All[key] += sr.gamma;
            }
        }
    }
    std::vector<RiskFactorKey> sensiKeys(sensiKeysTmp.begin(), sensiKeysTmp.end());
    std::vector<bool> sensiKeyHasNonZeroVariance(sensiKeys.size(), false);
    std::vector<std::string> portfolios(portfoliosTmp.begin(), portfoliosTmp.end());
    LOG("Have " << sensiKeys.size() << " sensitivity keys in " << portfolios.size() << " portfolios");

    // build global covariance matrix
    Matrix omega(sensiKeys.size(), sensiKeys.size(), 0.0);
    Size unusedCovariance = 0;
    for (const auto& c : covariance_) {
        auto k1 = std::find(sensiKeys.begin(), sensiKeys.end(), c.first.first);
        auto k2 = std::find(sensiKeys.begin(), sensiKeys.end(), c.first.second);
        if (k1 != sensiKeys.end() && k2 != sensiKeys.end()) {
            omega(k1 - sensiKeys.begin(), k2 - sensiKeys.begin()) = c.second;
            if (k1 == k2)
                sensiKeyHasNonZeroVariance[k1 - sensiKeys.begin()] = true;
        } else {
            ++unusedCovariance;
        }
    }
    LOG("Found " << covariance_.size() << " covariance matrix entries, " << unusedCovariance
                 << " do not match a portfolio sensitivity and will not be used.");
    for (Size i = 0; i < sensiKeyHasNonZeroVariance.size(); ++i) {
        if (!sensiKeyHasNonZeroVariance[i]) {
            WLOG("Zero variance assigned to sensitivity key " << sensiKeys[i]);
        }
    }

    // make covariance matrix positive semi-definite
    boost::shared_ptr<QuantExt::CovarianceSalvage> covarianceSalvage;
    LOG("Covariance matrix has dimension " << sensiKeys.size() << " x " << sensiKeys.size());
    if (salvageCovarianceMatrix_) {
        LOG("Make covariance matrix positive semi-definite using spectral method");
        covarianceSalvage = boost::make_shared<QuantExt::SpectralCovarianceSalvage>();
    } else {
        LOG("Covariance matrix is no salvaged, check for positive semi-definiteness");
        SymmetricSchurDecomposition ssd(omega);
        Real evMin = ssd.eigenvalues().back();
        QL_REQUIRE(evMin > 0.0 || close_enough(evMin, 0.0),
                   "ParametricVar: input covariance matrix is not positive semi-definite, smallest eigenvalue is "
                       << evMin);
        LOG("Smallest eigenvalue is " << evMin);
        covarianceSalvage = boost::make_shared<QuantExt::NoCovarianceSalvage>();
    }
    LOG("Done.");

    // loop over portfolios (index 0 = all portfolios)
    for (Size i = 0; i <= (!breakdown_ || portfolios.size() == 1 ? 0 : portfolios.size()); ++i) {
        std::string portfolioName = i == 0 ? (portfolios.size() > 1 ? "(all)" : portfolios.front()) : portfolios[i - 1];
        // build delta and gamma for given portfolio
        const auto& val1 = (i == 0 ? value1All : value1[portfolios[i - 1]]);
        const auto& val2 = (i == 0 ? value2All : value2[portfolios[i - 1]]);
        Array delta(sensiKeys.size(), 0.0);
        Matrix gamma(sensiKeys.size(), sensiKeys.size(), 0.0);
        for (auto const& p : val1) {
            auto k1 = p.first.first;
            auto k2 = p.first.second;
            Size idx1 = std::find(sensiKeys.begin(), sensiKeys.end(), k1) - sensiKeys.begin();
            QL_REQUIRE(idx1 < sensiKeys.size(), "ParametricVarCalculator::computeVar: key1 \""
                                                    << k1 << "\" in value1 not found, this is unexpected.");
            if (k2 == RiskFactorKey()) {
                // delta
                delta[idx1] += p.second;
            } else {
                // cross gamma
                Size idx2 = std::find(sensiKeys.begin(), sensiKeys.end(), k2) - sensiKeys.begin();
                QL_REQUIRE(idx2 < sensiKeys.size(), "ParametricVarCalculator::computeVar: key2 \""
                                                        << k2 << "\" in value1 not found, this is unexpected.");
                gamma[idx1][idx2] = gamma[idx2][idx1] = p.second;
            }
        }
        for (auto const& p : val2) {
            // diagonal gamma
            auto k1 = p.first.first;
            Size idx1 = std::find(sensiKeys.begin(), sensiKeys.end(), k1) - sensiKeys.begin();
            QL_REQUIRE(idx1 < sensiKeys.size(), "ParametricVarCalculator::computeVar: key1 \""
                                                    << k1 << "\" in value2 not found, this is unexpected.");
            gamma[idx1][idx1] = p.second;
        }
        // loop over risk class and type filters (index 0 == all risk types)
        for (Size j = 0; j < (breakdown_ ? RiskFilter::numberOfRiskClasses() : 1); ++j) {
            for (Size k = 0; k < (breakdown_ ? RiskFilter::numberOfRiskTypes() : 1); ++k) {
                // TODO should we rather project on the set of indices with non-zero sensis
                // instead of copying the initial sensis and setting the non-releveant entries
                // to zero?
                Array deltaFiltered(delta);
                Matrix gammaFiltered(gamma);
                RiskFilter rf(j, k);
                LOG("Compute parametric var for portfolio \"" << portfolioName << "\""
                                                              << ", risk class " << rf.riskClassLabel()
                                                              << ", risk type " << rf.riskTypeLabel());
                // set sensis which do not belong to risk type filter to zero
                for (Size idx = 0; idx < sensiKeys.size(); ++idx) {
                    if (!rf.allowed(sensiKeys[idx].keytype)) {
                        deltaFiltered[idx] = 0.0;
                        for (Size ii = 0; ii < sensiKeys.size(); ++ii) {
                            gammaFiltered[idx][ii] = gammaFiltered[ii][idx] = 0.0;
                        }
                    }
                }
                // are all sensis zero, then skip the computation
                bool zeroSensis = close_enough(QuantExt::detail::absMax(deltaFiltered), 0.0) &&
                                  close_enough(QuantExt::detail::absMax(gammaFiltered), 0.0);
                // compute var and write to report
                std::vector<Real> var = zeroSensis
                                            ? std::vector<Real>(p_.size(), 0.0)
                                            : computeVar(omega, deltaFiltered, gammaFiltered, p_, *covarianceSalvage);
                if (!close_enough(QuantExt::detail::absMax(var), 0.0)) {
                    report.next();
                    report.add(portfolioName);
                    report.add(rf.riskClassLabel());
                    report.add(rf.riskTypeLabel());
                    for (auto const& v : var)
                        report.add(v);
                }
            } // for k (risk types)
        }     // for j (risk classes)
    }         // for i (portfolios)
    LOG("parametric var computation done.");
    report.end();

} // calculate

std::vector<Real> ParametricVarCalculator::computeVar(const Matrix& omega, const Array& delta, const Matrix& gamma,
                                                      const std::vector<Real>& p,
                                                      const QuantExt::CovarianceSalvage& covarianceSalvage) {
    if (method_ == "Delta") {
        std::vector<Real> res(p.size());
        for (Size i = 0; i < p.size(); ++i) {
            res[i] = QuantExt::deltaVar(omega, delta, p[i], covarianceSalvage);
        }
        return res;
    } else if (method_ == "DeltaGammaNormal") {
        std::vector<Real> res(p.size());
        for (Size i = 0; i < p.size(); ++i) {
            res[i] = QuantExt::deltaGammaVarNormal(omega, delta, gamma, p[i], covarianceSalvage);
        }
        return res;
    } else if (method_ == "MonteCarlo") {
        QL_REQUIRE(mcSamples_ != Null<Size>(),
                   "ParametricVarCalculator::computeVar(): method MonteCarlo requires mcSamples");
        QL_REQUIRE(mcSeed_ != Null<Size>(),
                   "ParametricVarCalculator::computeVar(): method MonteCarlo requires mcSamples");
        return QuantExt::deltaGammaVarMc<PseudoRandom>(omega, delta, gamma, p, mcSamples_, mcSeed_, covarianceSalvage);
    } else {
        QL_FAIL("ParametricVarCalculator::computeVar(): method " << method_ << " not known.");
    }
}

void loadCovarianceDataFromCsv(std::map<std::pair<RiskFactorKey, RiskFactorKey>, Real>& data,
                               const std::string& fileName, const char delim) {
    LOG("Load Covariance Data from file " << fileName);
    ore::data::CSVFileReader reader(fileName, false);
    std::vector<std::string> dummy;
    while (reader.next()) {
        data[std::make_pair(*parseRiskFactorKey(reader.get(0), dummy), *parseRiskFactorKey(reader.get(1), dummy))] =
            ore::data::parseReal(reader.get(2));
    }
    LOG("Read " << data.size() << " valid data lines from file " << fileName);
}

} // namespace analytics
} // namespace ore