#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <iostream>
#include <thread>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>

#include "structures.h"
#include "transformations.h"
#include "cauchy.h"

#include "point_to_point_tait_bryan_wc_jacobian.h"
#include "point_to_point_source_to_target_tait_bryan_wc_jacobian.h"

#include "rgd.h"

struct ScanPose{
	Eigen::Affine3d m;
	pcl::PointCloud<pcl::PointXYZ> pc;
};

pcl::PointCloud<pcl::PointXYZ> pc_ground_truth;
std::vector<ScanPose> scan_poses;
int current_scan_index = 0;

float sradius = 1.0;
bool show_ground_truth = true;

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -50.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();
void set_initial_guess(std::vector<ScanPose>& scan_poses);

void split(std::string &str, char delim, std::vector<std::string> &out)
{
	size_t start;
	size_t end = 0;

	while ((start = str.find_first_not_of(delim, end)) != std::string::npos)
	{
		end = str.find(delim, start);
		out.push_back(str.substr(start, end - start));
	}
}

std::vector<std::pair<int,int>> nns(ScanPose &sp1, ScanPose &sp2, float radius);

std::vector<std::pair<int,int>> pairs_temp;
std::vector<Bucket> buckets_render;
bool show_ndt_covariances = true;

int main(int argc, char *argv[]){

	std::vector<std::string> pcd_file_names;
	pcd_file_names.push_back("../data/pcd/scan000.pcd");
	pcd_file_names.push_back("../data/pcd/scan001.pcd");
	pcd_file_names.push_back("../data/pcd/scan002.pcd");
	pcd_file_names.push_back("../data/pcd/scan003.pcd");
	pcd_file_names.push_back("../data/pcd/scan004.pcd");
	pcd_file_names.push_back("../data/pcd/scan005.pcd");
	pcd_file_names.push_back("../data/pcd/scan006.pcd");
	pcd_file_names.push_back("../data/pcd/scan007.pcd");
	pcd_file_names.push_back("../data/pcd/scan008.pcd");
	pcd_file_names.push_back("../data/pcd/scan009.pcd");
	pcd_file_names.push_back("../data/pcd/scan010.pcd");
	pcd_file_names.push_back("../data/pcd/scan011.pcd");
	pcd_file_names.push_back("../data/pcd/scan012.pcd");

	for(size_t i = 0; i < pcd_file_names.size(); i++){
		std::cout << "loading file: " << pcd_file_names[i] << std::endl;
		ScanPose sp;
		sp.m = Eigen::Affine3d::Identity();
		pcl::PointCloud<pcl::PointXYZ> pc;
		if (pcl::io::loadPCDFile(pcd_file_names[i], pc) == -1) {
			std::cout << "PROBLEM WITH LODAING pcd: " << pcd_file_names[i] << std::endl;
			return 1;
		}else{
			sp.pc = pc;
			scan_poses.push_back(sp);
		}
	}

	if (pcl::io::loadPCDFile("../data/pcd/ground_truth.pcd", pc_ground_truth) == -1) {
		std::cout << "PROBLEM WITH LODAING pcd: ../data/pcd/ground_truth.pcd" << std::endl;
		return 1;
	}

	set_initial_guess(scan_poses);

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("point_cloud_registration");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd  = 3)
{

    Eigen::LLT<Eigen::Matrix<double,3,3> > cholSolver(covar);
    Eigen::Matrix3d transform = cholSolver.matrixL();

    const double pi = 3.141592;
    const double di =0.02;
    const double dj =0.04;
    const double du =di*2*pi;
    const double dv =dj*pi;
    glColor3f(color.x(), color.y(),color.z());

    for (double i = 0; i < 1.0; i+=di)  //horizonal
    {
        for (double j = 0; j < 1.0; j+=dj)  //vertical
        {
            double u = i*2*pi;      //0     to  2pi
            double v = (j-0.5)*pi;  //-pi/2 to pi/2

            const Eigen::Vector3d pp0( cos(v)* cos(u),cos(v) * sin(u),sin(v));
            const Eigen::Vector3d pp1(cos(v) * cos(u + du) ,cos(v) * sin(u + du) ,sin(v));
            const Eigen::Vector3d pp2(cos(v + dv)* cos(u + du) ,cos(v + dv)* sin(u + du) ,sin(v + dv));
            const Eigen::Vector3d pp3( cos(v + dv)* cos(u),cos(v + dv)* sin(u),sin(v + dv));
            Eigen::Vector3d tp0 = transform * (nstd*pp0) + mean;
            Eigen::Vector3d tp1 = transform * (nstd*pp1) + mean;
            Eigen::Vector3d tp2 = transform * (nstd*pp2) + mean;
            Eigen::Vector3d tp3 = transform * (nstd*pp3) + mean;

            glBegin(GL_LINE_LOOP);
            glVertex3dv(tp0.data());
            glVertex3dv(tp1.data());
            glVertex3dv(tp2.data());
            glVertex3dv(tp3.data());
            glEnd();
        }
    }
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glBegin(GL_LINES);
		glColor3f(1.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(1.0f, 0.0f, 0.0f);

		glColor3f(0.0f, 1.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 1.0f, 0.0f);

		glColor3f(0.0f, 0.0f, 1.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 1.0f);
	glEnd();

	glBegin(GL_POINTS);
	glColor3f(1.0, 0.0, 0.0);
	for(size_t i = 0; i < scan_poses.size(); i++){
		if(i == current_scan_index){
			glColor3f(0.0, 1.0, 0.0);
		}else{
			glColor3f(1.0, 0.0, 0.0);
		}
		if(i+1 == current_scan_index){
			glColor3f(0.0, 0.0, 1.0);
		}
		for(size_t j = 0; j < scan_poses[i].pc.size(); j++){
			Eigen::Vector3d v(scan_poses[i].pc[j].x, scan_poses[i].pc[j].y, scan_poses[i].pc[j].z);
			Eigen::Vector3d vt = scan_poses[i].m * v;
			glVertex3f(vt.x(), vt.y(), vt.z());
		}
	}
	glEnd();

	glColor3f(0,0,0);
	glBegin(GL_LINES);
	for(size_t i = 0 ; i < pairs_temp.size(); i++){
		pcl::PointXYZ p1 = scan_poses[0].pc[pairs_temp[i].first];
		pcl::PointXYZ p2 = scan_poses[1].pc[pairs_temp[i].second];

		Eigen::Vector3d v1(p1.x, p1.y, p1.z);
		Eigen::Vector3d v1t = scan_poses[0].m * v1;

		glVertex3f(v1t.x(), v1t.y(), v1t.z());

		Eigen::Vector3d v2(p2.x, p2.y, p2.z);
		Eigen::Vector3d v2t = scan_poses[1].m * v2;

		glVertex3f(v2t.x(), v2t.y(), v2t.z());

	}
	glEnd();

	if(show_ground_truth){
		glColor3f(0.7, 0.7, 0.7);
		glBegin(GL_POINTS);
		for(size_t i = 0; i < pc_ground_truth.size(); i++){
			glVertex3f(pc_ground_truth[i].x, pc_ground_truth[i].y, pc_ground_truth[i].z);
		}
		glEnd();
	}

	if(show_ndt_covariances){
		for(size_t i = 0 ; i < buckets_render.size(); i++){
			if(buckets_render[i].number_of_points > 10){
				draw_ellipse(buckets_render[i].cov, buckets_render[i].mean, Eigen::Vector3f(0.0, 0.0, 1.0), 1);
			}
		}
	}

	glutSwapBuffers();
}

void ndt_job(int i, Job* job, std::vector<Bucket>* buckets, Eigen::SparseMatrix<double > *AtPA,
		Eigen::SparseMatrix<double > *AtPB, std::vector<PointBucketIndexPair> *index_pair_internal, std::vector<Point3D> *pp,
		std::vector<TaitBryanPose> *poses, std::vector<Eigen::Affine3d> *mposes_inv, size_t trajectory_size) {

	std::vector<Eigen::Triplet<double>> tripletListA;
	std::vector<Eigen::Triplet<double>> tripletListP;
	std::vector<Eigen::Triplet<double>> tripletListB;

	for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) {
		Bucket& b = (*buckets)[ii];
		if (b.number_of_points < 5)continue;

		Eigen::Vector3d mean(0, 0, 0);
		Eigen::Matrix3d cov;
		cov.setZero();

		for (int index = b.index_begin; index < b.index_end; index++) {
			const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point];
			mean += Eigen::Vector3d(p.x, p.y, p.z);
		}
		mean /= b.number_of_points;

		for (int index = b.index_begin; index < b.index_end; index++) {
			const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point];
			cov(0, 0) += (mean.x() - p.x) * (mean.x() - p.x);
			cov(0, 1) += (mean.x() - p.x) * (mean.y() - p.y);
			cov(0, 2) += (mean.x() - p.x) * (mean.z() - p.z);
			cov(1, 0) += (mean.y() - p.y) * (mean.x() - p.x);
			cov(1, 1) += (mean.y() - p.y) * (mean.y() - p.y);
			cov(1, 2) += (mean.y() - p.y) * (mean.z() - p.z);
			cov(2, 0) += (mean.z() - p.z) * (mean.x() - p.x);
			cov(2, 1) += (mean.z() - p.z) * (mean.y() - p.y);
			cov(2, 2) += (mean.z() - p.z) * (mean.z() - p.z);
		}
		cov /= b.number_of_points;

		(*buckets)[ii].mean = mean;
		(*buckets)[ii].cov = cov;

		Eigen::Matrix3d infm = cov.inverse();

		if (!(infm(0, 0) == infm(0, 0)))continue;
		if (!(infm(0, 1) == infm(0, 1)))continue;
		if (!(infm(0, 2) == infm(0, 2)))continue;

		if (!(infm(1, 0) == infm(1, 0)))continue;
		if (!(infm(1, 1) == infm(1, 1)))continue;
		if (!(infm(1, 2) == infm(1, 2)))continue;

		if (!(infm(2, 0) == infm(2, 0)))continue;
		if (!(infm(2, 1) == infm(2, 1)))continue;
		if (!(infm(2, 2) == infm(2, 2)))continue;

		for (int index = b.index_begin; index < b.index_end; index++) {
			const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point];

			Eigen::Vector3d point_local(p.x, p.y, p.z);
			point_local = (*mposes_inv)[p.index_pose] * point_local;

			TaitBryanPose pose_s = (*poses)[p.index_pose];
			double delta_x;
			double delta_y;
			double delta_z;

			point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z,
				pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka,
				point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z());

			Eigen::Matrix<double, 3, 6, Eigen::RowMajor> jacobian;
			point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian,
				pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka,
				point_local.x(), point_local.y(), point_local.z());

			int ir = tripletListB.size();
			int c = p.index_pose * 6;

			for (int row = 0; row < 3; row++) {
				for (int col = 0; col < 6; col++) {
					if (jacobian(row, col) != 0.0) {
						tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col));
					}
				}
			}

			tripletListP.emplace_back(ir, ir, infm(0, 0));
			tripletListP.emplace_back(ir, ir + 1, infm(0, 1));
			tripletListP.emplace_back(ir, ir + 2, infm(0, 2));
			tripletListP.emplace_back(ir + 1, ir, infm(1, 0));
			tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1));
			tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2));
			tripletListP.emplace_back(ir + 2, ir, infm(2, 0));
			tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1));
			tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2));

			tripletListB.emplace_back(ir, 0, delta_x);
			tripletListB.emplace_back(ir + 1, 0, delta_y);
			tripletListB.emplace_back(ir + 2, 0, delta_z);
		}
	}

	Eigen::SparseMatrix<double> matA(tripletListB.size(), trajectory_size * 6);
	Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
	Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

	matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
	matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
	matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

	Eigen::SparseMatrix<double> AtPAt(trajectory_size * 6, trajectory_size * 6);
	Eigen::SparseMatrix<double> AtPBt(trajectory_size * 6, 1);

	{
		Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
		AtPAt = AtP * matA;
		AtPBt = AtP * matB;

		(*AtPA) = AtPAt;
		(*AtPB) = AtPBt;
	}
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case '-':{
			current_scan_index --;
			if(current_scan_index < 0)current_scan_index = 0;
			break;
		}
		case '=':{
			current_scan_index ++;
			if(current_scan_index >= scan_poses.size())current_scan_index = scan_poses.size() - 1;
			break;
		}
		case 'a':{
			TaitBryanPose pose;
			pose.px = 0;
			pose.py = -0.1;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = 0;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 'd':{
			TaitBryanPose pose;
			pose.px = 0;
			pose.py = 0.1;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = 0;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 'w':{
			TaitBryanPose pose;
			pose.px = 0.1;
			pose.py = 0;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = 0;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 's':{
			TaitBryanPose pose;
			pose.px = -0.1;
			pose.py = 0;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = 0;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 'z':{
			TaitBryanPose pose;
			pose.px = 0;
			pose.py = 0;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = -0.01;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 'x':{
			TaitBryanPose pose;
			pose.px = 0;
			pose.py = 0;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = 0.01;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 'p':{
			for(size_t i = 0; i < scan_poses.size(); i++){
				std::cout << "scan: " << i << std::endl;
				std::cout << scan_poses[i].m.matrix() << std::endl;
			}
			break;
		}
		case 'n':{
			pairs_temp = nns(scan_poses[0], scan_poses[1], sradius);
			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < scan_poses.size() ; i++){
				for(size_t j = i+1 ; j < scan_poses.size() ; j++){
					//if(i == j)continue;
					std::vector<std::pair<int,int>> nn = nns(scan_poses[i], scan_poses[j], sradius);
					std::cout << nn.size() << "," << scan_poses[i].pc.size() << "," << scan_poses[j].pc.size() << std::endl;

					TaitBryanPose pose_1 = pose_tait_bryan_from_affine_matrix(scan_poses[i].m);
					TaitBryanPose pose_2 = pose_tait_bryan_from_affine_matrix(scan_poses[j].m);

					for(size_t k = 0 ; k < nn.size(); k+=1){
						pcl::PointXYZ &p_1 = scan_poses[i].pc[nn[k].first];
						pcl::PointXYZ &p_2 = scan_poses[j].pc[nn[k].second];
						double delta_x;
						double delta_y;
						double delta_z;
						point_to_point_tait_bryan_wc(delta_x, delta_y, delta_z, pose_1.px, pose_1.py, pose_1.pz, pose_1.om, pose_1.fi, pose_1.ka, pose_2.px, pose_2.py, pose_2.pz, pose_2.om, pose_2.fi, pose_2.ka, p_1.x, p_1.y, p_1.z, p_2.x, p_2.y, p_2.z);

						Eigen::Matrix<double, 3, 12, Eigen::RowMajor> jacobian;
						point_to_point_tait_bryan_wc_jacobian(jacobian, pose_1.px, pose_1.py, pose_1.pz, pose_1.om, pose_1.fi, pose_1.ka, pose_2.px, pose_2.py, pose_2.pz, pose_2.om, pose_2.fi, pose_2.ka, p_1.x, p_1.y, p_1.z, p_2.x, p_2.y, p_2.z);

						int ir = tripletListB.size();
						int ic_1 = i * 6;
						int ic_2 = j * 6;

						tripletListA.emplace_back(ir     , ic_1    , -jacobian(0,0));
						tripletListA.emplace_back(ir     , ic_1 + 1, -jacobian(0,1));
						tripletListA.emplace_back(ir     , ic_1 + 2, -jacobian(0,2));
						tripletListA.emplace_back(ir     , ic_1 + 3, -jacobian(0,3));
						tripletListA.emplace_back(ir     , ic_1 + 4, -jacobian(0,4));
						tripletListA.emplace_back(ir     , ic_1 + 5, -jacobian(0,5));

						tripletListA.emplace_back(ir     , ic_2    , -jacobian(0,6));
						tripletListA.emplace_back(ir     , ic_2 + 1, -jacobian(0,7));
						tripletListA.emplace_back(ir     , ic_2 + 2, -jacobian(0,8));
						tripletListA.emplace_back(ir     , ic_2 + 3, -jacobian(0,9));
						tripletListA.emplace_back(ir     , ic_2 + 4, -jacobian(0,10));
						tripletListA.emplace_back(ir     , ic_2 + 5, -jacobian(0,11));

						tripletListA.emplace_back(ir + 1 , ic_1    , -jacobian(1,0));
						tripletListA.emplace_back(ir + 1 , ic_1 + 1, -jacobian(1,1));
						tripletListA.emplace_back(ir + 1 , ic_1 + 2, -jacobian(1,2));
						tripletListA.emplace_back(ir + 1 , ic_1 + 3, -jacobian(1,3));
						tripletListA.emplace_back(ir + 1 , ic_1 + 4, -jacobian(1,4));
						tripletListA.emplace_back(ir + 1 , ic_1 + 5, -jacobian(1,5));

						tripletListA.emplace_back(ir + 1 , ic_2    , -jacobian(1,6));
						tripletListA.emplace_back(ir + 1 , ic_2 + 1, -jacobian(1,7));
						tripletListA.emplace_back(ir + 1 , ic_2 + 2, -jacobian(1,8));
						tripletListA.emplace_back(ir + 1 , ic_2 + 3, -jacobian(1,9));
						tripletListA.emplace_back(ir + 1 , ic_2 + 4, -jacobian(1,10));
						tripletListA.emplace_back(ir + 1 , ic_2 + 5, -jacobian(1,11));

						tripletListA.emplace_back(ir + 2 , ic_1    , -jacobian(2,0));
						tripletListA.emplace_back(ir + 2 , ic_1 + 1, -jacobian(2,1));
						tripletListA.emplace_back(ir + 2 , ic_1 + 2, -jacobian(2,2));
						tripletListA.emplace_back(ir + 2 , ic_1 + 3, -jacobian(2,3));
						tripletListA.emplace_back(ir + 2 , ic_1 + 4, -jacobian(2,4));
						tripletListA.emplace_back(ir + 2 , ic_1 + 5, -jacobian(2,5));

						tripletListA.emplace_back(ir + 2 , ic_2    , -jacobian(2,6));
						tripletListA.emplace_back(ir + 2 , ic_2 + 1, -jacobian(2,7));
						tripletListA.emplace_back(ir + 2 , ic_2 + 2, -jacobian(2,8));
						tripletListA.emplace_back(ir + 2 , ic_2 + 3, -jacobian(2,9));
						tripletListA.emplace_back(ir + 2 , ic_2 + 4, -jacobian(2,10));
						tripletListA.emplace_back(ir + 2 , ic_2 + 5, -jacobian(2,11));

						tripletListP.emplace_back(ir    , ir    ,  1);//cauchy(delta_x, 1));
						tripletListP.emplace_back(ir + 1, ir + 1,  1);//cauchy(delta_y, 1));
						tripletListP.emplace_back(ir + 2, ir + 2,  1);//cauchy(delta_z, 1));

						tripletListB.emplace_back(ir    , 0,  delta_x);
						tripletListB.emplace_back(ir + 1, 0,  delta_y);
						tripletListB.emplace_back(ir + 2, 0,  delta_z);
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);

			tripletListP.emplace_back(ir     , ir,     1000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 1000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 1000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 1000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 1000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 1000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), scan_poses.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(scan_poses.size() * 6, scan_poses.size() * 6);
			Eigen::SparseMatrix<double> AtPB(scan_poses.size() * 6, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == scan_poses.size() * 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < scan_poses.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(scan_poses[i].m);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];

					scan_poses[i].m = affine_matrix_from_pose_tait_bryan(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'l':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < scan_poses.size() ; i++){
				Eigen::Affine3d pose_source = scan_poses[i].m;

				for(size_t j = 0 ; j < scan_poses.size() ; j++){
					if(i == j)continue;
					std::vector<std::pair<int,int>> nn = nns(scan_poses[i], scan_poses[j], sradius);
					std::cout << nn.size() << "," << scan_poses[i].pc.size() << "," << scan_poses[j].pc.size() << std::endl;

					for(size_t k = 0 ; k < nn.size(); k+=1){
						pcl::PointXYZ &p_1 = scan_poses[i].pc[nn[k].first];
						pcl::PointXYZ &p_2 = scan_poses[j].pc[nn[k].second];

						Eigen::Vector3d p_t(p_2.x, p_2.y, p_2.z);// = trajectory[j] * p_2;
						Eigen::Vector3d p_s(p_1.x, p_1.y, p_1.z);// = p_1;

						int ir = tripletListB.size();
						int ic_1 = i * 6;
						Eigen::Matrix3d px;
						px(0,0) = 0;
						px(0,1) = -p_s.z();
						px(0,2) =  p_s.y();
						px(1,0) = p_s.z();
						px(1,1) = 0;
						px(1,2) = -p_s.x();
						px(2,0) = -p_s.y();
						px(2,1) = p_s.x();
						px(2,2) = 0;

						Eigen::Matrix3d R = pose_source.inverse().rotation();
						Eigen::Matrix3d Rpx = R*px;

						tripletListA.emplace_back(ir     ,ic_1 + 0, R(0,0));
						tripletListA.emplace_back(ir     ,ic_1 + 1, R(0,1));
						tripletListA.emplace_back(ir     ,ic_1 + 2, R(0,2));
						tripletListA.emplace_back(ir     ,ic_1 + 3, -Rpx(0,0));
						tripletListA.emplace_back(ir     ,ic_1 + 4, -Rpx(0,1));
						tripletListA.emplace_back(ir     ,ic_1 + 5, -Rpx(0,2));

						tripletListA.emplace_back(ir + 1 ,ic_1 + 0, R(1,0));
						tripletListA.emplace_back(ir + 1 ,ic_1 + 1, R(1,1));
						tripletListA.emplace_back(ir + 1 ,ic_1 + 2, R(1,2));
						tripletListA.emplace_back(ir + 1 ,ic_1 + 3, -Rpx(1,0));
						tripletListA.emplace_back(ir + 1 ,ic_1 + 4, -Rpx(1,1));
						tripletListA.emplace_back(ir + 1 ,ic_1 + 5, -Rpx(1,2));

						tripletListA.emplace_back(ir + 2 ,ic_1 + 0, R(2,0));
						tripletListA.emplace_back(ir + 2 ,ic_1 + 1, R(2,1));
						tripletListA.emplace_back(ir + 2 ,ic_1 + 2, R(2,2));
						tripletListA.emplace_back(ir + 2 ,ic_1 + 3, -Rpx(2,0));
						tripletListA.emplace_back(ir + 2 ,ic_1 + 4, -Rpx(2,1));
						tripletListA.emplace_back(ir + 2 ,ic_1 + 5, -Rpx(2,2));

						tripletListP.emplace_back(ir    , ir    ,  1);
						tripletListP.emplace_back(ir + 1, ir + 1,  1);
						tripletListP.emplace_back(ir + 2, ir + 2,  1);

						Eigen::Vector3d target = scan_poses[i].m.inverse() * (scan_poses[j].m * p_t);

						tripletListB.emplace_back(ir    , 0,  -(target.x() - p_s.x()));
						tripletListB.emplace_back(ir + 1, 0,  -(target.y() - p_s.y()));
						tripletListB.emplace_back(ir + 2, 0,  -(target.z() - p_s.z()));
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);

			tripletListP.emplace_back(ir     , ir,     1000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 1000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 1000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 1000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 1000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 1000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), scan_poses.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(scan_poses.size() * 6, scan_poses.size() * 6);
			Eigen::SparseMatrix<double> AtPB(scan_poses.size() * 6, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == scan_poses.size() * 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < scan_poses.size(); i++){
					RodriguesPose pose_update;
					pose_update.px = h_x[counter++];
					pose_update.py = h_x[counter++];
					pose_update.pz = h_x[counter++];
					pose_update.sx = h_x[counter++];
					pose_update.sy = h_x[counter++];
					pose_update.sz = h_x[counter++];

					scan_poses[i].m = (scan_poses[i].m.inverse() * affine_matrix_from_pose_rodrigues(pose_update)).inverse();
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case '1':{
			sradius -= 0.01;
			std::cout << "sradius: " << sradius << std::endl;
			if(sradius < 0)sradius = 0.01;
			break;
		}
		case '2':{
			sradius += 0.01;
			std::cout << "sradius: " << sradius << std::endl;
			break;
		}
		case '3':{
			for(size_t i = 0; i < scan_poses.size(); i++){
				pcl::PointCloud<pcl::PointXYZ> pc;
				for(size_t j = 0; j < scan_poses[i].pc.size(); j++){
					Eigen::Vector3d v(scan_poses[i].pc[j].x, scan_poses[i].pc[j].y, scan_poses[i].pc[j].z);
					Eigen::Vector3d vt = scan_poses[i].m * v;
					pcl::PointXYZ p;
					p.x = vt.x();
					p.y = vt.y();
					p.z = vt.z();
					pc.push_back(p);
				}
				pcl::io::savePCDFileBinary(std::to_string(i) + ".pcd", pc);
				std::cout << "file: " << std::to_string(i) + ".pcd" << std::endl;
 			}
			break;
		}
		case 'g':{
			show_ground_truth =! show_ground_truth;
			break;
		}
		case 'o':{
			GridParameters rgd_params;
			rgd_params.resolution_X = sradius;
			rgd_params.resolution_Y = sradius;
			rgd_params.resolution_Z = sradius;
			rgd_params.bounding_box_extension = sradius;

			std::vector<Point3D> points_global;
			for(size_t i = 0; i < scan_poses.size(); i++){
				for(size_t j = 0; j < scan_poses[i].pc.size(); j++){
					Eigen::Vector3d v(scan_poses[i].pc[j].x, scan_poses[i].pc[j].y, scan_poses[i].pc[j].z);
					Eigen::Vector3d vt = scan_poses[i].m * v;
					Point3D p;
					p.x = vt.x();
					p.y = vt.y();
					p.z = vt.z();
					p.index_pose = i;
					points_global.push_back(p);
				}
			}

			std::vector<PointBucketIndexPair> index_pair;
			std::vector<Bucket> buckets;

			grid_calculate_params(points_global, rgd_params);
			build_rgd(points_global, index_pair, buckets, rgd_params, 8);

			std::vector<Job> jobs = get_jobs(buckets.size(), 8);

			std::vector<std::thread> threads;

			std::vector<Eigen::SparseMatrix<double>> AtPAtmp(jobs.size());
			std::vector<Eigen::SparseMatrix<double>> AtPBtmp(jobs.size());

			for (size_t i = 0; i < jobs.size(); i++) {
				AtPAtmp[i] = Eigen::SparseMatrix<double>(scan_poses.size() * 6, scan_poses.size() * 6);
				AtPBtmp[i] = Eigen::SparseMatrix<double>(scan_poses.size() * 6, 1);
			}

			std::vector<TaitBryanPose> poses;
			std::vector<Eigen::Affine3d> mposes_inv;
			for(size_t i = 0; i < scan_poses.size(); i++){
				poses.push_back(pose_tait_bryan_from_affine_matrix(scan_poses[i].m));
				mposes_inv.push_back(scan_poses[i].m.inverse());
			}

			for (size_t k = 0; k < jobs.size(); k++) {
				threads.push_back(std::thread(ndt_job, k, &jobs[k], &buckets, &(AtPAtmp[k]), &(AtPBtmp[k]), &index_pair, &points_global, &poses, &mposes_inv, scan_poses.size()));
			}

			for (size_t j = 0; j < threads.size(); j++) {
				threads[j].join();
			}

			bool init = false;
			Eigen::SparseMatrix<double> AtPA_ndt(scan_poses.size() * 6, scan_poses.size() * 6);
			Eigen::SparseMatrix<double> AtPB_ndt(scan_poses.size() * 6, 1);

			for (size_t k = 0; k < jobs.size(); k++) {
				if (!init) {
					if (AtPBtmp[k].size() > 0) {
						AtPA_ndt = AtPAtmp[k];
						AtPB_ndt = AtPBtmp[k];
						init = true;
					}
				}
				else {
					if (AtPBtmp[k].size() > 0) {

						AtPA_ndt += AtPAtmp[k];
						AtPB_ndt += AtPBtmp[k];
					}
				}
			}
			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA_ndt);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB_ndt);

			std::vector<double> h_x;

			for (int k = 0; k < x.outerSize(); ++k) {
				for (Eigen::SparseMatrix<double>::InnerIterator it(x, k); it; ++it) {
					if (it.value() == it.value()) {
						h_x.push_back(it.value());
						std::cout << it.value() << std::endl;
					}
				}
			}

			if(h_x.size() == scan_poses.size() * 6){
				buckets_render = buckets;
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				int counter = 0;
				for (size_t i = 0; i < scan_poses.size(); i++) {

					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(scan_poses[i].m);

					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];

					scan_poses[i].m = affine_matrix_from_pose_tait_bryan(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}

			break;
		}
		case '4':{
			show_ndt_covariances = !show_ndt_covariances;
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "-: current_scan_index--" << std::endl;
	std::cout << "=: current_scan_index++" << std::endl;
	std::cout << "awsdzx: move current_scan (green)" << std::endl;
	std::cout << "p: print poses" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "l: optimize (Lie algebra)" << std::endl;
	std::cout << "1: sradius -= 0.01" << std::endl;
	std::cout << "2: sradius += 0.01" << std::endl;
	std::cout << "3: save current point clouds" << std::endl;
	std::cout << "g: show_ground_truth =! show_ground_truth" << std::endl;
	std::cout << "o: optimize (NDT)" << std::endl;
	std::cout << "n: nns" << std::endl;
	std::cout << "4: show ndt covariances on/off" << std::endl;
}

void set_initial_guess(std::vector<ScanPose>& scan_poses){
	scan_poses[0].m(0,0) = 0.380925;
	scan_poses[0].m(0,1) = 0.924606;
	scan_poses[0].m(0,3) = -6.06303;

	scan_poses[0].m(1,0) = -0.924606;
	scan_poses[0].m(1,1) = 0.380925;
	scan_poses[0].m(1,3) = -8.2396;

	scan_poses[1].m(0,0) = 0.710913;
	scan_poses[1].m(0,1) = 0.70328;
	scan_poses[1].m(0,3) = -1.03452;

	scan_poses[1].m(1,0) = -0.70328;
	scan_poses[1].m(1,1) = 0.710913;
	scan_poses[1].m(1,3) = -7.77656;

	scan_poses[2].m(0,0) = 0.613745;
	scan_poses[2].m(0,1) = 0.789504;
	scan_poses[2].m(0,3) = 3.99851;

	scan_poses[2].m(1,0) = -0.789504;
	scan_poses[2].m(1,1) = 0.613745;
	scan_poses[2].m(1,3) = -8.18709;

	scan_poses[3].m(0,0) = 0.751805;
	scan_poses[3].m(0,1) = 0.659385;
	scan_poses[3].m(0,3) = 9.99224;

	scan_poses[3].m(1,0) = -0.659385;
	scan_poses[3].m(1,1) = 0.751805;
	scan_poses[3].m(1,3) = -8.54882;

	scan_poses[4].m(0,0) = -0.996673;
	scan_poses[4].m(0,1) = 0.0815022;
	scan_poses[4].m(0,3) = 12.6963;

	scan_poses[4].m(1,0) = -0.0815022;
	scan_poses[4].m(1,1) = -0.996673;
	scan_poses[4].m(1,3) = -8.73861;

	scan_poses[5].m(0,0) = 0.639602;
	scan_poses[5].m(0,1) = 0.768705;
	scan_poses[5].m(0,3) = 17.8592;

	scan_poses[5].m(1,0) = -0.768705;
	scan_poses[5].m(1,1) = 0.639602;
	scan_poses[5].m(1,3) = -2.78423;

	scan_poses[6].m(0,0) = 0.745174;
	scan_poses[6].m(0,1) = 0.66687;
	scan_poses[6].m(0,3) = 22.9195;

	scan_poses[6].m(1,0) = -0.66687;
	scan_poses[6].m(1,1) = 0.745174;
	scan_poses[6].m(1,3) = -1.98392;

	scan_poses[7].m(0,0) = -0.0491839;
	scan_poses[7].m(0,1) = 0.998789;
	scan_poses[7].m(0,3) = 31.7827;

	scan_poses[7].m(1,0) = -0.998789;
	scan_poses[7].m(1,1) = -0.0491839;
	scan_poses[7].m(1,3) = -2.2143;

	scan_poses[8].m(0,0) = -0.128844;
	scan_poses[8].m(0,1) = 0.991665;
	scan_poses[8].m(0,3) = 39.0272;

	scan_poses[8].m(1,0) = -0.991665;
	scan_poses[8].m(1,1) = -0.128844;
	scan_poses[8].m(1,3) = -2.29705;

	scan_poses[9].m(0,0) = -0.34215;
	scan_poses[9].m(0,1) = 0.939646;
	scan_poses[9].m(0,3) = 48.1018;

	scan_poses[9].m(1,0) = -0.939646;
	scan_poses[9].m(1,1) = -0.34215;
	scan_poses[9].m(1,3) = -1.94245;

	scan_poses[10].m(0,0) = -0.158532;
	scan_poses[10].m(0,1) = 0.987354;
	scan_poses[10].m(0,3) = 54.2044;

	scan_poses[10].m(1,0) = -0.987354;
	scan_poses[10].m(1,1) = -0.158532;
	scan_poses[10].m(1,3) = -7.96743;

	scan_poses[11].m(0,0) = -0.197888;
	scan_poses[11].m(0,1) = 0.980225;
	scan_poses[11].m(0,3) = 65.5777;

	scan_poses[11].m(1,0) = -0.980225;
	scan_poses[11].m(1,1) = -0.197888;
	scan_poses[11].m(1,3) = -8.39231;

	scan_poses[12].m(0,0) = -0.360872;
	scan_poses[12].m(0,1) = 0.932615;
	scan_poses[12].m(0,3) = 78.1712;

	scan_poses[12].m(1,0) = -0.932615;
	scan_poses[12].m(1,1) = -0.360872;
	scan_poses[12].m(1,3) = -7.76261;
}

std::vector<std::pair<int,int>> nns(ScanPose &sp1, ScanPose &sp2, float radius)
{
	pcl::PointCloud<pcl::PointXYZ> pc1;
	pcl::PointCloud<pcl::PointXYZ> pc2;

	for(size_t i = 0; i < sp1.pc.size(); i++){
		Eigen::Vector3d v(sp1.pc[i].x, sp1.pc[i].y, sp1.pc[i].z);
		Eigen::Vector3d vt = sp1.m * v;
		pc1.push_back(pcl::PointXYZ(vt.x(), vt.y(), vt.z()));
	}

	for(size_t i = 0 ; i < sp2.pc.size(); i++){
		Eigen::Vector3d v(sp2.pc[i].x, sp2.pc[i].y, sp2.pc[i].z);
		Eigen::Vector3d vt = sp2.m * v;
		pc2.push_back(pcl::PointXYZ(vt.x(), vt.y(), vt.z()));
	}

	std::vector<std::pair<int,int>> result;

	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	for(size_t i = 0; i < pc1.size(); i++){
		cloud->push_back(pc1[i]);
	}
	int K = 1;

	std::vector<int> pointIdxRadiusSearch;
	std::vector<float> pointRadiusSquaredDistance;

	kdtree.setInputCloud (cloud);
	for(size_t k = 0; k < pc2.size(); k++){
		if ( kdtree.radiusSearch (pc2[k], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
			for (std::size_t i = 0; i < pointIdxRadiusSearch.size (); ++i){
				result.emplace_back(pointIdxRadiusSearch[i], k);
				break;
			}
		}
	}

	return result;
}


/*
 *            Copyright 2009-2020 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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 "A_ol I_ol" BA_olI_ol,
 * WITHOUT WARRANTIE_ol OR CONDITION_ol OF ANY KIND, either express or implied.
 * _olee the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

// Third party includes
#include <boost/math/constants/constants.hpp>

// Local VOTCA includes
#include "votca/xtp/aobasis.h"
#include "votca/xtp/aomatrix.h"
#include "votca/xtp/qmmolecule.h"
#include "votca/xtp/radial_euler_maclaurin_rule.h"

namespace votca {
namespace xtp {

std::vector<double> EulerMaclaurinGrid::CalculatePruningIntervals(
    const std::string& element) {
  std::vector<double> r;
  // get Bragg-Slater Radius for this element
  double BSradius = _BraggSlaterRadii.at(element);
  // row type of element
  Index RowType = _pruning_set.at(element);

  if (RowType == 1) {
    r.push_back(0.25 * BSradius);
    r.push_back(0.5 * BSradius);
    r.push_back(1.0 * BSradius);
    r.push_back(4.5 * BSradius);
  } else if (RowType == 2) {
    r.push_back(0.1667 * BSradius);
    r.push_back(0.5 * BSradius);
    r.push_back(0.9 * BSradius);
    r.push_back(3.5 * BSradius);
  } else if (RowType == 3) {
    r.push_back(0.1 * BSradius);
    r.push_back(0.4 * BSradius);
    r.push_back(0.8 * BSradius);
    r.push_back(2.5 * BSradius);
  } else {
    throw std::runtime_error(
        "EulerMaclaurinGrid::CalculatePruningIntervals:Pruning unsupported for "
        "RowType");
  }
  return r;
}

void EulerMaclaurinGrid::FillElementRangeMap(const AOBasis& aobasis,
                                             const QMMolecule& atoms,
                                             double eps) {
  std::map<std::string, min_exp>::iterator it;
  for (const QMAtom& atom : atoms) {
    std::string name = atom.getElement();
    // is this element already in map?
    it = _element_ranges.find(name);
    // only proceed, if element data does not exist yet
    if (it == _element_ranges.end()) {
      min_exp this_atom;
      double range_max = std::numeric_limits<double>::min();
      double decaymin = std::numeric_limits<double>::max();
      Index lvalue = std::numeric_limits<Index>::min();
      const std::vector<const AOShell*> shells =
          aobasis.getShellsofAtom(atom.getId());
      // and loop over all shells to figure out minimum decay constant and
      // angular momentum of this function
      for (const AOShell* shell : shells) {
        Index lmax = Index(shell->getL());
        if (shell->getMinDecay() < decaymin) {
          decaymin = shell->getMinDecay();
          lvalue = lmax;
        }
        double range = DetermineCutoff(2 * decaymin, 2 * lvalue + 2, eps);
        if (range > range_max) {
          this_atom.alpha = decaymin;
          this_atom.l = lvalue;
          this_atom.range = range;
          range_max = range;
        }
      }  // shells
      _element_ranges[name] = this_atom;
    }  // new element
  }    // atoms
}

void EulerMaclaurinGrid::RefineElementRangeMap(const AOBasis& aobasis,
                                               const QMMolecule& atoms,
                                               double eps) {
  AOOverlap overlap;
  overlap.Fill(aobasis);

  // get collapsed index list
  std::vector<Index> idxstart;
  const std::vector<Index>& idxsize = aobasis.getFuncPerAtom();
  Index start = 0;
  for (Index size : idxsize) {
    idxstart.push_back(start);
    start += size;
  }
  // refining by going through all atom combinations
  for (Index i = 0; i < atoms.size(); ++i) {
    const QMAtom& atom_a = atoms[i];
    Index a_start = idxstart[i];
    Index a_size = idxsize[i];
    double range_max = std::numeric_limits<double>::min();
    // get preset values for this atom type
    double alpha_a = _element_ranges.at(atom_a.getElement()).alpha;
    Index l_a = _element_ranges.at(atom_a.getElement()).l;
    const Eigen::Vector3d& pos_a = atom_a.getPos();
    // Cannot iterate only over j<i because it is not symmetric due to shift_2g
    for (Index j = 0; j < atoms.size(); ++j) {
      if (i == j) {
        continue;
      }
      const QMAtom& atom_b = atoms[j];
      Index b_start = idxstart[j];
      Index b_size = idxsize[j];
      const Eigen::Vector3d& pos_b = atom_b.getPos();
      // find overlap block of these two atoms
      Eigen::MatrixXd overlapblock =
          overlap.Matrix().block(a_start, b_start, a_size, b_size);
      // determine abs max of this block
      double s_max = overlapblock.cwiseAbs().maxCoeff();

      if (s_max > 1e-5) {
        double range = DetermineCutoff(
            alpha_a + _element_ranges.at(atom_b.getElement()).alpha,
            l_a + _element_ranges.at(atom_b.getElement()).l + 2, eps);
        // now do some update trickery from Gaussian product formula
        double dist = (pos_b - pos_a).norm();
        double shift_2g =
            dist * alpha_a /
            (alpha_a + _element_ranges.at(atom_b.getElement()).alpha);
        range += (shift_2g + dist);
        if (range > range_max) {
          range_max = range;
        }
      }
    }
    if (std::round(range_max) > _element_ranges.at(atom_a.getElement()).range) {
      _element_ranges.at(atom_a.getElement()).range = std::round(range_max);
    }
  }
}

void EulerMaclaurinGrid::CalculateRadialCutoffs(const AOBasis& aobasis,
                                                const QMMolecule& atoms,
                                                const std::string& gridtype) {

  double eps = Accuracy[gridtype];
  FillElementRangeMap(aobasis, atoms, eps);
  RefineElementRangeMap(aobasis, atoms, eps);
  return;
}

std::map<std::string, GridContainers::radial_grid>
    EulerMaclaurinGrid::CalculateAtomicRadialGrids(const AOBasis& aobasis,
                                                   const QMMolecule& atoms,
                                                   const std::string& type) {

  CalculateRadialCutoffs(aobasis, atoms, type);
  std::map<std::string, GridContainers::radial_grid> result;
  for (const auto& element : _element_ranges) {
    result[element.first] = CalculateRadialGridforAtom(type, element);
  }
  return result;
}

GridContainers::radial_grid EulerMaclaurinGrid::CalculateRadialGridforAtom(
    const std::string& type, const std::pair<std::string, min_exp>& element) {
  GridContainers::radial_grid result;
  Index np = getGridParameters(element.first, type);
  double cutoff = element.second.range;
  result.radius = Eigen::VectorXd::Zero(np);
  result.weight = Eigen::VectorXd::Zero(np);
  double alpha =
      -cutoff /
      (log(1.0 - std::pow((1.0 + double(np)) / (2.0 + double(np)), 3)));
  double factor = 3.0 / (1.0 + double(np));

  for (Index i = 0; i < np; i++) {
    double q = double(i + 1) / (double(np) + 1.0);
    double r = -alpha * std::log(1.0 - std::pow(q, 3));
    double w = factor * alpha * r * r / (1.0 - std::pow(q, 3)) * std::pow(q, 2);
    result.radius[i] = r;
    result.weight[i] = w;
  }
  return result;
}

double EulerMaclaurinGrid::DetermineCutoff(double alpha, Index l, double eps) {
  // determine norm of function
  /* For a function f(r) = r^k*exp(-alpha*r^2) determine
     the radial distance r such that the fraction of the
     function norm that is neglected if the 3D volume
     integration is terminated at a distance r is less
     than or equal to eps. */

  double cutoff = 1.0;     // initial value
  double increment = 0.5;  // increment

  while (increment > 0.01) {
    double residual = CalcResidual(alpha, l, cutoff);
    if (residual > eps) {
      cutoff += increment;
    } else {
      cutoff -= increment;
      if (cutoff < 0.0) {
        cutoff = 0.0;
      }
      increment = 0.5 * increment;
      cutoff += increment;
    }
  }
  return cutoff;
}

double EulerMaclaurinGrid::CalcResidual(double alpha, Index l, double cutoff) {
  return RadialIntegral(alpha, l + 2, cutoff) /
         RadialIntegral(alpha, l + 2, 0.0);
}

double EulerMaclaurinGrid::RadialIntegral(double alpha, Index l,
                                          double cutoff) {
  const double pi = boost::math::constants::pi<double>();
  Index ilo = l % 2;
  double value = 0.0;
  double valexp;
  if (ilo == 0) {
    double expo = std::sqrt(alpha) * cutoff;
    if (expo <= 40.0) {
      value = 0.5 * std::sqrt(pi / alpha) * std::erfc(expo);
    }
  }
  double exponent = alpha * cutoff * cutoff;
  if (exponent > 500.0) {
    valexp = 0.0;
    value = 0.0;
  } else {
    valexp = std::exp(-exponent);
    value = valexp / 2.0 / alpha;
  }
  for (Index i = ilo + 2; i <= l; i += 2) {
    value = (double(i - 1) * value + std::pow(cutoff, i - 1) * valexp) / 2.0 /
            alpha;
  }
  return value;
}

Index EulerMaclaurinGrid::getGridParameters(const std::string& element,
                                            const std::string& type) {
  if (type == "medium") {
    return MediumGrid.at(element);
  } else if (type == "coarse") {
    return CoarseGrid.at(element);
  } else if (type == "xcoarse") {
    return XcoarseGrid.at(element);
  } else if (type == "fine") {
    return FineGrid.at(element);
  } else if (type == "xfine") {
    return XfineGrid.at(element);
  }
  throw std::runtime_error("Grid type " + type + " is not implemented");
  return -1;
}
}  // namespace xtp
}  // namespace votca


//  Copyright John Maddock 2007.
//  Use, modification and distribution are 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_MATH_ROUND_HPP
#define BOOST_MATH_ROUND_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/config.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

namespace boost{ namespace math{

template <class T, class Policy>
inline T round(const T& v, const Policy& pol)
{
   BOOST_MATH_STD_USING
   if(!(boost::math::isfinite)(v))
      return policies::raise_rounding_error("boost::math::round<%1%>(%1%)", 0, v, v, pol);
   return v < 0 ? static_cast<T>(ceil(v - 0.5f)) : static_cast<T>(floor(v + 0.5f));
}
template <class T>
inline T round(const T& v)
{
   return round(v, policies::policy<>());
}
//
// The following functions will not compile unless T has an
// implicit convertion to the integer types.  For user-defined
// number types this will likely not be the case.  In that case
// these functions should either be specialized for the UDT in
// question, or else overloads should be placed in the same
// namespace as the UDT: these will then be found via argument
// dependent lookup.  See our concept archetypes for examples.
//
template <class T, class Policy>
inline int iround(const T& v, const Policy& pol)
{
   BOOST_MATH_STD_USING
   T r = boost::math::round(v, pol);
   if(fabs(r) > (std::numeric_limits<int>::max)())
      return static_cast<int>(policies::raise_rounding_error("boost::math::iround<%1%>(%1%)", 0, v, 0, pol));
   return static_cast<int>(r);
}
template <class T>
inline int iround(const T& v)
{
   return iround(v, policies::policy<>());
}

template <class T, class Policy>
inline long lround(const T& v, const Policy& pol)
{
   BOOST_MATH_STD_USING
   T r = boost::math::round(v, pol);
   if(fabs(r) > (std::numeric_limits<long>::max)())
      return static_cast<long int>(policies::raise_rounding_error("boost::math::lround<%1%>(%1%)", 0, v, 0L, pol));
   return static_cast<long int>(r);
}
template <class T>
inline long lround(const T& v)
{
   return lround(v, policies::policy<>());
}

#ifdef BOOST_HAS_LONG_LONG

template <class T, class Policy>
inline boost::long_long_type llround(const T& v, const Policy& pol)
{
   BOOST_MATH_STD_USING
   T r = boost::math::round(v, pol);
   if(fabs(r) > (std::numeric_limits<boost::long_long_type>::max)())
      return static_cast<boost::long_long_type>(policies::raise_rounding_error("boost::math::llround<%1%>(%1%)", 0, v, 0LL, pol));
   return static_cast<boost::long_long_type>(r);
}
template <class T>
inline boost::long_long_type llround(const T& v)
{
   return llround(v, policies::policy<>());
}

#endif

}} // namespaces

#endif // BOOST_MATH_ROUND_HPP


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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 "NewtonEulerDS.hpp"
#include "BlockVector.hpp"
#include "BlockMatrix.hpp"
#include <boost/math/quaternion.hpp>

#include <iostream>
// #define DEBUG_NOCOLOR
//#define DEBUG_BEGIN_END_ONLY
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include <debug.h>

void computeMObjToAbs(SP::SiconosVector q, SP::SimpleMatrix mObjToAbs)
{
  DEBUG_BEGIN("computeMObjToAbs(SP::SiconosVector q, SP::SimpleMatrix mObjToAbs)\n");
  double q0 = q->getValue(3);
  double q1 = q->getValue(4);
  double q2 = q->getValue(5);
  double q3 = q->getValue(6);

  ::boost::math::quaternion<double>    quatQ(q0, q1, q2, q3);
  ::boost::math::quaternion<double>    quatcQ(q0, -q1, -q2, -q3);
  ::boost::math::quaternion<double>    quatx(0, 1, 0, 0);
  ::boost::math::quaternion<double>    quaty(0, 0, 1, 0);
  ::boost::math::quaternion<double>    quatz(0, 0, 0, 1);
  ::boost::math::quaternion<double>    quatBuff;
  /*See equation with label eq:newton_Mobjtoabs from the DevNote.pdf
   * Chapter gradient computation, case of NewtonEuler formulation
   * with quaternion
   */
  quatBuff = quatcQ * quatx * quatQ;
  mObjToAbs->setValue(0, 0, quatBuff.R_component_2());
  mObjToAbs->setValue(0, 1, quatBuff.R_component_3());
  mObjToAbs->setValue(0, 2, quatBuff.R_component_4());
  quatBuff = quatcQ * quaty * quatQ;
  mObjToAbs->setValue(1, 0, quatBuff.R_component_2());
  mObjToAbs->setValue(1, 1, quatBuff.R_component_3());
  mObjToAbs->setValue(1, 2, quatBuff.R_component_4());
  quatBuff = quatcQ * quatz * quatQ;
  mObjToAbs->setValue(2, 0, quatBuff.R_component_2());
  mObjToAbs->setValue(2, 1, quatBuff.R_component_3());
  mObjToAbs->setValue(2, 2, quatBuff.R_component_4());
  DEBUG_END("computeMObjToAbs(SP::SiconosVector q, SP::SimpleMatrix mObjToAbs)\n");

}

void computeT(SP::SiconosVector q, SP::SimpleMatrix T)
{
  DEBUG_BEGIN("computeT(SP::SiconosVector q, SP::SimpleMatrix T)\n")
  //  std::cout <<"\n NewtonEulerDS::computeT(SP::SiconosVector q)\n  " <<std::endl;
  double q0 = q->getValue(3) / 2.0;
  double q1 = q->getValue(4) / 2.0;
  double q2 = q->getValue(5) / 2.0;
  double q3 = q->getValue(6) / 2.0;
  T->setValue(3, 3, -q1);
  T->setValue(3, 4, -q2);
  T->setValue(3, 5, -q3);
  T->setValue(4, 3, q0);
  T->setValue(4, 4, -q3);
  T->setValue(4, 5, q2);
  T->setValue(5, 3, q3);
  T->setValue(5, 4, q0);
  T->setValue(5, 5, -q1);
  T->setValue(6, 3, -q2);
  T->setValue(6, 4, q1);
  T->setValue(6, 5, q0);
  DEBUG_END("computeT(SP::SiconosVector q, SP::SimpleMatrix T)\n")

}

// Private function to set linked with members of Dynamical top class
void NewtonEulerDS::connectToDS()
{
  // dim
  _n = 2 * 3;

}

// From a set of data; Mass filled-in directly from a siconosMatrix -
// This constructor leads to the minimum NewtonEuler System form: \f$ M\ddot q = p \f$
/*
Q0 : contains the center of mass coordinate, and the quaternion initial. (dim(Q0)=7)
Velocity0 : contains the initial velocity of center of mass and the omega initial. (dim(Velocity0)=6)
*/
NewtonEulerDS::NewtonEulerDS(): DynamicalSystem(6),
                                _computeJacobianFIntqByFD(false),
                                _computeJacobianFIntvByFD(false),
                                _computeJacobianMIntqByFD(false),
                                _computeJacobianMIntvByFD(false),
                                _epsilonFD(sqrt(std::numeric_limits< double >::epsilon()))
{
  _p.resize(3);
  _p[0].reset(new SiconosVector());
  _p[1].reset(new SiconosVector(_n)); // Needed in NewtonEulerR
  _p[2].reset(new SiconosVector());
  zeroPlugin();
  //assert(0);

  // --- NEWTONEULER INHERITED CLASS MEMBERS ---
  // -- Memory allocation for vector and matrix members --

  _qDim = 7;
  _n = 6;

  // Current state
  _q.reset(new SiconosVector(_qDim));
  // _deltaq.reset(new SiconosVector(_qDim));
  _v.reset(new SiconosVector(_n));

  _dotq.reset(new SiconosVector(_qDim));
  _workspace[freeresidu].reset(new SiconosVector(_n));
  _workspace[free].reset(new SiconosVector(dimension()));
  _massMatrix.reset(new SimpleMatrix(_n, _n));
  _luW.reset(new SimpleMatrix(_n, _n));
  _massMatrix->zero();
  _T.reset(new SimpleMatrix(_qDim, _n));

  _scalarMass = 0.;
}

void NewtonEulerDS::internalInit(SP::SiconosVector Q0, SP::SiconosVector Velocity0,
                                 double mass , SP::SiconosMatrix inertialMatrix)
{
  DEBUG_BEGIN("NewtonEulerDS::internalInit(SP::SiconosVector Q0, SP::SiconosVector Velocity0, double mass , SP::SiconosMatrix inertialMatrix)\n");
  _p.resize(3);
  _p[0].reset(new SiconosVector());
  _p[1].reset(new SiconosVector(_n)); // Needed in NewtonEulerR
  _p[2].reset(new SiconosVector());
  zeroPlugin();
  // --- NEWTONEULER INHERITED CLASS MEMBERS ---
  // -- Memory allocation for vector and matrix members --

  _scalarMass = mass;
  _qDim = 7;
  _n = 6;

  // Initial conditions
  _q0 = Q0;
  _v0 = Velocity0;

  _MObjToAbs.reset(new SimpleMatrix(3, 3));

  // Current state
  _q.reset(new SiconosVector(_qDim));
  // _deltaq.reset(new SiconosVector(_qDim));
  _v.reset(new SiconosVector(_n));
  (*_q) = (*_q0);
  _dotq.reset(new SiconosVector(_qDim));
  _massMatrix.reset(new SimpleMatrix(_n, _n));
  _jacobianFGyrv.reset(new SimpleMatrix(_n, _n));
  _luW.reset(new SimpleMatrix(_n, _n));
  _massMatrix->zero();
  _massMatrix->setValue(0, 0, _scalarMass);
  _massMatrix->setValue(1, 1, _scalarMass);
  _massMatrix->setValue(2, 2, _scalarMass);
  _I = inertialMatrix;
  Index dimIndex(2);
  dimIndex[0] = 3;
  dimIndex[1] = 3;
  Index startIndex(4);
  startIndex[0] = 0;
  startIndex[1] = 0;
  startIndex[2] = 3;
  startIndex[3] = 3;
  setBlock(_I, _massMatrix, dimIndex, startIndex);
  _workspace[freeresidu].reset(new SiconosVector(_n));
  _workspace[free].reset(new SiconosVector(dimension()));

  _T.reset(new SimpleMatrix(_qDim, _n));
  _T->zero();
  _T->setValue(0, 0, 1.0);
  _T->setValue(1, 1, 1.0);
  _T->setValue(2, 2, 1.0);
  computeT();
  computeMObjToAbs();
  initForces();
  DEBUG_END("NewtonEulerDS::internalInit(SP::SiconosVector Q0, SP::SiconosVector Velocity0, double mass , SP::SiconosMatrix inertialMatrix)\n");
}
NewtonEulerDS::NewtonEulerDS(SP::SiconosVector Q0, SP::SiconosVector Velocity0,
                             double  mass, SP::SiconosMatrix inertialMatrix):
  DynamicalSystem(6),
  _computeJacobianFIntqByFD(false),
  _computeJacobianFIntvByFD(false),
  _computeJacobianMIntqByFD(false),
  _computeJacobianMIntvByFD(false),
  _epsilonFD(sqrt(std::numeric_limits< double >::epsilon()))
{
  internalInit(Q0, Velocity0, mass, inertialMatrix);
}

void NewtonEulerDS::zeroPlugin()
{
  _pluginFExt.reset(new PluggedObject());
  _pluginMExt.reset(new PluggedObject());
  _pluginFInt.reset(new PluggedObject());
  _pluginMInt.reset(new PluggedObject());
  _pluginJacqFInt.reset(new PluggedObject());
  _pluginJacvFInt.reset(new PluggedObject());
  _pluginJacqMInt.reset(new PluggedObject());
  _pluginJacvMInt.reset(new PluggedObject());
}

// Destructor
NewtonEulerDS::~NewtonEulerDS()
{
}

bool NewtonEulerDS::checkDynamicalSystem()
{
  bool output = true;
  // ndof

  // q0 and velocity0
  if (! _q0 || ! _v0)
  {
    RuntimeException::selfThrow("NewtonEulerDS::checkDynamicalSystem - initial conditions are badly set.");
    output = false;
  }

  // fInt
  //   if( ( _fInt && computeFIntPtr) && ( ! _jacobianFIntq || ! _jacobianFIntv ) )
  //     // ie if fInt is defined and not constant => its Jacobian must be defined (but not necessarily plugged)
  //     {
  //       RuntimeException::selfThrow("NewtonEulerDS::checkDynamicalSystem - You defined fInt but not its Jacobian (according to q and velocity).");
  //       output = false;
  //     }

  if (!output) std::cout << "NewtonEulerDS Warning: your dynamical system seems to be uncomplete (check = false)" <<std::endl;
  return output;
}

void NewtonEulerDS::initializeNonSmoothInput(unsigned int level)
{
  DEBUG_PRINTF("NewtonEulerDS::initializeNonSmoothInput(unsigned int level) for level = %i\n",level);

  if (_p[level]->size() == 0)
  {
    if (level == 0)
    {
      _p[0]->resize(_qDim);
    }
    else
    {
      _p[level]->resize(_n);
    }
  }

#ifdef DEBUG_MESSAGES
  DEBUG_PRINT("display() after initialization");
  display();
#endif
}

void NewtonEulerDS::initForces()
{
  DEBUG_BEGIN("NewtonEulerDS::initForces()\n")
  _forces.reset(new SiconosVector(_n));
  _fGyr.reset(new SiconosVector(3,0.0));
  _jacobianFGyrv.reset(new SimpleMatrix(_n, _n));
  _jacobianvForces.reset(new SimpleMatrix(_n, _n));
  DEBUG_END("NewtonEulerDS::initForces()\n")
}

void NewtonEulerDS::initRhs(double time)
{
  //  _workMatrix.resize(sizeWorkMat);

  // Solve Mq[2]=fL+p.
  //*_q = *(_p[2]); // Warning: r/p update is done in Interactions/Relations

  if (_forces)
  {
    computeForces(time);
    //      *_q += *_forces;
  }

}

void NewtonEulerDS::initialize(double time, unsigned int sizeOfMemory)
{
  // set q and q[1] to q0 and velocity0, initialize acceleration.
  *_q = *_q0;
  *_v = *_v0;

  // If z has not been set, we initialize it with a null vector of size 1, since z is required in plug-in functions call.
  if (! _z)
    _z.reset(new SiconosVector(1));

  if (_pluginFExt->fPtr && !_fExt)
    _fExt.reset(new SiconosVector(3, 0));

  if (_pluginMExt->fPtr && !_mExt)
    _mExt.reset(new SiconosVector(3, 0));

  if (_pluginFInt->fPtr && !_fInt)
    _fInt.reset(new SiconosVector(3, 0));

  if ((_pluginJacqFInt->fPtr  || _computeJacobianFIntqByFD) && !_jacobianFIntq)
  {
    _jacobianFIntq.reset(new SimpleMatrix(3, _qDim));
    if (!_jacobianqForces)
      _jacobianqForces.reset(new SimpleMatrix(_n, _qDim));
  }

  if ((_pluginJacvFInt->fPtr || _computeJacobianFIntvByFD) && !_jacobianFIntv)
    _jacobianFIntv.reset(new SimpleMatrix(3, _n));

  if (_pluginMInt->fPtr && !_mInt)
    _mInt.reset(new SiconosVector(3, 0));

  if ((_pluginJacqMInt->fPtr || _computeJacobianMIntqByFD) && !_jacobianMIntq)
  {
    if (!_jacobianqForces)
      _jacobianqForces.reset(new SimpleMatrix(_n, _qDim));
    _jacobianMIntq.reset(new SimpleMatrix(3, _qDim));
  }
  if ((_pluginJacvMInt->fPtr || _computeJacobianMIntvByFD) && !_jacobianMIntv)
    _jacobianMIntv.reset(new SimpleMatrix(3, _n));

  // Set links to variables of top-class DynamicalSystem.
  // Warning: this results only in pointers links.
  // No more memory allocation for vectors or matrices.
  connectToDS(); // note that connection can not be done during constructor call, since user can complete the ds after (add plugin or anything else).
  checkDynamicalSystem();

  initRhs(time);

  if (_boundaryConditions)
  {
    _reactionToBoundaryConditions.reset(new SiconosVector(_boundaryConditions->velocityIndices()->size()));
  }

  // Initialize memory vectors
  initMemory(sizeOfMemory);

}

void NewtonEulerDS::computeFExt(double time)
{
  if (_pluginFExt->fPtr)
    ((FExt_NE)_pluginFExt->fPtr)(time, &(*_fExt)(0), _qDim, &(*_q0)(0) ); // parameter z are assumed to be equal to q0
}

void NewtonEulerDS::computeMExt(double time)
{
  if (_pluginMExt->fPtr)
    ((FExt_NE)_pluginFExt->fPtr)(time, &(*_mExt)(0), _qDim, &(*_q0)(0) ); // parameter z are assumed to be equal to q0
}

void NewtonEulerDS::computeFInt(double time)
{
  computeFInt(time, _q, _v);
}

void NewtonEulerDS::computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v)
{
  computeFInt(time,  q,  v, _fInt);
}

void NewtonEulerDS::computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector fInt)
{
  if (_pluginFInt->fPtr)
    ((FInt_NE)_pluginFInt->fPtr)(time, &(*q)(0), &(*v)(0), &(*fInt)(0), _qDim,  &(*_q0)(0));// parameter z are assumed to be equal to q0
}

void NewtonEulerDS::computeMInt(double time)
{
  computeMInt(time, _q, _v);
}

void NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v)
{
  computeMInt(time, q, v, _mInt);
}

void NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector mInt)
{
  if (_pluginMInt->fPtr)
    ((FInt_NE)_pluginMInt->fPtr)(time, &(*q)(0), &(*v)(0), &(*mInt)(0), _qDim,  &(*_q0)(0));// parameter z are assumed to be equal to q0
}

void NewtonEulerDS::computeJacobianFIntq(double time)
{
  computeJacobianFIntq(time, _q, _v);
}
void NewtonEulerDS::computeJacobianFIntv(double time)
{
  computeJacobianFIntv(time, _q, _v);
}

void NewtonEulerDS::computeJacobianFIntq(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianFIntq(...) starts");
  if (_pluginJacqFInt->fPtr)
    ((FInt_NE)_pluginJacqFInt->fPtr)(time, &(*q)(0), &(*velocity)(0), &(*_jacobianFIntq)(0, 0), _qDim,  &(*_q0)(0));
  else if (_computeJacobianFIntqByFD)
    computeJacobianFIntqByFD(time, q, velocity);
  DEBUG_EXPR(_jacobianFIntq->display(););
  DEBUG_END("NewtonEulerDS::computeJacobianFIntq(...)");
}

void NewtonEulerDS::computeJacobianFIntqByFD(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianFIntqByFD(...)\n");
  SP::SiconosVector fInt(new SiconosVector(3));
  computeFInt(time, q, velocity, fInt);

  double fInt0 = fInt->getValue(0);
  double fInt1 = fInt->getValue(1);
  double fInt2 = fInt->getValue(2);

  SP::SiconosVector qeps(new SiconosVector(*q));
  _jacobianFIntq->zero();
  (*qeps)(0) += _epsilonFD;
  for (int j =0; j < 7; j++)
  {
    computeFInt(time, qeps, velocity, fInt);
    _jacobianFIntq->setValue(0,j,  (fInt->getValue(0) - fInt0)/_epsilonFD );
    _jacobianFIntq->setValue(1,j,  (fInt->getValue(1) - fInt1)/_epsilonFD );
    _jacobianFIntq->setValue(2,j,  (fInt->getValue(2) - fInt2)/_epsilonFD );
    (*qeps)(j) -= _epsilonFD;
    if (j<6) (*qeps)(j+1) += _epsilonFD;
  }
  DEBUG_END("NewtonEulerDS::computeJacobianFIntqByFD(...)\n");

}

void NewtonEulerDS::computeJacobianFIntv(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  if (_pluginJacvFInt->fPtr)
    ((FInt_NE)_pluginJacvFInt->fPtr)(time, &(*q)(0), &(*velocity)(0), &(*_jacobianFIntv)(0, 0), _qDim,  &(*_q0)(0));
  else if (_computeJacobianFIntvByFD)
    computeJacobianFIntvByFD(time, q, velocity);
}

void NewtonEulerDS::computeJacobianFIntvByFD(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianFIntvByFD(...)\n");
  SP::SiconosVector fInt(new SiconosVector(3));
  computeFInt(time, q, velocity, fInt);

  double fInt0 = fInt->getValue(0);
  double fInt1 = fInt->getValue(1);
  double fInt2 = fInt->getValue(2);

  SP::SiconosVector veps(new SiconosVector(*velocity));
  _jacobianFIntv->zero();

  (*veps)(0) += _epsilonFD;
  for (int j =0; j < 6; j++)
  {
    computeFInt(time, q, veps, fInt);
    _jacobianFIntv->setValue(0,j,  (fInt->getValue(0) - fInt0)/_epsilonFD );
    _jacobianFIntv->setValue(1,j,  (fInt->getValue(1) - fInt1)/_epsilonFD );
    _jacobianFIntv->setValue(2,j,  (fInt->getValue(2) - fInt2)/_epsilonFD );
    (*veps)(j) -= _epsilonFD;
    if (j<5) (*veps)(j+1) += _epsilonFD;
  }

  DEBUG_END("NewtonEulerDS::computeJacobianFIntvByFD(...)\n");

}
void NewtonEulerDS::computeJacobianFGyrvByFD(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianFGyrvByFD(...)\n");
  SP::SiconosVector fGyr(new SiconosVector(3));
  computeFGyr(velocity, fGyr);

  double fGyr0 = fGyr->getValue(0);
  double fGyr1 = fGyr->getValue(1);
  double fGyr2 = fGyr->getValue(2);

  SP::SiconosVector veps(new SiconosVector(*velocity));
  _jacobianFGyrv->zero();

  (*veps)(0) += _epsilonFD;
  for (int j =0; j < 6; j++)
  {
    computeFGyr(veps, fGyr);
    _jacobianFGyrv->setValue(3,j,  (fGyr->getValue(0) - fGyr0)/_epsilonFD );
    _jacobianFGyrv->setValue(4,j,  (fGyr->getValue(1) - fGyr1)/_epsilonFD );
    _jacobianFGyrv->setValue(5,j,  (fGyr->getValue(2) - fGyr2)/_epsilonFD );
    (*veps)(j) -= _epsilonFD;
    if (j<5) (*veps)(j+1) += _epsilonFD;
  }
  DEBUG_EXPR(_jacobianFGyrv->display());
  DEBUG_END("NewtonEulerDS::computeJacobianFGyrvByFD(...)\n");

}
void NewtonEulerDS::computeJacobianMIntq(double time)
{
  computeJacobianMIntq(time, _q, _v);
}
void NewtonEulerDS::computeJacobianMIntv(double time)
{
  computeJacobianMIntv(time, _q, _v);
}

void NewtonEulerDS::computeJacobianMIntq(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntq(...) starts");
  if (_pluginJacqMInt->fPtr)
    ((FInt_NE)_pluginJacqMInt->fPtr)(time, &(*q)(0), &(*velocity)(0), &(*_jacobianMIntq)(0, 0), _qDim,  &(*_q0)(0));
  else if (_computeJacobianMIntqByFD)
    computeJacobianMIntqByFD(time, q, velocity);
  DEBUG_EXPR(_jacobianMIntq->display());
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntq(...) ends");

}

void NewtonEulerDS::computeJacobianMIntqByFD(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntqByFD(...) starts\n");

  SP::SiconosVector mInt(new SiconosVector(3));
  computeMInt(time, q, velocity, mInt);
  double mInt0 = mInt->getValue(0);
  double mInt1 = mInt->getValue(1);
  double mInt2 = mInt->getValue(2);

  SP::SiconosVector qeps(new SiconosVector(*q));

  (*qeps)(0) += _epsilonFD;
  for (int j =0; j < 7; j++)
  {
    computeMInt(time, qeps, velocity, mInt);
    _jacobianMIntq->setValue(0,j,  (mInt->getValue(0) - mInt0)/_epsilonFD );
    _jacobianMIntq->setValue(1,j,  (mInt->getValue(1) - mInt1)/_epsilonFD );
    _jacobianMIntq->setValue(2,j,  (mInt->getValue(2) - mInt2)/_epsilonFD );
    (*qeps)(j) -= _epsilonFD;
    if (j<6) (*qeps)(j+1) += _epsilonFD;
  }
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntqByFD(...) ends\n");
}

void NewtonEulerDS::computeJacobianMIntv(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  if (_pluginJacvMInt->fPtr)
    ((FInt_NE)_pluginJacvMInt->fPtr)(time, &(*q)(0), &(*velocity)(0), &(*_jacobianMIntv)(0, 0), _qDim,  &(*_q0)(0));
  else if (_computeJacobianMIntvByFD)
    computeJacobianMIntvByFD(time,  q, velocity);
}

void NewtonEulerDS::computeJacobianMIntvByFD(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntvByFD(...) starts\n");

  SP::SiconosVector mInt(new SiconosVector(3));
  computeMInt(time, q, velocity, mInt);
  double mInt0 = mInt->getValue(0);
  double mInt1 = mInt->getValue(1);
  double mInt2 = mInt->getValue(2);

  SP::SiconosVector veps(new SiconosVector(*velocity));

  (*veps)(0) += _epsilonFD;
  for (int j =0; j < 6; j++)
  {
    computeMInt(time, q, veps, mInt);
    _jacobianMIntv->setValue(0,j,  (mInt->getValue(0) - mInt0)/_epsilonFD );
    _jacobianMIntv->setValue(1,j,  (mInt->getValue(1) - mInt1)/_epsilonFD );
    _jacobianMIntv->setValue(2,j,  (mInt->getValue(2) - mInt2)/_epsilonFD );
    (*veps)(j) -= _epsilonFD;
    if (j<5) (*veps)(j+1) += _epsilonFD;
  }
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntvByFD(...) ends\n");
}

void NewtonEulerDS::computeRhs(double time, bool isDSup)
{
  // if isDSup == true, this means that there is no need to re-compute mass ...
  //  *_q = *(_p[2]); // Warning: r/p update is done in Interactions/Relations
  if (_forces)
  {
    computeForces(time);
    //*_q += *_forces;
  }
}

void NewtonEulerDS::computeJacobianRhsx(double time, bool isDSup)
{
  RuntimeException::selfThrow("NewtonEulerDS::computeJacobianRhsx - not yet implemented.");
}

void NewtonEulerDS::computeForces(double time)
{
  computeForces(time, _q, _v);
}

void NewtonEulerDS::computeFGyr(SP::SiconosVector v, SP::SiconosVector fGyr)
{
  /*computation of \Omega times I \Omega*/
  DEBUG_BEGIN("NewtonEulerDS::computeFGyr(SP::SiconosVector v, SP::SiconosVector fGyr)\n");
  if (_I)
  {
    DEBUG_EXPR( _I->display());
    DEBUG_EXPR( v->display());
    SiconosVector bufOmega(3);
    SiconosVector bufIOmega(3);
    bufOmega.setValue(0, v->getValue(3));
    bufOmega.setValue(1, v->getValue(4));
    bufOmega.setValue(2, v->getValue(5));
    prod(*_I, bufOmega, bufIOmega, true);
    cross_product(bufOmega, bufIOmega, *fGyr);
  }
  DEBUG_EXPR(fGyr->display());
  DEBUG_END("NewtonEulerDS::computeFGyr(SP::SiconosVector v, SP::SiconosVector fGyr)\n");

}
void NewtonEulerDS::computeFGyr(SP::SiconosVector v)
{
  /*computation of \Omega times I \Omega*/
  //DEBUG_BEGIN("NewtonEulerDS::computeFGyr(SP::SiconosVector v)\n");
  computeFGyr( v, _fGyr);
  //DEBUG_END("NewtonEulerDS::computeFGyr(SP::SiconosVector v)\n");

}

void NewtonEulerDS::computeForces(double time, SP::SiconosVector q, SP::SiconosVector v)
{
  // Warning: an operator (fInt ...) may be set (ie allocated and not NULL) but not plugged, that's why two steps are required here.
  if (_forces)
  {
    _forces->zero();
    // 1 - Computes the required functions
    if (_fExt)
    {
      computeFExt(time);
      _forces->setBlock(0, *_fExt);
    }
    if (_mExt)
    {
      computeMExt(time);
      SiconosVector aux(3);
      //computeMObjToAbs();
      prod( *_mExt, *_MObjToAbs, aux); // aux =  transpose(_MObjToAbs) * _mext
      *_mExt = aux;
      _forces->setBlock(3, *_mExt);
    }
    if (_fInt)
    {
      computeFInt(time, q, v);
      // std::cout << "_fInt : "<< std::endl;
      // _fInt->display();
      _forces->setValue(0, _forces->getValue(0) - _fInt->getValue(0));
      _forces->setValue(1, _forces->getValue(1) - _fInt->getValue(1));
      _forces->setValue(2, _forces->getValue(2) - _fInt->getValue(2));

    }
    if (_mInt)
    {
      computeMInt(time, q , v);
      SiconosVector aux(3);
      //computeMObjToAbs();
      prod(*_mInt, *_MObjToAbs, aux);// aux =  transpose(_MObjToAbs) * _mInt
      *_mInt = aux;
      // std::cout << "_MObjToAbs " <<std::endl;
      // _MObjToAbs->display();
      //std::cout << "NewtonEulerDS::computeForces: _mint: " <<std::endl;
      //_mInt->display();
      _forces->setValue(3, _forces->getValue(3) - _mInt->getValue(0));
      _forces->setValue(4, _forces->getValue(4) - _mInt->getValue(1));
      _forces->setValue(5, _forces->getValue(5) - _mInt->getValue(2));
    }

    computeFGyr(v);
    // std::cout << "_fGyr " <<std::endl;
    // _fGyr->display();
    _forces->setValue(3, _forces->getValue(3) - _fGyr->getValue(0));
    _forces->setValue(4, _forces->getValue(4) - _fGyr->getValue(1));
    _forces->setValue(5, _forces->getValue(5) - _fGyr->getValue(2));

    // std::cout << "_forces : "<< std::endl;
    // _forces->display();
  }
  // else nothing.
}

void NewtonEulerDS::computeJacobianqForces(double time)
{
  if (_jacobianqForces)
  {
    _jacobianqForces->zero();
    if (_jacobianFIntq)
    {
      computeJacobianFIntq(time);
      _jacobianqForces->setBlock(0,0,-1.0 * *_jacobianFIntq);
    }
    if (_jacobianMIntq)
    {
      computeJacobianMIntq(time);
      SP::SimpleMatrix aux (new SimpleMatrix(3,_qDim));
      SP::SimpleMatrix RT (new SimpleMatrix(3,3));
      //computeMObjToAbs();
      RT->trans(*_MObjToAbs);
      prod(*RT, *_jacobianMIntq, *aux);
      _jacobianqForces->setBlock(3,0, -1.0* *aux);
      //_jacobianqForces->setBlock(3,0,-1.0* *_jacobianMIntq);
    }
    // std::cout << "_jacobianqForces : "<< std::endl;
    // _jacobianqForces->display();
  }
  //else nothing.
}

void NewtonEulerDS::computeJacobianvForces(double time)
{
  if (_jacobianvForces)
  {
    _jacobianvForces->zero();
    if (_jacobianFIntv)
    {
      computeJacobianFIntv(time);
      _jacobianvForces->setBlock(0,0,-1.0 * *_jacobianFIntv);
    }
    if (_jacobianMIntv)
    {
      computeJacobianMIntv(time);
      _jacobianvForces->setBlock(3,0,-1.0 * *_jacobianMIntv);
    }
    if (_jacobianFGyrv)
    {
      //computeJacobianFGyrvByFD(time,_q,_v);
      computeJacobianFGyrv(time);
      *_jacobianvForces -= *_jacobianFGyrv;
    }
    // std::cout << "_jacobianvForces : "<< std::endl;
    // _jacobianvForces->display();
  }
  //else nothing.
}

void NewtonEulerDS::computeJacobianFGyrv(double time)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianFGyrv(double time) \n");
  if (_jacobianFGyrv)
  {
    //Omega /\ I \Omega:
    _jacobianFGyrv->zero();
    SiconosVector omega(3);
    omega.setValue(0, _v->getValue(3));
    omega.setValue(1, _v->getValue(4));
    omega.setValue(2, _v->getValue(5));
    SiconosVector Iomega(3);
    prod(*_I, omega, Iomega, true);
    SiconosVector ei(3);
    SiconosVector Iei(3);
    SiconosVector ei_Iomega(3);
    SiconosVector omega_Iei(3);

    /*See equation of DevNotes.pdf, equation with label eq:NE_nablaFL1*/
    for (int i = 0; i < 3; i++)
    {
      ei.zero();
      ei.setValue(i, 1.0);
      prod(*_I, ei, Iei, true);
      cross_product(omega, Iei, omega_Iei);
      cross_product(ei, Iomega, ei_Iomega);
      for (int j = 0; j < 3; j++)
        _jacobianFGyrv->setValue(3 + j, 3 + i, ei_Iomega.getValue(j) + omega_Iei.getValue(j));
    }
    // Check if Jacobian is valid. Warning to the transpose operation in
    // _jacobianFGyrv->setValue(3 + j, 3 + i, ei_Iomega.getValue(j) + omega_Iei.getValue(j));
  }
  //else nothing.
  DEBUG_EXPR(_jacobianFGyrv->display());
  DEBUG_END("NewtonEulerDS::computeJacobianFGyrv(double time) \n");
}

void NewtonEulerDS::display() const
{
  std::cout << "=====> NewtonEuler System display (number: " << _number << ")." <<std::endl;
  std::cout << "- _n : " << _n <<std::endl;
  std::cout << "- q " <<std::endl;
  if (_q) _q->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- q0 " <<std::endl;
  if (_q0) _q0->display();
  std::cout << "- v " <<std::endl;
  if (_v) _v->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- v0 " <<std::endl;
  if (_v0) _v0->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- dotq " <<std::endl;
  if (_dotq) _dotq->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- p[0] " <<std::endl;
  if (_p[0]) _p[0]->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- p[1] " <<std::endl;
  if (_p[1]) _p[1]->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- p[2] " <<std::endl;
  if (_p[2]) _p[2]->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "mass :" <<  _scalarMass <<std::endl;
  std::cout << "Inertia :" <<std::endl;
  if (_I) _I->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "===================================== " <<std::endl;
}

// --- Functions for memory handling ---
void NewtonEulerDS::initMemory(unsigned int steps)
{
  DynamicalSystem::initMemory(steps);

  if (steps == 0)
    std::cout << "Warning : NewtonEulerDS::initMemory with size equal to zero" <<std::endl;
  else
  {
    _qMemory.reset(new SiconosMemory(steps, _qDim));
    _vMemory.reset(new SiconosMemory(steps, _n));
    _forcesMemory.reset(new SiconosMemory(steps, _n));
    _dotqMemory.reset(new SiconosMemory(steps, _qDim));
    swapInMemory();
  }
}

void NewtonEulerDS::swapInMemory()
{
  //  _xMemory->swap(_x[0]);
  _qMemory->swap(*_q);
  _vMemory->swap(*_v);
  _dotqMemory->swap(*_dotq);
  _forcesMemory->swap(*_forces);
}

void NewtonEulerDS::resetAllNonSmoothPart()
{
  if (_p[1])
    _p[1]->zero();
  else
    _p[1].reset(new SiconosVector(_n));
}
void NewtonEulerDS::resetNonSmoothPart(unsigned int level)
{
  if (_p[level]->size() > 0)
    _p[level]->zero();
}

void NewtonEulerDS::computeT()
{
  ::computeT(_q,_T);
}

void NewtonEulerDS::computeTdot()
{
  if (!_Tdot)
  {
    _Tdot.reset(new SimpleMatrix(_qDim, _n));
    _Tdot->zero();
  }

  ::computeT(_dotq,_Tdot);
}

void NewtonEulerDS::normalizeq()
{
  double normq = sqrt(_q->getValue(3) * _q->getValue(3) + _q->getValue(4) * _q->getValue(4) + _q->getValue(5) * _q->getValue(5) + _q->getValue(6) * _q->getValue(6));
  assert(normq > 0);
  normq = 1 / normq;
  _q->setValue(3, _q->getValue(3) * normq);
  _q->setValue(4, _q->getValue(4) * normq);
  _q->setValue(5, _q->getValue(5) * normq);
  _q->setValue(6, _q->getValue(6) * normq);
}
void NewtonEulerDS::computeMObjToAbs()
{
  ::computeMObjToAbs(_q, _MObjToAbs);
}

void NewtonEulerDS::setComputeJacobianFIntqFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  //    Plugin::setFunction(&computeJacobianFIntqPtr, pluginPath,functionName);
  _pluginJacqFInt->setComputeFunction(pluginPath, functionName);
}
void NewtonEulerDS::setComputeJacobianFIntvFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  //    Plugin::setFunction(&computeJacobianFIntvPtr, pluginPath,functionName);
  _pluginJacvFInt->setComputeFunction(pluginPath, functionName);
}
void NewtonEulerDS::setComputeJacobianFIntqFunction(FInt_NE fct)
{
  _pluginJacqFInt->setComputeFunction((void *)fct);
}
void NewtonEulerDS::setComputeJacobianFIntvFunction(FInt_NE fct)
{
  _pluginJacvFInt->setComputeFunction((void *)fct);
}

void NewtonEulerDS::setComputeJacobianMIntqFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  //    Plugin::setFunction(&computeJacobianFIntqPtr, pluginPath,functionName);
  _pluginJacqMInt->setComputeFunction(pluginPath, functionName);
}
void NewtonEulerDS::setComputeJacobianMIntvFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  //    Plugin::setFunction(&computeJacobianFIntvPtr, pluginPath,functionName);
  _pluginJacvMInt->setComputeFunction(pluginPath, functionName);
}
void NewtonEulerDS::setComputeJacobianMIntqFunction(FInt_NE fct)
{
  _pluginJacqMInt->setComputeFunction((void *)fct);
}
void NewtonEulerDS::setComputeJacobianMIntvFunction(FInt_NE fct)
{
  _pluginJacvMInt->setComputeFunction((void *)fct);
}

double NewtonEulerDS::computeKineticEnergy()
{
  DEBUG_BEGIN("NewtonEulerDS::computeKineticEnergy()\n");
  assert(_v);
  assert(_massMatrix);
  DEBUG_EXPR(_v->display());
  DEBUG_EXPR(_massMatrix->display());

  SiconosVector tmp(6);
  prod(*_massMatrix, *_v, tmp, true);
  double K =0.5*inner_prod(tmp,*_v);

  DEBUG_PRINTF("Kinetic Energy = %e\n", K);
  DEBUG_END("NewtonEulerDS::computeKineticEnergy()\n");
  return K;
}


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef _ALARCON2004OXYGENBASEDCELLCYCLEODESYSTEM_HPP_
#define _ALARCON2004OXYGENBASEDCELLCYCLEODESYSTEM_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include <cmath>

#include "AbstractOdeSystem.hpp"

/**
 * Represents the Alarcon et al. (2004) system of ODEs (see ticket #461).
 * [doi:10.1016/j.jtbi.2004.04.016]
 *
 * The variables are
 *
 *  0. x = Cdh1-APC complexes
 *  1. y = cyclin-CDK
 *  2. z = p27
 *  3. m = mass
 *  4. u = RBNP
 *  5. P = oxygen concentration
 */
class Alarcon2004OxygenBasedCellCycleOdeSystem : public AbstractOdeSystem
{
private:

    /**
     * Constants for the Alarcon et al. (2004) model
     */

    /** Dimensionless parameter a_1. */
    double ma1;
    /** Dimensionless parameter a_2. */
    double ma2;
    /** Dimensionless parameter a_3. */
    double ma3;
    /** Dimensionless parameter a_4. */
    double ma4;
    /** Dimensionless parameter b_3. */
    double mb3;
    /** Dimensionless parameter b_4. */
    double mb4;
    /** Dimensionless parameter c_1. */
    double mc1;
    /** Dimensionless parameter c_2. */
    double mc2;
    /** Dimensionless parameter d_1. */
    double md1;
    /** Dimensionless parameter d_2. */
    double md2;
    /** Dimensionless parameter J_3. */
    double mJ3;
    /** Dimensionless parameter J_4. */
    double mJ4;
    /** Dimensionless parameter eta. */
    double mEta;
    /** Dimensionless parameter m_star. */
    double mMstar;
    /** Dimensionless parameter B. */
    double mB;
    /** Dimensionless parameter x_THR. */
    double mxThreshold;
    /** Dimensionless parameter y_THR. */
    double myThreshold;

    /** The oxygen concentration (this affects the ODE system). */
    double mOxygenConcentration;

    /** Whether the cell associated with this cell cycle ODE system is labelled (this affects the ODE system). */
    bool mIsLabelled;

    friend class boost::serialization::access;
    /**
     * Serialize the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractOdeSystem>(*this);
    }

public:

    /**
     * Constructor.
     *
     * @param oxygenConcentration is a non-dimensional oxygen concentration value between 0 and 1
     * @param isLabelled whether the cell associated with this cell cycle ODE system is labelled (this affects the ODE system)
     * @param stateVariables optional initial conditions for state variables (only used in archiving)
     */
    Alarcon2004OxygenBasedCellCycleOdeSystem(double oxygenConcentration,
                                             bool isLabelled,
                                             std::vector<double> stateVariables=std::vector<double>());

    /**
     * Destructor.
     */
    ~Alarcon2004OxygenBasedCellCycleOdeSystem();

    /**
     * Initialise parameter values.
     */
    void Init();

    /**
     * Compute the RHS of the Alarcon et al. (2004) system of ODEs.
     *
     * Returns a vector representing the RHS of the ODEs at each time step, y' = [y1' ... yn'].
     * An ODE solver will call this function repeatedly to solve for y = [y1 ... yn].
     *
     * @param time used to evaluate the RHS.
     * @param rY value of the solution vector used to evaluate the RHS.
     * @param rDY filled in with the resulting derivatives (using Alarcons et al. (2004) system of equations).
     */
    void EvaluateYDerivatives(double time, const std::vector<double>& rY, std::vector<double>& rDY);

    /**
     * Calculate whether the conditions for the cell cycle to finish have been met.
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS.
     *
     * @return whether or not stopping conditions have been met
     */
    bool CalculateStoppingEvent(double time, const std::vector<double>& rY);

    /**
     * Set #mIsLabelled.
     *
     * @param isLabelled whether the cell associated with this cell cycle ODE system is labelled (this affects the ODE system)
     */
    void SetIsLabelled(bool isLabelled);

    /**
     * @return #mIsLabelled.
     */
    bool IsLabelled() const;

    /**
     * @return #mOxygenConcentration.
     */
    double GetOxygenConcentration() const;
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(Alarcon2004OxygenBasedCellCycleOdeSystem)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct an Alarcon2004OxygenBasedCellCycleOdeSystem.
 */
template<class Archive>
inline void save_construct_data(
    Archive & ar, const Alarcon2004OxygenBasedCellCycleOdeSystem * t, const unsigned int file_version)
{
    // Save data required to construct instance
    const double oxygen_concentration = t->GetOxygenConcentration();
    ar & oxygen_concentration;

    const bool is_labelled = t->IsLabelled();
    ar & is_labelled;

    const std::vector<double>& state_variables = t->rGetConstStateVariables();
    ar & state_variables;
}

/**
 * De-serialize constructor parameters and initialise an Alarcon2004OxygenBasedCellCycleOdeSystem.
 */
template<class Archive>
inline void load_construct_data(
    Archive & ar, Alarcon2004OxygenBasedCellCycleOdeSystem * t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    double oxygen_concentration;
    ar & oxygen_concentration;

    bool is_labelled;
    ar & is_labelled;

    std::vector<double> state_variables;
    ar & state_variables;

    // Invoke inplace constructor to initialise instance
    ::new(t)Alarcon2004OxygenBasedCellCycleOdeSystem(oxygen_concentration, is_labelled, state_variables);
}
}
} // namespace ...

#endif /*_ALARCON2004OXYGENBASEDCELLCYCLEODESYSTEM_HPP_*/


/**
 * @file recurrent_attention.hpp
 * @author Marcus Edel
 *
 * Definition of the RecurrentAttention class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_LAYER_RECURRENT_ATTENTION_HPP
#define MLPACK_METHODS_ANN_LAYER_RECURRENT_ATTENTION_HPP

#include <mlpack/prereqs.hpp>
#include <boost/ptr_container/ptr_vector.hpp>

#include "../visitor/delta_visitor.hpp"
#include "../visitor/output_parameter_visitor.hpp"
#include "../visitor/reset_visitor.hpp"
#include "../visitor/weight_size_visitor.hpp"

#include "layer_types.hpp"
#include "add_merge.hpp"
#include "sequential.hpp"

namespace mlpack {
namespace ann /** Artificial Neural Network. */ {

/**
 * This class implements the Recurrent Model for Visual Attention, using a
 * variety of possible layer implementations.
 *
 * For more information, see the following paper.
 *
 * @code
 * @article{MnihHGK14,
 *   title={Recurrent Models of Visual Attention},
 *   author={Volodymyr Mnih, Nicolas Heess, Alex Graves, Koray Kavukcuoglu},
 *   journal={CoRR},
 *   volume={abs/1406.6247},
 *   year={2014}
 * }
 * @endcode
 *
 * @tparam InputDataType Type of the input data (arma::colvec, arma::mat,
 *         arma::sp_mat or arma::cube).
 * @tparam OutputDataType Type of the output data (arma::colvec, arma::mat,
 *         arma::sp_mat or arma::cube).
 */
template <
    typename InputDataType = arma::mat,
    typename OutputDataType = arma::mat
>
class RecurrentAttention
{
 public:
  /**
   * Default constructor: this will not give a usable RecurrentAttention object,
   * so be sure to set all the parameters before use.
   */
  RecurrentAttention();

  /**
   * Create the RecurrentAttention object using the specified modules.
   *
   * @param start The module output size.
   * @param start The recurrent neural network module.
   * @param start The action module.
   * @param rho Maximum number of steps to backpropagate through time (BPTT).
   */
  template<typename RNNModuleType, typename ActionModuleType>
  RecurrentAttention(const size_t outSize,
                     const RNNModuleType& rnn,
                     const ActionModuleType& action,
                     const size_t rho);

  /**
   * Ordinary feed forward pass of a neural network, evaluating the function
   * f(x) by propagating the activity forward through f.
   *
   * @param input Input data used for evaluating the specified function.
   * @param output Resulting output activation.
   */
  template<typename eT>
  void Forward(arma::Mat<eT>&& input, arma::Mat<eT>&& output);

  /**
   * Ordinary feed backward pass of a neural network, calculating the function
   * f(x) by propagating x backwards trough f. Using the results from the feed
   * forward pass.
   *
   * @param input The propagated input activation.
   * @param gy The backpropagated error.
   * @param g The calculated gradient.
   */
  template<typename eT>
  void Backward(const arma::Mat<eT>&& /* input */,
                arma::Mat<eT>&& gy,
                arma::Mat<eT>&& g);

  /*
   * Calculate the gradient using the output delta and the input activation.
   *
   * @param input The input parameter used for calculating the gradient.
   * @param error The calculated error.
   * @param gradient The calculated gradient.
   */
  template<typename eT>
  void Gradient(arma::Mat<eT>&& /* input */,
                arma::Mat<eT>&& /* error */,
                arma::Mat<eT>&& /* gradient */);

  //! Get the model modules.
  std::vector<LayerTypes<>>& Model() { return network; }

    //! The value of the deterministic parameter.
  bool Deterministic() const { return deterministic; }
  //! Modify the value of the deterministic parameter.
  bool& Deterministic() { return deterministic; }

  //! Get the parameters.
  OutputDataType const& Parameters() const { return parameters; }
  //! Modify the parameters.
  OutputDataType& Parameters() { return parameters; }

  //! Get the input parameter.
  InputDataType const& InputParameter() const { return inputParameter; }
  //! Modify the input parameter.
  InputDataType& InputParameter() { return inputParameter; }

  //! Get the output parameter.
  OutputDataType const& OutputParameter() const { return outputParameter; }
  //! Modify the output parameter.
  OutputDataType& OutputParameter() { return outputParameter; }

  //! Get the delta.
  OutputDataType const& Delta() const { return delta; }
  //! Modify the delta.
  OutputDataType& Delta() { return delta; }

  //! Get the gradient.
  OutputDataType const& Gradient() const { return gradient; }
  //! Modify the gradient.
  OutputDataType& Gradient() { return gradient; }

  /**
   * Serialize the layer
   */
  template<typename Archive>
  void serialize(Archive& ar, const unsigned int /* version */);

 private:
  //! Calculate the gradient of the attention module.
  void IntermediateGradient()
  {
    intermediateGradient.zeros();

    // Gradient of the action module.
    if (backwardStep == (rho - 1))
    {
      boost::apply_visitor(GradientVisitor(std::move(initialInput),
          std::move(actionError)), actionModule);
    }
    else
    {
      boost::apply_visitor(GradientVisitor(std::move(boost::apply_visitor(
          outputParameterVisitor, actionModule)), std::move(actionError)),
          actionModule);
    }

    // Gradient of the recurrent module.
    boost::apply_visitor(GradientVisitor(std::move(boost::apply_visitor(
        outputParameterVisitor, rnnModule)), std::move(recurrentError)),
        rnnModule);

    attentionGradient += intermediateGradient;
  }

  //! Locally-stored module output size.
  size_t outSize;

  //! Locally-stored start module.
  LayerTypes<> rnnModule;

  //! Locally-stored input module.
  LayerTypes<> actionModule;

  //! Number of steps to backpropagate through time (BPTT).
  size_t rho;

  //! Locally-stored number of forward steps.
  size_t forwardStep;

  //! Locally-stored number of backward steps.
  size_t backwardStep;

  //! If true dropout and scaling is disabled, see notes above.
  bool deterministic;

  //! Locally-stored weight object.
  OutputDataType parameters;

  //! Locally-stored model modules.
  std::vector<LayerTypes<>> network;

  //! Locally-stored weight size visitor.
  WeightSizeVisitor weightSizeVisitor;

  //! Locally-stored delta visitor.
  DeltaVisitor deltaVisitor;

  //! Locally-stored output parameter visitor.
  OutputParameterVisitor outputParameterVisitor;

  //! Locally-stored feedback output parameters.
  std::vector<arma::mat> feedbackOutputParameter;

  //! List of all module parameters for the backward pass (BBTT).
  std::vector<arma::mat> moduleOutputParameter;

  //! Locally-stored delta object.
  OutputDataType delta;

  //! Locally-stored gradient object.
  OutputDataType gradient;

  //! Locally-stored input parameter object.
  InputDataType inputParameter;

  //! Locally-stored output parameter object.
  OutputDataType outputParameter;

  //! Locally-stored recurrent error parameter.
  arma::mat recurrentError;

  //! Locally-stored action error parameter.
  arma::mat actionError;

  //! Locally-stored action delta.
  arma::mat actionDelta;

  //! Locally-stored recurrent delta.
  arma::mat rnnDelta;

  //! Locally-stored initial action input.
  arma::mat initialInput;

  //! Locally-stored reset visitor.
  ResetVisitor resetVisitor;

  //! Locally-stored attention gradient.
  arma::mat attentionGradient;

  //! Locally-stored intermediate gradient for the attention module.
  arma::mat intermediateGradient;
}; // class RecurrentAttention

} // namespace ann
} // namespace mlpack

// Include implementation.
#include "recurrent_attention_impl.hpp"

#endif