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

#include "structures.h"
#include "transformations.h"
#include "cauchy.h"
#include "perspective_camera_plucker_line_tait_bryan_wc_jacobian.h"
#include "perspective_camera_tait_bryan_wc_jacobian.h"
#include "perspective_camera_plucker_line_rodrigues_wc_jacobian.h"
#include "perspective_camera_plucker_line_quaternion_wc_jacobian.h"
#include "quaternion_constraint_jacobian.h"

struct LinePoint{
	double u;
	double v;
	int index_to_line;
};

struct Camera{
	Eigen::Affine3d pose;
	std::vector<std::pair<LinePoint, LinePoint>> uv;
};

typedef Eigen::Matrix<double, 6, 1> PLine;

std::vector<Camera> cameras;
PerspectiveCameraParams cam_params;
std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> lines;

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();
PLine get_plucker_line(const Eigen::Vector3d &from, const Eigen::Vector3d &to);

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

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

	cam_params.fx = 2000;
	cam_params.fy = 1000;
	cam_params.cx = 1000;
	cam_params.cy = 500;

	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(float x = 0; x <= 100; x += 5){
		Eigen::Vector3d a(x, -50.0, 400);
		Eigen::Vector3d b(x,  50.0, 400);
		lines.emplace_back(a,b);
	}
	for(float y = -50; y <= 50; y += 5){
		Eigen::Vector3d a(0, y, 400);
		Eigen::Vector3d b(100.0, y, 400);
		lines.emplace_back(a,b);
	}

	for(float x = -100; x <= 0; x += 5){
		Eigen::Vector3d a(x, -50.0, 440);
		Eigen::Vector3d b(x,  50.0, 440);
		lines.emplace_back(a,b);
	}
	for(float y = -50; y <= 50; y += 5){
		Eigen::Vector3d a(-100, y, 440);
		Eigen::Vector3d b(0.0, y, 440);
		lines.emplace_back(a,b);
	}

	for(size_t i = 0; i < cameras.size(); i++){
		for(size_t j = 0 ; j < lines.size(); j++){
			std::pair<LinePoint, LinePoint> uv;
			LinePoint kp;
			TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);

			projection_perspective_camera_tait_bryan_wc(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, lines[j].first.x(), lines[j].first.y(), lines[j].first.z());

			uv.first.u = kp.u;
			uv.first.v = kp.v;
			uv.first.index_to_line = j;

			projection_perspective_camera_tait_bryan_wc(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, lines[j].second.x(), lines[j].second.y(), lines[j].second.z());

			uv.second.u = kp.u;
			uv.second.v = kp.v;
			uv.second.index_to_line = j;

			cameras[i].uv.push_back(uv);
		}
	}

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

	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,1,0);
	glBegin(GL_LINES);
		for(size_t i = 0; i < lines.size(); i++){
			glVertex3f(lines[i].first.x(), lines[i].first.y(), lines[i].first.z());
			glVertex3f(lines[i].second.x(), lines[i].second.y(), lines[i].second.z());
		}
	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.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;

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

					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);

					PLine pl = get_plucker_line(lines[cameras[i].uv[j].first.index_to_line].first, lines[cameras[i].uv[j].first.index_to_line].second);
					Eigen::Matrix<double, 2, 1> delta;
					observation_equation_perspective_camera_plucker_line_tait_bryan_wc(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,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					Eigen::Matrix<double, 2, 6, Eigen::RowMajor> jacobian;
					observation_equation_perspective_camera_plucker_line_tait_bryan_wc_jacobian(jacobian, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					int ir = tripletListB.size();
					int ic_camera = i * 6;
					tripletListA.emplace_back(ir     , ic_camera    , -jacobian(0,0));
					tripletListA.emplace_back(ir     , ic_camera + 1, -jacobian(0,1));
					tripletListA.emplace_back(ir     , ic_camera + 2, -jacobian(0,2));
					tripletListA.emplace_back(ir     , ic_camera + 3, -jacobian(0,3));
					tripletListA.emplace_back(ir     , ic_camera + 4, -jacobian(0,4));
					tripletListA.emplace_back(ir     , ic_camera + 5, -jacobian(0,5));

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

					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);
			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, cameras.size() * 6);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 6, 1);

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

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

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

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

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

			std::vector<double> h_x;

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

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

				int counter = 0;

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

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

					RodriguesPose pose = pose_rodrigues_from_affine_matrix(cameras[i].pose);
					Eigen::Matrix<double, 2, 1> delta;
					PLine pl = get_plucker_line(lines[cameras[i].uv[j].first.index_to_line].first, lines[cameras[i].uv[j].first.index_to_line].second);
					observation_equation_perspective_camera_plucker_line_rodrigues_wc(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,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					Eigen::Matrix<double, 2, 6, Eigen::RowMajor> jacobian;
					observation_equation_perspective_camera_plucker_line_rodrigues_wc_jacobian(jacobian, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					int ir = tripletListB.size();
					int ic_camera = i * 6;
					tripletListA.emplace_back(ir     , ic_camera    , -jacobian(0,0));
					tripletListA.emplace_back(ir     , ic_camera + 1, -jacobian(0,1));
					tripletListA.emplace_back(ir     , ic_camera + 2, -jacobian(0,2));
					tripletListA.emplace_back(ir     , ic_camera + 3, -jacobian(0,3));
					tripletListA.emplace_back(ir     , ic_camera + 4, -jacobian(0,4));
					tripletListA.emplace_back(ir     , ic_camera + 5, -jacobian(0,5));

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

					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);
			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, cameras.size() * 6);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 6, 1);

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

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

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

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

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

			std::vector<double> h_x;

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

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

				int counter = 0;

				for(size_t i = 0; i < cameras.size(); i++){
					RodriguesPose pose = pose_rodrigues_from_affine_matrix(cameras[i].pose);
					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);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			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[i].uv.size(); j++){

					QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);
					Eigen::Matrix<double, 2, 1> delta;
					PLine pl = get_plucker_line(lines[cameras[i].uv[j].first.index_to_line].first, lines[cameras[i].uv[j].first.index_to_line].second);
					observation_equation_perspective_camera_plucker_line_quaternion_wc(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,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					Eigen::Matrix<double, 2, 7, Eigen::RowMajor> jacobian;
					observation_equation_perspective_camera_plucker_line_quaternion_wc_jacobian(jacobian, 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,
							pl(0,0), pl(1,0), pl(2,0), pl(3,0), pl(4,0), pl(5,0),
							cameras[i].uv[j].first.u, cameras[i].uv[j].first.v,
							cameras[i].uv[j].second.u, cameras[i].uv[j].second.v);

					int ir = tripletListB.size();
					int ic_camera = i * 7;
					tripletListA.emplace_back(ir     , ic_camera    , -jacobian(0,0));
					tripletListA.emplace_back(ir     , ic_camera + 1, -jacobian(0,1));
					tripletListA.emplace_back(ir     , ic_camera + 2, -jacobian(0,2));
					tripletListA.emplace_back(ir     , ic_camera + 3, -jacobian(0,3));
					tripletListA.emplace_back(ir     , ic_camera + 4, -jacobian(0,4));
					tripletListA.emplace_back(ir     , ic_camera + 5, -jacobian(0,5));
					tripletListA.emplace_back(ir     , ic_camera + 6, -jacobian(0,6));

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

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

			for(size_t i = 0 ; i < cameras.size(); i++){
				int ic = i * 7;
				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);
			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, cameras.size() * 7);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 7, 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){
				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.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);
				}
			}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 << "c: add noise to cameras" << std::endl;
	std::cout << "t: optimize cameras external orientation (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize cameras external orientation (Rodrigues)" << std::endl;
	std::cout << "q: optimize cameras external orientation (Quaternion)" << std::endl;
}

PLine get_plucker_line(const Eigen::Vector3d &from, const Eigen::Vector3d &to)
{
	PLine plucker_line;

	Eigen::Vector3d direction = (to-from);
	direction/=direction.norm();
	Eigen::Vector3d moment = from.cross(direction);

	plucker_line(0,0) = moment.x();
	plucker_line(1,0) = moment.y();
	plucker_line(2,0) = moment.z();
	plucker_line(3,0) = direction.x();
	plucker_line(4,0) = direction.y();
	plucker_line(5,0) = direction.z();

	return plucker_line;
}


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2018, Rice University
*  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 Rice University 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 OWNER 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.
*********************************************************************/

/* Author: Zachary Kingston */

#include <ompl/util/PPM.h>
#include <boost/filesystem.hpp>

#include "ConstrainedPlanningCommon.h"

static const double pi2 = 2 * boost::math::constants::pi<double>();

/** Torus manifold. */
class TorusConstraint : public ompl::base::Constraint
{
public:
    const double outer;
    const double inner;

    TorusConstraint(const double outer, const double inner, const std::string &maze)
      : ompl::base::Constraint(3, 1), outer(outer), inner(inner), file_(maze)
    {
        ppm_.loadFile(maze.c_str());
    }

    void getStartAndGoalStates(Eigen::Ref<Eigen::VectorXd> start, Eigen::Ref<Eigen::VectorXd> goal) const
    {
        const double h = ppm_.getHeight() - 1;
        const double w = ppm_.getWidth() - 1;

        for (unsigned int x = 0; x <= w; ++x)
            for (unsigned int y = 0; y <= h; ++y)
            {
                Eigen::Vector2d p = {x / w, y / h};

                auto &c = ppm_.getPixel(x, y);
                if (c.red == 255 && c.blue == 0 && c.green == 0)
                    mazeToAmbient(p, start);

                else if (c.green == 255 && c.blue == 0 && c.red == 0)
                    mazeToAmbient(p, goal);
            }
    }

    void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
    {
        Eigen::Vector3d c = {x[0], x[1], 0};
        out[0] = (x - outer * c.normalized()).norm() - inner;
    }

    void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
    {
        const double xySquaredNorm = x[0] * x[0] + x[1] * x[1];
        const double xyNorm = std::sqrt(xySquaredNorm);
        const double denom = std::sqrt(x[2] * x[2] + (xyNorm - outer) * (xyNorm - outer));
        const double c = (xyNorm - outer) * (xyNorm * xySquaredNorm) / (xySquaredNorm * xySquaredNorm * denom);
        out(0, 0) = x[0] * c;
        out(0, 1) = x[1] * c;
        out(0, 2) = x[2] / denom;
    }

    void ambientToMaze(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const
    {
        Eigen::Vector3d c = {x[0], x[1], 0};

        const double h = ppm_.getHeight();
        const double w = ppm_.getWidth();

        out[0] = std::atan2(x[2], c.norm() - outer) / pi2;
        out[0] += (out[0] < 0);
        out[0] *= h;
        out[1] = std::atan2(x[1], x[0]) / pi2;
        out[1] += (out[1] < 0);
        out[1] *= w;
    }

    void mazeToAmbient(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const
    {
        Eigen::Vector2d a = x * pi2;

        Eigen::Vector3d b = {std::cos(a[0]), 0, std::sin(a[0])};
        b *= inner;
        b[0] += outer;

        double norm = std::sqrt(b[0] * b[0] + b[1] * b[1]);
        out << std::cos(a[1]), std::sin(a[1]), 0;
        out *= norm;
        out[2] = b[2];
    }

    bool mazePixel(const Eigen::Ref<const Eigen::VectorXd> &x) const
    {
        const double h = ppm_.getHeight();
        const double w = ppm_.getWidth();

        if (x[0] < 0 || x[0] >= w || x[1] < 0 || x[1] >= h)
            return false;

        const ompl::PPM::Color &c = ppm_.getPixel(x[0], x[1]);
        return !(c.red == 0 && c.blue == 0 && c.green == 0);
    }

    bool isValid(const ompl::base::State *state) const
    {
        auto &&x = *state->as<ob::ConstrainedStateSpace::StateType>();
        Eigen::Vector2d coords;
        ambientToMaze(x, coords);

        return mazePixel(coords);
    }

    void dump(std::ofstream &file) const
    {
        file << outer << std::endl;
        file << inner << std::endl;

        boost::filesystem::path path(file_);
        file << boost::filesystem::canonical(path).string() << std::endl;
    }

private:
    const std::string file_;
    ompl::PPM ppm_;
};

bool torusPlanningOnce(ConstrainedProblem &cp, enum PLANNER_TYPE planner, bool output)
{
    cp.setPlanner(planner);

    // Solve the problem
    ob::PlannerStatus stat = cp.solveOnce(output, "torus");

    if (output)
    {
        OMPL_INFORM("Dumping problem information to `torus_info.txt`.");
        std::ofstream infofile("torus_info.txt");
        infofile << cp.type << std::endl;
        dynamic_cast<TorusConstraint *>(cp.constraint.get())->dump(infofile);
        infofile.close();
    }

    cp.atlasStats();

    if (output)
        cp.dumpGraph("torus");

    return stat;
}

bool torusPlanningBench(ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
{
    cp.setupBenchmark(planners, "torus");
    cp.runBenchmark();
    return 0;
}

bool torusPlanning(bool output, enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners,
                   struct ConstrainedOptions &c_opt, struct AtlasOptions &a_opt, bool bench, double outer, double inner,
                   const std::string &maze)
{
    // Create the ambient space state space for the problem.
    auto rvss = std::make_shared<ob::RealVectorStateSpace>(3);

    ob::RealVectorBounds bounds(3);
    bounds.setLow(-(outer + inner));
    bounds.setHigh(outer + inner);

    rvss->setBounds(bounds);

    // Create a shared pointer to our constraint.
    auto constraint = std::make_shared<TorusConstraint>(outer, inner, maze);

    ConstrainedProblem cp(space, rvss, constraint);
    cp.setConstrainedOptions(c_opt);
    cp.setAtlasOptions(a_opt);

    Eigen::Vector3d start, goal;
    constraint->getStartAndGoalStates(start, goal);

    cp.setStartAndGoalStates(start, goal);
    cp.ss->setStateValidityChecker(std::bind(&TorusConstraint::isValid, constraint, std::placeholders::_1));

    if (!bench)
        return torusPlanningOnce(cp, planners[0], output);
    else
        return torusPlanningBench(cp, planners);
}

auto help_msg = "Shows this help message.";
auto output_msg = "Dump found solution path (if one exists) in plain text and planning graph in GraphML to "
                  "`torus_path.txt` and `torus_graph.graphml` respectively.";
auto bench_msg = "Do benchmarking on provided planner list.";
auto outer_msg = "Outer radius of torus.";
auto inner_msg = "Inner radius of torus.";
auto maze_msg = "Filename of maze image (in .ppm format) to use as obstacles on the surface of the torus.";

int main(int argc, char **argv)
{
    bool output, bench;
    enum SPACE_TYPE space = PJ;
    std::vector<enum PLANNER_TYPE> planners = {RRT};

    struct ConstrainedOptions c_opt;
    struct AtlasOptions a_opt;

    double outer, inner;
    boost::filesystem::path path(__FILE__);
    std::string maze = (path.parent_path() / "mazes/thick.ppm").string();

    po::options_description desc("Options");
    desc.add_options()("help,h", help_msg);
    desc.add_options()("output,o", po::bool_switch(&output)->default_value(false), output_msg);
    desc.add_options()("bench", po::bool_switch(&bench)->default_value(false), bench_msg);
    desc.add_options()("outer", po::value<double>(&outer)->default_value(2), outer_msg);
    desc.add_options()("inner", po::value<double>(&inner)->default_value(1), inner_msg);
    desc.add_options()("maze,m", po::value<std::string>(&maze), maze_msg);

    addSpaceOption(desc, &space);
    addPlannerOption(desc, &planners);
    addConstrainedOptions(desc, &c_opt);
    addAtlasOptions(desc, &a_opt);

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 1;
    }

    if (maze == "")
    {
        OMPL_ERROR("--maze is a required.");
        return 1;
    }

    return torusPlanning(output, space, planners, c_opt, a_opt, bench, outer, inner, maze);
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  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_EXPOCVT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_EXPOCVT_HPP_INCLUDED

#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/shr.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/detail/constant/maxexponent.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/nbexponentbits.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/detail/constant/pow2mask.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF ( expocvt_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
  {
    using result =  bd::as_integer_t<A0, unsigned>;
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      using sA0 = bd::scalar_of_t<A0>;
      BOOST_ASSERT_MSG(is_flint(a0), "parameter is not a flint");
      A0 oddv = fma(a0+Maxexponent<sA0>(), Two<sA0>(), (1 << (Nbexponentbits<sA0>()+1)) + 1);
      return shift_left(bitwise_and(Pow2mask<A0>(), oddv), Nbexponentbits<sA0>());
    }
  };

} } }

#endif


#include <functional>
#include <iomanip>
#include <iostream>
#include <string>
#include <tuple>
#include <utility/include_all.h>
#include <utility/iterator.h>
#include <utility/math.h>
#include <utility/unit_math.h>
#include <vector>

#ifndef TESTING
struct virtual_particles {
  int32_t requiredSlices_x, requiredSlices_y, requiredSlices_z;
  float r;
  value_unit<float4, SI::m> p;
  value_unit<float4, SI::m> c;
  bool valid = false;

  template <typename V, typename U> virtual_particles(U &&position, V &&volume, float spacing) {
    p = position;
    c = position; // float4_u<SI::m>(0., 0., 0., p.val.w);
    float radius = pow(volume.val / (4.f / 3.f * CUDART_PI_F), 1.f / 3.f);
    float h = p.val.w * Kernel<kernel_kind::spline4>::kernel_size();
    valid = true;

    r = spacing * radius;

    auto k = static_cast<int32_t>(floor(c.val.z / r * 3. / (2. * sqrt(6.))));
    auto j = static_cast<int32_t>(floor(c.val.y / r / sqrt(3.) - 1. / 3. * (k % 2)));
    auto i = static_cast<int32_t>(floor((c.val.x / r - (j + k) % 2) / 2.));
    c = value_unit<float4, SI::m>{2.f * i + ((j + k) % 2), sqrtf(3.f) * (j + 1.f / 3.f * (k % 2)),
                                  2.f * sqrtf(6.f) / 3.f * k, 0.f};
    c = value_unit<float4, SI::m>{c.val.x * r, c.val.y * r, c.val.z * r, 0.f};
    // std::cout << c.val.x << ", " << c.val.y << ", " << c.val.z << std::endl;
    c = value_unit<float4, SI::m>{0.f, 0.f, 0.f, 0.f};
    requiredSlices_x = 2 * static_cast<int32_t>(ceil(h / r));
    requiredSlices_y = 2 * static_cast<int32_t>(ceil(h / (sqrtf(3.f) * r)));
    requiredSlices_z = 2 * static_cast<int32_t>(ceil(h / r * 3.f / (sqrtf(6.f) * 2.f)));
  }
  struct neighbor_it {
    const int32_t requiredSlices_x, requiredSlices_y, requiredSlices_z;
    const float r;
    value_unit<float4, SI::m> p;
    value_unit<float4, SI::m> s;
    value_unit<float4, SI::m> c;
    int32_t i, j, k;

    neighbor_it(int32_t x_len, int32_t y_len, int32_t z_len, float r, value_unit<float4, SI::m> pos,
                value_unit<float4, SI::m> center, int32_t _x, int32_t _y, int32_t _z)
        : requiredSlices_x(x_len), requiredSlices_y(y_len), requiredSlices_z(z_len), r(r), p(pos), c(center), i(_x),
          j(_y), k(_z) {
      increment();
    }

    value_unit<float4, SI::m> operator*() { return s; }
    bool operator==(const neighbor_it &rawIterator) const { return (k == rawIterator.k); }
    bool operator!=(const neighbor_it &rawIterator) const { return (k != rawIterator.k); }

    void increment() {
      //using namespace math::ops;
      bool in_range = false;
      do {
        i++;
        if (i == requiredSlices_x + 1) {
          i = -requiredSlices_x;
          j++;
          if (j == requiredSlices_y + 1) {
            j = -requiredSlices_y;
            k++;
          }
        }
        float4 initial{2.f * i + ((j + k) % 2), sqrtf(3.f) * (j + 1.f / 3.f * (k % 2)), 2.f * sqrtf(6.f) / 3.f * k,
                       0.f};

        s = value_unit<float4, SI::m>{c.val.x + initial.x * r, c.val.y + initial.y * r, c.val.z + initial.z * r,
                                      p.val.w};
        //float dist = math::sqdistance3(p.val, s.val);
        //dist = sqrt(dist);
        in_range = true; // dist < (p.val.w *
                         // Kernel<kernel_kind::spline4>::kernel_size());
                         // s.val.w = 1. - d / (p.val.w *
        // Kernel<kernel_kind::spline4>::kernel_size());
      } while (k != requiredSlices_z + 1 && !(in_range));
    }

    neighbor_it &operator++() {
      increment();
      return (*this);
    }
    const neighbor_it operator++(int) {
      auto temp(*this);
      increment();
      return temp;
    }
  };
  neighbor_it begin() const {
    return neighbor_it{requiredSlices_x,
                       requiredSlices_y,
                       requiredSlices_z,
                       r,
                       p,
                       c,
                       -requiredSlices_x,
                       -requiredSlices_y,
                       !valid ? requiredSlices_z + 1 : -requiredSlices_z};
  }
  neighbor_it end() const {
    return neighbor_it{requiredSlices_x, requiredSlices_y, requiredSlices_z,    r, p, c,
                       requiredSlices_x, requiredSlices_y, requiredSlices_z + 1};
  }
  neighbor_it cbegin() const {
    return neighbor_it{requiredSlices_x,
                       requiredSlices_y,
                       requiredSlices_z,
                       r,
                       p,
                       c,
                       -requiredSlices_x,
                       -requiredSlices_y,
                       !valid ? requiredSlices_z + 1 : -requiredSlices_z};
  }
  neighbor_it cend() const {
    return neighbor_it{requiredSlices_x, requiredSlices_y, requiredSlices_z,    r, p, c,
                       requiredSlices_x, requiredSlices_y, requiredSlices_z + 1};
  }
};

template <typename T> auto print_val(const std::string& str, T val) {
  std::cout << std::setw(32) << str << " = " << std::setw(24)
            << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << val.val << " -> " << std::hexfloat
            << val.val << std::defaultfloat << std::endl;
};
template <> auto print_val<float>(const std::string& str, float val) {
  std::cout << std::setw(32) << str << " = " << std::setw(24)
            << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << val << " -> " << std::hexfloat
            << val << std::defaultfloat << std::endl;
};
template <> auto print_val<int32_t>(const std::string& str, int32_t val) {
  std::cout << std::setw(32) << str << " = " << val << std::endl;
};

#define PRINT(x) print_val(#x, x);

template <kernel_kind K = kernel_kind::spline4, typename T> hostDeviceInline auto supportFromVol(T volume) {
  auto target_neighbors = Kernel<K>::neighbor_number;
  auto kernel_epsilon = (1.f) * powf((target_neighbors)*PI4O3_1, 1.f / 3.f) / Kernel<K>::kernel_size();
  auto h = kernel_epsilon * math::power<ratio<1, 3>>(volume);
  return h;
}

template <typename C> auto createLUT(C func, int32_t steps, float spacing) {
  float_u<SI::volume> volume(1.f);
  auto h = supportFromVol(volume);
  auto H = h * Kernel<kernel_kind::spline4>::kernel_size<float>();
  auto step = H / (static_cast<float>(steps)) * 2.f;

  auto current_position = float4_u<SI::m>{0.f, 0.f, 0.f, h.val};
  auto center_position = float4_u<SI::m>{0.f, 0.f, 0.f, h.val};

  using res_t = decltype(math::unit_get<1>(func(current_position, current_position)) * volume);
  std::vector<res_t> LUT;

  for (int32_t i = 0; i < steps; ++i) {
    math::unit_assign<1>(current_position, H - step * static_cast<float>(i));
    res_t density{0.f};
    for (const auto &var : virtual_particles(center_position, volume, spacing)) {
      if (math::unit_get<1>(var) > 0._m){
        continue;
	  }
      density += volume * math::unit_get<1>(func(current_position, var));
    }
    LUT.push_back(density);
  }
  return LUT;
}

auto createCounterLUT(int32_t steps, float spacing) {
  float_u<SI::volume> volume(1.f);
  auto h = supportFromVol(volume);
  auto H = h * Kernel<kernel_kind::spline4>::kernel_size<float>();
  auto step = H / (static_cast<float>(steps)) * 2.f;

  auto current_position = float4_u<SI::m>{0.f, 0.f, 0.f, h.val};
  auto center_position = float4_u<SI::m>{0.f, 0.f, 0.f, h.val};

  std::vector<int32_t> LUT;

  for (int32_t i = 0; i < steps; ++i) {
    math::unit_assign<1>(current_position, H - step * static_cast<float>(i));
    int32_t counter = 0;
    for (const auto &var : virtual_particles(center_position, volume, spacing)) {
      if (math::unit_get<1>(var) > 0._m){
        continue;
	  }
      if (spline4_kernel(current_position, var) > 0.f){
        counter++;
	  }
    }
    LUT.push_back(counter);
  }
  return LUT;
}

template <typename C> auto create4DLUT(C func, int32_t steps, float spacing) {
  float_u<SI::volume> volume(1.f);
  auto h = supportFromVol(volume);
  auto H = h * Kernel<kernel_kind::spline4>::kernel_size<float>();
  auto step = H / (static_cast<float>(steps)) * 2.f;

  auto current_position = float4_u<SI::m>{0.f, 0.f, 0.f, h.val};
  auto center_position = float4_u<SI::m>{0.f, 0.f, 0.f, h.val};

  using res_t = decltype((func(current_position, current_position)) * volume);
  std::vector<res_t> LUT;

  for (int32_t i = 0; i < steps; ++i) {
    math::unit_assign<1>(current_position, H - step * static_cast<float>(i));
    res_t density{0.f};
    for (const auto &var : virtual_particles(center_position, volume, spacing)) {
      if (math::unit_get<1>(var) > 0._m){
        continue;
	  }
      density += volume * (func(current_position, var));
    }
    LUT.push_back(density);
  }
  return LUT;
}

template <typename T> auto printLUT(const std::string& name, const std::string& type, T LUT) {
  std::cout << "std::vector<" << type << "> " << name << "{ ";
  for (auto v : LUT) {
    std::cout << std::scientific << std::setprecision(std::numeric_limits<float>::digits10 + 1)
              << static_cast<float>(math::unit_get<1>(v).val) << "f, ";
  }
  std::cout << "};" << std::endl;
}

auto printLUT(const std::string& name, const std::vector<int32_t>& LUT) {
  std::cout << "std::vector<int32_t> " << name << "{ ";
  for (auto v : LUT) {
    std::cout << v << ", ";
  }
  std::cout << "};" << std::endl;
}
#include <config/config.h>
#include <fstream>
#ifdef _WIN32
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;
#else
#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;
#endif

template <typename T> auto writeLUT(const std::string& name, const std::string& type, const std::vector<T>& LUT) {
  fs::path bin_dir(binaryDirectory);
  auto file = bin_dir / "config" / name;
  file.replace_extension("h");

  if (fs::exists(file)) {
    if (fs::exists(__FILE__)) {
      auto input_ts = fs::last_write_time(__FILE__);
      auto output_ts = fs::last_write_time(file);
      if (input_ts <= output_ts){
        return;
	  }
    }
  }
  std::cout << "Writing " << file.string() << std::endl;

  std::ofstream output(file.string());
  output << "std::vector<" << type << "> " << name << "{ ";
  int32_t ctr = 0;
  for (auto v : LUT) {
    output << std::scientific << std::setprecision(std::numeric_limits<float>::digits10 + 1)
           << static_cast<float>(math::unit_get<1>(v).val) << "f, " << (ctr++ % 5 == 0 ? "\n" : "");
  }
  output << "};" << std::endl;
  output.close();
}
auto writeLUT(const std::string& name, const std::vector<int32_t>& LUT) {
  fs::path bin_dir(binaryDirectory);
  auto file = bin_dir / "config" / name;
  file.replace_extension("h");
  if (fs::exists(file)) {
    if (fs::exists(__FILE__)) {
      auto input_ts = fs::last_write_time(__FILE__);
      auto output_ts = fs::last_write_time(file);
      if (input_ts <= output_ts){
        return;
	  }
    }
  }
  std::cout << "Writing " << file.string() << std::endl;
  std::ofstream output(file.string());
  int32_t ctr = 0;
  output << "std::vector<int32_t> " << name << "{ ";
  for (auto v : LUT) {
    output << v << ", " << (ctr++ % 10 == 0 ? "\n" : "");
  }
  output << "};" << std::endl;
  output.close();
}

int main() {
  std::cout << "Running LUT generation program" << std::endl;
  using pos_t = float4_u<SI::m>;
  bool test_spline4Kernel_LUT = false;
  bool test_spline4Gradient_LUT = false;
  bool test_estimateSpline4Gradient_LUT = false;
  bool test_testSpikyGradient_LUT = false;

  float_u<SI::volume> volume(1.f);
  auto h = supportFromVol(volume);
  auto H = h * Kernel<kernel_kind::spline4>::kernel_size<float>();

  auto position = float4_u<SI::m>{H.val / 2.f, 0.f, 0.f, h.val};

  float r = math::brentsMethod(
      [=](float_u<> radius) {
        float_u<> density = -1.f;
        for (const auto &var : virtual_particles(position, volume, math::getValue(radius))) {
          auto kernel = spline4_kernel(position, var);
          density += volume * kernel;
        }
        return density;
      },
      0.7f, 1.1f, 1e-7f, 10000);

  auto LUT = createLUT([](pos_t a, pos_t b) { return spline4_kernel(a, b); }, 2048, r);
  auto spline4gradientLUT = createLUT([](pos_t a, pos_t b) { return spline4_gradient(a, b); }, 2048, r);
  auto spikygradientLUT =
      createLUT([](pos_t a, pos_t b) { return PressureKernel<kernel_kind::spline4>::gradient(a, b); }, 2048, r);
  auto xbarLUT = createLUT([](pos_t a, pos_t b) { return b * spline4_kernel(a, b); }, 2048, r * 0.9f);
  auto ctrLUT = createCounterLUT(2048, r * 0.9f);

  // printLUT("boundaryLut" , "float", LUT);
  // printLUT("pressureLut", "float", spikygradientLUT);
  // printLUT("xbarLut", "float", xbarLUT);
  // printLUT("ctrLut", ctrLUT);

  writeLUT("boundaryLut", "float", LUT);
  writeLUT("pressureLut", "float", spline4gradientLUT);
  // writeLUT("pressureLut", "float", spikygradientLUT);
  writeLUT("xbarLut", "float", xbarLUT);
  writeLUT("ctrLut", ctrLUT);

  auto lookup = [&](float_u<SI::m> dist, float_u<SI::m> H, float_u<SI::volume> vol, auto LUT) {
    auto step = H / (static_cast<float>(LUT.size()) + 1) * 2.f;
    int32_t idx =
        math::clamp(static_cast<int32_t>(LUT.size()) - math::castTo<int32_t>(floor((dist.val + H.val) / step.val)), 0, static_cast<int32_t>(LUT.size()) - 1);
    return LUT[idx] / vol;
  };
  auto estimateGradient = [&](float_u<SI::m> dist, float_u<SI::m> H, float_u<SI::volume> vol, auto LUT) {
    auto step = H / (static_cast<float>(LUT.size()) + 1) * 2.f;
    int32_t idx =
        math::clamp(static_cast<int32_t>(LUT.size()) - math::castTo<int32_t>(floor((dist.val + H.val) / step.val)), 1, static_cast<int32_t>(LUT.size()) - 2);
    auto a = LUT[idx + 1] / vol;
    auto b = LUT[idx] / vol;
    auto ab = (b - a) / step;
    return ab;
  };
  auto lookupGradient = [&](float_u<SI::m> dist, float_u<SI::m> H, float_u<SI::volume> vol, auto LUT) {
    auto step = H / (static_cast<float>(LUT.size() + 1)) * 2.f;
    auto h_0 = support_from_volume(float_u<SI::volume>(1.f));
    auto h_c = support_from_volume(vol);

    auto ratio = h_0 / h_c;
    int32_t idx =
        math::clamp(static_cast<int32_t>(LUT.size()) - math::castTo<int32_t>(floor((dist.val + H.val) / step.val)), 0, static_cast<int32_t>(LUT.size()) - 1);
    return LUT[idx] / vol * ratio;
  };

  volume /= 10.f;
  h = support_from_volume(volume);
  H = h * kernelSize();
  position = float4_u<SI::m>{0.f, 2.f, 0.f, h.val};

  auto step = H / 32;
  // test kernel LUT
  if (test_spline4Kernel_LUT) {
    std::cout << std::endl
              << "Testing LUT for "
              << "spline4 kernel" << std::endl;
    for (float_u<SI::m> it = H; it >= -H - step; it -= step) {
      float_u<SI::recip_3<SI::m>> kernelSum{0.f};
      math::unit_assign<1>(position, it);
      for (const auto &var : virtual_particles(position, volume, r)) {
        if (math::unit_get<1>(var) > 0.0_m){
          continue;
		}
        kernelSum += kernel(position, var);
      }

      std::cout << std::setw(7) << std::fixed << std::setprecision(4) << it / H;
      std::cout << " -> " << std::fixed << std::setprecision(4) << std::setw(18)
                << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << math::unit_get<1>(kernelSum).val
                << " : " << std::setw(18) << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
                << lookup(it, H, volume, LUT).val << " : " << std::setw(18)
                << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
                << lookup(it, H, volume, LUT) / math::unit_get<1>(kernelSum) << std::endl;
    }
  }
  if (test_spline4Gradient_LUT) {
    std::cout << std::endl
              << "Testing LUT for "
              << "spline4 gradient" << std::endl;
    for (float_u<SI::m> it = H; it >= -H - step; it -= step) {
      float4_u<SI::multiply_ratios<SI::m, ratio<-4, 1>>> gradientSum{0.f, 0.f, 0.f, 0.f};
      math::unit_assign<1>(position, it);
      for (const auto &var : virtual_particles(position, volume, r)) {
        if (math::unit_get<1>(var) > 0._m){
          continue;
		}
        gradientSum += spline4_gradient(position, var);
      }

      std::cout << std::setw(7) << std::fixed << std::setprecision(4) << it / H;
      std::cout << " -> " << std::fixed << std::setprecision(4) << std::setw(18)
                << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
                << math::unit_get<1>(gradientSum).val << " : " << std::setw(18)
                << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
                << lookupGradient(it, H, volume, spline4gradientLUT).val << " : " << std::setw(18)
                << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
                << lookupGradient(it, H, volume, spline4gradientLUT) / math::unit_get<1>(gradientSum) << std::endl;
    }
  }
  if (test_estimateSpline4Gradient_LUT) {
    std::cout << std::endl
              << "Testing LUT for "
              << "estimating spline4 gradient" << std::endl;
    for (float_u<SI::m> it = H; it >= -H - step; it -= step) {
      float4_u<SI::SI_Unit<SI::m, ratio<-4, 1>>> gradientSum{0.f, 0.f, 0.f, 0.f};
      math::unit_assign<1>(position, it);
      for (const auto &var : virtual_particles(position, volume, r)) {
        if (math::unit_get<1>(var) > 0._m){
          continue;
		}
        gradientSum += spline4_gradient(position, var);
      }

      std::cout << std::setw(7) << std::fixed << std::setprecision(4) << it / H;
      std::cout << " -> " << std::fixed << std::setprecision(4) << std::setw(18)
                << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
                << math::unit_get<1>(gradientSum).val << " : " << std::setw(18)
                << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
                << estimateGradient(it, H, volume, LUT).val << " : " << std::setw(18)
                << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
                << estimateGradient(it, H, volume, LUT) / math::unit_get<1>(gradientSum) << std::endl;
    }
  }
  if (test_testSpikyGradient_LUT) {
    std::cout << std::endl
              << "Testing LUT for "
              << "spiky gradient" << std::endl;
    for (float_u<SI::m> it = H; it >= -H - step; it -= step) {
      float4_u<SI::SI_Unit<SI::m, ratio<-4, 1>>> gradientSum{0.f, 0.f, 0.f, 0.f};
      math::unit_assign<1>(position, it);
      for (const auto &var : virtual_particles(position, volume, r)) {
        if (math::unit_get<1>(var) > 0._m){
          continue;
		}
        gradientSum += PressureKernel<kernel_kind::spline4>::gradient(position, var);
      }

      std::cout << std::setw(7) << std::fixed << std::setprecision(4) << it / H;
      std::cout << " -> " << std::fixed << std::setprecision(4) << std::setw(18)
                << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
                << math::unit_get<1>(gradientSum).val << " : " << std::setw(18)
                << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
                << lookupGradient(it, H, volume, spikygradientLUT).val << " : " << std::setw(18)
                << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
                << lookupGradient(it, H, volume, spikygradientLUT) / math::unit_get<1>(gradientSum) << std::endl;
    }
  }
  // getchar();
}
#else
struct mem {
  int *x, *y, *z, *w;
};

int main() {
  mem arrays;
  cache_arrays((m_01, x));
  cache_arrays((m_02, x), (m_03, y));

  float4_u<SI::m> test_x;
  float4_u<SI::velocity> test_v;
  float_u<SI::s> test_s;
  test_x + test_v *test_s;
  test_x = test_v * test_s;
  std::cout << SI::is_same_unit<decltype(test_x)::unit, decltype(test_v * test_s)::unit>::value << std::endl;

  using Tuple1 = decltype(test_x)::unit;
  using Tuple2 = decltype(test_v * test_s)::unit;

  std::cout << typeid(Tuple1).name() << std::endl;
  std::cout << typeid(Tuple2).name() << std::endl;
  std::cout << typeid(SI::flatten2<Tuple2>::type).name() << std::endl;

  // constexpr auto s1 = std::tuple_size<Tuple1>::value;
  // constexpr auto s2 = std::tuple_size<Tuple2>::value;

  return 0;
}
#endif


#ifndef _MATRIX_OPERATOR_HPP_
#define _MATRIX_OPERATOR_HPP_

#include <iostream>
#include <iomanip>
#include <complex>
#include <cmath>
#include <Eigen/Dense>
#include <eigen3/Eigen/Dense>
#include <unsupported/Eigen/FFT>

typedef std::vector<std::vector<Eigen::MatrixXcf> > EcoFeats;

struct ECO_Train {
    EcoFeats part1;
    std::vector<Eigen::MatrixXcf> part2;
};

EcoFeats operator+(const EcoFeats &feats1, const EcoFeats &feats2);

EcoFeats operator-(const EcoFeats &feats1, const EcoFeats &feats2);

EcoFeats operator*(const EcoFeats &feats, const float& coef);

std::vector<Eigen::MatrixXcf> operator+(
    const std::vector<Eigen::MatrixXcf>& data1,
    const std::vector<Eigen::MatrixXcf>& data2);

std::vector<Eigen::MatrixXcf> operator-(
    const std::vector<Eigen::MatrixXcf>& data1,
    const std::vector<Eigen::MatrixXcf>& data2);

std::vector<Eigen::MatrixXcf> operator*(
    const std::vector<Eigen::MatrixXcf>& data,
    const float& coef);

ECO_Train operator+(const ECO_Train& data1, const ECO_Train& data2);

ECO_Train operator-(const ECO_Train& data1, const ECO_Train& data2);

ECO_Train operator*(const ECO_Train& data, const float& scale);

#endif


#include "../bb_point_filter.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>

#include <algorithm>

BOOST_AUTO_TEST_SUITE(bb_point_filter_tests)

BOOST_AUTO_TEST_CASE(example_test)
{
    //   (9)
    //                      6
    //    1                /
    //   / \       (8)    /
    //  /   \            /
    // 0     2  4       /   (7)
    //       | / \     /
    //       |/   \   /
    //       3      5
    poly_line line {0,
        std::vector<coordinate> {
            coordinate {0, 0},
            coordinate {1, 1},
            coordinate {2, 0},
            coordinate {2, -1},
            coordinate {3, 0},
            coordinate {4, -1},
            coordinate {5, 2}
        }
    };

    std::vector<point> points {
        {0, 0, coordinate {0, 0}},
        {0, 1, coordinate {1, 1}},
        {0, 2, coordinate {2, 0}},
        {0, 3, coordinate {2, -1}},
        {0, 4, coordinate {3, 0}},
        {0, 5, coordinate {4, -1}},
        {0, 6, coordinate {5, 2}},
        {point::NO_LINE_ID, 7, coordinate {4.9, 0}},
        {point::NO_LINE_ID, 8, coordinate {4, 0.5}},
        {point::NO_LINE_ID, 9, coordinate {1, 2.5}},
    };

    bb_point_filter filter(line.coordinates.begin(), line.coordinates.end(), line.id);
    auto filtered_points = filter(points);
    BOOST_CHECK_EQUAL(filtered_points.size(), 2);
    BOOST_CHECK_EQUAL(filtered_points[0].id, 7);
    BOOST_CHECK_EQUAL(filtered_points[1].id, 8);
}

BOOST_AUTO_TEST_SUITE_END()


#define GLM_ENABLE_EXPERIMENTAL 1
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Halide.h>
#include <fstream>
#include <glm/ext.hpp>
#include <glm/glm.hpp>
#include <halide_image_io.h>
#include <iostream>
#include <stdio.h>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <ImfRgbaFile.h>
#include <ImfStringAttribute.h>
#include <ImfMatrixAttribute.h>
#include <ImfArray.h>
#include <algorithm>
#include <ImfNamespace.h>
#include <tuple>
#include <math.h>

namespace IMF = OPENEXR_IMF_NAMESPACE;

using namespace IMF;
using namespace IMATH_NAMESPACE;

using std::string;
using std::stringstream;
using std::vector;

using namespace std;
using namespace Halide;
using namespace Halide::Tools;
using namespace Eigen;

using Vector4h = Matrix<Halide::Expr, 4, 1>;
using Matrix4h = Matrix<Halide::Expr, 4, 4>;

Var x, y, c;

struct Point
{
    float x;
    float y;
    float z;
    uint16_t r;
    uint16_t g;
    uint16_t b;
};

Matrix4h read4x4MatFromCSV(string csvFile)
{
    Matrix4h transfMat;
    ifstream in(csvFile);
    vector<float> floatVec;
    if (in)
    {
        string line;
        while (getline(in, line))
        {
            stringstream sep(line);
            string field;
            while (getline(sep, field, ','))
            {
                float val = stod(field);
                floatVec.push_back(val);
            }
        }
    }
    transfMat << floatVec[0], floatVec[1], floatVec[2], floatVec[3],
        floatVec[4], floatVec[5], floatVec[6], floatVec[7],
        floatVec[8], floatVec[9], floatVec[10], floatVec[11],
        floatVec[12], floatVec[13], floatVec[14], floatVec[15];
    return transfMat;
}

ostream &operator<<(ostream &os, const Matrix4h &m)
{
    os << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << " " << m(0, 3) << "\n"
       << m(1, 0) << " " << m(1, 1) << " " << m(1, 2) << " " << m(1, 3) << "\n"
       << m(2, 0) << " " << m(2, 1) << " " << m(2, 2) << " " << m(2, 3) << "\n"
       << m(3, 0) << " " << m(3, 1) << " " << m(3, 2) << " " << m(3, 3) << endl;
    return os;
}

void saveImageEXR(Expr result, int width, int height, const char fileName[])
{
    result = cast<float>(result);
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y) = result;
    Buffer<float> output(width, height);
    byteResult.compile_jit(target);
    byteResult.realize(output);

    Array2D<Rgba> pixels;
    pixels.resizeErase(height, width);
    for (int row{0}; row < height; row++)
    {
        for (int col{0}; col < width; col++)
        {
            pixels[row][col].r = output(col, row);
            pixels[row][col].g = output(col, row);
            pixels[row][col].b = output(col, row);
        }
    }
    RgbaOutputFile file(fileName, width, height, WRITE_RGBA);
    file.setFrameBuffer(&pixels[0][0], 1, width);
    file.writePixels(height);
}

void saveImageRaw(Expr result, size_t width, size_t height, const string &basename)
{
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y) = cast<uint8_t>(result);
    byteResult.compile_jit(target);
    Buffer<uint8_t> output(width, height);
    byteResult.realize(output);
    stringstream filename;
    filename << basename;
    save_image(output, filename.str());
}

void saveImage(Expr result, size_t width, size_t height, const string &basename)
{
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y) = cast<uint8_t>(clamp(result, 0.0f, 1.0f) * 255.0f);
    byteResult.compile_jit(target);
    Buffer<uint8_t> output(width, height);
    byteResult.realize(output);
    stringstream filename;
    filename << basename;
    save_image(output, filename.str());
}

ostream &operator<<(ostream &os, const Vector4h &v)
{
    os << v(0) << "\n"
       << v(1) << "\n"
       << v(2) << "\n"
       << v(3) << endl;
    return os;
}

Vector4h makeAxBzCLine(Vector4h a, Vector4h b)
{
    Vector4h result;
    result(0) = a(2) - b(2);
    result(1) = b(0) - a(0);
    result(2) = a(0) * b(2) - b(0) * a(2);
    result(3) = 1.0f;
    return result;
}

Vector4h cross3(Vector4h a, Vector4h b)
{
    Vector4h res;
    res(0) = a(1) * b(2) - a(2) * b(1);
    res(1) = a(2) * b(0) - a(0) * b(2);
    res(2) = a(0) * b(1) - a(1) * b(0);
    res(3) = 0;
    return res;
}

Expr dot3(Vector4h a, Vector4h b)
{
    Expr res;
    res = a(0) * b(0) + a(1) * b(1) + a(2) * b(2);
    return res;
}

tuple<Vector4h, Vector4h> pluckerLine(Vector4h p1, Vector4h p2)
{
    Vector4h l;
    Vector4h l_dash;
    l = p1(3) * p2 - p2(3) * p1;
    l(3) = 0;
    l_dash = cross3(p1, p2);
    return {l, l_dash};
}

Vector4h pluckerPlane(Vector4h l, Vector4h l_dash, Vector4h pluckPt)
{
    Vector4h u;
    u = -pluckPt(3) * l_dash + cross3(pluckPt, l);
    u(3) = dot3(pluckPt, l_dash);
    return u;
}

Vector4h intersectionLinePlane(Vector4h l, Vector4h l_dash, Vector4h plPlane)
{
    Vector4h intersection;
    intersection = -plPlane(3) * l + cross3(plPlane, l_dash);
    intersection(3) = dot3(plPlane, l);
    return intersection;
}

template <typename Function>
void writeBufferToXYZFile(Buffer<float> &buffer, Buffer<uint8_t> &color, string filename, string deliminator, Function condFunc)
{
    std::vector<Point> points;
    for (int j = 0; j < buffer.height(); j++)
    {
        for (int i = 0; i < buffer.width(); i++)
        {
            const auto x = buffer(i, j, 0);
            const auto y = buffer(i, j, 1);
            const auto z = buffer(i, j, 2);
            uint8_t r = color(i, j, 0);
            uint8_t g = color(i, j, 1);
            uint8_t b = color(i, j, 2);
            if (condFunc(x, y, z))
            {
                continue;
            }
            points.push_back({x, y, z, r, g, b});
        }
    }
    std::ofstream outFile;
    outFile.open(filename);
    outFile << points.size() << "\n";
    const auto d = deliminator;
    for (const auto &point : points)
    {
        outFile << point.x << d << point.y << d << point.z << d << point.r << d << point.g << d << point.b << "\n";
    }
}

void writeXYZ(Expr xs, Expr ys, Expr zs, int width, int height, string filename)
{
    Target target = get_host_target();
    Func result;
    result(x, y, c) = 0.0f;
    result(x, y, 0) = xs;
    result(x, y, 1) = ys;
    result(x, y, 2) = zs;

    Buffer<float> output(width, height, 3);
    result.compile_jit(target);
    result.realize(output);

    std::vector<Point> points;
    for (int j = 0; j < output.height(); j++)
    {
        for (int i = 0; i < output.width(); i++)
        {
            const auto x = output(i, j, 0);
            const auto y = output(i, j, 1);
            const auto z = output(i, j, 2);
            uint16_t zero = 0;
            points.push_back(Point{x, y, z, zero, zero, zero});
        }
    }
    std::ofstream outFile;
    outFile.open(filename);
    outFile << points.size() << "\n";
    const auto d = ";";
    for (const auto &point : points)
    {
        outFile << point.x << d << point.y << d << point.z << d << point.r << d << point.g << d << point.b << "\n";
    }
}

void debugImageEXR(Expr channel1Expr, Expr channel2Expr, Expr channel3Expr, int width, int height, const char fileName[])
{
    channel1Expr = cast<float>(channel1Expr);
    channel2Expr = cast<float>(channel2Expr);
    channel3Expr = cast<float>(channel3Expr);
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y, c) = 0.0f;
    byteResult(x, y, 0) = channel1Expr;
    byteResult(x, y, 1) = channel2Expr;
    byteResult(x, y, 2) = channel3Expr;
    Buffer<float> output(width, height, 3);
    byteResult.compile_jit(target);
    byteResult.realize(output);

    Array2D<Rgba> pixels;
    pixels.resizeErase(height, width);
    for (int row{0}; row < height; row++)
    {
        for (int col{0}; col < width; col++)
        {
            pixels[row][col].r = output(col, row, 0);
            pixels[row][col].g = output(col, row, 1);
            pixels[row][col].b = output(col, row, 2);
        }
    }
    RgbaOutputFile file(fileName, width, height, WRITE_RGBA);
    file.setFrameBuffer(&pixels[0][0], 1, width);
    file.writePixels(height);
}

int main(int argc, char **argv)
{
    const string LEFTCAM_SCAN_PNG = "images/scan/left-cam-tilt.png";
    const string RIGHTCAM_SCAN_PNG = "images/scan/right-cam-tilt.png";
    const string DEBUG_PNG = "images/debug/debug.png";
    const string CAM_MAT_PATH = "matrices/camera-matrix.csv";
    const string INV_CAM_MAT_PATH = "matrices/inv-camera-matrix.csv";
    const string TRANSF_LEFTCAM_PROJ_PATH = "matrices/transf-leftcam-proj.csv";
    const string TRANSF_RIGHTCAM_PROJ_PATH = "matrices/transf-rightcam-proj.csv";
    const string POINTCLOUD_LEFTCAM = "pointclouds/left-cam.txt";

    Buffer<uint8_t> leftCamScan = load_image(LEFTCAM_SCAN_PNG);
    Buffer<uint8_t> rightCamScan = load_image(RIGHTCAM_SCAN_PNG);
    const int HEIGHT = leftCamScan.height();
    const int WIDTH = leftCamScan.width();

    Expr leftCamScanRed = leftCamScan(x, y, 0);
    Expr leftCamScanFilterRed = leftCamScanRed * (leftCamScanRed > 80.0f);
    Expr rightCamScanRed = rightCamScan(x, y, 0);
    Expr rightCamScanFilterRed = rightCamScanRed * (rightCamScanRed > 80.0f);

    // read matrices
    Matrix4h camMat = read4x4MatFromCSV(CAM_MAT_PATH);
    Matrix4h invCamMat = read4x4MatFromCSV(INV_CAM_MAT_PATH);
    Matrix4h transfLeftCamProj = read4x4MatFromCSV(TRANSF_LEFTCAM_PROJ_PATH);
    Matrix4h transfRightCamProj = read4x4MatFromCSV(TRANSF_RIGHTCAM_PROJ_PATH);

    // define 3 points on projector to define laser scan
    Vector4h projPt1{0.0f, 0.0f, 0.0f, 1.0f};
    Vector4h projPt2{0.0f, 1.0f, 0.0f, 1.0f};
    Vector4h projPt3{0.0f, 0.0f, 1.0f, 1.0f};

    Vector4h projPt1_lcFrame = transfLeftCamProj * projPt1;
    Vector4h projPt2_lcFrame = transfLeftCamProj * projPt2;
    Vector4h projPt3_lcFrame = transfLeftCamProj * projPt3;

    Vector4h projPt1_rcFrame = transfRightCamProj * projPt1;
    Vector4h projPt2_rcFrame = transfRightCamProj * projPt2;
    Vector4h projPt3_rcFrame = transfRightCamProj * projPt3;

    const auto [projLine_lcFrame, projLineDash_lcFrame] = pluckerLine(projPt1_lcFrame, projPt2_lcFrame);
    Vector4h projPlane_lcFrame = pluckerPlane(projLine_lcFrame, projLineDash_lcFrame, projPt3_lcFrame);

    const auto [projLine_rcFrame, projLineDash_rcFrame] = pluckerLine(projPt1_rcFrame, projPt2_rcFrame);
    Vector4h projPlane_rcFrame = pluckerPlane(projLine_rcFrame, projLineDash_rcFrame, projPt3_rcFrame);

    Vector4h px_lc{x, y, 1.0f, 1.0f};
    Vector4h norm1_lc = invCamMat * px_lc;
    norm1_lc(3) = 1.0f;
    Vector4h norm2_lc = norm1_lc * 2.0f;
    norm2_lc(3) = 1.0f;

    Vector4h px_rc{x, y, 1.0f, 1.0f};
    Vector4h norm1_rc = invCamMat * px_rc;
    norm1_rc(3) = 1.0f;
    Vector4h norm2_rc = norm1_lc * 2.0f;
    norm2_rc(3) = 1.0f;

    const auto [pixelLine_lc, pixelLineDash_lc] = pluckerLine(norm1_lc, norm2_lc);
    Vector4h intersection_lc = intersectionLinePlane(pixelLine_lc, pixelLineDash_lc, projPlane_lcFrame);

    const auto [pixelLine_rc, pixelLineDash_rc] = pluckerLine(norm1_rc, norm2_rc);
    Vector4h intersection_rc = intersectionLinePlane(pixelLine_rc, pixelLineDash_rc, projPlane_rcFrame);

    intersection_lc *= leftCamScanFilterRed > 0;
    intersection_lc /= intersection_lc(3);
    debugImageEXR(intersection_lc(0), intersection_lc(1), intersection_lc(2), WIDTH, HEIGHT, "images/debug/intersection-lc.exr");
    writeXYZ(intersection_lc(0), intersection_lc(1), intersection_lc(2), WIDTH, HEIGHT, POINTCLOUD_LEFTCAM);

    intersection_rc *= rightCamScanFilterRed > 0;
    intersection_rc /= intersection_rc(3);
    writeXYZ(intersection_rc(0), intersection_rc(1), intersection_rc(2), WIDTH, HEIGHT, "pointclouds/right-cam.txt");

    //Func result;
    //result(x, y, c) = 0.0f;
    //result(x, y, 0) = intersection(0);
    //result(x, y, 1) = intersection(1);
    //result(x, y, 2) = intersection(2);

    //Buffer<float> output(WIDTH, HEIGHT, 3);
    //result.realize(output);

    //std::vector<Point> points;
    //for (int j = 0; j < output.height(); j++)
    //{
    //for (int i = 0; i < output.width(); i++)
    //{
    //const auto x = output(i, j, 0);
    //const auto y = output(i, j, 1);
    //const auto z = output(i, j, 2);
    //uint16_t zero = 0;
    //points.push_back(Point{x, y, z, zero, zero, zero});
    //}
    //}
    //std::ofstream outFile;
    //outFile.open(POINTCLOUD_LEFTCAM);
    //outFile << points.size() << "\n";
    //const auto d = ";";
    //for (const auto &point : points)
    //{
    //outFile << point.x << d << point.y << d << point.z << d << point.r << d << point.g << d << point.b << "\n";
    //}
    /*
    const float FOCAL_LEN = 36.1;
    const float PX_DIM = 10 * 10e-6;
    const bool POINT_CLOUD_GLOBAL_FRAME = true;
    const bool POINT_CLOUD_PROJ_FRAME = true;
    const string PROJECTOR_X_PNG = "images/x-val-img.png";
    const string TRANSF_MAT_WORLD_TO_PROJ_CSV = "matrices/transf-world-proj.csv";
    const string TRANSF_PROJ_CAM_CSV = "matrices/transf-proj-cam.csv";
    const string INV_CAM_MAT_CSV = "matrices/inv-cam-mat.csv";
    const string SAVE_DEPTH_IMAGE = "images/depth_img.png";
    const string SAVE_XYZ_PROJ = "pointclouds/proj.txt";
    const string SAVE_XYZ_WORLD = "pointclouds/world.txt";
    const string NO_PROJECTOR_PNG = "images/no-projector.png";

    Halide::Buffer<uint8_t> input = load_image(PROJECTOR_X_PNG);
    const int HEIGHT = input.height();
    const int WIDTH = input.width();

    Matrix4h InvK = read4x4MatFromCSV(INV_CAM_MAT_CSV);
    Matrix4h transMatProjToCam = read4x4MatFromCSV(TRANSF_PROJ_CAM_CSV);
    Matrix4h transMatWorldToProj = read4x4MatFromCSV(TRANSF_MAT_WORLD_TO_PROJ_CSV);

    cout << "Tranformation Matrix World to Projector" << endl;
    cout << transMatWorldToProj << endl;

    Vector4h px{x, y, 1.0f, 0.0f};

    Vector4h normCam1 = InvK * px;
    normCam1(3) = 1.0f;
    Vector4h normCam2 = 2 * normCam1;
    normCam2(3) = 1.0f;

    Vector4h pointCam1 = transMatProjToCam * normCam1;
    pointCam1 = pointCam1 / pointCam1(3);
    Vector4h pointCam2 = transMatProjToCam * normCam2;
    pointCam2 /= pointCam2(3);

    const auto [camLine, camLineDash] = pluckerLine(pointCam1, pointCam2);
    //Vector4h cameraLine = makeAxBzCLine(pointCam1, pointCam2);

    Vector4h pxProj{input(x, y) / 255.0f * 1920.0f, 0.0f, 1.0f, 0.0f};
    Vector4h normProj1 = InvK * pxProj;
    normProj1 = normProj1 / normProj1(2);
    normProj1(3) = 1.0f;
    Vector4h normProj2{0.0f, 0.0f, 0.0f, 1.0f};
    Vector4h normProj3{0.0f, 1.0f, 0.0f, 1.0f};

    const auto [projLine, projLineDash] = pluckerLine(normProj2, normProj3);
    Vector4h ProjPlane = pluckerPlane(projLine, projLineDash, normProj1);
    Vector4h intersection = intersectionLinePlane(camLine, camLineDash, ProjPlane);
    intersection = intersection / intersection(3);
    intersection *= (input(x, y) > 0); // does this make sense?

    Expr depth = intersection(3);

    saveImage(depth, input.width(), input.height(), SAVE_DEPTH_IMAGE);

    Halide::Buffer<uint8_t> color = load_image(NO_PROJECTOR_PNG);

    if (POINT_CLOUD_PROJ_FRAME)
    {
        Func result;
        result(x, y, c) = 0.0f;
        result(x, y, 0) = intersection(0);
        result(x, y, 1) = intersection(1);
        result(x, y, 2) = intersection(2);

        Buffer<float> output(input.width(), input.height(), 3);
        result.realize(output);
        writeBufferToXYZFile(output, color, SAVE_XYZ_PROJ, ";", conditionProjector);
    }
      */

    return 0;
}