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

#include "../include/structures.h"
#include "../include/transformations.h"
#include "../../rectangular_object_with_unknown_width_height_tait_bryan_wc_jacobian.h"
#include "../../rectangular_object_with_unknown_width_height_rodrigues_wc_jacobian.h"
#include "../../rectangular_object_with_unknown_width_height_quaternion_wc_jacobian.h"
#include "../../quaternion_constraint_jacobian.h"

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

std::vector<std::vector<Eigen::Affine3d>> bundle_of_rays;
struct Rectangle{
	std::vector<Eigen::Vector3d> corners_local;
	double scale_x;
	double scale_y;
	Eigen::Affine3d pose;
};
Rectangle rectangle;

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[]){

	std::vector<Eigen::Affine3d> br;
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -2;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -2;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();

	rectangle.scale_x = 1.0;
	rectangle.scale_y = 1.0;
	rectangle.pose = Eigen::Affine3d::Identity();
	rectangle.corners_local.emplace_back(-1,-1, 0);
	rectangle.corners_local.emplace_back( 1,-1, 0);
	rectangle.corners_local.emplace_back( 1, 1, 0);
	rectangle.corners_local.emplace_back(-1, 1, 0);

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

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

	return 0;
}

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

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

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

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

	return true;
}

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

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

	glColor3f(0,1,0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < bundle_of_rays.size(); i++){
		for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
			Eigen::Vector3d z_begin(0, 0,-100);
			Eigen::Vector3d z_end(0, 0, 100);

			Eigen::Vector3d z_begin_t = bundle_of_rays[i][j] * z_begin;
			Eigen::Vector3d z_end_t = bundle_of_rays[i][j] * z_end;

			glVertex3f(z_begin_t.x(), z_begin_t.y(), z_begin_t.z());
			glVertex3f(z_end_t.x(), z_end_t.y(), z_end_t.z());
		}
	}
	glEnd();

	std::vector<Eigen::Vector3d> corners_global;
	for(size_t i = 0; i < rectangle.corners_local.size(); i++){
		Eigen::Vector3d v(rectangle.corners_local[i].x() * rectangle.scale_x, rectangle.corners_local[i].y() * rectangle.scale_y, rectangle.corners_local[i].z());
		corners_global.push_back(rectangle.pose * v);
	}

	glLineWidth(5);
	glColor3f(1,0,0);
	glBegin(GL_LINE_STRIP);
		for(size_t i = 0 ; i < corners_global.size(); i++){
			glVertex3f(corners_global[i].x(), corners_global[i].y(), corners_global[i].z());
		}
		glVertex3f(corners_global[0].x(), corners_global[0].y(), corners_global[0].z());
	glEnd();
	glLineWidth(1);
	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(rectangle.pose);

			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					Eigen::Vector3d vx(bundle_of_rays[i][j](0,0), bundle_of_rays[i][j](1,0), bundle_of_rays[i][j](2,0));
					Eigen::Vector3d vy(bundle_of_rays[i][j](0,1), bundle_of_rays[i][j](1,1), bundle_of_rays[i][j](2,1));

					Eigen::Matrix<double, 2, 1> delta;
					rectangular_object_with_unknown_width_height_tait_bryan_wc(delta,
							pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					Eigen::Matrix<double, 2, 8> delta_jacobian;
					rectangular_object_with_unknown_width_height_tait_bryan_wc_jacobian(delta_jacobian,
							pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					int ir = tripletListB.size();
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir, k, -delta_jacobian(0,k));
					}
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir + 1, k, -delta_jacobian(1,k));
					}

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

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

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 8);
			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(8, 8);
			Eigen::SparseMatrix<double> AtPB(8, 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());
					std::cout << it.value() << std::endl;
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;

			if(h_x.size() == 8){
				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;
				TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(rectangle.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++];
				rectangle.pose = affine_matrix_from_pose_tait_bryan(pose);
				rectangle.scale_x += h_x[counter++];
				rectangle.scale_y += h_x[counter++];
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			TaitBryanPose pose_rand;
			pose_rand.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			rectangle.pose = rectangle.pose * affine_matrix_from_pose_tait_bryan(pose_rand);

			RodriguesPose pose = pose_rodrigues_from_affine_matrix(rectangle.pose);

			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					Eigen::Vector3d vx(bundle_of_rays[i][j](0,0), bundle_of_rays[i][j](1,0), bundle_of_rays[i][j](2,0));
					Eigen::Vector3d vy(bundle_of_rays[i][j](0,1), bundle_of_rays[i][j](1,1), bundle_of_rays[i][j](2,1));

					Eigen::Matrix<double, 2, 1> delta;
					rectangular_object_with_unknown_width_height_rodrigues_wc(delta,
							pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					Eigen::Matrix<double, 2, 8> delta_jacobian;
					rectangular_object_with_unknown_width_height_rodrigues_wc_jacobian(delta_jacobian,
							pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					int ir = tripletListB.size();
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir, k, -delta_jacobian(0,k));
					}
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir + 1, k, -delta_jacobian(1,k));
					}

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

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

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 8);
			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(8, 8);
			Eigen::SparseMatrix<double> AtPB(8, 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());
					std::cout << it.value() << std::endl;
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;

			if(h_x.size() == 8){
				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;
				RodriguesPose pose = pose_rodrigues_from_affine_matrix(rectangle.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++];
				rectangle.pose = affine_matrix_from_pose_rodrigues(pose);
				rectangle.scale_x += h_x[counter++];
				rectangle.scale_y += h_x[counter++];
			}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;

			TaitBryanPose pose_rand;
			pose_rand.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			rectangle.pose = rectangle.pose * affine_matrix_from_pose_tait_bryan(pose_rand);

			QuaternionPose pose = pose_quaternion_from_affine_matrix(rectangle.pose);

			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					Eigen::Vector3d vx(bundle_of_rays[i][j](0,0), bundle_of_rays[i][j](1,0), bundle_of_rays[i][j](2,0));
					Eigen::Vector3d vy(bundle_of_rays[i][j](0,1), bundle_of_rays[i][j](1,1), bundle_of_rays[i][j](2,1));

					Eigen::Matrix<double, 2, 1> delta;
					rectangular_object_with_unknown_width_height_quaternion_wc(delta,
							pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					Eigen::Matrix<double, 2, 9> delta_jacobian;
					rectangular_object_with_unknown_width_height_quaternion_wc_jacobian(delta_jacobian,
							pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					int ir = tripletListB.size();
					for(size_t k = 0 ; k < 9; k++){
						tripletListA.emplace_back(ir, k, -delta_jacobian(0,k));
					}
					for(size_t k = 0 ; k < 9; k++){
						tripletListA.emplace_back(ir + 1, k, -delta_jacobian(1,k));
					}

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

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

			int ir = tripletListB.size();

			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, 3 , -jacobian(0,0));
			tripletListA.emplace_back(ir, 4 , -jacobian(0,1));
			tripletListA.emplace_back(ir, 5 , -jacobian(0,2));
			tripletListA.emplace_back(ir, 6 , -jacobian(0,3));

			tripletListP.emplace_back(ir, ir, 1000000.0);

			tripletListB.emplace_back(ir, 0, delta);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 9);
			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(9, 9);
			Eigen::SparseMatrix<double> AtPB(9, 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());
					std::cout << it.value() << std::endl;
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;

			if(h_x.size() == 9){
				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;
				QuaternionPose pose = pose_quaternion_from_affine_matrix(rectangle.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++];
				rectangle.pose = affine_matrix_from_pose_quaternion(pose);
				rectangle.scale_x += h_x[counter++];
				rectangle.scale_y += h_x[counter++];
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'n':{
			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					TaitBryanPose pose;

					pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.05;
					pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.05;
					pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.05;

					pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
					pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
					pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;

					bundle_of_rays[i][j] = bundle_of_rays[i][j] * affine_matrix_from_pose_tait_bryan(pose);
				}
			}
			TaitBryanPose pose;

			pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;

			pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
			pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
			pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;

			rectangle.pose = rectangle.pose * affine_matrix_from_pose_tait_bryan(pose);

			rectangle.scale_x += ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			rectangle.scale_y += ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			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 << "n: modify rays" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize (Rodrigues)" << std::endl;
	std::cout << "q: optimize (Quaternion)" << std::endl;
}


/*
    Boost Competency Test - GSoC 2020
    digu_J - Digvijay Janartha
    NIT Hamirpur - INDIA
*/

#include <algorithm>
#include <iostream>
#include <iterator>
#include <utility>
#include <vector>
#include <random>
#include <chrono>

#include <boost/geometry/geometry.hpp>

#include "../../includes/concave_hull_k_nearest_neighbours_optimized.hpp"
#include "../../includes/concave_hull_k_nearest_neighbours.hpp"

namespace bg = boost::geometry;
using bg::dsv;

std::mt19937_64 rang(std::chrono::high_resolution_clock::now().time_since_epoch().count());

int my_rand(int l, int r)
{
    if (l > r)
    {
        std::swap(l, r);
    }
	std::uniform_int_distribution <int> uid(l, r);
	return uid(rang);
}

int main()
{
    std::cout << std::fixed << std::setprecision(0);
    #ifdef HOME
        freopen("input.txt", "r", stdin);
        freopen("output.txt", "w", stdout);
    #endif
    typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
    typedef bg::model::multi_point<point_t> mpoint_t;

    mpoint_t mpt1, hull1, hull2;

    int points = 200, k = 3;
    for (int i = 0; i < points; ++i)
    {
        int x = my_rand(0, 1000);
        int y = my_rand(0, 1000);
        bg::append(mpt1, point_t(x, y));
    }

    algo2::ConcaveHullKNN(mpt1, hull1, k);
    int t1 = (int)(clock() * 1000. / CLOCKS_PER_SEC);

    algo3::ConcaveHullKNN(mpt1, hull2, k);
    int t2 = (int)(clock() * 1000. / CLOCKS_PER_SEC) - t1;

    std::cout << "Dataset: " << dsv(mpt1) << std::endl;
    std::cout << "Convex hull: " << dsv(hull1) << std::endl;

    std:: cout << t1 << " " << t2 << std::endl;
    return 0;
}

/*
Output is very big, I made this file to check performance (execution time) using clock()
*/


#include <utility>
#include <iostream>
#include <vector>
#include "Classification.h"
#include <boost/algorithm/string.hpp>

const std::array<int, Classification::N> Classification::precision{{6, 6, 0, 2, 2, 2, 0}};

Classification::Classification(std::string model_file_name) :
        m_model_file_name{std::move(model_file_name)} {
    read_cf();
    dlib::deserialize(m_model_file_name) >> df;
}

void Classification::process_data(const std::string &line) {
    std::vector<std::string> data;
    boost::split(data, line, boost::is_any_of(";"));

    sample_type sample;

    for (int i = 0; i < N; ++i) {
        if (!data[i].empty()) {
            sample(i) = std::stod(data[i]);
        } else {
            sample(i) = 0;
        }
    }

    auto label = get_label(sample);
    get_data(label, sample);
}

void Classification::read_cf() {
    std::ifstream fs(m_model_file_name + ".cf");

    int n;
    fs >> n;
    for (int i = 0; i < N; ++i) {
        fs >> norm_cf[i][0];
        fs >> norm_cf[i][1];
    }
}
double Classification::get_label(const Classification::sample_type &sample) const {
    auto test_sample = sample;

    for (int i = 0; i < N; ++i) {
        test_sample(i) = test_sample(i) / norm_cf[i][1];
    }

    return df(test_sample);
}

void Classification::get_data(size_t num, const Classification::sample_type &sample) {
    std::vector<std::array<double, N>> raw_data;
    std::ifstream                      fs(m_model_file_name + "." + std::to_string(num));

    for (std::string line; std::getline(fs, line);) {

        std::vector<std::string> data;
        boost::split(data, line, boost::is_any_of(";"));

        std::array<double, N> smpl;

        std::generate_n(smpl.begin(), N, [i = 0, &data]() mutable { return std::stod(data[i++]); });

        raw_data.emplace_back(smpl);
    }

    sort_data(raw_data, sample);
    out_data(raw_data);
}

void Classification::sort_data(std::vector<std::array<double, N>> &raw_data, const sample_type &sample) const {
    auto x = sample(0);
    auto y = sample(1);

    std::sort(raw_data.begin(), raw_data.end(), [x, y](const auto &a, const auto &b) {
        return (std::sqrt(std::pow(x - a[0], 2) + std::pow(y - a[1], 2)) <
                std::sqrt(std::pow(x - b[0], 2) + std::pow(y - b[1], 2)));
    });
}

void Classification::out_data(const std::vector<std::array<double, N>> &raw_data) const {
    for (const auto &data : raw_data) {
        std::cout << std::fixed << std::setprecision(precision[0]);
        std::cout << data[0] << ";" << data[1];
        for (auto j = 2; j < N; ++j) {
            std::cout << std::fixed << std::setprecision(precision[j]);
            std::cout << ";" << data[j];
        }
        std::cout << "\n";
    }
}


#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

#include <yaml-cpp/yaml.h>

#include "YamlFileIO.h"

namespace common{

bool loadExtFileOpencv(const std::string &file, Eigen::Matrix4d &T)
{
    cv::FileStorage fs;
    try
    {
        fs.open(file, cv::FileStorage::READ);

    }catch(cv::Exception &ex){
        std::cout << std::string(ex.what());
        return false;
    }catch(...){
        std::cout << "Fail to read " << file << std::endl;
        return false;
    }

    Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
    Eigen::Vector3d t = Eigen::Vector3d::Zero();
    cv::Mat rvec, tvec;
    fs["extrinsic_rotation"] >> rvec;
    fs["extrinsic_translation"] >> tvec;
    cv::cv2eigen(rvec, R);
    cv::cv2eigen(tvec, t);
    fs.release();

    T = Eigen::Matrix4d::Identity();
    T.block<3, 3>(0, 0) = R;
    T.block<3, 1>(0, 3) = t;
    return true;
}

void saveExtFileOpencv(const std::string &file, const Eigen::Matrix4d &T)
{
    cv::FileStorage fs(file, cv::FileStorage::WRITE);
    if (!fs.isOpened())
    {
        std::cout << "Fail to write " << file << std::endl;
        return;
    }

    Eigen::Matrix3d R = T.block<3, 3>(0, 0);
    Eigen::Vector3d t = T.block<3, 1>(0, 3);
    cv::Mat rvec, tvec;
    cv::eigen2cv(R, rvec);
    cv::eigen2cv(t, tvec);
    fs << "extrinsic_rotation" << rvec;
    fs << "extrinsic_translation" << tvec;

    fs.release();
}

bool loadIntrinFileKalibr(const std::string &file,
                        cv::Mat &K,
                        cv::Mat &D,
                        std::string &distort_type,
                        int &img_width,
                        int &img_height)
{
    if (file.empty())
    {
        std::cout << "[loadIntrinFileKalibr] Empty input file name!\n";
        return false;
    }

    YAML::Node root_node;
    YAML::Node cam0_node;
    try
    {
        root_node = YAML::LoadFile(file);
    }
    catch (YAML::BadFile &e)
    {
        std::cout << "[loadIntrinFileKalibr] Could not open file: " << file << "\n";
        return false;
    }
    catch (YAML::ParserException &e)
    {
        std::cout << "[loadIntrinFileKalibr] Invalid file format: " << file << "\n";
        return false;
    }
    if (root_node.IsNull())
    {
        std::cout << "[loadIntrinFileKalibr] Could not open file: " << file << "\n";
        return false;
    }

    cam0_node = root_node["cam0"];
    std::string distortion_type = cam0_node["distortion_model"].as<std::string>();;

    double cam0_distortion[5] = {0.0};
    if(distortion_type == "equidistant")
    {
        cam0_distortion[0] = cam0_node["distortion_coeffs"][0].as<double>();
        cam0_distortion[1] = cam0_node["distortion_coeffs"][1].as<double>();
        cam0_distortion[2] = cam0_node["distortion_coeffs"][2].as<double>();
        cam0_distortion[3] = cam0_node["distortion_coeffs"][3].as<double>();
        D = (cv::Mat_<double>(1, 4) << cam0_distortion[0], cam0_distortion[1],
                cam0_distortion[2], cam0_distortion[3]);
        // revise letter spelling
        distort_type = "equi-distant";
    }
    if (distortion_type == "radtan")
    {
        cam0_distortion[0] = cam0_node["distortion_coeffs"][0].as<double>();
        cam0_distortion[1] = cam0_node["distortion_coeffs"][1].as<double>();
        cam0_distortion[2] = cam0_node["distortion_coeffs"][2].as<double>();
        cam0_distortion[3] = cam0_node["distortion_coeffs"][3].as<double>();
        cam0_distortion[4] = cam0_node["distortion_coeffs"][4].as<double>();
        D = (cv::Mat_<double>(1, 5) << cam0_distortion[0], cam0_distortion[1],
                cam0_distortion[2], cam0_distortion[3], cam0_distortion[4]);

        distort_type = "radtan";
    }
    Eigen::Matrix3d cam0_intrinsic = Eigen::Matrix3d::Identity();
    cam0_intrinsic(0, 0) = cam0_node["intrinsics"][0].as<double>();
    cam0_intrinsic(1, 1) = cam0_node["intrinsics"][1].as<double>();
    cam0_intrinsic(0, 2) = cam0_node["intrinsics"][2].as<double>();
    cam0_intrinsic(1, 2) = cam0_node["intrinsics"][3].as<double>();

    img_width = cam0_node["resolution"][0].as<int>();
    img_height = cam0_node["resolution"][1].as<int>();
    cv::eigen2cv(cam0_intrinsic, K);

    std::cout << "[loadIntrinFileKalibr] distortion type: " << distort_type << std::endl;
    std::cout << "[loadIntrinFileKalibr] cam0 intrinsic : \n"
              << cam0_intrinsic << "\n";
    std::cout << "[loadIntrinFileKalibr] cam0 distrotion : " << cam0_distortion[0] << ", " << cam0_distortion[3] << "\n";
    return true;
}

bool loadIntrinFileOpencv(const std::string &file_name,
                        cv::Mat &K,
                        cv::Mat &D,
                        std::string &distort_type,
                        int &img_width,
                        int &img_height)
{
    if (file_name.empty())
    {
        std::cout << "[loadIntrinFileOpencv] Empty input file name " << file_name << "\n";
        return false;
    }

    cv::FileStorage fsCamera;
    try
    {
        fsCamera.open(file_name, cv::FileStorage::READ);

    }catch(cv::Exception &ex){
        std::cout << std::string(ex.what());
        return false;
    }catch(...){
        std::cout << "invalid camera calibration file: " << file_name << "\n";
        return false;
    }

    img_width = (int)fsCamera["image_width"];
    img_height = (int)fsCamera["image_height"];
    std::string distortion_type = (std::string)fsCamera["camera_model"];
    if (distortion_type == "equidistant")
        distort_type = "equi-distant";
    if (distortion_type == "radtan")
        distort_type = "radtan";

    fsCamera["camera_matrix"] >> K;
    fsCamera["distortion_coefficients"] >> D;
    std::cout << "camera_matrix : \n"
              << K << "\n";
    std::cout << "distortion_coefficients : \n"
              << D << "\n";

    fsCamera.release();

    return true;
}

bool saveIntrinFileOpencv(const std::string &file_name,
                        const cv::Mat &K,
                        const cv::Mat &D,
                        const std::string &distort_type,
                        const int img_width,
                        const int img_height)
{
    if (file_name.empty())
    {
        std::cout << "[saveIntrinFileOpencv] Empty file name!\n";
        return false;
    }

    cv::FileStorage fs(file_name, cv::FileStorage::WRITE);
    if (!fs.isOpened())
    {
        std::cout << "[saveIntrinFileOpencv] Fail to open " << file_name << "\n";
        return false;
    }

    fs << "camera_model" << distort_type;
    fs << "camera_matrix" << K;
    fs << "distortion_coefficients" << D;

    fs << "avg_reprojection_error" << 1.2799437834241592e-01;
    fs << "image_width" << img_width;
    fs << "image_height" << img_height;
    fs << "serial_num" <<  1;
    fs.release();
    return true;
}

bool loadCCTagResultFile(const std::string &filepath,
                    std::vector<std::string> &v_img_path,
                    std::vector<std::vector<cv::Point2d>> &vv_kpts)
{
    if (filepath.empty())
    {
        std::cout << " Empty file name!\n";
        return false;
    }

    YAML::Node frm_node;
    try
    {
        frm_node = YAML::LoadFile(filepath);
    }
    catch (YAML::BadFile &e)
    {
        std::cout << " Could not open file: " << filepath << "\n";
        return false;
    }
    catch (YAML::ParserException &e)
    {
        std::cout << " Invalid file format: " << filepath << "\n";
        return false;
    }
    if (frm_node.IsNull())
    {
        std::cout << " Could not open file: " << filepath << "\n";
        return false;
    }

    v_img_path.clear();
    vv_kpts.clear();

    // parse FramePoses
    for (YAML::const_iterator it = frm_node.begin(); it != frm_node.end(); ++it)
    {
        int f = it->first.as<int>();
        std::string id = std::to_string(f);

        // image path
        std::string img_path = frm_node[id]["image_file"].as<std::string>();
        std::vector<cv::Point2d> v_keypoints;
        for (size_t k = 0; k < frm_node[id]["keypoints"].size(); ++k)
        {
            cv::Point2d keypoint;
            keypoint.x = frm_node[id]["keypoints"][k][0].as<double>();
            keypoint.y = frm_node[id]["keypoints"][k][1].as<double>();
            v_keypoints.emplace_back(keypoint);
        }

        std::cout<< " Frame " << id << " detect " << v_keypoints.size()
                  << " cctag keypoints.\n";
        v_img_path.emplace_back(img_path);
        vv_kpts.emplace_back(v_keypoints);
    }
    std::cout<< " read " << v_img_path.size() << " frames! \n";

    return true;
}
}


// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: Copyright 2019-2022 Heal Research

#include "operon/core/individual.hpp"
#include "operon/operators/non_dominated_sorter.hpp"
#include <Eigen/Core>

namespace Operon {
auto RankOrdinalSorter::Sort(Operon::Span<Operon::Individual const> pop, Operon::Scalar eps) const -> NondominatedSorterBase::Result
{
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
    using Vec = Eigen::Array<Eigen::Index, -1, 1>;
    using Mat = Eigen::Array<Eigen::Index, -1, -1>;

    const auto n = static_cast<Eigen::Index>(pop.size());
    const auto m = static_cast<Eigen::Index>(pop.front().Size());
    assert(m >= 2);

    // 1) sort indices according to the stable sorting rules
    Mat p(n, m); // permutation matrix
    Mat r(m, n); // ordinal rank matrix
    p.col(0) = Vec::LinSpaced(n, 0, n - 1);
    r(0, p.col(0)) = Vec::LinSpaced(n, 0, n - 1);

    Operon::Less cmp;
    std::vector<Operon::Scalar> buf(n); // buffer to store fitness values to avoid pointer indirections during sorting
    for (auto i = 1; i < m; ++i) {
        std::transform(pop.begin(), pop.end(), buf.begin(), [i](auto const& ind) { return ind[i]; });
        p.col(i) = p.col(i - 1); // this is a critical part of the approach
        std::stable_sort(p.col(i).begin(), p.col(i).end(), [&](auto a, auto b) { return cmp(buf[a], buf[b], eps); });
        r(i, p.col(i)) = Vec::LinSpaced(n, 0, n - 1);
    }
    // 2) save min and max positions as well as the column index for the max position
    Vec maxc(n);
    Vec maxp(n);
    for (auto i = 0; i < n; ++i) {
        auto c = r.col(i);
        auto max = std::max_element(c.begin(), c.end());
        maxp(i) = *max;
        maxc(i) = std::distance(c.begin(), max);
    }

    // 3) compute ranks / fronts
    Vec rank = Vec::Zero(n); // individual ranks
    for (auto i : p(Eigen::seq(0, n - 2), 0)) {
        if (maxp(i) == n - 1) {
            continue;
        }
        for (auto j : p(Eigen::seq(maxp(i) + 1, n - 1), maxc(i))) {
            rank(j) += static_cast<int64_t>(rank(i) == rank(j) && (r.col(i) < r.col(j)).all());
        }
    }
    std::vector<std::vector<size_t>> fronts(rank.maxCoeff() + 1);
    for (auto i = 0; i < n; ++i) {
        fronts[rank(i)].push_back(i);
    }
    return fronts;
#else
    throw std::runtime_error("RankOrdinal requires Eigen >= 3.4.0");
#endif
}
} // namespace Operon


/*
 * This file belongs to the Galois project, a C++ library for exploiting parallelism.
 * The code is being released under the terms of the 3-Clause BSD License (a
 * copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include <cmath>
#include <fstream>
#include <iostream>
#include <ostream>
#include "matrixCompletion.h"
#include "galois/runtime/TiledExecutor.h"
#include "galois/ParallelSTL.h"
#include "galois/graphs/Graph.h"
#include "Lonestar/BoilerPlate.h"

#ifdef HAS_EIGEN
#include <Eigen/Sparse>
#include <Eigen/Dense>
#endif

#ifdef _OPENMP
#include <omp.h>
#endif

static const char* const name = "Matrix Completion";
static const char* const desc =
    "Computes Matrix Decomposition using Stochastic "
    "Gradient Descent or Alternating Least Squares";
static const char* const url = 0;

enum Algo {
  syncALS,
  simpleALS,
  sgdByItems,
  sgdByEdges,
  sgdBlockEdge,
  sgdBlockJump,
};

enum Step { bold, bottou, intel, inverse, purdue };

/*
 * Commandline options for different Algorithms
 */
static cll::opt<Algo>
    algo("algo", cll::desc("Choose an algorithm:"),
         cll::values(
             clEnumValN(Algo::syncALS, "syncALS", "Alternating least squares"),
             clEnumValN(Algo::simpleALS, "simpleALS",
                        "Simple alternating least squares"),
             clEnumValN(Algo::sgdBlockEdge, "sgdBlockEdge",
                        "SGD Edge blocking (default)"),
             clEnumValN(Algo::sgdBlockJump, "sgdBlockJump",
                        "SGD using Block jumping "),
             clEnumValN(Algo::sgdByItems, "sgdByItems", "Simple SGD on Items"),
             clEnumValN(Algo::sgdByEdges, "sgdByEdges", "Simple SGD on edges"),
             clEnumValEnd),
         cll::init(Algo::sgdBlockEdge));
/*
 * Commandline options for different learning functions
 */
static cll::opt<Step> learningRateFunction(
    "learningRateFunction", cll::desc("Choose learning rate function:"),
    cll::values(clEnumValN(Step::intel, "intel", "Intel"),
                clEnumValN(Step::purdue, "purdue", "Purdue"),
                clEnumValN(Step::bottou, "bottou", "Bottou"),
                clEnumValN(Step::bold, "bold", "Bold (default)"),
                clEnumValN(Step::inverse, "inverse", "Inverse"), clEnumValEnd),
    cll::init(Step::bold));

static cll::opt<int> cutoff("cutoff");

static const unsigned ALS_CHUNK_SIZE = 4;

size_t NUM_ITEM_NODES = 0;

struct PurdueStepFunction : public StepFunction {
  virtual std::string name() const { return "Purdue"; }
  virtual LatentValue stepSize(int round) const {
    return learningRate * 1.5 / (1.0 + decayRate * pow(round + 1, 1.5));
  }
};

struct IntelStepFunction : public StepFunction {
  virtual std::string name() const { return "Intel"; }
  virtual LatentValue stepSize(int round) const {
    return learningRate * pow(decayRate, round);
  }
};

struct BottouStepFunction : public StepFunction {
  virtual std::string name() const { return "Bottou"; }
  virtual LatentValue stepSize(int round) const {
    return learningRate / (1.0 + learningRate * lambda * round);
  }
};

struct InverseStepFunction : public StepFunction {
  virtual std::string name() const { return "Inverse"; }
  virtual LatentValue stepSize(int round) const { return 1.0 / (round + 1); }
};

struct BoldStepFunction : public StepFunction {
  virtual std::string name() const { return "Bold"; }
  virtual bool isBold() const { return true; }
  virtual LatentValue stepSize(int round) const { return 0.0; }
};

template <typename Graph>
double sumSquaredError(Graph& g) {
  typedef typename Graph::GraphNode GNode;
  // computing Root Mean Square Error
  // Assuming only item nodes have edges
  galois::GAccumulator<double> error;

  galois::do_all(
      galois::iterate(g.begin(), g.begin() + NUM_ITEM_NODES), [&](GNode n) {
        for (auto ii = g.edge_begin(n), ei = g.edge_end(n); ii != ei; ++ii) {
          GNode dst = g.getEdgeDst(ii);
          LatentValue e =
              predictionError(g.getData(n).latentVector,
                              g.getData(dst).latentVector, g.getEdgeData(ii));
          error += (e * e);
        }
      });
  return error.reduce();
}

template <typename Graph>
size_t countEdges(Graph& g) {
  typedef typename Graph::GraphNode GNode;
  galois::GAccumulator<size_t> edges;
  galois::runtime::Fixed2DGraphTiledExecutor<Graph> executor(g);
  std::cout << "NUM_ITEM_NODES : " << NUM_ITEM_NODES << "\n";
  executor.execute(g.begin(), g.begin() + NUM_ITEM_NODES,
                   g.begin() + NUM_ITEM_NODES, g.end(), itemsPerBlock,
                   usersPerBlock,
                   [&](GNode src, GNode dst,
                       typename Graph::edge_iterator edge) { edges += 1; },
                   false); // false = no locks
  return edges.reduce();
}

template <typename Graph>
void verify(Graph& g, const std::string& prefix) {
  std::cout << countEdges(g) << " : " << g.sizeEdges() << "\n";
  if (countEdges(g) != g.sizeEdges()) {
    GALOIS_DIE("Error: edge list of input graph probably not sorted");
  }

  double error = sumSquaredError(g);
  double rmse  = std::sqrt(error / g.sizeEdges());

  std::cout << prefix << "RMSE: " << rmse << "\n";
}

template <typename T, unsigned Size>
struct ExplicitFiniteChecker {};

template <typename T>
struct ExplicitFiniteChecker<T, 4U> {
  static_assert(std::numeric_limits<T>::is_iec559, "Need IEEE floating point");
  bool isFinite(T v) {
    union {
      T value;
      uint32_t bits;
    } a = {v};
    if (a.bits == 0x7F800000) {
      return false; // +inf
    } else if (a.bits == 0xFF800000) {
      return false; // -inf
    } else if (a.bits >= 0x7F800001 && a.bits <= 0x7FBFFFFF) {
      return false; // signaling NaN
    } else if (a.bits >= 0xFF800001 && a.bits <= 0xFFBFFFFF) {
      return false; // signaling NaN
    } else if (a.bits >= 0x7FC00000 && a.bits <= 0x7FFFFFFF) {
      return false; // quiet NaN
    } else if (a.bits >= 0xFFC00000 && a.bits <= 0xFFFFFFFF) {
      return false; // quiet NaN
    }
    return true;
  }
};

template <typename T>
struct ExplicitFiniteChecker<T, 8U> {
  static_assert(std::numeric_limits<T>::is_iec559, "Need IEEE floating point");
  bool isFinite(T v) {
    union {
      T value;
      uint64_t bits;
    } a = {v};
    if (a.bits == 0x7FF0000000000000) {
      return false; // +inf
    } else if (a.bits == 0xFFF0000000000000) {
      return false; // -inf
    } else if (a.bits >= 0x7FF0000000000001 && a.bits <= 0x7FF7FFFFFFFFFFFF) {
      return false; // signaling NaN
    } else if (a.bits >= 0xFFF0000000000001 && a.bits <= 0xFFF7FFFFFFFFFFFF) {
      return false; // signaling NaN
    } else if (a.bits >= 0x7FF8000000000000 && a.bits <= 0x7FFFFFFFFFFFFFFF) {
      return false; // quiet NaN
    } else if (a.bits >= 0xFFF8000000000000 && a.bits <= 0xFFFFFFFFFFFFFFFF) {
      return false; // quiet NaN
    }
    return true;
  }
};

template <typename T>
bool isFinite(T v) {
#ifdef __FAST_MATH__
  return ExplicitFiniteChecker<T, sizeof(T)>().isFinite(v);
#else
  return std::isfinite(v);
#endif
}

double countFlops(size_t nnz, int rounds, int k) {
  double flop = 0;
  if (useExactError) {
    // dotProduct = 2K, square = 1, sum = 1
    flop += nnz * (2.0 * k + 1 + 1);
  } else {
    // Computed during gradient update: square = 1, sum = 1
    flop += nnz * (1 + 1);
  }
  // dotProduct = 2K, gradient = 10K,
  flop += rounds * (nnz * (12.0 * k));
  return flop;
}

/*
 * Common function to execute different algorithms
 * till convergence.
 *
 * @param StepFunction to be used
 * @param Graph
 * @param fn (algorithm)
 *
 */
template <typename Graph, typename Fn>
void executeUntilConverged(const StepFunction& sf, Graph& g, Fn fn) {
  galois::GAccumulator<double> errorAccum;
  std::vector<LatentValue> steps(updatesPerEdge);
  LatentValue last    = -1.0;
  unsigned deltaRound = updatesPerEdge;
  LatentValue rate    = learningRate;

  galois::StatTimer executeAlgoTimer("Algorithm Execution Time");
  galois::TimeAccumulator elapsed;
  elapsed.start();

  unsigned long lastTime = 0;

  for (unsigned int round = 0;; round += deltaRound) {
    if (fixedRounds > 0 && round >= fixedRounds)
      break;
    if (fixedRounds > 0)
      deltaRound = std::min(deltaRound, fixedRounds - round);

    for (unsigned i = 0; i < updatesPerEdge; ++i) {
      // Assume that loss decreases
      if (sf.isBold())
        steps[i] = i == 0 ? rate : steps[i - 1] * 1.05;
      else
        steps[i] = sf.stepSize(round + i);
    }

    executeAlgoTimer.start();
    fn(&steps[0], round + deltaRound, useExactError ? &errorAccum : NULL);
    executeAlgoTimer.stop();
    double error = useExactError ? errorAccum.reduce() : sumSquaredError(g);

    elapsed.stop();

    unsigned long curElapsed = elapsed.get();
    elapsed.start();
    unsigned long millis = curElapsed - lastTime;
    lastTime             = curElapsed;

    double gflops = countFlops(g.sizeEdges(), deltaRound, LATENT_VECTOR_SIZE) /
                    millis / 1e6;

    int curRound = round + deltaRound;
    galois::gPrint("R: ", curRound, " elapsed (ms): ", curElapsed,
                   " GFLOP/s: ", gflops);
    if (useExactError) {
      galois::gPrint(" RMSE (R ", curRound,
                     "): ", std::sqrt(error / g.sizeEdges()), "\n");
    } else {
      galois::gPrint(" Approx. RMSE (R ", (curRound - 1),
                     ".5): ", std::sqrt(std::abs(error / g.sizeEdges())), "\n");
    }

    galois::gPrint("Error Change : ", std::abs((last - error) / last), "\n");
    if (!isFinite(error))
      break;
    if (fixedRounds <= 0 &&
        (round >= maxUpdates || std::abs((last - error) / last) < tolerance))
      break;
    if (sf.isBold()) {
      // Assume that loss decreases first round
      if (last >= 0.0 && last < error)
        rate = steps[deltaRound - 1] * 0.5;
      else
        rate = steps[deltaRound - 1] * 1.05;
    }
    last = error;
  }
}

/*
 * Divides the Items and users into 2D blocks.
 * Locks each block to work on it.
 */
struct SGDBlockJumpAlgo {
  bool isSgd() const { return true; }
  typedef galois::substrate::PaddedLock<true> SpinLock;
  static const bool precomputeOffsets = true; // false;

  std::string name() const { return "sgdBlockJumpAlgo"; }

  struct Node {
    LatentValue latentVector[LATENT_VECTOR_SIZE];
  };

  typedef galois::graphs::LC_CSR_Graph<Node, EdgeType>
      //    ::with_numa_alloc<true>::type
      ::with_no_lockable<true>::type Graph;
  typedef Graph::GraphNode GNode;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  size_t userIdToUserNode(size_t userId) { return userId + NUM_ITEM_NODES; }

  struct BlockInfo {
    size_t id;
    size_t x;
    size_t y;
    size_t userStart;
    size_t userEnd;
    size_t itemStart;
    size_t itemEnd;
    size_t numitems;
    size_t updates;
    double error;
    int* userOffsets;

    std::ostream& print(std::ostream& os) {
      os << "id: " << id << " x: " << x << " y: " << y
         << " userStart: " << userStart << " userEnd: " << userEnd
         << " itemStart: " << itemStart << " itemEnd: " << itemEnd
         << " updates: " << updates << "\n";
      return os;
    }
  };

  struct Process {
    Graph& g;
    SpinLock *xLocks, *yLocks;
    BlockInfo* blocks;
    size_t numXBlocks, numYBlocks;
    LatentValue* steps;
    size_t maxUpdates;
    galois::GAccumulator<double>* errorAccum;

    struct GetDst : public std::unary_function<Graph::edge_iterator, GNode> {
      Graph* g;
      GetDst() {}
      GetDst(Graph* _g) : g(_g) {}
      GNode operator()(Graph::edge_iterator ii) const {
        return g->getEdgeDst(ii);
      }
    };

    /**
     * Preconditions: row and column of slice are locked.
     *
     * Postconditions: increments update count, does sgd update on each item
     * and user in the slice
     */
    template <bool Enable = precomputeOffsets>
    size_t runBlock(BlockInfo& si,
                    typename std::enable_if<!Enable>::type* = 0) {
      typedef galois::NoDerefIterator<Graph::edge_iterator> no_deref_iterator;
      typedef boost::transform_iterator<GetDst, no_deref_iterator>
          edge_dst_iterator;

      LatentValue stepSize = steps[si.updates - maxUpdates + updatesPerEdge];
      size_t seen          = 0;
      double error         = 0.0;

      // Set up item iterators
      size_t itemId      = 0;
      Graph::iterator mm = g.begin(), em = g.begin();
      std::advance(mm, si.itemStart);
      std::advance(em, si.itemEnd);

      GetDst fn{&g};

      // For each item in the range
      for (; mm != em; ++mm, ++itemId) {
        GNode item      = *mm;
        Node& itemData  = g.getData(item);
        size_t lastUser = si.userEnd + NUM_ITEM_NODES;

        edge_dst_iterator start(no_deref_iterator(g.edge_begin(
                                    item, galois::MethodFlag::UNPROTECTED)),
                                fn);
        edge_dst_iterator end(no_deref_iterator(g.edge_end(
                                  item, galois::MethodFlag::UNPROTECTED)),
                              fn);

        // For each edge in the range
        for (auto ii =
                 std::lower_bound(start, end, si.userStart + NUM_ITEM_NODES);
             ii != end; ++ii) {
          GNode user = g.getEdgeDst(*ii.base());

          if (user >= lastUser)
            break;

          LatentValue e = doGradientUpdate(itemData.latentVector,
                                           g.getData(user).latentVector, lambda,
                                           g.getEdgeData(*ii.base()), stepSize);
          if (errorAccum)
            error += e * e;
          ++seen;
        }
      }

      si.updates += 1;
      if (errorAccum) {
        *errorAccum += (error - si.error);
        si.error = error;
      }

      return seen;
    }

    template <bool Enable = precomputeOffsets>
    size_t runBlock(BlockInfo& si, typename std::enable_if<Enable>::type* = 0) {
      LatentValue stepSize = steps[si.updates - maxUpdates + updatesPerEdge];
      size_t seen          = 0;
      double error         = 0.0;

      // Set up item iterators
      size_t itemId      = 0;
      Graph::iterator mm = g.begin(), em = g.begin();
      std::advance(mm, si.itemStart);
      std::advance(em, si.itemEnd);

      // For each item in the range
      for (; mm != em; ++mm, ++itemId) {
        if (si.userOffsets[itemId] < 0)
          continue;

        GNode item      = *mm;
        Node& itemData  = g.getData(item);
        size_t lastUser = si.userEnd + NUM_ITEM_NODES;

        // For each edge in the range
        for (auto ii = g.edge_begin(item) + si.userOffsets[itemId],
                  ei = g.edge_end(item);
             ii != ei; ++ii) {
          GNode user = g.getEdgeDst(ii);

          if (user >= lastUser)
            break;

          LatentValue e = doGradientUpdate(itemData.latentVector,
                                           g.getData(user).latentVector, lambda,
                                           g.getEdgeData(ii), stepSize);
          if (errorAccum)
            error += e * e;
          ++seen;
        }
      }

      si.updates += 1;
      if (errorAccum) {
        *errorAccum += (error - si.error);
        si.error = error;
      }

      return seen;
    }

    /**
     * Searches next slice to work on.
     *
     * @returns slice id to work on, x and y locks are held on the slice
     */
    size_t getNextBlock(BlockInfo* sp) {
      size_t numBlocks   = numXBlocks * numYBlocks;
      size_t nextBlockId = sp->id + 1;
      for (size_t i = 0; i < 2 * numBlocks; ++i, ++nextBlockId) {
        // Wrap around
        if (nextBlockId == numBlocks)
          nextBlockId = 0;

        BlockInfo& nextBlock = blocks[nextBlockId];

        if (nextBlock.updates < maxUpdates && xLocks[nextBlock.x].try_lock()) {
          if (yLocks[nextBlock.y].try_lock()) {
            // Return while holding locks
            return nextBlockId;
          } else {
            xLocks[nextBlock.x].unlock();
          }
        }
      }

      return numBlocks;
    }

    void operator()(unsigned tid, unsigned total) {
      galois::StatTimer timer("PerThreadTime");
      // TODO: Report Accumulators at the end
      galois::GAccumulator<size_t> edgesVisited;
      galois::GAccumulator<size_t> blocksVisited;
      size_t numBlocks = numXBlocks * numYBlocks;
      size_t xBlock    = (numXBlocks + total - 1) / total;
      size_t xStart    = std::min(xBlock * tid, numXBlocks - 1);
      size_t yBlock    = (numYBlocks + total - 1) / total;
      size_t yStart    = std::min(yBlock * tid, numYBlocks - 1);
      BlockInfo* sp    = &blocks[xStart + yStart + numXBlocks];

      timer.start();

      while (true) {
        sp = &blocks[getNextBlock(sp)];
        if (sp == &blocks[numBlocks])
          break;
        blocksVisited += 1;
        edgesVisited += runBlock(*sp);

        xLocks[sp->x].unlock();
        yLocks[sp->y].unlock();
      }

      timer.stop();
    }
  };

  void operator()(Graph& g, const StepFunction& sf) {
    galois::StatTimer preProcessTimer("PreProcessingTime");
    preProcessTimer.start();
    const size_t numUsers = g.size() - NUM_ITEM_NODES;
    const size_t numYBlocks =
        (NUM_ITEM_NODES + itemsPerBlock - 1) / itemsPerBlock;
    const size_t numXBlocks = (numUsers + usersPerBlock - 1) / usersPerBlock;
    const size_t numBlocks  = numXBlocks * numYBlocks;

    SpinLock* xLocks = new SpinLock[numXBlocks];
    SpinLock* yLocks = new SpinLock[numYBlocks];

    std::cout << "itemsPerBlock: " << itemsPerBlock
              << " usersPerBlock: " << usersPerBlock
              << " numBlocks: " << numBlocks << " numXBlocks: " << numXBlocks
              << " numYBlocks: " << numYBlocks << "\n";

    // Initialize
    BlockInfo* blocks = new BlockInfo[numBlocks];
    for (size_t i = 0; i < numBlocks; i++) {
      BlockInfo& si = blocks[i];
      si.id         = i;
      si.x          = i % numXBlocks;
      si.y          = i / numXBlocks;
      si.updates    = 0;
      si.error      = 0.0;
      si.userStart  = si.x * usersPerBlock;
      si.userEnd    = std::min((si.x + 1) * usersPerBlock, numUsers);
      si.itemStart  = si.y * itemsPerBlock;
      si.itemEnd    = std::min((si.y + 1) * itemsPerBlock, NUM_ITEM_NODES);
      si.numitems   = si.itemEnd - si.itemStart;
      if (precomputeOffsets) {
        si.userOffsets = new int[si.numitems];
      } else {
        si.userOffsets = nullptr;
      }
    }

    // Partition item edges in blocks to users according to range [userStart,
    // userEnd)
    if (precomputeOffsets) {
      galois::do_all(galois::iterate(g.begin(), g.begin() + NUM_ITEM_NODES),
                     [&](GNode item) {
                       size_t sliceY = item / itemsPerBlock;
                       BlockInfo* s  = &blocks[sliceY * numXBlocks];

                       size_t pos = item - s->itemStart;
                       auto ii = g.edge_begin(item), ei = g.edge_end(item);
                       size_t offset = 0;
                       for (size_t i = 0; i < numXBlocks; ++i, ++s) {
                         size_t start = userIdToUserNode(s->userStart);
                         size_t end   = userIdToUserNode(s->userEnd);

                         if (ii != ei && g.getEdgeDst(ii) >= start &&
                             g.getEdgeDst(ii) < end) {
                           s->userOffsets[pos] = offset;
                         } else {
                           s->userOffsets[pos] = -1;
                         }
                         for (; ii != ei && g.getEdgeDst(ii) < end;
                              ++ii, ++offset)
                           ;
                       }
                     });
    }
    preProcessTimer.stop();

    // galois::StatTimer executeTimer("Total Execution Time");
    galois::StatTimer executeTimer("Time");
    executeTimer.start();
    executeUntilConverged(sf, g,
                          [&](LatentValue* steps, size_t maxUpdates,
                              galois::GAccumulator<double>* errorAccum) {
                            Process fn{g,      xLocks,     yLocks,
                                       blocks, numXBlocks, numYBlocks,
                                       steps,  maxUpdates, errorAccum};
                            galois::on_each(fn);
                          });
    executeTimer.stop();
  }
};

/*
 * Simple SGD going over all the destination(users) for a given
 * source(Item)
 */
class SGDItemsAlgo {
  static const bool makeSerializable = false;

  struct BasicNode {
    LatentValue latentVector[LATENT_VECTOR_SIZE];
  };

  using Node = BasicNode;

public:
  bool isSgd() const { return true; }

  typedef typename galois::graphs::LC_CSR_Graph<Node, EdgeType>
      //::template with_numa_alloc<true>::type
      ::template with_out_of_line_lockable<true>::type ::
          template with_no_lockable<!makeSerializable>::type Graph;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  std::string name() const { return "sgdItemsAlgo"; }

  size_t numItems() const { return NUM_ITEM_NODES; }

private:
  using GNode         = typename Graph::GraphNode;
  using edge_iterator = typename Graph::edge_iterator;

  struct Execute {
    Graph& g;
    galois::GAccumulator<unsigned>& edgesVisited;

    void operator()(LatentValue* steps, int maxUpdates,
                    galois::GAccumulator<double>* errorAccum) {

      const LatentValue stepSize = steps[0];
      galois::for_each(
          galois::iterate(g.begin(), g.begin() + NUM_ITEM_NODES),
          [&](GNode src, auto& ctx) {
            for (auto ii : g.edges(src)) {

              GNode dst         = g.getEdgeDst(ii);
              LatentValue error = doGradientUpdate(
                  g.getData(src, galois::MethodFlag::UNPROTECTED).latentVector,
                  g.getData(dst).latentVector, lambda, g.getEdgeData(ii),
                  stepSize);

              edgesVisited += 1;
              if (useExactError)
                *errorAccum += error;
            }
          },
          galois::wl<galois::worklists::PerSocketChunkFIFO<64>>(),
          galois::no_pushes(), galois::loopname("sgdItemsAlgo"));
    }
  };

public:
  void operator()(Graph& g, const StepFunction& sf) {
    verify(g, "sgdItemsAlgo");
    galois::GAccumulator<unsigned> edgesVisited;

    // galois::StatTimer executeTimer("Total Execution Time");
    galois::StatTimer executeTimer("Time");
    executeTimer.start();

    Execute fn{g, edgesVisited};
    executeUntilConverged(sf, g, fn);

    executeTimer.stop();

    galois::runtime::reportStat_Single("sgdItemsAlgo", "EdgesVisited",
                                       edgesVisited.reduce());
  }
};

/**
 * Simple by-edge grouped by items (only one edge per item on the WL at any
 * time)
 */
class SGDEdgeItem {
  static const bool makeSerializable = false;

  struct BasicNode {
    // latent vector to be learned.
    LatentValue latentVector[LATENT_VECTOR_SIZE];
    // if a item's update is interrupted, where to start when resuming.
    unsigned int edge_offset;
  };

  using Node = BasicNode;

public:
  bool isSgd() const { return true; }

  typedef typename galois::graphs::LC_CSR_Graph<Node, EdgeType>
      //::template with_numa_alloc<true>::type
      ::template with_out_of_line_lockable<true>::type ::
          template with_no_lockable<!makeSerializable>::type Graph;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  std::string name() const { return "sgdEdgeItem"; }

  size_t numItems() const { return NUM_ITEM_NODES; }

private:
  using GNode         = typename Graph::GraphNode;
  using edge_iterator = typename Graph::edge_iterator;

  struct Execute {
    Graph& g;
    galois::GAccumulator<unsigned>& edgesVisited;
    void operator()(LatentValue* steps, int maxUpdates,
                    galois::GAccumulator<double>* errorAccum) {
      const LatentValue stepSize = steps[0];
      galois::for_each(
          galois::iterate(g.begin(), g.begin() + NUM_ITEM_NODES),
          [&](GNode src, auto& ctx) {
            auto ii = g.edge_begin(src, galois::MethodFlag::UNPROTECTED);
            auto ee = g.edge_end(src, galois::MethodFlag::UNPROTECTED);

            if (ii == ee)
              return;

            // Do not need lock on the source node, since only one thread can
            // work on a given src(item).
            auto& srcData = g.getData(src, galois::MethodFlag::UNPROTECTED);
            // Advance to the edge that has not been worked yet.
            std::advance(ii, srcData.edge_offset);
            // Take lock on the destination as multiple source may update the
            // same destination.
            auto& dstData = g.getData(g.getEdgeDst(ii));
            LatentValue error =
                doGradientUpdate(srcData.latentVector, dstData.latentVector,
                                 lambda, g.getEdgeData(ii), stepSize);

            ++srcData.edge_offset;
            ++ii;

            edgesVisited += 1;
            if (useExactError)
              *errorAccum += error;

            if (ii == ee) {
              // Finished the last edge.
              // Start from the first edge.
              srcData.edge_offset = 0;
              return;
            } else {
              // More edges to work on, therefore push the current src
              // to the worklist.
              ctx.push(src);
            }
          },
          galois::wl<galois::worklists::PerSocketChunkLIFO<8>>(),
          galois::loopname("sgdEdgeItem"));
    }
  };

public:
  void operator()(Graph& g, const StepFunction& sf) {
    verify(g, "sgdEdgeItem");
    galois::GAccumulator<unsigned> edgesVisited;

    // galois::StatTimer executeTimer("Total Execution Time");
    galois::StatTimer executeTimer("Time");
    executeTimer.start();

    Execute fn{g, edgesVisited};
    executeUntilConverged(sf, g, fn);

    executeTimer.stop();

    galois::runtime::reportStat_Single("sgdEdgeItem", "EdgesVisited",
                                       edgesVisited.reduce());
  }
};

/*
 * Simple edge-wise operator
 * Use Fixed2DGraphTiledExecutor to divide Items and Users in to blocks.
 * Locks blocks (blocks may share Items or Users) to work on them.
 *
 */
class SGDBlockEdgeAlgo {
  static const bool makeSerializable = false;

  struct BasicNode {
    LatentValue latentVector[LATENT_VECTOR_SIZE];
  };

  using Node = BasicNode;

public:
  bool isSgd() const { return true; }

  typedef typename galois::graphs::LC_CSR_Graph<Node, EdgeType>
      //::template with_numa_alloc<true>::type
      ::template with_out_of_line_lockable<true>::type ::
          template with_no_lockable<!makeSerializable>::type Graph;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  std::string name() const { return "sgdBlockEdge"; }

  size_t numItems() const { return NUM_ITEM_NODES; }

private:
  using GNode         = typename Graph::GraphNode;
  using edge_iterator = typename Graph::edge_iterator;

  struct Execute {
    Graph& g;
    galois::GAccumulator<unsigned>& edgesVisited;

    void operator()(LatentValue* steps, int maxUpdates,
                    galois::GAccumulator<double>* errorAccum) {
      galois::runtime::Fixed2DGraphTiledExecutor<Graph> executor(g);
      executor.execute(
          g.begin(), g.begin() + NUM_ITEM_NODES, g.begin() + NUM_ITEM_NODES,
          g.end(), itemsPerBlock, usersPerBlock,
          [&](GNode src, GNode dst, edge_iterator edge) {
            const LatentValue stepSize = steps[0];
            LatentValue error          = doGradientUpdate(
                g.getData(src).latentVector, g.getData(dst).latentVector,
                lambda, g.getEdgeData(edge), stepSize);
            edgesVisited += 1;
            if (useExactError)
              *errorAccum += error;
          },
          true // use locks
      );
    }
  };

public:
  void operator()(Graph& g, const StepFunction& sf) {
    verify(g, "sgdBlockEdgeAlgo");
    galois::GAccumulator<unsigned> edgesVisited;

    // galois::StatTimer executeTimer("Total Execution Time");
    galois::StatTimer executeTimer("Time");
    executeTimer.start();

    Execute fn{g, edgesVisited};
    executeUntilConverged(sf, g, fn);

    executeTimer.stop();

    galois::runtime::reportStat_Single("sgdBlockEdgeAlgo", "EdgesVisited",
                                       edgesVisited.reduce());
  }
};

/**
 * ALS algorithms
 */

#ifdef HAS_EIGEN

struct SimpleALSalgo {
  bool isSgd() const { return false; }
  std::string name() const { return "AlternatingLeastSquares"; }
  struct Node {
    LatentValue latentVector[LATENT_VECTOR_SIZE];
  };

  typedef typename galois::graphs::LC_CSR_Graph<Node, EdgeType>::with_no_lockable<
      true>::type Graph;
  typedef Graph::GraphNode GNode;
  // Column-major access
  typedef Eigen::SparseMatrix<LatentValue> Sp;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, Eigen::Dynamic> MT;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, 1> V;
  typedef Eigen::Map<V> MapV;

  Sp A;
  Sp AT;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  void copyToGraph(Graph& g, MT& WT, MT& HT) {
    // Copy out
    for (GNode n : g) {
      LatentValue* ptr = &g.getData(n).latentVector[0];
      MapV mapV{ptr};
      if (n < NUM_ITEM_NODES) {
        mapV = WT.col(n);
      } else {
        mapV = HT.col(n - NUM_ITEM_NODES);
      }
    }
  }

  void copyFromGraph(Graph& g, MT& WT, MT& HT) {
    for (GNode n : g) {
      LatentValue* ptr = &g.getData(n).latentVector[0];
      MapV mapV{ptr};
      if (n < NUM_ITEM_NODES) {
        WT.col(n) = mapV;
      } else {
        HT.col(n - NUM_ITEM_NODES) = mapV;
      }
    }
  }

  void initializeA(Graph& g) {
    typedef Eigen::Triplet<int> Triplet;
    std::vector<Triplet> triplets{g.sizeEdges()};
    auto it = triplets.begin();
    for (auto n : g) {
      for (auto edge : g.out_edges(n)) {
        *it++ = Triplet(n, g.getEdgeDst(edge) - NUM_ITEM_NODES,
                        g.getEdgeData(edge));
      }
    }
    A.resize(NUM_ITEM_NODES, g.size() - NUM_ITEM_NODES);
    A.setFromTriplets(triplets.begin(), triplets.end());
    AT = A.transpose();
  }

  void operator()(Graph& g, const StepFunction&) {
    galois::TimeAccumulator elapsed;
    elapsed.start();

    // Find W, H that minimize ||W H^T - A||_2^2 by solving alternating least
    // squares problems:
    //   (W^T W + lambda I) H^T = W^T A (solving for H^T)
    //   (H^T H + lambda I) W^T = H^T A^T (solving for W^T)
    MT WT{LATENT_VECTOR_SIZE, NUM_ITEM_NODES};
    MT HT{LATENT_VECTOR_SIZE, g.size() - NUM_ITEM_NODES};
    typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, LATENT_VECTOR_SIZE>
        XTX;
    typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, Eigen::Dynamic> XTSp;
    typedef galois::substrate::PerThreadStorage<XTX> PerThrdXTX;

    galois::gPrint("ALS::Start initializeA\n");
    initializeA(g);
    galois::gPrint("ALS::End initializeA\n");
    galois::gPrint("ALS::Start copyFromGraph\n");
    copyFromGraph(g, WT, HT);
    galois::gPrint("ALS::End copyFromGraph\n");

    double last = -1.0;
    galois::StatTimer mmTime("MMTime");
    galois::StatTimer update1Time("UpdateTime1");
    galois::StatTimer update2Time("UpdateTime2");
    galois::StatTimer copyTime("CopyTime");
    galois::StatTimer totalExecTime("totalExecTime");
    galois::StatTimer totalAlgoTime("Time");
    PerThrdXTX xtxs;

    totalAlgoTime.start();
    for (unsigned round = 1;; ++round) {
      totalExecTime.start();
      mmTime.start();
      // TODO parallelize this using tiled executor
      XTSp WTA = WT * A;
      mmTime.stop();

      update1Time.start();
      // TODO: Change to Do_all, pass ints to iterator
      galois::for_each(
          galois::iterate(boost::counting_iterator<int>(0),
                          boost::counting_iterator<int>(A.outerSize())),
          [&](int col, galois::UserContext<int>&) {
            // Compute WTW = W^T * W for sparse A
            XTX& WTW = *xtxs.getLocal();
            WTW.setConstant(0);
            for (Sp::InnerIterator it(A, col); it; ++it)
              WTW.triangularView<Eigen::Upper>() +=
                  WT.col(it.row()) * WT.col(it.row()).transpose();
            for (int i = 0; i < LATENT_VECTOR_SIZE; ++i)
              WTW(i, i) += lambda;
            HT.col(col) =
                WTW.selfadjointView<Eigen::Upper>().llt().solve(WTA.col(col));
          });
      update1Time.stop();

      mmTime.start();
      XTSp HTAT = HT * AT;
      mmTime.stop();

      update2Time.start();
      galois::for_each(
          galois::iterate(boost::counting_iterator<int>(0),
                          boost::counting_iterator<int>(AT.outerSize())),
          [&](int col, galois::UserContext<int>&) {
            // Compute HTH = H^T * H for sparse A
            XTX& HTH = *xtxs.getLocal();
            HTH.setConstant(0);
            for (Sp::InnerIterator it(AT, col); it; ++it)
              HTH.triangularView<Eigen::Upper>() +=
                  HT.col(it.row()) * HT.col(it.row()).transpose();
            for (int i = 0; i < LATENT_VECTOR_SIZE; ++i)
              HTH(i, i) += lambda;
            WT.col(col) =
                HTH.selfadjointView<Eigen::Upper>().llt().solve(HTAT.col(col));
          });
      update2Time.stop();

      copyTime.start();
      copyToGraph(g, WT, HT);
      copyTime.stop();
      totalExecTime.stop();

      double error = sumSquaredError(g);
      elapsed.stop();
      std::cout << "R: " << round << " elapsed (ms): " << elapsed.get()
                << " RMSE (R " << round
                << "): " << std::sqrt(error / g.sizeEdges()) << "\n";
      elapsed.start();

      if (fixedRounds <= 0 && round > 1 &&
          std::abs((last - error) / last) < tolerance)
        break;
      if (fixedRounds > 0 && round >= fixedRounds)
        break;

      last = error;
    }
    totalAlgoTime.stop();
  }
};

struct SyncALSalgo {

  bool isSgd() const { return false; }

  std::string name() const { return "SynchronousAlternatingLeastSquares"; }

  struct Node {
    LatentValue latentVector[LATENT_VECTOR_SIZE];
  };

  static const bool NEEDS_LOCKS = false;
  typedef typename galois::graphs::LC_CSR_Graph<Node, EdgeType> BaseGraph;
  typedef typename std::conditional<
      NEEDS_LOCKS,
      typename BaseGraph::template with_out_of_line_lockable<true>::type,
      typename BaseGraph::template with_no_lockable<true>::type>::type Graph;
  typedef typename Graph::GraphNode GNode;
  // Column-major access
  typedef Eigen::SparseMatrix<LatentValue> Sp;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, Eigen::Dynamic> MT;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, 1> V;
  typedef Eigen::Map<V> MapV;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, LATENT_VECTOR_SIZE>
      XTX;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, Eigen::Dynamic> XTSp;

  typedef galois::substrate::PerThreadStorage<XTX> PerThrdXTX;
  typedef galois::substrate::PerThreadStorage<V> PerThrdV;

  Sp A;
  Sp AT;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  void copyToGraph(Graph& g, MT& WT, MT& HT) {
    // Copy out
    for (GNode n : g) {
      LatentValue* ptr = &g.getData(n).latentVector[0];
      MapV mapV{ptr};
      if (n < NUM_ITEM_NODES) {
        mapV = WT.col(n);
      } else {
        mapV = HT.col(n - NUM_ITEM_NODES);
      }
    }
  }

  void copyFromGraph(Graph& g, MT& WT, MT& HT) {
    for (GNode n : g) {
      LatentValue* ptr = &g.getData(n).latentVector[0];
      MapV mapV{ptr};
      if (n < NUM_ITEM_NODES) {
        WT.col(n) = mapV;
      } else {
        HT.col(n - NUM_ITEM_NODES) = mapV;
      }
    }
  }

  void initializeA(Graph& g) {
    typedef Eigen::Triplet<int> Triplet;
    std::vector<Triplet> triplets{g.sizeEdges()};
    auto it = triplets.begin();
    for (auto n : g) {
      for (auto edge : g.out_edges(n)) {
        *it++ = Triplet(n, g.getEdgeDst(edge) - NUM_ITEM_NODES,
                        g.getEdgeData(edge));
      }
    }
    A.resize(NUM_ITEM_NODES, g.size() - NUM_ITEM_NODES);
    A.setFromTriplets(triplets.begin(), triplets.end());
    AT = A.transpose();
  }

  void update(Graph& g, size_t col, MT& WT, MT& HT, PerThrdXTX& xtxs,
              PerThrdV& rhs) {
    // Compute WTW = W^T * W for sparse A
    V& r = *rhs.getLocal();
    if (col < NUM_ITEM_NODES) {
      r.setConstant(0);
      // HTAT = HT * AT; r = HTAT.col(col)
      for (Sp::InnerIterator it(AT, col); it; ++it)
        r += it.value() * HT.col(it.row());
      XTX& HTH = *xtxs.getLocal();
      HTH.setConstant(0);
      for (Sp::InnerIterator it(AT, col); it; ++it)
        HTH.triangularView<Eigen::Upper>() +=
            HT.col(it.row()) * HT.col(it.row()).transpose();
      for (int i = 0; i < LATENT_VECTOR_SIZE; ++i)
        HTH(i, i) += lambda;
      WT.col(col) = HTH.selfadjointView<Eigen::Upper>().llt().solve(r);
    } else {
      col = col - NUM_ITEM_NODES;
      r.setConstant(0);
      // WTA = WT * A; x = WTA.col(col)
      for (Sp::InnerIterator it(A, col); it; ++it)
        r += it.value() * WT.col(it.row());
      XTX& WTW = *xtxs.getLocal();
      WTW.setConstant(0);
      for (Sp::InnerIterator it(A, col); it; ++it)
        WTW.triangularView<Eigen::Upper>() +=
            WT.col(it.row()) * WT.col(it.row()).transpose();
      for (int i = 0; i < LATENT_VECTOR_SIZE; ++i)
        WTW(i, i) += lambda;
      HT.col(col) = WTW.selfadjointView<Eigen::Upper>().llt().solve(r);
    }
  }

  struct NonDetTraits {
    typedef std::tuple<> base_function_traits;
  };

  struct Process {
    struct LocalState {
      LocalState(Process&, galois::PerIterAllocTy&) {}
    };

    struct DeterministicId {
      uintptr_t operator()(size_t x) const { return x; }
    };

    typedef std::tuple<galois::per_iter_alloc, galois::intent_to_read,
                       galois::local_state<LocalState>,
                       galois::det_id<DeterministicId>>
        ikdg_function_traits;
    typedef std::tuple<galois::per_iter_alloc, galois::fixed_neighborhood,
                       galois::local_state<LocalState>,
                       galois::det_id<DeterministicId>>
        add_remove_function_traits;
    typedef std::tuple<> nondet_function_traits;

    SyncALSalgo& self;
    Graph& g;
    MT& WT;
    MT& HT;
    PerThrdXTX& xtxs;
    PerThrdV& rhs;

    Process(SyncALSalgo& self, Graph& g, MT& WT, MT& HT, PerThrdXTX& xtxs,
            PerThrdV& rhs)
        : self(self), g(g), WT(WT), HT(HT), xtxs(xtxs), rhs(rhs) {}

    void operator()(size_t col, galois::UserContext<size_t>& ctx) {
      self.update(g, col, WT, HT, xtxs, rhs);
    }
  };

  void operator()(Graph& g, const StepFunction&) {
    if (!useSameLatentVector) {
      galois::gWarn("Results are not deterministic with different numbers of "
                    "threads unless -useSameLatentVector is true");
    }
    galois::TimeAccumulator elapsed;
    elapsed.start();

    // Find W, H that minimize ||W H^T - A||_2^2 by solving alternating least
    // squares problems:
    //   (W^T W + lambda I) H^T = W^T A (solving for H^T)
    //   (H^T H + lambda I) W^T = H^T A^T (solving for W^T)
    MT WT{LATENT_VECTOR_SIZE, NUM_ITEM_NODES};
    MT HT{LATENT_VECTOR_SIZE, g.size() - NUM_ITEM_NODES};

    initializeA(g);
    copyFromGraph(g, WT, HT);

    double last = -1.0;
    galois::StatTimer updateTime("UpdateTime");
    galois::StatTimer copyTime("CopyTime");
    galois::StatTimer totalExecTime("totalExecTime");
    galois::StatTimer totalAlgoTime("Time");
    PerThrdXTX xtxs;
    PerThrdV rhs;

    totalAlgoTime.start();
    for (unsigned round = 1;; ++round) {

      totalExecTime.start();
      updateTime.start();

      typedef galois::worklists::PerThreadChunkLIFO<ALS_CHUNK_SIZE> WL_ty;
      galois::for_each(
          galois::iterate(boost::counting_iterator<size_t>(0),
                          boost::counting_iterator<size_t>(NUM_ITEM_NODES)),
          Process(*this, g, WT, HT, xtxs, rhs), galois::wl<WL_ty>(),
          galois::loopname("syncALS-users"));
      galois::for_each(
          galois::iterate(boost::counting_iterator<size_t>(NUM_ITEM_NODES),
                          boost::counting_iterator<size_t>(g.size())),
          Process(*this, g, WT, HT, xtxs, rhs), galois::wl<WL_ty>(),
          galois::loopname("syncALS-items"));

      updateTime.stop();

      copyTime.start();
      copyToGraph(g, WT, HT);
      copyTime.stop();
      totalExecTime.stop();

      double error = sumSquaredError(g);
      elapsed.stop();
      std::cout << "R: " << round << " elapsed (ms): " << elapsed.get()
                << " RMSE (R " << round
                << "): " << std::sqrt(error / g.sizeEdges()) << "\n";
      elapsed.start();

      if (fixedRounds <= 0 && round > 1 &&
          std::abs((last - error) / last) < tolerance)
        break;
      if (fixedRounds > 0 && round >= fixedRounds)
        break;

      last = error;
    } // end for
    totalAlgoTime.stop();
  }
};

#endif // HAS_EIGEN

/**
 * Initializes latent vector with random values and returns basic graph
 * parameters.
 *
 * @tparam Graph type of g
 * @param g Graph to initialize
 * @returns number of item nodes, i.e. nodes with outgoing edges. They should
 * be the first nodes of the graph in memory
 */

template <typename Graph>
size_t initializeGraphData(Graph& g) {
  galois::gPrint("initializeGraphData\n");
  galois::StatTimer initTimer("InitializeGraph");
  initTimer.start();
  double top = 1.0 / std::sqrt(LATENT_VECTOR_SIZE);
  galois::substrate::PerThreadStorage<std::mt19937> gen;

#if __cplusplus >= 201103L || defined(HAVE_CXX11_UNIFORM_INT_DISTRIBUTION)
  std::uniform_real_distribution<LatentValue> dist(0, top);
#else
  std::uniform_real<LatentValue> dist(0, top);
#endif

  if (useDetInit) {
    galois::do_all(galois::iterate(g), [&](typename Graph::GraphNode n) {
      auto& data = g.getData(n);
      auto val   = genVal(n);
      for (int i = 0; i < LATENT_VECTOR_SIZE; i++) {
        data.latentVector[i] = val;
      }
    });
  } else {
    galois::do_all(galois::iterate(g), [&](typename Graph::GraphNode n) {
      auto& data = g.getData(n);

      // all threads initialize their assignment with same generator or
      // a thread local one
      if (useSameLatentVector) {
        std::mt19937 sameGen;
        for (int i = 0; i < LATENT_VECTOR_SIZE; i++) {
          data.latentVector[i] = dist(sameGen);
        }
      } else {
        for (int i = 0; i < LATENT_VECTOR_SIZE; i++) {
          data.latentVector[i] = dist(*gen.getLocal());
        }
      }
    });
  }

   auto activeThreads = galois::getActiveThreads();
   std::vector<uint32_t> largestNodeID_perThread(activeThreads);

    galois::on_each([&](unsigned tid, unsigned nthreads) {
      unsigned int block_size = g.size() / nthreads;
      if ((g.size() % nthreads) > 0)
        ++block_size;

      uint32_t start = tid * block_size;
      uint32_t end   = (tid + 1) * block_size;
      if (end > g.size())
        end = g.size();

      largestNodeID_perThread[tid] = 0;
      for (uint32_t i = start; i < end; ++i) {
        if(std::distance(g.edge_begin(i), g.edge_end(i))) {
          if(largestNodeID_perThread[tid] < i)
            largestNodeID_perThread[tid] = i;
          }
      }
    });

    uint32_t largestNodeID = 0;
    for(uint32_t t = 0; t < activeThreads; ++t){
      if(largestNodeID < largestNodeID_perThread[t])
        largestNodeID = largestNodeID_perThread[t];
    }
    size_t numItemNodes = largestNodeID + 1;

  initTimer.stop();
  return numItemNodes;
}

StepFunction* newStepFunction() {
  switch (learningRateFunction) {
  case Step::intel:
    return new IntelStepFunction;
  case Step::purdue:
    return new PurdueStepFunction;
  case Step::bottou:
    return new BottouStepFunction;
  case Step::inverse:
    return new InverseStepFunction;
  case Step::bold:
    return new BoldStepFunction;
  default:
    GALOIS_DIE("unknown step function");
  }
}

template <typename Graph>
void writeBinaryLatentVectors(Graph& g, const std::string& filename) {
  std::ofstream file(filename);
  for (auto ii = g.begin(), ei = g.end(); ii != ei; ++ii) {
    auto& v = g.getData(*ii).latentVector;
    for (int i = 0; i < LATENT_VECTOR_SIZE; ++i) {
      file.write(reinterpret_cast<char*>(&v[i]), sizeof(v[i]));
    }
  }
  file.close();
}

template <typename Graph>
void writeAsciiLatentVectors(Graph& g, const std::string& filename) {
  std::ofstream file(filename);
  for (auto ii = g.begin(), ei = g.end(); ii != ei; ++ii) {
    auto& v = g.getData(*ii).latentVector;
    for (int i = 0; i < LATENT_VECTOR_SIZE; ++i) {
      file << v[i] << " ";
    }
    file << "\n";
  }
  file.close();
}

/**
 * Run the provided algorithm (provided through the template argument).
 *
 * @param Algo algorithm to run
 */
template <typename Algo>
void run() {
  typename Algo::Graph g;
  Algo algo;

  galois::runtime::reportNumaAlloc("NumaAlloc0");

  // Bipartite graph in general graph data structure should be following:
  // * items are the first m nodes
  // * users are the next n nodes
  // * only items have outedges
  algo.readGraph(g);

  galois::runtime::reportNumaAlloc("NumaAlloc1");

  // initialize latent vectors and get number of item nodes
  NUM_ITEM_NODES = initializeGraphData(g);

  galois::runtime::reportNumaAlloc("NumaAlloc2");

  std::cout << "num users: " << g.size() - NUM_ITEM_NODES
            << " num items: " << NUM_ITEM_NODES
            << " num ratings: " << g.sizeEdges() << "\n";

  std::unique_ptr<StepFunction> sf{newStepFunction()};
  std::cout << "latent vector size: " << LATENT_VECTOR_SIZE
            << " algo: " << algo.name() << " lambda: " << lambda;

  if (algo.isSgd()) {
    std::cout << " learning rate: " << learningRate
              << " decay rate: " << decayRate
              << " step function: " << sf->name();
  }

  std::cout << "\n";

  if (!skipVerify) {
    verify(g, "Initial");
  }

  // algorithm call
  galois::StatTimer totalTimer("Total Time");
  totalTimer.start();
  algo(g, *sf);
  totalTimer.stop();

  if (!skipVerify) {
    verify(g, "Final");
  }

  if (outputFilename != "") {
    std::cout << "Writing latent vectors to " << outputFilename << "\n";
    switch (outputType) {
    case OutputType::binary:
      writeBinaryLatentVectors(g, outputFilename);
      break;
    case OutputType::ascii:
      writeAsciiLatentVectors(g, outputFilename);
      break;
    default:
      GALOIS_DIE("Invalid output type for latent vector output");
    }
  }

  galois::runtime::reportNumaAlloc("NumaAlloc");
}

int main(int argc, char** argv) {
  galois::SharedMemSys G;
  LonestarStart(argc, argv, name, desc, url);

  switch (algo) {
#ifdef HAS_EIGEN
  case Algo::syncALS:
    run<SyncALSalgo>();
    break;
  case Algo::simpleALS:
    run<SimpleALSalgo>();
    break;
#endif
  case Algo::sgdByItems:
    run<SGDItemsAlgo>();
    break;
  case Algo::sgdByEdges:
    run<SGDEdgeItem>();
    break;
  case Algo::sgdBlockEdge:
    run<SGDBlockEdgeAlgo>();
    break;
  case Algo::sgdBlockJump:
    run<SGDBlockJumpAlgo>();
    break;
  default:
    GALOIS_DIE("unknown algorithm");
    break;
  }

  return 0;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-hyperbolic
    Function object implementing sinhc capabilities

    Returns hyperbolic cardinal sine: \f$\frac{\sinh(x)}{x}\f$.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = sinhc(x);
    @endcode

    is similar to:

    @code
    T r = sinh(x)/x;
    @endcode

    @see sinh

  **/
  const boost::dispatch::functor<tag::sinhc_> sinhc = {};
} }
#endif

#include <boost/simd/function/scalar/sinhc.hpp>
#include <boost/simd/function/simd/sinhc.hpp>

#endif


#include <Eigen/Sparse>

typedef Eigen::SparseMatrix<double, Eigen::RowMajor> csr_matrix;

extern "C" {

    void eigen_sparse_matrix_assign(csr_matrix* lvalue, const csr_matrix* rvalue) {
        *lvalue = *rvalue;
        lvalue->makeCompressed();
    }

}


// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm

#include "boundedValue.hpp"

#include <Eigen/Dense>
#include <iostream>

#include <limits>

namespace VideoStitch {
namespace Calibration {

BoundedValue::BoundedValue() : modifier(new std::array<double, 4>) {
  /*By default, bound is [-1; 1]*/
  shift = 0;
  scale = 1;
  Eigen::Map<Eigen::Matrix<double, 2, 2, Eigen::RowMajor> > R(modifier->data());
  R.setIdentity();
}

BoundedValue::~BoundedValue() {}

bool BoundedValue::isConstant() const { return (scale <= std::numeric_limits<double>::epsilon()); }

void BoundedValue::setBounds(double min, double max) {
  shift = 0.5 * (min + max);
  scale = 0.5 * (max - min);
  Eigen::Map<Eigen::Matrix<double, 2, 2, Eigen::RowMajor> > R(modifier->data());
  R.setIdentity();
}

double *BoundedValue::getMinimizerPtr() const { return modifier->data(); }

void BoundedValue::setMinimizerValues(const double *ptr) {
  (*modifier)[0] = ptr[0];
  (*modifier)[1] = ptr[1];
  (*modifier)[2] = ptr[2];
  (*modifier)[3] = ptr[3];
}

double BoundedValue::getValue() const {
  Eigen::Map<Eigen::Matrix<double, 2, 2, Eigen::RowMajor> > R(modifier->data());

  Eigen::Vector2d vec;
  vec(0) = 0;
  vec(1) = 1;

  Eigen::Vector2d updated_vec = (Eigen::Vector2d)(R * vec);

  double val = scale * updated_vec(0) + shift;
  return val;
}

void BoundedValue::getJacobian(Eigen::Matrix<double, 1, 4> &J) const {
  Eigen::Map<Eigen::Matrix<double, 2, 2, Eigen::RowMajor> > R(modifier->data());

  Eigen::Vector2d vec;
  vec(0) = 0;
  vec(1) = 1;
  Eigen::Vector2d updated_vec = (Eigen::Vector2d)(R * vec);

  /* scale [1 0; 0 0] * update * R * vec */
  /* scale * R(0, 0) * updated_vec(0) +  scale * R(0, 1) * updated_vec(1) */

  J(0, 0) = scale * updated_vec(0);
  J(0, 1) = 0;
  J(0, 2) = scale * updated_vec(1);
  J(0, 3) = 0;
}

void BoundedValue::setValue(double val) {
  Eigen::Map<Eigen::Matrix<double, 2, 2, Eigen::RowMajor> > R(modifier->data());

  /*Scale == 0 means we have no uncertainty, shift IS the value*/
  if (fabs(scale) < 1e-12) {
    R.setIdentity();
    return;
  }

  double b = (val - shift) / scale;
  double angle = -asin(b);

  assert(std::abs(b) <= 1.);
  R(0, 0) = cos(angle);
  R(0, 1) = -sin(angle);
  R(1, 0) = sin(angle);
  R(1, 1) = cos(angle);
}

void BoundedValue::tieTo(const BoundedValue &other) {
  shift = other.shift;
  scale = other.scale;
  modifier = other.modifier;
}

void BoundedValue::untie() {
  // create a new modifier object, with the content of the old one
  std::shared_ptr<std::array<double, 4> > tmp_copy = modifier;
  modifier.reset(new std::array<double, 4>);
  *modifier = *tmp_copy;
}

}  // namespace Calibration
}  // namespace VideoStitch


#include <string.h>
#include <math.h>

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

extern "C" {
void dsyevr_ (
	char	*jobz,
	char	*range,
	char	*uplo,
	int	*n,
	double	*a,
	int	*lda,
	double	*vl,
	double	*vu,
	int	*il,
	int	*iu,
	double	*abstol,
	int	*m,
	double	*w,
	double	*z,
	int	*ldz,
	int	*isuppz,
	double	*work,
	int	*lwork,
	int	*iwork,
	int	*liwork,
	int	*info
);
}

using namespace std;

template <class F> class Point;

template <class F> class PointBase {
public:
	vector<F>	x;

	template <class G> PointBase<F> & operator =(Point<G> const &);
//	template <class G> PointBase<double> & operator =(PointBase<G> const &);
	template <class G> PointBase<F> & operator +=(PointBase<G> const &);

	PointBase<F>(void) {};

	PointBase<F>(int s) {
		x.resize(s);
		for (int i=0; i<s; i++) x[i] = 0.;
	}

	template <class G> PointBase<F>(PointBase<G> const & g) {
		for (int i=0; i<g.x.size(); i++) x.push_back(g.x[i]);
	}

	PointBase<F> & operator *= (double d) {
		for (int i=0; i<x.size(); i++) x[i] *= d;
	}

	F dotp(vector<F> const &v) const {
		F	ret = 0;

		for (int i=0; i<x.size(); i++) ret += v[i]*x[i];

		return	ret;
	}
};

template <class F> template <class G> PointBase<F> & PointBase<F>::operator=(Point<G> const &f)
{
	x = f.x;

	return *this;
}

template <class F> template <class G> PointBase<F> & PointBase<F>::operator +=(PointBase<G>  const & r)
{
	for (int i=0; i<x.size(); i++) x[i] += r.x[i];
	return *this;
}

template <class F> class Point: public PointBase<F> {
public:
	F	chi2;
	vector<string>	types;

	bool	load(istream &);
};

template <class F> bool Point<F>::load(istream & in)
{
	char	line[BUFSIZ];
	this->x.clear();
	string	s;

#define CLUE	"    Function number"

	while (! in.getline(line,sizeof line).eof() && strlen(line) > 0 && strncmp(line,CLUE,sizeof(CLUE)-1)) ;

	stringstream	parse(line);

//    Function number   284    F =  1.0973091125D+00
	parse >> s >> s >> s >> s >> s >> chi2;

	while (! in.getline(line,sizeof line).eof() && strlen(line) > 0) {

		F	xx[5] = { FP_NAN, FP_NAN, FP_NAN, FP_NAN, FP_NAN };

		for (int j=0; line[j]; j++) if (line[j] == 'D') line[j] = 'e';
		int n = sscanf(line,"%f %f %f %f %f",xx,xx+1,xx+2,xx+3,xx+4);
		for (int j=0; j<n; j++) this->x.push_back(xx[j]);
	}

	return !in.eof();
}

class Select {
public:
	float	chi2;

	Select() {
		chi2	= 1e38;
	}

	bool match(Point<float> const & p) const {
		if (p.chi2  > chi2) return false;

		return true;
	}
};

#include <boost/program_options.hpp>
namespace po = boost::program_options;

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

	po::options_description desc("Options");
	desc.add_options()
		("help", "help message")
		("chi2", po::value<float>()->default_value(100000.0), "select points for dimension reduction - smaller chi2 only")
		("dim", po::value<int>()->default_value(2), "reduce to this number of dimensions")
		("all", "map all points, not only those selected by chi2")
;

	po::variables_map opt;
	try { po::store(po::parse_command_line(argc, argv, desc), opt); }
	catch (exception &e) {
		cerr << argv[0] << ": " << e.what() << endl;
		return 1;
	}
	po::notify(opt);

	if (opt.count("help")) { cerr << desc; return 1; }

	Select	sel;
	sel.chi2 = opt["chi2"].as<float>();

	int	dimreduce = opt["dim"].as<int>();
	bool allpoints = opt.count("all");

	vector<Point<float> >	pt;
	Point<float>		p;

	while (p.load(cin)) {
		pt.push_back(p);
		if (pt.size() % 100 == 0) cerr << pt.size() << " points read      \r" << flush;
	}
	cerr << pt.size() << " points read      " << endl;

	PointBase<double> sum(pt[0].x.size());
	vector<PointBase<float> > matched;
	long nmatch = 0;

	for (int i=0; i<pt.size(); i++)
		if (sel.match(pt[i])) {
			PointBase<float>	pb = pt[i];
			matched.push_back(pb);
			sum += pt[i];
			nmatch++;
		}

	cerr << nmatch << " points match criteria" << endl;

	PointBase<float> minusavg(sum);
	double invnmatch = 1. / nmatch;
        minusavg *= - invnmatch;

	for (int i=0; i<matched.size(); i++) matched[i] += minusavg;

	int	npar = pt[0].x.size(), npar2 = npar*npar;
	double *cov = new double[npar2]; // lower triangular
	vector<double>	par(npar);

	for (int k=0; k<npar2; k++) cov[k] = 0.;

	for (int n=0; n < nmatch; n++) {
		for (int i=0; i<npar; i++) {
			par[i] = matched[n].x[i];
		}

		for (int i=0; i<npar; i++) {
			int	icol = i*npar;
			for (int j=i; j<npar; j++) cov[icol + j] += par[i] * par[j];
		}
	}

	for (int k=0; k<npar2; k++) cov[k] *= invnmatch;

	double zero = 0., abstol = 0. ;
	int	izero = 0,lwork = npar * 100;
	int	m, *isuppz = new int[2*npar], info, liwork = 10*npar, *iwork = new int[liwork];
	double	*w = new double[npar], *z = new double[npar2], *work = new double[lwork];

	dsyevr_("V", // JOBZ
		"A", // RANGE
		"L", // UPLO
		&npar, // N
		cov, // A
		&npar, // LDA
		&zero, // VL
		&zero, // VU
		&izero, // IL
		&izero, // IU
		&abstol,
		&m,
		w,
		z,
		&npar, // LDZ
		isuppz,
		work,
		&lwork,
		iwork,
		&liwork,
		&info);

	cerr << "dsyevr() = " << info << endl;
	if (info) return 1;

	vector<float>	evnorm(npar);
	float	evsum = 0, evcum = 0;

	cerr << "raw eigenvalues: " << endl;
	for (int i=0; i<npar; i++) { cerr << w[i] << " "; evsum += w[i]; }
	cerr << endl;

	cerr << "normalized reverse cummulative: " << endl;
	for (int i=0; i<npar; i++) {
		evnorm[i] = w[npar-i-1] / evsum;
		evcum += evnorm[i];
		cerr << evcum << " ";
	}
	cerr << endl;

	vector<vector<float> > evec(dimreduce);

	for (int i=0; i<dimreduce; i++) {
		evec[i].resize(npar);
		int	icol = (npar-i-1) * npar;
		float	dotp = 0, norm = 0.;
		for (int j=0; j<npar; j++) {
			float	e = evec[i][j] = z[icol+j];
			norm += e*e;
		}
		norm = 1./sqrt(norm)/sqrt(1.*npar);;

		cerr << "eigenvector[" << i << "]:" ;
		for (int j=0; j<npar; j++) {
			cerr << evec[i][j] * norm << " ";
			dotp += evec[i][j] * norm;
		}
		cerr << endl << "dotprod with diagonal: " << dotp << endl;
	}

	for (int i=0; i<pt.size(); i++)
		if (allpoints || sel.match(pt[i])) {
			for (int j=0; j<dimreduce; j++) {
				cout << pt[i].dotp(evec[j]) << ", ";
			}
			cout << pt[i].chi2;
			cout << endl;
		}

	delete [] isuppz;
	delete [] iwork;
	delete [] w;
	delete [] z;
	delete [] work;

	return 0;
}