#include "initial_state.h"

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

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Alpha_shape_3.h>
#include <CGAL/Alpha_shape_cell_base_3.h>
#include <CGAL/Alpha_shape_vertex_base_3.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <cassert>
#include <list>

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

#define PI 3.14159265

using namespace std;

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

typedef CGAL::Exact_predicates_inexact_constructions_kernel Gt;
typedef CGAL::Alpha_shape_vertex_base_3<Gt>          		Vb;
typedef CGAL::Alpha_shape_cell_base_3<Gt>            		Fb;
typedef CGAL::Triangulation_data_structure_3<Vb,Fb>  		Tds;
typedef CGAL::Delaunay_triangulation_3<Gt,Tds>       		Triangulation_3;
typedef CGAL::Alpha_shape_3<Triangulation_3>         		Alpha_shape_3;
typedef Gt::Point_3                                  		Point;
typedef Alpha_shape_3::Alpha_iterator                		Alpha_iterator;

InitState::InitState() {
	// empty
}

void InitState::SetTethered(bool TetheredCheck) {
	tethered = TetheredCheck;
}

void InitState::SetMstrk(ModelClass &mdl) {
	double beta = mdl.srf_angle;

	M_strk.eye(4,4);
	M_strk(0,0) = cos(beta);
	M_strk(0,2) = sin(beta);
	M_strk(2,0) = -sin(beta);
	M_strk(2,2) = cos(beta);

	M_90.eye(4,4);
	M_90(0,0) = 0.0;
	M_90(0,2) = -1.0;
	M_90(2,0) = 1.0;
	M_90(2,2) = 0.0;

}

void InitState::SetBounds(ModelClass &mdl) {
	phi_bound = mdl.stroke_bound;
	theta_bound = mdl.deviation_bound;
	eta_bound = mdl.wing_pitch_bound;
}

void InitState::SetMinimumNumberOfPoints(int NrMinPoints) {
	min_nr_points = NrMinPoints;
}

void InitState::SetMinimumConnectionPoints(int MinConnPoints) {
	min_connect_points = MinConnPoints;
}

void InitState::SetFixedBodyLocs(vector<arma::Col<double>> FixedLocs, double WingLengthL, double WingLengthR) {
	origin_loc.set_size(3);
	neck_loc.set_size(3);
	joint_L_loc.set_size(3);
	joint_R_loc.set_size(3);
	wingtip_L_loc.set_size(3);
	wingtip_R_loc.set_size(3);
	origin_loc(0) = FixedLocs[0](0);
	origin_loc(1) = FixedLocs[0](1);
	origin_loc(2) = FixedLocs[0](2);
	neck_loc(0) = FixedLocs[1](0);
	neck_loc(1) = FixedLocs[1](1);
	neck_loc(2) = FixedLocs[1](2);
	joint_L_loc(0) = FixedLocs[2](0);
	joint_L_loc(1) = FixedLocs[2](1);
	joint_L_loc(2) = FixedLocs[2](2);
	joint_R_loc(0) = FixedLocs[3](0);
	joint_R_loc(1) = FixedLocs[3](1);
	joint_R_loc(2) = FixedLocs[3](2);
	InitState::CalculateFixedBodyRefFrame();
	wing_length_L = WingLengthL;
	wing_length_R = WingLengthR;
}

void InitState::ProjectFrame2PCL(FocalGrid &fg, frame_data &frame_in) {
	frame_in.seg_pcl.clear();
	vector<tuple<double,double,double,int>> pcl_out = fg.ProjectFrames2Cloud(frame_in.seg_frame);
	frame_in.seg_pcl = fg.ConvertVector2Mat(pcl_out);
}

void InitState::SetSphereRadius(double SphereRadius) {
	sphere_radius = SphereRadius*wing_length_L;
}

void InitState::FindInitialState(frame_data &frame_in, FocalGrid &fg, ModelClass &mdl) {
	bool body_bbox_found = InitState::FindBodyBoundingBox(frame_in);
	bool wingtip_bbox_found = InitState::FindWingBoundingBoxes(frame_in, fg);
	if (body_bbox_found && wingtip_bbox_found) {
		InitState::SetInitState(mdl,frame_in);
	}
}

void InitState::SetInitState(ModelClass &mdl, frame_data &frame_in) {
	frame_in.init_state.reset();
	frame_in.init_state.zeros(7,3);
	frame_in.body_and_wing_pcls.reset();
	frame_in.body_and_wing_pcls.zeros(4,1);
	if (arma::trace(M_body_fixed)!=0.0) {
		frame_in.init_state.col(0) = mdl.GetStateFromM(M_body);
		frame_in.body_and_wing_pcls.insert_cols(frame_in.body_and_wing_pcls.n_cols,body_pcl);
	}
	if (arma::trace(M_wing_L)!=0.0) {
		frame_in.init_state.col(1) = mdl.GetStateFromM(M_wing_L);
		frame_in.body_and_wing_pcls.insert_cols(frame_in.body_and_wing_pcls.n_cols,wingtip_pcl_L);
	}
	if (arma::trace(M_wing_R)!=0.0) {
		frame_in.init_state.col(2) = mdl.GetStateFromM(M_wing_R);
		frame_in.body_and_wing_pcls.insert_cols(frame_in.body_and_wing_pcls.n_cols,wingtip_pcl_R);
	}
}

bool InitState::FindBodyBoundingBox(frame_data &frame_in) {
	bool success = true;
	body_pcl.reset();
	M_body.reset();
	try {
		arma::Row<double> seg_ids = arma::unique(frame_in.seg_pcl.row(3));
		int N_seg = seg_ids.n_cols;
		double body_id = seg_ids.min();
		arma::uvec body_pt_ids = arma::find(frame_in.seg_pcl.row(3) == body_id);
		body_pcl = frame_in.seg_pcl.cols(body_pt_ids);
		body_pcl.row(3).fill(1.0);
		bbox_body = InitState::BoundingBox(body_pcl);
		M_body.eye(4,4);
		M_body.submat(0,0,2,2) = M_body_fixed.submat(0,0,2,2);
		M_body(0,3) = M_body_fixed(0,3);
		M_body(1,3) = M_body_fixed(1,3);
		M_body(2,3) = M_body_fixed(2,3);
		//InitState::Alpha3D(body_pcl);
	}
	catch (...) {
		success = false;
		M_body.eye(4,4);
		body_pcl.zeros(4,1);
	}
	return success;
}

bool InitState::FindWingBoundingBoxes(frame_data &frame_in, FocalGrid &fg) {
	bool success = true;
	wingtip_pcl_L.reset();
	wingtip_pcl_R.reset();
	M_wing_L.reset();
	M_wing_R.reset();
	try {
		if (tethered = true) {

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

			arma::Mat<double> pcl_mat = frame_in.seg_pcl;
			int N_pts = pcl_mat.n_cols;

			arma::Row<double> seg_ids = arma::unique(pcl_mat.row(3));
			int N_seg = seg_ids.n_cols;

			// Remove body points
			double body_id = seg_ids.min();
			arma::uvec wing_pt_ids1 = arma::find(pcl_mat.row(3) > body_id);
			arma::Mat<double> wing_tip_pcl1 = pcl_mat.cols(wing_pt_ids1);

			// Remove points within the cylinder around the body's longitudinal axis:
			arma::Col<double> body_ref_center = {M_body_fixed(0,3), M_body_fixed(1,3), M_body_fixed(2,3)};

			arma::Mat<double> xyz_body_ref = M_body_fixed.submat(0,0,2,2).t()*(wing_tip_pcl1.rows(0,2)-arma::repmat(body_ref_center,1,wing_tip_pcl1.n_cols));
			arma::Row<double> radius_yz = (xyz_body_ref.row(1)%xyz_body_ref.row(1))+(xyz_body_ref.row(2)%xyz_body_ref.row(2));
			arma::uvec wing_pt_ids2 = arma::find(radius_yz > pow(sphere_radius,2));
			//arma::Mat<double> wing_tip_pcl2 = wing_tip_pcl1.cols(wing_pt_ids2);

			arma::Mat<double> wing_tip_pcl2 = wing_tip_pcl1;

			arma::Row<double> wing_seg_ids = arma::unique(wing_tip_pcl2.row(3));
			int N_wing_segs = wing_seg_ids.n_cols;

			for (int j=0; j<N_wing_segs; j++) {
				arma::uvec wing_pt_ids3 = arma::find(wing_tip_pcl2.row(3) == wing_seg_ids(j));
				if (wing_pt_ids3.n_rows>min_nr_points) {
					arma::Mat<double> wing_tip_pcl3 = wing_tip_pcl2.cols(wing_pt_ids3);
					pcl_vec.push_back(wing_tip_pcl3);
				}
			}

			// Find connected matrices:
			vector<arma::Mat<double>> pcl_vec2 = InitState::ConnectPointclouds(fg, pcl_vec);
			tuple<vector<arma::Mat<double>>,vector<arma::Col<double>>> wing_tip_LR = InitState::SelectWingTipsTethered(pcl_vec2);
			vector<arma::Mat<double>> wingtip_pcl_LR = get<0>(wing_tip_LR);
			vector<arma::Col<double>> wingtip_info_LR = get<1>(wing_tip_LR);

			if (wingtip_info_LR[0](7)>1.0) {
				bbox_wingtip_L = InitState::BoundingBox(wingtip_pcl_LR[0]);
				wingtip_pcl_L  = wingtip_pcl_LR[0];
				double phi_L = wingtip_info_LR[0](4);
				double theta_L = wingtip_info_LR[0](5);
				double eta_L = wingtip_info_LR[0](6);
				arma::Mat<double> R_phi_L = {{1.0, 0.0, 0.0},
					{0.0, cos(phi_L), -sin(phi_L)},
					{0.0, sin(phi_L), cos(phi_L)}};
				arma::Mat<double> R_theta_L = {{cos(-theta_L), -sin(-theta_L), 0.0},
					{sin(-theta_L), cos(-theta_L), 0.0},
					{0.0, 0.0, 1.0}};
				arma::Mat<double> R_eta_L = {{cos(eta_L), 0.0, sin(eta_L)},
					{0.0, 1.0, 0.0},
					{-sin(eta_L), 0.0, sin(eta_L)}};
				arma::Mat<double> R_wing_L = M_strk.submat(0,0,2,2)*M_90.submat(0,0,2,2)*R_phi_L*R_theta_L;
				M_wing_L.eye(4,4);
				M_wing_L.submat(0,0,2,2) = R_wing_L;
			}
			else {
				wingtip_pcl_L.zeros(4,1);
				M_wing_L.zeros(4,4);
			}

			if (wingtip_info_LR[1](7)>1.0) {
				bbox_wingtip_R = InitState::BoundingBox(wingtip_pcl_LR[1]);
				wingtip_pcl_R  = wingtip_pcl_LR[1];
				double phi_R = wingtip_info_LR[1](4);
				double theta_R = wingtip_info_LR[1](5);
				double eta_R = wingtip_info_LR[1](6);
				arma::Mat<double> R_phi_R = {{1.0, 0.0, 0.0},
					{0.0, cos(-phi_R), -sin(-phi_R)},
					{0.0, sin(-phi_R), cos(-phi_R)}};
				arma::Mat<double> R_theta_R = {{cos(theta_R), -sin(theta_R), 0.0},
					{sin(theta_R), cos(theta_R), 0.0},
					{0.0, 0.0, 1.0}};
				arma::Mat<double> R_eta_R = {{cos(eta_R), 0.0, sin(eta_R)},
					{0.0, 1.0, 0.0},
					{-sin(eta_R), 0.0, sin(eta_R)}};
				arma::Mat<double> R_wing_R = M_strk.submat(0,0,2,2)*M_90.submat(0,0,2,2)*R_phi_R*R_theta_R;
				M_wing_R.eye(4,4);
				M_wing_R.submat(0,0,2,2) = R_wing_R;
			}
			else {
				wingtip_pcl_R.zeros(4,1);
				M_wing_R.zeros(4,4);
			}

		}
		else {
			// Free flight algorithm
		}
	}
	catch (...) {
		success = false;
		wingtip_pcl_L.zeros(4,1);
		wingtip_pcl_R.zeros(4,1);
		M_wing_L.eye(4,4);
		M_wing_R.eye(4,4);
	}
	return success;
}

void InitState::CalculateFixedBodyRefFrame() {

	arma::Col<double> x_vector;
	arma::Col<double> y_vector;
	arma::Col<double> z_vector;

	x_vector = (neck_loc-origin_loc)/arma::norm(neck_loc-origin_loc);
	x_vector = x_vector/arma::norm(x_vector);
	y_vector = (joint_L_loc-joint_R_loc)/arma::norm(joint_L_loc-joint_R_loc);
	y_vector = y_vector/arma::norm(y_vector);
	z_vector = arma::cross(x_vector,y_vector)/(arma::norm(x_vector)*arma::norm(y_vector));
	z_vector = z_vector/arma::norm(z_vector);

	// Construct the rotation matrix from the reference frame:
	arma::Mat<double> M_body(3,3);
	M_body.col(0) = x_vector;
	M_body.col(1) = y_vector;
	M_body.col(2) = z_vector;

	// Find the mid-point between the two hinges:
	arma::Col<double> center_loc;
	center_loc = (joint_L_loc+joint_R_loc)/2.0;

	M_body_fixed.eye(4,4);

	M_body_fixed.submat(0,0,2,2) = M_body;
	M_body_fixed(0,3) = center_loc(0);
	M_body_fixed(1,3) = center_loc(1);
	M_body_fixed(2,3) = center_loc(2);
}

vector<arma::Mat<double>> InitState::ConnectPointclouds(FocalGrid &fg, vector<arma::Mat<double>> &pcl_vec) {

	int N_pcls = pcl_vec.size();

	arma::Mat<double> pcl_mat;

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

	if (N_pcls>1) {
		arma::Mat<double> pcl_mat;
		pcl_mat = pcl_vec[0];
		for (int i=1; i<N_pcls; i++) {
			pcl_mat = arma::join_rows(pcl_mat,pcl_vec[i]);
		}
		arma::Mat<int> connectivity_mat = fg.FindConnectedPointClouds(pcl_mat);

		arma::Col<int> pcl_codes = connectivity_mat.diag();

		vector<vector<int>> conn_vec;
		vector<int> conn_row;
		vector<int>::iterator it;
		bool is_new = true;
		int vec_ind;

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

			if (i==0) {
				conn_row.clear();
				conn_row.push_back(i);
				conn_vec.push_back(conn_row);
				vec_ind = 0;
			}
			else {
				is_new = true;
				for (int k=0; k<conn_vec.size(); k++) {
					it = find(conn_vec[k].begin(),conn_vec[k].end(),i);
					if (it != conn_vec[k].end()) {
						is_new = false;
						vec_ind = k;
					}
				}
				if (is_new==true) {
					conn_row.clear();
					conn_row.push_back(i);
					conn_vec.push_back(conn_row);
					vec_ind = conn_vec.size()-1;
				}
			}

			for (int j=0; j<N_pcls; j++) {
				if (i != j) {
					if (connectivity_mat(i,j) > min_connect_points) {
						it = find(conn_vec[vec_ind].begin(),conn_vec[vec_ind].end(),j);
						if (it == conn_vec[vec_ind].end()) {
							conn_vec[vec_ind].push_back(j);
						}
					}
				}
			}
		}

		int N_conn_pcls = conn_vec.size();
		arma::Mat<double> temp_pcl;

		for (int m=0; m<N_conn_pcls; m++) {
			for (int n=0; n<conn_vec[m].size(); n++) {
			}
		}

		for (int m=0; m<N_conn_pcls; m++) {
			temp_pcl = pcl_vec[conn_vec[m][0]];
			if (conn_vec[m].size() > 1) {
				for (int n=1; n<conn_vec[m].size(); n++) {
					temp_pcl = arma::join_rows(temp_pcl,pcl_vec[conn_vec[m][n]]);
				}
			}
			temp_pcl.row(3).fill(m+2);
			pcl_out.push_back(temp_pcl);
		}
	}
	else if (N_pcls == 1) {
		pcl_out.push_back(pcl_vec[0]);
	}
	else {
		// Do nothing
	}

	return pcl_out;
}

tuple<vector<arma::Mat<double>>,vector<arma::Col<double>>> InitState::SelectWingTipsTethered(vector<arma::Mat<double>> &pcl_vec) {

	int N_segs = pcl_vec.size();

	vector<arma::Mat<double>> pcl_wingtip_L;
	vector<arma::Col<double>> potential_wingtip_L;
	vector<arma::Col<double>> wingtip_info_L;
	vector<arma::Mat<double>> pcl_wingtip_R;
	vector<arma::Col<double>> potential_wingtip_R;
	vector<arma::Col<double>> wingtip_info_R;

	arma::Col<double> wingtip_info(8);

	for (int i=0; i<N_segs; i++) {
		arma::Row<double> joint_L_dist = arma::sum((pcl_vec[i].rows(0,2).each_col()-joint_L_loc)%(pcl_vec[i].rows(0,2).each_col()-joint_L_loc));
		arma::Row<double> joint_R_dist = arma::sum((pcl_vec[i].rows(0,2).each_col()-joint_R_loc)%(pcl_vec[i].rows(0,2).each_col()-joint_R_loc));
		arma::uvec joint_L_dist_sort = arma::sort_index(joint_L_dist,"descend");
		arma::uvec joint_R_dist_sort = arma::sort_index(joint_R_dist,"descend");
		if (joint_L_dist_sort.n_rows > 10) {
			arma::Col<double> potential_L_wingtip(4);
			arma::Col<double> potential_R_wingtip(4);
			potential_L_wingtip = arma::mean(pcl_vec[i].cols(joint_L_dist_sort.rows(0,9)),1);
			potential_L_wingtip(3) = sqrt(arma::mean(joint_L_dist(joint_L_dist_sort.rows(0,9))));
			potential_R_wingtip = arma::mean(pcl_vec[i].cols(joint_R_dist_sort.rows(0,9)),1);
			potential_R_wingtip(3) = sqrt(arma::mean(joint_R_dist(joint_R_dist_sort.rows(0,9))));

			// Calculate the wing kinematic angles w.r.t. the fixed body ref frame
			arma::Col<double> wing_L_axis = M_90.submat(0,0,2,2).t()*M_strk.submat(0,0,2,2).t()*M_body_fixed.submat(0,0,2,2).t()*(potential_L_wingtip.rows(0,2)-joint_L_loc);
			arma::Col<double> wing_R_axis = M_90.submat(0,0,2,2).t()*M_strk.submat(0,0,2,2).t()*M_body_fixed.submat(0,0,2,2).t()*(potential_R_wingtip.rows(0,2)-joint_R_loc);

			//double phi_L   = atan2(-wing_L_axis(0),wing_L_axis(1));
			double phi_L   = atan2(wing_L_axis(2),wing_L_axis(1));
			//double theta_L = atan2(wing_L_axis(2),sqrt(pow(wing_L_axis(0),2)+pow(wing_L_axis(1),2)));
			double theta_L = atan2(wing_L_axis(0),sqrt(pow(wing_L_axis(1),2)+pow(wing_L_axis(2),2)));
			//double phi_R   = atan2(-wing_R_axis(0),-wing_R_axis(1));
			double phi_R   = atan2(wing_R_axis(2),-wing_R_axis(1));
			//double theta_R = atan2(wing_R_axis(2),sqrt(pow(wing_R_axis(0),2)+pow(wing_R_axis(1),2)));
			double theta_R = atan2(wing_R_axis(0),sqrt(pow(wing_R_axis(1),2)+pow(wing_R_axis(2),2)));

			// To determine eta convert to wing reference frame and determine the principal components in the x-z plane:

			arma::Mat<double> M_phi_L = {{1.0, 0.0, 0.0},
				{0.0, cos(phi_L), -sin(phi_L)},
				{0.0, sin(phi_L), cos(phi_L)}};

			arma::Mat<double> M_theta_L = {{cos(-theta_L), -sin(-theta_L), 0.0},
				{sin(-theta_L), cos(-theta_L), 0.0},
				{0.0, 0.0, 1.0}};

			arma::Mat<double> M_phi_R = {{1.0, 0.0, 0.0},
				{0.0, cos(-phi_R), -sin(-phi_R)},
				{0.0, sin(-phi_R), cos(-phi_R)}};

			arma::Mat<double> M_theta_R = {{cos(theta_R), -sin(theta_R), 0.0},
				{sin(theta_R), cos(theta_R), 0.0},
				{0.0, 0.0, 1.0}};

			arma::Mat<double> wing_L_pts = M_theta_L.t()*M_phi_L.t()*M_90.submat(0,0,2,2).t()*M_strk.submat(0,0,2,2).t()*M_body_fixed.submat(0,0,2,2).t()*(pcl_vec[i].rows(0,2).each_col()-joint_L_loc);
			arma::Mat<double> wing_R_pts = M_theta_R.t()*M_phi_R.t()*M_90.submat(0,0,2,2).t()*M_strk.submat(0,0,2,2).t()*M_body_fixed.submat(0,0,2,2).t()*(pcl_vec[i].rows(0,2).each_col()-joint_R_loc);

			// PCA
			//arma::Mat<double> pca_coeff_L;
			//arma::Mat<double> pca_score_L;
			//arma::Col<double> pca_latent_L;
			//arma::Col<double> pca_tsquared_L;

			//arma::Mat<double> A_L = arma::join_cols(wing_L_pts.row(0)-arma::mean(wing_L_pts.row(0)), wing_L_pts.row(2)-arma::mean(wing_L_pts.row(2)));

			//arma::princomp(pca_coeff_L, pca_score_L, pca_latent_L, pca_tsquared_L, A_L.t());

			//cout << "eigenvectors" << endl;
			//cout << arma::eig_gen(pca_coeff_L) << endl;

			double eta_L = atan2(arma::mean(wing_L_pts.row(2)),-arma::mean(wing_L_pts.row(0)));
			double eta_R = atan2(arma::mean(wing_R_pts.row(2)),-arma::mean(wing_R_pts.row(0)));

			/*
			if (eta_L < -eta_bound) {
				eta_L = eta_L+PI;
			}
			else if (eta_L > eta_bound) {
				eta_L = eta_L-PI;
			}

			if (eta_R < -eta_bound) {
				eta_R = eta_L+PI;
			}
			else if (eta_R > eta_bound) {
				eta_R = eta_R-PI;
			}
			*/

			bool L_candidate = false;
			bool R_candidate = false;

			if ((abs(phi_L) <= phi_bound) && (abs(theta_L) <= theta_bound)) {
				if ((potential_L_wingtip(3)>0.7*wing_length_L)&& (potential_L_wingtip(3)<1.3*wing_length_L)) {
					L_candidate = true;
				}
			}

			if ((abs(phi_R) <= phi_bound) && (abs(theta_R) <= theta_bound)) {
				if ((potential_R_wingtip(3)>0.7*wing_length_R)&& (potential_R_wingtip(3)<1.3*wing_length_R)) {
					R_candidate = true;
				}
			}

			if ((L_candidate==true) && (R_candidate==false)) {
				pcl_wingtip_L.push_back(pcl_vec[i]);
				wingtip_info(0) = potential_L_wingtip(0);
				wingtip_info(1) = potential_L_wingtip(1);
				wingtip_info(2) = potential_L_wingtip(2);
				wingtip_info(3) = potential_L_wingtip(3);
				wingtip_info(4) = phi_L;
				wingtip_info(5) = theta_L;
				wingtip_info(6) = eta_L;
				wingtip_info(7) = pcl_vec[i].n_cols;
				wingtip_info_L.push_back(wingtip_info);
			}
			else if ((R_candidate==true) && (L_candidate==false)) {
				pcl_wingtip_R.push_back(pcl_vec[i]);
				wingtip_info(0) = potential_R_wingtip(0);
				wingtip_info(1) = potential_R_wingtip(1);
				wingtip_info(2) = potential_R_wingtip(2);
				wingtip_info(3) = potential_R_wingtip(3);
				wingtip_info(4) = phi_R;
				wingtip_info(5) = theta_R;
				wingtip_info(6) = eta_R;
				wingtip_info(7) = pcl_vec[i].n_cols;
				wingtip_info_R.push_back(wingtip_info);
			}
			else if ((L_candidate == true) && (R_candidate==true)) {

				//bool select_L = false;
				//bool select_R = false;

				if (abs(phi_L)>abs(phi_R)) {
					pcl_wingtip_R.push_back(pcl_vec[i]);
					wingtip_info(0) = potential_R_wingtip(0);
					wingtip_info(1) = potential_R_wingtip(1);
					wingtip_info(2) = potential_R_wingtip(2);
					wingtip_info(3) = potential_R_wingtip(3);
					wingtip_info(4) = phi_R;
					wingtip_info(5) = theta_R;
					wingtip_info(6) = eta_R;
					wingtip_info(7) = pcl_vec[i].n_cols;
					wingtip_info_R.push_back(wingtip_info);
				}
				else if (abs(phi_L)<abs(phi_R)) {
					pcl_wingtip_L.push_back(pcl_vec[i]);
					wingtip_info(0) = potential_L_wingtip(0);
					wingtip_info(1) = potential_L_wingtip(1);
					wingtip_info(2) = potential_L_wingtip(2);
					wingtip_info(3) = potential_L_wingtip(3);
					wingtip_info(4) = phi_L;
					wingtip_info(5) = theta_L;
					wingtip_info(6) = eta_L;
					wingtip_info(7) = pcl_vec[i].n_cols;
					wingtip_info_L.push_back(wingtip_info);
				}
				else {
					// do nothing
				}
			}
			else {
				// do nothing
			}
		}
	}

	arma::Mat<double> wingtip_pcl_left;
	arma::Mat<double> wingtip_pcl_right;
	arma::Col<double> wingtip_info_left;
	arma::Col<double> wingtip_info_right;

	if (pcl_wingtip_L.size()==1) {
		wingtip_pcl_left = pcl_wingtip_L[0];
		wingtip_pcl_left.row(3).fill(2.0);
		wingtip_info_left = wingtip_info_L[0];
	}
	else if (pcl_wingtip_L.size()>1) {
		int N_segs_L = pcl_wingtip_L.size();
		arma::Row<double> seg_size_L(N_segs_L);
		for (int j=0; j<N_segs_L; j++) {
			seg_size_L(j) = wingtip_info_L[j](7);
		}
		arma::uvec L_sort = arma::sort_index(seg_size_L,"descend");
		wingtip_pcl_left = pcl_wingtip_L[L_sort[0]];
		wingtip_pcl_left.row(3).fill(2.0);
		wingtip_info_left = wingtip_info_L[L_sort[0]];
	}
	else {
		wingtip_pcl_left.zeros(4,1);
		wingtip_info_left.zeros(7);
	}

	if (pcl_wingtip_R.size()==1) {
		wingtip_pcl_right = pcl_wingtip_R[0];
		wingtip_pcl_right.row(3).fill(3.0);
		wingtip_info_right = wingtip_info_R[0];
	}
	else if (pcl_wingtip_R.size()>1) {
		int N_segs_R = pcl_wingtip_R.size();
		arma::Row<double> seg_size_R(N_segs_R);
		for (int j=0; j<N_segs_R; j++) {
			seg_size_R(j) = wingtip_info_R[j](7);
		}
		arma::uvec R_sort = arma::sort_index(seg_size_R,"descend");
		wingtip_pcl_right = pcl_wingtip_R[R_sort[0]];
		wingtip_pcl_right.row(3).fill(3.0);
		wingtip_info_right = wingtip_info_R[R_sort[0]];
	}
	else {
		wingtip_pcl_right.zeros(4,1);
		wingtip_info_right.zeros(8);
	}

	vector<arma::Mat<double>> pcl_out;
	pcl_out.push_back(wingtip_pcl_left);
	pcl_out.push_back(wingtip_pcl_right);

	vector<arma::Col<double>> info_out;
	info_out.push_back(wingtip_info_left);
	info_out.push_back(wingtip_info_right);

	tuple<vector<arma::Mat<double>>,vector<arma::Col<double>>> wingtips_out = make_tuple(pcl_out,info_out);

	return wingtips_out;
}

vector<arma::Mat<double>> InitState::SelectWingTips(vector<arma::Mat<double>> &pcl_vec) {

	int N_segs = pcl_vec.size();

	arma::Row<double> seg_size;
	seg_size.zeros(N_segs);

	arma::Col<double> body_center = {bbox_body.box_center[0], bbox_body.box_center[1], bbox_body.box_center[2]};

	vector<arma::Mat<double>> pcl_wingtip;
	vector<arma::Col<double>> potential_wingtip;

	for (int i=0; i<N_segs; i++) {
		arma::Row<double> center_dist = arma::sum((pcl_vec[i].rows(0,2).each_col()-body_center)%(pcl_vec[i].rows(0,2).each_col()-body_center));
		arma::uvec dist_sort = arma::sort_index(center_dist,"descend");
		if (dist_sort.n_rows > 10) {
			arma::Col<double> potential_wingtip_pt(4);
			potential_wingtip_pt = arma::mean(pcl_vec[i].cols(dist_sort.rows(0,9)),1);
			potential_wingtip_pt(3) = sqrt(arma::mean(center_dist(dist_sort.rows(0,9))));
			if (potential_wingtip_pt(3) > (0.8*wing_length_L) && potential_wingtip_pt(3) < (1.2*wing_length_L)) {
				seg_size(i) = pcl_vec[i].n_cols;
				pcl_wingtip.push_back(pcl_vec[i]);
				potential_wingtip.push_back(potential_wingtip_pt);
			}
			else {
				seg_size(i) = 0;
				pcl_wingtip.push_back(pcl_vec[i]);
				potential_wingtip.push_back(potential_wingtip_pt);
			}
		}
	}

	arma::uvec seg_size_sort = arma::sort_index(seg_size,"descend");

	// Select the biggest pointcloud, calculate the distance of the other pointclouds to the wingtip
	// and pick the largest product between distance*pointcloud_size

	vector<arma::Mat<double>> wing_tip_vec;
	vector<arma::Col<double>> wing_tip_pos;

	if (N_segs == 1) {
		wing_tip_vec.push_back(pcl_wingtip[seg_size_sort(0)]);
		wing_tip_pos.push_back(potential_wingtip[seg_size_sort(0)]);
	}
	else if (N_segs > 1) {
		wing_tip_vec.push_back(pcl_wingtip[seg_size_sort(0)]);
		wing_tip_pos.push_back(potential_wingtip[seg_size_sort(0)]);
		arma::Col<double> wt_1 = potential_wingtip[seg_size_sort(0)];
		arma::Col<double> wt_2(4);
		arma::Row<double> candidate_score;
		candidate_score.zeros(N_segs);
		for (int j=0; j<N_segs; j++) {
			wt_2 = potential_wingtip[j];
			candidate_score(j) = sqrt(pow(wt_1(0)-wt_2(0),2)+pow(wt_1(1)-wt_2(1),2)+pow(wt_1(2)-wt_2(2),2))*seg_size(j);
		}
		wing_tip_vec.push_back(pcl_wingtip[candidate_score.index_max()]);
		wing_tip_pos.push_back(potential_wingtip[candidate_score.index_max()]);
	}

	return wing_tip_vec;
}

void InitState::Alpha3D(arma::Mat<double> &pcl_in) {
	// Convert pcl_in to a point list:
	list<Point> lp;
	for (int i=0; i<pcl_in.n_cols; i++) {
		lp.push_back(Point(pcl_in(0,i),pcl_in(1,i),pcl_in(2,i)));
	}

	// Compute alpha shape
	Alpha_shape_3 as(lp.begin(),lp.end());
	cout << "Alpha shape computed in REGULARIZED mode by default" << endl;

	// Find optimal alpha value
	Alpha_iterator opt = as.find_optimal_alpha(1);
	cout << "Optimal alpha value to get one connected component is " << *opt << endl;

	as.set_alpha(*opt);
	assert(as.number_of_solid_components() == 1);
}

bbox InitState::BoundingBox(arma::Mat<double> &pcl_in) {

  	// Return bounding box structure

  	struct bbox bounding_box;

  	bounding_box.box_center[0] = 0.0;
  	bounding_box.box_center[1] = 0.0;
  	bounding_box.box_center[2] = 0.0;
  	bounding_box.box_corners[0][0] = 0.0;
  	bounding_box.box_corners[1][0] = 0.0;
  	bounding_box.box_corners[2][0] = 0.0;
	bounding_box.box_corners[0][1] = 0.0;
  	bounding_box.box_corners[1][1] = 0.0;
  	bounding_box.box_corners[2][1] = 0.0;
  	bounding_box.box_corners[0][2] = 0.0;
  	bounding_box.box_corners[1][2] = 0.0;
  	bounding_box.box_corners[2][2] = 0.0;
  	bounding_box.box_corners[0][3] = 0.0;
  	bounding_box.box_corners[1][3] = 0.0;
  	bounding_box.box_corners[2][3] = 0.0;
  	bounding_box.box_corners[0][4] = 0.0;
  	bounding_box.box_corners[1][4] = 0.0;
  	bounding_box.box_corners[2][4] = 0.0;
  	bounding_box.box_corners[0][5] = 0.0;
  	bounding_box.box_corners[1][5] = 0.0;
  	bounding_box.box_corners[2][5] = 0.0;
  	bounding_box.box_corners[0][6] = 0.0;
  	bounding_box.box_corners[1][6] = 0.0;
  	bounding_box.box_corners[2][6] = 0.0;
  	bounding_box.box_corners[0][7] = 0.0;
  	bounding_box.box_corners[1][7] = 0.0;
  	bounding_box.box_corners[2][7] = 0.0;
	bounding_box.q_box[0] = 0.0;
	bounding_box.q_box[1] = 0.0;
	bounding_box.q_box[2] = 0.0;
	bounding_box.q_box[3] = 0.0;
	bounding_box.R_box[0][0] = 0.0;
	bounding_box.R_box[0][1] = 0.0;
	bounding_box.R_box[0][2] = 0.0;
	bounding_box.R_box[1][0] = 0.0;
	bounding_box.R_box[1][1] = 0.0;
	bounding_box.R_box[1][2] = 0.0;
	bounding_box.R_box[2][0] = 0.0;
	bounding_box.R_box[2][1] = 0.0;
	bounding_box.R_box[2][2] = 0.0;
	bounding_box.mass_center[0] = 0.0;
	bounding_box.mass_center[1] = 0.0;
	bounding_box.mass_center[2] = 0.0;
	bounding_box.eigen_values[0] = 0.0;
	bounding_box.eigen_values[1] = 0.0;
	bounding_box.eigen_values[2] = 0.0;
	bounding_box.eigen_vectors[0][0] = 0.0;
	bounding_box.eigen_vectors[0][1] = 0.0;
	bounding_box.eigen_vectors[0][2] = 0.0;
	bounding_box.eigen_vectors[1][0] = 0.0;
	bounding_box.eigen_vectors[1][1] = 0.0;
	bounding_box.eigen_vectors[1][2] = 0.0;
	bounding_box.eigen_vectors[2][0] = 0.0;
	bounding_box.eigen_vectors[2][1] = 0.0;
	bounding_box.eigen_vectors[2][2] = 0.0;
	bounding_box.moment_of_inertia[0] = 0.0;
	bounding_box.moment_of_inertia[1] = 0.0;
	bounding_box.moment_of_inertia[2] = 0.0;
	bounding_box.eccentricity[0] = 0.0;
	bounding_box.eccentricity[1] = 0.0;
	bounding_box.eccentricity[2] = 0.0;
	bounding_box.volume =  0.0;

	return bounding_box;
}

np::ndarray InitState::ReturnBBox(vector<struct bbox> &bbox_in) {
	int N_boxes = bbox_in.size();
	p::tuple shape = p::make_tuple(24,1);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray bbox_out = np::zeros(shape,dtype);

	if (N_boxes > 0) {
		p::tuple shape = p::make_tuple(24,N_boxes);
		np::dtype dtype = np::dtype::get_builtin<double>();
		bbox_out = np::zeros(shape,dtype);
		for (int i=0; i<N_boxes; i++) {
			arma::Mat<double> rot_mat = {{bbox_in[i].R_box[0][0], bbox_in[i].R_box[0][1], bbox_in[i].R_box[0][2], bbox_in[i].box_center[0]},
										 {bbox_in[i].R_box[1][0], bbox_in[i].R_box[1][1], bbox_in[i].R_box[1][2], bbox_in[i].box_center[1]},
										 {bbox_in[i].R_box[2][0], bbox_in[i].R_box[2][1], bbox_in[i].R_box[2][2], bbox_in[i].box_center[2]},
										 {0.0, 0.0, 0.0, 1.0}};
			for (int j=0; j<8; j++) {
				arma::Col<double> corner_point = {bbox_in[i].box_corners[0][j], bbox_in[i].box_corners[1][j], bbox_in[i].box_corners[2][j], 1.0};
				arma::Col<double> new_corner_point = rot_mat*corner_point;
				bbox_out[j*3][i]   = new_corner_point(0);
				bbox_out[j*3+1][i] = new_corner_point(1);
				bbox_out[j*3+2][i] = new_corner_point(2);
			}
		}
	}
	else {
		for (int j=0; j<8; j++) {
			bbox_out[j*3]   = 0.0;
			bbox_out[j*3+1] = 0.0;
			bbox_out[j*3+2] = 0.0;
		}
	}
	return bbox_out;
}

np::ndarray InitState::ReturnJointLocs() {
	p::tuple shape = p::make_tuple(3,2);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray loc_out = np::zeros(shape,dtype);

	loc_out[0][0] = joint_L_loc(0);
	loc_out[1][0] = joint_L_loc(1);
	loc_out[2][0] = joint_L_loc(2);
	loc_out[0][1] = joint_R_loc(0);
	loc_out[1][1] = joint_R_loc(1);
	loc_out[2][1] = joint_R_loc(2);

	return loc_out;
}

np::ndarray InitState::ReturnSRF() {
	p::tuple shape = p::make_tuple(4,4);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray M_out = np::zeros(shape,dtype);

	arma::Mat<double> M_stroke_out;
	M_stroke_out.eye(4,4);

	M_stroke_out.submat(0,0,2,2) = M_body_fixed.submat(0,0,2,2)*M_strk.submat(0,0,2,2);

	M_stroke_out(0,3) = M_body_fixed(0,3);
	M_stroke_out(1,3) = M_body_fixed(1,3);
	M_stroke_out(2,3) = M_body_fixed(2,3);

	M_out[0][0] = M_stroke_out(0,0);
	M_out[0][1] = M_stroke_out(0,1);
	M_out[0][2] = M_stroke_out(0,2);
	M_out[0][3] = M_stroke_out(0,3);
	M_out[1][0] = M_stroke_out(1,0);
	M_out[1][1] = M_stroke_out(1,1);
	M_out[1][2] = M_stroke_out(1,2);
	M_out[1][3] = M_stroke_out(1,3);
	M_out[2][0] = M_stroke_out(2,0);
	M_out[2][1] = M_stroke_out(2,1);
	M_out[2][2] = M_stroke_out(2,2);
	M_out[2][3] = M_stroke_out(2,3);
	M_out[3][0] = M_stroke_out(3,0);
	M_out[3][1] = M_stroke_out(3,1);
	M_out[3][2] = M_stroke_out(3,2);
	M_out[3][3] = M_stroke_out(3,3);

	return M_out;
}

np::ndarray InitState::ReturnWingtipPCLS() {
	int N = 0;
	bool left_wt_exists = false;
	bool right_wt_exists = false;

	if (wingtip_pcl_L.n_cols > 1) {
		N = N + wingtip_pcl_L.n_cols;
		left_wt_exists = true;
	}

	if (wingtip_pcl_R.n_cols > 1) {
		N = N + wingtip_pcl_R.n_cols;
		right_wt_exists = true;
	}

	if (N==0) {
		N = 2;
	}

	p::tuple shape = p::make_tuple(4,N);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray pcl_out = np::zeros(shape,dtype);

	if (left_wt_exists == true && right_wt_exists == true) {

		int k=0;

		for (int i=0; i<wingtip_pcl_L.n_cols; i++) {
			pcl_out[0][k] = wingtip_pcl_L(0,i);
			pcl_out[1][k] = wingtip_pcl_L(1,i);
			pcl_out[2][k] = wingtip_pcl_L(2,i);
			pcl_out[3][k] = 1.0;
			k++;
		}
		for (int j=0; j<wingtip_pcl_R.n_cols; j++) {
			pcl_out[0][k] = wingtip_pcl_R(0,j);
			pcl_out[1][k] = wingtip_pcl_R(1,j);
			pcl_out[2][k] = wingtip_pcl_R(2,j);
			pcl_out[3][k] = 2.0;
			k++;
		}
	}
	else if (left_wt_exists == true && right_wt_exists == false) {

		for (int i=0; i<wingtip_pcl_L.n_cols; i++) {
			pcl_out[0][i] = wingtip_pcl_L(0,i);
			pcl_out[1][i] = wingtip_pcl_L(1,i);
			pcl_out[2][i] = wingtip_pcl_L(2,i);
			pcl_out[3][i] = 1.0;
		}

		pcl_out[0][N-1] = 0.0;
		pcl_out[1][N-1] = 0.0;
		pcl_out[2][N-1] = 0.0;
		pcl_out[3][N-1] = 2.0;
	}
	else if (left_wt_exists == false && right_wt_exists == true) {

		for (int i=0; i<wingtip_pcl_R.n_cols; i++) {
			pcl_out[0][i] = wingtip_pcl_R(0,i);
			pcl_out[1][i] = wingtip_pcl_R(1,i);
			pcl_out[2][i] = wingtip_pcl_R(2,i);
			pcl_out[3][i] = 2.0;
		}

		pcl_out[0][N-1] = 0.0;
		pcl_out[1][N-1] = 0.0;
		pcl_out[2][N-1] = 0.0;
		pcl_out[3][N-1] = 1.0;
	}
	else {

		pcl_out[0][0] = 0.0;
		pcl_out[1][0] = 0.0;
		pcl_out[2][0] = 0.0;
		pcl_out[3][0] = 1.0;

		pcl_out[0][0] = 0.0;
		pcl_out[1][0] = 0.0;
		pcl_out[2][0] = 0.0;
		pcl_out[3][0] = 2.0;
	}

	return pcl_out;
}

np::ndarray InitState::ReturnWingtipBoxes() {
	int N = 0;
	bool left_wt_exists = false;
	bool right_wt_exists = false;

	if (wingtip_pcl_L.n_cols > 1) {
		N = N + 1;
		left_wt_exists = true;
	}

	if (wingtip_pcl_R.n_cols > 1) {
		N = N + 1;
		right_wt_exists = true;
	}

	if (N==0) {
		N=1;
	}

	p::tuple shape = p::make_tuple(24,N);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray bbox_out = np::zeros(shape,dtype);

	if (left_wt_exists == true && right_wt_exists == true) {

		arma::Mat<double> rot_L = {{bbox_wingtip_L.R_box[0][0], bbox_wingtip_L.R_box[0][1], bbox_wingtip_L.R_box[0][2], bbox_wingtip_L.box_center[0]},
								   {bbox_wingtip_L.R_box[1][0], bbox_wingtip_L.R_box[1][1], bbox_wingtip_L.R_box[1][2], bbox_wingtip_L.box_center[1]},
								   {bbox_wingtip_L.R_box[2][0], bbox_wingtip_L.R_box[2][1], bbox_wingtip_L.R_box[2][2], bbox_wingtip_L.box_center[2]},
								   {0.0, 0.0, 0.0, 1.0}};

		for (int j=0; j<8; j++) {
			arma::Col<double> corner_point = {bbox_wingtip_L.box_corners[0][j], bbox_wingtip_L.box_corners[1][j], bbox_wingtip_L.box_corners[2][j], 1.0};
			arma::Col<double> new_corner_point = rot_L*corner_point;
			bbox_out[j*3][0]   = new_corner_point(0);
			bbox_out[j*3+1][0] = new_corner_point(1);
			bbox_out[j*3+2][0] = new_corner_point(2);
		}

		arma::Mat<double> rot_R = {{bbox_wingtip_R.R_box[0][0], bbox_wingtip_R.R_box[0][1], bbox_wingtip_R.R_box[0][2], bbox_wingtip_R.box_center[0]},
								   {bbox_wingtip_R.R_box[1][0], bbox_wingtip_R.R_box[1][1], bbox_wingtip_R.R_box[1][2], bbox_wingtip_R.box_center[1]},
								   {bbox_wingtip_R.R_box[2][0], bbox_wingtip_R.R_box[2][1], bbox_wingtip_R.R_box[2][2], bbox_wingtip_R.box_center[2]},
								   {0.0, 0.0, 0.0, 1.0}};

		for (int j=0; j<8; j++) {
			arma::Col<double> corner_point = {bbox_wingtip_R.box_corners[0][j], bbox_wingtip_R.box_corners[1][j], bbox_wingtip_R.box_corners[2][j], 1.0};
			arma::Col<double> new_corner_point = rot_R*corner_point;
			bbox_out[j*3][1]   = new_corner_point(0);
			bbox_out[j*3+1][1] = new_corner_point(1);
			bbox_out[j*3+2][1] = new_corner_point(2);
		}
	}
	else if (left_wt_exists == true && right_wt_exists == false) {

		arma::Mat<double> rot_L = {{bbox_wingtip_L.R_box[0][0], bbox_wingtip_L.R_box[0][1], bbox_wingtip_L.R_box[0][2], bbox_wingtip_L.box_center[0]},
								   {bbox_wingtip_L.R_box[1][0], bbox_wingtip_L.R_box[1][1], bbox_wingtip_L.R_box[1][2], bbox_wingtip_L.box_center[1]},
								   {bbox_wingtip_L.R_box[2][0], bbox_wingtip_L.R_box[2][1], bbox_wingtip_L.R_box[2][2], bbox_wingtip_L.box_center[2]},
								   {0.0, 0.0, 0.0, 1.0}};

		for (int j=0; j<8; j++) {
			arma::Col<double> corner_point = {bbox_wingtip_L.box_corners[0][j], bbox_wingtip_L.box_corners[1][j], bbox_wingtip_L.box_corners[2][j], 1.0};
			arma::Col<double> new_corner_point = rot_L*corner_point;
			bbox_out[j*3][0]   = new_corner_point(0);
			bbox_out[j*3+1][0] = new_corner_point(1);
			bbox_out[j*3+2][0] = new_corner_point(2);
		}
	}
	else if (left_wt_exists == false && right_wt_exists == true) {

		arma::Mat<double> rot_R = {{bbox_wingtip_R.R_box[0][0], bbox_wingtip_R.R_box[0][1], bbox_wingtip_R.R_box[0][2], bbox_wingtip_R.box_center[0]},
								   {bbox_wingtip_R.R_box[1][0], bbox_wingtip_R.R_box[1][1], bbox_wingtip_R.R_box[1][2], bbox_wingtip_R.box_center[1]},
								   {bbox_wingtip_R.R_box[2][0], bbox_wingtip_R.R_box[2][1], bbox_wingtip_R.R_box[2][2], bbox_wingtip_R.box_center[2]},
								   {0.0, 0.0, 0.0, 1.0}};

		for (int j=0; j<8; j++) {
			arma::Col<double> corner_point = {bbox_wingtip_R.box_corners[0][j], bbox_wingtip_R.box_corners[1][j], bbox_wingtip_R.box_corners[2][j], 1.0};
			arma::Col<double> new_corner_point = rot_R*corner_point;
			bbox_out[j*3][0]   = new_corner_point(0);
			bbox_out[j*3+1][0] = new_corner_point(1);
			bbox_out[j*3+2][0] = new_corner_point(2);
		}
	}
	else {
		for (int j=0; j<8; j++) {
			bbox_out[j*3][0]   = 0.0;
			bbox_out[j*3+1][0] = 0.0;
			bbox_out[j*3+2][0] = 0.0;
		}
	}

	return bbox_out;
}

arma::Mat<double> InitState::OrthoNormalizeR(arma::Mat<double> R_in) {
	arma::Mat<double> R_out(3,3);
	R_out = arma::real(arma::expmat(0.5*(arma::real(arma::logmat(R_in))-arma::trans(arma::real(arma::logmat(R_in))))));
	return R_out;
}

np::ndarray InitState::ReturnSegPCL(frame_data &frame_in) {
	int N_pts = frame_in.seg_pcl.n_cols;
	p::tuple shape = p::make_tuple(4,N_pts);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray array_out = np::zeros(shape,dtype);
	for (int i=0; i<N_pts; i++) {
		array_out[0][i] = frame_in.seg_pcl(0,i);
		array_out[1][i] = frame_in.seg_pcl(1,i);
		array_out[2][i] = frame_in.seg_pcl(2,i);
		array_out[3][i] = frame_in.seg_pcl(3,i);
	}
	return array_out;
}


//
// Created by huangkun on 2019/12/30.
//

#ifndef OPENGV2_SPLINEBUNDLEADJUSTMENT_HPP
#define OPENGV2_SPLINEBUNDLEADJUSTMENT_HPP

#include <Eigen/Eigen>
#include <ceres/rotation.h>

#include <opengv2/spline/BsplineReal.hpp>
#include <opengv2/bundle_adjustment/BundleAdjustmentBase.hpp>

namespace opengv2 {
    class SplineBundleAdjustment : public BundleAdjustmentBase {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        SplineBundleAdjustment(bool fixTsb, bool robust, bool RtConstrain,
                               bool planarConstrain, double RtConstrainWeight = 3);

        void
        run(const std::map<double, Bodyframe::Ptr> &keyframes,
            const std::map<int, LandmarkBase::Ptr> &landmarks) override;

        struct ReprojectionError {
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

            ReprojectionError(const Eigen::Ref<const Eigen::Vector3d> &bearingVector,
                              std::shared_ptr<std::vector<std::vector<double>>> basis,
                              bool isMono)
                    : isMono_(isMono), bearingVector_(bearingVector), basis_(std::move(basis)) {}

            template<typename T>
            bool operator()(const T *const cp0, const T *const cp1, const T *const cp2, const T *const cp3,
                            const T *const lm, const T *const Tcb, T *residuals) const {
                // Map to Eigen type
                Eigen::Map<Eigen::Matrix<T, 7, 1> const> const cp_0(cp0);
                Eigen::Map<Eigen::Matrix<T, 7, 1> const> const cp_1(cp1);
                Eigen::Map<Eigen::Matrix<T, 7, 1> const> const cp_2(cp2);
                Eigen::Map<Eigen::Matrix<T, 7, 1> const> const cp_3(cp3);
                Eigen::Map<Eigen::Matrix<T, 3, 1> const> const lm_e(lm);
                Eigen::Quaternion<T> Qcb(Tcb);
                Qcb.normalize();
                Eigen::Map<Eigen::Matrix<T, 3, 1> const> const tcb(Tcb + 4);
                Eigen::Map<Eigen::Matrix<T, 3, 1>> res(residuals);

                Eigen::Matrix<T, 7, 1> Qbw_twb = (*basis_)[0][0] * cp_0 + (*basis_)[0][1] * cp_1 +
                                                 (*basis_)[0][2] * cp_2 + (*basis_)[0][3] * cp_3;
                Eigen::Map<Eigen::Quaternion<T>> Qbw(Qbw_twb.data());
                Qbw.normalize();
                Eigen::Map<Eigen::Matrix<T, 3, 1>> twb(Qbw_twb.data() + 4);

                Eigen::Matrix<T, 3, 1> Xb = Qbw * (lm_e - twb);
                Eigen::Matrix<T, 3, 1> Xc = Qcb * Xb + tcb;
                if (isMono_)
                    Xc.normalize();

                res = bearingVector_ - Xc;
                return true;
            }

            static ceres::CostFunction *
            Create(const Eigen::Ref<const Eigen::Vector3d> &bearingVector,
                   const std::shared_ptr<std::vector<std::vector<double>>> &basis,
                   bool isMono) {
                return (new ceres::AutoDiffCostFunction<ReprojectionError, 3, 7, 7, 7, 7, 3, 7>(
                        new ReprojectionError(bearingVector, basis, isMono)));
            }

            bool isMono_;
            Eigen::Vector3d bearingVector_;

            // for spline evaluation
            std::shared_ptr<std::vector<std::vector<double>>> basis_;
        };

        // The angle between 2nd row of Rbw and twb's 1st derivative should be constant
        // the constrain only work when straight motion, or translation difference in extrinsic parameter is very small
        struct ConstrainTcbConstant {
            explicit ConstrainTcbConstant(std::shared_ptr<std::vector<std::vector<double>>> basis)
                    : basis_(std::move(basis)) {}

            template<typename T>
            bool operator()(const T *const cp0, const T *const cp1, const T *const cp2,
                            const T *const cp3, T *residuals) const {
                // Map to Eigen type
                Eigen::Map<Eigen::Matrix<T, 7, 1> const> const cp_0(cp0);
                Eigen::Map<Eigen::Matrix<T, 7, 1> const> const cp_1(cp1);
                Eigen::Map<Eigen::Matrix<T, 7, 1> const> const cp_2(cp2);
                Eigen::Map<Eigen::Matrix<T, 7, 1> const> const cp_3(cp3);
                Eigen::Map<Eigen::Matrix<T, 3, 1>> res(residuals);

                Eigen::Matrix<T, 4, 1> Qbw_v = (*basis_)[0][0] * cp_0.head(4) + (*basis_)[0][1] * cp_1.head(4) +
                                               (*basis_)[0][2] * cp_2.head(4) + (*basis_)[0][3] * cp_3.head(4);
                Eigen::Map<Eigen::Quaternion<T>> Qbw(Qbw_v.data());
                Qbw.normalize();

                Eigen::Matrix<T, 3, 1> twb1d = (*basis_)[1][0] * cp_0.tail(3) + (*basis_)[1][1] * cp_1.tail(3) +
                                               (*basis_)[1][2] * cp_2.tail(3) + (*basis_)[1][3] * cp_3.tail(3);
                twb1d.normalize();

                // 2nd row of Rbw
                const T &Qx = Qbw.x();
                const T &Qy = Qbw.y();
                const T &Qz = Qbw.z();
                const T &Qw = Qbw.w();
                Eigen::Matrix<T, 3, 1> y(2. * (Qx * Qy + Qw * Qz), 1. - 2. * (Qx * Qx + Qz * Qz),
                                         2. * (Qy * Qz - Qw * Qx));
                y.normalize();

                res = twb1d - y;
                return true;
            }

            static ceres::CostFunction *
            Create(const std::shared_ptr<std::vector<std::vector<double>>> &basis) {
                return (new ceres::AutoDiffCostFunction<ConstrainTcbConstant, 3, 7, 7, 7, 7>(
                        new ConstrainTcbConstant(basis)));
            }

            // for spline evaluation
            std::shared_ptr<std::vector<std::vector<double>>> basis_;
        };

    protected:
        void optimize(ceres::Problem &problem,
                      const std::map<double, Bodyframe::Ptr> &keyframes,
                      const std::map<int, LandmarkBase::Ptr> &landmarks) override;

        bool fixTsb_;
        bool robust_;
        bool RtConstrain_;
        bool planarConstrain_;

        double RtConstrainWeight_;

        BsplineReal<7> Tspline_; // ceres type unit quaternion [w,x,y,z] Qbw, twb, by cubic spline
    };
}

#endif //OPENGV2_SPLINEBUNDLEADJUSTMENT_HPP


/*
 * Copyright 2015 Applied Research Center for Computer Networks
 *
 * 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 "ipv4addr.hh"

#include <regex>
#include <boost/format.hpp>

#include <mutex>

namespace runos {

static unsigned long parseipv4address(const std::string &str)
{
    static const std::string ipv4_pattern =
        "([[:digit:]]{1,3}).([[:digit:]]{1,3})."
        "([[:digit:]]{1,3}).([[:digit:]]{1,3})";

    static const auto ipv4_regex =
        std::regex(ipv4_pattern, std::regex::optimize);

    std::smatch match;

    if (not std::regex_match(str, match, ipv4_regex)) {
        throw ipv4addr::bad_representation("Bad IPv4 address: " + str);
    }

    unsigned long res = 0;
    for (size_t oct = 1; oct <= 4; ++oct) {
        res <<= 8;
        res |= (uint8_t) std::stoi(match[oct], 0, 10);
    }

    return res;
}

static bool compare_prefix(const std::string &pattern,
                           const ipv4addr::bits_type &value,
                           uint8_t subnet)
{
    ipv4addr::bits_type bit_pattern(parseipv4address(pattern));
    uint8_t shift = 32 - subnet;
    return bit_pattern >> shift == value >> shift;
}

ipv4addr::ipv4addr(const std::string &str)
    : data_(parseipv4address(str))
{ }

ipv4addr::ipv4addr(uint32_t num)
    : data_(num)
{ }

ipv4addr::ipv4addr(const bytes_type &octets) noexcept
    :data_( ((unsigned long) octets[0] << 24UL) |
            ((unsigned long) octets[1] << 16UL) |
            ((unsigned long) octets[2] << 8UL ) |
            ((unsigned long) octets[3]) )
{ }

ipv4addr::bytes_type ipv4addr::to_octets() const noexcept
{
    uint32_t num = to_number();
    return bytes_type{{
        (uint8_t) ((num & 0xff000000UL) >> 24UL),
        (uint8_t) ((num & 0xff0000UL)   >> 16UL),
        (uint8_t) ((num & 0xff00UL)     >> 8UL ),
        (uint8_t) ((num & 0xffUL)              ),
    }};
}

std::ostream& operator<<(std::ostream &out, const ipv4addr &ipv4)
{
    static boost::format static_format("%u.%u.%u.%u");
    boost::format format(static_format); // boost::format is not thread safety
    const auto& data = ipv4.to_octets();
    return out << format % unsigned(data[0]) % unsigned(data[1])
                         % unsigned(data[2]) % unsigned(data[3]);
}

bool is_private(const ipv4addr &addr) noexcept
{
    return compare_prefix("10.0.0.0", addr.data_, 8)
        || compare_prefix("172.16.0.0", addr.data_, 12)
        || compare_prefix("192.168.0.0.", addr.data_, 16);
}

bool is_loopback(const ipv4addr &addr) noexcept
{
    return compare_prefix("127.0.0.0", addr.data_, 8);
}

bool is_multicast(const ipv4addr &addr) noexcept
{
    return compare_prefix("224.0.0.0", addr.data_, 4);
}

bool is_broadcast(const ipv4addr &addr) noexcept
{
    return addr.data_.all();
}

}// namespace runos

using runos::ipv4addr;
size_t std::hash<ipv4addr>::operator()(const ipv4addr& addr) const
{
    return std::hash<unsigned long>()(addr.to_number());
}


using namespace std;
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Hello

#include <memory>
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <glm/glm.hpp>
#include <glm/ext.hpp>
#include <glm/mat4x4.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include "Boundingbox.h"
#include "Boundingbox.cc"

 // This piece of code is testing the bounding boxs collision which is being forced to happen by making B1 and B2 collide
BOOST_AUTO_TEST_CASE(Boundingbox)
{
    auto b1 = make_shared<BoundingBox>(BoundingBox(Vector3(0.0f, 0.0f, 0.0f), 0.5f, 0.5f, 0.5f));
    auto b2 = make_shared<BoundingBox>(BoundingBox(Vector3(0.4f, 0.4f, 0.4f), 0.5f, 0.5f, 0.5f));
    BOOST_CHECK( b1->CollidesWith(b2) );
    BOOST_CHECK( b2->CollidesWith(b1) );

}


#include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|

#ifndef LIMBO_MODEL_SPGP_HPP
#define LIMBO_MODEL_SPGP_HPP

#include <cassert>
#include <iostream>
#include <limits>
#include <vector>

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/LU>

#include <limbo/opt.hpp>
#include <limbo/tools.hpp>

namespace limbo {
    namespace defaults {
        struct model_spgp {
            BO_PARAM(double, jitter, 0.000001);
            BO_PARAM(double, samples_percent, 10);
            BO_PARAM(int, min_m, 1);

            /// kernel parameters
            BO_PARAM(double, sig, 0.01);
            BO_PARAM(double, pred_kernel_sigma_sq, 0.5);
            BO_PARAM(double, pred_kernel_l, 0.5);
        };
    }

    namespace model {
        /// @ingroup model
        /// Sparse Gaussian process.
        /// Reference and credits for this code:
        /// Sparse Gaussian Processes using Pseudo-inputs (NIPS, 2005)
        /// Edward Snelson and Zoubin Ghahramani
        /// It is parametrized by:
        /// - a mean function
        /// - [optionnal] an optimizer for the hyper-parameters
        template <typename Params, typename KernelFunction, typename MeanFunction, class HyperParamsOptimizer = opt::NLOptGrad<Params, nlopt::LD_LBFGS>>
        class SPGP {
        public:
            struct HyperParams {
                Eigen::MatrixXd b;
                double c;
                double sig;
                Eigen::MatrixXd xb;

                HyperParams(const Eigen::VectorXd& w, const size_t& m, const size_t& dim_in)
                {
                    b = w.segment(m * dim_in, dim_in).transpose().array().exp();
                    c = std::exp(w[(m + 1) * dim_in]);
                    sig = std::exp(w[(m + 1) * dim_in + 1]);
                    xb = w.segment(0, m * dim_in);
                    xb.resize(m, dim_in);
                }
            };

            /// useful because the model might be created before knowing anything about the process
            SPGP() : _dim_in(-1), _dim_out(-1) {}

            /// useful because the model might be created before having samples
            SPGP(int dim_in, int dim_out) : _dim_in(dim_in), _dim_out(dim_out)
            {
                _mean_function = MeanFunction(_dim_out);
                _kernel_function = KernelFunction(_dim_in);
                _b = Eigen::VectorXd::Ones(_dim_in) * Params::model_spgp::pred_kernel_l();
                _c = Params::model_spgp::pred_kernel_sigma_sq();
                _sig = Params::model_spgp::sig();
            }

            /// useful to construct without optimizing
            SPGP(const std::vector<Eigen::VectorXd>& samples,
                const std::vector<Eigen::VectorXd>& observations)
            {
                _init(samples, observations);
            }

            /// execute the hyperparameters optimization
            void optimize_hyperparams()
            {
                _optimize_init = true;
                _optimize_hyperparams();
            }

            /// Compute the SPGP from samples and observations. This call needs to be explicit!
            void compute(const std::vector<Eigen::VectorXd>& samples,
                const std::vector<Eigen::VectorXd>& observations)
            {
                compute(_to_matrix(samples), _to_matrix(observations));
            }
            void compute(const std::vector<Eigen::VectorXd>& samples,
                const Eigen::MatrixXd& observations)
            {
                compute(_to_matrix(samples), observations);
            }
            void compute(const Eigen::MatrixXd& samples,
                const Eigen::MatrixXd& observations)
            {
                assert(samples.rows() != 0);
                assert(observations.rows() != 0);
                assert(samples.rows() == observations.rows());

                _optimize_init = true;
                _init(samples, observations);
                _compute();
            }

            /// add sample and recompute the SPGP
            void add_sample(const Eigen::VectorXd& sample, const Eigen::VectorXd& observation)
            {
                if (_samples.rows() == 0) {
                    _dim_in = sample.size();
                    _kernel_function = KernelFunction(_dim_in); // the cost of building a functor should be relatively low

                    _dim_out = observation.size();
                    _mean_function = MeanFunction(_dim_out); // the cost of building a functor should be relatively low

                    // default values for the kernel
                    _b = Eigen::VectorXd::Ones(_dim_in) * Params::model_spgp::pred_kernel_l();
                    _c = Params::model_spgp::pred_kernel_sigma_sq();
                    _sig = Params::model_spgp::sig();
                }
                else {
                    assert(sample.size() == _dim_in);
                    assert(observation.size() == _dim_out);
                }

                // add the new sample
                _samples.conservativeResize(_samples.rows() + 1, _dim_in);
                _samples.row(_samples.rows() - 1) = sample.transpose();
                _update_m();

                // add the new observation and update
                _observations.conservativeResize(_observations.rows() + 1, _dim_out);
                _observations.row(_observations.rows() - 1) = observation.transpose();
                _compute_observations_zm(); // TODO: if the mean function doesn't use _obs_mean we can do a partial calculation

                _optimize_init = true; // maybe we can add one random pseudo-input before optimizing to avoid re-initialization
                _compute();
            }

            /**
             \\rst
             return :math:`\mu`, :math:`\sigma^2` (unormalized). If there is no sample, return the value according to the mean function. Using this method instead of separate calls to mu() and sigma() is more efficient because some computations are shared between mu() and sigma().
             \\endrst
	  		*/
            std::tuple<Eigen::VectorXd, double> query(const Eigen::VectorXd& v) const
            {
                std::pair<Eigen::MatrixXd, Eigen::MatrixXd>&& result = predict(v.transpose());
                return std::make_tuple(result.first, result.second(0, 0));
            }

            /**
             \\rst
             return :math:`\mu`, :math:`\sigma^2`. Predict a bunch of points.
             \\endrst
            */
            std::pair<Eigen::MatrixXd, Eigen::MatrixXd> predict(const Eigen::MatrixXd& xt) const
            {
                return _predict(xt, true, true);
            }

            /**
             \\rst
             return :math:`\mu` (unormalized). If there is no sample, return the value according to the mean function.
             \\endrst
	  		*/
            Eigen::MatrixXd mu(const Eigen::MatrixXd& v) const
            {
                return _predict(v, true, false).first;
            }
            std::vector<Eigen::VectorXd> mu_mult(const Eigen::MatrixXd& v) const
            {
                return _to_vector(_predict(v, true, false).first);
            }

            /**
             \\rst
             return :math:`\sigma^2` (unormalized). If there is no sample, return the max :math:`\sigma^2`.
             \\endrst
	  		*/
            double sigma(const Eigen::VectorXd& v) const
            {
                return (_predict(v, false, true).second)(0, 0);
            }
            // TODO: Deberia machear la interfaz de la GP
            Eigen::VectorXd sigma_mult(const Eigen::MatrixXd& v) const
            {
                return (_predict(v, false, true).second);
            }

            /// return the number of dimensions of the input
            int dim_in() const
            {
                assert(_dim_in != -1); // need to compute first !
                return _dim_in;
            }

            /// return the number of dimensions of the output
            int dim_out() const
            {
                assert(_dim_out != -1); // need to compute first !
                return _dim_out;
            }

            // NOTE: This is worth keeping?
            // const KernelFunction& kernel_function() const { return _kernel_function; }
            // KernelFunction& kernel_function() { return _kernel_function; }

            const MeanFunction& mean_function() const { return _mean_function; }
            MeanFunction& mean_function() { return _mean_function; }

            /// return the maximum observation (only call this if the output of the GP is of dimension 1)
            Eigen::VectorXd max_observation() const
            {
                if (_observations.cols() > 1)
                    std::cout << "WARNING max_observation with multi dimensional "
                                 "observations doesn't make sense"
                              << std::endl;
                return tools::make_vector(_observations.maxCoeff());
            }

            /// return the mean observation
            Eigen::VectorXd mean_observation() const
            {
                return _samples.size() > 0 ? _obs_mean
                                           : Eigen::VectorXd::Zero(_dim_out);
            }

            /// return the number of samples used to compute the SPGP
            int nb_samples() const { return _samples.rows(); }

            /// return the number of pseudo samples used to compute the SPGP
            int nb_pseudo_samples() const { return _pseudo_samples.rows(); }

            ///  recomputes the SPGP
            void recompute(bool update_obs_mean = true)
            {
                _optimize_init = true;
                _compute();
            }

            /// return the list of samples that have been tested so far
            std::vector<Eigen::VectorXd> samples() const { return _to_vector(_samples); }

            /// return the list of pseudo-samples beign used
            std::vector<Eigen::VectorXd> pseudo_samples() const { return _to_vector(_pseudo_samples); }

            // test the likelihood function given a dataset
            void test_likelihood(const Eigen::MatrixXd& data,
                const Eigen::MatrixXd& samples,
                const Eigen::MatrixXd& observations)
            {
                size_t m = samples.rows() / 10;
                _init(samples, observations);
                int size_w = (m + 1) * samples.cols() + 2;
                for (int i = 0; i < data.rows(); i++) {
                    Eigen::VectorXd w = data.row(i).segment(0, size_w);
                    double fw = data.row(i).segment(size_w, 1)(0);
                    Eigen::VectorXd dfw = data.row(i).segment(size_w + 1, size_w);

                    opt::eval_t result = _likelihood(w, true);
                    double rfw = std::get<0>(result);
                    Eigen::VectorXd rdfw = std::get<1>(result).get();

                    std::cout << "[" << i + 1 << "] "
                              << "fw: " << std::abs(rfw + fw)
                              << " dfw: " << (rdfw - dfw).norm() << std::endl;
                }
            }

        protected:
            /// set on initialization
            size_t _m;
            size_t _dim_in = -1;
            size_t _dim_out = -1;
            double _del;
            Eigen::MatrixXd _samples;
            Eigen::MatrixXd _observations;
            Eigen::MatrixXd _observations_zm;
            Eigen::VectorXd _obs_mean;
            MeanFunction _mean_function;
            KernelFunction _kernel_function;

            /// set after the hyperparameters calculation
            Eigen::MatrixXd _pseudo_samples;
            Eigen::VectorXd _b; // lengthscales
            double _c; // noise
            double _sig; // signal variance
            // Eigen::VectorXd _noises;

            /// calculated in _compute
            Eigen::MatrixXd _Lm;
            Eigen::MatrixXd _bet;
            Eigen::MatrixXd _matrixL;

            /// auxiliary variables
            bool _optimize_init = true;
            bool _optimized = false;
            Eigen::VectorXd _w_init;
            HyperParamsOptimizer _hp_optimize;

            void _init(const std::vector<Eigen::VectorXd>& samples, const std::vector<Eigen::VectorXd>& observations)
            {
                _init(_to_matrix(samples), _to_matrix(observations));
            }
            void _init(const Eigen::MatrixXd& samples, const Eigen::MatrixXd& observations)
            {
                _samples = samples;
                _observations = observations;

                _dim_in = _samples.cols();
                _dim_out = _observations.cols();
                _mean_function = MeanFunction(_dim_out);
                _kernel_function = KernelFunction(_dim_in);

                _compute_observations_zm();
                _update_m();

                _del = Params::model_spgp::jitter();

                _optimize_init = true;
                srand(time(NULL));
            }

            void _compute_observations_zm()
            {
                _obs_mean = _observations.colwise().mean();
                _observations_zm.resize(_observations.rows(), _observations.cols());
                for (int i = 0; i < _observations.rows(); i++) {
                    _observations_zm.row(i) = _observations.row(i) - (_mean_function(_samples.row(i), *this)).transpose();
                }
            }

            void _update_m()
            {
                // NOTE: We could add the option to use a function to change the m dinamycally based on the samples.
                _m = Params::model_spgp::samples_percent() * _samples.rows() / 100;
                if (_m < Params::model_spgp::min_m())
                    _m = Params::model_spgp::min_m();
            }

            void _compute(bool optimize = true)
            {
                if (optimize)
                    _optimize_hyperparams();

                Eigen::MatrixXd kernel_m = _compute_kernel_matrix(_pseudo_samples, _pseudo_samples) + Eigen::MatrixXd::Identity(_pseudo_samples.rows(), _pseudo_samples.rows()) * Params::model_spgp::jitter();
                _matrixL = kernel_m.llt().matrixL();
                Eigen::MatrixXd kernel_mn = _compute_kernel_matrix(_pseudo_samples, _samples);

                Eigen::MatrixXd V = (_matrixL.template triangularView<Eigen::Lower>()).solve(kernel_mn);
                Eigen::MatrixXd ep = Eigen::MatrixXd::Ones(_observations_zm.rows(), 1).array() + (_k_diag(_observations_zm).array() - V.array().pow(2).colwise().sum().transpose()) / _sig;

                Eigen::MatrixXd ep_sqrt = ep.array().sqrt();
                V = V.cwiseQuotient(ep_sqrt.transpose().replicate(_m, 1));
                Eigen::MatrixXd y = _observations_zm.cwiseQuotient(ep_sqrt);

                _Lm = (_sig * Eigen::MatrixXd::Identity(_m, _m) + V * V.transpose()).llt().matrixL();
                _bet = (_Lm.template triangularView<Eigen::Lower>()).solve(V * y);
            }

            void _optimize_hyperparams()
            {
                if (_optimize_init) {

                    // Initialize parameter vector
                    _update_m();
                    _w_init = Eigen::VectorXd((_m + 1) * _dim_in + 2);

                    // Initialize pseudo-inputs to a random subset of training inputs
                    Eigen::VectorXd positions = Eigen::VectorXd::LinSpaced(_samples.rows(), 0, _samples.rows() - 1);
                    std::random_shuffle(positions.data(), positions.data() + positions.size());
                    for (size_t i = 0; i < _m; ++i)
                        _w_init.segment(i * _dim_in, _dim_in) = _samples.row(positions[i]);

                    // Initialize hyperparameters sensibly in log space
                    _w_init.segment(_m * _dim_in, _dim_in) = -2 * ((_samples.colwise().maxCoeff() - _samples.colwise().minCoeff()).array() / 2).log(); // -2*log((max(x)-min(x))'/2)
                    _w_init[(_m + 1) * _dim_in] = std::log(_observations_zm.array().pow(2).mean());
                    _w_init[(_m + 1) * _dim_in + 1] = std::log((_observations_zm.array().pow(2) / 4).mean());

                    _optimize_init = false;
                }

                auto optimize = [&](const Eigen::VectorXd& x, bool g) {
                    return this->_likelihood(x, g);
                };
                Eigen::VectorXd result = _hp_optimize(optimize, _w_init, false);

                HyperParams hp(result, _m, _dim_in);
                _pseudo_samples = hp.xb;
                _b = hp.b.transpose();
                _c = hp.c;
                _sig = hp.sig;
                // _noises = hp.sig * Eigen::VectorXd::Ones(_m);

                _optimized = true;
            }

            opt::eval_t _likelihood(const Eigen::VectorXd& input, bool eval_grad = false) const // opt wrapper
            {
                // Unzip parameters
                HyperParams hp(input, _m, _dim_in);
                return _likelihood_wp(hp.xb, hp.b, hp.c, hp.sig, _samples, _observations_zm, eval_grad, true);
            }

            opt::eval_t _likelihood_wp(Eigen::MatrixXd& xb, const Eigen::MatrixXd& b, const double& c, const double& sig,
                const Eigen::MatrixXd& samples, const Eigen::MatrixXd& observations_zm, bool eval_grad, bool inverse) const
            {
                // Prepare xb and x
                int n = samples.rows();
                Eigen::MatrixXd idm = Eigen::MatrixXd::Identity(_m, _m);
                Eigen::MatrixXd b_sqrt = b.array().sqrt();
                xb = xb.cwiseProduct(b_sqrt.replicate(_m, 1));
                Eigen::MatrixXd x = samples.cwiseProduct(b_sqrt.replicate(n, 1));

                // Construct Q
                Eigen::MatrixXd Q = xb * xb.transpose();
                Q = Eigen::MatrixXd(Q.diagonal().replicate(1, _m) + Q.diagonal().transpose().replicate(_m, 1) - 2 * Q);
                Q = (Q.array() * -0.5).exp() * c;
                Q += _del * idm;

                // NOTE: Need to be changed for the generic kernel construction
                Eigen::MatrixXd k_m = x.cwiseProduct(x).rowwise().sum().transpose().replicate(_m, 1);
                Eigen::MatrixXd k_n = xb.cwiseProduct(xb).rowwise().sum().replicate(1, n);
                Eigen::MatrixXd K = -2 * xb * x.transpose() + k_m + k_n;
                K = (K.array() * -0.5).exp() * c;

                // Resolve
                Eigen::MatrixXd L = Q.llt().matrixL();
                Eigen::TriangularView<Eigen::MatrixXd, Eigen::Lower> L_t = L.template triangularView<Eigen::Lower>();
                Eigen::MatrixXd V = L_t.solve(K);

                Eigen::MatrixXd ep = 1 + (c - V.array().pow(2).colwise().sum().transpose()) / sig; // ep = 1 + (c-sum(V.^2)')/sig;
                K = K.array() / ep.array().sqrt().transpose().replicate(_m, 1);
                V = V.array() / ep.array().sqrt().transpose().replicate(_m, 1);
                Eigen::MatrixXd y = observations_zm.array() / ep.array().sqrt();

                Eigen::MatrixXd Lm = (sig * idm + V * V.transpose()).llt().matrixL();
                Eigen::TriangularView<Eigen::MatrixXd, Eigen::Lower> Lm_t = Lm.template triangularView<Eigen::Lower>();
                Eigen::MatrixXd invLmV = Lm_t.solve(V);
                Eigen::MatrixXd bet = invLmV * y;

                // Calculate likelihood
                Eigen::MatrixXd fw = Lm.diagonal().array().log().colwise().sum() + (n - _m) / 2 * std::log(sig) + (y.transpose() * y - bet.transpose() * bet).array() / (2 * sig) + ep.array().log().sum() / 2 + 0.5 * n * std::log(2 * M_PI);

                if (!eval_grad) {
                    if (inverse)
                        return opt::no_grad(-fw(0));
                    else
                        return opt::no_grad(fw(0));
                }

                // ** Derivates calculation - precomputations
                Eigen::MatrixXd Lt = L * Lm;
                Eigen::MatrixXd B1 = Lt.transpose().lu().solve(invLmV); // Lt is psd but Lt' it's not. We must use lu here. NOTE: Maybe it can be changed to solve the system with psd.
                Eigen::MatrixXd b1 = Lt.transpose().lu().solve(bet);

                Eigen::MatrixXd invLV = L.transpose().lu().solve(V);
                Eigen::MatrixXd invL = L.inverse();
                Eigen::MatrixXd invQ = invL.transpose() * invL; // delete invL
                Eigen::MatrixXd invLt = Lt.inverse();
                Eigen::MatrixXd invA = invLt.transpose() * invLt; // delete invLt

                //NOTE: Throws exception after solving a transpose if multiplied directly with V, that's why its beign casted.
                Eigen::MatrixXd mu = (((Eigen::MatrixXd)Lm.transpose().lu().solve(bet).transpose()) * V).transpose(); // NOTE: Check if the cast is correct here
                Eigen::MatrixXd sumVsq = V.array().pow(2).colwise().sum().transpose();

                //bigsum = y.*(bet'*invLmV)'/sig - sum(invLmV.*invLmV)'/2 - (y.^2+mu.^2)/2/sig + 0.5
                Eigen::MatrixXd bigsum = (((Eigen::MatrixXd)y.cwiseProduct((bet.transpose() * invLmV).transpose()) / sig) - ((Eigen::MatrixXd)invLmV.array().pow(2).colwise().sum().transpose() / 2) - ((Eigen::MatrixXd)(y.array().pow(2) + mu.array().pow(2)) / (2 * sig))).array() + 0.5;

                Eigen::MatrixXd TT = invLV * (invLV.transpose().cwiseProduct(bigsum.replicate(1, _m)));

                // Pseudo inputs and lengthscales
                Eigen::MatrixXd dfxb(_m, _dim_in);
                Eigen::MatrixXd dfb(_dim_in, 1);
                for (size_t i = 0; i < _dim_in; i++) {
                    // dnnQ = dist(xb(:,i),xb(:,i)).*Q;
                    Eigen::MatrixXd r = xb.block(0, i, xb.rows(), 1);

                    Eigen::MatrixXd dnnQ = _dist(xb.block(0, i, xb.rows(), 1), xb.block(0, i, xb.rows(), 1)).cwiseProduct(Q);
                    // dNnK = dist(-xb(:,i),-x(:,i)).*K;
                    Eigen::MatrixXd dNnK = _dist(xb.block(0, i, xb.rows(), 1) * -1, x.block(0, i, x.rows(), 1) * -1).cwiseProduct(K);

                    // epdot = (dNnK.*invLV)*(-2/sig); epPmod = -sum(epdot)';
                    Eigen::MatrixXd epdot = dNnK.cwiseProduct(invLV) * (-2 / sig);
                    Eigen::MatrixXd epPmod = -1 * epdot.colwise().sum().transpose();

                    // dfxb(:,i) = - b1.*(dNnK*(y-mu)/sig + dnnQ*b1) + sum((invQ - invA*sig).*dnnQ,2) + epdot*bigsum - 2/sig*sum(dnnQ.*TT,2);
                    dfxb.block(0, i, dfxb.rows(), 1) = b1.cwiseProduct((dNnK * (y - mu)) / sig + dnnQ * b1) * -1 + (invQ - invA * sig).cwiseProduct(dnnQ).rowwise().sum() + epdot * bigsum - (dnnQ.cwiseProduct(TT).rowwise().sum()) * (2 / sig);

                    // dfb(i,1) = (((y-mu)'.*(b1'*dNnK))/sig + (epPmod.*bigsum)')*x(:,i);
                    dfb.block(i, 0, 1, 1) = ((y - mu).transpose().cwiseProduct(b1.transpose() * dNnK) / sig + epPmod.cwiseProduct(bigsum).transpose()) * x.block(0, i, x.rows(), 1);

                    // Overwrite dNnK and write derivates
                    dNnK = Eigen::MatrixXd(dNnK.cwiseProduct(B1));
                    dfxb.block(0, i, dfxb.rows(), 1) += dNnK.rowwise().sum(); // dfxb(:,i) = dfxb(:,i) + sum(dNnK,2);
                    dfb.block(i, 0, 1, 1) -= dNnK.colwise().sum() * x.block(0, i, x.rows(), 1); // dfb(i,1) = dfb(i,1) - sum(dNnK,1)*x(:,i);

                    dfxb.block(0, i, dfxb.rows(), 1) *= std::sqrt(b(i)); // dfxb(:,i) = dfxb(:,i)*sqrt(b(i));

                    dfb.block(i, 0, 1, 1) /= std::sqrt(b(i)); // dfb(i,1) = dfb(i,1)/sqrt(b(i));
                    dfb.block(i, 0, 1, 1) += (dfxb.block(0, i, dfxb.rows(), 1).transpose() * // dfb(i,1) = dfb(i,1) + (dfxb(:,i)'*xb(:,i))/b(i);
                                                 xb.block(0, i, xb.rows(), 1))
                        / b(i);
                    dfb.block(i, 0, 1, 1) *= std::sqrt(b(i)) / 2; // dfb(i,1) = dfb(i,1)*sqrt(b(i))/2;
                }

                // Size
                // epc = (c./ep - sumVsq - del*sum((invLV).^2)')/sig
                Eigen::MatrixXd epc = (ep.cwiseInverse() * c - sumVsq - _del * (((Eigen::MatrixXd)invLV.array().pow(2)).colwise().sum().transpose())) / sig;

                // dfc = (m + del*trace(invQ-sig*invA) - sig*sum(sum(invA.*Q')))/2 -
                //          mu'*(y-mu)/sig + (b1'*(Q-del*eye(m))*b1)/2 + epc'*bigsum;
                Eigen::MatrixXd dfc = (_m + _del * (invQ - sig * invA).trace() - sig * (invA.cwiseProduct(Q.transpose()).sum())) / 2 - ((mu.transpose() * (y - mu)) / sig).array() + ((b1.transpose() * (Q - _del * idm) * b1) / 2).array() + (epc.transpose() * bigsum).array();

                // Noise - dfsig = sum(bigsum./ep);
                Eigen::MatrixXd dfsig = bigsum.cwiseQuotient(ep).colwise().sum();

                // Assemble the result
                Eigen::VectorXd dfw(dfxb.size() + dfb.size() + dfc.size() + dfsig.size());
                dfw.segment(0, dfxb.size()) = Eigen::Map<Eigen::MatrixXd>(dfxb.data(), 1, dfxb.size()).transpose();
                dfw.segment(dfxb.size(), dfb.size()) = dfb;
                dfw.segment(dfxb.size() + dfb.size(), dfc.size()) = dfc;
                dfw.segment(dfxb.size() + dfb.size() + dfc.size(), dfsig.size()) = dfsig;

                if (inverse) {
                    Eigen::VectorXd rdfw = dfw.array() * -1;
                    return {-fw(0), rdfw}; // limbo maximizes instead of minimizing
                }
                else {
                    return {fw(0), dfw};
                }
            }

            std::pair<Eigen::MatrixXd, Eigen::MatrixXd> _predict(const Eigen::MatrixXd& xt, bool calc_mu = true, bool calc_s2 = true) const
            {
                Eigen::MatrixXd mu(xt.rows(), _dim_out);
                Eigen::MatrixXd s2(xt.rows(), 1);

                if (_samples.rows() == 0) {
                    for (size_t i = 0; i < (size_t)xt.rows(); ++i) {
                        if (calc_mu)
                            mu.row(i) = _mean_function(xt.row(i), *this);
                        if (calc_s2)
                            s2(i, 0) = _kernel_function(xt.row(i), xt.row(i));
                    }
                    return {mu, s2.array()};
                }

                Eigen::MatrixXd K = _compute_kernel_matrix(_pseudo_samples, xt);
                Eigen::MatrixXd lst = (_matrixL.template triangularView<Eigen::Lower>()).solve(K);
                Eigen::MatrixXd lmst = (_Lm.template triangularView<Eigen::Lower>()).solve(lst);

                if (calc_mu) {
                    for (size_t i = 0; i < (size_t)xt.rows(); ++i)
                        mu.row(i) = _mean_function(xt.row(i), *this);
                    mu += (_bet.transpose() * lmst).transpose();
                }

                if (calc_s2)
                    s2 = _k_diag(xt).array() - lst.array().pow(2).colwise().sum().transpose() + _sig * lmst.array().pow(2).colwise().sum().transpose() + (_optimized ? _sig : 0);
                return {mu, s2.array()};
            }

            Eigen::MatrixXd _compute_kernel_matrix(const Eigen::MatrixXd& p_1, const Eigen::MatrixXd& p_2) const
            {
                Eigen::MatrixXd K;
                int n_1 = p_1.rows();
                int n_2 = p_2.rows();

                Eigen::MatrixXd bx_1 = _b.transpose().array().sqrt().replicate(n_1, 1);
                Eigen::MatrixXd bx_2 = _b.transpose().array().sqrt().replicate(n_2, 1);

                Eigen::MatrixXd x_1 = p_1.cwiseProduct(bx_1);
                Eigen::MatrixXd x_2 = p_2.cwiseProduct(bx_2);

                K = (-2 * x_1 * x_2.transpose()).array() + x_2.array().pow(2).rowwise().sum().transpose().replicate(n_1, 1) + x_1.array().pow(2).rowwise().sum().replicate(1, n_2);
                K = _c * (K * -0.5).array().exp();

                return K;
            }

            Eigen::MatrixXd _k_diag(const Eigen::MatrixXd& x) const
            {
                // NOTE: This calculation it's only for the ExpArd kernel
                return Eigen::MatrixXd::Constant(x.rows(), 1, _c);
            }

            // Compute pairwise distance matrix from two column vectors x0 and x1
            Eigen::MatrixXd _dist(const Eigen::MatrixXd& x_0, const Eigen::MatrixXd& x_1) const
            {
                return x_0.replicate(1, x_1.size()) - x_1.transpose().replicate(x_0.size(), 1);
            }

            // Construct a matrixXd from a std::vector of eigen::vectorXds
            // NOTE: Maybe this could be done better?
            Eigen::MatrixXd _to_matrix(const std::vector<Eigen::VectorXd>& xs) const
            {
                Eigen::MatrixXd result(xs.size(), xs[0].size());
                for (size_t i = 0; i < (size_t)result.rows(); ++i) {
                    result.row(i) = xs[i];
                }
                return result;
            }
            Eigen::MatrixXd _to_matrix(std::vector<Eigen::VectorXd>& xs) const { return _to_matrix(xs); }

            // Construct a std::vector of eigen::vectorXds from a matrixXd
            // NOTE: Maybe this could be done better?
            std::vector<Eigen::VectorXd> _to_vector(const Eigen::MatrixXd& m) const
            {
                std::vector<Eigen::VectorXd> result(m.rows());
                for (size_t i = 0; i < result.size(); ++i) {
                    result[i] = m.row(i);
                }
                return result;
            }
            std::vector<Eigen::VectorXd> _to_vector(Eigen::MatrixXd& m) const { return _to_vector(m); }

            /*
            void _compute_kernel_m()
            {
                _kernel_m.resize(m, m);

                // O(m^2) [should be negligible]
                for (int i = 0; i < m; i++)
                    for (int j = 0; j <= i; ++j)
                        _kernel_m(i, j) = _kernel_function(_pseudo_samples.row(i), _pseudo_samples.row(j)) + ((i == j) ? _noises[i] : 0); // noise only on the diagonal

                for (int i = 0; i < m; i++)
                    for (int j = 0; j < i; ++j)
                        _kernel_m(j, i) = _kernel_m(i, j);

                // O(m^3)
                _matrixL = Eigen::LLT<Eigen::MatrixXd>(_kernel_m).matrixL();
            }

            void _compute_kernel_mn()
            {
                int n = _samples.size();
                _kernel_mn.resize(m, n);

                // O(m*n)
                for (int i = 0; i < m; i++)
                    for (int j = 0; j < n; ++j)
                        _kernel_mn(i, j) = _kernel_function(_pseudo_samples.row(i), _samples.row(j));
            }

            Eigen::MatrixXd _compute_kernel(Eigen::MatrixXd& x_1, Eigen::MatrixXd& x_2)
            {
                int m = x_1.size();
                int n = x_2.size();
                Eigen::MatrixXd kernel(m, n);

                for (int i = 0; i < m; i++)
                    for (int j = 0; j < n; ++j)
                        kernel(i, j) = _kernel_function(x_1.row(i), x_2.row(j));

                return kernel;
            }

            double _kernel_function(const Eigen::VectorXd& v1, const Eigen::VectorXd& v2) const
            {
                // SquaredExpARD kernel
                const Eigen::VectorXd _ell = _b;
                double _sf2 = _c;
                double z = (v1 - v2).cwiseQuotient(_ell).squaredNorm();
                return _sf2 * std::exp(-0.5 * z);
            }
            */
        };
    }
}

#endif