//
// Created by squintingsmile on 2020/9/22.
//

#include <cstring>
#include <cmath>
#include <chrono>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <fstream>
#include "LPboxADMMsolver.h"

using namespace Eigen::internal;
using std::abs;
using std::sqrt;
Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols,
		", ", ", ", "", "", " << ", ";");

/* The original function is imported from the Eigen library. Added clocks to monitor times */
void _conjugate_gradient(const SparseMatrix& mat, const DenseVector& rhs, DenseVector& x,
		const Eigen::DiagonalPreconditioner<_double_t>& precond, int& iters, typename DenseVector::RealScalar& tol_error)
{

	typedef typename DenseVector::RealScalar RealScalar;
	typedef typename DenseVector::Scalar Scalar;
	typedef Eigen::Matrix<Scalar,Dynamic,1> VectorType;

	RealScalar tol = tol_error;
	int maxIters = iters;

	int n = mat.cols();

	VectorType residual = rhs - mat * x; //initial residual

	RealScalar rhsNorm2 = rhs.squaredNorm();

	if(rhsNorm2 == 0)
	{
		x.setZero();
		iters = 0;
		tol_error = 0;
		return;
	}

	const RealScalar considerAsZero = (std::numeric_limits<RealScalar>::min)();
	RealScalar threshold = Eigen::numext::maxi(RealScalar(tol*tol*rhsNorm2),considerAsZero);
	RealScalar residualNorm2 = residual.squaredNorm();

	if (residualNorm2 < threshold)
	{
		iters = 0;
		tol_error = sqrt(residualNorm2 / rhsNorm2);
		return;
	}

	VectorType p(n);
	p = precond.solve(residual);      // initial search direction

	VectorType z(n), tmp(n);
	RealScalar absNew = Eigen::numext::real(residual.dot(p));  // the square of the absolute value of r scaled by invM
	int i = 0;
	while(i < maxIters)
	{
		tmp.noalias() = mat * p;                    // the bottleneck of the algorithm

		Scalar alpha = absNew / p.dot(tmp);         // the amount we travel on dir

		x += alpha * p;                             // update solution

		residual -= alpha * tmp;                    // update residual

		residualNorm2 = residual.squaredNorm();

		if(residualNorm2 < threshold) {
			i++;
			break;
		}

		z = precond.solve(residual);                // approximately solve for "A z = residual"

		RealScalar absOld = absNew;
		absNew = Eigen::numext::real(residual.dot(z));     // update the absolute value of r
		RealScalar beta = absNew / absOld;          // calculate the Gram-Schmidt value used to create the new search direction
		p = z + beta * p;                           // update search direction
		i++;
	}
	tol_error = sqrt(residualNorm2 / rhsNorm2);
	iters = i;
	return;
}

/* The .noalias() function explicity states that result of the matrix multiplication can be written
 * on lhs directly without allocating an empty vector first and store the result, then assigning
 * it to lhs. This hopefully will save some time
 */
void mat_mul_vec(const SparseMatrix &mat, const DenseVector &vec, DenseVector& res) {
	if (mat.cols() == mat.rows() || &vec != &res) {
		res.noalias() = mat * vec;
	} else {
		res = mat * vec;
	}
}

/* This is used to avoid direct matrix multiplication. Suppose that we want to calculate A^TAx where A is large
 * so calculating A^TAx is intractable. Then we calculate Ax then calculate A^T(Ax) to reduce the number of operations.
 * In this vector mat_expression, each entry is a vector of sparse matrices. For each entry, the result is calculated by
 * entry[n] ... entry[2] * (entry[1] * (entry[0] * x)) and the results calculated by different entries are sum together
 */
void calculate_mat_expr_multiplication(const std::vector<std::vector<const SparseMatrix*>> &mat_expressions,
		DenseVector &x, DenseVector &result, DenseVector &temp_vec) {

	if (mat_expressions.size() == 0) {
		printf("Empty matrix expression when doing multiplication\n");
		return;
	}

	DenseVector *original_x;
	if (&x == &result && mat_expressions.size() > 1) {
		original_x = new DenseVector(x);
	} else {
		original_x = &x;
	}

	{
		const std::vector<const SparseMatrix*> &expression = mat_expressions[0];
		int counter = 0;
		for (const SparseMatrix *mat : expression) {
			if (counter == 0) {
				mat_mul_vec(*mat, x, result);
			} else {
				mat_mul_vec(*mat, result, result);
			}
			counter += 1;
		}
	}

	if (mat_expressions.size() > 1) {
		for (auto iter = std::next(mat_expressions.begin(), 1); iter != mat_expressions.end(); iter++) {
			int counter = 0;
			for (const SparseMatrix *mat : *iter) {
				if (counter == 0) {
					mat_mul_vec(*mat, *original_x, temp_vec);
				} else {
					mat_mul_vec(*mat, temp_vec, temp_vec);
				}
				counter += 1;
			}

			result += temp_vec;
		}

		if (&x == &result) {
			delete original_x;
		}
	}
}

/* The original function is imported from the Eigen library. Passing the two temporary vectors to save the time of
 * allocating vectors
 */

void _conjugate_gradient(const std::vector<std::vector<const SparseMatrix*>> &mat_expressions, const DenseVector& rhs,
		DenseVector& x, const Eigen::DiagonalPreconditioner<_double_t>& precond, int& iters,
		typename DenseVector::RealScalar& tol_error, DenseVector &temp_vec_for_cg, DenseVector &temp_vec_for_mat_mul) {

	typedef typename DenseVector::RealScalar RealScalar;
	typedef typename DenseVector::Scalar Scalar;
	typedef Eigen::Matrix<Scalar,Dynamic,1> VectorType;

	RealScalar tol = tol_error;
	int maxIters = iters;

	int n = mat_expressions[0][0]->cols();

	calculate_mat_expr_multiplication(mat_expressions, x, temp_vec_for_cg, temp_vec_for_mat_mul);
	VectorType residual = rhs - temp_vec_for_cg; //initial residual

	RealScalar rhsNorm2 = rhs.squaredNorm();

	if(rhsNorm2 == 0) {
		x.setZero();
		iters = 0;
		tol_error = 0;
		return;
	}

	const RealScalar considerAsZero = (std::numeric_limits<RealScalar>::min)();
	RealScalar threshold = Eigen::numext::maxi(RealScalar(tol*tol*rhsNorm2),considerAsZero);
	RealScalar residualNorm2 = residual.squaredNorm();

	if (residualNorm2 < threshold)
	{
		iters = 0;
		tol_error = sqrt(residualNorm2 / rhsNorm2);
		return;
	}
	VectorType p(n);
	p = precond.solve(residual);      // initial search direction

	VectorType z(n), tmp(n);
	RealScalar absNew = Eigen::numext::real(residual.dot(p));  // the square of the absolute value of r scaled by invM
	int i = 0;
	while(i < maxIters)
	{
		calculate_mat_expr_multiplication(mat_expressions, p, tmp, temp_vec_for_mat_mul);

		Scalar alpha = absNew / p.dot(tmp);         // the amount we travel on dir

		x += alpha * p;                             // update solution

		residual -= alpha * tmp;                    // update residual

		residualNorm2 = residual.squaredNorm();

		if(residualNorm2 < threshold) {
			i++;
			break;
		}

		z = precond.solve(residual);                // approximately solve for "A z = residual"

		RealScalar absOld = absNew;
		absNew = Eigen::numext::real(residual.dot(z));     // update the absolute value of r
		RealScalar beta = absNew / absOld;          // calculate the Gram-Schmidt value used to create the new search direction
		p = z + beta * p;                           // update search direction
		i++;
	}
	tol_error = sqrt(residualNorm2 / rhsNorm2);
	iters = i;
	return;
}

_double_t LPboxADMMsolver::std_dev(std::vector<_double_t>& arr, size_t begin, size_t end) {
	_double_t mean = 0;
	_double_t std_deviation = 0;
	size_t size = end - begin;
	for (int i = begin; i < end; i++) {
		mean += arr[i];
	}

	mean /= size;
	for (int i = 0; i < size; i++) {
		std_deviation += (arr[begin + i] - mean) * (arr[begin + i] - mean);
	}

	std_deviation /= size - 1;
	if (std_deviation == 0) {
		return 0;
	}

	return std::pow(std_deviation, 1.0 / 2);
}

void LPboxADMMsolver::project_vec_greater_than(DenseVector &x, DenseVector &res, const int &greater_val, const int &set_val) {
	int len = x.size();
	for (int i = 0; i < len; i++) {
		res[i] = x[i] > greater_val ? set_val : x[i];
	}
}

void LPboxADMMsolver::project_vec_less_than(DenseVector &x, DenseVector &res, const int &less_val, const int &set_val) {
	int len = x.size();
	for (int i = 0; i < len; i++) {
		res(i) = x(i) < less_val ? set_val : x(i);
	}
}

void LPboxADMMsolver::project_box(int n, const double *x, double *y) {
	y = new double[n];
	while (n--) {
		if (x[n] > 1) {
			y[n] = 1;
		} else {
			if (x[n] < 0) {
				y[n] = 0;
			} else {
				y[n] = x[n];
			}
		}
	}
}

/* TODO: Utilize eigen .array() functions to speed up this operation */
void LPboxADMMsolver::project_box(int n, const DenseVector &x, DenseVector &y) {
	while (n--) {
		if (x[n] > 1) {
			y[n] = 1;
		} else {
			if (x[n] < 0) {
				y[n] = 0;
			} else {
				y[n] = x[n];
			}
		}
	}
}

void LPboxADMMsolver::project_shifted_Lp_ball(int n, const DenseVector &x, int p, DenseVector &y) {
	y.array() = x.array() - 0.5;
	_double_t normp_shift = y.norm();

	y.array() = y.array() * std::pow(n, 1.0 / p) / (2 * normp_shift) + 0.5;
}

_double_t LPboxADMMsolver::compute_cost(const DenseVector& x, const SparseMatrix& A,
		const DenseVector& b, DenseVector &temp_vec_for_mat_mul) {
	mat_mul_vec(A, x, temp_vec_for_mat_mul);
	double val = x.transpose().dot(temp_vec_for_mat_mul);
	double val2 = b.dot(x);
	return val + val2;
}

_double_t LPboxADMMsolver::compute_cost(const DenseVector& x, const SparseMatrix& A, const DenseVector& b) {
	double val = x.transpose().dot(A * x);
	double val2 = b.dot(x);
	return val + val2;
}

_double_t LPboxADMMsolver::compute_std_obj(std::vector<_double_t> obj_list, int history_size) {
	size_t s = obj_list.size();
	_double_t std_obj;
	if (s <= history_size) {
		std_obj = std_dev(obj_list, 0, s);
	} else {
		std_obj = std_dev(obj_list, s - history_size, s);
	}

	return std_obj / std::abs(obj_list[s-1]);
}

void LPboxADMMsolver::ADMM_bqp_unconstrained_init() {
	std_threshold = 1e-6;
	gamma_val = 1.0;
	gamma_factor = 0.99;
	initial_rho = 5;
	learning_fact = 1 + 3.0 / 100;
	rho_upper_limit = 1000;
	history_size = 5;
	rho_change_step = 5;
	rel_tol = 1e-5;
	stop_threshold = 1e-3;
	max_iters = 1e4;
	projection_lp = 2;
	pcg_tol = 1e-3;
	pcg_maxiters = 1e3;
}

void LPboxADMMsolver::ADMM_bqp_linear_eq_init() {
	stop_threshold = 1e-4;
	std_threshold = 1e-6;
	gamma_val = 1.6;
	gamma_factor = 0.95;
	rho_change_step = 5;
	max_iters = 5e3;
	initial_rho = 1;
	history_size = 3;
	learning_fact = 1 + 5.0 / 100;
	pcg_tol = 1e-4;
	pcg_maxiters = 1e3;
	rel_tol = 5e-5;
	projection_lp=2;
}

void LPboxADMMsolver::ADMM_bqp_linear_ineq_init() {
	stop_threshold = 1e-4;
	std_threshold = 1e-6;
	gamma_val = 1.6;
	gamma_factor = 0.95;
	rho_change_step = 5;
	max_iters = 1e4;
	initial_rho = 25;
	history_size = 3;
	learning_fact = 1 + 1.0 / 100;
	pcg_tol = 1e-4;
	pcg_maxiters = 1e3;
	rel_tol = 5e-5;
	projection_lp = 2;

}

void LPboxADMMsolver::ADMM_bqp_linear_eq_and_uneq_init() {
	stop_threshold = 1e-4;
	std_threshold = 1e-6;
	gamma_val = 1.6;
	gamma_factor = 0.95;
	rho_change_step = 5;
	max_iters = 1e4;
	initial_rho = 25;
	history_size = 3;
	learning_fact = 1 + 1.0 / 100;
	pcg_tol = 1e-4;
	pcg_maxiters = 1e3;
	rel_tol = 5e-5;
	projection_lp = 2;
}

int LPboxADMMsolver::ADMM_bqp(MatrixInfo matrix_info, SolverInstruction instruction,
		Solution& sol) {

	const SparseMatrix *A_ptr, *C_ptr, *E_ptr;
	const DenseVector *b_ptr, *d_ptr, *f_ptr;

	A_ptr = matrix_info.A;
	b_ptr = matrix_info.b;

	SparseMatrix C_transpose, E_transpose, _2A_plus_rho1_rho2, rho3_C_transpose, rho4_E_transpose;

	DenseVector y3, z3, z4, Csq_diag, Esq_diag;

	int l = matrix_info.l;
	int m = matrix_info.m;
	int n = matrix_info.n;

	FILE *fp;
	if (does_log) {
		fp = fopen(log_file_path.c_str(), "w+");
	}

	DenseVector x_sol    = DenseVector(n);
	DenseVector y1       = DenseVector(n);
	DenseVector y2       = DenseVector(n);
	DenseVector z1       = DenseVector(n);
	DenseVector z2       = DenseVector(n);
	DenseVector prev_idx = DenseVector(n);
	DenseVector best_sol = DenseVector(n);
	DenseVector temp_vec = DenseVector(n);
	DenseVector cur_idx  = DenseVector(n);
	SparseMatrix temp_mat = SparseMatrix(n, n);

	/* The temp_vec_for_cg and temp_vec_for_mat_mat are vector that used to store the temporary vectors
	 * in the algorithm to prevent allocating new vectos. Adding this shall has around 5% of performance
	 * improvement (although might reduce the performance under some tasks by 10% since it increases the cost
	 * of other operations besides matrix multiplication, such as vector addition and vector multiplication
	 * for reasons unknown)
	 */
	DenseVector temp_vec_for_cg = DenseVector(n);
	DenseVector temp_vec_for_mat_mul = DenseVector(n);
	_double_t cur_obj;
	bool rhoUpdated = true;

	x_sol = *(matrix_info.x0);

	z1.array() = 0;
	z2.array() = 0;
	cur_idx.array() = 0;

	Eigen::DiagonalPreconditioner<_double_t> diagonalPreconditioner;

	_double_t rho1 = initial_rho;
	_double_t rho2 = initial_rho;
	_double_t rho3 = initial_rho;
	_double_t rho4 = initial_rho;
	_double_t prev_rho1 = rho1;
	_double_t prev_rho2 = rho2;
	_double_t prev_rho3 = rho3;
	_double_t prev_rho4 = rho4;

	std::vector<_double_t> obj_list; /* Stores the objective value calculated during each iteration */
	_double_t std_obj = 1;

	_double_t cvg_test1;
	_double_t cvg_test2;
	_double_t rho_change_ratio;

	if (instruction.update_y3) {
		y3 = DenseVector(l);
	}

	if (instruction.update_z3) {
		z3 = DenseVector(m);
		z3.array() = 0;
	}

	if (instruction.update_z4) {
		z4 = DenseVector(l);
		z4.array() = 0;
	}

	/* If this task contains equality constraints, initialize C and d and relevant matrices */
	if (instruction.problem_type & equality) {
		C_ptr = matrix_info.C;
		C_transpose = (*C_ptr).transpose();
		rho3_C_transpose = rho3 * C_transpose;
		d_ptr = matrix_info.d;
	}

	/* If this task contains inequality constraints, initialize E and f and relevant matrices */
	if (instruction.problem_type & inequality) {
		E_ptr = matrix_info.E;
		E_transpose = (*E_ptr).transpose();
		rho4_E_transpose = rho4 * E_transpose;
		f_ptr = matrix_info.f;
	}

	/* Storing the matrix 2 * A + (rho1 + rho2) * I to save calculation time */
	_2A_plus_rho1_rho2 = 2 * (*A_ptr);
	_2A_plus_rho1_rho2.diagonal().array() += rho1 + rho2;

	SparseMatrix preconditioner_diag_mat(n, n); /* The diagonal matrix used by the preconditioner */
	preconditioner_diag_mat.reserve(n);
	std::vector<Triplet> preconditioner_diag_mat_triplets;
	preconditioner_diag_mat_triplets.reserve(n);
	/* The diagonal elements in the original expression evaluated by the preconditioner is given by the
	 * diagonal elements of 2 * _A + (rho1 + rho2) * I + rho3 * _C^T * _C
	 */
	for (int i = 0; i < n; i++) {
		preconditioner_diag_mat_triplets.push_back(Triplet(i, i, 0));
	}
	preconditioner_diag_mat.setFromTriplets(preconditioner_diag_mat_triplets.begin(), preconditioner_diag_mat_triplets.end());
	preconditioner_diag_mat.diagonal().array() = _2A_plus_rho1_rho2.diagonal().array();
	preconditioner_diag_mat.makeCompressed();

	/* The matrix expression for the unconstraint case is 2 * _A + (rho1 + rho2) * I */
	std::vector<std::vector<const SparseMatrix*>> matrix_expressions;
	matrix_expressions.emplace_back();
	matrix_expressions.back().push_back(&_2A_plus_rho1_rho2);

	/* If the problem has equality constraint, add rho3 * C^T * C to the matrix expression */
	if (instruction.problem_type & equality) {
		matrix_expressions.emplace_back();
		matrix_expressions.back().push_back(C_ptr);
		matrix_expressions.back().push_back(&rho3_C_transpose);

		/* Calculating the diagonal elements of Csq for preconditioner */
		Csq_diag = DenseVector(n);
		Csq_diag.setZero();
		for(int j = 0; j < C_transpose.outerSize(); ++j) {
			typename SparseMatrix::InnerIterator it(C_transpose, j);
			while (it) {
				if(it.value() != 0.0) {
					Csq_diag[j] += it.value() * it.value();
				}
				++it;
			}
		}
		preconditioner_diag_mat.diagonal().array() += rho3 * Csq_diag.array();
	}

	/* If the problem has equality constraint, add rho4 * E^T * E to the matrix expression */
	if (instruction.problem_type &inequality) {
		matrix_expressions.emplace_back();
		matrix_expressions.back().push_back(E_ptr);
		matrix_expressions.back().push_back(&rho4_E_transpose);

		/* Calculating the diagonal elements of Esq for preconditioner */
		Esq_diag = DenseVector(n);
		Esq_diag.setZero();
		for(int j = 0; j < E_transpose.outerSize(); ++j) {
			typename SparseMatrix::InnerIterator it(E_transpose, j);
			while (it) {
				if(it.value() != 0.0) {
					Esq_diag[j] += it.value() * it.value();
				}
				++it;
			}
		}
		preconditioner_diag_mat.diagonal().array() += rho4 * Esq_diag.array();
	}

	y1 = x_sol;
	y2 = x_sol;
	if (instruction.update_y3) {
		y3 = *f_ptr - *E_ptr * x_sol;
	}

	prev_idx = (x_sol.array() >= 0.5).matrix().cast<_double_t>();
	best_sol = x_sol;

	_double_t best_bin_obj = compute_cost(x_sol, *A_ptr, *b_ptr, temp_vec_for_mat_mul);

	if (does_log) {
		fprintf(fp, "Initial state\n");
        fprintf(fp, "norm of x_sol: %lf\n", x_sol.norm());
        fprintf(fp, "norm of b: %lf\n", (*b_ptr).norm());
        fprintf(fp, "norm of y1: %lf\n", y1.norm());
        fprintf(fp, "norm of y2: %lf\n", y2.norm());
        if (instruction.update_y3) {
            fprintf(fp, "norm of y3: %lf\n", y3.norm());
        }

        fprintf(fp, "norm of z1: %lf\n", z1.norm());
        fprintf(fp, "norm of z2: %lf\n", z2.norm());

        if (instruction.update_z3) {
            fprintf(fp, "norm of z3: %lf\n", z3.norm());
        }

        if (instruction.update_z4) {
            fprintf(fp, "norm of z4: %lf\n", z4.norm());
        }

        fprintf(fp, "norm of cur_idx: %lf\n", cur_idx.norm());
        fprintf(fp, "rho1: %lf\n", rho1);
        fprintf(fp, "rho2: %lf\n", rho2);
        fprintf(fp, "rho3: %lf\n", rho3);
        fprintf(fp, "rho4: %lf\n", rho4);
        fprintf(fp, "-------------------------------------------------\n");
	}

	std::chrono::steady_clock::time_point start, end;
	start = std::chrono::steady_clock::now();

	long time_elapsed = 0;
	for (int iter = 0; iter < max_iters; iter++) {
		if (does_log) {
			fprintf(fp, "Iteration: %d\n", iter);
		}
		temp_vec = x_sol + z1 / rho1;

		/* Project vector on [0, 1] box */
		project_box(n, temp_vec, y1);

		temp_vec = x_sol + z2 / rho2;

		/* Project vector on shifted lp box */
		project_shifted_Lp_ball(n, temp_vec, projection_lp, y2);

		if (instruction.update_y3) {
			mat_mul_vec(*E_ptr, x_sol, temp_vec_for_mat_mul);
			y3 = *f_ptr - temp_vec_for_mat_mul - z4 / rho4;
			project_vec_less_than(y3, y3, 0, 0);
		}

		/* If the iteration is nonzero and it divides rho_change_step, it means
		 * that the rho updated in the last iteration
		 */
		if (iter != 0 && rhoUpdated) {
			/* Note that we need the previous rho to update in order to get the difference between
			 * the updated matrix and the not updated matrix. Another possible calculation is by
			 * calculating rho * rho_change_ration / learning_fact
			 */
			_2A_plus_rho1_rho2.diagonal().array() += rho_change_ratio * (prev_rho1 + prev_rho2);
			if (instruction.problem_type != unconstrained) {
				preconditioner_diag_mat.diagonal().array() += rho_change_ratio * (prev_rho1 + prev_rho2);
			}

			if (instruction.update_rho3) {
				preconditioner_diag_mat.diagonal().array() += rho_change_ratio * prev_rho3 * Csq_diag.array();
				rho3_C_transpose = learning_fact * rho3_C_transpose;
			}

			if (instruction.update_rho4) {
				preconditioner_diag_mat.diagonal().array() += rho_change_ratio * prev_rho4 * Esq_diag.array();
				rho4_E_transpose = learning_fact * rho4_E_transpose;
			}
		}

		/* If the problem in unconstrained, the rhs vector is
		 * rho1 * y1 + rho2 * y2 - (b + z1 + z2)
		 */
		if (instruction.problem_type == unconstrained) {
			temp_vec = rho1 * y1 + rho2 * y2 - (*b_ptr + z1 + z2);
		}

		/* If the problem in equality, the rhs vector is
		 * rho1 * y1 + rho2 * y2 + rho3 * C^T * d - (b + z1 + z2 + C^T * z3)
		 */
		if (instruction.problem_type == equality) {
			temp_vec = rho1 * y1 + rho2 * y2 - (*b_ptr + z1 + z2);
			mat_mul_vec(rho3_C_transpose, *d_ptr, temp_vec_for_mat_mul);
			temp_vec += temp_vec_for_mat_mul;
			mat_mul_vec(C_transpose, z3, temp_vec_for_mat_mul);
			temp_vec -= temp_vec_for_mat_mul;
		}

		/* If the problem in equality, the rhs vector is
		 * rho1 * y1 + rho2 * y2 + rho4 * E^T * (f - y3) - (b + z1 + z2 + E^T * z4)
		 */
		if (instruction.problem_type == inequality) {
			temp_vec = rho1 * y1 + rho2 * y2 - (*b_ptr + z1 + z2);
			mat_mul_vec(rho4_E_transpose, *f_ptr - y3, temp_vec_for_mat_mul);
			temp_vec += temp_vec_for_mat_mul;
			mat_mul_vec(E_transpose, z4, temp_vec_for_mat_mul);
			temp_vec -= temp_vec_for_mat_mul;
		}

		/* If the problem in equality, the rhs vector is
		 * rho1 * y1 + rho2 * y2 + rho3 * C^T * d + rho4 * E^T * (f - y3) - (b + z1 + z2 + C^T * z3 + E^T * z4)
		 */
		if (instruction.problem_type == equality_and_inequality) {
			temp_vec = rho1 * y1 + rho2 * y2 - (*b_ptr + z1 + z2);
			mat_mul_vec(rho3_C_transpose, *d_ptr, temp_vec_for_mat_mul);
			temp_vec += temp_vec_for_mat_mul;
			mat_mul_vec(C_transpose, z3, temp_vec_for_mat_mul);
			temp_vec -= temp_vec_for_mat_mul;
			mat_mul_vec(rho4_E_transpose, *f_ptr - y3, temp_vec_for_mat_mul);
			temp_vec += temp_vec_for_mat_mul;
			mat_mul_vec(E_transpose, z4, temp_vec_for_mat_mul);
			temp_vec -= temp_vec_for_mat_mul;
		}

		/* Explicit version of conjugate gradient used for profiling */
		if (rhoUpdated) {
			if (instruction.problem_type != unconstrained) {
				diagonalPreconditioner.compute(preconditioner_diag_mat);
			} else {
				diagonalPreconditioner.compute(_2A_plus_rho1_rho2);
			}
			rhoUpdated = false;
		}
		_double_t tol = pcg_tol;
		x_sol = y1;
		int maxiter = pcg_maxiters;
		_conjugate_gradient(matrix_expressions, temp_vec, x_sol, diagonalPreconditioner,
				maxiter, tol, temp_vec_for_cg, temp_vec_for_mat_mul);

		if (does_log) {
			fprintf(fp, "Conjugate gradient stops after %d iterations\n", maxiter);
			fprintf(fp, "Conjugate gradient stops with residual %lf\n", tol);
		}

		z1 = z1 + gamma_val * rho1 * (x_sol - y1);
		z2 = z2 + gamma_val * rho2 * (x_sol - y2);
		if (instruction.update_z3) {
			z3 = z3 + gamma_val * rho3 * (*C_ptr * x_sol - *d_ptr);
		}

		if (instruction.update_z4) {
			z4 = z4 + gamma_val * rho4 * ((*E_ptr) * x_sol + y3 - (*f_ptr));
		}

		_double_t temp0 = std::max(x_sol.norm(), _double_t(2.2204e-16));
		cvg_test1 = (x_sol - y1).norm() / temp0;
		cvg_test2 = (x_sol - y2).norm() / temp0;
		if (cvg_test1 <= stop_threshold && cvg_test2 <= stop_threshold) {
			printf("iter: %d, stop_threshold: %.6f\n", iter, std::max(cvg_test1, cvg_test2));
			if (does_log) {
				fprintf(fp, "iter: %d, stop_threshold: %.6f\n", iter, std::max(cvg_test1, cvg_test2));
			}
			break;
		}

		if ((iter+1) % rho_change_step == 0) {
			prev_rho1 = rho1;
			prev_rho2 = rho2;
			rho1 = learning_fact * rho1;
			rho2 = learning_fact * rho2;

			if (instruction.update_rho3) {
				prev_rho3 = rho3;
				rho3 = learning_fact * rho3;
			}

			if (instruction.update_rho4) {
				prev_rho4 = rho4;
				rho4 = learning_fact * rho4;
			}

			gamma_val = std::max(gamma_val * gamma_factor, _double_t(1.0));
			rhoUpdated = true;
			rho_change_ratio = learning_fact - 1.0;
		}

		_double_t obj_val = compute_cost(x_sol, *A_ptr, *b_ptr);
		obj_list.push_back(obj_val);
		if (obj_list.size() >= history_size) {
			std_obj = compute_std_obj(obj_list, history_size);
		}
		if (std_obj <= std_threshold) {
			if (does_log) {
				fprintf(fp, "iter: %d, std_threshold: %.6f\n", iter, std_obj);
			}
			printf("iter: %d, std_threshold: %.6f\n", iter, std_obj);
			break;
		}

		cur_idx = (x_sol.array() >= 0.5).matrix().cast<_double_t>();
		prev_idx = cur_idx;
		cur_obj = compute_cost(prev_idx, *A_ptr, *b_ptr);

		if (best_bin_obj >= cur_obj) {
			best_bin_obj = cur_obj;
			best_sol = x_sol;
		}

		if (does_log) {
			fprintf(fp, "current objective: %lf\n", obj_val);
			fprintf(fp, "current binary objective: %lf\n", cur_obj);

			if (instruction.problem_type == equality || instruction.problem_type == equality_and_inequality) {
				fprintf(fp, "equality constraint violation: %lf\n", (*matrix_info.C * cur_idx - *matrix_info.d).norm() / x_sol.rows());
			}

			if (instruction.problem_type == inequality || instruction.problem_type == equality_and_inequality) {
				DenseVector diff = *matrix_info.E * cur_idx - *matrix_info.f;
				project_vec_less_than(diff, diff, 0, 0);
				fprintf(fp, "inequality constraint violation: %lf\n", diff.norm() / x_sol.rows());
			}

			fprintf(fp, "norm of x_sol: %lf\n", x_sol.norm());
			fprintf(fp, "norm of y1: %lf\n", y1.norm());
			fprintf(fp, "norm of y2: %lf\n", y2.norm());
			if (instruction.update_y3) {
				fprintf(fp, "norm of y3: %lf\n", y3.norm());
			}

			fprintf(fp, "norm of z1: %lf\n", z1.norm());
			fprintf(fp, "norm of z2: %lf\n", z2.norm());

			if (instruction.update_z3) {
				fprintf(fp, "norm of z3: %lf\n", z3.norm());
			}

			if (instruction.update_z4) {
				fprintf(fp, "norm of z4: %lf\n", z4.norm());
			}

			fprintf(fp, "norm of cur_idx: %lf\n", cur_idx.norm());
			fprintf(fp, "rho1: %lf\n", rho1);
			fprintf(fp, "rho2: %lf\n", rho2);
			if (instruction.update_rho3) {
				fprintf(fp, "rho3: %lf\n", rho3);
			}
			if (instruction.update_rho4) {
				fprintf(fp, "rho4: %lf\n", rho4);
			}
			fprintf(fp, "-------------------------------------------------\n");
		}
	}

	sol.x_sol = new DenseVector(x_sol);
	sol.y1 = new DenseVector(y1);
	sol.y2 = new DenseVector(y2);
	sol.best_sol = new DenseVector(best_sol);

	end = std::chrono::steady_clock::now();
	time_elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
	std::cout << "Time elapsed: " << time_elapsed << "us" << std::endl;
	if (does_log) {
		fprintf(fp, "Time elapsed: %ldus\n", time_elapsed);
		fclose(fp);
	}
	sol.time_elapsed = time_elapsed;

	return 1;
}

int LPboxADMMsolver::ADMM_bqp_unconstrained(int n, const SparseMatrix &_A, const DenseVector &_b,
		const DenseVector &x0, Solution& sol) {

	SolverInstruction solver_instruction;
	MatrixInfo matrix_info;

	matrix_info.x0 = &x0;
	matrix_info.A = &_A;
	matrix_info.b = &_b;
	matrix_info.n = n;

	solver_instruction.problem_type = unconstrained;
	solver_instruction.update_y3 = 0;
	solver_instruction.update_z3 = 0;
	solver_instruction.update_z4 = 0;
	solver_instruction.update_rho3 = 0;
	solver_instruction.update_rho4 = 0;

	ADMM_bqp(matrix_info, solver_instruction, sol);
	return 1;
}

int LPboxADMMsolver::ADMM_bqp_unconstrained(int n, _double_t *A, _double_t *b, _double_t *x0,
		Solution& sol) {
	/* Initialize the parameters */
	auto _A = SparseMatrix(n, n);
	auto _b = DenseVector(n);
	auto _x0 = DenseVector(n);

	/* Generating the sparse matrix for A */
	std::vector<Triplet> triplet_list;
	for (int i = 0; i < n * n; i++) {
		int row = i % n;
		int col = i / n;

		if (A[i] == 0.0 && !(row == col)) {
			continue;
		}
		triplet_list.push_back(Triplet(row, col, A[i]));
	}
	_A.setFromTriplets(triplet_list.begin(), triplet_list.end());
	memcpy(_b.data(), b, n * sizeof(_double_t));
	memcpy(_x0.data(), x0, n * sizeof(_double_t));

	int ret = ADMM_bqp_unconstrained(n, _A, _b, _x0, sol);
	return ret;
}

int LPboxADMMsolver::ADMM_bqp_linear_eq(int n, const SparseMatrix &_A, const DenseVector &_b,
		const DenseVector &x0, int m, const SparseMatrix &_C, const DenseVector &_d, Solution& sol) {

	SolverInstruction solver_instruction;
	MatrixInfo matrix_info;

	matrix_info.x0 = &x0;
	matrix_info.A = &_A;
	matrix_info.b = &_b;
	matrix_info.n = n;
	matrix_info.C = &_C;
	matrix_info.d = &_d;
	matrix_info.m = m;

	solver_instruction.problem_type = equality;
	solver_instruction.update_y3 = 0;
	solver_instruction.update_z3 = 1;
	solver_instruction.update_z4 = 0;

	/* If this value is set to be true, then the admm update will update
     * rho3 at each iteration with rate learning_fact. The default setting
     * is false, following the strategy in the clustering task */
	solver_instruction.update_rho3 = 0;
	solver_instruction.update_rho4 = 0;

	ADMM_bqp(matrix_info, solver_instruction, sol);
	return 1;
}

int LPboxADMMsolver::ADMM_bqp_linear_eq(int n, _double_t *A, _double_t *b, _double_t *x0, int m,
		_double_t *C, _double_t *d, Solution& sol) {
	/* Initialize the parameters */
	auto _A = SparseMatrix(n, n);
	auto _b = DenseVector(n);
	auto _x0 = DenseVector(n);
	auto _C = SparseMatrix(m, n);
	auto _d = DenseVector(m);

	/* Generating the sparse matrix for A */
	std::vector<Triplet> triplet_list; /* Maybe one can reserve some space here */
	for (int i = 0; i < n * n; i++) {
		if (A[i] == 0.0) {
			continue;
		}
		int row = i % n;
		int col = i / n;
		triplet_list.push_back(Triplet(row, col, A[i]));
	}
	_A.setFromTriplets(triplet_list.begin(), triplet_list.end());

	/* Generating the sparse matrix for C */
	triplet_list.clear();
	for (int i = 0; i < m * n; i++) {
		if (C[i] == 0.0) {
			continue;
		}
		int row = i % n;
		int col = i / n;
		triplet_list.push_back(Triplet(row, col, C[i]));
	}
	_C.setFromTriplets(triplet_list.begin(), triplet_list.end());

	memcpy(_x0.data(), x0, n * sizeof(_double_t));
	memcpy(_b.data(), b, n * sizeof(_double_t));
	memcpy(_d.data(), d, m * sizeof(_double_t));

	int ret = ADMM_bqp_linear_eq(n, _A, _b, _x0, m, _C, _d, sol);
	return ret;
}

int LPboxADMMsolver::ADMM_bqp_linear_ineq(int n, const SparseMatrix &_A, const DenseVector &_b,
		const DenseVector &x0, int l, const SparseMatrix &_E, const DenseVector &_f, Solution& sol) {

	SolverInstruction solver_instruction;
	MatrixInfo matrix_info;

	matrix_info.x0 = &x0;
	matrix_info.A = &_A;
	matrix_info.b = &_b;
	matrix_info.n = n;
	matrix_info.E = &_E;
	matrix_info.f = &_f;
	matrix_info.l = l;

	solver_instruction.problem_type = inequality;
	solver_instruction.update_y3 = 1;
	solver_instruction.update_z3 = 0;
	solver_instruction.update_z4 = 1;
	solver_instruction.update_rho3 = 0;
	solver_instruction.update_rho4 = 1;

	ADMM_bqp(matrix_info, solver_instruction, sol);
	return 1;
}

int LPboxADMMsolver::ADMM_bqp_linear_ineq(int n, _double_t *A, _double_t *b, _double_t *x0,
		int l, _double_t *E, _double_t *f, Solution& sol) {
	/* Initialize the parameters */
	auto _A       = SparseMatrix(n, n);
	auto _b       = DenseVector(n);
	auto _x0	  = DenseVector(n);
	auto _E       = SparseMatrix(l, n);
	auto _f       = DenseVector(l);

	/* Generating the sparse matrix for A */
	std::vector<Triplet> triplet_list; /* Maybe one can reserve some space here */
	for (int i = 0; i < n * n; i++) {
		if (A[i] == 0.0) {
			continue;
		}
		int row = i % n;
		int col = i / n;
		triplet_list.push_back(Triplet(row, col, A[i]));
	}
	_A.setFromTriplets(triplet_list.begin(), triplet_list.end());

	/* Generating the sparse matrix for C */
	triplet_list.clear();
	for (int i = 0; i < l * n; i++) {
		if (E[i] == 0.0) {
			continue;
		}
		int row = i % n;
		int col = i / n;
		triplet_list.push_back(Triplet(row, col, E[i]));
	}
	_E.setFromTriplets(triplet_list.begin(), triplet_list.end());

	memcpy(_x0.data(), x0, n * sizeof(_double_t));
	memcpy(_b.data(), b, n * sizeof(_double_t));
	memcpy(_f.data(), f, l * sizeof(_double_t));

	int ret = ADMM_bqp_linear_ineq(n, _A, _b, _x0, l, _E, _f, sol);
	return ret;
}

int LPboxADMMsolver::ADMM_bqp_linear_eq_and_uneq(int n, const SparseMatrix &_A,
		const DenseVector &_b, const DenseVector &x0, int m, const SparseMatrix &_C, const DenseVector &_d,
		int l, const SparseMatrix &_E, const DenseVector &_f, Solution& sol) {

	SolverInstruction solver_instruction;
	MatrixInfo matrix_info;

	matrix_info.x0 = &x0;
	matrix_info.A = &_A;
	matrix_info.b = &_b;
	matrix_info.n = n;
	matrix_info.C = &_C;
	matrix_info.d = &_d;
	matrix_info.m = m;
	matrix_info.E = &_E;
	matrix_info.f = &_f;
	matrix_info.l = l;

	solver_instruction.problem_type = equality_and_inequality;
	solver_instruction.update_y3 = 1;
	solver_instruction.update_z3 = 1;
	solver_instruction.update_z4 = 1;
	solver_instruction.update_rho3 = 1;
	solver_instruction.update_rho4 = 1;

	ADMM_bqp(matrix_info, solver_instruction, sol);
	return 1;

}

/* Currently assuming that the input matrix are column majored */
int LPboxADMMsolver::ADMM_bqp_linear_eq_and_uneq(int n, _double_t *A, _double_t *b,
		_double_t *x0, int m, _double_t *C, _double_t *d, int l, _double_t *E, _double_t *f, Solution& sol) {
	/* Initialize the parameters */
	auto _A       = SparseMatrix(n, n);
	auto _b       = DenseVector(n);
	auto _x0      = DenseVector(n);
	auto _C       = SparseMatrix(m, n);
	auto _d       = DenseVector(m);
	auto _E       = SparseMatrix(l, n);
	auto _f       = DenseVector(l);

	/* Generating the sparse matrix for A */

	std::vector<Triplet> triplet_list; /* Maybe one can reserve some space here */
	for (int i = 0; i < n * n; i++) {
		if (A[i] == 0.0) {
			continue;
		}
		int row = i % n;
		int col = i / n;
		triplet_list.push_back(Triplet(row, col, A[i]));
	}
	_A.setFromTriplets(triplet_list.begin(), triplet_list.end());

	/* Generating the sparse matrix for C */
	triplet_list.clear();
	for (int i = 0; i < m * n; i++) {
		if (E[i] == 0.0) {
			continue;
		}
		int row = i % n;
		int col = i / n;
		triplet_list.push_back(Triplet(row, col, E[i]));
	}
	_E.setFromTriplets(triplet_list.begin(), triplet_list.end());

	/* Generating the sparse matrix for C */
	triplet_list.clear();
	for (int i = 0; i < l * n; i++) {
		if (C[i] == 0.0) {
			continue;
		}
		int row = i % n;
		int col = i / n;
		triplet_list.push_back(Triplet(row, col, C[i]));
	}
	_C.setFromTriplets(triplet_list.begin(), triplet_list.end());

	memcpy(_x0.data(), x0, n * sizeof(_double_t));
	memcpy(_b.data(), b, n * sizeof(_double_t));
	memcpy(_d.data(), d, m * sizeof(_double_t));
	memcpy(_f.data(), f, l * sizeof(_double_t));

	int ret = ADMM_bqp_linear_eq_and_uneq(n, _A, _b, _x0, m, _C, _d, l, _E, _f, sol);
	return ret;
}

int LPboxADMMsolver::ADMM_bqp_unconstrained_legacy(int n, const SparseMatrix &_A, const DenseVector &_b,
		const DenseVector &x0, Solution& sol) {

	auto x_sol    = DenseVector(n);
	auto y1       = DenseVector(n);
	auto y2       = DenseVector(n);
	auto z1       = DenseVector(n);
	auto z2       = DenseVector(n);
	auto prev_idx = DenseVector(n);
	auto best_sol = DenseVector(n);
	auto temp_vec = DenseVector(n);
	auto temp_mat = SparseMatrix(n, n);
	auto cur_idx  = DenseVector(n);
	_double_t cur_obj;
	bool rhoUpdated = true;

	x_sol = x0;

	/* Initializing preconditioner for conjugate gradient */
	Eigen::DiagonalPreconditioner<_double_t> diagonalPreconditioner;

	_double_t rho1 = initial_rho;
	_double_t rho2 = initial_rho;
	_double_t prev_rho1 = rho1;
	_double_t prev_rho2 = rho2;
	std::vector<_double_t> obj_list; /* Stores the objective value calculated during each iteration */
	_double_t std_obj = 1;

	/* temp_mat stores the matrix that is used in the conjugate gradient step */
	temp_mat = 2 * _A;
	temp_mat.diagonal().array() += rho1 + rho2;
	temp_mat.makeCompressed();

	_double_t cvg_test1;
	_double_t cvg_test2;
	_double_t rho_change_ratio;

	y1 = x_sol;
	y2 = x_sol;

	FILE *fp;
	if (does_log) {
		fp = fopen(log_file_path.c_str(), "w+");
	}

	prev_idx = (x_sol.array() >= 0.5).matrix().cast<_double_t>();
	best_sol = x_sol;

	_double_t best_bin_obj = compute_cost(x_sol, _A, _b);

	if (does_log) {
		fprintf(fp, "Initial state\n");
        fprintf(fp, "norm of x_sol: %lf\n", x_sol.norm());
        fprintf(fp, "norm of y1: %lf\n", y1.norm());
        fprintf(fp, "norm of y2: %lf\n", y2.norm());

        fprintf(fp, "norm of z1: %lf\n", z1.norm());
        fprintf(fp, "norm of z2: %lf\n", z2.norm());

        fprintf(fp, "norm of cur_idx: %lf\n", cur_idx.norm());
        fprintf(fp, "rho1: %lf\n", rho1);
        fprintf(fp, "rho2: %lf\n", rho2);
        fprintf(fp, "-------------------------------------------------\n");
	}

	std::chrono::steady_clock::time_point start, end;
	start = std::chrono::steady_clock::now();

	long time_elapsed = 0;
	for (int iter = 0; iter < max_iters; iter++) {
		temp_vec = x_sol + z1 / rho1;

		if (does_log) {
			fprintf(fp, "Iteration: %d\n", iter);
		}
		/* Project vector on [0, 1] box to calculate y1 */
		project_box(n, temp_vec, y1);

		temp_vec = x_sol + z2 / rho2;

		/* Project vector on shifted lp box to calculate y2 */
		project_shifted_Lp_ball(n, temp_vec, projection_lp, y2);

		/* If the iteration is nonzero and it divides rho_change_step, it means
		 * that the rho updated in the last iteration and the matrix used by conjugate
		 * gradient should be updated
		 */
		if (iter != 0 && rhoUpdated) {
			temp_mat.diagonal().array() += (prev_rho1 + prev_rho2) * rho_change_ratio;
			temp_mat.makeCompressed();
		}

		/* Calculate the vector b in the conjugate gradient algorithm */
		temp_vec = rho1 * y1 + rho2 * y2 - (_b + z1 + z2);

		/* Explicit version of conjugate gradient used for profiling */

		/* Since the matrix used by the conjugate gradient changes only when after rho is updated
		 * we only need to recalculate the preconditioner if rho is updated in the last iteration
		 */
		if (rhoUpdated) {
			diagonalPreconditioner.compute(temp_mat);
			rhoUpdated = false;
		}

		x_sol = y1;
		_double_t tol = pcg_tol;
		int maxiter = pcg_maxiters;
		_conjugate_gradient(temp_mat, temp_vec, x_sol, diagonalPreconditioner, maxiter, tol);
		if (does_log) {
			fprintf(fp, "Conjugate gradient stops after %d iterations\n", maxiter);
			fprintf(fp, "Conjugate gradient stops with residual %lf\n", tol);
		}

		z1 = z1 + gamma_val * rho1 * (x_sol - y1);
		z2 = z2 + gamma_val * rho2 * (x_sol - y2);

		/* Testing the conditions to see if the algorithm converges */
		_double_t temp0 = std::max(x_sol.norm(), _double_t(2.2204e-16));
		cvg_test1 = (x_sol-y1).norm() / temp0;
		cvg_test2 = (x_sol-y2).norm() / temp0;
		if (cvg_test1 <= stop_threshold && cvg_test2 <= stop_threshold) {
			printf("iter: %d, stop_threshold: %.6f\n", iter, std::max(cvg_test1, cvg_test2));
			if (does_log) {
				fprintf(fp, "iter: %d, stop_threshold: %.6f\n", iter, std::max(cvg_test1, cvg_test2));
			}
			break;
		}

		/* Update the rho value every rho_change_step */
		if ((iter+1) % rho_change_step == 0) {
			prev_rho1 = rho1;
			prev_rho2 = rho2;
			rho1 = learning_fact * rho1;
			rho2 = learning_fact * rho2;
			gamma_val = std::max(gamma_val * gamma_factor, _double_t(1.0));
			rhoUpdated = true;
			rho_change_ratio = learning_fact - 1.0;
		}

		/* Computer the relaxed cost function (x is not binary)*/
		_double_t obj_val = compute_cost(x_sol,_A,_b);
		obj_list.push_back(obj_val);
		if (obj_list.size() >= history_size) {
			std_obj = compute_std_obj(obj_list, history_size);
		}
		if (std_obj <= std_threshold) {
			printf("iter: %d, std_threshold: %.6f\n", iter, std_obj);
			if (does_log) {
				fprintf(fp, "iter: %d, std_threshold: %.6f\n", iter, std_obj);
			}
			break;
		}

		/* Calculating the actual cost */
		cur_idx = (x_sol.array() >= 0.5).matrix().cast<_double_t>(); /* The value type of the vector should be double
																	  therefore needs casting the boolean vector to double */
		prev_idx = cur_idx;
		cur_obj = compute_cost(prev_idx, _A, _b);

		/* Setting the best binary solution */
		if (best_bin_obj >= cur_obj) {
			best_bin_obj = cur_obj;
			best_sol = x_sol;
		}

		if (does_log) {
			fprintf(fp, "current objective: %lf\n", obj_val);
			fprintf(fp, "current binary objective: %lf\n", cur_obj);
			fprintf(fp, "norm of x_sol: %lf\n", x_sol.norm());
			fprintf(fp, "norm of binary x_sol: %lf\n", cur_idx.norm());
			fprintf(fp, "norm of y1: %lf\n", y1.norm());
			fprintf(fp, "norm of y2: %lf\n", y2.norm());

			fprintf(fp, "norm of z1: %lf\n", z1.norm());
			fprintf(fp, "norm of z2: %lf\n", z2.norm());

			fprintf(fp, "rho1: %lf\n", rho1);
			fprintf(fp, "rho2: %lf\n", rho2);
			fprintf(fp, "-------------------------------------------------\n");
		}

	}
	sol.x_sol = new DenseVector(x_sol);
	sol.y1 = new DenseVector(y1);
	sol.y2 = new DenseVector(y2);
	sol.best_sol = new DenseVector(best_sol);

	end = std::chrono::steady_clock::now();
	time_elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
	std::cout << "Time elapsed: " << time_elapsed << "us" << std::endl;
	sol.time_elapsed = time_elapsed;

	if (does_log) {
		fprintf(fp, "Time elapsed: %ldus\n", time_elapsed);
		fclose(fp);
	}
	return 1;
}


#pragma once

#include "random_engine.hpp"
#include <boost/multiprecision/cpp_int.hpp>

namespace cryptb
{
	class rsa
	{
		// Private key- for decrypting / digital signing
		// DON'T SHARE d WITH THE CLIENT!
		// Tends to be a number with around 2048 bits.
		boost::multiprecision::cpp_int d{ 0 };

		// It's completely safe to share e and N with the entire world.
		// In fact, you should.

		// Public key- for encrypting / verifying digital signature
		// Tends to be a very small number. Choosing the number 3 for example, is common.
		boost::multiprecision::cpp_int e{ 0 };

		// Public key- for everything. N is needed for all operations.
		// Tends to be a number with about 4096 bits.
		boost::multiprecision::cpp_int N{ 0 };

		static boost::multiprecision::cpp_int findd(const boost::multiprecision::cpp_int& PhiN, const boost::multiprecision::cpp_int& e);

	public:
		rsa(const rsa&) = default;
		rsa(rsa&&) = default;
		rsa& operator=(const rsa&) = default;
		rsa& operator=(rsa&&) = default;

		// Constructor for generating RSA public-private key pair using the given random engine.
		//
		// When "num_bytes_in_prime_number" == 128 that's 2048-bit RSA
		// Should take a second and a half (very expensive function, call on an asynchronous thread).
		//
		rsa(random_engine& rand, const int num_bytes_in_prime_number = 128);

		// Constructor for loading RSA public-private key pairs from values
		rsa(boost::multiprecision::cpp_int&& e, boost::multiprecision::cpp_int&& d, boost::multiprecision::cpp_int&& N) :
			e(std::move(e)), d(std::move(d)), N(std::move(N)) {}

		// Private secret key, don't share.
		const boost::multiprecision::cpp_int& get_d() const
		{
			return this->d;
		}

		// Public key, no danger. Allowed to reveal to the entire world.
		const boost::multiprecision::cpp_int& get_e() const
		{
			return this->e;
		}

		// Public key, no danger. Allowed to reveal to the entire world.
		const boost::multiprecision::cpp_int& get_N() const
		{
			return this->N;
		}
	};
}


#include <ql/quantlib.hpp>

#include <boost/make_shared.hpp>

using namespace QuantLib;

int example01() {

    try {

        Date refDate(13, November, 2013);
        Date settlDate = TARGET().advance(refDate, 2 * Days);
        Settings::instance().evaluationDate() = refDate;

        Handle<Quote> rateLevel(new SimpleQuote(0.03));
        Handle<YieldTermStructure> yts(
            new FlatForward(refDate, rateLevel, Actual365Fixed()));

        boost::shared_ptr<IborIndex> iborIndex(new Euribor(6 * Months, yts));
        boost::shared_ptr<SwapIndex> swapIndex(
            new EuriborSwapIsdaFixA(10 * Years, yts));

        iborIndex->addFixing(refDate, 0.0200);
        swapIndex->addFixing(refDate, 0.0315);

        // Handle<Quote> volatilityLevel(new SimpleQuote(0.30));
        // Handle<SwaptionVolatilityStructure> swaptionVol(
        //     new ConstantSwaptionVolatility(refDate, TARGET(), Following,
        //                                    volatilityLevel,
        // Actual365Fixed()));

        Handle<SwaptionVolatilityStructure> swaptionVol(
            new SingleSabrSwaptionVolatility(refDate, TARGET(), Following, 0.05,
                                             0.80, -0.30, 0.20,
                                             Actual365Fixed(), swapIndex));

        // Real strike = 0.0001;
        // while(strike < 1.0) {
        //     std::cout << strike << " " <<
        // swaptionVol->volatility(10.0,10.0,strike) << std::endl;
        //     strike += 0.0010;
        // }
        // return 0;

        Date termDate = TARGET().advance(settlDate, 10 * Years);

        Schedule sched1(settlDate, termDate, 1 * Years, TARGET(),
                        ModifiedFollowing, ModifiedFollowing,
                        DateGeneration::Forward, false);
        Schedule sched2(settlDate, termDate, 6 * Months, TARGET(),
                        ModifiedFollowing, ModifiedFollowing,
                        DateGeneration::Forward, false);

        Real nominal = 100000.0;

        boost::shared_ptr<FloatFloatSwap> cmsswap(
            new FloatFloatSwap( // CMS Swap
                VanillaSwap::Payer, nominal, nominal, sched1, swapIndex,
                Thirty360(), sched2, iborIndex, Actual360(), false, false, 1.0,
                0.00, Null<Real>(), Null<Real>(), 1.0, 0.00267294));

        // boost::shared_ptr<FloatFloatSwap> cmsswap(new FloatFloatSwap(  //
        // Reversed Floored CMS Swap
        //     VanillaSwap::Payer, nominal, nominal, sched1, swapIndex,
        //     Thirty360(), sched2, iborIndex, Actual360(),
        //     false,false,-1.0,0.03,Null<Real>(),0.0,1.0,0.0));

        // boost::shared_ptr<FloatFloatSwap> cmsswap(new FloatFloatSwap( // fix
        // float swap as FloatFloatSwap instrument
        //     VanillaSwap::Payer, nominal, nominal, sched1, swapIndex,
        //     Thirty360(), sched2, iborIndex, Actual360(),
        //     false,false,0.0,0.05,Null<Real>(),Null<Real>(),1.0,0.0));

        std::vector<Date> exerciseDates;
        std::vector<Date> sigmaSteps;
        std::vector<Real> sigma;

        sigma.push_back(0.01);
        for (Size i = 1; i < sched1.size() - 1; i++) {
            exerciseDates.push_back(swapIndex->fixingDate(sched1[i]));
            sigmaSteps.push_back(exerciseDates.back());
            sigma.push_back(0.01);
        }

        // double v[] =
        // {0.01,0.0115446,0.0111677,0.0113092,0.0112404,0.0111856,0.0111218,0.0110547,0.0112791,0.00945792};
        // sigma = std::vector<Real>(v,v+10);

        // boost::shared_ptr<FloatFloatSwap> underlying(new FloatFloatSwap(
        //     VanillaSwap::Receiver, nominal, nominal, sched1, swapIndex,
        //     Thirty360(), sched2, iborIndex, Actual360(),
        //     false,false,1.0,0.0,Null<Real>(),Null<Real>(),1.0,0.00267294));

        boost::shared_ptr<Exercise> exercise(
            new BermudanExercise(exerciseDates));
        boost::shared_ptr<FloatFloatSwaption> callRight(
            new FloatFloatSwaption(cmsswap, exercise));

        std::vector<Date> cmsFixingDates(exerciseDates);
        std::vector<Period> cmsTenors(exerciseDates.size(), 10 * Years);

        Handle<Quote> reversionLevel(new SimpleQuote(0.02));

        boost::shared_ptr<NumericHaganPricer> haganPricer(
            new NumericHaganPricer(swaptionVol,
                                   GFunctionFactory::NonParallelShifts,
                                   reversionLevel));
        setCouponPricer(cmsswap->leg(0), haganPricer);

        // hull white model (change the model->calibrate call below)
        std::vector<Date> sigmaSteps2(sigmaSteps.begin(), sigmaSteps.end() - 1);
        std::vector<Real> sigma2(sigma.begin(), sigma.end() - 1);
        // boost::shared_ptr<Gsr> model(new
        // Gsr(yts,sigmaSteps2,sigma2,reversionLevel->value()));
        boost::shared_ptr<Gaussian1dModel> model2(
            new Gsr(yts, sigmaSteps2, sigma2, reversionLevel->value()));

        Handle<SwaptionVolatilityStructure> hwVol(
            new Gaussian1dSwaptionVolatility(model2, swapIndex));
        Real moneyIn[] = { 0.20, 0.50, 0.75, 1.0, 1.5, 2.0, 5.0, 10.0 };
        std::vector<Real> money(moneyIn, moneyIn + 8);

        // markov model (change the model->calibrate call below)
        // boost::math::ntl::RR::SetPrecision(113);
        boost::shared_ptr<MarkovFunctional> model(new MarkovFunctional(
            yts, reversionLevel->value(), sigmaSteps, sigma, swaptionVol,
            cmsFixingDates, cmsTenors,
            swapIndex // set vol structure for mf here
            /*,MarkovFunctional::ModelSettings().withAdjustments(
                MarkovFunctional::ModelSettings::SabrSmile |
                MarkovFunctional::ModelSettings::
                    SmileExponentialExtrapolation)
                    .withSmileMoneynessCheckpoints(money)*/));

        boost::shared_ptr<Gaussian1dFloatFloatSwaptionEngine> floatEngine(
            new Gaussian1dFloatFloatSwaptionEngine(model));

        callRight->setPricingEngine(floatEngine);

        std::cout << "determine cal basket" << std::endl;

        boost::shared_ptr<Gaussian1dSwaptionEngine> stdEngine(
            new Gaussian1dSwaptionEngine(model));

        boost::shared_ptr<SwapIndex> swapBase(
            new EuriborSwapIsdaFixA(30 * Years, yts));

        LevenbergMarquardt opt;
        EndCriteria ec(2000, 500, 1E-8, 1E-8, 1E-8);

        Size iteration = 0;
        while (iteration < 1) { // set number of iterations here ...
            std::vector<boost::shared_ptr<CalibrationHelper> > basket =
                callRight->calibrationBasket(
                    swapBase, *swaptionVol, // set vol structure for basket here
                    // BasketGeneratingEngine::Naive
                    BasketGeneratingEngine::MaturityStrikeByDeltaGamma);

            for (Size i = 0; i < basket.size(); i++)
                basket[i]->setPricingEngine(stdEngine);
            model->calibrate(basket, opt, ec); // for markov
            // model->calibrate(basket, opt, ec, Constraint(),
            // std::vector<Real>(), model->FixedReversions()); // for gsr

            std::cout << "option date & maturity date & nominal & strike & "
                         "model vol \\\\" << std::endl;
            for (Size i = 0; i < basket.size(); i++) {
                boost::shared_ptr<SwaptionHelper> h =
                    boost::dynamic_pointer_cast<SwaptionHelper>(basket[i]);
                std::cout << exerciseDates[i] << " & "
                          << h->underlyingSwap()->fixedSchedule().dates().back()
                          << " & " << h->underlyingSwap()->nominal() << " & "
                          << h->underlyingSwap()->fixedRate() << " & "
                          << model->volatility()[i] << " \\\\" << std::endl;
            }
            std::cout << model->volatility().back() << std::endl;
            iteration++;
        }

        Real analyticSwapNpv = CashFlows::npv(cmsswap->leg(1), **yts, false) -
                               CashFlows::npv(cmsswap->leg(0), **yts, false);
        Real callRightNpv = callRight->NPV();
        Real firstCouponNpv = -cmsswap->leg(0)[0]->amount() *
                                  yts->discount(cmsswap->leg(0)[0]->date()) +
                              cmsswap->leg(1)[0]->amount() *
                                  yts->discount(cmsswap->leg(1)[0]->date());
        Real underlyingNpv =
            callRight->result<Real>("underlyingValue") + firstCouponNpv;

        std::cout << "Swap Npv (Hagan)     & " << analyticSwapNpv << "\\\\"
                  << std::endl;
        std::cout << "Call Right Npv (MF)  & " << callRightNpv << "\\\\"
                  << std::endl;
        std::cout << "Underlying Npv (MF)  & " << underlyingNpv << "\\\\"
                  << std::endl;
        std::cout << "fair margin swap & "
                  << -analyticSwapNpv / CashFlows::bps(cmsswap->leg(1), **yts,
                                                       false) << std::endl;

        // std::cout << "Model trace : " << std::endl << model->modelOutputs()
        // << std::endl;

        return 0;
    }
    catch (std::exception &e) {
        std::cerr << e.what() << std::endl;
        return 1;
    }
    catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }
}

int example02() {

    try {

        Date refDate(13, November, 2013);
        Date settlDate = TARGET().advance(refDate, 2 * Days);
        Settings::instance().evaluationDate() = refDate;

        Handle<Quote> rateLevel(new SimpleQuote(0.03));
        Handle<YieldTermStructure> yts(
            new FlatForward(refDate, rateLevel, Actual365Fixed()));

        boost::shared_ptr<IborIndex> iborIndex(new Euribor(6 * Months, yts));
        boost::shared_ptr<SwapIndex> swapIndex(
            new EuriborSwapIsdaFixA(10 * Years, yts));

        iborIndex->addFixing(refDate, 0.0200);
        swapIndex->addFixing(refDate, 0.0315);

        // Handle<Quote> volatilityLevel(new SimpleQuote(0.20)); // vol here !
        // Handle<SwaptionVolatilityStructure> swaptionVol(
        //     new ConstantSwaptionVolatility(refDate, TARGET(), Following,
        //                                    volatilityLevel, Actual365Fixed()));

        Handle<SwaptionVolatilityStructure> swaptionVol(
            new SingleSabrSwaptionVolatility(refDate, TARGET(), Following,
        0.10,
                                             0.80, -0.30, 0.40,
                                             Actual365Fixed(), swapIndex));

        // Real strike = 0.0001;
        // while(strike < 0.50) {
        //     std::cout << strike << " " <<
        // swaptionVol->volatility(10.0,10.0,strike) << std::endl;
        //     strike += 0.0050;
        // }
        // return 0;

        Date termDate = TARGET().advance(settlDate, 10 * Years);

        Schedule sched1(settlDate, termDate, 1 * Years, TARGET(),
                        ModifiedFollowing, ModifiedFollowing,
                        DateGeneration::Forward, false);
        Schedule sched2(settlDate, termDate, 6 * Months, TARGET(),
                        ModifiedFollowing, ModifiedFollowing,
                        DateGeneration::Forward, false);

        Real nominal = 100000.0;

        boost::shared_ptr<FloatFloatSwap> cmsswap(
            new FloatFloatSwap( // CMS Swap
                VanillaSwap::Payer, nominal, nominal, sched1, swapIndex,
                Thirty360(), sched2, iborIndex, Actual360(), false, false, 1.0,
                0.00, Null<Real>(), Null<Real>(), 1.0, 0.0));

        Handle<Quote> reversionLevel(new SimpleQuote(0.01)); // reversion here !

        boost::shared_ptr<NumericHaganPricer> haganPricerN(
            new NumericHaganPricer(swaptionVol,
                                   GFunctionFactory::Standard,
                                   reversionLevel));
        boost::shared_ptr<AnalyticHaganPricer> haganPricerA(
            new AnalyticHaganPricer(swaptionVol,
                                    GFunctionFactory::Standard,
                                    reversionLevel));

        // auto integrator =
        // boost::make_shared<GaussLobattoIntegral>(1000,1E-4);

        boost::shared_ptr<LinearTsrPricer> tsrPricer(new LinearTsrPricer(
            swaptionVol, reversionLevel, Handle<YieldTermStructure>(),
            LinearTsrPricer::Settings().withRateBound(0.0, 1.0)
            //.withVegaRatio(0.01)
            ));

        boost::shared_ptr<CmsReplicationPricer> replPricer(
            new CmsReplicationPricer(swaptionVol, reversionLevel));

        Real strike = 0.0001;
        while (strike <= 0.1000) {

            auto tmpCap = boost::shared_ptr<CappedFlooredCoupon>(new CappedFlooredCmsCoupon(
                Date(13, November, 2023), 100000.0, Date(13, November, 2022),
                Date(13, November, 2023), 2, swapIndex, 1.0, 0.0, strike,
                Null<Rate>(), Date(), Date(), DayCounter(), false));
            auto cap = boost::make_shared<StrippedCappedFlooredCoupon>(tmpCap);
            auto tmpFloor = boost::shared_ptr<CappedFlooredCoupon>(
                new CappedFlooredCmsCoupon(
                    Date(13, November, 2023), 100000.0,
                    Date(13, November, 2022), Date(13, November, 2023), 2,
                    swapIndex, 1.0, 0.0, Null<Real>(), strike, Date(), Date(),
                    DayCounter(), false));
            auto floor = boost::make_shared<StrippedCappedFlooredCoupon>(tmpFloor);
            auto swaplet = boost::shared_ptr<CappedFlooredCmsCoupon>(
                new CappedFlooredCmsCoupon(
                    Date(13, November, 2023), 100000.0,
                    Date(13, November, 2022), Date(13, November, 2023), 2,
                    swapIndex, 1.0, 0.0, Null<Rate>(), Null<Rate>(), Date(),
                    Date(), DayCounter(), false));

            cap->setPricer(tsrPricer);
            floor->setPricer(tsrPricer);
            swaplet->setPricer(tsrPricer);
            Real cap1 = cap->adjustedFixing();
            Real floor1 = floor->adjustedFixing();
            Real swaplet1 = swaplet->adjustedFixing();

            // cap->setPricer(haganPricerN);
            // floor->setPricer(haganPricerN);
            // swaplet->setPricer(haganPricerN);
            // Real cap2 = cap->adjustedFixing();
            // Real floor2 = floor->adjustedFixing();
            // Real swaplet2 = swaplet->adjustedFixing();
            // cap->setPricer(replPricer);
            // floor->setPricer(replPricer);
            // swaplet->setPricer(replPricer);
            // Real cap2 = 0.0;//cap->adjustedFixing();
            // Real floor2 = 0.0;//floor->adjustedFixing();
            // Real swaplet2 = 0.0;//swaplet->adjustedFixing();

            // cap->setPricer(tsrPricer);
            // floor->setPricer(tsrPricer);
            // swaplet->setPricer(tsrPricer);
            // Real cap3 = cap->adjustedFixing();
            // Real floor3 = floor->adjustedFixing();
            // Real swaplet3 = swaplet->adjustedFixing();

            // std::cout << strike << " " << cap1 << " "
            //           << " " << floor1 << " "
            //           << (cap1 - floor1 - (swaplet1 - strike)) << std::endl;

            strike += 0.0001;
        }

        return 0;
    }
    catch (std::exception &e) {
        std::cerr << e.what() << std::endl;
        return 1;
    }
    catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }
}

int example03() {

    try {

        Date refDate(13, November, 2013);
        Date settlDate = TARGET().advance(refDate, 2 * Days);
        Settings::instance().evaluationDate() = refDate;

        Handle<Quote> rateLevel1(new SimpleQuote(0.0350));
        Handle<Quote> rateLevel2(new SimpleQuote(0.0300));
        Handle<YieldTermStructure> yts1(
            new FlatForward(refDate, rateLevel1, Actual365Fixed()));
        Handle<YieldTermStructure> yts2(
            new FlatForward(refDate, rateLevel2, Actual365Fixed()));

        boost::shared_ptr<IborIndex> iborIndex(new Euribor(6 * Months, yts1));
        boost::shared_ptr<SwapIndex> swapIndex1(
            new EuriborSwapIsdaFixA(10 * Years, yts1));
        boost::shared_ptr<SwapIndex> swapIndex2(
            new EuriborSwapIsdaFixA(2 * Years, yts2));

        boost::shared_ptr<SwapSpreadIndex> swapSpreadIndex(
            new SwapSpreadIndex("cms10_2", swapIndex1, swapIndex2));

        Handle<Quote> volatilityLevel(new SimpleQuote(0.40)); // vol here !
        Handle<SwaptionVolatilityStructure> swaptionVol(
            new ConstantSwaptionVolatility(refDate, TARGET(), Following,
                                           volatilityLevel, Actual365Fixed()));

        // Handle<SwaptionVolatilityStructure> swaptionVol(
        //     new SingleSabrSwaptionVolatility(refDate, TARGET(), Following,
        // 0.15,
        //                                      0.80, -0.30, 0.20,
        //                                      Actual365Fixed(), swapIndex));

        Handle<Quote> reversionLevel(new SimpleQuote(0.00)); // reversion here !

        boost::shared_ptr<LinearTsrPricer> tsrPricer(new LinearTsrPricer(
            swaptionVol, reversionLevel, Handle<YieldTermStructure>(),
            LinearTsrPricer::Settings().withRateBound(0.0, 1.0)
            //.withVegaRatio(0.01)
            ));

        Handle<Quote> correlation(new SimpleQuote(0.20)); // correlation here

        boost::shared_ptr<CappedFlooredCoupon> tmpSpreadCoupon(
            new CappedFlooredCmsSpreadCoupon(
                Date(13, November, 2034), 1.0, Date(13, November, 2033),
                Date(13, November, 2034), 2, swapSpreadIndex, 1.0, 0.0,
                Null<Real>(), 0.0050, Date(), Date(), DayCounter(), false));

        boost::shared_ptr<StrippedCappedFlooredCoupon> spreadCoupon =
            boost::make_shared<StrippedCappedFlooredCoupon>(tmpSpreadCoupon);

        // boost::shared_ptr<CmsSpreadCoupon> spreadCoupon(new CmsSpreadCoupon(
        //     Date(13, November, 2024), 1.0, Date(13, November, 2023),
        //     Date(13, November, 2024), 2, swapSpreadIndex, 1.0, 0.0,
        //     Date(), Date(), DayCounter(),false));

        std::cout << "integration_points;rate" << std::setprecision(16)
                  << std::endl;
        for (Size i = 4; i < 64; i++) {

            boost::shared_ptr<LognormalCmsSpreadPricer> spreadPricer(
                new LognormalCmsSpreadPricer(tsrPricer, correlation,
                                             Handle<YieldTermStructure>(), i));

            spreadCoupon->setPricer(spreadPricer);

            std::cout << i << ";" << spreadCoupon->rate() << std::endl;
        }

        return 0;
    }
    catch (std::exception &e) {
        std::cerr << e.what() << std::endl;
        return 1;
    }
    catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }
}

int example04() {

    try {

        Date refDate(13, November, 2013);
        Date settlDate = TARGET().advance(refDate, 2 * Days);
        Settings::instance().evaluationDate() = refDate;

        Handle<Quote> rateLevel1(new SimpleQuote(0.0350));
        Handle<YieldTermStructure> yts1(
            new FlatForward(refDate, rateLevel1, Actual365Fixed()));
        Handle<Quote> rateLevel2(new SimpleQuote(0.0300));
        Handle<YieldTermStructure> yts2(
            new FlatForward(refDate, rateLevel2, Actual365Fixed()));

        boost::shared_ptr<IborIndex> iborIndex(new Euribor(6 * Months, yts1));
        boost::shared_ptr<SwapIndex> swapIndex1(
            new EuriborSwapIsdaFixA(10 * Years, yts1));
        boost::shared_ptr<SwapIndex> swapIndex2(
            new EuriborSwapIsdaFixA(2 * Years, yts2));

        boost::shared_ptr<SwapSpreadIndex> swapSpreadIndex(
            new SwapSpreadIndex("cms10_2", swapIndex1, swapIndex2));

        Handle<Quote> volatilityLevel(new SimpleQuote(0.40)); // vol here !
        Handle<SwaptionVolatilityStructure> swaptionVol(
            new ConstantSwaptionVolatility(refDate, TARGET(), Following,
                                           volatilityLevel, Actual365Fixed()));

        Handle<Quote> reversionLevel(new SimpleQuote(0.00)); // reversion here !

        Schedule sched(settlDate, settlDate + 10 * Years, 1 * Years, TARGET(),
                       ModifiedFollowing, ModifiedFollowing,
                       DateGeneration::Backward, false);

        boost::shared_ptr<FloatFloatSwap> underlying(new FloatFloatSwap(VanillaSwap::Payer,
                                                                        100.0,100.0,
                                                                        sched,iborIndex,Actual360(),
                                                                        sched,swapSpreadIndex,Thirty360(),
                                                                        false,
                                                                        false));

        std::vector<Date> callDates = sched.dates();

        std::cout << "call dates:" << std::endl;
        for(Size i=0;i<callDates.size();i++)
            std::cout << callDates[i] << std::endl;

        boost::shared_ptr<Exercise> exercise = boost::make_shared<BermudanExercise>(callDates);

        boost::shared_ptr<FloatFloatSwaption> swaption = boost::make_shared<FloatFloatSwaption>(underlying,exercise);

        std::vector<Date> volStepDates;
        std::vector<Real> vols(1,0.01);

        boost::shared_ptr<Gaussian1dModel> model = boost::make_shared<Gsr>(yts1,volStepDates,
                                                                           vols,0.01);

        boost::shared_ptr<Gaussian1dFloatFloatSwaptionEngine> engine =
            boost::make_shared<Gaussian1dFloatFloatSwaptionEngine>(
                model, 64, 7.0, true, false, Handle<Quote>(),
                Handle<YieldTermStructure>(), false,
                Gaussian1dFloatFloatSwaptionEngine::Naive);

        swaption->setPricingEngine(engine);

        std::cout << "swaption npv = " << swaption->NPV() << std::endl;

        std::vector<Real> probs = swaption->result<std::vector<Real> >("probabilities");
        for(Size i=0;i<probs.size();i++) {
            std::cout << i << " => " << probs[i] << std::endl;
        }

    }
    catch (std::exception &e) {
        std::cerr << e.what() << std::endl;
        return 1;
    }
    catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }

    return 0;

}

int main(int, char * []) {

    return example02();

}


#include "lcp_solver.hpp"

#include <Eigen/Core>

#include <constants.hpp>
#include <logger.hpp>
#include <utils/eigen_ext.hpp>

namespace ccd {
namespace opt {

    inline Eigen::VectorXd fischer(
        const Eigen::VectorXd& x, const Eigen::VectorXd& s);
    inline double fischer_error(const Eigen::VectorXd& phi);
    Eigen::SparseMatrix<double> jac_fischer(const Eigen::VectorXd& x,
        const Eigen::VectorXd& s,
        const Eigen::SparseMatrix<double>& A);
    inline Eigen::VectorXd grad_fischer_error(
        const Eigen::VectorXd& phi, const Eigen::SparseMatrix<double>& J);
    Eigen::VectorXd perturb(
        const Eigen::VectorXd& x, const Eigen::VectorXd& phi);
    bool is_converged(double old_err, double err, int iter);
    Eigen::VectorXd compute_search_direction(const Eigen::VectorXd& phi,
        const Eigen::SparseMatrix<double>& J,
        const Eigen::VectorXd& grad_psi,
        const int iter);
    // Armijo backtracking combined with a projected line-search
    bool projected_line_search(const Eigen::VectorXd& x,
        const Eigen::VectorXd& dir,
        const std::function<double(const Eigen::VectorXd&)>& f,
        const double fx,
        const Eigen::VectorXd& grad_fx,
        double& step_length,
        int iter,
        const double armijo_rule_coeff = 0.001);

    // Solve the LCP:
    //      s = Ax + b
    //      0 ≤ x ⟂ s ≥ 0
    // by minimizing the Fisher error:
    //      f(x) = (x² + s²) - x - s
    // using Newton's method.
    bool lcp_newton(const Eigen::MatrixXd& A_dense,
        const Eigen::VectorXd& b,
        Eigen::VectorXd& x)
    {
        // Use a sparse matrix
        const Eigen::SparseMatrix<double> A = A_dense.sparseView();

        int num_vars = b.rows();
        assert(A.rows() == num_vars);

        // x₀ = 0
        x = Eigen::VectorXd::Zero(num_vars);

        double old_err, err = std::numeric_limits<double>::infinity();
        for (int i = 0; i < Constants::FISCHER_MAX_ITER; i++) {
            Eigen::VectorXd s = A * x + b; // slack variable

            // Test all stopping criteria used
            Eigen::VectorXd phi = fischer(x, s);
            old_err = err;
            err = fischer_error(phi); // ψ

            if (is_converged(old_err, err, i)) {
                return true;
            }

            Eigen::VectorXd px = perturb(x, phi);
            Eigen::SparseMatrix<double> J = jac_fischer(px, s, A);
            Eigen::VectorXd grad_psi = grad_fischer_error(phi, J);

            // Test if we have dropped into a local minimia if so we are stuck
            if (grad_psi.norm() < Constants::FISCHER_ABS_TOL) {
                spdlog::warn("solver=ficher_newton_lcp iter={:d} "
                             "failure='||∇ψ|| < {:g}' ||∇ψ||={:g} "
                             "failsafe=none",
                    i, Constants::FISCHER_ABS_TOL, grad_psi.norm());
                return false;
            }

            Eigen::VectorXd delta_x
                = compute_search_direction(phi, J, grad_psi, i);

            // Update iterate with result from Armijo backtracking
            double step_length = 1.0;
            bool success = projected_line_search(x, delta_x,
                [&A, &b](const Eigen::VectorXd& x) {
                    Eigen::VectorXd s = A * x + b;
                    Eigen::VectorXd phi = fischer(x, s);
                    return fischer_error(phi);
                },
                /*fx=*/err, grad_psi, step_length, i);
            if (!success) {
                delta_x = -grad_psi;
                success = projected_line_search(x, delta_x,
                    [&A, &b](const Eigen::VectorXd& x) {
                        Eigen::VectorXd s = A * x + b;
                        Eigen::VectorXd phi = fischer(x, s);
                        return fischer_error(phi);
                    },
                    /*fx=*/err, grad_psi, step_length, i);
                if (!success) {
                    return false;
                }
            }

            x += step_length * delta_x;
        }

        spdlog::warn("solver=ficher_newton_lcp "
                     "failure='too many iterations' MAX_ITER={:d}",
            Constants::FISCHER_MAX_ITER);
        return false;
    }

    /// @brief Compute the Fischer error measure.
    inline Eigen::VectorXd fischer(
        const Eigen::VectorXd& x, const Eigen::VectorXd& s)
    {
        //             _________
        // ϕ(x, s) = ⎷(x² + s²) - x - s
        //
        return (x.array().pow(2) + s.array().pow(2)).sqrt().matrix() - x - s;
    }

    inline double fischer_error(const Eigen::VectorXd& phi)
    {
        // ψ(x, s) = ½ϕ(x, s)ᵀϕ(x, s)
        return phi.squaredNorm() / 2;
    }

    Eigen::SparseMatrix<double> jac_fischer(const Eigen::VectorXd& x,
        const Eigen::VectorXd& s,
        const Eigen::SparseMatrix<double>& A)
    {
        //                ___________             _x̲ᵢ̲_+̲_s̲ᵢ̲a̲ᵢ̲_
        // ∇ϕᵢ(x, s) = ∇⎷(xᵢ² + sᵢ²) - xᵢ - sᵢ =  xᵢ² + s²    - aᵢ - 1
        Eigen::ArrayXd denom = (x.array().pow(2) + s.array().pow(2)).sqrt();
        Eigen::VectorXd p = (x.array() / denom) - 1;
        Eigen::VectorXd q = (s.array() / denom) - 1;
        return Eigen::SparseDiagonal<double>(p)
            + Eigen::SparseDiagonal<double>(q) * A;
    }

    inline Eigen::VectorXd grad_fischer_error(
        const Eigen::VectorXd& phi, const Eigen::SparseMatrix<double>& J)
    {
        // ∇ψ(x, s) = ∇ϕ(x, s)ᵀϕ(x, s) = Jᵀϕ
        return J.transpose() * phi;
    }

    /// @brief Compute the sign of the input.
    inline double sign(double x) { return (0 < x) - (x < 0); }

    Eigen::VectorXd perturb(
        const Eigen::VectorXd& x, const Eigen::VectorXd& phi)
    {
        // Bitmask for singular indices
        Eigen::ArrayXb is_singular
            = phi.array().abs() < Constants::FISCHER_SINGULAR_TOL
            && x.array().abs() < Constants::FISCHER_SINGULAR_TOL;

        // Perturbation: works on full system
        Eigen::VectorXd px = x; // perturbed x
        Eigen::VectorXd x_sign = x.unaryExpr(&sign);
        // x_sign(x_sign==0) = 1;
        x_sign += (x_sign.array() == 0).matrix().cast<double>();
        // px(S==1) = Constants::FISCHER_SINGULAR_TOL * dir(S==1);
        for (size_t i = 0; i < x.rows(); i++) {
            if (is_singular(i)) {
                px(i) = Constants::FISCHER_SINGULAR_TOL * x_sign(i);
            }
        }

        return px;
    }

    bool is_converged(double old_err, double err, int iter)
    {
        // Test relative error
        if (abs(err - old_err) / abs(old_err) < Constants::FISCHER_REL_TOL) {
            spdlog::debug("solver=fischer_newton_lcp iter={} "
                          "status=success rel_tol={} rel_err={}",
                iter, Constants::FISCHER_REL_TOL,
                abs(err - old_err) / abs(old_err));
            return true;
        }
        // Test absolute error
        if (err < Constants::FISCHER_ABS_TOL) {
            spdlog::debug("solver=fischer_newton_lcp iter={} "
                          "status=success abs_tol={} abs_err={}",
                iter, Constants::FISCHER_ABS_TOL, err);
            return true;
        }
        return false;
    }

    Eigen::VectorXd compute_search_direction(const Eigen::VectorXd& phi,
        const Eigen::SparseMatrix<double>& J,
        const Eigen::VectorXd& grad_psi,
        const int iter)
    {
        // Solve Δx = -J⁻¹ϕ
        Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
        solver.compute(J);
        Eigen::VectorXd delta_x;
        if (solver.info() == Eigen::Success) {
            delta_x = solver.solve(-phi);
            if (solver.info() != Eigen::Success) {
                spdlog::warn(
                    "solver=ficher_newton_lcp iter={:d} "
                    "failure='sparse solve for newton direction failed' "
                    "failsafe='revert to gradient descent'",
                    iter);
                delta_x = -grad_psi; // Revert to gradient descent
            }
        } else {
            spdlog::warn("solver=ficher_newton_lcp iter={:d} "
                         "failure='sparse decomposition of the hessian failed' "
                         "failsafe='revert to gradient descent'",
                iter);
            delta_x = -grad_psi; // Revert to gradient descent
        }

        return delta_x;
    }

    // Armijo backtracking combined with a projected line-search
    bool projected_line_search(const Eigen::VectorXd& x,
        const Eigen::VectorXd& dir,
        const std::function<double(const Eigen::VectorXd&)>& f,
        const double fx,
        const Eigen::VectorXd& grad_fx,
        double& step_length,
        int iter,
        const double armijo_rule_coeff)
    {
        const double EPS = std::numeric_limits<double>::epsilon();

        // Test if the search direction is smaller than numerical precision.
        if (dir.array().abs().maxCoeff() < EPS) {
            spdlog::warn("solver=ficher_newton_lcp iter={:d} "
                         "failure='search direction too small' max(|Δx|)={:g} "
                         "failsafe=none",
                iter, dir.array().abs().maxCoeff());
            return false;
        }

        // Test if our search direction is a 'sufficient' descent direction
        double descent_magnitude = (grad_fx.transpose() * dir)(0);
        if (descent_magnitude > -EPS * descent_magnitude) {
            spdlog::warn("solver=ficher_newton_lcp iter={:d} "
                         "failure='search direction not descent direction' "
                         "(∇ϕ)ᵀΔx={:g} failsafe='revert to gradient descent'",
                iter, descent_magnitude);
            return false;
        }

        step_length = 1.0; // Current step length

        // Sufficent decrease parameter for Armijo backtracking line search
        double armijo_term = armijo_rule_coeff * descent_magnitude;

        Eigen::VectorXd x_k;
        do {
            x_k = (x + step_length * dir).array().max(0); // project it

            // Perform Armijo codition to see if we got a sufficient decrease
            if (f(x_k) <= fx + step_length * armijo_term) {
                return true;
            }

            step_length /= 2;
        } while ((step_length * dir).norm() >= Constants::FISCHER_SINGULAR_TOL);

        spdlog::warn("solver=ficher_newton_lcp iter={:d} "
                     "failure='step length too small' step_length={:g}",
            iter, step_length);
        return false;
    }

} // namespace opt
} // namespace ccd