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

#include "structures.h"
#include "transformations.h"
#include "cauchy.h"
#include "perspective_camera_with_intrinsics_tait_bryan_cw_jacobian.h"
#include "perspective_camera_with_intrinsics_rodrigues_cw_jacobian.h"
#include "perspective_camera_with_intrinsics_quaternion_cw_jacobian.h"
#include "quaternion_constraint_jacobian.h"
#include "perspective_camera_tait_bryan_wc_jacobian.h"

double k1 = 0.1686;
double k2 = -0.515827;
double k3 = 0.446983;
double k4 = 0.0;
double k5 = 0.0;
double k6 = 0.0;
double p1 = -0.00138148;
double p2 = 0.000127175;

double k1tmp;
double k2tmp;
double k3tmp;
double k4tmp;
double k5tmp;
double k6tmp;
double p1tmp;
double p2tmp;

struct KeyPoint{
	double u;
	double v;
	std::pair<int,int> index_to_tie_point;
};

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

std::vector<std::vector<Eigen::Vector3d>> tie_points;
std::vector<Camera> cameras;
PerspectiveCameraParams cam_params;

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 = -62, rotate_y = 21;
float translate_z = -12.4502;
float translate_x = -0.899999, translate_y = -1.49993;
bool show_only_one = true;

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[]){
	k1tmp = k1;
	k2tmp = k2;
	k3tmp = k3;
	p1tmp = p1;
	p2tmp = p2;

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

	cam_params.fx = 1727;
	cam_params.fy = 1727;
	cam_params.cx = 985;
	cam_params.cy = 522;

	for(size_t i = 0 ; i < 8; i++){
		std::vector<Eigen::Vector3d> tps;
		for(size_t j = 0 ; j < 8; j++){
		Eigen::Vector3d p;
		p.x() = i;
		p.y() = j;
		p.z() = 0;
		tps.push_back(p);
		}
		tie_points.push_back(tps);
	}

	for(float i = 3 ; i < 5; i+=0.5){
		Camera c;
		c.pose = Eigen::Affine3d::Identity();
		c.pose(0,3) = 4;
		c.pose(1,3) = i;
		c.pose(2,3) = -10;
		cameras.push_back(c);
	}

	for(size_t i = 0 ; i < cameras.size(); i++){
		for(size_t j = 0; j < tie_points.size(); j++){
			std::vector<KeyPoint> kps;
			for(size_t k = 0; k < tie_points[j].size(); k++){
				KeyPoint kp;
				kp.index_to_tie_point = std::pair<int,int>(j,k);

				TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose.inverse());
				projection_perspective_camera_with_intrinsics_tait_bryan_cw(kp.u, kp.v, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
						tie_points[j][k].x(), tie_points[j][k].y(), tie_points[j][k].z(),  k1,  k2,  k3, p1, p2);
				kps.push_back(kp);
			}
			cameras[i].key_points.push_back(kps);
		}
	}

	k1 = 0.0;
	k2 = 0.0;
	p1 = 0.0;
	p2 = 0.0;
	k3 = 0.0;

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

Eigen::Vector3d get_intersection(float u, float v, Eigen::Affine3d camera_pose_wc){
	cv::Mat mat(1, 2, CV_32F);
	mat.at<float>(0, 0) = u;
	mat.at<float>(0, 1) = v;

	cv::Mat cv_cam_matrix = (cv::Mat_<float>(3, 3) << cam_params.fx, 0, cam_params.cx, 0, cam_params.fy, cam_params.cy, 0, 0, 1);
	cv::Mat cv_dist_params = (cv::Mat_<float>(5, 1) << k1, k2, p1, p2, k3);

	mat = mat.reshape(2);
	cv::undistortPoints(mat, mat, cv_cam_matrix, cv_dist_params, cv::Mat(), cv_cam_matrix,
						cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::MAX_ITER, 20, 1e-6));
	mat = mat.reshape(1);

	double undistorted_u = mat.at<float>(0, 0);
	double undistorted_v = mat.at<float>(0, 1);

	Eigen::Matrix3d K;
	K(0,0) = cam_params.fx;
	K(0,1) = 0;
	K(0,2) = cam_params.cx;
	K(1,0) = 0;
	K(1,1) = cam_params.fy;
	K(1,2) = cam_params.cy;
	K(2,0) = 0;
	K(2,1) = 0;
	K(2,2) = 1;

	Eigen::Vector3d p(undistorted_u, undistorted_v, 1);
	Eigen::Vector3d r =  K.inverse() * p;
	Eigen::Matrix3d R = camera_pose_wc.rotation();
	Eigen::Vector3d T = camera_pose_wc.translation();
	Eigen::Vector3d rt = R*r + T;

	Eigen::Vector3d n(0,0,-1);
	Eigen::Vector3d a(camera_pose_wc(0,3), camera_pose_wc(1,3), camera_pose_wc(2,3));
	Eigen::Vector3d b = rt;

	Eigen::Vector3d ba = b-a;
	Eigen::Vector3d intersection = a + (((0 - n.dot(a))/n.dot(ba)) * ba);
	return intersection;
}

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);

	glLineWidth(2);
	glColor3f(1,0,0);
	glBegin(GL_LINES);
	for(size_t i = 0 ; i < tie_points.size(); i++){
		for(size_t j = 0 ; j < tie_points[i].size(); j++){
			if(j+1 < tie_points[i].size()){
				glVertex3f(tie_points[i][j].x(), tie_points[i][j].y(), tie_points[i][j].z());
				glVertex3f(tie_points[i][j+1].x(), tie_points[i][j+1].y(), tie_points[i][j+1].z());
			}

			if(i+1 < tie_points.size()){
				glVertex3f(tie_points[i][j].x(), tie_points[i][j].y(), tie_points[i][j].z());
				glVertex3f(tie_points[i+1][j].x(), tie_points[i+1][j].y(), tie_points[i+1][j].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();
	}

	int number_cameras = 1;
	if(!show_only_one){
		number_cameras = cameras.size();
	}
	glColor3f(0,1,0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < number_cameras; i++){
		for(size_t j = 0; j < cameras[i].key_points.size(); j++){
			for(size_t k = 0; k < cameras[i].key_points[j].size(); k++){
				if(k+1 < tie_points[j].size()){
					Eigen::Vector3d intersection = get_intersection(cameras[i].key_points[j][k].u, cameras[i].key_points[j][k].v, cameras[i].pose);
					glVertex3f(intersection.x(), intersection.y(), intersection.z());

					intersection = get_intersection(cameras[i].key_points[j][k+1].u, cameras[i].key_points[j][k+1].v, cameras[i].pose);
					glVertex3f(intersection.x(), intersection.y(), intersection.z());
				}

				if(j+1 < tie_points.size()){
					Eigen::Vector3d intersection = get_intersection(cameras[i].key_points[j][k].u, cameras[i].key_points[j][k].v, cameras[i].pose);
					glVertex3f(intersection.x(), intersection.y(), intersection.z());

					intersection = get_intersection(cameras[i].key_points[j+1][k].u, cameras[i].key_points[j+1][k].v, cameras[i].pose);
					glVertex3f(intersection.x(), intersection.y(), intersection.z());
				}
			}
		}
	}
	glEnd();
	glPointSize(1);

	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.px = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1;
				pose.py = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1;
				pose.pz = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1;
				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 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			int number_intrinsic_paramters = 5;

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras[i].key_points.size(); j++){
					for(size_t k = 0; k < cameras[i].key_points[j].size(); k++){
						TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose.inverse());

						Eigen::Matrix<double, 2, 1> delta;
						observation_equation_perspective_camera_with_intrinsics_tait_bryan_cw(delta, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy,
								pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								std::floor(cameras[i].key_points[j][k].u), std::floor(cameras[i].key_points[j][k].v), k1, k2, k3, p1, p2);

						Eigen::Matrix<double, 2, 14> jacobian;
						observation_equation_perspective_camera_with_intrinsics_tait_bryan_cw_jacobian(jacobian, cam_params.fx, cam_params.fy,
								pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								k1, k2, k3, p1, p2);

						int ir = tripletListB.size();
						int ic_camera = i * 6;

						for(size_t l = 0; l < number_intrinsic_paramters; l++){
							tripletListA.emplace_back(ir     , l, -jacobian(0,l));
						}
						for(size_t l = 0; l < number_intrinsic_paramters; l++){
							tripletListA.emplace_back(ir + 1  , l, -jacobian(1,l));
						}

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

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

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

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

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 6 + number_intrinsic_paramters);
			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() * 6 + number_intrinsic_paramters, cameras.size() * 6 + number_intrinsic_paramters);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 6 + number_intrinsic_paramters, 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() * 6 + number_intrinsic_paramters){
				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;

				k1 += h_x[counter++];
				k2 += h_x[counter++];
				k3 += h_x[counter++];
				p1 += h_x[counter++];
				p2 += h_x[counter++];

				for(size_t i = 0; i < cameras.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose.inverse());
					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++];

					cameras[i].pose = affine_matrix_from_pose_tait_bryan(pose).inverse();
				}

				std::cout << "desired intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1tmp << " k2: " << k2tmp << " k3: " << k3tmp << " k4: " << k4tmp << " k5: " << k5tmp << " k6: " << k6tmp << " p1: " << p1tmp << " p2: " << p2tmp << std::endl;
				std::cout << "computer intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1 << " k2: " << k2 << " k3: " << k3 << " k4: " << k4 << " k5: " << k5 << " k6: " << k6 << " p1: " << p1 << " p2: " << p2 << std::endl;

			}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.px += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.py += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.pz += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				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;

			int number_intrinsic_paramters = 5;

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras[i].key_points.size(); j++){
					for(size_t k = 0; k < cameras[i].key_points[j].size(); k++){

						RodriguesPose pose = pose_rodrigues_from_affine_matrix(cameras[i].pose.inverse());

						Eigen::Matrix<double, 2, 1> delta;
						observation_equation_perspective_camera_with_intrinsics_rodrigues_cw(delta, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy,
								pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								std::floor(cameras[i].key_points[j][k].u), std::floor(cameras[i].key_points[j][k].v), k1, k2, k3, p1, p2);

						Eigen::Matrix<double, 2, 14> jacobian;
						observation_equation_perspective_camera_with_intrinsics_rodrigues_cw_jacobian(jacobian, cam_params.fx, cam_params.fy,
								pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								k1, k2, k3, p1, p2);

						int ir = tripletListB.size();
						int ic_camera = i * 6;

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

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

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

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

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

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

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 6 + number_intrinsic_paramters);
			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() * 6 + number_intrinsic_paramters, cameras.size() * 6 + number_intrinsic_paramters);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 6 + number_intrinsic_paramters, 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() * 6 + number_intrinsic_paramters){
				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;

				k1 += h_x[counter++];
				k2 += h_x[counter++];
				k3 += h_x[counter++];
				p1 += h_x[counter++];
				p2 += h_x[counter++];

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

					cameras[i].pose = affine_matrix_from_pose_rodrigues(pose).inverse();
				}

				std::cout << "desired intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1tmp << " k2: " << k2tmp << " k3: " << k3tmp << " p1: " << p1tmp << " p2: " << p2tmp << std::endl;
				std::cout << "computer intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1 << " k2: " << k2 << " k3: " << k3 << " p1: " << p1 << " p2: " << p2 << std::endl;

			}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.px += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				posetb.py += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				posetb.pz += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				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;

			int number_intrinsic_paramters = 5;

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

						Eigen::Matrix<double, 2, 1> delta;
						observation_equation_perspective_camera_with_intrinsics_quaternion_cw(delta, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy,
								pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								std::floor(cameras[i].key_points[j][k].u), std::floor(cameras[i].key_points[j][k].v), k1, k2, k3, p1, p2);

						Eigen::Matrix<double, 2, 15> jacobian;
						observation_equation_perspective_camera_with_intrinsics_quaternion_cw_jacobian(jacobian, cam_params.fx, cam_params.fy,
								pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								k1, k2, k3, p1, p2);

						int ir = tripletListB.size();
						int ic_camera = i * 7;

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

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

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

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

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

						tripletListB.emplace_back(ir    , 0,  delta(0,0));
						tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
					}
				}
			}
			std::cout << "joo2" << std::endl;
			for(size_t i = 0 ; i < cameras.size(); i++){
				int ic = i * 7 + number_intrinsic_paramters;
				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 + 3 , -jacobian(0,0));
				tripletListA.emplace_back(ir, ic + 4 , -jacobian(0,1));
				tripletListA.emplace_back(ir, ic + 5 , -jacobian(0,2));
				tripletListA.emplace_back(ir, ic + 6 , -jacobian(0,3));

				tripletListP.emplace_back(ir, ir, 1000000.0);

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

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 7 + number_intrinsic_paramters);
			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() * 7 + number_intrinsic_paramters, cameras.size() * 7 + number_intrinsic_paramters);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 7 + number_intrinsic_paramters, 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() * 7 + number_intrinsic_paramters){
				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;

				k1 += h_x[counter++];
				k2 += h_x[counter++];
				k3 += h_x[counter++];
				p1 += h_x[counter++];
				p2 += h_x[counter++];

				for(size_t i = 0; i < cameras.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose.inverse());
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					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).inverse();
				}

				std::cout << "desired intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1tmp << " k2: " << k2tmp << " k3: " << k3tmp << " p1: " << p1tmp << " p2: " << p2tmp << std::endl;
				std::cout << "computer intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1 << " k2: " << k2 << " k3: " << k3 << " p1: " << p1 << " p2: " << p2 << std::endl;

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case '1':{
			k1 -= 0.01;
			std::cout << "k1: " << k1 << std::endl;
			break;
		}
		case '2':{
			k1 += 0.01;
			std::cout << "k1: " << k1 << std::endl;
			break;
		}
		case '3':{
			k2 -= 0.01;
			std::cout << "k2: " << k2 << std::endl;
			break;
		}
		case '4':{
			k2 += 0.01;
			std::cout << "k2: " << k2 << std::endl;
			break;
		}
		case '5':{
			k3 -= 0.01;
			std::cout << "k3: " << k3 << std::endl;
			break;
		}
		case '6':{
			k3 += 0.01;
			std::cout << "k3: " << k3 << std::endl;
			break;
		}
		case '7':{
			p1 -= 0.01;
			std::cout << "p1: " << p1 << std::endl;
			break;
		}
		case '8':{
			p1 += 0.01;
			std::cout << "p1: " << p1 << std::endl;
			break;
		}
		case '9':{
			p2 -= 0.01;
			std::cout << "p2: " << p2 << std::endl;
			break;
		}
		case '0':{
			p2 += 0.01;
			std::cout << "p2: " << p2 << std::endl;
			break;
		}
		case 'e':{
			show_only_one =! show_only_one;
			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 wc)" << std::endl;
	std::cout << "r: optimize (Rodriguez cw)" << std::endl;
	std::cout << "q: optimize (Quaternion cw)" << std::endl;
	std::cout << "c: add noise to cameras" << std::endl;
	std::cout << "1: k1 -= 0.01" << std::endl;
	std::cout << "2: k1 += 0.01" << std::endl;
	std::cout << "3: k2 -= 0.01" << std::endl;
	std::cout << "4: k2 += 0.01" << std::endl;
	std::cout << "5: k3 -= 0.01" << std::endl;
	std::cout << "6: k3 += 0.01" << std::endl;
	std::cout << "7: p1 -= 0.01" << std::endl;
	std::cout << "8: p1 += 0.01" << std::endl;
	std::cout << "9: p2 -= 0.01" << std::endl;
	std::cout << "0: p2 += 0.01" << std::endl;
	std::cout << "e: show_only_one=!show_only_one" << std::endl;
}


/*--------------------------------------------------------------------

Search for LCG multiplier using the LLL-Spectral Test!  January 2000

Authors: Karl Entacher, Karl.Entacher@fh-sbg.ac.at

         Thomas Schell, Dept. of Scientific Computing, Univ. Salzburg

--------------------------------------------------------------------*/

/*-------------Libraries (needs Victor Shoups NTL-Lib----------------*/

#include <iostream>

#include <fstream>

#include <time.h>

#include <sys/resource.h>

#include <math.h>

#include <float.h>

#include <NTL/ZZ.h>

#include <NTL/RR.h>

#include <NTL/mat_ZZ.h>

#include <NTL/LLL.h>

using namespace std;
using namespace NTL;

/*------------------------ Definitions ------------------------------*/

ZZ lambda, modul, *m_primes;

struct {

  ZZ lambda;

  double min_norm_x_x;

} best;

double fact[8];

const int x_xmax = 8;       /* Test for dimensions <= 8 */

const ZZ n_min=to_ZZ(1);

//const ZZ n_max=to_ZZ(10000);
const ZZ n_max=to_ZZ(10000000);

/*-----------------------------------------------------------------------

   Call the function with:

        lll_search ["output-file"] [modulus] [prim-factors of modulus-1]

   Example:

        lll_search "output-file" 2147483647 2 3 7 11 31 151 331

  ----------------------------------------------------------------------*/

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

/*--------------------------    Input-Check        -----------------------*/

  if (!(argc >= 2)) {

    cout << "lll_search_rnd <output-file-name> <modul> <prime factors of modul-1>" << endl;

    exit(0);

  }

/*--------- reads the input-parameters: "file" modulus primefactors ------*/

  ofstream out_file(argv[1], ios::out);

  modul = to_ZZ(argv[2]);

  if (argc-5 > 0) {

    m_primes = new ZZ [argc-3];

    for (int i = 3; i < argc; i++)

      m_primes[i-3] = to_ZZ(argv[i]);

  }

/*------------- output of modulus and factors of modulus-1 ---------------*/

  out_file << modul;

  for (int i = 0; i < argc-3; i++)

    out_file << " " << m_primes[i];

  out_file << endl;

/*-----------------Search for the first primitiv root and its output-------*/

  int is_primitive_root;

  ZZ pr = to_ZZ(1);

  do {

    int k = 0;

    pr++;

    while (k < argc-3 && (is_primitive_root = (PowerMod(pr, (modul-1) / m_primes[k++], modul) != 1)));

  } while (!is_primitive_root);

  cout << "primitive root " << pr << endl;

/*------------- Constants for the normalized Spectral Test   -----------------*/

  fact[0] = 0.0;                                // intentionally left uninitialized -> not used

  fact[1] = to_double(to_RR(1.0) / pow(to_RR(4.0/3.0), to_RR(1.0/4.0)) / pow(to_RR(modul), to_RR(1.0/2.0)));

  fact[2] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(1.0/6.0)) / pow(to_RR(modul), to_RR(1.0/3.0)));

  fact[3] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(1.0/4.0)) / pow(to_RR(modul), to_RR(1.0/4.0)));

  fact[4] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(3.0/10.0)) / pow(to_RR(modul), to_RR(1.0/5.0)));

  fact[5] = to_double(to_RR(1.0) / pow(to_RR(64.0/3.0), to_RR(1.0/12.0)) / pow(to_RR(modul), to_RR(1.0/6.0)));

  fact[6] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(3.0/7.0)) / pow(to_RR(modul), to_RR(1.0/7.0)));

  fact[7] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(1.0/2.0)) / pow(to_RR(modul), to_RR(1.0/8.0)));

/*------------Search, Matrix (Basis) Input, LLL und Output -------------*/

  best.lambda = to_ZZ(0);

  best.min_norm_x_x = 0.0;

  ZZ t_h = to_ZZ(10);

  struct rusage cur_ru;

  for (ZZ n = n_min; n <= n_max; n++) {

    double min_norm_x_x = 0.0;

    if (GCD(n, modul - 1) == 1) {

      lambda = PowerMod(pr, n, modul);

      mat_ZZ x;

      x.SetDims(x_xmax, x_xmax);

      min_norm_x_x = 1.0;

      for (int j = 2; j <= x_xmax; j++) {

    x.SetDims(j,j);

    x[0][0] = modul;     // first index = rows, second index = columns

    for (int i = 1; i < j; i++)          // fill in the 1s

      x[i][i] = 1;

    for (int i = 1; i < j; i++)

      x[i][0] =-(power(lambda, i));

    ZZ det, rg;

    rg = LLL(det, x, 0);

    double min_x_x = to_double(x[0] * x[0]);

    for (int i = 1; i < j; i++) {

      double x_x = to_double(x[i] * x[i]);

      if (min_x_x > x_x)

        min_x_x = x_x;

    }

    double norm_x_x = fact[j-1] * sqrt(min_x_x);

    if (min_norm_x_x > norm_x_x)

      min_norm_x_x = norm_x_x;

      }

      if (min_norm_x_x > best.min_norm_x_x) {

    best.min_norm_x_x = min_norm_x_x;

    best.lambda = lambda;

    getrusage(RUSAGE_SELF, &cur_ru);

    out_file << "time\t" << cur_ru.ru_utime.tv_sec << "\tn\t" << n << "\tl\t" << lambda << "\t" << min_norm_x_x << endl;

      }

    }

    ZZ n_ = n - n_min;

    if (n_ >= t_h) {

      getrusage(RUSAGE_SELF, &cur_ru);

      cout << n_ << "\t" << cur_ru.ru_utime.tv_sec << endl;

      t_h *= 10;

    }

  }

  getrusage(RUSAGE_SELF, &cur_ru);

  cout << "total time elapsed\t" << cur_ru.ru_utime.tv_sec << endl;

  out_file << "time\t" << cur_ru.ru_utime.tv_sec << "\tl\t" << best.lambda << "\t" << best.min_norm_x_x << endl;

  out_file.close();

}


// Run like this
//./prediction --dataL competition/S1a/dataset6_training.csv --dataT competition/S1b/dataset6_testing.csv --sigma=1.14683 --length=0.0825932 --nu=0.600677 --tau=3.49e-14 --ldl
// Developed by Alexander Litvinenko (RWTH Aachen) and Ronald Kriemann (MIS MPG Leipzig)
// Based on the HLIBPro library (v. 2.9) www.hlibpro.com
// No warranties.

#include <iostream>
#include <fstream>
#include <string>

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

#include <gsl/gsl_multimin.h>

#include "hlib.hh"

using namespace std;
using boost::format;
using namespace HLIB;
using namespace boost::program_options;
using  real_t    = HLIB::real;

enum {
    IDX_SIGMA  = 0,
    IDX_LENGTH = 1,
    IDX_NU     = 2,
    IDX_TAU    = 3
};

// global options
int        nmin      = CFG::Cluster::nmin;
double     eps       = 1e-6;
double     fac_eps   = 1e-6;
double     shift     = 1e-7;
bool       use_ldl   = false;

//
// read dataset from file
//
void
read_data ( const std::string &       datafile,
            std::vector< T2Point > &  vertices,
            BLAS::Vector< double > &  Z_data_ )
{
    std::ifstream  in( datafile );

    if ( ! in ) // error
        exit( 1 );

    size_t  N_vtx = 0;

    #if 1

    std::string  line;

    std::getline( in, line );

    if ( line == "x,y,values" )
    {
        std::list< T2Point >  pos;
        std::list< double >   vals;

        while ( std::getline( in, line ) )
        {
            auto    parts = split( line, "," );
            double  x = atof( parts[0].c_str() );
            double  y = atof( parts[1].c_str() );
            double  v = atof( parts[2].c_str() );

            pos.push_back( T2Point( x, y ) );
            vals.push_back( v );
        }// while

        N_vtx = pos.size();

        std::cout << "learning dataset" << std::endl;
        std::cout << N_vtx << std::endl;

        vertices.resize( N_vtx );
        Z_data_ = BLAS::Vector< double >( N_vtx );

        int  i = 0;

        for ( auto  p : pos )
            vertices[ i++ ] = p;

        i = 0;
        int j=0;
        for ( auto  v : vals )
        {
          Z_data_( i++ ) = v;
        }

   }// if
    if ( line == "x,y,ignor" )
    {
        std::list< T2Point >  pos;
        std::list< double >   vals;

        while ( std::getline( in, line ) )
        {
            auto    parts = split( line, "," );
            double  x = atof( parts[0].c_str() );
            double  y = atof( parts[1].c_str() );
            double  v = atof( parts[2].c_str() );

            pos.push_back( T2Point( x, y ) );
           // vals.push_back( v );
        }// while

        N_vtx = pos.size();

        std::cout << "learning dataset" << std::endl;
        std::cout << N_vtx << std::endl;

        vertices.resize( N_vtx );
        Z_data_ = BLAS::Vector< double >( N_vtx );

        int  i = 0;

        for ( auto  p : pos )
            vertices[ i++ ] = p;

        i = 0;

        for ( auto  v : vals )
           Z_data_( i++ ) = 0.0;
    }// if

    if ( line == "x,y" )
    {
        std::list< T2Point >  pos;

        while ( std::getline( in, line ) )
        {
            auto    parts = split( line, "," );
            double  x = atof( parts[0].c_str() );
            double  y = atof( parts[1].c_str() );

            pos.push_back( T2Point( x, y ) );
        }// while

        N_vtx = pos.size();
        std::cout << "testing dataset" << std::endl;
        std::cout << N_vtx << std::endl;

        vertices.resize( N_vtx );

        int  i = 0;

        for ( auto  p : pos )
            vertices[ i++ ] = p;
    }// if
    //else
    //{
    //    std::cout << "you should not be here" << std::endl;
    //    HERROR( ERR_NOT_IMPL, "", "" );
    //}

    #else

    in >> N_vtx;

    std::cout << "reading " << N_vtx << " datapoints" << std::endl;

    vertices.resize( N_vtx );
    Z_data_ = BLAS::Vector< double >( N_vtx );

    for ( idx_t  i = 0; i < idx_t(N_vtx); ++i )
    {
        int     index = i;
        double  x, y, z;
        // double  v     = 0.0;

        in >> index >> x >> y >> z;
        // in >> index >> x >> y >> z >> v;

        vertices[ index ] = T2Point( x, y );
        //Z_data( index )   = v;
    }// for

    #endif

    //
    // for visualization of data, export 2D points with v value in csv file
    //

    // std::ofstream  out( "data.csv" );

    // out << "x,y,z,v" << std::endl;
    // out << "x,y,z" << std::endl;

    //for ( uint  i = 0; i < N_vtx; ++i )
    //   out << vertices[i].x() << "," << vertices[i].y() << ",0" << std::endl;
//      out << vertices[i].x() << "," << vertices[i].y() << ",0," << Z_data( i ) << std::endl;
}

//
// define PredictionProblem to forecast unknown values in new locations
//
struct PredictionProblem
{
    std::vector< T2Point >                vertices;
    std::vector< T2Point >                vertices_predict;
    std::unique_ptr< TCoordinate >        coord;
    std::unique_ptr< TCoordinate >        coord_predict;
    std::unique_ptr< TClusterTree >       ct;
    std::unique_ptr< TClusterTree >       ct_predict;
    std::unique_ptr< TBlockClusterTree >  bct;
    std::unique_ptr< TBlockClusterTree >  bct_predict;
    std::unique_ptr< TVector >            Z;
    std::unique_ptr< TVector >            Z_predict;

    PredictionProblem ( const std::string &  datafile, const std::string &  datafile_predict )
    {
        init( datafile, datafile_predict );
    }

    void
    init ( const std::string &  datafile, const std::string &  datafile_predict )
    {
        BLAS::Vector< double >  Z_data;
        BLAS::Vector< double >  Z_data_predict;

        read_data( datafile, vertices, Z_data );
        read_data( datafile_predict, vertices_predict, Z_data_predict );

        std::cout << "both datasets are succesfully read" << std::endl;
        coord = std::make_unique< TCoordinate >( vertices );
        coord_predict = std::make_unique< TCoordinate >( vertices_predict );

        TAutoBSPPartStrat  part_strat;
        TBSPCTBuilder      ct_builder( & part_strat, nmin );

        ct = ct_builder.build( coord.get() );

        //print_vtk( & coord, "ct_coord" );
        ct_predict = ct_builder.build( coord_predict.get() );
        //print_vtk( & coord_predict, "ct_coord_predict" );

        TStdGeomAdmCond    adm_cond( 2.0, use_min_diam );
        TBCBuilder         bct_builder;

        bct = bct_builder.build( ct.get(), ct.get(), & adm_cond );
        bct_predict = bct_builder.build( ct_predict.get(), ct.get(), & adm_cond );

        Z   = std::make_unique< TScalarVector >( *ct->root(), std::move( Z_data ) );
        Z_predict   = std::make_unique< TScalarVector >( *ct_predict->root(), std::move( Z_data_predict ) );
        ct->perm_e2i()->permute( Z.get() );
    }

    //BLAS::Vector< double >
    std::unique_ptr< TVector >
    eval ( const double  sigma,
           const double  length,
           const double  nu,
           const double tau )
    {

        TMaternCovCoeffFn< T2Point >  matern_coefffn( sigma, length, nu,  vertices );

        TMaternCovCoeffFn< T2Point >  matern_coefffn_predict( sigma, length, nu, vertices_predict, vertices );
        TPermCoeffFn< double >        coefffn( & matern_coefffn, ct->perm_i2e(), ct->perm_i2e() );
        TPermCoeffFn< double >        coefffn_predict( & matern_coefffn_predict, ct_predict->perm_i2e(), ct->perm_i2e() );

        TACAPlus< double >            aca( & coefffn );
        TACAPlus< double >            aca_predict( & coefffn_predict );
        auto                          acc = fixed_prec( eps );
        TDenseMatBuilder< double >    h_builder( & coefffn, & aca );
        TDenseMatBuilder< double >    h_builder_predict( & coefffn_predict, & aca_predict );

        auto                          C        = h_builder.build( bct.get(), acc );
        //TPSMatrixVis  mvis;

        //mvis.svd(true).print( C.get(), "myC" ); Output matrix C as a .eps file

        auto   C_predict= h_builder_predict.build( bct_predict.get(), unsymmetric, acc ); //A_21 - rectangular matrix

        if ( shift != 0.0 )
            add_identity( C.get(), tau*tau );

        auto                          fac_acc  = fixed_prec( fac_eps );
        auto                          C_fac    = C->copy();
        auto                          fac_opts = fac_options_t{ point_wise, CFG::Arith::storage_type, false };

        if ( use_ldl )
        {
            ldl( C_fac.get(), fac_acc, fac_opts );
            //std::cout <<  "LDL is used " << std::endl;
        }
        else
        {
            chol( C_fac.get(), fac_acc );
            //std::cout <<  "Cholesky is used " << std::endl;
        }
        //std::cout << "    |С|_F             = " << norm_F( C.get() ) << std::endl;
        //std::cout << "    |С|_2             = " << norm_2( C.get() ) << std::endl;
        //std::cout << "    |С_fac|_F             = " << norm_F( C_fac.get() ) << std::endl;
        //std::cout << "    |С_fac|_2             = " << norm_2( C_fac.get() ) << std::endl;

        std::unique_ptr< TFacInvMatrix >   C_inv;

        if ( use_ldl )
        {
            C_inv = std::make_unique< TLDLInvMatrix >( C_fac.get(), symmetric, point_wise );
        }
        else
        {
            C_inv = std::make_unique< TLLInvMatrix >( C_fac.get(), symmetric );
        }
        //std::cout << "    |С_inv|_2             = " << norm_2( C_inv.get() ) << std::endl;

        const size_t                  N     = vertices.size();

        TStopCriterion                sstop( 350, 1e-16, 0.0 );
        TCG                           solver( sstop );
        auto                          sol = C->row_vector();

        auto                          Z_predict = C_predict->row_vector();

        FILE* f1;

        solver.solve( C.get(), sol.get(), Z.get(), C_inv.get() );

        //DBG::write( C_predict.get(), "A12.mat", "A12" );
        //std::cout << "  ||invC_Z|| = " << sol->norm2() << std::endl;
        mul_vec( real_t(1), C_predict.get(), sol.get(), real_t(0), Z_predict.get(), apply_normal );

        //std::cout << "  ||Z_predict|| = " << Z_predict->norm2() << std::endl;
        //C_predict->mul_vec( 1.0, sol.get(), 0.0, Z_predict.get(), apply_normal );

        ct_predict->perm_i2e()->permute( Z_predict.get() );
        //TMatlabVectorIO  vio;
        //vio.write( Z_predict,  "x.mat", "x" );

        f1 = fopen("111prediction.txt", "w");
        for ( size_t  i = 0; i < Z_predict->size(); i++ )
          fprintf(f1," %.6f, %.6f, %.6f\n",   vertices_predict[i].x(),  vertices_predict[i].y(), Z_predict->entry(i));
        fclose(f1);
        return std::move( Z_predict );
    }
};

//
// wrapper from GSL to LogLikeliHoodProblem
//
/*double
  eval_logli ( const gsl_vector *  param,
  void *              data )
  {
  double sigma  = gsl_vector_get( param, IDX_SIGMA );
  double length = gsl_vector_get( param, IDX_LENGTH );
  double nu     = gsl_vector_get( param, IDX_NU );
  double tau    = gsl_vector_get( param, IDX_TAU );

  LogLikeliHoodProblem *  problem = static_cast< LogLikeliHoodProblem * >( data );

  return - problem->eval( sigma, length, nu, tau );
  }
*/
//
// optimization function using GSL
//

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

    //CFG::set_verbosity( 3 );
    INIT();

    //std::string  datafile = "datafile.txt";
    //std::string  datafile_predict = "datafile_predict.txt";
    std::string  datafile = "LearningSet.txt";
    std::string  datafile_predict = "TestingSet.txt";

    double  sigma  = 2.7779; //take these values from previous experiments (Part 1a)
    double  length = 0.07;
    double  nu     = 1.0365;
    double  tau    = 4.2045e-8;

    //
    // define command line options
    //

    options_description             all_opts;
    options_description             vis_opts( "usage: loglikelihood [options] datafile\n  where options include" );
    options_description             hid_opts( "Hidden options" );
    positional_options_description  pos_opts;
    variables_map                   vm;

    // standard options
    vis_opts.add_options()
        ( "help,h",                       ": print this help text" )
        ( "threads,t",   value<int>(),    ": number of parallel threads" )
        ( "verbosity,v", value<int>(),    ": verbosity level" )
        ( "nmin",        value<int>(),    ": set minimal cluster size" )
        ( "eps,e",       value<double>(), ": set H accuracy" )
        ( "epslu",       value<double>(), ": set only H factorization accuracy" )
        ( "shift",       value<double>(), ": regularization parameter" )
        ( "ldl",                          ": use LDL factorization" )
        ( "sigma",       value<double>(), ": sigma parameter" )
        ( "nu",          value<double>(), ": nu parameter" )
        ( "length",      value<double>(), ": length parameter" )
        ( "tau",         value<double>(), ": tau paramater" )
        ;

    hid_opts.add_options()
        ( "dataL",        value<std::string>(), ": datafile defining learning problem" )
        ( "dataT",        value<std::string>(), ": datafile defining testing problem" )
        ;

    // options for command line parsing
    all_opts.add( vis_opts ).add( hid_opts );

    // all "non-option" arguments should be "--data" arguments
    pos_opts.add( "dataL", -1 );
    pos_opts.add( "dataT", -1 );

    //
    // parse command line options
    //

    try
    {
        store( command_line_parser( argc, argv ).options( all_opts ).positional( pos_opts ).run(), vm );
        notify( vm );
    }// try
    catch ( required_option &  e )
    {
        std::cout << e.get_option_name() << " requires an argument, try \"-h\"" << std::endl;
        exit( 1 );
    }// catch
    catch ( unknown_option &  e )
    {
        std::cout << e.what() << ", try \"-h\"" << std::endl;
        exit( 1 );
    }// catch

    //
    // eval command line options
    //

    if ( vm.count( "help") )
    {
        std::cout << vis_opts << std::endl;
        exit( 1 );
    }// if

    if ( vm.count( "nmin"      ) ) nmin     = vm["nmin"].as<int>();
    if ( vm.count( "eps"       ) ) eps      = vm["eps"].as<double>();
    if ( vm.count( "epslu"     ) ) fac_eps  = vm["epslu"].as<double>();
    if ( vm.count( "shift"     ) ) shift    = vm["shift"].as<double>();
    if ( vm.count( "threads"   ) ) CFG::set_nthreads( vm["threads"].as<int>() );
    if ( vm.count( "verbosity" ) ) CFG::set_verbosity( vm["verbosity"].as<int>() );
    if ( vm.count( "ldl"       ) ) use_ldl  = true;
    if ( vm.count( "sigma"     ) ) sigma    = vm["sigma"].as<double>();
    if ( vm.count( "nu"        ) ) nu       = vm["nu"].as<double>();
    if ( vm.count( "length"    ) ) length   = vm["length"].as<double>();
    if ( vm.count( "tau"       ) ) tau      = vm["tau"].as<double>();

    // default to general eps
    if ( fac_eps == -1 )
        fac_eps = eps;

    if ( vm.count( "dataL" ) )
        datafile = vm["dataL"].as<std::string>();
    else
    {
        std::cout << "usage: loglikelihood [options] datafile" << std::endl;
        exit( 1 );
    }// if

    if ( vm.count( "dataT" ) )
        datafile_predict = vm["dataT"].as<std::string>();
    else
    {
        std::cout << "usage: loglikelihood [options] datafile" << std::endl;
        exit( 1 );
    }// if

    std::cout << "parameters : " << " σ = " << sigma << ", ℓ = " << length << ", ν = " << nu << ", τ = " << tau << std::endl;

    PredictionProblem  problem( datafile, datafile_predict );
    problem.eval( sigma, length, nu, tau);

    DONE();
}