#define _USE_MATH_DEFINES

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <limits>

#include <time.h>
#include <cmath>
#include <iostream>
#include <numeric>
#include "radialpose.h"
#include "misc/unit_test_misc.h"
#include "misc/refinement.h"

template<typename Solver, typename AddDistortionFunc>
void benchmark_solver(std::string name, PoseEstimator<Solver>* estimator, AddDistortionFunc addDistortion, std::vector<double> params, int iters) {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<Camera> poses;
	std::vector<double> residuals;
	residuals.reserve(iters);
	std::vector<int> num_of_poses;
	num_of_poses.reserve(iters);

	srand(12345);

	for (int iter = 0; iter < iters; ++iter) {

		generate_scene_and_image(estimator->minimal_sample_size(), 2, 100, 90, false, &pose_gt, &x, &X, 100.0);
		addDistortion(params, &pose_gt, &x);
		add_focal(2000.0, &pose_gt, &x);

		int n_sols = estimator->estimate(x, X, &poses);
		double err = minimum_pose_distance(pose_gt, poses, true, false);

		residuals.push_back(std::log10(err));
		num_of_poses.push_back(poses.size());
	}

	std::sort(residuals.begin(), residuals.end());

	double average = accumulate(residuals.begin(), residuals.end(), 0.0) / residuals.size();

	double median = residuals[residuals.size() / 2];
	double q25 = residuals[residuals.size() / 4];
	double q75 = residuals[(3 * residuals.size()) / 4];
	double q95 = residuals[(95 * residuals.size()) / 100];
	double average_num_pose = accumulate(num_of_poses.begin(), num_of_poses.end(), 0.0) / num_of_poses.size();

	std::cout << "BENCH " << name << ": average=" << average << ", median=" << median << " (q25=" << q25 << ", q75=" << q75 << ", q95=" << q95 << "), avg_num_poses=" << average_num_pose << "\n";
}

/////////////////////////// TESTS START HERE /////////////////////////

bool test_rational_distortion() {

	Matrix<double, 2, Dynamic> x0(2, 100);
	Matrix<double, 2, Dynamic> x1(2, 100);

	int nps[] = { 1, 2, 3, 0, 0, 0, 1, 3 };
	int nds[] = { 0, 0, 0, 1, 2, 0, 1, 3 };

	double lambdas[] = { -0.1, -0.01, -0.001 };
	double mus[] = { -0.1, -0.01, -0.001 };

	x0.setRandom();
	x0 *= 0.5;

	std::vector<double> params;

	int fail = 0;
	for (int i = 0; i < 8; ++i) {

		int np = nps[i];
		int nd = nds[i];
		for (int j = 0; j < np; ++j) {
			params.push_back(mus[j]);
		}
		for (int j = 0; j < nd; ++j) {
			params.push_back(lambdas[j]);
		}

		inverse_rational_model(params, np, nd, x0, &x1);
		forward_rational_model(params, np, nd, x1, &x1);
		double res = (x0 - x1).norm();
		if (res > TOL_POSE) {
			fail++;
		}

		forward_rational_model(params, np, nd, x0, &x1);
		inverse_rational_model(params, np, nd, x1, &x1);
		res = (x0 - x1).norm();
		if (res > TOL_POSE) {
			fail++;
		}
	}

	return fail == 0;

}

bool test_1param_div_model() {

	Matrix<double, 2, Dynamic> x0(2, 100);
	Matrix<double, 2, Dynamic> x1(2, 100);

	double lambdas[] = { 0.0, -0.01, -0.1, -0.2 };

	x0.setRandom();
	x0 *= 0.5;

	int fail = 0;
	for (int i = 0; i < 4; ++i) {
		inverse_1param_division_model(lambdas[i], x0, &x1);
		forward_1param_division_model(lambdas[i], x1, &x1);

		double res = (x0 - x1).norm();
		if (res > TOL_POSE) {
			fail++;
		}

		forward_1param_division_model(lambdas[i], x0, &x1);
		inverse_1param_division_model(lambdas[i], x1, &x1);
		res = (x0 - x1).norm();
		if (res > TOL_POSE) {
			fail++;
		}
	}

	return fail == 0;
}

bool test_planar_oskarsson() {

	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	generate_scene_and_image(4, 2, 20, 70, true, &pose_gt, &x, &X);
	add_distortion_1pdiv(-0.2, &pose_gt, &x);
	add_focal(2000.0, &pose_gt, &x);

	std::vector<Camera> poses;

	// Transform such that make X(3,:) = 0
	Vector3d t0 = X.rowwise().mean();
	X.colwise() -= t0;

	JacobiSVD<Matrix<double, 3, Dynamic>> svd(X, ComputeThinU);
	Matrix<double, 3, 3> R0 = svd.matrixU();
	if (R0.determinant() < 0)
		R0 *= -1.0;
	X = R0.transpose() * X;
	pose_gt.t = pose_gt.t + pose_gt.R * t0;
	pose_gt.R = pose_gt.R * R0;

	oskarsson_arxiv18::PlanarSolver estimator;
	int n_sols = estimator.estimate(x, X, &poses);
	double err = minimum_pose_distance(pose_gt, poses);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);
}

bool test_kukelova_1d_radial() {

	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	generate_scene_and_image(5, 2, 20, 70, false, &pose_gt, &x, &X);
	add_focal(2000.0, &pose_gt, &x);

	kukelova_iccv13::Radial1DSolver estimator;
	std::vector<Camera> poses;
	int n_sols = estimator.estimate(x, X, &poses);

	// Fix sign and t_Z = 0 for all poses since we are invariant to these.
	for (int i = 0; i < poses.size(); ++i) {
		if (poses[i].R(0) * pose_gt.R(0) < 0) {
			poses[i].R *= -1.0;
			poses[i].t *= -1.0;
		}
		if (poses[i].R(2) * pose_gt.R(2) < 0) {
			poses[i].R.row(2) *= -1.0;
			poses[i].t.row(2) *= -1.0;
		}

		poses[i].t(2) = 0.0;
	}
	pose_gt.t(2) = 0.0;

	double err = minimum_pose_distance(pose_gt, poses, false, false);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);

}

bool test_kukelova_solver_1p_div() {

	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	generate_scene_and_image(5, 2, 20, 70, false, &pose_gt, &x, &X);
	add_distortion_1pdiv(-0.2, &pose_gt, &x);
	add_focal(5.0, &pose_gt, &x);

	std::vector<Camera> poses;

	kukelova_iccv13::Solver estimator(1);

	int n_sols = estimator.estimate(x, X, &poses);

	double err = minimum_pose_distance(pose_gt, poses);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);

}

bool test_kukelova_solver_3p_div() {

	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	generate_scene_and_image(5, 2, 20, 70, false, &pose_gt, &x, &X);

	std::vector<double> params = { -0.12, -0.034, -0.0056 };
	add_rational_undistortion(params, 0, 3, &pose_gt, &x);
	add_focal(5.0, &pose_gt, &x);

	std::vector<Camera> poses;

	kukelova_iccv13::Solver estimator(3);

	int n_sols = estimator.estimate(x, X, &poses);

	double err = minimum_pose_distance(pose_gt, poses, true, true);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);

}

bool test_larsson17_solver_1p_div() {

	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	generate_scene_and_image(4, 2, 20, 70, false, &pose_gt, &x, &X);
	add_distortion_1pdiv(-0.2, &pose_gt, &x);
	add_focal(5.0, &pose_gt, &x);
	larsson_iccv17::NonPlanarSolver estimator;

	std::vector<Camera> poses;

	int n_sols = estimator.estimate(x, X, &poses);

	double err = minimum_pose_distance(pose_gt, poses);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);

}

bool test_bujnak10_solver_1p_div() {

	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	generate_scene_and_image(4, 2, 20, 70, false, &pose_gt, &x, &X);
	add_distortion_1pdiv(-0.2, &pose_gt, &x);
	add_focal(5.0, &pose_gt, &x);
	bujnak_accv10::NonPlanarSolver estimator;

	std::vector<Camera> poses;
	int n_sols = estimator.estimate(x, X, &poses);

	double err = minimum_pose_distance(pose_gt, poses);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);

}

bool test_larsson19_distortion_1_0() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params = { -0.12 };
	larsson_iccv19::Solver<1, 0, true> estimator;

	generate_scene_and_image(estimator.minimal_sample_size(), 2, 20, 70, false, &pose_gt, &x, &X);
	add_rational_distortion(params, 1, 0, &pose_gt, &x);
	add_focal(2.0, &pose_gt, &x);

	std::vector<Camera> poses;
	int n_sols = estimator.estimate(x, X, &poses);

	double err = minimum_pose_distance(pose_gt, poses, true, true);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);
}

bool test_larsson19_distortion_1_0_planar() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params = { -0.12 };
	larsson_iccv19::Solver<1, 0, true> estimator;

	generate_scene_and_image(estimator.minimal_sample_size(), 2, 20, 70, true, &pose_gt, &x, &X);
	add_rational_distortion(params, 1, 0, &pose_gt, &x);
	add_focal(2.0, &pose_gt, &x);

	// Transform such that make X(3,:) = 0
	Vector3d t0 = X.rowwise().mean();
	X.colwise() -= t0;

	JacobiSVD<Matrix<double, 3, Dynamic>> svd(X, ComputeThinU);
	Matrix<double, 3, 3> R0 = svd.matrixU();
	if (R0.determinant() < 0)
		R0 *= -1.0;
	X = R0.transpose() * X;
	pose_gt.t = pose_gt.t + pose_gt.R * t0;
	pose_gt.R = pose_gt.R * R0;
	X.row(2).array() = 0.0;

	std::vector<Camera> poses;
	int n_sols = estimator.estimate(x, X, &poses);

	double err = minimum_pose_distance(pose_gt, poses, true, true);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);
}

bool test_larsson19_distortion_2_0() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params = { -0.12, 0.034 };
	larsson_iccv19::Solver<2, 0, true> estimator;

	generate_scene_and_image(estimator.minimal_sample_size(), 2, 20, 70, false, &pose_gt, &x, &X);
	add_rational_distortion(params, 2, 0, &pose_gt, &x);
	add_focal(2.0, &pose_gt, &x);

	std::vector<Camera> poses;
	int n_sols = estimator.estimate(x, X, &poses);

	double err = minimum_pose_distance(pose_gt, poses, true, true);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);
}

bool test_larsson19_distortion_3_0() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params = { -0.12, 0.034, -0.0056 };
	larsson_iccv19::Solver<3,0,true> estimator;

	generate_scene_and_image(estimator.minimal_sample_size(), 2, 20, 70, false, &pose_gt, &x, &X);
	add_rational_distortion(params, 3, 0, &pose_gt, &x);
	add_focal(2.0, &pose_gt, &x);

	std::vector<Camera> poses;
	int n_sols = estimator.estimate(x, X, &poses);

	double err = minimum_pose_distance(pose_gt, poses, true, true);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);
}

bool test_larsson19_distortion_3_3() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params = { -0.12, 0.034, -0.0056, 0.13, -0.012, 0.003 };
	larsson_iccv19::Solver<3, 3, true> estimator;

	generate_scene_and_image(estimator.minimal_sample_size(), 2, 20, 70, false, &pose_gt, &x, &X);
	add_rational_distortion(params, 3, 3, &pose_gt, &x);
	add_focal(2.0, &pose_gt, &x);

	std::vector<Camera> poses;
	int n_sols = estimator.estimate(x, X, &poses);

	double err = minimum_pose_distance(pose_gt, poses, true, false);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);
}

bool test_larsson19_precond() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params = { -0.12, 0.034, -0.0056 };
	larsson_iccv19::Solver<3, 0, true> estimator;

	//estimator.normalize_image_coord = true;
	//estimator.normalize_world_coord = true;

	generate_scene_and_image(estimator.minimal_sample_size(), 2, 20, 70, false, &pose_gt, &x, &X, 100.0);
	add_rational_distortion(params, 3, 0, &pose_gt, &x);
	add_focal(2000.0, &pose_gt, &x);

	estimator.use_precond = false;
	std::vector<Camera> poses;
	int n_sols = estimator.estimate(x, X, &poses);
	double err1 = minimum_pose_distance(pose_gt, poses, true, true);

//	std::cout << "precond=false, error=" << err1 << "\n";

	estimator.use_precond = true;
	n_sols = estimator.estimate(x, X, &poses);
	double err2 = minimum_pose_distance(pose_gt, poses, true, true);

//	std::cout << "precond=true, error=" << err2 << "\n";

	if (err2 > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err2 < TOL_POSE);
}

bool test_larsson19_refinement() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params6 = { -0.12, -0.034, 0.0056, 0.13, -0.023, 0.0035 };

	larsson_iccv19::Solver<3, 3, true> estimator;

	estimator.root_refinement = true;

	generate_scene_and_image(estimator.minimal_sample_size(), 2, 20, 70, false, &pose_gt, &x, &X, 1.0);
	add_rational_distortion(params6, 3, 3, &pose_gt, &x);
	add_focal(2000.0, &pose_gt, &x);

	std::vector<Camera> poses;
	int n_sols = estimator.estimate(x, X, &poses);

	double err = minimum_pose_distance(pose_gt, poses, true, false);

	//	std::cout << "precond=true, error=" << err2 << "\n";

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);

}

bool test_larsson19_undistortion_1_0() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params = { -0.12 };
	larsson_iccv19::Solver<1, 0, false> estimator;

	generate_scene_and_image(estimator.minimal_sample_size(), 2, 20, 70, false, &pose_gt, &x, &X);
	add_rational_undistortion(params, 1, 0, &pose_gt, &x);
	add_focal(2.0, &pose_gt, &x);

	std::vector<Camera> poses;
	int n_sols = estimator.estimate(x, X, &poses);

	double err = minimum_pose_distance(pose_gt, poses, true, true);

	if (err > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err < TOL_POSE);
}

bool test_radial_upgrade() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 2, Dynamic> x1;
	Matrix<double, 2, Dynamic> x2;

	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params3 = { -0.12, -0.034, 0.0056 };

	kukelova_iccv13::Solver estimator_U03(3);
	larsson_iccv19::Solver<3, 0, true> estimator_D30;

	generate_scene_and_image(estimator_D30.minimal_sample_size(), 2, 20, 70, false, &pose_gt, &x, &X, 1.0);
	x1 = x; x2 = x;

	add_rational_distortion(params3, 3, 0, &pose_gt, &x1);
	add_focal(2000.0, &pose_gt, &x1);

	add_rational_undistortion(params3, 0, 3, &pose_gt, &x2);
	add_focal(2000.0, &pose_gt, &x2);

	// Transform to current coordinate system
	std::vector<Camera> poses;

	X = pose_gt.R * X;
	X.row(0).array() += pose_gt.t(0);
	X.row(1).array() += pose_gt.t(1);

	pose_gt.R = Matrix3d::Identity();
	pose_gt.t(0) = 0.0;
	pose_gt.t(1) = 0.0;

	estimator_D30.use_radial_solver = false;
	estimator_D30.normalize_world_coord = true;
	estimator_D30.center_world_coord = false;
	estimator_D30.check_chirality = false;
	estimator_D30.check_reprojection_error = false;

	estimator_U03.use_radial_solver = false;
	estimator_U03.normalize_world_coord = true;
	estimator_U03.center_world_coord = false;
	estimator_U03.check_chirality = false;
	estimator_U03.check_reprojection_error = false;

	int n_sols1 = estimator_D30.estimate(x1, X, &poses);
	double err1 = minimum_pose_distance(pose_gt, poses, true, false);
	if (err1 > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	int n_sols2 = estimator_U03.estimate(x2, X, &poses);
	double err2 = minimum_pose_distance(pose_gt, poses, true, false);
	if (err2 > TOL_POSE)
		debug_print_poses(pose_gt, poses);

	return (err1 < TOL_POSE && err2 < TOL_POSE);
}

int run_solver_benchmark() {

	int trials = 1000;
	//int trials = 10; // To speed things up during debug.
	std::vector<double> params1 = { -0.12 };
	std::vector<double> params2 = { -0.12, -0.034 };
	std::vector<double> params3 = { -0.12, -0.034, 0.0056 };
	std::vector<double> params6 = { -0.12, -0.034, 0.0056, 0.13, -0.023, 0.0035 };

	larsson_iccv19::Solver<1, 0, true> estimator_D10;
	larsson_iccv19::Solver<2, 0, true> estimator_D20;
	larsson_iccv19::Solver<3, 0, true> estimator_D30;
	larsson_iccv19::Solver<3, 3, true> estimator_D33;

	larsson_iccv19::Solver<1, 0, false> estimator_U10;
	larsson_iccv19::Solver<2, 0, false> estimator_U20;
	larsson_iccv19::Solver<3, 0, false> estimator_U30;
	larsson_iccv19::Solver<3, 3, false> estimator_U33;

	larsson_iccv17::NonPlanarSolver estimator_L17_U01;
	bujnak_accv10::NonPlanarSolver estimator_B10_U01;
	kukelova_iccv13::Solver estimator_K13_U01(1);
	kukelova_iccv13::Solver estimator_K13_U02(2);
	kukelova_iccv13::Solver estimator_K13_U03(3);

	auto addUndistortion01 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 0, 1, p, x); };
	auto addUndistortion02 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 0, 2, p, x); };
	auto addUndistortion03 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 0, 3, p, x); };

	auto addUndistortion10 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 1, 0, p, x); };
	auto addUndistortion20 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 2, 0, p, x); };
	auto addUndistortion30 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 3, 0, p, x); };
	auto addUndistortion33 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 3, 3, p, x); };

	auto addDistortion10 = [](std::vector<double> params, Camera *p, Matrix<double, 2, Dynamic> *x) {	add_rational_distortion(params, 1, 0, p, x); };
	auto addDistortion20 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_distortion(params, 2, 0, p, x); };
	auto addDistortion30 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_distortion(params, 3, 0, p, x); };
	auto addDistortion33 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_distortion(params, 3, 3, p, x); };

	benchmark_solver("KUKELOVA_13_U01", &estimator_K13_U01, addUndistortion01, params1, trials);
	benchmark_solver("KUKELOVA_13_U02", &estimator_K13_U02, addUndistortion02, params2, trials);
	benchmark_solver("KUKELOVA_13_U03", &estimator_K13_U03, addUndistortion03, params3, trials);
	benchmark_solver("LARSSON_17_U01", &estimator_L17_U01, addUndistortion01, params1, trials);
	benchmark_solver("BUJNAK_10_U01", &estimator_B10_U01, addUndistortion01, params1, trials);
	benchmark_solver("LARSSON_19_D10", &estimator_D10, addDistortion10, params1, trials);
	benchmark_solver("LARSSON_19_D20", &estimator_D20, addDistortion20, params2, trials);
	benchmark_solver("LARSSON_19_D30", &estimator_D30, addDistortion30, params3, trials);
	benchmark_solver("LARSSON_19_D33", &estimator_D33, addDistortion33, params6, trials);
	benchmark_solver("LARSSON_19_U10", &estimator_U10, addUndistortion10, params1, trials);

	return 0;
}

int run_solver_benchmark2() {

	int trials = 1000;
	//int trials = 10; // To speed things up during debug.
	std::vector<double> params1 = { -0.12 };
	std::vector<double> params2 = { -0.12, -0.034 };
	std::vector<double> params3 = { -0.12, -0.034, 0.0056 };
	std::vector<double> params6 = { -0.12, -0.034, 0.0056, -0.08, -0.024, 0.0026 };

	larsson_iccv19::Solver<1, 0, true> estimator_D10;
	larsson_iccv19::Solver<2, 0, true> estimator_D20;
	larsson_iccv19::Solver<3, 0, true> estimator_D30;
	larsson_iccv19::Solver<3, 3, true> estimator_D33;

	auto addUndistortion01 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 0, 1, p, x); };
	auto addUndistortion02 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 0, 2, p, x); };
	auto addUndistortion03 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 0, 3, p, x); };
	auto addDistortion10 = [](std::vector<double> params, Camera *p, Matrix<double, 2, Dynamic> *x) {	add_rational_distortion(params, 1, 0, p, x); };
	auto addDistortion20 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_distortion(params, 2, 0, p, x); };
	auto addDistortion30 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_distortion(params, 3, 0, p, x); };
	auto addDistortion33 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_distortion(params, 3, 3, p, x); };

	bool use_rescaling = true;
	bool use_precond = true;
	bool use_qz_solver = false;

	//FLIPPYBOOL(use_rescaling) {
	//	FLIPPYBOOL(use_qz_solver) {
	FLIPPYBOOL(normalize_image) {
		FLIPPYBOOL(normalize_world) {
			FLIPPYBOOL(center_world) {

			//FLIPPYBOOL(use_precond) {

				estimator_D10.use_rescaling = use_rescaling;
				estimator_D20.use_rescaling = use_rescaling;
				estimator_D30.use_rescaling = use_rescaling;
				estimator_D33.use_rescaling = use_rescaling;

				estimator_D10.use_qz_solver = use_qz_solver;
				estimator_D20.use_qz_solver = use_qz_solver;
				estimator_D30.use_qz_solver = use_qz_solver;
				estimator_D33.use_qz_solver = use_qz_solver;
				estimator_D10.normalize_image_coord = normalize_image;
				estimator_D20.normalize_image_coord = normalize_image;
				estimator_D30.normalize_image_coord = normalize_image;
				estimator_D33.normalize_image_coord = normalize_image;

				estimator_D10.normalize_world_coord = normalize_world;
				estimator_D20.normalize_world_coord = normalize_world;
				estimator_D30.normalize_world_coord = normalize_world;
				estimator_D33.normalize_world_coord = normalize_world;;

				estimator_D10.center_world_coord = center_world;
				estimator_D20.center_world_coord = center_world;
				estimator_D30.center_world_coord = center_world;
				estimator_D33.center_world_coord = center_world;

				estimator_D10.use_precond = use_precond;
				estimator_D20.use_precond = use_precond;
				estimator_D30.use_precond = use_precond;
				estimator_D33.use_precond = use_precond;

				std::cout << "Benchmark: use_rescaling=" << use_rescaling << ", use_qz_solver=" << use_qz_solver << ", norm_image=" << normalize_image << ", norm_world=" << normalize_world << ", center_world=" << center_world << ", use_precond=" << use_precond << "\n";
				benchmark_solver("LARSSON_19_D10", &estimator_D10, addDistortion10, params1, trials);
				benchmark_solver("LARSSON_19_D20", &estimator_D20, addDistortion20, params2, trials);
				benchmark_solver("LARSSON_19_D30", &estimator_D30, addDistortion30, params3, trials);
				benchmark_solver("LARSSON_19_D33", &estimator_D33, addDistortion33, params6, trials);

				std::cout << "------------------------\n";
			}
		}
	}

	return 0;
}

int run_solver_benchmark3() {

	int trials = 1000;
	//int trials = 10; // To speed things up during debug.
	std::vector<double> params1 = { -0.12 };
	std::vector<double> params2 = { -0.12, -0.034 };
	std::vector<double> params3 = { -0.12, -0.034, 0.0056 };
	std::vector<double> params6 = { -0.12, -0.034, 0.0056, 0.13, -0.023, 0.0035 };

	larsson_iccv19::Solver<1, 0, true> estimator_D10;
	larsson_iccv19::Solver<2, 0, true> estimator_D20;
	larsson_iccv19::Solver<3, 0, true> estimator_D30;
	larsson_iccv19::Solver<3, 3, true> estimator_D33;
	larsson_iccv19::Solver<1, 0, false> estimator_U10;

	auto addUndistortion01 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 0, 1, p, x); };
	auto addUndistortion02 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 0, 2, p, x); };
	auto addUndistortion03 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 0, 3, p, x); };
	auto addUndistortion10 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_undistortion(params, 1, 0, p, x); };

	auto addDistortion10 = [](std::vector<double> params, Camera *p, Matrix<double, 2, Dynamic> *x) {	add_rational_distortion(params, 1, 0, p, x); };
	auto addDistortion20 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_distortion(params, 2, 0, p, x); };
	auto addDistortion30 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_distortion(params, 3, 0, p, x); };
	auto addDistortion33 = [](std::vector<double> params, Camera* p, Matrix<double, 2, Dynamic>* x) {	add_rational_distortion(params, 3, 3, p, x); };

	FLIPPYBOOL(root_refinement) {

		estimator_D10.root_refinement = root_refinement;
		estimator_D20.root_refinement = root_refinement;
		estimator_D30.root_refinement = root_refinement;
		estimator_D33.root_refinement = root_refinement;
		estimator_U10.root_refinement = root_refinement;

		std::cout << "Benchmark: root_refinement=" << root_refinement << "\n";
		benchmark_solver("LARSSON_19_D10", &estimator_D10, addDistortion10, params1, trials);
		benchmark_solver("LARSSON_19_D20", &estimator_D20, addDistortion20, params2, trials);
		benchmark_solver("LARSSON_19_D30", &estimator_D30, addDistortion30, params3, trials);
		benchmark_solver("LARSSON_19_D33", &estimator_D33, addDistortion33, params6, trials);
		benchmark_solver("LARSSON_19_U10", &estimator_U10, addUndistortion10, params1, trials);

		std::cout << "------------------------\n";

	}

	return 0;
}

bool test_refinement_dist() {

	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params = { -0.12, 0.034, -0.0056, 0.01, 0.02, 0.003 };

	generate_scene_and_image(10, 2, 20, 70, false, &pose_gt, &x, &X);
	add_rational_distortion(params, 3, 3, &pose_gt, &x);
	add_focal(2.0, &pose_gt, &x);
	add_noise(0.001, &x);

	Camera p = pose_gt;

	Vector3d tmp;
	tmp.setRandom();
	tmp *= 0.01;
	Quaterniond qq;
	qq.w() = 1.0;
	qq.x() = tmp(0);
	qq.y() = tmp(1);
	qq.z() = tmp(2);
	qq.normalize();

	p.R = qq.toRotationMatrix() * p.R;

	p.t(0) -= 0.02;
	p.t(1) -= 0.01;
	p.t(2) += 0.01;
	p.focal += 0.01;
	p.dist_params[0] += 0.001;
	p.dist_params[1] += 0.0001;
	p.dist_params[2] += 0.00001;
	p.dist_params[3] += 0.001;
	p.dist_params[4] += 0.0001;
	p.dist_params[5] += 0.00001;

	double err_before = (pose_gt.R - p.R).norm() + (pose_gt.t - p.t).norm() + abs(pose_gt.focal - p.focal);
	refinement_dist(x, X, p, 3, 3);
	double err_after = (pose_gt.R - p.R).norm() + (pose_gt.t - p.t).norm() + abs(pose_gt.focal - p.focal);
	std::cout << "test_refinement_dist: error = " << err_before << " --> " << err_after << "\n";

	return err_after < err_before;
}

bool test_refinement_undist() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params = { -0.12, 0.034, -0.0056, 0.1, 0.01, 0.001 };

	generate_scene_and_image(10, 2, 20, 70, false, &pose_gt, &x, &X);
	add_rational_undistortion(params, 3, 3, &pose_gt, &x);
	add_focal(2.0, &pose_gt, &x);
	add_noise(0.001, &x);

	Camera p = pose_gt;

	Vector3d tmp;
	tmp.setRandom();
	tmp *= 0.01;
	Quaterniond qq;
	qq.w() = 1.0;
	qq.x() = tmp(0);
	qq.y() = tmp(1);
	qq.z() = tmp(2);
	qq.normalize();

	p.R = qq.toRotationMatrix() * p.R;
	p.t(0) -= 0.02;
	p.t(1) -= 0.01;
	p.t(2) += 0.01;
	p.focal += 0.01;
	p.dist_params[0] += 0.001;
	p.dist_params[1] += 0.0001;
	p.dist_params[2] += 0.00001;
	p.dist_params[3] += 0.001;
	p.dist_params[4] += 0.0001;
	p.dist_params[5] += 0.00001;
	double err_before = (pose_gt.R - p.R).norm() + (pose_gt.t - p.t).norm() + abs(pose_gt.focal - p.focal);

	refinement_undist(x, X, p, 3, 3);

	double err_after = (pose_gt.R - p.R).norm() + (pose_gt.t - p.t).norm() + abs(pose_gt.focal - p.focal);
	std::cout << "test_refinement_undist: error = " << err_before << " --> " << err_after << "\n";

	return err_after < err_before;
}

bool test_refinement_undist_division() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params = { -0.12 };

	generate_scene_and_image(10, 2, 20, 70, false, &pose_gt, &x, &X);
	add_rational_undistortion(params, 0, 1, &pose_gt, &x);
	add_focal(2.0, &pose_gt, &x);
	add_noise(0.0001, &x);

	Camera p = pose_gt;

	Vector3d tmp;
	tmp.setRandom();
	tmp *= 0.01;
	Quaterniond qq;
	qq.w() = 1.0;
	qq.x() = tmp(0);
	qq.y() = tmp(1);
	qq.z() = tmp(2);
	qq.normalize();

	p.R = qq.toRotationMatrix() * p.R;
	p.t(0) -= 0.02;
	p.t(1) -= 0.01;
	p.t(2) += 0.01;
	p.focal += 0.01;
	p.dist_params[0] += 0.001;
	double err_before = (pose_gt.R - p.R).norm() + (pose_gt.t - p.t).norm() + abs(pose_gt.focal - p.focal);

	refinement_undist(x, X, p, 0, 1);

	double err_after = (pose_gt.R - p.R).norm() + (pose_gt.t - p.t).norm() + abs(pose_gt.focal - p.focal);
	std::cout << "test_refinement_undist: error = " << err_before << " --> " << err_after << "\n";

	return err_after < err_before;
}

int main() {
	std::cout << "Running tests...\n\n";
	srand((unsigned int)time(0));
	srand(2.0);

	int passed = 0;
	int num_tests = 0;

	TEST(test_kukelova_1d_radial);
	TEST(test_kukelova_solver_1p_div);
	TEST(test_kukelova_solver_3p_div);
	TEST(test_larsson17_solver_1p_div);
	TEST(test_bujnak10_solver_1p_div);
	TEST(test_planar_oskarsson);
	TEST(test_larsson19_distortion_1_0);
	TEST(test_larsson19_distortion_2_0);
	TEST(test_larsson19_distortion_3_0);
	TEST(test_larsson19_distortion_3_3);
	TEST(test_larsson19_undistortion_1_0);
	TEST(test_larsson19_distortion_1_0_planar);
	TEST(test_larsson19_precond);
	TEST(test_1param_div_model);
	TEST(test_rational_distortion);
	TEST(test_radial_upgrade);
	TEST(test_refinement_dist);
	TEST(test_refinement_undist);
	TEST(test_refinement_undist_division);

	std::cout << "\nDone! Passed " << passed << "/" << num_tests << " tests.\n";

	run_solver_benchmark();

}


// ======================================================================
// \title  FrameTransformImpl.cpp
// \author gene
// \brief  cpp file for FrameTransform component implementation class
//
// \copyright
// Copyright 2009-2015, by the California Institute of Technology.
// ALL RIGHTS RESERVED.  United States Government Sponsorship
// acknowledged. Any commercial use must be negotiated with the Office
// of Technology Transfer at the California Institute of Technology.
//
// This software may be subject to U.S. export control laws and
// regulations.  By accepting this document, the user agrees to comply
// with all U.S. export laws and regulations.  User has the
// responsibility to obtain export licenses, or other export authority
// as may be required before exporting such information to foreign
// countries or providing access to foreign persons.
// ======================================================================

#include <Gnc/Utils/FrameTransform/FrameTransformComponentImpl.hpp>
#include "Fw/Types/BasicTypes.hpp"

#include <Eigen/Geometry>

namespace Gnc {

  // ----------------------------------------------------------------------
  // Construction, initialization, and destruction
  // ----------------------------------------------------------------------

  FrameTransformComponentImpl ::
#if FW_OBJECT_NAMES == 1
    FrameTransformComponentImpl(
        const char *const compName
    ) :
      FrameTransformComponentBase(compName),
#else
      FrameTransformImpl(void),
#endif
      paramsInited(false),
      a_X_b(Eigen::Affine3d::Identity()),
      b_X_a(Eigen::Affine3d::Identity()),
      doRotX(false),
      doRotV(true)
  {

  }

  void FrameTransformComponentImpl ::
    init(
        const NATIVE_INT_TYPE instance
    )
  {
    FrameTransformComponentBase::init(instance);
  }

  FrameTransformComponentImpl ::
    ~FrameTransformComponentImpl(void)
  {

  }

  void FrameTransformComponentImpl ::
    parameterUpdated(FwPrmIdType id)
  {
#ifndef BUILD_TIR5
    printf("prm %d updated\n", id);
#endif
  }

  void FrameTransformComponentImpl ::
    parametersLoaded()
  {
      using namespace Eigen;

      Fw::ParamValid valid[7];
      const Matrix3d a_R_b(Quaterniond(paramGet_a_q_b__w(valid[0]),
                                       paramGet_a_q_b__x(valid[1]),
                                       paramGet_a_q_b__y(valid[2]),
                                       paramGet_a_q_b__z(valid[3])));
      const Translation3d a_r_b(paramGet_a_r_b__x(valid[4]),
                                paramGet_a_r_b__y(valid[5]),
                                paramGet_a_r_b__z(valid[6]));

      for (unsigned int i = 0; i < 7; i++) {
          if (Fw::PARAM_VALID != valid[i]) {
              this->log_WARNING_HI_FTFO_InitFailed(FTFO_PrmUnset);
              return;
          }
      }

      this->a_X_b = a_R_b * a_r_b; // translation after rotation
      this->b_X_a = this->a_X_b.inverse();

      this->doRotX = paramGet_doRotX(valid[0]);
      this->doRotV = paramGet_doRotV(valid[1]);

      for (unsigned int i = 0; i < 2; i++) {
          if ((Fw::PARAM_VALID   != valid[i])  &&
              (Fw::PARAM_DEFAULT != valid[i])) {
              return;
          }
      }

      this->paramsInited = true;
  }

  // ----------------------------------------------------------------------
  // Handler implementations for user-defined typed input ports
  // ----------------------------------------------------------------------

  void FrameTransformComponentImpl ::
    prmTrigger_handler(
        const NATIVE_INT_TYPE portNum,
        FwPrmIdType dummy
    )
  {
      this->loadParameters();
  }

  void FrameTransformComponentImpl ::
    odomInA_handler(
        const NATIVE_INT_TYPE portNum,
        ROS::nav_msgs::OdometryAccel &Odometry
    )
  {
      using namespace Eigen;
      ROS::geometry_msgs::PoseWithCovariance poseCov = Odometry.getpose();
      ROS::geometry_msgs::Pose pose = poseCov.getpose();
      if (this->doRotX) {
          ROS::geometry_msgs::Point p = pose.getposition();
          const Vector3d p_b = this->b_X_a * Vector3d(p.getx(), p.gety(), p.getz());
          p.setx(p_b(0));
          p.sety(p_b(1));
          p.setz(p_b(2));
          pose.setposition(p);
      }
      /* NOTE(mereweth) - could account for offset from A to B
       * if both are body, but would need to know world_q_a or world_q_b
       */

      ROS::geometry_msgs::Quaternion q = pose.getorientation();
      const Quaterniond q_b(this->b_X_a.rotation()
                            * Quaterniond(q.getw(), q.getx(), q.gety(), q.getz()));
      q.setw(q_b.w());
      q.setx(q_b.x());
      q.sety(q_b.y());
      q.setz(q_b.z());
      pose.setorientation(q);
      poseCov.setpose(pose);
      Odometry.setpose(poseCov);

      ROS::geometry_msgs::TwistWithCovariance twistCov = Odometry.gettwist();
      ROS::geometry_msgs::Twist twist = twistCov.gettwist();
      if (this->doRotV) {
          ROS::geometry_msgs::Vector3 lin = twist.getlinear();
          const Vector3d lin_b = this->b_X_a.rotation()
              * Vector3d(lin.getx(), lin.gety(), lin.getz());
          lin.setx(lin_b(0));
          lin.sety(lin_b(1));
          lin.setz(lin_b(2));
          twist.setlinear(lin);
      }

      ROS::geometry_msgs::Vector3 ang = twist.getangular();
      const Vector3d ang_b = this->b_X_a.rotation()
          * Vector3d(ang.getx(), ang.gety(), ang.getz());
      ang.setx(ang_b(0));
      ang.sety(ang_b(1));
      ang.setz(ang_b(2));
      twist.setangular(ang);
      twistCov.settwist(twist);
      Odometry.settwist(twistCov);

      this->odomOutB_out(portNum, Odometry);
  }

  void FrameTransformComponentImpl ::
    odomInB_handler(
        const NATIVE_INT_TYPE portNum,
        ROS::nav_msgs::OdometryAccel &Odometry
    )
  {
      using namespace Eigen;
      ROS::geometry_msgs::PoseWithCovariance poseCov = Odometry.getpose();
      ROS::geometry_msgs::Pose pose = poseCov.getpose();
      if (this->doRotX) {
          ROS::geometry_msgs::Point p = pose.getposition();
          const Vector3d p_a = this->a_X_b * Vector3d(p.getx(), p.gety(), p.getz());
          p.setx(p_a(0));
          p.sety(p_a(1));
          p.setz(p_a(2));
          pose.setposition(p);
      }
      /* NOTE(mereweth) - could account for offset from A to B
       * if both are body, but would need to know world_q_a or world_q_b
       */

      ROS::geometry_msgs::Quaternion q = pose.getorientation();
      const Quaterniond q_a(this->a_X_b.rotation()
          * Quaterniond(q.getw(), q.getx(), q.gety(), q.getz()));
      q.setw(q_a.w());
      q.setx(q_a.x());
      q.sety(q_a.y());
      q.setz(q_a.z());
      pose.setorientation(q);
      poseCov.setpose(pose);
      Odometry.setpose(poseCov);

      ROS::geometry_msgs::TwistWithCovariance twistCov = Odometry.gettwist();
      ROS::geometry_msgs::Twist twist = twistCov.gettwist();
      if (this->doRotV) {
          ROS::geometry_msgs::Vector3 lin = twist.getlinear();
          const Vector3d lin_a = this->a_X_b.rotation()
              * Vector3d(lin.getx(), lin.gety(), lin.getz());
          lin.setx(lin_a(0));
          lin.sety(lin_a(1));
          lin.setz(lin_a(2));
          twist.setlinear(lin);
      }

      ROS::geometry_msgs::Vector3 ang = twist.getangular();
      const Vector3d ang_a = this->a_X_b.rotation()
          * Vector3d(ang.getx(), ang.gety(), ang.getz());
      ang.setx(ang_a(0));
      ang.sety(ang_a(1));
      ang.setz(ang_a(2));
      twist.setangular(ang);
      twistCov.settwist(twist);
      Odometry.settwist(twistCov);

      this->odomOutA_out(portNum, Odometry);
  }

  // ----------------------------------------------------------------------
  // Command handler implementations
  // ----------------------------------------------------------------------

  void FrameTransformComponentImpl ::
    FTFO_InitParams_cmdHandler(
        const FwOpcodeType opCode,
        const U32 cmdSeq
    )
  {
      this->parametersLoaded();
      if (this->paramsInited) {
          this->cmdResponse_out(opCode, cmdSeq, Fw::COMMAND_OK);
      }
      else {
          this->cmdResponse_out(opCode, cmdSeq, Fw::COMMAND_EXECUTION_ERROR);
      }
  }

} // end namespace Gnc


/** @file
*****************************************************************************

Implementation of interfaces for a lattice-based ppSNARG for R1CS.

See r1cs_lattice_ppsnarg.hpp

*****************************************************************************
* @author     Samir Menon, Brennan Shacklett, and David J. Wu
* @copyright  MIT license (see LICENSE file)
*****************************************************************************/

#ifndef R1CS_LATTICE_PPSNARG_TCC_
#define R1CS_LATTICE_PPSNARG_TCC_

#include <algorithm>
#include <cassert>
#include <functional>
#include <iostream>
#include <sstream>
#include <inttypes.h>
#include <NTL/mat_ZZ_p.h>

#include <libff/common/profiling.hpp>
#include <libff/common/utils.hpp>

#include <lattice_snarg/algebra/lattice/lwe.hpp>
#include <libsnark/reductions/r1cs_to_qap/r1cs_to_qap.hpp>

namespace libsnark {

template<typename ppT>
static LWE::matrix generate_Y(const int dim) {
    NTL::ZZ_p::init(LWE::p);
    LWE::matrix Y(NTL::INIT_SIZE, dim, dim);

    for (int i = 1; i <= dim; i++) {
        for (int j = 1; j <= dim; j++) {
            Y(i, j) = libff::Fr<ppT>::random_element().as_ZZ_p();
        }
    }

    return Y;
}

template<typename ppT>
static LWE::matrix make_query_matrix(const std::vector<libff::Fr_vector<ppT>> &A_query,
                                     const std::vector<libff::Fr_vector<ppT>> &B_query,
                                     const std::vector<libff::Fr_vector<ppT>> &C_query,
                                     const std::vector<libff::Fr_vector<ppT>> &H_query,
                                     const libff::Fr_vector<ppT> &Zs, size_t num_inputs) {
    // Strip out the first (num_inputs + 1) components of A, B, C
    // (corresponds to constant term and the bits of the statement)
    int ABC_rows = A_query[0].size() - num_inputs - 1;
    int rows = ABC_rows + 3 + H_query[0].size();
    int cols = LWE::l;

    NTL::ZZ_p::init(LWE::p);
    LWE::matrix mat(NTL::INIT_SIZE, rows, 4*cols);
    NTL::clear(mat);

    /* Creates the following matrix:
     * A|B|C 0
     *   0   H
     */

    // Copy A, B, C
    for (int i = 1; i <= ABC_rows; i++) {
        for (int j = 1; j <= cols; j++) {
            mat(i, j)          = A_query[j - 1][i - 1 + num_inputs + 1].as_ZZ_p();
            mat(i, j + cols)   = B_query[j - 1][i - 1 + num_inputs + 1].as_ZZ_p();
            mat(i, j + 2*cols) = C_query[j - 1][i - 1 + num_inputs + 1].as_ZZ_p();
        }
    }

    // Copy Zs to the bottom of A, B and C
    for (int i = 1; i <= 3; i++) {
        for (int j = 1; j <= cols; j++) {
            mat(ABC_rows + i, j + (i - 1)*cols) = Zs[j - 1].as_ZZ_p();
        }
    }

    // Copy H
    for (unsigned i = 1; i <= H_query[0].size(); i++) {
        for (int j = 1; j <= cols; j++) {
            mat(i + ABC_rows + 3, j + 3*cols) = H_query[j - 1][i - 1].as_ZZ_p();
        }
    }

    return mat;
}

static void encrypt_queries(std::vector<LWE::ciphertext> &enc_queries,
                     const LWE::secret_key &sk, const LWE::matrix &queries) {
    int nrows = queries.NumRows();

    enc_queries.resize(nrows);
    for (int i = 0; i < nrows; i++) {
        enc_queries[i] = LWE::encrypt(sk, queries[i]);
    }
}

template <typename ppT>
r1cs_lattice_ppsnarg_keypair<ppT> r1cs_lattice_ppsnarg_generator(const r1cs_lattice_ppsnarg_constraint_system<ppT> &cs) {
    libff::enter_block("Call to r1cs_lattice_ppsnarg_generator");

    std::vector<libff::Fr_vector<ppT>> A_queries(r1cs_lattice_ppsnarg_num_queries);
    std::vector<libff::Fr_vector<ppT>> B_queries(r1cs_lattice_ppsnarg_num_queries);
    std::vector<libff::Fr_vector<ppT>> C_queries(r1cs_lattice_ppsnarg_num_queries);
    std::vector<libff::Fr_vector<ppT>> H_queries(r1cs_lattice_ppsnarg_num_queries);

    libff::Fr_vector<ppT> Zs;

    // The first (num_inputs + 1) components of the A, B, and C queries. These
    // components are part of the verification state.
    std::vector<libff::Fr_vector<ppT>> A_prefix(r1cs_lattice_ppsnarg_num_queries);
    std::vector<libff::Fr_vector<ppT>> B_prefix(r1cs_lattice_ppsnarg_num_queries);
    std::vector<libff::Fr_vector<ppT>> C_prefix(r1cs_lattice_ppsnarg_num_queries);

    libff::enter_block("Generate (packed) QAP queries");
    size_t num_inputs = 0;
    for (size_t i = 0; i < r1cs_lattice_ppsnarg_num_queries; i++) {
        // Draw random field element for this query
        const libff::Fr<ppT> t = libff::Fr<ppT>::random_element();

        qap_instance_evaluation<libff::Fr<ppT> > qap_inst = r1cs_to_qap_instance_map_with_evaluation(cs, t);

        if (i == 0) {
            libff::print_indent(); printf("* QAP number of variables: %zu\n", qap_inst.num_variables());
            libff::print_indent(); printf("* QAP pre degree: %zu\n", cs.constraints.size());
            libff::print_indent(); printf("* QAP degree: %zu\n", qap_inst.degree());
            libff::print_indent(); printf("* QAP number of input variables: %zu\n", qap_inst.num_inputs());

            num_inputs = qap_inst.num_inputs();
        }

        A_queries[i] = std::move(qap_inst.At);
        B_queries[i] = std::move(qap_inst.Bt);
        C_queries[i] = std::move(qap_inst.Ct);
        H_queries[i] = std::move(qap_inst.Ht);

        Zs.emplace_back(qap_inst.Zt);

        for (size_t j = 0; j < num_inputs + 1; j++) {
            A_prefix[i].emplace_back(A_queries[i][j]);
            B_prefix[i].emplace_back(B_queries[i][j]);
            C_prefix[i].emplace_back(C_queries[i][j]);
        }
    }

    LWE::matrix query_mat = make_query_matrix<ppT>(A_queries, B_queries, C_queries, H_queries, Zs, num_inputs);
    libff::leave_block("Generate (packed) QAP queries");

    libff::enter_block("Apply random linear shift to packed queries");
    LWE::matrix Y = generate_Y<ppT>(4*LWE::l);
    query_mat *= Y;
    libff::leave_block("Apply random linear shift to packed queries");

    libff::enter_block("Generate verification key");
    LWE::secret_key sk = LWE::keygen();
    NTL::ZZ_p::init(NTL::ZZ(LWE::p));
    LWE::matrix Yprime = NTL::inv(NTL::transpose(Y));
    libff::leave_block("Generate verification key");

    libff::enter_block("Generate CRS");
    std::vector<LWE::ciphertext> enc_queries;
    encrypt_queries(enc_queries, sk, query_mat);
    libff::leave_block("Generate CRS");

    libff::leave_block("Call to r1cs_lattice_ppsnarg_generator");

    r1cs_lattice_ppsnarg_verification_key<ppT> vk = r1cs_lattice_ppsnarg_verification_key<ppT>(std::move(sk),
                                                                                std::move(Zs),
                                                                                std::move(Yprime),
                                                                                std::move(A_prefix),
                                                                                std::move(B_prefix),
                                                                                std::move(C_prefix));

    r1cs_lattice_ppsnarg_crs<ppT> crs = r1cs_lattice_ppsnarg_crs<ppT>(std::move(enc_queries), cs);

    return r1cs_lattice_ppsnarg_keypair<ppT>(std::move(crs), std::move(vk));
}

template <typename ppT>
r1cs_lattice_ppsnarg_proof<ppT> r1cs_lattice_ppsnarg_prover(const r1cs_lattice_ppsnarg_crs<ppT> &crs,
                                                const r1cs_lattice_ppsnarg_primary_input<ppT> &primary_input,
                                                const r1cs_lattice_ppsnarg_auxiliary_input<ppT> &auxiliary_input) {
    libff::enter_block("Call to r1cs_lattice_ppsnarg_prover");

#ifdef DEBUG
    assert(crs.constraint_system.is_satisfied(primary_input, auxiliary_input));
#endif

    const libff::Fr<ppT> d1 = libff::Fr<ppT>::random_element(),
                         d2 = libff::Fr<ppT>::random_element(),
                         d3 = libff::Fr<ppT>::random_element();

    libff::enter_block("Compute the polynomial H");
    const qap_witness<libff::Fr<ppT> > qap_wit = r1cs_to_qap_witness_map(crs.constraint_system, primary_input, auxiliary_input, d1, d2, d3);
    libff::leave_block("Compute the polynomial H");

#ifdef DEBUG
    const libff::Fr<ppT> t = libff::Fr<ppT>::random_element();
    qap_instance_evaluation<libff::Fr<ppT> > qap_inst = r1cs_to_qap_instance_map_with_evaluation(crs.constraint_system, t);
    assert(qap_inst.is_satisfied(qap_wit));
#endif

    NTL::ZZ_p::init(NTL::ZZ(LWE::q));

    libff::enter_block("Compute the proof");

    size_t num_inputs = qap_wit.num_inputs();
    size_t num_ABC_coeffs = qap_wit.coefficients_for_ABCs.size() - num_inputs;
    size_t proof_dim = num_ABC_coeffs + 3 + qap_wit.coefficients_for_H.size();
    LWE::vector pi(NTL::INIT_SIZE, proof_dim);
    for (size_t i = 0; i < num_ABC_coeffs; i++) {
        pi[i] = qap_wit.coefficients_for_ABCs[i + num_inputs].as_ZZ_p();
    }
    pi[num_ABC_coeffs]     = qap_wit.d1.as_ZZ_p();
    pi[num_ABC_coeffs + 1] = qap_wit.d2.as_ZZ_p();
    pi[num_ABC_coeffs + 2] = qap_wit.d3.as_ZZ_p();
    for (size_t i = 0; i < qap_wit.coefficients_for_H.size(); i++) {
        pi[num_ABC_coeffs + 3 + i] = qap_wit.coefficients_for_H[i].as_ZZ_p();
    }

    assert(proof_dim == crs.enc_queries.size());
    LWE::ciphertext ct = crs.enc_queries[0] * pi[0];
    for (size_t i = 1; i < proof_dim; i++) {
        ct += crs.enc_queries[i] * pi[i];
    }
    libff::leave_block("Compute the proof");

    libff::leave_block("Call to r1cs_lattice_ppsnarg_prover");

    r1cs_lattice_ppsnarg_proof<ppT> proof = r1cs_lattice_ppsnarg_proof<ppT>(std::move(ct));

    return proof;
}

template<typename ppT>
bool r1cs_lattice_ppsnarg_verifier(const r1cs_lattice_ppsnarg_verification_key<ppT> &vk,
                                     const r1cs_lattice_ppsnarg_primary_input<ppT> &primary_input,
                                     const r1cs_lattice_ppsnarg_proof<ppT> &proof) {
    bool result = true;

    libff::enter_block("Call to r1cs_lattice_ppsnarg_verifier");

    libff::enter_block("Decrypting proof");
    LWE::vector proof_decrypt = vk.Yprime*LWE::decrypt(vk.sk, proof.response);

    LWE::vector A(NTL::INIT_SIZE, r1cs_lattice_ppsnarg_num_queries);
    LWE::vector B(NTL::INIT_SIZE, r1cs_lattice_ppsnarg_num_queries);
    LWE::vector C(NTL::INIT_SIZE, r1cs_lattice_ppsnarg_num_queries);
    LWE::vector H(NTL::INIT_SIZE, r1cs_lattice_ppsnarg_num_queries);
    for (size_t i = 0; i < r1cs_lattice_ppsnarg_num_queries; i++) {
        A[i] = proof_decrypt[i];
        B[i] = proof_decrypt[i + r1cs_lattice_ppsnarg_num_queries];
        C[i] = proof_decrypt[i + 2*r1cs_lattice_ppsnarg_num_queries];
        H[i] = proof_decrypt[i + 3*r1cs_lattice_ppsnarg_num_queries];

        // Add in components corresponding to the constant term as well as the
        // components corresponding to the statement
        A[i] += vk.A_prefix[i][0].as_ZZ_p();
        B[i] += vk.B_prefix[i][0].as_ZZ_p();
        C[i] += vk.C_prefix[i][0].as_ZZ_p();

        for (size_t j = 0; j < primary_input.size(); j++) {
            A[i] += primary_input[j].as_ZZ_p() * vk.A_prefix[i][j + 1].as_ZZ_p();
            B[i] += primary_input[j].as_ZZ_p() * vk.B_prefix[i][j + 1].as_ZZ_p();
            C[i] += primary_input[j].as_ZZ_p() * vk.C_prefix[i][j + 1].as_ZZ_p();
        }
    }
    libff::leave_block("Decrypting proof");

    libff::Fr_vector<ppT> Z = vk.Z;

    libff::enter_block("Check QAP divisibility");
    NTL::ZZ_p::init(NTL::ZZ(LWE::p));
    for (size_t i = 0; i < r1cs_lattice_ppsnarg_num_queries; i++) {
        if (A[i]*B[i] != H[i]*Z[i].as_ZZ_p() + C[i]) {
            if (!libff::inhibit_profiling_info) {
                libff::print_indent(); printf("QAP divisiblity check failed.\n");
            }
            result = false;
        }
    }
    libff::leave_block("Check QAP divisibility");

    libff::leave_block("Call to r1cs_lattice_ppsnarg_verifier");
    return result;
}

} // libsnark

#endif // R1CS_LATTICE_PPSNARG_TCC_


#include "PressureProjection.h"

#include <iostream>

#include <Eigen/Sparse>

#include "tbb/tbb.h"

namespace FluidSim2D::SimTools
{

PressureProjection::PressureProjection(const LevelSet& surface,
										const VectorGrid<float>& cutCellWeights,
										const VectorGrid<float>& ghostFluidWeights,
										const VectorGrid<float>& solidVelocity)
	: mySurface(surface)
	, myCutCellWeights(cutCellWeights)
	, myGhostFluidWeights(ghostFluidWeights)
	, mySolidVelocity(solidVelocity)
	, myUseInitialGuessPressure(false)
	, myInitialGuessPressure(nullptr)
{
	// For efficiency sake, this should only take in velocity on a staggered grid
	// that matches the center sampled surface and collision

	assert(solidVelocity.sampleType() == VectorGridSettings::SampleType::STAGGERED);

#if !defined(NDEBUG)
	for (int axis : {0, 1})
	{
		Vec2i faceCount = solidVelocity.size(axis);

		Vec2i cellSize = faceCount;
		--cellSize[axis];

		assert(cellSize == surface.size());
	}
#endif

	assert(solidVelocity.isGridMatched(cutCellWeights) &&
		solidVelocity.isGridMatched(ghostFluidWeights));

	myPressure = ScalarGrid<float>(surface.xform(), surface.size(), 0);
	myValidFaces = VectorGrid<VisitedCellLabels>(surface.xform(), surface.size(), VisitedCellLabels::UNVISITED_CELL, VectorGridSettings::SampleType::STAGGERED);
}

void PressureProjection::drawPressure(Renderer& renderer) const
{
	myPressure.drawSupersampledValues(renderer, .25, 1, 2);
}

void PressureProjection::project(VectorGrid<float>& velocity)
{
	using SolveReal = double;
	using Vector = Eigen::VectorXd;

	assert(velocity.isGridMatched(mySolidVelocity));

	enum class MaterialLabels { SOLID_CELL, AIR_CELL, LIQUID_CELL };

	UniformGrid<MaterialLabels> materialCellLabels(mySurface.size(), MaterialLabels::SOLID_CELL);

	tbb::parallel_for(tbb::blocked_range<int>(0, materialCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i cell = materialCellLabels.unflatten(cellIndex);

				bool isFluidCell = false;

				for (int axis = 0; axis < 2 && !isFluidCell; ++axis)
					for (int direction : {0, 1})
					{
						Vec2i face = cellToFace(cell, axis, direction);

						if (myCutCellWeights(face, axis) > 0)
						{
							isFluidCell = true;
							break;
						}
					}

				if (isFluidCell)
				{
					if (mySurface(cell) <= 0)
						materialCellLabels(cell) = MaterialLabels::LIQUID_CELL;
					else
						materialCellLabels(cell) = MaterialLabels::AIR_CELL;
				}
			}
		});

	constexpr int UNSOLVED_CELL = -1;

	UniformGrid<int> liquidCellIndices(mySurface.size(), UNSOLVED_CELL);

	int liquidCellCount = 0;

	forEachVoxelRange(Vec2i(0), liquidCellIndices.size(), [&](const Vec2i& cell)
		{
			if (materialCellLabels(cell) == MaterialLabels::LIQUID_CELL)
				liquidCellIndices(cell) = liquidCellCount++;
		});

	Vector rhsVector = Vector::Zero(liquidCellCount);
	Vector initialGuessVector = Vector::Zero(liquidCellCount);

	Eigen::SparseMatrix<SolveReal> sparseMatrix(liquidCellCount, liquidCellCount);

	{
		tbb::enumerable_thread_specific<std::vector<Eigen::Triplet<SolveReal>>> parallelSparseMatrixElements;

		tbb::parallel_for(tbb::blocked_range<int>(0, liquidCellIndices.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			auto& localSparseMatrixElements = parallelSparseMatrixElements.local();

			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i cell = liquidCellIndices.unflatten(cellIndex);

				int liquidIndex = liquidCellIndices(cell);

				if (liquidIndex >= 0)
				{
					assert(materialCellLabels(cell) == MaterialLabels::LIQUID_CELL);

					// Compute divergence to add to RHS
					SolveReal divergence = 0;

					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i face = cellToFace(cell, axis, direction);

							SolveReal weight = myCutCellWeights(face, axis);

							SolveReal sign = (direction == 0) ? 1 : -1;

							// Add divergence from faces
							if (weight > 0)
								divergence += sign * weight * velocity(face, axis);
							if (weight < 1)
								divergence += sign * (1. - weight) * mySolidVelocity(face, axis);
						}

					rhsVector(liquidIndex) = divergence;

					SolveReal diagonal = 0;

					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i adjacentCell = cellToCell(cell, axis, direction);

							// Bounds check. If out-of-bounds, treat like a stationary grid-aligned solid.
							if (adjacentCell[axis] < 0 || adjacentCell[axis] >= mySurface.size()[axis])
								continue;

							Vec2i face = cellToFace(cell, axis, direction);

							SolveReal weight = myCutCellWeights(face, axis);

							if (weight > 0)
							{
								int adjacentLiquidIndex = liquidCellIndices(adjacentCell);
								if (adjacentLiquidIndex >= 0)
								{
									assert(materialCellLabels(adjacentCell) == MaterialLabels::LIQUID_CELL);

									localSparseMatrixElements.emplace_back(liquidIndex, adjacentLiquidIndex, -weight);
									diagonal += weight;
								}
								else
								{
									assert(materialCellLabels(adjacentCell) == MaterialLabels::AIR_CELL);

									SolveReal theta = myGhostFluidWeights(face, axis);

									theta = clamp(theta, SolveReal(.01), SolveReal(1));
									diagonal += weight / theta;
								}
							}
							else assert(materialCellLabels(adjacentCell) == MaterialLabels::SOLID_CELL);
						}

					assert(diagonal > 0);
					localSparseMatrixElements.emplace_back(liquidIndex, liquidIndex, diagonal);

					if (myUseInitialGuessPressure)
					{
						assert(myInitialGuessPressure != nullptr);
						initialGuessVector(liquidIndex) = (*myInitialGuessPressure)(cell);
					}
				}
				else assert(materialCellLabels(cell) != MaterialLabels::LIQUID_CELL);
			}
		});

		std::vector<Eigen::Triplet<SolveReal>> sparseMatrixElements;
		mergeLocalThreadVectors(sparseMatrixElements, parallelSparseMatrixElements);
		sparseMatrix.setFromTriplets(sparseMatrixElements.begin(), sparseMatrixElements.end());
	}

	Eigen::ConjugateGradient<Eigen::SparseMatrix<SolveReal>, Eigen::Upper | Eigen::Lower> solver;
	solver.compute(sparseMatrix);

	if (solver.info() != Eigen::Success)
	{
		std::cout << "   Solver failed to build" << std::endl;
		return;
	}

	solver.setTolerance(1E-5);

	Vector solutionVector = solver.solveWithGuess(rhsVector, initialGuessVector);

	if (solver.info() != Eigen::Success)
	{
		std::cout << "   Solver failed to converge" << std::endl;
		return;
	}
	else
	{
		std::cout << "    Solver iterations:     " << solver.iterations() << std::endl;
		std::cout << "    Solver error: " << solver.error() << std::endl;
	}

	// Copy resulting vector to pressure grid
	tbb::parallel_for(tbb::blocked_range<int>(0, liquidCellIndices.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = liquidCellIndices.unflatten(cellIndex);

			int liquidIndex = liquidCellIndices(cell);

			if (liquidIndex >= 0)
			{
				assert(materialCellLabels(cell) == MaterialLabels::LIQUID_CELL);
				myPressure(cell) = solutionVector(liquidIndex);
			}
			else
			{
				myPressure(cell) = 0;
				assert(materialCellLabels(cell) != MaterialLabels::LIQUID_CELL);
			}
		}
	});

	// Build valid faces
	for (int axis : {0, 1})
	{
		tbb::parallel_for(tbb::blocked_range<int>(0, myValidFaces.grid(axis).voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			for (int faceIndex = range.begin(); faceIndex != range.end(); ++faceIndex)
			{
				Vec2i face = myValidFaces.grid(axis).unflatten(faceIndex);

				bool isValidFace = false;
				if (myCutCellWeights(face, axis) > 0)
				{
					Vec2i backwardCell = faceToCell(face, axis, 0);
					Vec2i forwardCell = faceToCell(face, axis, 1);

					if (backwardCell[axis] < 0 || forwardCell[axis] >= mySurface.size()[axis])
						continue;

					if (liquidCellIndices(backwardCell) >= 0 || liquidCellIndices(forwardCell) >= 0)
					{
						assert(materialCellLabels(backwardCell) == MaterialLabels::LIQUID_CELL ||
							materialCellLabels(forwardCell) == MaterialLabels::LIQUID_CELL);

						isValidFace = true;
					}
				}

				if (isValidFace)
					myValidFaces(face, axis) = VisitedCellLabels::FINISHED_CELL;
				else
					assert(myValidFaces(face, axis) == VisitedCellLabels::UNVISITED_CELL);
			}
		});
	}

	// Apply pressure update
	for (int axis : {0, 1})
	{
		tbb::parallel_for(tbb::blocked_range<int>(0, myValidFaces.grid(axis).voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			for (int faceIndex = range.begin(); faceIndex != range.end(); ++faceIndex)
			{
				Vec2i face = myValidFaces.grid(axis).unflatten(faceIndex);

				SolveReal tempVelocity = 0;

				if (myValidFaces(face, axis) == VisitedCellLabels::FINISHED_CELL)
				{
					Vec2i backwardCell = faceToCell(face, axis, 0);
					Vec2i forwardCell = faceToCell(face, axis, 1);

					assert(myCutCellWeights(face, axis) > 0);
					assert(backwardCell[axis] >= 0 && forwardCell[axis] <= mySurface.size()[axis]);
					assert(materialCellLabels(backwardCell) == MaterialLabels::LIQUID_CELL || materialCellLabels(forwardCell) == MaterialLabels::LIQUID_CELL);

					SolveReal gradient = myPressure(forwardCell) - myPressure(backwardCell);

					if (materialCellLabels(backwardCell) == MaterialLabels::AIR_CELL ||
						materialCellLabels(forwardCell) == MaterialLabels::AIR_CELL)
					{
						SolveReal theta = myGhostFluidWeights(face, axis);
						theta = clamp(theta, SolveReal(.01), SolveReal(1));

						gradient /= theta;
					}

					tempVelocity = velocity(face, axis) - gradient;
				}

				velocity(face, axis) = tempVelocity;
			}
		});
	}

	//
	// Debug test to verify divergence free constraints are met
	//

	{
		tbb::enumerable_thread_specific<SolveReal> parallelMaxDivergence(SolveReal(0));
		tbb::enumerable_thread_specific<SolveReal> parallelAccumulatedDivergence(SolveReal(0));
		tbb::enumerable_thread_specific<SolveReal> parallelCellCount(SolveReal(0));

		tbb::parallel_for(tbb::blocked_range<int>(0, materialCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				auto& localMaxDivergence = parallelMaxDivergence.local();
				auto& localAccumulatedDivergence = parallelAccumulatedDivergence.local();
				auto& localCellCount = parallelCellCount.local();

				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = materialCellLabels.unflatten(cellIndex);

					if (materialCellLabels(cell) == MaterialLabels::LIQUID_CELL)
					{
						assert(liquidCellIndices(cell) >= 0);

						SolveReal divergence = 0;
						for (int axis : {0, 1})
							for (int direction : {0, 1})
							{
								Vec2i face = cellToFace(cell, axis, direction);
								SolveReal sign = (direction == 0) ? -1 : 1;

								SolveReal weight = myCutCellWeights(face, axis);

								if (weight > 0)
									divergence += sign * weight * velocity(face, axis);
							}

						localAccumulatedDivergence += std::fabs(divergence);
						localMaxDivergence = std::max(localMaxDivergence, std::fabs(divergence));
						++localCellCount;
					}
					else assert(liquidCellIndices(cell) == UNSOLVED_CELL);
				}
			});

		SolveReal accumulatedDivergence = 0;
		SolveReal maxDivergence;
		SolveReal cellCount = 0;

		parallelMaxDivergence.combine_each([&](SolveReal localMaxDivergence)
			{
				maxDivergence = std::max(localMaxDivergence, maxDivergence);
			});

		parallelAccumulatedDivergence.combine_each([&](SolveReal localAccumulatedDivergence)
			{
				accumulatedDivergence += localAccumulatedDivergence;
			});

		parallelCellCount.combine_each([&](SolveReal localCellCount)
			{
				cellCount += localCellCount;
			});

		assert(cellCount == liquidCellCount);

		std::cout << "Accumulated divergence: " << accumulatedDivergence << std::endl;
		std::cout << "Average divergence: " << accumulatedDivergence / SolveReal(cellCount) << std::endl;
		std::cout << "Max divergence: " << maxDivergence << std::endl;
	}

}

}


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
* @file vertex_vertex_thorup2kminus1_long_test.cpp
* @brief
* @author Jakub Oćwieja
* @version 1.0
* @date 2014-05-19
*/

#include "test_utils/logger.hpp"
#include "test_utils/read_dist.hpp"
#include "test_utils/test_result_check.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include "paal/distance_oracle/vertex_vertex/thorup_2kminus1.hpp"
#include "paal/utils/parse_file.hpp"

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/range/adaptor/sliced.hpp>

#include <boost/test/unit_test.hpp>

#include <fstream>

using namespace paal;

static const int CHECKED_VERTICES_NUM = 50;

template <typename Oracle>
void perform_full_check(const Graph& g, const Oracle& oracle, double ratio) {

    using DistanceType = boost::property_map<Graph, boost::edge_weight_t>::value_type;

    for (auto v: boost::make_iterator_range(vertices(g)) | boost::adaptors::sliced(0, CHECKED_VERTICES_NUM)) {
        std::vector<DistanceType> distances(num_vertices(g));
        // TODO Replace with graph_metric with graph_type::Large specialization when available
        dijkstra_shortest_paths(g, v, boost::distance_map(&distances[0]));

        for (auto u: boost::make_iterator_range(vertices(g))) {
            check_result(oracle(u,v), distances[u], ratio);
        }
    }

}

BOOST_AUTO_TEST_CASE( vv_thorup2kminus1_long_test ) {

    std::string test_dir = paal::system::get_test_data_dir("DISTANCE");
    using paal::system::build_path;

    parse(build_path(test_dir, "cases.txt"), [&](const std::string  & fname, std::istream & is_test_cases) {
        LOGLN("TEST " << fname);

        std::ifstream ifs(build_path(test_dir, "cases/" + fname + ".in"));
        assert(ifs.good());

        Graph g = read_dist(ifs);

        for (int k: {2,3}) {
            LOGLN("k: " << k);
            perform_full_check(g, make_distance_oracle_thorup2kminus1approximation(g, k), k*2-1);
        }
    });
}


#ifdef __JSGEODA__
#include <emscripten/bind.h>
#endif

#include <iostream>
#include <sstream>
#include <vector>

#include <rapidjson/document.h>
#include <rapidjson/writer.h>
#include <rapidjson/stringbuffer.h>

#include <boost/algorithm/string.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include "../libgeoda_src/clustering/DorlingCartogram.h"
#include "../libgeoda_src/weights/GalWeight.h"
#include "../libgeoda_src/GenUtils.h"
#include "../libgeoda_src/gda_weights.h"
#include "../libgeoda_src/gda_clustering.h"
#include "../libgeoda_src/libgeoda.h"

#include "geojson.h"
#include "jsgeoda.h"

std::map<std::string, GdaGeojson*> geojson_maps;

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
typedef bg::model::point<double, 2, bg::cs::cartesian> pt_2d;
typedef bg::model::box<pt_2d> box_2d;
typedef std::pair<pt_2d, unsigned> pt_2d_val;
typedef std::pair<box_2d, unsigned> box_2d_val;
typedef bgi::rtree< box_2d_val, bgi::quadratic<16> > rtree_box_2d_t;
typedef bgi::rtree< pt_2d_val, bgi::quadratic<16> > rtree_pt_2d_t;

typedef boost::geometry::model::d2::point_xy<double> point_type;
typedef boost::geometry::model::polygon<point_type> polygon_type;
typedef boost::geometry::model::multi_polygon<polygon_type> multi_polygon_type;

extern "C" {
    void new_geojsonmap(const char* file_name, uint8_t* data, size_t len);
}

void free_geojsonmap()
{
	std::map<std::string, GdaGeojson*>::iterator it;
	for (it = geojson_maps.begin(); it != geojson_maps.end(); ++it) {
		delete it->second;
	}
	geojson_maps.clear();
}

/**
 * Create a geojson map in memory using the user drag-n-dropped
 * *.geojson file via webpage
 *
 * @param int addr The pointer address of the byte array
 * @param size_t len The length of the member of byte array
 *
 * @return std::string Return a unique string represents as a key
 * 			to access the geojson map. Used as a reference to
 * 			all jsgeoda APIs. e.g. queen_weights(map_uid)
 *
 */
void new_geojsonmap(const char* file_name, uint8_t* in, size_t len) {
//void new_geojsonmap(std::string file_name, int& in, const size_t & len) {
    //We get out pointer as a plain int from javascript
    //We use a reinterpret_cast to turn our plain int into a uint8_t pointer. After
    //which we can play with the data just like we would normally.
    char* _in = reinterpret_cast<char*>(in);
    char* data = (char*)malloc(sizeof(char) * (len+1));
    //memcpy(data, _in, len);
    for (size_t i=0; i<len; ++i) {
        data[i] = _in[i];
    }
    data[len] = '\0';

    // store globally, has to be release by calling free_geojsonmap()
    GdaGeojson *json_map = new GdaGeojson(file_name, data);
    geojson_maps[std::string(file_name)] = json_map;
    free(data);
}

/*
// deprecated for now
void new_shapefilemap(std::string file_name) {
    std::cout << "new_shapefilemap" << file_name << std::endl;
    GeoDa* g = CreateGeoDaFromSHP (file_name.c_str());
    std::cout << "new_shapefilemap:" << g->GetNumObs() << std::endl;
    delete g;
}
*/

int get_num_obs(std::string map_uid) {
	//std::cout << "get_num_obs()" << map_uid << std::endl;
	GdaGeojson *json_map = geojson_maps[map_uid];
	if (json_map) {
		return json_map->GetNumObs();
	}
	return 0;
}

std::vector<double> get_bounds(std::string map_uid) {
    GdaGeojson *json_map = geojson_maps[map_uid];
    if (json_map) {
        return json_map->GetBounds();
    }
    return std::vector<double>();
}

int get_map_type(std::string map_uid) {
	//std::cout << "get_map_type()" << map_uid << std::endl;
	GdaGeojson *json_map = geojson_maps[map_uid];
	if (json_map) {
		return json_map->GetMapType();
	}
	return 0;
}

CCentroids get_centroids(std::string map_uid)
{
    CCentroids ct;

    GdaGeojson *json_map = geojson_maps[map_uid];
    if (json_map) {
        const std::vector<gda::PointContents*>& pts = json_map->GetCentroids();
        size_t num_obs = json_map->GetNumObs();
        ct.x.resize(num_obs);
        ct.y.resize(num_obs);

        for (size_t i=0; i<num_obs; ++i) {
            ct.x[i] = pts[i]->x;
            ct.y[i] = pts[i]->y;
        }
    }
    return ct;
}

std::vector<double> get_numeric_col(std::string map_uid, std::string col_name) {
    //std::cout << "get_numeric_col()" << map_uid << std::endl;
    GdaGeojson *json_map = geojson_maps[map_uid];
    if (json_map) {
        return json_map->GetNumericCol(col_name);
    }
    return std::vector<double>();
}

std::vector<std::string> get_col_names(const std::string& map_uid)
{
    GdaGeojson *json_map = geojson_maps[map_uid];
    if (json_map) {
        return json_map->GetColNames();
    }
    return std::vector<std::string>();
}

std::vector<std::string> get_string_col(std::string map_uid, std::string col_name) {
    //std::cout << "get_string_col()" << map_uid << std::endl;
    GdaGeojson *json_map = geojson_maps[map_uid];
    if (json_map) {
        return json_map->GetStringCol(col_name);
    }
    return std::vector<std::string>();
}

bool is_numeric_col(std::string map_uid, std::string col_name) {
    GdaGeojson *json_map = geojson_maps[map_uid];
    if (json_map) {
        return json_map->IsNumericCol(col_name);
    }
    return false;
}

std::vector<int> spatial_count(const std::string map_uid, const std::string aggregate_map_uid)
{
    std::vector<int> counts;
    GdaGeojson *map = geojson_maps[map_uid];
    GdaGeojson *aggregate_map = geojson_maps[aggregate_map_uid];
    if (map && aggregate_map) {
        // using selected layer (points) to create rtree
        rtree_pt_2d_t rtree_bbox;
        gda::MainMap& mm = map->GetMainMap();
        int num_obs = mm.num_obs;
        for (int i =0; i < num_obs; ++i) {
            gda::GeometryContent* geom = mm.records[i];
            gda::PointContents* pc = (gda::PointContents*)geom;
            rtree_bbox.insert(std::make_pair(pt_2d(pc->x, pc->y), i));
        }
        // query points in polygons
        gda::MainMap& polys = aggregate_map->GetMainMap();
        num_obs = polys.num_obs;
        counts.resize(num_obs, 0);
        for (int i =0; i < num_obs; ++i) {
            gda::GeometryContent *geom = polys.records[i];
            gda::PolygonContents *pc = (gda::PolygonContents *) geom;

            multi_polygon_type multi_poly;
            for (int p = 0; p < pc->num_parts; ++p) {
                polygon_type poly;
                int start = pc->parts[p];
                int end = p < pc->num_parts - 1 ? pc->parts[p+1] : pc->num_points;
                if (!pc->holes[p]) {
                    for(int j = start; j < end; ++j) {
                        double x = pc->points[j].x;
                        double y = pc->points[j].y;
                        bg::append(poly, bg::model::d2::point_xy<double>(x, y));
                    }
                }
                multi_poly.push_back(poly);
            }

            // query points in this box
            box_2d b(pt_2d(pc->box[0], pc->box[1]), pt_2d(pc->box[2], pc->box[3]));
            std::vector<pt_2d_val> q;
            rtree_bbox.query(bgi::within(b), std::back_inserter(q));
            for (int j=0; j<q.size(); j++) {
                const pt_2d_val& v = q[j];
                int pt_idx = v.second;
                double x = v.first.get<0>();
                double y = v.first.get<1>();

                point_type p(x, y);
                if (boost::geometry::within(p, multi_poly)) {
                    counts[i] += 1;
                }
            }
        }
    }
    return counts;
}

std::vector<int> get_neighbors(const std::string map_uid, const std::string weight_uid, int id)
{
    std::vector<int> nbrs;
    GdaGeojson *json_map = geojson_maps[map_uid];
    if (json_map) {
        GeoDaWeight *w = json_map->GetWeights(weight_uid);
        if (w) {
            const  std::vector<long>& _nbrs = w->GetNeighbors(id);
            for (size_t i=0; i<_nbrs.size(); ++i) {
                nbrs.push_back(_nbrs[i]);
            }
        }
    }
    return nbrs;
}

CartogramResult cartogram(const std::string map_uid, std::vector<double> values)
{
    CartogramResult r;

    GdaGeojson *json_map = geojson_maps[map_uid];
    if (json_map) {
        GalWeight* w = (GalWeight*)json_map->CreateRookWeights(1, false, 0);
        int num_obs = w->num_obs;
        const std::vector<gda::PointContents*>& cents = json_map->GetCentroids();
        CartNbrInfo* cart_nbr_info = new CartNbrInfo(w->gal, num_obs);
        std::vector<double> x(num_obs), y(num_obs);
        double orig_data_min=values[0], orig_data_max = values[0];
        for (size_t i=0; i<num_obs; ++i) {
            x[i] = cents[i]->x;
            y[i] = cents[i]->y;
            if (orig_data_min > values[i]) orig_data_min = values[i];
            if (orig_data_max < values[i]) orig_data_max = values[i];
        }
        DorlingCartogram* cartogram = new DorlingCartogram(cart_nbr_info, x, y, values, orig_data_min, orig_data_max);
        cartogram->improve(100);

        for (size_t i=0; i<num_obs; i++) {
            r.x.push_back(cartogram->output_x[i]);
            r.y.push_back(cartogram->output_y[i]);
            r.r.push_back(cartogram->output_radius[i]);
        }
    }
    return r;
}

#ifdef __JSGEODA__
//Using this command to compile
//  emcc --bind -O3 readFile.cpp -s WASM=1 -s TOTAL_MEMORY=268435456 -o api.js --std=c++11
//Note that you need to make sure that there's enough memory available to begin with.
//I got only 16mb without passing the TOTAL_MEMORY setting.
EMSCRIPTEN_BINDINGS(wasmgeoda) {

    emscripten::register_vector<std::string>("VectorString");
    emscripten::register_vector<int>("VectorInt");
    emscripten::register_vector<std::vector<int>>("VecVecInt");
    emscripten::register_vector<double>("VectorDouble");
    emscripten::register_vector<std::vector<double>>("VecVecDouble");

    //emscripten::register_map<std::string, std::vector<float> >("map<string, vector<float>>");

    emscripten::class_<CCentroids>("CCentroids")
        .function("get_x", &CCentroids::get_x)
        .function("get_y", &CCentroids::get_y)
        ;

    emscripten::class_<CartogramResult>("CartogramResult")
        .function("get_x", &CartogramResult::get_x)
        .function("get_y", &CartogramResult::get_y)
        .function("get_radius", &CartogramResult::get_radius)
        ;

    emscripten::class_<LisaResult>("LisaResult")
        .function("is_valid", &LisaResult::get_is_valid)
        .function("significances", &LisaResult::get_sig_local)
        .function("sig_categories", &LisaResult::get_sig_cat)
        .function("clusters", &LisaResult::get_cluster)
        .function("spatial_lags", &LisaResult::get_lag)
        .function("lisa_values", &LisaResult::get_lisa)
        .function("nn", &LisaResult::get_nn)
        .function("labels", &LisaResult::get_labels)
        .function("colors", &LisaResult::get_colors)
        ;

    emscripten::class_<WeightsResult>("WeightsResult")
        .function("is_valid", &WeightsResult::get_is_valid)
        .function("get_weight_type", &WeightsResult::get_weight_type)
        .function("get_num_obs", &WeightsResult::get_num_obs)
        .function("get_is_symmetric", &WeightsResult::get_is_symmetric)
        .function("get_is_valid", &WeightsResult::get_is_valid)
        .function("get_sparsity", &WeightsResult::get_sparsity)
        .function("get_max_nbrs", &WeightsResult::get_max_nbrs)
        .function("get_min_nbrs", &WeightsResult::get_min_nbrs)
        .function("get_median_nbrs", &WeightsResult::get_median_nbrs)
        .function("get_mean_nbrs", &WeightsResult::get_mean_nbrs)
        .function("get_uid", &WeightsResult::get_uid)
        .function("get_map_uid", &WeightsResult::get_map_uid)
        ;

    emscripten::class_<ClusteringResult>("ClusteringResult")
        .function("is_valid", &ClusteringResult::get_is_valid)
        .function("clusters", &ClusteringResult::get_clusters)
        .function("total_ss", &ClusteringResult::get_total_ss)
        .function("between_ss", &ClusteringResult::get_between_ss)
        .function("within_ss", &ClusteringResult::get_within_ss)
        .function("ratio", &ClusteringResult::get_ratio)
        ;

    //emscripten::function("new_geojsonmap", &new_geojsonmap);
    emscripten::function("free_geojsonmap", &free_geojsonmap);

    emscripten::function("get_bounds", &get_bounds);
    emscripten::function("get_num_obs", &get_num_obs);
    emscripten::function("get_map_type", &get_map_type);
    emscripten::function("is_numeric_col", &is_numeric_col);
    emscripten::function("get_numeric_col", &get_numeric_col);
    emscripten::function("get_string_col", &get_string_col);
    emscripten::function("get_col_names", &get_col_names);

    emscripten::function("min_distance_threshold", &get_min_dist_threshold);
    emscripten::function("queen_weights", &queen_weights);
    emscripten::function("rook_weights", &rook_weights);
    emscripten::function("knn_weights", &knn_weights);
    emscripten::function("dist_weights", &dist_weights);
    emscripten::function("kernel_weights", &kernel_weights);
    emscripten::function("kernel_bandwidth_weights", &kernel_bandwidth_weights);

    emscripten::function("local_moran", &local_moran);
    emscripten::function("local_moran_eb", &local_moran_eb);
    emscripten::function("local_g", &local_g);
    emscripten::function("local_gstar", &local_gstar);
    emscripten::function("local_geary", &local_geary);
    emscripten::function("local_joincount", &local_joincount);
    emscripten::function("quantile_lisa", &quantile_lisa);
    emscripten::function("neighbor_match_test", &neighbor_match_test);
    emscripten::function("multi_quantile_lisa", &multi_quantile_lisa);
    emscripten::function("local_multijoincount", &local_multijoincount);
    emscripten::function("local_multigeary", &local_multigeary);

    emscripten::function("redcap", &redcap);
    emscripten::function("schc", &schc);
    emscripten::function("azp_greedy", &azp_greedy);
    emscripten::function("azp_sa", &azp_sa);
    emscripten::function("azp_tabu", &azp_tabu);
    emscripten::function("maxp_greedy", &maxp_greedy);
    emscripten::function("maxp_sa", &maxp_sa);
    emscripten::function("maxp_tabu", &maxp_tabu);

    emscripten::function("natural_breaks", &natural_breaks);
    emscripten::function("quantile_breaks", &quantile_breaks);
    emscripten::function("percentile_breaks", &percentile_breaks);
    emscripten::function("stddev_breaks", &stddev_breaks);
    emscripten::function("hinge15_breaks", &hinge15_breaks);
    emscripten::function("hinge30_breaks", &hinge30_breaks);

    emscripten::function("excess_risk", &excess_risk);
    emscripten::function("eb_risk", &eb_risk);
    emscripten::function("spatial_lag", &spatial_lag);
    emscripten::function("spatial_rate", &spatial_rate);
    emscripten::function("spatial_eb", &spatial_eb);

    emscripten::function("cartogram", &cartogram);
    emscripten::function("get_centroids", &get_centroids);
    emscripten::function("get_neighbors", &get_neighbors);
    emscripten::function("spatial_count", &spatial_count);
}

# else

int main() {
    std::cout << "print_json" << std::endl;
    std::string file_path = "../data/natregimes.geojson";
    GdaGeojson gda(file_path.c_str());
    geojson_maps["natregimes.geojson"] = &gda;
    WeightsResult r = queen_weights("natregimes.geojson",1,0,0);
    CCentroids c = get_centroids("natregimes.geojson");
    //std::vector<std::string> col_names = {"Crm_prs", "Crm_prp", "Litercy", "Donatns", "Infants", "Suicids"};
    std::vector<std::string> col_names = {"hr60", "po60"};
    //std::vector<std::vector<int> > clt = redcap("natregimes.geojson", r.uid, 4, col_names, "", -1,
    // "firstorder-singlelinkage");
    return 0;
}

#endif


/*
 * Copyright 2018 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <stack>

#include <autoware_planning_msgs/PathPoint.h>
#include <autoware_planning_msgs/TrajectoryPoint.h>
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/MapMetaData.h>

#include <tf2/utils.h>

#include <Eigen/Core>

#include "obstacle_avoidance_planner/spline_interpolate.h"
#include "obstacle_avoidance_planner/util.h"

namespace util
{
template <typename T>
geometry_msgs::Point transformToRelativeCoordinate2D(
  const T & point, const geometry_msgs::Pose & origin)
{
  geometry_msgs::Transform origin_coord2point;
  origin_coord2point.translation.x = point.x;
  origin_coord2point.translation.y = point.y;
  tf2::Transform tf_origin_coord2point;
  tf2::fromMsg(origin_coord2point, tf_origin_coord2point);

  geometry_msgs::Transform origin_coord2origin;
  origin_coord2origin.translation.x = origin.position.x;
  origin_coord2origin.translation.y = origin.position.y;
  origin_coord2origin.rotation = origin.orientation;
  tf2::Transform tf_origin_coord2origin;
  tf2::fromMsg(origin_coord2origin, tf_origin_coord2origin);

  tf2::Transform tf_origin2origin_coord = tf_origin_coord2origin.inverse() * tf_origin_coord2point;
  geometry_msgs::Pose rel_pose;
  tf2::toMsg(tf_origin2origin_coord, rel_pose);
  geometry_msgs::Point relative_p = rel_pose.position;
  return relative_p;
}
template geometry_msgs::Point transformToRelativeCoordinate2D<geometry_msgs::Point>(
  const geometry_msgs::Point &, const geometry_msgs::Pose & origin);
template geometry_msgs::Point transformToRelativeCoordinate2D<geometry_msgs::Point32>(
  const geometry_msgs::Point32 &, const geometry_msgs::Pose & origin);

geometry_msgs::Point transformToAbsoluteCoordinate2D(
  const geometry_msgs::Point & point, const geometry_msgs::Pose & origin)
{
  geometry_msgs::Transform origin2point;
  origin2point.translation.x = point.x;
  origin2point.translation.y = point.y;
  tf2::Transform tf_origin2point;
  tf2::fromMsg(origin2point, tf_origin2point);

  geometry_msgs::Transform origin_coord2origin;
  origin_coord2origin.translation.x = origin.position.x;
  origin_coord2origin.translation.y = origin.position.y;
  origin_coord2origin.rotation = origin.orientation;
  tf2::Transform tf_origin_coord2origin;
  tf2::fromMsg(origin_coord2origin, tf_origin_coord2origin);
  tf2::Transform tf_origin_coord2point = tf_origin_coord2origin * tf_origin2point;

  geometry_msgs::Pose abs_pose;
  tf2::toMsg(tf_origin_coord2point, abs_pose);
  geometry_msgs::Point abs_p = abs_pose.position;
  return abs_p;
}

double calculate2DDistance(const geometry_msgs::Point & a, const geometry_msgs::Point & b)
{
  const double dx = a.x - b.x;
  const double dy = a.y - b.y;
  return std::sqrt(dx * dx + dy * dy);
}

double calculateSquaredDistance(const geometry_msgs::Point & a, const geometry_msgs::Point & b)
{
  const double dx = a.x - b.x;
  const double dy = a.y - b.y;
  return dx * dx + dy * dy;
}

double getYawFromPoints(const geometry_msgs::Point & a, const geometry_msgs::Point & a_root)
{
  const double dx = a.x - a_root.x;
  const double dy = a.y - a_root.y;
  return std::atan2(dy, dx);
}

double normalizeRadian(const double angle)
{
  double n_angle = std::fmod(angle, 2 * M_PI);
  n_angle = n_angle > M_PI ? n_angle - 2 * M_PI : n_angle < -M_PI ? 2 * M_PI + n_angle : n_angle;
  return n_angle;
}

geometry_msgs::Quaternion getQuaternionFromPoints(
  const geometry_msgs::Point & a, const geometry_msgs::Point & a_root)
{
  const double roll = 0;
  const double pitch = 0;
  const double yaw = util::getYawFromPoints(a, a_root);
  tf2::Quaternion quaternion;
  quaternion.setRPY(roll, pitch, yaw);
  return tf2::toMsg(quaternion);
}

template <typename T>
geometry_msgs::Point transformMapToImage(
  const T & map_point, const nav_msgs::MapMetaData & occupancy_grid_info)
{
  geometry_msgs::Point relative_p =
    transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin);
  double resolution = occupancy_grid_info.resolution;
  double map_y_height = occupancy_grid_info.height;
  double map_x_width = occupancy_grid_info.width;
  double map_x_in_image_resolution = relative_p.x / resolution;
  double map_y_in_image_resolution = relative_p.y / resolution;
  geometry_msgs::Point image_point;
  image_point.x = map_y_height - map_y_in_image_resolution;
  image_point.y = map_x_width - map_x_in_image_resolution;
  return image_point;
}
template geometry_msgs::Point transformMapToImage<geometry_msgs::Point>(
  const geometry_msgs::Point &, const nav_msgs::MapMetaData & map_info);
template geometry_msgs::Point transformMapToImage<geometry_msgs::Point32>(
  const geometry_msgs::Point32 &, const nav_msgs::MapMetaData & map_info);

std::shared_ptr<geometry_msgs::Point> transformMapToImagePtr(
  const geometry_msgs::Point & map_point, const nav_msgs::MapMetaData & occupancy_grid_info)
{
  geometry_msgs::Point relative_p =
    transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin);
  double resolution = occupancy_grid_info.resolution;
  double map_y_height = occupancy_grid_info.height;
  double map_x_width = occupancy_grid_info.width;
  double map_x_in_image_resolution = relative_p.x / resolution;
  double map_y_in_image_resolution = relative_p.y / resolution;
  double image_x = map_y_height - map_y_in_image_resolution;
  double image_y = map_x_width - map_x_in_image_resolution;
  if (image_x >= 0 && image_x < (int)map_y_height && image_y >= 0 && image_y < (int)map_x_width) {
    geometry_msgs::Point image_point;
    image_point.x = image_x;
    image_point.y = image_y;
    std::shared_ptr<geometry_msgs::Point> image_point_ptr;
    image_point_ptr = std::make_shared<geometry_msgs::Point>(image_point);
    return image_point_ptr;
  } else {
    return nullptr;
  }
}

bool transformMapToImage(
  const geometry_msgs::Point & map_point, const nav_msgs::MapMetaData & occupancy_grid_info,
  geometry_msgs::Point & image_point)
{
  geometry_msgs::Point relative_p =
    transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin);
  const double map_y_height = occupancy_grid_info.height;
  const double map_x_width = occupancy_grid_info.width;
  const double scale = 1 / occupancy_grid_info.resolution;
  const double map_x_in_image_resolution = relative_p.x * scale;
  const double map_y_in_image_resolution = relative_p.y * scale;
  const double image_x = map_y_height - map_y_in_image_resolution;
  const double image_y = map_x_width - map_x_in_image_resolution;
  if (image_x >= 0 && image_x < (int)map_y_height && image_y >= 0 && image_y < (int)map_x_width) {
    image_point.x = image_x;
    image_point.y = image_y;
    return true;
  } else {
    return false;
  }
}

bool interpolate2DPoints(
  const std::vector<double> & base_x, const std::vector<double> & base_y, const double resolution,
  std::vector<geometry_msgs::Point> & interpolated_points)
{
  if (base_x.empty() || base_y.empty()) {
    return false;
  }
  std::vector<double> base_s = spline::calcEuclidDist(base_x, base_y);
  if (base_s.empty()) {
    return false;
  }
  std::vector<double> new_s;
  for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) {
    new_s.push_back(i);
  }
  // new_s.push_back(base_s.back());
  spline::SplineInterpolate spline;
  std::vector<double> interpolated_x;
  std::vector<double> interpolated_y;
  spline.interpolate(base_s, base_x, new_s, interpolated_x);
  spline.interpolate(base_s, base_y, new_s, interpolated_y);
  for (size_t i = 0; i < interpolated_x.size(); i++) {
    geometry_msgs::Point point;
    point.x = interpolated_x[i];
    point.y = interpolated_y[i];
    interpolated_points.push_back(point);
  }
}

std::vector<geometry_msgs::Point> getInterpolatedPoints(
  const std::vector<geometry_msgs::Pose> & first_points,
  const std::vector<geometry_msgs::Pose> & second_points, const double delta_arc_length)
{
  std::vector<double> tmp_x;
  std::vector<double> tmp_y;
  for (const auto point : first_points) {
    tmp_x.push_back(point.position.x);
    tmp_y.push_back(point.position.y);
  }
  for (const auto & point : second_points) {
    tmp_x.push_back(point.position.x);
    tmp_y.push_back(point.position.y);
  }
  std::vector<geometry_msgs::Point> interpolated_points;
  util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points);
  return interpolated_points;
}

std::vector<geometry_msgs::Point> getInterpolatedPoints(
  const std::vector<geometry_msgs::Pose> & points, const double delta_arc_length)
{
  std::vector<double> tmp_x;
  std::vector<double> tmp_y;
  for (const auto & point : points) {
    tmp_x.push_back(point.position.x);
    tmp_y.push_back(point.position.y);
  }
  std::vector<geometry_msgs::Point> interpolated_points;
  util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points);
  return interpolated_points;
}

template <typename T>
std::vector<geometry_msgs::Point> getInterpolatedPoints(
  const T & points, const double delta_arc_length)
{
  std::vector<double> tmp_x;
  std::vector<double> tmp_y;
  for (const auto & point : points) {
    tmp_x.push_back(point.pose.position.x);
    tmp_y.push_back(point.pose.position.y);
  }
  std::vector<geometry_msgs::Point> interpolated_points;
  util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points);
  return interpolated_points;
}
template std::vector<geometry_msgs::Point>
getInterpolatedPoints<std::vector<autoware_planning_msgs::TrajectoryPoint>>(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> &, const double);
template std::vector<geometry_msgs::Point>
getInterpolatedPoints<std::vector<autoware_planning_msgs::PathPoint>>(
  const std::vector<autoware_planning_msgs::PathPoint> &, const double);

template <typename T>
int getNearestIdx(
  const T & points, const geometry_msgs::Pose & pose, const int default_idx,
  const double delta_yaw_threshold)
{
  double min_dist = std::numeric_limits<double>::max();
  int nearest_idx = default_idx;
  const double point_yaw = tf2::getYaw(pose.orientation);
  for (int i = 0; i < points.size(); i++) {
    const double dist = calculateSquaredDistance(points[i].pose.position, pose.position);
    const double points_yaw = tf2::getYaw(points[i].pose.orientation);
    const double diff_yaw = points_yaw - point_yaw;
    const double norm_diff_yaw = normalizeRadian(diff_yaw);
    if (dist < min_dist && std::fabs(norm_diff_yaw) < delta_yaw_threshold) {
      min_dist = dist;
      nearest_idx = i;
    }
  }
  return nearest_idx;
}
template int getNearestIdx<std::vector<autoware_planning_msgs::TrajectoryPoint>>(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> &, const geometry_msgs::Pose &,
  const int, const double);
template int getNearestIdx<std::vector<autoware_planning_msgs::PathPoint>>(
  const std::vector<autoware_planning_msgs::PathPoint> &, const geometry_msgs::Pose &, const int,
  const double);

int getNearestIdx(
  const std::vector<geometry_msgs::Point> & points, const geometry_msgs::Pose & pose,
  const int default_idx, const double delta_yaw_threshold)
{
  double min_dist = std::numeric_limits<double>::max();
  int nearest_idx = default_idx;
  const double point_yaw = tf2::getYaw(pose.orientation);
  for (int i = 0; i < points.size(); i++) {
    if (i > 0) {
      const double dist = calculateSquaredDistance(points[i], pose.position);
      const double points_yaw = getYawFromPoints(points[i], points[i - 1]);
      const double diff_yaw = points_yaw - point_yaw;
      const double norm_diff_yaw = normalizeRadian(diff_yaw);
      if (dist < min_dist && std::fabs(norm_diff_yaw) < delta_yaw_threshold) {
        min_dist = dist;
        nearest_idx = i;
      }
    }
  }
  return nearest_idx;
}

int getNearestIdx(
  const std::vector<autoware_planning_msgs::PathPoint> & points, const geometry_msgs::Pose & pose,
  const int default_idx)
{
  double min_dist = std::numeric_limits<double>::max();
  int nearest_idx = default_idx;
  for (int i = 0; i < points.size(); i++) {
    if (i > 0) {
      const double dist = calculateSquaredDistance(points[i - 1].pose.position, pose.position);
      const double points_dx = points[i].pose.position.x - points[i - 1].pose.position.x;
      const double points_dy = points[i].pose.position.y - points[i - 1].pose.position.y;
      const double points2pose_dx = pose.position.x - points[i - 1].pose.position.x;
      const double points2pose_dy = pose.position.y - points[i - 1].pose.position.y;
      const double ip = points_dx * points2pose_dx + points_dy * points2pose_dy;
      if (ip < 0) {
        return nearest_idx;
      }
      if (dist < min_dist && ip > 0) {
        min_dist = dist;
        nearest_idx = i - 1;
      }
    }
  }
  return nearest_idx;
}

std::vector<autoware_planning_msgs::TrajectoryPoint> convertPathToTrajectory(
  const std::vector<autoware_planning_msgs::PathPoint> & path_points)
{
  std::vector<autoware_planning_msgs::TrajectoryPoint> traj_points;
  for (const auto & point : path_points) {
    autoware_planning_msgs::TrajectoryPoint tmp_point;
    tmp_point.pose = point.pose;
    tmp_point.twist = point.twist;
    traj_points.push_back(tmp_point);
  }
  return traj_points;
}

std::vector<std::vector<int>> getHistogramTable(const std::vector<std::vector<int>> & input)
{
  std::vector<std::vector<int>> histogram_table = input;
  for (int i = 0; i < input.size(); i++) {
    for (int j = 0; j < input[i].size(); j++) {
      if (input[i][j]) {
        histogram_table[i][j] = 0;
      } else {
        histogram_table[i][j] = (i > 0) ? histogram_table[i - 1][j] + 1 : 1;
      }
    }
  }
  return histogram_table;
}

struct HistogramBin
{
  int height;
  int variable_pos;
  int original_pos;
};

Rectangle getLargestRectancleInRow(
  const std::vector<int> & histo, const int curret_row, const int row_size)
{
  std::vector<int> search_histo = histo;
  search_histo.push_back(0);
  std::stack<HistogramBin> stack;
  Rectangle largest_rect;
  for (int i = 0; i < search_histo.size(); i++) {
    HistogramBin bin;
    bin.height = search_histo[i];
    bin.variable_pos = i;
    bin.original_pos = i;
    if (stack.empty()) {
      stack.push(bin);
    } else {
      if (stack.top().height < bin.height) {
        stack.push(bin);
      } else if (stack.top().height >= bin.height) {
        int target_i = i;
        while (!stack.empty() && bin.height <= stack.top().height) {
          HistogramBin tmp_bin = stack.top();
          stack.pop();
          int area = (i - tmp_bin.variable_pos) * tmp_bin.height;
          if (area > largest_rect.area) {
            largest_rect.max_y_idx = tmp_bin.variable_pos;
            largest_rect.min_y_idx = i - 1;
            largest_rect.max_x_idx = curret_row - tmp_bin.height + 1;
            largest_rect.min_x_idx = curret_row;
            largest_rect.area = area;
          }

          target_i = tmp_bin.variable_pos;
        }
        bin.variable_pos = target_i;
        stack.push(bin);
      }
    }
  }
  return largest_rect;
}

Rectangle getLargestRectangle(const std::vector<std::vector<int>> & input)
{
  std::vector<std::vector<int>> histogram_table = getHistogramTable(input);
  Rectangle largest_rectangle;
  for (int i = 0; i < histogram_table.size(); i++) {
    Rectangle rect = getLargestRectancleInRow(histogram_table[i], i, input.size());
    if (rect.area > largest_rectangle.area) {
      largest_rectangle = rect;
    }
  }
  return largest_rectangle;
}
}  // namespace util


#pragma once

#include <Eigen/Dense>

namespace Caesar {

class Pose2d {
public:
  Pose2d() {
    position_ << 0.0, 0.0;
    yaw_ = 0.0;
  };

  Pose2d(double x, double y, double yaw) {
    position_ << x, y;
    yaw_ = yaw;
  };

  ~Pose2d(){};

  double x() const {return position_.x();};

  double y() const {return position_.y();};

  double yaw() const { return yaw_;};

private:
  Eigen::Vector2d position_;
  double yaw_;
};
};


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integer_list.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/type.hpp>
#include <boost/hana/type_list.hpp>

#include <type_traits>
using namespace boost::hana;

//! [ints]
BOOST_HANA_CONSTEXPR_LAMBDA auto odd = [](auto x) {
    return x % int_<2> != int_<0>;
};

BOOST_HANA_CONSTANT_ASSERT(
    partition(integer_list<int, 1, 2, 3, 4, 5, 6, 7>, odd)
    ==
    pair(
        integer_list<int, 1, 3, 5, 7>,
        integer_list<int, 2, 4, 6>
    )
);
//! [ints]

//! [types]
BOOST_HANA_CONSTANT_ASSERT(
    partition(type_list<void, int, float, char, double>, trait<std::is_floating_point>)
    ==
    pair(
        type_list<float, double>,
        type_list<void, int, char>
    )
);
//! [types]

int main() { (void)odd; }