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

#include "equirectangular_camera_colinearity_tait_bryan_wc_jacobian.h"
#include "equirectangular_camera_coplanarity_tait_bryan_wc_jacobian.h"
#include "equirectangular_camera_coplanarity_rodrigues_wc_jacobian.h"
#include "equirectangular_camera_coplanarity_quaternion_wc_jacobian.h"
#include "quaternion_constraint_jacobian.h"
#include "structures.h"
#include "transformations.h"
#include "cauchy.h"

struct KeyPoint{
	double u;
	double v;
	int index_to_tie_point;
};

struct Camera{
	Eigen::Affine3d pose;
	std::vector<KeyPoint> key_points;
};

std::vector<Eigen::Vector3d> tie_points;
std::vector<Camera> cameras;
int cols = 4096;
int rows = 2048;

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 = -215, rotate_y = 273;
float translate_z = -68.0;
float translate_x = 4, translate_y = 30.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();

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

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

	for(size_t i = 0 ; i < 100; i++){
		Eigen::Vector3d p;
		p.x() = ((rand()%1000000)/1000000.0 - 0.5) * 2.0 * 100.0;
		p.y() = ((rand()%1000000)/1000000.0 - 0.5) * 2.0 * 200.0;
		p.z() = ((rand()%1000000)/1000000.0 - 0.5) * 2.0 * 100.0;
		tie_points.push_back(p);
	}

	for(int i = -50 ; i < 50; i+=20){
		Camera c;
		c.pose = Eigen::Affine3d::Identity();
		c.pose(0,3) = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
		c.pose(1,3) = i;
		c.pose(2,3) = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
		cameras.push_back(c);
	}

	for(size_t i = 0 ; i < cameras.size(); i++){
		for(size_t j = 0; j < tie_points.size(); j++){
			KeyPoint kp;
			kp.index_to_tie_point = j;

			TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
			equrectangular_camera_colinearity_tait_bryan_wc(kp.u, kp.v, rows, cols, M_PI, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, tie_points[j].x(), tie_points[j].y(), tie_points[j].z());

			cameras[i].key_points.push_back(kp);
		}
	}

	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("equirectangular_camera_coplanarity_ba");
	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 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);

	glColor3f(1,0,0);
	glPointSize(5);
	glBegin(GL_POINTS);
	for(size_t i = 0 ; i < tie_points.size(); i++){
		glVertex3f(tie_points[i].x(), tie_points[i].y(), tie_points[i].z());
	}
	glEnd();

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

		glBegin(GL_LINES);
			glColor3f(1.0f, 0.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,0), m(1,3) + m(1,0), m(2,3) + m(2,0));

			glColor3f(0.0f, 1.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,1), m(1,3) + m(1,1), m(2,3) + m(2,1));

			glColor3f(0.0f, 0.0f, 1.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,2), m(1,3) + m(1,2), m(2,3) + m(2,2));
		glEnd();
	}

	glColor3f(0.8,0.8,0.8);
	glBegin(GL_LINE_STRIP);
	for(size_t i = 0 ; i < cameras.size(); i++){
		Eigen::Affine3d m = cameras[i].pose;
		glVertex3f(m(0,3), m(1,3), m(2,3));
	}
	glEnd();
	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'c':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose pose;
				pose.om = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;
				pose.fi = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;
				pose.ka = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;

				Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose);
				cameras[i].pose = cameras[i].pose * m;
			}
			break;
		}
		case 'p':{
			for(size_t i = 0; i < tie_points.size(); i++){
				tie_points[i].x() += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
				tie_points[i].y() += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
				tie_points[i].z() += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
			}
			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 < cameras.size(); i++){
				for(size_t j = 0; j < cameras.size(); j++){
					for(size_t k = 0; k < cameras[i].key_points.size(); k++){
						if(i != j){
							TaitBryanPose pose_1 = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
							TaitBryanPose pose_2 = pose_tait_bryan_from_affine_matrix(cameras[j].pose);

							double px_1 = pose_1.px;
							double py_1 = pose_1.py;
							double pz_1 = pose_1.pz;
							double om_1 = pose_1.om;
							double fi_1 = pose_1.fi;
							double ka_1 = pose_1.ka;

							double px_2 = pose_2.px;
							double py_2 = pose_2.py;
							double pz_2 = pose_2.pz;
							double om_2 = pose_2.om;
							double fi_2 = pose_2.fi;
							double ka_2 = pose_2.ka;

							double u_1 = (cameras[i].key_points[k].u);
							double v_1 = (cameras[i].key_points[k].v);
							double u_2 = (cameras[j].key_points[k].u);
							double v_2 = (cameras[j].key_points[k].v);

							double delta;
							observation_equation_equirectangular_camera_coplanarity_tait_bryan_wc(delta, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, om_1, fi_1, ka_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, om_2, fi_2, ka_2);

							Eigen::Matrix<double, 1, 12, Eigen::RowMajor> jacobian;
							observation_equation_equirectangular_camera_coplanarity_tait_bryan_wc_jacobian(jacobian, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, om_1, fi_1, ka_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, om_2, fi_2, ka_2);

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

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

							tripletListA.emplace_back(ir     , ic_2 + 0, -jacobian(0,9));
							tripletListA.emplace_back(ir     , ic_2 + 1, -jacobian(0,10));
							tripletListA.emplace_back(ir     , ic_2 + 2, -jacobian(0,11));

							tripletListP.emplace_back(ir    , ir    ,  cauchy(delta, 1));

							tripletListB.emplace_back(ir    , 0,  delta);
						}
					}
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 3);
			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(cameras.size() * 3, cameras.size() * 3);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 3, 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() == cameras.size() * 3){
				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 < cameras.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_tait_bryan(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				cameras[i].pose = affine_matrix_from_pose_tait_bryan(posetb);
			}

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

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras.size(); j++){
					for(size_t k = 0; k < cameras[i].key_points.size(); k++){
						if(i != j){
							RodriguesPose pose_1 = pose_rodrigues_from_affine_matrix(cameras[i].pose);
							RodriguesPose pose_2 = pose_rodrigues_from_affine_matrix(cameras[j].pose);

							double px_1 = pose_1.px;
							double py_1 = pose_1.py;
							double pz_1 = pose_1.pz;
							double sx_1 = pose_1.sx;
							double sy_1 = pose_1.sy;
							double sz_1 = pose_1.sz;

							double px_2 = pose_2.px;
							double py_2 = pose_2.py;
							double pz_2 = pose_2.pz;
							double sx_2 = pose_2.sx;
							double sy_2 = pose_2.sy;
							double sz_2 = pose_2.sz;

							double u_1 = (cameras[i].key_points[k].u);
							double v_1 = (cameras[i].key_points[k].v);
							double u_2 = (cameras[j].key_points[k].u);
							double v_2 = (cameras[j].key_points[k].v);

							double delta;
							observation_equation_equirectangular_camera_coplanarity_rodrigues_wc(delta, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, sx_1, sy_1, sz_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, sx_2, sy_2, sz_2);

							Eigen::Matrix<double, 1, 12, Eigen::RowMajor> jacobian;
							observation_equation_equirectangular_camera_coplanarity_rodrigues_wc_jacobian(jacobian, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, sx_1, sy_1, sz_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, sx_2, sy_2, sz_2);

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

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

							tripletListA.emplace_back(ir, ic_2 + 0, -jacobian(0,9));
							tripletListA.emplace_back(ir, ic_2 + 1, -jacobian(0,10));
							tripletListA.emplace_back(ir, ic_2 + 2, -jacobian(0,11));

							tripletListP.emplace_back(ir, ir,  cauchy(delta, 1));

							tripletListB.emplace_back(ir, 0, delta);
						}
					}
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 3);
			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(cameras.size() * 3, cameras.size() * 3);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 3, 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() == cameras.size() * 3){
				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 < cameras.size(); i++){
					RodriguesPose pose = pose_rodrigues_from_affine_matrix(cameras[i].pose);
					pose.sx += h_x[counter++];
					pose.sy += h_x[counter++];
					pose.sz += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_rodrigues(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				cameras[i].pose = affine_matrix_from_pose_tait_bryan(posetb);
			}

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

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras.size(); j++){
					for(size_t k = 0; k < cameras[i].key_points.size(); k++){
						if(i != j){
							QuaternionPose pose_1 = pose_quaternion_from_affine_matrix(cameras[i].pose);
							QuaternionPose pose_2 = pose_quaternion_from_affine_matrix(cameras[j].pose);

							double px_1 = pose_1.px;
							double py_1 = pose_1.py;
							double pz_1 = pose_1.pz;
							double q0_1 = pose_1.q0;
							double q1_1 = pose_1.q1;
							double q2_1 = pose_1.q2;
							double q3_1 = pose_1.q3;

							double px_2 = pose_2.px;
							double py_2 = pose_2.py;
							double pz_2 = pose_2.pz;
							double q0_2 = pose_2.q0;
							double q1_2 = pose_2.q1;
							double q2_2 = pose_2.q2;
							double q3_2 = pose_2.q3;

							double u_1 = (cameras[i].key_points[k].u);
							double v_1 = (cameras[i].key_points[k].v);
							double u_2 = (cameras[j].key_points[k].u);
							double v_2 = (cameras[j].key_points[k].v);

							double delta;
							observation_equation_equirectangular_camera_coplanarity_quaternion_wc(delta, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, q0_1, q1_1, q2_1, q3_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, q0_2, q1_2, q2_2, q3_2);

							Eigen::Matrix<double, 1, 14, Eigen::RowMajor> jacobian;
							observation_equation_equirectangular_camera_coplanarity_quaternion_wc_jacobian(jacobian, rows, cols, M_PI, u_1, v_1, px_1, py_1, pz_1, q0_1, q1_1, q2_1, q3_1, rows, cols, u_2, v_2, px_2, py_2, pz_2, q0_2, q1_2, q2_2, q3_2);

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

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

							tripletListA.emplace_back(ir, ic_2 + 0, -jacobian(0,10));
							tripletListA.emplace_back(ir, ic_2 + 1, -jacobian(0,11));
							tripletListA.emplace_back(ir, ic_2 + 2, -jacobian(0,12));
							tripletListA.emplace_back(ir, ic_2 + 3, -jacobian(0,13));

							tripletListP.emplace_back(ir, ir, cauchy(delta, 1));

							tripletListB.emplace_back(ir, 0,  delta);
						}
					}
				}
			}

			for(size_t i = 0 ; i < cameras.size(); i++){
				int ic = i * 4;
				int ir = tripletListB.size();
				QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);

				double delta;
				quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3);

				Eigen::Matrix<double, 1, 4> jacobian;
				quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3);

				tripletListA.emplace_back(ir, ic + 0 , -jacobian(0,0));
				tripletListA.emplace_back(ir, ic + 1 , -jacobian(0,1));
				tripletListA.emplace_back(ir, ic + 2 , -jacobian(0,2));
				tripletListA.emplace_back(ir, ic + 3 , -jacobian(0,3));

				tripletListP.emplace_back(ir, ir, 1000000.0);

				tripletListB.emplace_back(ir, 0, delta);
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 4);
			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(cameras.size() * 4, cameras.size() * 4);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 4, 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() == cameras.size() * 4){
				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 < cameras.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);
					pose.q0 += h_x[counter++];
					pose.q1 += h_x[counter++];
					pose.q2 += h_x[counter++];
					pose.q3 += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_quaternion(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			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 << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize (Rodriguez)" << std::endl;
	std::cout << "q: optimize (Quaternion)" << std::endl;
	std::cout << "c: add noise to cameras" << std::endl;
	std::cout << "p: add noise to tie points" << std::endl;
}


/*
Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

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 copyright holder(s) 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.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <math.h>
#include <stdio.h>
#include <ctime>
#include <time.h>

using namespace std;
using namespace Eigen;
using namespace KDL;

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

	ros::init(argc, argv, "Grasp_quality");	// ROS node
	ros::NodeHandle nh;

	int n_c = 1; //number of contacts
	int n_q = 19; //number of joints
	int n_z = 1; // number of synergie
	int synergy = 1;

	double Kx = 1;
	double Ky = 1;
	double Kz = 1;

	double f_i_max = 20;
	double mu_friction = 1;
	double PCR;

	Eigen::VectorXd f(6*n_c); // contact force
	Eigen::VectorXd H_f(3*n_c); // contact force with type model of contact
	Eigen::VectorXd f_y(3*n_c); // controllable contact force

	Eigen::VectorXd w(6);

	Eigen::MatrixXd G(6, 6*n_c);
	Eigen::MatrixXd G_H_t(6,3*n_c); // Grasp Matrix
	Eigen::MatrixXd J(6*n_c, n_q);
	Eigen::MatrixXd H_J(3*n_c,n_q); // Hand Jacobian
	Eigen::MatrixXd G_r_k(3*n_c,6); // weighted right pseudoinverse of G

	Eigen::MatrixXd K(3*n_c,3*n_c); // grasp stiffness matrix
	Eigen::MatrixXd Kis(3,3);
	Eigen::MatrixXd Ks(3*n_c,3*n_c); // contact stiffness matrix
	Eigen::MatrixXd Kp(n_q,n_q); // joint stiffness matrix

	Eigen::MatrixXd S(n_q,n_z); // Synergie matriz for underactuacted hand

	Eigen::VectorXd d_f(3*n_c);

	// calculation Ks(Cj)
	int s = 0; // step for component
	for( int i = 0 ; i < n_c ; i++ )
	{
		Kis(0,0) = 0;
		Kis(1,1) = 0;
		Kis(2,2) = 0;

		if( H_f(i+s+3) >= 0 )
			if(  sqrt( H_f(i+s+0)*H_f(i+s+0) + H_f(i+s+1)*H_f(i+s+1)) <= mu_friction * H_f(i+s+2) )
			{
				Kis(0,0) =  Kx;
				Kis(1,1) =  Ky;
				Kis(2,2) =  Kz;
			}
			else
				Ks(2,2) = Kz;

		Ks.block<3,3>(s,s) = Kis;
		s += 3;
	}

	// calculation the grasp stiffness matrix K(Cj)
	Eigen::MatrixXd K_(3*n_c,3*n_c);
	K_ = Ks.inverse() + H_J * Kp.inverse() * H_J.transpose() ;
	K  = K_.inverse();

	// evaluation the constraint N(K(Cj)*Gt) = 0
	// it must be satisfied to immobilize the object
	Eigen::MatrixXd Kcj_Gt = K * G_H_t.transpose();
	FullPivLU<MatrixXd> lu_(Kcj_Gt);
	Eigen::MatrixXd Null_Kcj_Gt = lu_.kernel();

	bool Matrix_is_Zero = true;

	for (int i = 0 ; i < Null_Kcj_Gt.rows() ; i++ )
		for ( int j = 0 ; j < Null_Kcj_Gt.cols() ; j++)
			if( Null_Kcj_Gt(i,j) != 0 )
			{	Matrix_is_Zero = false; break; }

	if( !Matrix_is_Zero ) // condition-constrain of PGR is not satisfy
		return 0;

	Eigen::MatrixXd H_(3,6);

	H_ << 1, 0, 0, 0, 0, 0,
		  0, 1, 0, 0, 0, 0,
		  0, 0, 1, 0, 0, 0;

	Eigen::MatrixXd H(3*n_c,6*n_c);

	int i = 0;
	int j = 0;
	for(int n = 0 ;  n < n_c ; n++)
	{
		H.block<3,6>(i,j) = H_;
		i += 3;
		j += 6;
	}

	//define the sinergie matrix
	for(int j = 0 ; j < n_q ; j++)
		for(int i = 0; i < n_z ; i++)
			S(j,i) = 1;

	// apply the kind of the point id contact model
	G_H_t = G * H.transpose();
	H_J = H * J ;
	H_f = H * f ;

	// calculatin of external wrench
	w = - G_H_t * H_f;

	//calculation weighted right pseudoinverse
	Eigen::MatrixXd G_inv;
	Eigen::MatrixXd G_;
	G_ = G_H_t * K * G_H_t.transpose();
	G_inv = G_.inverse();
	G_r_k = K * G.transpose() * G_inv;

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

	//calculation of a basis for the subspace of the controllable internal forces
	// with null of GHt
	FullPivLU<MatrixXd> lu(G_H_t);
	Eigen::MatrixXd E_ = lu.kernel();
	Eigen::VectorXd y_(E_.cols());

	for(int i = 0; i < y_.size(); i++)
		y_(i)=1;

	f_y = - G_r_k * w + E_* y_ ; //calculation of the controllable contact forces

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////777777777777777777777777777777777777
	///////////////////////////////////////////////////////////////////////////////////////

	//calculation of a basis for the subspace of the controllable internal force
	// with synergy
	Eigen::MatrixXd I = MatrixXd::Identity(3*n_c, 3*n_c);
	Eigen::MatrixXd F(3*n_c,n_q);
	Eigen::MatrixXd E;
	Eigen::VectorXd y(1);

	y(0) = synergy;

	F = ( I - G_r_k * G ) * K * H_J; // and maps independently controlled joint reference displacements δqr’s into active internal forces

	E = F * S ;

	f_y = - G_r_k * w + E * y ; // calculation of the controllable contact forces

	// search the minimum value of d(f_y) vector
	Eigen::VectorXd f_i(3);
	Eigen::VectorXd n_i(3);
	Eigen::VectorXd f_i_n(1);
	int step = 0;
	double f_i_ ;

	for(int i = 0 ; i < (f_y.size()/3)  ; i++)
	{
		f_i(0) = f_y(step+0);
		f_i(1) = f_y(step+1);
		f_i(2) = f_y(step+2);

		f_i_ = f_i(0)+f_i(1)+f_i(2);
		f_i_n = f_i * n_i.transpose();

		d_f(step+0)  =  f_i_n(0);
		d_f(step+1)  =  mu_friction*f_i_n(0) - (sin(acos(f_i_n(0)/f_i_)));
		d_f(step+2)  =  f_i_max - f_i.norm();

		step += 3;
	}

	double d_min = d_f(0);

	for(int i = 0 ; i < d_f.size() ; i++)
		if( d_min > d_f(i) )
			d_min = d_f(i);

	Eigen::VectorXd Singular ;
	JacobiSVD<MatrixXd> svd(G_r_k, ComputeThinU | ComputeThinV);
    Singular = svd.singularValues();

    double sigma_max = Singular[0];

    PCR = d_min / sigma_max;

	cout << " YEAH ENJOY " << endl;
	cout << "   fine   " << endl;

	ros::spin();
	return 0;
}


#pragma once

#include <iostream>

#include <boost/container/flat_map.hpp>

#include <xtensor/xoperation.hpp>
#include <xtensor/xmath.hpp>
#include <xtensor/xtensor.hpp>
#include <xtensor/xexpression.hpp>
#include <xtensor/xview.hpp>

#include "nifty/ufd/ufd.hxx"
#include "nifty/graph/undirected_list_graph.hxx"
#include "nifty/graph/opt/lifted_multicut/lifted_multicut_objective.hxx"
namespace nifty{
namespace graph{
namespace opt{
namespace lifted_multicut{

    ///\cond
    namespace detail_plmc{
        template<std::size_t DIM>
        struct EvalHelper;

        template<>
        struct EvalHelper<2>{

            template<class OBJ, class D_LABELS>
            static auto evaluate(
                const OBJ & obj,
                const xt::xexpression<D_LABELS> & e_labels
            ){
                const auto & labels = e_labels.derived_cast();
                const auto & weights = obj.weights();
                const auto & offsets = obj.offsets();
                const auto & shape = obj.shape();
                const auto & n_offsets = obj.n_offsets();

                auto e = 0.0;

                for(int p0=0; p0<shape[0]; ++p0)
                for(int p1=0; p1<shape[1]; ++p1){
                    const auto label_p = labels(p0, p1);
                    for(int offset_index=0; offset_index<n_offsets; ++offset_index){
                        const int q0 = p0 + offsets(offset_index, 0);
                        const int q1 = p1 + offsets(offset_index, 1);
                        if(q0 >= 0 && q0 < shape[0]  && q1 >= 0 && q1 < shape[1]){
                            const auto label_q = labels(q0, q1);
                            if(label_p != label_q){
                                e += weights(p0, p1, offset_index);
                            }
                        }
                    }
                }
                return e;
            }
        };
    }
    ///\endcond

    template<std::size_t DIM>
    class PixelWiseLmcObjective{
    public:

        PixelWiseLmcObjective(
        ) {

        }
        template<class D_WEIGHTS, class D_OFFSETS>
        PixelWiseLmcObjective(
            const xt::xexpression<D_WEIGHTS> & e_weights,
            const xt::xexpression<D_OFFSETS> & e_offsets
        )
        :   weights_(e_weights),
            offsets_(e_offsets),
            shape_(),
            n_offsets_(),
            n_variables_()
        {
            // shape and n_offset
            const auto & wshape = weights_.shape();
            std::copy(wshape.begin(), wshape.end(), shape_.begin());
            n_offsets_ = wshape[DIM];

            // n_var
            n_variables_ = shape_[0];
            for(auto d=1; d<DIM; ++d){
                n_variables_ *= shape_[d];
            }

        }

        template<class D_LABELS>
        auto evaluate(
            const xt::xexpression<D_LABELS> & e_labels
        )const{
            return detail_plmc::EvalHelper<DIM>::evaluate(*this, e_labels);
        }
        const auto & weights()const{
            return weights_;
        }
        const auto & offsets()const{
            return offsets_;
        }
        const auto & shape()const{
            return shape_;
        }
        auto n_offsets()const{
            return n_offsets_;
        }
        auto n_variables() const {
            return n_variables_;
        }
    private:

        xt::xtensor<int,       2, xt::layout_type::row_major> offsets_;
        xt::xtensor<float, DIM+1, xt::layout_type::row_major> weights_;

        std::array<int, DIM> shape_;
        std::size_t n_offsets_;
        uint64_t n_variables_;
    };

    template<std::size_t DIM>
    class PixelWiseLmcConnetedComponentsFusion;

    template<>
    class PixelWiseLmcConnetedComponentsFusion<2>
    {
    public:
        static const std::size_t DIM = 2;

        typedef nifty::graph::UndirectedGraph<>                 CCGraphType;
        typedef LiftedMulticutObjective<CCGraphType, double >   CCObjectiveType;
        typedef LiftedMulticutBase<CCObjectiveType>             CCBaseType;
        typedef typename CCBaseType::VisitorBaseType            CVisitorBaseType;
        typedef typename CCBaseType::NodeLabelsType             CCNodeLabels;
        // factory for the lifted primal rounder
        typedef nifty::graph::opt::common::SolverFactoryBase<CCBaseType> CCLmcFactoryBase;

        PixelWiseLmcConnetedComponentsFusion(
            const PixelWiseLmcObjective<DIM> & objective,
            std::shared_ptr<CCLmcFactoryBase> solver_fatory
        )
        :   objective_(objective),
            ufd_(objective.n_variables()),
            solver_fatory_(solver_fatory)
        {

        }

        struct  Settings
        {

        };

        template<class D_LABELS_A, class D_LABELS_B>
        auto fuse(
            const xt::xexpression<D_LABELS_A>  & e_labels_a,
            const xt::xexpression<D_LABELS_B>  & e_labels_b
        ){

            ufd_.reset();

            const auto & shape = objective_.shape();
            const auto & labels_a = e_labels_a.derived_cast();
            const auto & labels_b = e_labels_b.derived_cast();

            typename xt::xtensor<int, DIM>::shape_type reshape{size_t(shape[0]), size_t(shape[1])};
            auto res = xt::xtensor<int, DIM, xt::layout_type::row_major>(reshape);

            this->merge_ufd(e_labels_a, e_labels_b);

            //
            auto e_a = objective_.evaluate(labels_a);
            auto e_b = objective_.evaluate(labels_b);
            this->do_it(res, [&](
                const auto & cc_node_labels,
                const auto & cc_energy
            ){
                if(cc_energy < std::min(e_a, e_b)){

                    auto res_iter = res.begin();
                    for(auto var=0; var<objective_.n_variables(); ++var){
                        const auto dense_var = *res_iter;
                        *res_iter = cc_node_labels[dense_var];
                        //*res_iter = cc_node_labels[to_dense[ufd_.find(var)]];
                        ++res_iter;
                    }

                }
                else if(e_a < cc_energy){
                    std::copy(labels_a.begin(), labels_a.end(), res.begin());
                }
                else{
                    std::copy(labels_b.begin(), labels_b.end(), res.begin());
                }
            });

            return res;

        }

        template<class D_LABELS>
        auto fuse(
            const xt::xexpression<D_LABELS>  & e_labels
        ){

            ufd_.reset();

            const auto & shape = objective_.shape();

            typename xt::xtensor<int, DIM>::shape_type reshape{size_t(shape[0]), size_t(shape[1])};
            auto res = xt::xtensor<int, DIM, xt::layout_type::row_major>(reshape);

            this->merge_ufd2(e_labels);

            //

            this->do_it(res, [&](
                const auto & cc_node_labels,
                const auto & cc_energy
            ){
                if(1){//cc_energy < std::min(e_a, e_b)){

                    auto res_iter = res.begin();
                    for(auto var=0; var<objective_.n_variables(); ++var){
                        const auto dense_var = *res_iter;
                        *res_iter = cc_node_labels[dense_var];
                        //*res_iter = cc_node_labels[to_dense[ufd_.find(var)]];
                        ++res_iter;
                    }

                }
                //else if(e_a < cc_energy){
                //    std::copy(labels_a.begin(), labels_a.end(), res.begin());
                //}
                //else{
                //    std::copy(labels_b.begin(), labels_b.end(), res.begin());
                //}
            });

            return res;

        }

    private:

        template<class D_LABELS_A, class D_LABELS_B>
        auto merge_ufd(
            const xt::xexpression<D_LABELS_A>  & e_labels_a,
            const xt::xexpression<D_LABELS_B>  & e_labels_b
        ){
            const auto & shape = objective_.shape();
            const auto & labels_a = e_labels_a.derived_cast();
            const auto & labels_b = e_labels_b.derived_cast();

            uint64_t node_p = 0;
            for(int p0=0; p0<shape[0]; ++p0)
            for(int p1=0; p1<shape[1]; ++p1){

                const auto p_label_a = labels_a(p0, p1);
                const auto p_label_b = labels_b(p0, p1);

                if(p0 + 1 < shape[0]){
                    const auto q_label_a = labels_a(p0+1, p1);
                    const auto q_label_b = labels_b(p0+1, p1);
                    if(p_label_a == q_label_a && p_label_b == q_label_b){
                        const auto node_q = node_p + shape[1];
                        ufd_.merge(node_p, node_q);
                    }
                }
                if(p1 + 1 < shape[1]){
                    const auto q_label_a = labels_a(p0, p1+1);
                    const auto q_label_b = labels_b(p0, p1+1);
                    if(p_label_a == q_label_a && p_label_b == q_label_b){
                        const auto node_q = node_p + 1;
                        ufd_.merge(node_p, node_q);
                    }
                }
                ++node_p;
            }
        }

        template<class D_LABELS>
        auto merge_ufd2(
            const xt::xexpression<D_LABELS>  & e_labels
        ){
            const auto & shape = objective_.shape();
            const auto & labels = e_labels.derived_cast();
            const auto n_offsets = labels.shape()[DIM];

            const auto pview = xt::view(labels,0,0,xt::all());
            const auto bla = pview(0);

            //uint64_t node_p = 0;
            //for(int p0=0; p0<shape[0]; ++p0)
            //for(int p1=0; p1<shape[1]; ++p1){
            //    if(p0 + 1 < shape[0]){
            //        bool do_merge = true;
            //        for(auto o=0; o<n_offsets; ++o){
            //            const auto p_label = labels(p0,  p1,o);
            //            const auto q_label = labels(p0+1,p1,o);
            //            if(p_label != q_label ){
            //                do_merge = false;
            //                break;
            //            }
            //        }
            //        if(do_merge){
            //            const auto node_q = node_p + shape[1];
            //            ufd_.merge(node_p, node_q);
            //        }
            //    }
            //    if(p1 + 1 < shape[1]){
            //        bool do_merge = true;
            //        for(auto o=0; o<n_offsets; ++o){
            //            const auto p_label = labels(p0, p1,  o);
            //            const auto q_label = labels(p0, p1+1,o);
            //            if(p_label != q_label ){
            //                do_merge = false;
            //                break;
            //            }
            //        }
            //        if(do_merge){
            //            const auto node_q = node_p + 1;
            //            ufd_.merge(node_p, node_q);
            //        }
            //    }
            //    ++node_p;
            //}
        }

        template<class F>
        auto do_it(
            xt::xtensor<int, DIM, xt::layout_type::row_major> & res,
            F && f
        ){

            const auto & shape = objective_.shape();
            const auto & offsets = objective_.offsets();
            const auto & weights = objective_.weights();
            const auto & n_offsets = objective_.n_offsets();
            // const auto & labels_a = e_labels_a.derived_cast();
            // const auto & labels_b = e_labels_b.derived_cast();

            // make map dense
            const auto cc_n_variables = ufd_.numberOfSets();
            boost::container::flat_map<uint64_t, uint64_t> to_dense;
            ufd_.representativeLabeling(to_dense);
            {
                auto res_iter = res.begin();
                for(auto var=0; var<objective_.n_variables(); ++var){
                    *res_iter = to_dense[ufd_.find(var)];
                    ++res_iter;
                }
            }

            // build the normal graph
            CCGraphType cc_graph(cc_n_variables);

            uint64_t node_p = 0 ;
            for(int p0=0; p0<shape[0]; ++p0)
            for(int p1=0; p1<shape[1]; ++p1){

                const auto p_label = ufd_.find(node_p);

                if(p0 + 1 < shape[0]){
                    const auto node_q = node_p + shape[1];
                    const auto q_label = ufd_.find(node_q);
                    if(p_label != q_label){
                        cc_graph.insertEdge(to_dense[p_label],to_dense[q_label]);
                    }
                }
                if(p1 + 1 < shape[1]){
                    const auto node_q = node_p + 1;
                    const auto q_label = ufd_.find(node_q);
                    if(p_label != q_label){
                        cc_graph.insertEdge(to_dense[p_label],to_dense[q_label]);
                    }
                }
                ++node_p;
            }

            CCObjectiveType cc_obj(cc_graph);

            node_p = 0 ;

            // fill the lifted obj
            for(int p0=0; p0<shape[0]; ++p0)
            for(int p1=0; p1<shape[1]; ++p1){

                const auto p_label = ufd_.find(node_p);

                for(int offset_index=0; offset_index<n_offsets; ++offset_index){
                    const int q0 = p0 + offsets(offset_index, 0);
                    const int q1 = p1 + offsets(offset_index, 1);
                    if(q0 >= 0 && q0 < shape[0]  && q1 >= 0 && q1 < shape[1]){

                        const auto node_q = q0*shape[1] + q1;
                        const auto q_label = ufd_.find(node_q);
                        if(p_label != q_label){

                            cc_obj.setCost(to_dense[p_label], to_dense[q_label],
                                weights(p0,p1,offset_index));
                        }
                    }
                }
                ++node_p;
            }

            auto solver = solver_fatory_->create(cc_obj);
            CCNodeLabels cc_node_labels(cc_graph);

            nifty::graph::opt::common::VerboseVisitor<CCBaseType> visitor;
            solver->optimize(cc_node_labels, nullptr);
            auto e_res = cc_obj.evalNodeLabels(cc_node_labels);
            delete solver;
            f(cc_node_labels, e_res);
        }

        const PixelWiseLmcObjective<DIM> & objective_;
        nifty::ufd::Ufd<uint64_t> ufd_;
        std::shared_ptr<CCLmcFactoryBase>  solver_fatory_;
    };

} // namespace nifty::graph::opt::lifted_multicut
} // namespace nifty::graph::opt
} // namespace nifty::graph
} // namespace nifty


// Copyright (C) 2014 The Regents of the University of California (Regents)
// and Google, Inc. All rights reserved.
//
// 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 Regents or University of California, Google,
//       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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu), John Flynn (jflynn@google.com)

#include "theia/sfm/pose/four_point_relative_pose_partial_rotation.h"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <glog/logging.h>
#include <limits>
#include <math.h>

#include "theia/alignment/alignment.h"

namespace theia {

using Eigen::AngleAxisd;
using Eigen::EigenSolver;
using Eigen::JacobiSVD;
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Quaterniond;
using Eigen::Vector3d;
using Eigen::Vector4d;

namespace {

bool SolveQEP(const Matrix4d& M,
              const Matrix4d& C,
              const Matrix4d& K,
              std::vector<double>* eigenvalues,
              std::vector<Vector4d>* eigenvectors) {
  // Solves the quadratic eigenvalue problem:
  //
  //   Q(s)x = 0
  //
  // where:
  //
  //   Q(s) = s^2*M + s*C + K
  //
  // Returns true if the problem could be solved, false otherwise.
  //
  // This is converted to a generalized eigenvalue as described in:
  //   http://en.wikipedia.org/wiki/Quadratic_eigenvalue_problem
  //
  // The generalized eigenvalue problem is in this form:
  //
  // [ C  K ]z = s[ -M  0  ]z
  // [-I  0 ]     [ 0   -I ]
  //
  // With eigenvector z = [ sx ] and s is the eigenvalue.
  //                      [  x ]
  //
  // The eigenvector of the quadratic eigenvalue problem can be extracted from
  // z.
  //
  // Where z is the eigenvector, s is the eigenvalue.
  // This generalized eigenvalue can be converted to a standard eigenvalue
  // problem by multiplying by the inverse of the RHS matrix. The inverse of
  // the RHS matrix is particularly simple:
  //
  // [ -inv(M)  0 ]
  // [ 0       -I ]
  //
  // So the generalized eigenvalue problem reduces to a standard eigenvalue
  // problem on the constraint matrix:
  //
  // [ -inv(M)C  -inv(M)K ]z = sz
  // [ I         0        ]

  Matrix4d inv_M;
  static const double kDeterminantThreshold = 1e-7;
  bool invert_success;
  // Check that determinant of M is larger than threshold. This threshold only
  // seems to be reached when there is no rotation. TODO(jflynn): verify.
  M.computeInverseWithCheck(inv_M, invert_success, kDeterminantThreshold);
  if (!invert_success) {
    return false;
  }

  // Negate inverse of M.
  inv_M = -1.0 * inv_M;

  // Set up constraint matrix.
  Matrix<double, 8, 8> constraint = Matrix<double, 8, 8>::Zero();
  // Set upper-left to - inv(M) * C.
  constraint.block<4, 4>(0, 0) = inv_M * C;
  // Set upper-right to -inv(M) * K.
  constraint.block<4, 4>(0, 4) = inv_M * K;
  // Set lower-left to identity.
  constraint.block<4, 4>(4, 0) = Matrix4d::Identity();

  // Extract the left eigenvectors and values from the constraint matrix.
  static const double kImagEigenValueTolerance = 1e-9;
  EigenSolver<Matrix<double, 8, 8> > eig_solver(constraint);

  for (int i = 0; i < 8; i++) {
    // Only consider the real eigenvalues and corresponding eigenvectors.
    if (fabs(eig_solver.eigenvalues()[i].imag()) < kImagEigenValueTolerance) {
      eigenvalues->push_back(eig_solver.eigenvalues()[i].real());
      eigenvectors->push_back(
          Vector4d(eig_solver.eigenvectors().col(i).tail(4).real()));
    }
  }
  return true;
}

}  // namespace

void FourPointRelativePosePartialRotation(
    const Vector3d& axis,
    const Vector3d image_one_ray_directions[4],
    const Vector3d image_one_ray_origins[4],
    const Vector3d image_two_ray_directions[4],
    const Vector3d image_two_ray_origins[4],
    std::vector<Quaterniond>* soln_rotations,
    std::vector<Vector3d>* soln_translations) {
  CHECK_DOUBLE_EQ(axis.squaredNorm(), 1.0);
  CHECK_NOTNULL(soln_rotations)->clear();
  CHECK_NOTNULL(soln_translations)->clear();

  // The generalized epipolar constraint between two sets of rays in different
  // coordinate systems (two generalized cameras).
  //
  // The rays are converted to plucker coordinates:
  //   L = [ q  ]
  //       [ p  ]
  //
  // Assuming the first coordinate system has zero rotation and translation,
  // the constraint is:
  //
  //   q2' * [t]x R * q1 + q2' * R * p1 + q1' * R * p2 = 0
  //
  // v' means transpose v, t is unknown translation, [t]x is the matrix
  // cross product form.
  //
  // Re-arranging to isolate t:
  //
  //   -(q2 x (R * q1))' * t + q2' * R * p1 + q1' * R' * p2 = 0
  //
  // In vector form:
  //
  //  [-(q2 x (R * q1))', q2' * R * p1 + q1' * R' * p2 ] * [ t ] = 0
  //                                                       [ 1 ]
  //
  // Each correspondence gives another constraint and the constraints can
  // be stacked to create a constraint matrix.
  //
  // The rotation matrix R can be parameterized, up to scale factor, as:
  //
  //   R ~ 2 * (v * v' + s[v]x) + (s^2 - 1)I
  //
  //   I = Identity matrix.
  //
  // where is v is the known (unit length) axis and s is related to the
  // unknown angle of rotation.
  //
  // The epipolar constraint holds if the rotation matrix is scaled, so the
  // rotation matrix parameterization above can be used. Each row of the
  // constraint matrix is thus a function of s^2 and s, so the constraint can be
  // written as:
  //
  //   [ M * s^2 + C *s + k ] * [ t ] = 0
  //                            [ 1 ]
  //
  // This is standard quadratic eigenvalue problem (QEP) and is solved using
  // standard methods.

  // Creates the matrices for the QEP problem.
  Matrix4d M;
  Matrix4d C;
  Matrix4d K;
  for (int i = 0; i < 4; ++i) {
    const Vector3d& q1(image_one_ray_directions[i]);
    const Vector3d p1(image_one_ray_origins[i].cross(q1));
    const Vector3d& q2(image_two_ray_directions[i]);
    const Vector3d p2(image_two_ray_origins[i].cross(q2));

    M.row(i).head(3) = -q2.cross(q1);
    M.row(i)[3] = q2.dot(p1) + q1.dot(p2);

    C.row(i).head(3) = -2.0 * q2.cross(axis.cross(q1));
    C.row(i)[3] = -2.0 * (q1.dot(axis.cross(p2)) - q2.dot(axis.cross(p1)));

    K.row(i).head(3) = -(2.0 * q1.dot(axis) * q2.cross(axis) - q2.cross(q1));
    K.row(i)[3] =
        -q2.dot(p1) - q1.dot(p2) +
        2.0 * (q2.dot(axis) * p1.dot(axis) + q1.dot(axis) * p2.dot(axis));
  }

  std::vector<double> eigenvalues;
  std::vector<Vector4d> eigenvectors;
  static const double kWTolerance = 1e-7;
  if (SolveQEP(M, C, K, &eigenvalues, &eigenvectors)) {
    // Extracts the translations and rotations from the eigenvalues and
    // eigenvectors of the QEP problem.
    for (int i = 0; i < eigenvalues.size(); ++i) {
      if (fabs(eigenvectors[i].w()) > kWTolerance) {
        Quaterniond quat(eigenvalues[i], axis[0], axis[1], axis[2]);
        quat.normalize();
        soln_rotations->push_back(quat);
        soln_translations->push_back(
            Vector3d(eigenvectors[i].x() / eigenvectors[i].w(),
                     eigenvectors[i].y() / eigenvectors[i].w(),
                     eigenvectors[i].z() / eigenvectors[i].w()));
      }
    }
  } else {
    // When there is zero rotation the vector part of the quaternion disappears
    // and it becomes ([0], 1) (where [0] is the zero vector) and the SolveQEP
    // method cannot be used.
    // However from the equations for M, C, and K above we can see that the
    // C and K matrices contain the axis, which is 0 assuming zero rotation,
    // so to solve for the translation we can directly extract the null space
    // of M.
    // Alternatively this can be derived directly from the generalized epipolar
    // constraint, after substituting identity for the rotation matrix.
    eigenvectors.clear();

    JacobiSVD<Matrix4d> svd = M.jacobiSvd(Eigen::ComputeFullV);
    const Vector4d eigenvector(svd.matrixV().col(3));

    if (fabs(eigenvector[3]) > kWTolerance) {
      soln_rotations->push_back(Quaterniond(AngleAxisd(0.0, axis)));
      soln_translations->push_back(Vector3d(eigenvector[0] / eigenvector[3],
                                            eigenvector[1] / eigenvector[3],
                                            eigenvector[2] / eigenvector[3]));
    }
  }
}

}  // namespace theia


/*
 [auto_generated]
 boost/numeric/odeint/stepper/adams_moulton.hpp

 [begin_description]
 Implementation of the Adams-Moulton method. This is method is not a real stepper, it is more a helper class
 which computes the corrector step in the Adams-Bashforth-Moulton method.
 [end_description]

 Copyright 2011-2012 Karsten Ahnert
 Copyright 2011-2013 Mario Mulansky
 Copyright 2012 Christoph Koke

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_MOULTON_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_MOULTON_HPP_INCLUDED

#include <boost/numeric/odeint/util/bind.hpp>

#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4_classic.hpp>

#include <boost/numeric/odeint/stepper/detail/adams_moulton_call_algebra.hpp>
#include <boost/numeric/odeint/stepper/detail/adams_moulton_coefficients.hpp>
#include <boost/numeric/odeint/stepper/detail/rotating_buffer.hpp>

namespace boost {
namespace numeric {
namespace odeint {

/*
 * Static implicit Adams-Moulton multistep-solver without step size control and without dense output.
 */
template<
size_t Steps ,
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
class adams_moulton
{
private:

public :

    typedef State state_type;
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef Value value_type;
    typedef Deriv deriv_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef Time time_type;
    typedef Algebra algebra_type;
    typedef Operations operations_type;
    typedef Resizer resizer_type;
    typedef stepper_tag stepper_category;

    typedef adams_moulton< Steps , State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_type;

    static const size_t steps = Steps;

    typedef unsigned short order_type;
    static const order_type order_value = steps + 1;

    typedef detail::rotating_buffer< wrapped_deriv_type , steps > step_storage_type;

    adams_moulton( )
    : m_coefficients() , m_dxdt() , m_resizer() ,
      m_algebra_instance() , m_algebra( m_algebra_instance )
    { }

    adams_moulton( algebra_type &algebra )
    : m_coefficients() , m_dxdt() , m_resizer() ,
      m_algebra_instance() , m_algebra( algebra )
    { }

    adams_moulton& operator=( const adams_moulton &stepper )
    {
        m_dxdt = stepper.m_dxdt;
        m_resizer = stepper.m_resizer;
        m_algebra = stepper.m_algebra;
        return *this;
    }

    order_type order( void ) const { return order_value; }

    /*
     * Version 1 : do_step( system , x , t , dt , buf );
     *
     * solves the forwarding problem
     */
    template< class System , class StateInOut , class StateIn , class ABBuf >
    void do_step( System system , StateInOut &x , StateIn const & pred , time_type t , time_type dt , const ABBuf &buf )
    {
        do_step( system , x , pred , t , x , dt , buf );
    }

    template< class System , class StateInOut , class StateIn , class ABBuf >
    void do_step( System system , const StateInOut &x , StateIn const & pred , time_type t , time_type dt , const ABBuf &buf )
    {
        do_step( system , x , pred , t , x , dt , buf );
    }

    /*
     * Version 2 : do_step( system , in , t , out , dt , buf );
     *
     * solves the forwarding problem
     */
    template< class System , class StateIn , class PredIn , class StateOut , class ABBuf >
    void do_step( System system , const StateIn &in , const PredIn &pred , time_type t , StateOut &out , time_type dt , const ABBuf &buf )
    {
        do_step_impl( system , in , pred , t , out , dt , buf );
    }

    template< class System , class StateIn , class PredIn , class StateOut , class ABBuf >
    void do_step( System system , const StateIn &in , const PredIn &pred , time_type t , const StateOut &out , time_type dt , const ABBuf &buf )
    {
        do_step_impl( system , in , pred , t , out , dt , buf );
    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_impl( x );
    }

    algebra_type& algebra()
    {   return m_algebra; }

    const algebra_type& algebra() const
    {   return m_algebra; }

private:

    template< class System , class StateIn , class PredIn , class StateOut , class ABBuf >
    void do_step_impl( System system , const StateIn &in , const PredIn &pred , time_type t , StateOut &out , time_type dt , const ABBuf &buf )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );
        sys( pred , m_dxdt.m_v , t );
        detail::adams_moulton_call_algebra< steps , algebra_type , operations_type >()( m_algebra , in , out , m_dxdt.m_v , buf , m_coefficients , dt );
    }

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
    }

    const detail::adams_moulton_coefficients< value_type , steps > m_coefficients;
    wrapped_deriv_type m_dxdt;
    resizer_type m_resizer;

protected:

    algebra_type m_algebra_instance;
    algebra_type &m_algebra;
};

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_MOULTON_HPP_INCLUDED


#ifndef _CELERITE2_REVERSE_HPP_DEFINED_
#define _CELERITE2_REVERSE_HPP_DEFINED_

#include <Eigen/Core>
#include "internal.hpp"

namespace celerite2 {
namespace core {

template <typename Input, typename Coeffs, typename Diag, typename LowRank, typename Work, typename InputOut, typename CoeffsOut, typename DiagOut,
          typename LowRankOut>
void factor_rev(const Eigen::MatrixBase<Input> &t,           // (N,)
                const Eigen::MatrixBase<Coeffs> &c,          // (J,)
                const Eigen::MatrixBase<Diag> &a,            // (N,)
                const Eigen::MatrixBase<LowRank> &U,         // (N, J)
                const Eigen::MatrixBase<LowRank> &V,         // (N, J)
                const Eigen::MatrixBase<Diag> &d,            // (N,)
                const Eigen::MatrixBase<LowRank> &W,         // (N, J)
                const Eigen::MatrixBase<Work> &S,            // (N, J*J)
                const Eigen::MatrixBase<Diag> &bd,           // (N,)
                const Eigen::MatrixBase<LowRank> &bW,        // (N, J)
                Eigen::MatrixBase<InputOut> const &bt_out,   // (N,)
                Eigen::MatrixBase<CoeffsOut> const &bc_out,  // (J,)
                Eigen::MatrixBase<DiagOut> const &ba_out,    // (N,)
                Eigen::MatrixBase<LowRankOut> const &bU_out, // (N, J)
                Eigen::MatrixBase<LowRankOut> const &bV_out  // (N, J)

) {
  UNUSED(a);
  UNUSED(V);

  ASSERT_ROW_MAJOR(Work);

  typedef typename Diag::Scalar Scalar;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, LowRank::ColsAtCompileTime> Inner;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;

  Eigen::Index N = U.rows(), J = U.cols();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_VEC(DiagOut, ba, N);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bV, N, J);

  // Make local copies of the gradients that we need
  Inner Sn(J, J), bS(J, J);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Sn.data(), 1, J * J);
  Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, 1> bSWT;
  CoeffVector p(J), bp(J);

  Scalar dt, factor;
  bS.setZero();
  bt.setZero();
  bc.setZero();
  ba.noalias() = bd;
  bV.noalias() = bW;
  bV.array().colwise() /= d.array();
  for (Eigen::Index n = N - 1; n > 0; --n) {
    dt = t(n - 1) - t(n);
    p  = exp(c.array() * dt);

    ptr = S.row(n);

    // Step 6
    ba(n) -= W.row(n) * bV.row(n).transpose();
    bU.row(n).noalias() = -(bV.row(n) + 2.0 * ba(n) * U.row(n)) * Sn * p.asDiagonal();
    bS.noalias() -= U.row(n).transpose() * (bV.row(n) + ba(n) * U.row(n));

    // Step 4
    bp.array() = (bS * Sn + Sn.transpose() * bS).diagonal().array() * p.array();
    bc.noalias() += dt * bp;
    factor = (c.array() * bp.array()).sum();
    bt(n) -= factor;
    bt(n - 1) += factor;

    // Step 3
    bS   = p.asDiagonal() * bS * p.asDiagonal();
    bSWT = bS * W.row(n - 1).transpose();
    ba(n - 1) += W.row(n - 1) * bSWT;
    bV.row(n - 1).noalias() += W.row(n - 1) * (bS + bS.transpose());
  }

  bU.row(0).setZero();
  ba(0) -= bV.row(0) * W.row(0).transpose();
}

template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename Work, typename InputOut, typename CoeffsOut,
          typename LowRankOut, typename RightHandSideOut>
void solve_lower_rev(const Eigen::MatrixBase<Input> &t,                // (N,)
                     const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                     const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                     const Eigen::MatrixBase<LowRank> &W,              // (N, J)
                     const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                     const Eigen::MatrixBase<RightHandSide> &Z,        // (N, nrhs)
                     const Eigen::MatrixBase<Work> &F,                 // (N, J*nrhs)
                     const Eigen::MatrixBase<RightHandSide> &bZ,       // (N, nrhs)
                     Eigen::MatrixBase<InputOut> const &bt_out,        // (N,)
                     Eigen::MatrixBase<CoeffsOut> const &bc_out,       // (J,)
                     Eigen::MatrixBase<LowRankOut> const &bU_out,      // (N, J)
                     Eigen::MatrixBase<LowRankOut> const &bW_out,      // (N, J)
                     Eigen::MatrixBase<RightHandSideOut> const &bY_out // (N, nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  Eigen::Index N = t.rows(), J = c.rows();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bW, N, J);
  CAST_BASE(RightHandSideOut, bY);

  bt.setZero();
  bc.setZero();
  bU.setZero();
  bW.setZero();
  bY = bZ;
  internal::forward_rev<true>(t, c, U, W, Y, Z, F, bY, bt, bc, bU, bW, bY);
}

template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename Work, typename InputOut, typename CoeffsOut,
          typename LowRankOut, typename RightHandSideOut>
void solve_upper_rev(const Eigen::MatrixBase<Input> &t,                // (N,)
                     const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                     const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                     const Eigen::MatrixBase<LowRank> &W,              // (N, J)
                     const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                     const Eigen::MatrixBase<RightHandSide> &Z,        // (N, nrhs)
                     const Eigen::MatrixBase<Work> &F,                 // (N, J*nrhs)
                     const Eigen::MatrixBase<RightHandSide> &bZ,       // (N, nrhs)
                     Eigen::MatrixBase<InputOut> const &bt_out,        // (N,)
                     Eigen::MatrixBase<CoeffsOut> const &bc_out,       // (J,)
                     Eigen::MatrixBase<LowRankOut> const &bU_out,      // (N, J)
                     Eigen::MatrixBase<LowRankOut> const &bW_out,      // (N, J)
                     Eigen::MatrixBase<RightHandSideOut> const &bY_out // (N, nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  Eigen::Index N = t.rows(), J = c.rows();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bW, N, J);
  CAST_BASE(RightHandSideOut, bY);

  bt.setZero();
  bc.setZero();
  bU.setZero();
  bW.setZero();
  bY = bZ;
  internal::backward_rev<true>(t, c, U, W, Y, Z, F, bY, bt, bc, bU, bW, bY);
}

template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename Work, typename InputOut, typename CoeffsOut,
          typename LowRankOut, typename RightHandSideOut>
void matmul_lower_rev(const Eigen::MatrixBase<Input> &t,                // (N,)
                      const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                      const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                      const Eigen::MatrixBase<LowRank> &V,              // (N, J)
                      const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                      const Eigen::MatrixBase<RightHandSide> &Z,        // (N, nrhs)
                      const Eigen::MatrixBase<Work> &F,                 // (N, J*nrhs)
                      const Eigen::MatrixBase<RightHandSide> &bZ,       // (N, nrhs)
                      Eigen::MatrixBase<InputOut> const &bt_out,        // (N,)
                      Eigen::MatrixBase<CoeffsOut> const &bc_out,       // (J,)
                      Eigen::MatrixBase<LowRankOut> const &bU_out,      // (N, J)
                      Eigen::MatrixBase<LowRankOut> const &bV_out,      // (N, J)
                      Eigen::MatrixBase<RightHandSideOut> const &bY_out // (N, nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  Eigen::Index N = t.rows(), J = c.rows();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bV, N, J);
  CAST_MAT(RightHandSideOut, bY, N, Y.cols());

  bt.setZero();
  bc.setZero();
  bU.setZero();
  bV.setZero();
  bY.setZero();
  internal::forward_rev<false>(t, c, U, V, Y, Z, F, bZ, bt, bc, bU, bV, bY);
}

template <typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename Work, typename InputOut, typename CoeffsOut,
          typename LowRankOut, typename RightHandSideOut>
void matmul_upper_rev(const Eigen::MatrixBase<Input> &t,                // (N,)
                      const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                      const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                      const Eigen::MatrixBase<LowRank> &V,              // (N, J)
                      const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                      const Eigen::MatrixBase<RightHandSide> &Z,        // (N, nrhs)
                      const Eigen::MatrixBase<Work> &F,                 // (N, J*nrhs)
                      const Eigen::MatrixBase<RightHandSide> &bZ,       // (N, nrhs)
                      Eigen::MatrixBase<InputOut> const &bt_out,        // (N,)
                      Eigen::MatrixBase<CoeffsOut> const &bc_out,       // (J,)
                      Eigen::MatrixBase<LowRankOut> const &bU_out,      // (N, J)
                      Eigen::MatrixBase<LowRankOut> const &bV_out,      // (N, J)
                      Eigen::MatrixBase<RightHandSideOut> const &bY_out // (N, nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  Eigen::Index N = t.rows(), J = c.rows();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bV, N, J);
  CAST_MAT(RightHandSideOut, bY, N, Y.cols());

  bt.setZero();
  bc.setZero();
  bU.setZero();
  bV.setZero();
  bY.setZero();
  internal::backward_rev<false>(t, c, U, V, Y, Z, F, bZ, bt, bc, bU, bV, bY);
}

} // namespace core
} // namespace celerite2

#endif // _CELERITE2_REVERSE_HPP_DEFINED_


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_GAMMALN_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_GAMMALN_HPP_INCLUDED

#include <boost/simd/function/std.hpp>
#include <boost/simd/arch/common/detail/generic/gammaln_kernel.hpp>

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/invpi.hpp>
#include <boost/simd/constant/logpi.hpp>
#include <boost/simd/constant/logsqrt2pi.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/zero.hpp>

#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/two.hpp>

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/any.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/if_dec.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_inc.hpp>
#include <boost/simd/function/if_minus.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/is_lez.hpp>
#include <boost/simd/function/log.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/logical_andnot.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/function/sinpi.hpp>
#include <boost/simd/function/sqr.hpp>

#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( gammaln_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_< bd::single_<A0>, X>
                             )
  {
    using sA0 = bd::scalar_of_t<A0>;
    A0 operator() (const A0& a0) const BOOST_NOEXCEPT
    {
      auto inf_result = logical_and(is_lez(a0), is_flint(a0));
      A0 x = if_nan_else(inf_result, a0);
      A0 q = bs::abs(x);
      #ifndef BOOST_SIMD_NO_INFINITES
      inf_result = logical_or(is_equal(x, Inf<A0>()), inf_result);
      #endif
      auto ltza0 = is_ltz(a0);
      size_t nb = bs::nbtrue(ltza0);
      A0 r;
      A0 r1 =  other(q);
      if(nb > 0)
      {
        //treat negative
        r = bs::if_else(inf_result, Inf<A0>(), negative(q, r1));
        if (nb >= A0::static_size) return r;
      }
      A0 r2 = if_else(ltza0, r, r1);
      return bs::if_else(is_equal(a0, Minf<A0>()),
                          Nan<A0>(),
                          bs::if_else(inf_result, Inf<A0>(), r2)
                         );
    }
  private :
    using lA0 = bs::as_logical_t<A0>;
    static inline A0 negative(const A0& q,  const A0& w)
    {
      A0 p = bs::floor(q);
      A0 z = q - p;
      auto test2 = is_less(z, bs::Half<A0>() );
      z = bs::if_minus(test2, z, bs::One<A0>());
      z = q*bs::sinpi(z);
      z =  bs::abs(z);
      return -log(Invpi<A0>()*bs::abs(z))-w;
    }
    static inline A0 other(const A0& x)
    {
      auto xlt650 = is_less(x,A0(6.50) );
      size_t nb = nbtrue(xlt650);
      A0 r0x = x;
      A0 r0z = x;
      A0 r0s = One<A0>();
      A0 r1 = Zero<A0>();
      A0 p =  Nan<A0>();
      if (nb > 0)
      {
        auto kernelC = False<lA0>();
        A0 z = One<A0>();
        A0 tx = if_else_zero(xlt650, x);
        A0 nx = Zero<A0>();

        const A0 _075 = A0(0.75);
        const A0 _150 = A0(1.50);
        const A0 _125 = A0(1.25);
        const A0 _250 = A0(2.50);
        auto xge150 = is_greater_equal(x, _150);
        auto txgt250= is_greater(tx,_250);

        // x >= 1.5
        while (bs::any(logical_and(xge150, txgt250)))
        {
          nx = if_dec(txgt250, nx);
          tx = if_else(txgt250, x + nx, tx);
          z = if_else(txgt250, z*tx, z);
          txgt250= is_greater(tx,_250);
        }
        r0x = if_plus(xge150, x, nx - Two<A0>());
        r0z = if_else(xge150, z, r0z);
        r0s = if_else(xge150,One<A0>(), r0s);

        // x >= 1.25 && x < 1.5
        auto xge125 = is_greater_equal(x, _125);
        auto xge125t = logical_andnot(xge125, xge150);
        if (bs::any(xge125))
        {
          r0x =  if_else(xge125t, dec(x)    , r0x);
          r0z =  if_else(xge125t, z*x       , r0z);
          r0s =  if_else(xge125t, Mone<A0>(), r0s);
        }
        // x >= 0.75&& x < 1.5
        auto xge075  = is_greater_equal(x, _075);
        auto xge075t = logical_andnot(xge075, xge125);
        if (bs::any(xge075t))
        {
          kernelC =  xge075t;
          r0x =  if_else(xge075t, dec(x)    , r0x);
          r0z =  if_else(xge075t, One<A0>() , r0z);
          r0s =  if_else(xge075t, Mone<A0>(), r0s);
          p = detail::gammaln_kernel<A0>::gammalnC(r0x);
        }
        // tx < 1.5 && x < 0.75
        auto txlt150 = logical_andnot(is_less(tx,_150), xge075);
        if (bs::any(txlt150))
        {
          auto orig = txlt150;
          while( bs::any(txlt150) )
          {
            z  = if_else(txlt150, z*tx, z);
            nx = if_inc(txlt150, nx);
            tx = if_else(txlt150, x + nx, tx);
            txlt150= logical_andnot(is_less(tx,_150), xge075);
          }
          r0x =  if_plus(orig, r0x, nx - Two<A0>());
          r0z =  if_else(orig,z         , r0z);
          r0s =  if_else(orig,Mone<A0>(), r0s);
        }
        p =  if_else(kernelC, p, detail::gammaln_kernel<A0>::gammalnB(r0x));
        if (nb >= A0::static_size)
          return fma(r0x, p, r0s*bs::log(bs::abs(r0z)));
      }
      r0z = if_else(xlt650, bs::abs(r0z), x);
      A0 m = bs::log(r0z);
      r1 = fma(r0x, p, r0s*m);
      A0 r2 = fma(x-Half<A0>(),m,Logsqrt2pi<A0>()-x);
      r2 += detail::gammaln_kernel<A0>::gammaln2(rec(sqr(x)))/x;
      return if_else(xlt650, r1, r2);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( gammaln_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_< bd::double_<A0>, X >
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const A0& a0) const BOOST_NOEXCEPT
    {
      auto inf_result = logical_and(is_lez(a0), is_flint(a0));
      A0 x = if_nan_else(inf_result, a0);
      A0 q = bs::abs(x);
      #ifndef BOOST_SIMD_NO_INFINITES
      inf_result = is_equal(q, Inf<A0>());
      #endif
      auto test = is_less(a0, A0(-34.0));
      size_t nb = bs::nbtrue(test);
      A0 r =  Nan<A0>();
      if(nb > 0)
      {
        //treat negative large with reflection
        r = large_negative(q);
        if (nb >= A0::static_size)
          return bs::if_else(inf_result, Nan<A0>(), r);
      }
      A0 r1 = other(a0);
      A0 r2 = if_else(test, r, r1);
      return bs::if_else(is_equal(a0, Minf<A0>()),
                          Nan<A0>(),
                          bs::if_else(inf_result, Inf<A0>(), r2)
                         );
    }
  private :
    static inline A0 large_negative(const A0& q)
    {
      A0 w = gammaln(q);
      A0 p = bs::floor(q);
      A0 z = q - p;
      auto test2 = is_less(z, bs::Half<A0>() );
      z = bs::if_minus(test2, z, bs::One<A0>());
      z = q*bs::sinpi(z);
      z =  bs::abs(z);
      return Logpi<A0>()-log(z)-w;
    }
    static inline A0 other(const A0& xx)
    {
      A0 x =  xx;
      auto test = is_less(x, A0(13.0) );
      size_t nb = nbtrue(test);
      A0 r1 = Zero<A0>();
      if (nb > 0)
      {
        A0 z = One<A0>();
        A0 p = Zero<A0>();
        A0 u = if_else_zero(test, x);
        auto test1 = is_greater_equal(u,Three<A0>());
        while(bs::any(test1))
        {
          p = if_dec(test1, p);
          u = if_else(test1, x+p, u);
          z = if_else(test1, z*u, z);
          test1 = is_greater_equal(u,Three<A0>());
        }
        //all u are less than 3
        auto test2 = is_less(u,Two<A0>());

        while(bs::any(test2))
        {
          z = if_else(test2, z/u, z);
          p = if_inc(test2, p);
          u = if_else(test2, x+p, u);
          test2 = is_less(u,Two<A0>());
        }
        z = bs::abs(z);
        x +=  p-Two<A0>();
        r1 = x * detail::gammaln_kernel<A0>::gammaln1(x)+bs::log(z);
        if (nb >= A0::static_size) return r1;
      }
      A0 r2 = fma(xx-Half<A0>(),bs::log(xx), Logsqrt2pi<A0>()-xx);
      A0 p = rec(sqr(xx));
      r2 += detail::gammaln_kernel<A0>::gammalnA(p)/xx;
      return if_else(test, r1, r2);
    }
  };

} } }

#endif


/*
 * Copyright (c) 2013-2020 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef OPTIMIZE_HPP
#define OPTIMIZE_HPP

#include <iostream>
#include <list>
#include <limits>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/autodif.hpp>

// 0: Do not use TRIM
// 1: use TRIM and use slow trim algorithm
// 2: use TRIM and use fast bat slightly inefficient trim algorithm
// 3: use TRIM and use new trim algorithm

#ifndef OPTIMIZE_TRIM
#define OPTIMIZE_TRIM 0
#endif

#ifndef OPTIMIZE_ZERODIVIDE
#define OPTIMIZE_ZERODIVIDE 2
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

// change scalar function f to -f
template <class F> struct Opt_MakeMinus {
	F f;
	Opt_MakeMinus(F f) : f(f) {}
	template <class T> T operator()(const ub::vector<T>& x) {
		return -f(x);
	}
};

// change scalar function T -> T
// to 1-dimentional vector function T^1 -> T
template <class F> struct Opt_MakeVec {
	F f;
	Opt_MakeVec(F f) : f(f) {}
	template <class T> T operator()(const ub::vector<T>& x) {
		return f(x(0));
	}
};

template <class T, class F>
std::list< ub::vector< interval<T> > >
optimize(const ub::vector< interval<T> >& init, F f, T limit, bool unify = true, int verbose = 0)
{
	std::list< ub::vector< interval<T> > > targets;
	targets.push_back(init);
	return optimize_list(targets, f, limit, unify, verbose);
}

template <class T, class F>
std::list< ub::vector< interval<T> > >
optimize_list(std::list< ub::vector< interval<T> > > targets, F f, T limit, bool unify = true, int verbose = 0)
{
	int s = (targets.front()).size();
	ub::vector< interval<T> > I, C, I1, I2, IR, fdi, C2;
	interval<T> fc, fi, mvf, fc2;
	T tmp, tmp2;
	std::list< ub::vector< interval<T> > > solutions;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	int i, j, k, mi;
	bool flag, errflag;
	interval<T> A, B, J, J2, Itmp;
#if OPTIMIZE_TRIM == 3
	ub::vector< interval<T> > A0, A1, A2; // for new trim algorithm
#endif // OPTIMIZE_TRIM == 3

	C2.resize(s);

	T delta = std::numeric_limits<T>::max();

	while (!targets.empty()) {
		I = targets.front();
		targets.pop_front();
		errflag = false; // evaluation error occurs or not

		try {
			fi = f(I);
		}
		catch (std::domain_error& e) {
			errflag = true;
			goto label;
		}

		if (fi.lower() > delta) {
			continue;
		}

		C = mid(I);
		try {
			fc = f(C);
			autodif< interval<T> >::split(f(autodif< interval<T> >::init(I)), fi, fdi);
		}
		catch (std::domain_error& e) {
			// errflag = true;
			goto label;
		}

		fdi.resize(s); // prepare for constant f
		mvf = fc + inner_prod(fdi, I - C);
		if (mvf.lower() > delta) {
			continue;
		}

		// update delta at C
		tmp = fc.upper();
		if (tmp < delta) delta = tmp;

		// C2 is likely to give small value
		for (i=0; i<s; i++) {
			tmp = mid(fdi(i));
			if (tmp > 0.) tmp2 = I(i).lower();
			else if (tmp < 0.) tmp2 = I(i).upper();
			else tmp2 = mid(I(i));
			C2(i).assign(tmp2, tmp2);
		}

		// update delta at C2
		try {
			fc2 = f(C2);
		}
		catch (std::domain_error& e) {
			// errflag = true;
			goto label;
		}
		tmp = fc2.upper();
		if (tmp < delta) delta = tmp;

#if OPTIMIZE_TRIM >= 1
		// interval shrinking

#if OPTIMIZE_TRIM == 3
		// prepare for new trim algorithm
		A0.resize(s);
		A1.resize(s);
		A2.resize(s);
		for (j=0; j<s; j++) {
			A0(j) = fdi(j) * (I(j)-C(j));
		}
		Itmp = 0.;
		for (j=0; j<s; j++) {
			A1(j) = Itmp;
			Itmp += A0(j);
		}
		Itmp = 0.;
		for (j=s-1; j>=0; j--) {
			A2(j) = Itmp;
			Itmp += A0(j);
		}
#endif // OPTIMIZE_TRIM == 3

		IR = I;
		flag = false; // non-existence in I turns out or not
		for (j=0; j<s; j++) {
			B = fdi(j);

#if OPTIMIZE_TRIM == 1
			// calculate A simply
			// simple but slow
			A = 0.;
			for (k=0; k<s; k++) {
				if (k == j) continue;
				A += fdi(k) * (I(k)-C(k));
			}
			A += fc;
#endif // OPTIMIZE_TRIM == 1
#if OPTIMIZE_TRIM == 2
			// old trim algorithm
			// calculate back A from mvf
			A = mvf;
			Itmp = B * (I(j)-C(j));
			rop<T>::begin();
			tmp = rop<T>::sub_down(A.lower(), Itmp.lower());
			tmp2 = rop<T>::sub_up(A.upper(), Itmp.upper());
			rop<T>::end();
			A.assign(tmp, tmp2);
#endif // OPTIMIZE_TRIM == 2
#if OPTIMIZE_TRIM == 3
			// new trim algorithm
			A = fc + A1(j) + A2(j);
#endif // OPTIMIZE_TRIM == 3

			// A -= delta;
			A -= interval<T>(-std::numeric_limits<T>::infinity(), delta);

			if (zero_in(B)) {
#if OPTIMIZE_ZERODIVIDE >= 1
				bool bdummy;
				if (rad(B) <= 0) continue;
				J = C(j) - division_part1(A, B, bdummy);
				J2 = C(j) - division_part2(A, B);
				if (overlap(IR(j), J)) {
					if (overlap(IR(j), J2)) {
#if OPTIMIZE_ZERODIVIDE == 2
						if (overlap(J, J2)) continue;
						// interval division
						I1 = IR;
						I2 = IR;
						I1(j) = intersect(IR(j), J);
						I2(j) = intersect(IR(j), J2);
						targets.push_back(I1);
						targets.push_back(I2);
						flag = true;
						break;
#else
						continue;
#endif
					} else {
						IR(j) = intersect(IR(j), J);
					}
				} else {
					if (overlap(IR(j), J2)) {
						IR(j) = intersect(IR(j), J2);
					} else {
						flag = true;
						break;
					}
				}
#else
				continue;
#endif
			} else {
				J = C(j) - A/B;
				if (overlap(IR(j), J)) {
					IR(j) = intersect(IR(j), J);
				} else {
					flag = true;
					break;
				}
			}
		}

		// non-existence in I turns out
		if (flag == true) {
			continue;
		}

		I = IR;

#endif // OPTIMIZE_TRIM >= 1

		label:;

		tmp2 = 0.;
		for (i=0; i<s; i++) {
			tmp = width(I(i));
			if (tmp > tmp2) {
				tmp2 = tmp; mi = i;
			}
		}

		if (tmp2 < limit && errflag == false) {
			if (verbose >= 1) {
				std::cout << I << "\n";
			}
			if (unify) {
				while (true) {
					flag = false;
                                        p = solutions.begin();
					while (p != solutions.end()) {
						if (overlap(*p, I)) {
							I = hull(I, *p);
							p = solutions.erase(p);
							flag = true;
							continue;
						}
						p++;
					}
					if (flag == false) break;
				}
			}
			solutions.push_back(I);
			continue;
		}

		tmp = mid(I(mi));
		I1 = I; I2 = I;
		I1(mi).assign(I1(mi).lower(), tmp);
		I2(mi).assign(tmp, I2(mi).upper());
		targets.push_back(I1);
		targets.push_back(I2);
	}

	if (verbose >= 1) {
		std::cout << delta << "\n";
	}

	return solutions;
}

// rename of optimize
template <class T, class F>
std::list< ub::vector< interval<T> > >
minimize(const ub::vector< interval<T> >& x, F f, T limit, bool unify = true, int verbose = 0)
{
        return optimize(x, f, limit, unify, verbose);
}

// specify maximum number of subdivision instead of width limit
template <class T, class F>
std::list< ub::vector< interval<T> > >
minimize(const ub::vector< interval<T> >& x, F f, int n = 1000, bool unify = true, int verbose = 0)
{
	int i;
	T tmp;
	tmp = 1.;
	for (i=0; i<x.size(); i++) {
		tmp *= width(x(i));
	}
	T limit = (T)std::pow(tmp / n, 1.0/x.size());
        return minimize(x, f, limit, unify, verbose);
}

// return only mininum value
template <class T, class F>
interval<T>
minimize_value(const ub::vector< interval<T> >& x, F f, T limit, int verbose = 0) {
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	T ret_l, ret_u;
	interval<T> tmp;

	ret_l = std::numeric_limits<T>::infinity();
	ret_u = std::numeric_limits<T>::infinity();

	result = minimize(x, f, limit, false, verbose); // disable unify
	p = result.begin();
	while (p != result.end()) {
		tmp = f(*p);
		ret_u = std::min(ret_u, tmp.upper());
		ret_l = std::min(ret_l, tmp.lower());
		p++;
	}

	return kv::interval<T>(ret_l, ret_u);
}

// return only mininum value
// specify maximum number of subdivision instead of width limit
template <class T, class F>
interval<T>
minimize_value(const ub::vector< interval<T> >& x, F f, int n = 1000, int verbose = 0) {
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	T ret_l, ret_u;
	interval<T> tmp;

	ret_l = std::numeric_limits<T>::infinity();
	ret_u = std::numeric_limits<T>::infinity();

	result = minimize(x, f, n, false, verbose); // disable unify
	p = result.begin();
	while (p != result.end()) {
		tmp = f(*p);
		ret_u = std::min(ret_u, tmp.upper());
		ret_l = std::min(ret_l, tmp.lower());
		p++;
	}

	return kv::interval<T>(ret_l, ret_u);
}

// maximize/maximize_value

template <class T, class F>
std::list< ub::vector< interval<T> > >
maximize(const ub::vector< interval<T> >& x, F f, T limit, bool unify = true, int verbose = 0)
{
        return minimize(x, Opt_MakeMinus<F>(f), limit, unify, verbose);
}

template <class T, class F>
std::list< ub::vector< interval<T> > >
maximize(const ub::vector< interval<T> >& x, F f, int n = 1000, bool unify = true, int verbose = 0)
{
        return minimize(x, Opt_MakeMinus<F>(f), n, unify, verbose);
}

template <class T, class F>
interval<T>
maximize_value(const ub::vector< interval<T> >& x, F f, T limit, int verbose = 0) {
	return -minimize_value(x, Opt_MakeMinus<F>(f), limit, verbose);
}

template <class T, class F>
interval<T>
maximize_value(const ub::vector< interval<T> >& x, F f, int n = 1000, int verbose = 0) {
	return -minimize_value(x, Opt_MakeMinus<F>(f), n, verbose);
}

// one dimensional version of minimize/minimize_value

template <class T, class F>
std::list< interval<T> >
minimize(const interval<T>& x, F f, T limit, bool unify = true, int verbose = 0)
{
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	std::list< interval<T> > result2;
	v(0) = x;
        result = minimize(v, g, limit, unify, verbose);
	p = result.begin();
	while (p != result.end()) {
		result2.push_back((*(p++))(0));
	}
	return result2;
}

template <class T, class F>
std::list< interval<T> >
minimize(const interval<T>& x, F f, int n = 1000, bool unify = true, int verbose = 0)
{
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	std::list< interval<T> > result2;
	v(0) = x;
        result = minimize(v, g, n, unify, verbose);
	p = result.begin();
	while (p != result.end()) {
		result2.push_back((*(p++))(0));
	}
	return result2;
}

template <class T, class F>
interval<T>
minimize_value(const interval<T>& x, F f, T limit, int verbose = 0) {
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	v(0) = x;
	return minimize_value(v, g, limit, verbose);
}

template <class T, class F>
interval<T>
minimize_value(const interval<T>& x, F f, int n = 1000, int verbose = 0) {
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	v(0) = x;
	return minimize_value(v, g, n, verbose);
}

// one dimensional version of maximize/maximize_value

template <class T, class F>
std::list< interval<T> >
maximize(const interval<T>& x, F f, T limit, bool unify = true, int verbose = 0)
{
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	std::list< interval<T> > result2;
	v(0) = x;
        result = maximize(v, g, limit, unify, verbose);
	p = result.begin();
	while (p != result.end()) {
		result2.push_back((*(p++))(0));
	}
	return result2;
}

template <class T, class F>
std::list< interval<T> >
maximize(const interval<T>& x, F f, int n = 1000, bool unify = true, int verbose = 0)
{
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	std::list< interval<T> > result2;
	v(0) = x;
        result = maximize(v, g, n, unify, verbose);
	p = result.begin();
	while (p != result.end()) {
		result2.push_back((*(p++))(0));
	}
	return result2;
}

template <class T, class F>
interval<T>
maximize_value(const interval<T>& x, F f, T limit, int verbose = 0) {
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	v(0) = x;
	return maximize_value(v, g, limit, verbose);
}

template <class T, class F>
interval<T>
maximize_value(const interval<T>& x, F f, int n = 1000, int verbose = 0) {
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	v(0) = x;
	return maximize_value(v, g, n, verbose);
}

} // namespace kv

#endif // OPTIMIZE_HPP