#include <iostream>
#include <iomanip>
#include <algorithm>
#include <numeric>
#include <functional>
#include <limits>
#include <ctime>
#include <cmath>
#include <cassert>

#include <boost/timer.hpp>
#include <boost/random.hpp>
#include <boost/math/tr1.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/lambda/lambda.hpp>

#include "FunctionMinimization.h"

using namespace boost::lambda;

namespace Grante {

double FunctionMinimization::BarzilaiBorweinMinimize(
	FunctionMinimizationProblem& prob,
	std::vector<double>& x_opt, double conv_tol,
	unsigned int max_iter, bool verbose) {
	unsigned int dim = prob.Dimensions();

	// Gradient, last gradient and alpha value required for BB iteration
	std::vector<double> grad(dim, 0.0);
	std::vector<double> grad_last(dim, 0.0);
	double alpha_last = std::numeric_limits<double>::infinity();

	// Initialize x
	std::vector<double> x(dim);
	prob.ProvideStartingPoint(x);

	boost::timer total_timer;
	double obj = std::numeric_limits<double>::signaling_NaN();
	for (unsigned int iter = 0; max_iter == 0 || iter < max_iter; ++iter) {
		obj = prob.Eval(x, grad);

		// Convergence check
		double grad_norm = EuclideanNorm(grad);
		if (verbose && (iter % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time      objective      |grad|" << std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << iter << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << obj << "   ";
			// Gradient norm
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << grad_norm;
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		if (grad_norm < conv_tol) {
			x_opt = x;
			return (obj);
		}

		// Choose alpha
		double alpha = grad_norm;	// Initialization heuristic
		if (iter == 0) {
			// First iteration: assert feasibility by line search
			WolfeLineSearch linesearch(&prob, x, grad, grad, obj, 1e-4, 0.9);
			linesearch.ComputeStepLength(alpha);
			alpha = 1.0 / alpha;
		}
		if (iter >= 1) {
			double anom = 0.0;
			double adenom = 0.0;
			for (unsigned int d = 0; d < dim; ++d) {
				anom += -grad_last[d]*(grad[d] - grad_last[d]);
				adenom += grad_last[d]*grad_last[d];
			}
			alpha = alpha_last * (anom / adenom);
#if 0
			std::cout << "   alpha = alpha_last(" << alpha_last
				<< ") * (anom(" << anom << ") / adenom(" << adenom << ")"
				<< " = " << alpha << std::endl;
#endif
		}
		assert(alpha > 0.0);

		// Update x
		for (unsigned int d = 0; d < dim; ++d)
			x[d] -= grad[d] / alpha;

		// Keep iterates of the gradient and stepsize
		alpha_last = alpha;
		grad_last = grad;
	}

	// Iteration limit reached
	x_opt = x;
	return (prob.Eval(x, grad));
}

double FunctionMinimization::LimitedMemoryBFGSMinimize(
	FunctionMinimizationProblem& prob,
	std::vector<double>& x_opt, double conv_tol, unsigned int max_iter,
	bool verbose, unsigned int lbfgs_m) {
	unsigned int dim = prob.Dimensions();

	// Gradient, last gradient and alpha value required for BB iteration
	std::vector<double> grad(dim, 0.0);
	std::vector<double> grad_last(dim, 0.0);

	// Initialize x
	std::vector<double> x(dim);
	prob.ProvideStartingPoint(x);
	std::vector<double> xprev(dim);
	std::vector<double> gradprev(dim);

	// List of previous s,y,rho_i
	lbfgs_mem_type lbfgs_mem;

	boost::timer total_timer;
	double obj = std::numeric_limits<double>::signaling_NaN();
	bool is_valid = false;
	bool is_restart = false;
	unsigned int ls_evals = 0;
	unsigned int iter = 0;
	for ( ; max_iter == 0 || iter < max_iter; ++iter) {
		// If no information about current query point has been computed, do
		// so now
		if (is_valid == false) {
			obj = prob.Eval(x, grad);
			is_valid = true;
		}
		double grad_norm = EuclideanNorm(grad);

		if (verbose && (iter % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time      objective      |grad|   "
				<< "mem     ls#" << std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << iter << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << obj << "   ";
			// Gradient norm
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << grad_norm;
			// LBFGS memory size
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << lbfgs_mem.size() << "  ";
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << ls_evals << "  ";
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		// Convergence check based on gradient norm
		if (prob.HasConverged(x, grad, conv_tol))
			break;	// converged

		// Insert differential information into Hessian approximation
		if (iter > 0 && is_restart == false) {
			// xprev: s_k
			std::transform(x.begin(), x.end(), xprev.begin(),
				xprev.begin(), _1 - _2);
			// gradprev: y_k
			std::transform(grad.begin(), grad.end(), gradprev.begin(),
				gradprev.begin(), _1 - _2);

			// Heuristically ensure stability by ignoring unstable updates.
			// As to what entails 'unstable' there exist different opinions.
			// TODO: replace with true damped-Newton update (in inverse H
			// form used by L-BFGS)
			double ys_p = std::inner_product(xprev.begin(), xprev.end(),
				gradprev.begin(), 0.0);
			double yy_p = std::inner_product(gradprev.begin(), gradprev.end(),
				gradprev.begin(), 0.0);
//			if (ys_p >= 1.0e-12) {
			if (ys_p >= 1.0e-12*yy_p) {
#if 0
				std::cout << "    lbfgs update with ys_p " << ys_p
					<< std::endl;
#endif
				double rho = 1.0 / ys_p;
				lbfgs_mem.push_front(lbfgs_mem_type::value_type(
					xprev, gradprev, rho));

				// Remove old element from the lbfgs memory, if necessary
				if (lbfgs_mem.size() > lbfgs_m)
					lbfgs_mem.pop_back();
			} else {
				std::cout << "    LBFGS update too large (ys "
					<< ys_p << ", yy " << yy_p << ")" << std::endl;
			}
		}
		// Save current iterate for next update
		std::copy(x.begin(), x.end(), xprev.begin());
		std::copy(grad.begin(), grad.end(), gradprev.begin());

		// Compute new ascent direction H_k \nabla_x f(x_k)
		// Recent-to-oldest
		std::list<double> alpha_list;
		for (lbfgs_mem_type::const_iterator li = lbfgs_mem.begin();
			li != lbfgs_mem.end(); ++li) {
			// alpha_i = rho_i s_i' q
			double alpha_i = li->get<2>() * std::inner_product(
				grad.begin(), grad.end(), li->get<0>().begin(), 0.0);
			// q <= q - alpha_i y_i
			std::transform(grad.begin(), grad.end(), li->get<1>().begin(),
				grad.begin(), _1 - alpha_i * _2);
			alpha_list.push_back(alpha_i);
		}
		// Diagonal scaling: q = H^0 q
		double gamma = 1.0;
		if (iter > 0 && is_restart == false && lbfgs_mem.empty() == false) {
			lbfgs_mem_type::const_iterator li_last = lbfgs_mem.begin();

			// gamma = (s_{k-1}' y_{k-1}) / (y_{k-1}' y_{k-1})
			gamma = std::inner_product(li_last->get<0>().begin(),
				li_last->get<0>().end(), li_last->get<1>().begin(), 0.0);
			gamma /= std::inner_product(li_last->get<1>().begin(),
				li_last->get<1>().end(), li_last->get<1>().begin(), 0.0);
		}
		std::transform(grad.begin(), grad.end(), grad.begin(), gamma * _1);
		// Reverse: oldest-to-recent
		std::list<double>::const_reverse_iterator ai = alpha_list.rbegin();
		for (lbfgs_mem_type::const_reverse_iterator li = lbfgs_mem.rbegin();
			li != lbfgs_mem.rend(); ++li, ++ai) {
			// beta = rho_i y_i' q
			double beta = li->get<2>() * std::inner_product(
				grad.begin(), grad.end(), li->get<1>().begin(), 0.0);

			// q <- q + (alpha_i-beta)*s_i
			std::transform(grad.begin(), grad.end(), li->get<0>().begin(),
				grad.begin(), _1 + (*ai-beta)*_2);
		}
		// Now 'grad' contains an adjusted gradient direction
		is_restart = false;

		// Check cosine angle between gradient and transformed gradient
		double x0_phi_grad = std::inner_product(grad.begin(), grad.end(),
			gradprev.begin(), 0.0);
		double cos_a = x0_phi_grad /
			(EuclideanNorm(gradprev) * EuclideanNorm(grad));
		if (cos_a <= -1.0e-8) {
			std::cout << "### FATAL: LBFGS approximation lost psd, angle "
				<< cos_a << std::endl;
			assert(0);
		} else if (cos_a <= 1.0e-7) {
			// Numerical issues, degenerate true Hessian or converged.
			std::cout << "### WARNING: LBFGS gradient orthogonality issue, "
				<< "aborting." << std::endl;
			break;
		}

		if ((boost::math::isnan)(x0_phi_grad)) {
			std::cout << "### WARNING: LBFGS gradient or perturbed gradient NaN, "
				<< "aborting." << std::endl;
			break;
		}
		// Check it is a descent direction
		if (x0_phi_grad <= -1.0e-8) {
			std::cout << "### FATAL: LBFGS approximation lost psd, x0_phi_grad "
				<< x0_phi_grad << std::endl;
			assert(0);
		} else if (x0_phi_grad <= 1.0e-10) {
#if 0
			std::cout << "### WARNING: LBFGS gradient, numerical issue, "
				<< "phi'(0) = " << x0_phi_grad << std::endl;
#endif
			// Numerical issues, degenerate true Hessian or converged.
			break;
		}

		// Perform linesearch in descent direction
		WolfeLineSearch linesearch(&prob, x, gradprev, grad, obj, 1e-4, 0.9);
		//SimpleLineSearch linesearch(&prob, x, gradprev, grad, obj);
		double alpha = 1.0;
		// Be very careful on the first step
		if (iter == 0) {
//			alpha = 1.0e-5;
			if (EuclideanNorm(grad) >= 1.0)
				alpha = 1.0 / EuclideanNorm(grad);
			alpha = std::max(1.0e-12, alpha);
		}

		ls_evals = linesearch.ComputeStepLengthUpdate(x, grad, obj, alpha);
#if 0
		std::cout << "   step size " << alpha
			<< " in " << eval << " evaluations" << std::endl;
#endif

		if ((boost::math::isnan)(alpha)) {
			std::cout << "   * Line search failed (alpha="
				<< alpha << ")" << std::endl;
			std::copy(xprev.begin(), xprev.end(), x.begin());
			break;
		}
		if (alpha <= 1.0e-12) {
			std::cout << "   * Line search yielded step size alpha="
				<< alpha << std::endl;
			if (iter > 0) {
				std::cout << "   * Assuming convergence." << std::endl;
				break;
			}
		}
		// Successful line search with up-to-date step
		is_valid = true;
	}

	// Iteration limit reached
	x_opt = x;
	return (prob.Eval(x, grad));
}

double FunctionMinimization::SubgradientMethodMinimize(
	FunctionMinimizationProblem& prob,
	std::vector<double>& x_opt, double conv_tol, unsigned int max_iter,
	bool verbose) {
	unsigned int dim = prob.Dimensions();
	std::vector<double> grad(dim, 0.0);

	// Initialize x
	std::vector<double> x(dim);
	prob.ProvideStartingPoint(x);

	boost::timer total_timer;
	for (unsigned int iter = 0; (max_iter == 0) || iter < max_iter; ++iter) {
		double obj = prob.Eval(x, grad);

		// Convergence check
		double grad_norm = EuclideanNorm(grad);
		if (verbose && (iter % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time      objective      |grad|" << std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << iter << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << obj << "   ";
			// Gradient norm
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << grad_norm;
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		if (grad_norm < conv_tol) {
			x_opt = x;
			return (obj);
		}

		// Choose step size
		double alpha_m = 200.0;
		double alpha = (1.0 + alpha_m) /
			(static_cast<double>(iter + 1) + alpha_m);
		alpha /= grad_norm * grad_norm;

		// Update
		for (unsigned int d = 0; d < dim; ++d)
			x[d] -= alpha * grad[d];
	}

	// Iteration limit reached
	x_opt = x;
	return (prob.Eval(x, grad));
}

double FunctionMinimization::GradientMethodMinimize(
	FunctionMinimizationProblem& prob,
	std::vector<double>& x_opt, double conv_tol, unsigned int max_iter,
	bool verbose) {
	unsigned int dim = prob.Dimensions();
	std::vector<double> grad(dim, 0.0);
	std::vector<double> grad_prev(dim, 0.0);

	// Initialize x
	std::vector<double> x(dim);
	prob.ProvideStartingPoint(x);

	boost::timer total_timer;
	double alpha = -1.0;
	double beta = 0.5;
	for (unsigned int iter = 0; (max_iter == 0) || iter < max_iter; ++iter) {
		double obj = prob.Eval(x, grad);

		// Convergence check
		double grad_norm = EuclideanNorm(grad);
		if (verbose && (iter % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time      objective      |grad|       alpha"
				<< std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << iter << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << obj << "   ";
			// Gradient norm
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << grad_norm
				<< "   ";
			// alpha
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setw(5)
				<< std::setiosflags(std::ios::left) << alpha;
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		if (grad_norm < conv_tol) {
			x_opt = x;
			return (obj);
		}

		// Choose step size
		if (alpha < 0.0) {
			alpha = 1.0 / (grad_norm * grad_norm);
		} else {
			if (std::inner_product(grad.begin(), grad.end(),
				grad_prev.begin(), 0.0) < 0.0) {
				alpha *= beta;
			}
		}
		std::copy(grad.begin(), grad.end(), grad_prev.begin());

		// Update
		for (unsigned int d = 0; d < dim; ++d)
			x[d] -= alpha * grad[d];
	}

	// Iteration limit reached
	x_opt = x;
	return (prob.Eval(x, grad));
}

bool FunctionMinimization::CheckDerivative(FunctionMinimizationProblem& prob,
	double x_range, unsigned int test_count, double dim_eps, double grad_tol) {
	assert(dim_eps > 0.0);
	assert(grad_tol > 0.0);

	// Random number generation, for random perturbations
	boost::mt19937 rgen(static_cast<const boost::uint32_t>(std::time(0))+1);
	boost::uniform_real<double> rdestu;	// range [0,1]
	boost::variate_generator<boost::mt19937,
		boost::uniform_real<double> > rand_perturb(rgen, rdestu);

	// Random number generation, for random dimensions
	unsigned int dim = prob.Dimensions();
	boost::mt19937 rgen2(static_cast<const boost::uint32_t>(std::time(0))+2);
	boost::uniform_int<unsigned int> rdestd(0, dim-1);
	boost::variate_generator<boost::mt19937,
		boost::uniform_int<unsigned int> > rand_dim(rgen2, rdestd);

	// Get base
	std::vector<double> x0(dim);
	prob.ProvideStartingPoint(x0);
	std::vector<double> xtest(dim);
	std::vector<double> grad(dim);
	std::vector<double> grad_d(dim);	// dummy

	for (unsigned int test_id = 0; test_id < test_count; ++test_id) {
		xtest = x0;
		for (unsigned int d = 0; d < dim; ++d)
			xtest[d] += 2.0*x_range*rand_perturb() - x_range;

		// Get exact derivative
		double xtest_fval = prob.Eval(xtest, grad);

		// Compute first-order finite difference approximation
		unsigned int test_dim = rand_dim();
		xtest[test_dim] += dim_eps;
		double xtest_d_fval = prob.Eval(xtest, grad_d);
		double deriv_fd = (xtest_d_fval - xtest_fval) / dim_eps;

		// Check accuracy
		if (fabs(deriv_fd - grad[test_dim]) > grad_tol) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			std::cout << std::endl;
			std::cout << "### DERIVATIVE CHECKER WARNING" << std::endl;
			std::cout << "### during test " << (test_id+1) << " a violation "
				<< "in gradient computation was found:" << std::endl;
			std::cout << std::setprecision(6)
				<< std::setiosflags(std::ios::scientific);
			std::cout << "### dim " << test_dim << ", exact " << grad[test_dim]
				<< ", finite-diff " << deriv_fd
				<< ", absdiff " << fabs(deriv_fd - grad[test_dim])
				<< std::endl;
			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);

			return (false);
		}
	}
	return (true);
}

double FunctionMinimization::EuclideanNorm(const std::vector<double>& vec) {
	double rs = 0.0;
	for (std::vector<double>::const_iterator vi = vec.begin();
		vi != vec.end(); ++vi) {
		rs += (*vi) * (*vi);
	}
	return (sqrt(rs));
}

// Wolfe line-search method, see [Nocedal&Wright], page 60.
FunctionMinimization::WolfeLineSearch::WolfeLineSearch(
	FunctionMinimizationProblem* prob, const std::vector<double>& x0,
	const std::vector<double>& x0_grad,
	const std::vector<double>& H_grad, double x0_fval,
	double c1, double c2)
	: prob(prob), x0(x0), x0_grad(x0_grad), H_grad(H_grad),
		x0_fval(x0_fval), x0_phi_grad(0),
		evaluation_count(0),
		xalpha_val(std::numeric_limits<double>::signaling_NaN()),
		c1(c1), c2(c2)
{
	assert(x0.size() > 0);
	xalpha.resize(x0.size());
	xalphagrad.resize(x0.size());

	// phi'(0) = - p' \nabla_x f(x_k)
	x0_phi_grad = -std::inner_product(x0_grad.begin(), x0_grad.end(),
		H_grad.begin(), 0.0);
	assert(x0_phi_grad < 0.0);
}

unsigned int FunctionMinimization::WolfeLineSearch::ComputeStepLength(
	double& alpha) {
	// Previous alpha
	double alpha_prev = 0.0;
	double alpha_prev_fval = x0_fval;
	double alpha_prev_grad = x0_phi_grad;
	double alpha_max = 1e6;
//	alpha = 1.0;

#if 0
	for (double bf = 0.0; bf < 1.0; bf += 0.01) {
		double tfval, tfgrad;
		Evaluate(bf, tfval, tfgrad);
		std::cout << bf << " " << tfval << " " << tfgrad << " # TEST" << std::endl;
	}
#endif

	// Current alpha
	double phi_alpha_fval;
	double phi_alpha_grad;

	// Find a stepsize interval satisfying the strong Wolfe conditions for a
	// given iterate x and gradient gx and descent direction d:
	//   1. Armijo: f(x + alpha d) <= f(x) + c1 alpha gx' d,
	//   2. Curvature: |nabla_alpha f(x + alpha d)| <= c2 |gx' d|.
	for (unsigned int n = 0; true; ++n) {
		Evaluate(alpha, phi_alpha_fval, phi_alpha_grad);

		// If Armijo condition is violated: zoom, as a point satisfying the
		// Wolfe condition must exist in [alpha_{i-1}, alpha_i].
		if (phi_alpha_fval > (x0_fval + c1*alpha*x0_phi_grad)) {
			if (n == 0) {
				alpha = Zoom(0.0, alpha, x0_fval, phi_alpha_fval, x0_phi_grad);
			} else {
				alpha = Zoom(alpha_prev, alpha, alpha_prev_fval,
					phi_alpha_fval, alpha_prev_grad);
			}
			break;
		}
		if (n > 0 && phi_alpha_fval >= alpha_prev_fval) {
			alpha = Zoom(alpha_prev, alpha, alpha_prev_fval,
				phi_alpha_fval, alpha_prev_grad);
			break;
		}

		// The Armijo condition is satisfied.  If in addition the curvature
		// condition ("function must be flat around alpha") is satisfied, then
		// we found a point satisfying the Wolfe conditions.
#if 0
		// STRONG
		if (std::fabs(phi_alpha_grad) <= -c2*x0_phi_grad)
			break;
#endif
		if (phi_alpha_grad >= c2*x0_phi_grad)
			break;

		// If the gradient increases again, then a point satisfying the Wolfe
		// condition must exist in [alpha_{i-1}, alpha_i].
		if (phi_alpha_grad >= 0) {
			alpha = Zoom(alpha_prev, alpha, alpha_prev_fval,
				phi_alpha_fval, alpha_prev_grad);
			break;
		}

		// Further progress can be made, scale step size up by a factor
		alpha_prev = alpha;
		alpha_prev_fval = phi_alpha_fval;
		alpha_prev_grad = phi_alpha_grad;
		alpha *= std::sqrt(2.0);
		assert(alpha <= alpha_max);
	}

#if 0
	// If line search failed: try fall back to backtracking line search
	if ((boost::math::isnan)(alpha)) {
		alpha = 1.0;
		for (unsigned int bt_try = 0; bt_try < 20; ++bt_try) {
			alpha *= 0.25;
			Evaluate(alpha, phi_alpha_fval, phi_alpha_grad);
			std::cout << "    backtrack, alpha " << alpha
				<< ", phi(a) " << phi_alpha_fval
				<< ", phi'(a) " << phi_alpha_grad << std::endl;
			std::cout << "    phi(a) = " << phi_alpha_fval
				<< " <= " << x0_fval << " + " << c1*alpha*x0_phi_grad
				<< "?" << std::endl;
			if (phi_alpha_fval <= (x0_fval + c1*alpha*x0_phi_grad))
				return (evaluation_count);
		}
		std::cout << "  Backtracking line search failed." << std::endl;
		alpha = std::numeric_limits<double>::signaling_NaN();
	}
#endif

	return (evaluation_count);
}

unsigned int FunctionMinimization::WolfeLineSearch::ComputeStepLengthUpdate(
	std::vector<double>& x_out, std::vector<double>& grad_out,
	double& fval_out, double& alpha) {
//	alpha = 0.0;
	unsigned int ecount = ComputeStepLength(alpha);

	if ((boost::math::isnan)(alpha))
		return (ecount);

	// A valid step size has been computed
	if (xalpha_val != alpha) {
		// Is not up-to-date, update
		double d1;	// dummy
		double d2;
		Evaluate(alpha, d1, d2);
	}
	assert(xalpha_val == alpha);
	x_out = xalpha;
	grad_out = xalphagrad;
	fval_out = xalphaobj;

	return (ecount);
}

// phi(alpha), alpha >= 0
void FunctionMinimization::WolfeLineSearch::Evaluate(
	double alpha, double& phi_fval, double& phi_grad) {
	// x(alpha) = x - alpha*p
	std::transform(x0.begin(), x0.end(), H_grad.begin(),
		xalpha.begin(), _1 - alpha * _2);
	std::fill(xalphagrad.begin(), xalphagrad.end(), 0.0);

	// Evaluate phi(alpha) and derivative phi'(alpha)
	// phi(alpha) = f(x_k - alpha H_grad)
	// phi'(alpha) = -H_grad' \nabla_x f(x_k - alpha H_grad)
	phi_fval = prob->Eval(xalpha, xalphagrad);
	xalphaobj = phi_fval;	// save phi(alpha)
	xalpha_val = alpha;	// save alpha

	// Univariate derivative is projection onto ascent direction
	phi_grad = -std::inner_product(xalphagrad.begin(), xalphagrad.end(),
		H_grad.begin(), 0.0);
	evaluation_count += 1;
#if 0
	std::cout << "   phi(" << alpha << ") = " << phi_fval << ", grad "
		<< phi_grad << std::endl;
#endif
}

double FunctionMinimization::WolfeLineSearch::Zoom(double alpha_lo,
	double alpha_hi, double alpha_lo_fval, double alpha_hi_fval,
	double alpha_lo_grad) {
	double phi_trial_fval;
	double phi_trial_grad;

#if 0
	double tfval, tfgrad;
	Evaluate(0.0, tfval, tfgrad);
	std::cout << "  phi(0) = " << tfval << ", grad " << tfgrad << std::endl;
#endif
#if 0
	for (double bf = alpha_lo; bf < alpha_hi; bf += 0.01) {
		double tfval, tfgrad;
		Evaluate(bf, tfval, tfgrad);
		std::cout << bf << " " << tfval << " " << tfgrad << " # TEST" << std::endl;
	}
#endif

	unsigned int tries_max = 150;
	unsigned int tries = 0;
	for (; tries < tries_max; ++tries) {
		if ((alpha_hi - alpha_lo) < 1.0e-14) {
			std::cout << "   * Wolfe line search, too small bracket: ["
				<< alpha_lo << "; " << alpha_hi << "]" << std::endl;
			//return (std::numeric_limits<double>::signaling_NaN());
			return (alpha_hi);
		}
#if 0
		{	// check preconditions: alpha_lo satisfies Armijo cond
			double tfval_lo, tfgrad_lo;
			Evaluate(alpha_lo, tfval_lo, tfgrad_lo);
			std::cout << "      ### lo: alpha = " << alpha_lo
				<< ", phi(a) = " << tfval_lo
				<< ", phi'(a) = " << tfgrad_lo << std::endl;
			assert(tfval_lo <= (x0_fval + c1*alpha_lo*x0_phi_grad));

			double tfval_hi, tfgrad_hi;
			Evaluate(alpha_hi, tfval_hi, tfgrad_hi);
			std::cout << "      ### hi: alpha = " << alpha_hi
				<< ", phi(a) = " << tfval_hi
				<< ", phi'(a) = " << tfgrad_hi << std::endl;

			// Check derivative
			double cd_fval, cd_grad;
			Evaluate(alpha_lo + 1e-8, cd_fval, cd_grad);
			double cd_apx = (cd_fval - tfval_lo) / 1e-8;
			std::cout << "        # lo deriv: " << tfgrad_lo
				<< " (exa) vs " << cd_apx << " (apx)"
				<< ", phi(0) " << x0_phi_grad
				<< std::endl;
			Evaluate(alpha_hi + 1e-8, cd_fval, cd_grad);
			cd_apx = (cd_fval - tfval_hi) / 1e-8;
			std::cout << "        # hi deriv: " << tfgrad_hi
				<< " (exa) vs " << cd_apx << " (apx)"
				<< ", phi(hi) " << tfval_hi
				<< std::endl;

#if 1
			if (std::fabs(cd_apx - tfgrad_hi) >= 1.0) {
				for (double bf = alpha_lo; bf < alpha_hi; bf +=
0.01*(alpha_hi-alpha_lo)) {
					double tfval, tfgrad;
					Evaluate(bf, tfval, tfgrad);
					std::cout << bf << " " << tfval << " " << tfgrad << " # TEST" << std::endl;
				}
			}
#endif
		}
#endif
		double alpha_trial = std::numeric_limits<double>::signaling_NaN();
		if ((boost::math::isinf)(alpha_hi_fval)) {
			alpha_trial = 0.9*alpha_lo + 0.1*alpha_hi;
			std::cout << "   * Warning: phi(" << alpha_hi << ") is NaN, "
				<< "trying bisection." << std::endl;
		} else {
			// Interpolation by quadratic, fixing
			// phi(alpha_lo), phi'(alpha_lo), phi(alpha_hi)
			double q_a = (alpha_lo_fval - alpha_hi_fval +
				alpha_lo_grad*(alpha_hi-alpha_lo)) /
				(-alpha_lo*alpha_lo - alpha_hi*alpha_hi +
					2.0*alpha_lo*alpha_hi);
			double q_b = alpha_lo_grad - 2.0*q_a*alpha_lo;
			// double q_c = alpha_hi_fval - q_a*alpha_hi*alpha_hi
			//    - alpha_lo_grad*alpha_hi + 2.0*q_a*alpha_lo*alpha_hi;
			if (q_a <= 1.0e-15) {
				std::cout << "   * Wolfe line search failed due to small "
					<< "quadratic coefficient (q_a=" << q_a << ")." << std::endl;
				std::cout << "     alpha_lo/hi " << alpha_lo << ", "
					<< alpha_hi << std::endl;
				std::cout << "     alpha_lo/hi fval " << alpha_lo_fval << ", "
					<< alpha_hi_fval << std::endl;
				std::cout << "     alpha_lo grad " << alpha_lo_grad << std::endl;
				break;
			}
			alpha_trial = - q_b / (2.0*q_a);
		}
#if 0
		std::cout << "        # alpha_trial: " << alpha_trial
			<< std::endl;
#endif
		Evaluate(alpha_trial, phi_trial_fval, phi_trial_grad);

#if 0
		std::cout << "        # Armijo (c1): "
			<< ((phi_trial_fval <= x0_fval + c1*alpha_trial*x0_phi_grad) ?
				"SATISFIED" : "NOT SATISFIED") << std::endl;
		std::cout << "        # Curvature (c2): "
			<< ((phi_trial_grad >= c2*x0_phi_grad) ? "SATISFIED"
				: "NOT SATISFIED") << std::endl;
		std::cout << "        #   phi'(a) = " << phi_trial_grad
			<< " >= c2*" << x0_phi_grad
			<< " = " << (c2*x0_phi_grad) << std::endl;
#endif

		// Check Armijo condition:
		// phi(alpha) <= phi(0) + c1 alpha phi'(0)
		if (phi_trial_fval > (x0_fval + c1*alpha_trial*x0_phi_grad) ||
			phi_trial_fval >= alpha_lo_fval) {
			// Decrease upper bracket
			alpha_hi = alpha_trial;
			continue;
		}

		// Found a point satisfying the Wolfe conditions
#if 0
		// Strong Wolfe
		if (std::fabs(phi_trial_grad) <= -c2*x0_phi_grad)
			return (alpha_trial);
#endif
		if (phi_trial_grad >= c2*x0_phi_grad)
			return (alpha_trial);

		// Flip bracket
		if (phi_trial_grad*(alpha_hi - alpha_lo) >= 0.0)
			alpha_hi = alpha_lo;

		// Increase lower bracket
		alpha_lo = alpha_trial;
		alpha_lo_fval = phi_trial_fval;
	}

	if (tries >= tries_max) {
		std::cout << "   * Wolfe line search exhausted function evaluation "
			<< "budget (" << tries_max << ")." << std::endl;
	}
	return (std::numeric_limits<double>::signaling_NaN());
}

FunctionMinimization::SimpleLineSearch::SimpleLineSearch(
	FunctionMinimizationProblem* prob, const std::vector<double>& x0,
	const std::vector<double>& x0_grad,
	const std::vector<double>& H_grad, double x0_fval)
	: prob(prob), x0(x0), x0_grad(x0_grad), H_grad(H_grad),
		x0_fval(x0_fval), x0_phi_grad(0),
		evaluation_count(0),
		xalpha_val(std::numeric_limits<double>::signaling_NaN())
{
	assert(x0.size() > 0);
	xalpha.resize(x0.size());
	xalphagrad.resize(x0.size());

	// phi'(0) = - p' \nabla_x f(x_k)
	x0_phi_grad = -std::inner_product(x0_grad.begin(), x0_grad.end(),
		H_grad.begin(), 0.0);
	assert(x0_phi_grad < 0.0);
}

unsigned int FunctionMinimization::SimpleLineSearch::ComputeStepLength(
	double& alpha) {
	// Previous alpha
	double alpha_min = 1.0e-12;
	double alpha_max = 1e6;

	double fa_enlarge = 1.7;
	double fa_shrink = 0.5;

	// Current alpha
	double phi_alpha_fval;
	double phi_alpha_grad;

	// Find a stepsize interval satisfying the strong Wolfe conditions for a
	// given iterate x and gradient gx and descent direction d:
	//   1. Armijo: f(x + alpha d) <= f(x) + c1 alpha gx' d,
	//   2. Curvature: |nabla_alpha f(x + alpha d)| <= c2 |gx' d|.
	unsigned int max_test = 200;
	for (unsigned int n = 0; true; ++n) {
#if 0
		std::cout << "SL n " << n << ", alpha " << alpha << std::endl;
#endif
		if (alpha <= alpha_min) {
			break;
		} else if (alpha >= alpha_max || n >= max_test) {
			if (n >= max_test)
				std::cout << "### WARNING: line search count exceeded, alpha "
<< alpha << std::endl;
			alpha = std::numeric_limits<double>::signaling_NaN();
			break;
		}
		Evaluate(alpha, phi_alpha_fval, phi_alpha_grad);

		// If Armijo condition is violated, shrink.
		if (phi_alpha_fval > (x0_fval + 1.0e-4*alpha*x0_phi_grad)) {
			alpha *= fa_shrink;
#if 0
			std::cout << "    armijo fail, shrink to " << alpha << std::endl;
#endif
			continue;
		}

		// The Armijo condition is satisfied.  If in addition the curvature
		// condition ("function must be flat around alpha") is satisfied, then
		// we found a point satisfying the Wolfe conditions.
		// Otherwise, enlarge alpha.
		if (phi_alpha_grad < 0.9*x0_phi_grad) {
			alpha *= fa_enlarge;
#if 0
			std::cout << "    wolfe fail, increase to " << alpha << std::endl;
#endif
			continue;
		}

		break;
	}

	return (evaluation_count);
}

unsigned int FunctionMinimization::SimpleLineSearch::ComputeStepLengthUpdate(
	std::vector<double>& x_out, std::vector<double>& grad_out,
	double& fval_out, double& alpha) {
	unsigned int ecount = ComputeStepLength(alpha);

	if ((boost::math::isnan)(alpha))
		return (ecount);

	// A valid step size has been computed
	if (xalpha_val != alpha) {
		// Is not up-to-date, update
		double d1;	// dummy
		double d2;
		Evaluate(alpha, d1, d2);
	}
	assert(xalpha_val == alpha);
	x_out = xalpha;
	grad_out = xalphagrad;
	fval_out = xalphaobj;

	return (ecount);
}

void FunctionMinimization::SimpleLineSearch::Evaluate(
	double alpha, double& phi_fval, double& phi_grad) {
	// x(alpha) = x - alpha*p
	std::transform(x0.begin(), x0.end(), H_grad.begin(),
		xalpha.begin(), _1 - alpha * _2);
	std::fill(xalphagrad.begin(), xalphagrad.end(), 0.0);

	// Evaluate phi(alpha) and derivative phi'(alpha)
	// phi(alpha) = f(x_k - alpha H_grad)
	// phi'(alpha) = -H_grad' \nabla_x f(x_k - alpha H_grad)
	phi_fval = prob->Eval(xalpha, xalphagrad);
	xalphaobj = phi_fval;	// save phi(alpha)
	xalpha_val = alpha;	// save alpha

	// Univariate derivative is projection onto ascent direction
	phi_grad = -std::inner_product(xalphagrad.begin(), xalphagrad.end(),
		H_grad.begin(), 0.0);
	evaluation_count += 1;
#if 0
	std::cout << "   phi(" << alpha << ") = " << phi_fval << ", grad "
		<< phi_grad << std::endl;
#endif
}

}


#include "DelaunayTriangulation.h"

#include <Eigen/Dense>

#include "SIMPLib/Math/MatrixMath.h"

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
DelaunayTriangulation::DelaunayTriangulation(TriMesh::VertexCoordList vertices, double offset, double tolerance, double alpha, Observable* observable)
: m_Vertices(vertices)
, m_Offset(offset)
, m_Tolerance(tolerance)
, m_Alpha(alpha)
, m_Observer(observable)
{
  initialize();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
DelaunayTriangulation::~DelaunayTriangulation()
{
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DelaunayTriangulation::initialize()
{
  m_PointBounds[0] = std::numeric_limits<double>::max();
  m_PointBounds[1] = std::numeric_limits<double>::lowest();
  m_PointBounds[2] = std::numeric_limits<double>::max();
  m_PointBounds[3] = std::numeric_limits<double>::lowest();
  m_PointBounds[4] = std::numeric_limits<double>::max();
  m_PointBounds[5] = std::numeric_limits<double>::lowest();

  m_NumDuplicatePoints = 0;
  m_NumDegeneracies = 0;

  m_Delaunay = TriMesh::NullPointer();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
TriangleGeom::Pointer DelaunayTriangulation::triangulate()
{
  initialize();

  if(m_Observer)
  {
    connect(this, SIGNAL(filterGeneratedMessage(const PipelineMessage&)), m_Observer, SLOT(broadcastPipelineMessage(const PipelineMessage&)));
  }

  Eigen::Transform<double, 3, Eigen::Affine> transform = findProjectionPlane();

  TriMesh::VertexCoordList projectedVertices(m_Vertices);

  auto numVerts = m_Vertices.size();

  for(size_t v = 0; v < numVerts; v++)
  {
    Eigen::Vector3d point(m_Vertices[v][0], m_Vertices[v][1], m_Vertices[v][2]);
    // Eigen::Vector3d transformedPoint = transform * point;
    // projectedVertices[v][0] = transformedPoint(0);
    // projectedVertices[v][1] = transformedPoint(1);
    // projectedVertices[v][2] = transformedPoint(2);
    projectedVertices[v][0] = point(0);
    projectedVertices[v][1] = point(1);
    projectedVertices[v][2] = point(2);
  }

  findPointBounds(projectedVertices);

  double center[3];
  center[0] = (m_PointBounds[0] + m_PointBounds[1]) / 2.0;
  center[1] = (m_PointBounds[2] + m_PointBounds[3]) / 2.0;
  center[2] = (m_PointBounds[4] + m_PointBounds[5]) / 2.0;

  double diff = 0.0;
  double l = 0.0;

  for(size_t i = 0; i < 3; i++)
  {
    diff = static_cast<double>(m_PointBounds[2 * i + 1]) - static_cast<double>(m_PointBounds[2 * i]);
    l += diff * diff;
  }
  double tol = sqrt(l);
  double radius = m_Offset * tol;
  tol *= m_Tolerance;

  double x[3];

  for(size_t ptId = 0; ptId < 8; ptId++)
  {
    x[0] = center[0] + radius * cos(ptId * 45.0 * SIMPLib::Constants::k_PiOver180);
    x[1] = center[1] + radius * sin(ptId * 45.0 * SIMPLib::Constants::k_PiOver180);
    x[2] = center[2];
    projectedVertices.emplace_back(std::initializer_list<float>{float(x[0]), float(x[1]), float(x[2])});
  }

  m_Delaunay = TriMesh::New(projectedVertices);
  m_Delaunay->addTriangle(numVerts + 0, numVerts + 1, numVerts + 2);
  m_Delaunay->addTriangle(numVerts + 2, numVerts + 3, numVerts + 4);
  m_Delaunay->addTriangle(numVerts + 4, numVerts + 5, numVerts + 6);
  m_Delaunay->addTriangle(numVerts + 6, numVerts + 7, numVerts + 0);
  m_Delaunay->addTriangle(numVerts + 0, numVerts + 2, numVerts + 6);
  m_Delaunay->addTriangle(numVerts + 2, numVerts + 4, numVerts + 6);

  int64_t nei[3];
  int64_t neiPts[3];
  int64_t tri[4];
  int64_t pts[3];
  int64_t nodes[4][3];
  int64_t p1;
  int64_t p2;

  tri[0] = 0;

  int64_t progIncrement = static_cast<int64_t>(numVerts / 100);
  int64_t prog = 1;
  int64_t progressInt = 0;
  int64_t counter = 0;

  for(int64_t ptId = 0; ptId < int64_t(numVerts); ptId++)
  {
    x[0] = projectedVertices[ptId][0];
    x[1] = projectedVertices[ptId][1];
    x[2] = projectedVertices[ptId][2];

    nei[0] = (-1); // where we are coming from...nowhere initially

    if((tri[0] = findTriangle(x, tri[0], tol, nei, pts)) >= 0)
    {
      if(nei[0] < 0) // in triangle
      {
        // delete this triangle; create three new triangles
        // first triangle is replaced with one of the new ones

        nodes[0][0] = ptId;
        nodes[0][1] = pts[0];
        nodes[0][2] = pts[1];
        m_Delaunay->removeLinkFromTriangle(pts[2], tri[0]);
        m_Delaunay->replaceTriangleVertices(nodes[0][0], nodes[0][1], nodes[0][2], tri[0]);
        m_Delaunay->addLinkToTriangle(ptId, tri[0]);

        nodes[1][0] = ptId;
        nodes[1][1] = pts[1];
        nodes[1][2] = pts[2];
        tri[1] = m_Delaunay->addTriangle(nodes[1][0], nodes[1][1], nodes[1][2]);

        nodes[2][0] = ptId;
        nodes[2][1] = pts[2];
        nodes[2][2] = pts[0];
        tri[2] = m_Delaunay->addTriangle(nodes[2][0], nodes[2][1], nodes[2][2]);

        // Check edge neighbors for Delaunay criterion. If not satisfied, flip
        // edge diagonal. (This is done recursively.)
        checkEdge(ptId, x, pts[0], pts[1], tri[0], true);
        checkEdge(ptId, x, pts[1], pts[2], tri[1], true);
        checkEdge(ptId, x, pts[2], pts[0], tri[2], true);
      }

      else // on triangle edge
      {
        // update cell list
        m_Delaunay->getTriangleVertices(nei[0], neiPts);
        for(size_t i = 0; i < 3; i++)
        {
          if(neiPts[i] != nei[1] && neiPts[i] != nei[2])
          {
            p1 = neiPts[i];
          }
          if(pts[i] != nei[1] && pts[i] != nei[2])
          {
            p2 = pts[i];
          }
        }

        m_Delaunay->removeLinkFromTriangle(nei[2], tri[0]);
        m_Delaunay->removeLinkFromTriangle(nei[2], nei[0]);

        nodes[0][0] = ptId;
        nodes[0][1] = p2;
        nodes[0][2] = nei[1];
        m_Delaunay->replaceTriangleVertices(nodes[0][0], nodes[0][1], nodes[0][2], tri[0]);

        nodes[1][0] = ptId;
        nodes[1][1] = p1;
        nodes[1][2] = nei[1];
        m_Delaunay->replaceTriangleVertices(nodes[1][0], nodes[1][1], nodes[1][2], nei[0]);

        m_Delaunay->addLinkToTriangle(ptId, tri[0]);
        m_Delaunay->addLinkToTriangle(ptId, nei[0]);

        tri[1] = nei[0];

        nodes[2][0] = ptId;
        nodes[2][1] = p2;
        nodes[2][2] = nei[2];
        tri[2] = m_Delaunay->addTriangle(nodes[2][0], nodes[2][1], nodes[2][2]);

        nodes[3][0] = ptId;
        nodes[3][1] = p1;
        nodes[3][2] = nei[2];
        tri[3] = m_Delaunay->addTriangle(nodes[3][0], nodes[3][1], nodes[3][2]);

        // Check edge neighbors for Delaunay criterion.
        for(size_t i = 0; i < 4; i++)
        {
          checkEdge(ptId, x, nodes[i][1], nodes[i][2], tri[i], true);
        }
      }
    } // if triangle found

    else
    {
      tri[0] = 0; // no triangle found
    }

    if(counter > prog)
    {
      progressInt = static_cast<int64_t>((static_cast<float>(counter) / numVerts) * 100.0f);
      QString ss = m_MessageTitle + QObject::tr(" || %1% Complete").arg(progressInt);
      (m_MessagePrefix, "", ss);
      prog = prog + progIncrement;
    }
    counter++;
  } // for all points

  std::vector<int64_t> triUse(m_Delaunay->getNumberOfTriangles(), 1);

  for(int64_t ptId = numVerts; ptId < int64_t((numVerts + 8)); ptId++)
  {
    std::vector<int64_t> neighborTris = m_Delaunay->getTrianglesToVertex(ptId);
    for(auto&& neighbor : neighborTris)
    {
      triUse[neighbor] = 0;
    }
  }

  ////alpha begin
  // if(m_Alpha > 0.0)
  //{
  //  double alpha2 = m_Alpha * m_Alpha;
  //  double x1[3], x2[3], x3[3];
  //  double xx1[3], xx2[3], xx3[3];
  //  int64_t cellId, numNei, ap1, ap2, neighbor;

  //  TriMesh::Pointer alphaVert = TriMesh::New(projectedVertices);

  //  vtkCellArray *alphaVerts = vtkCellArray::New();
  //  alphaVerts->Allocate(numPoints);
  //  vtkCellArray *alphaLines = vtkCellArray::New();
  //  alphaLines->Allocate(numPoints);

  //  char *pointUse = new char[numPoints + 8];
  //  for(ptId = 0; ptId < (numPoints + 8); ptId++)
  //  {
  //    pointUse[ptId] = 0;
  //  }

  //  //traverse all triangles; evaluating Delaunay criterion
  //  for(i = 0; i < numTriangles; i++)
  //  {
  //    if(triUse[i] == 1)
  //    {
  //      this->Mesh->GetCellPoints(i, npts, triPts);

  //      // if any point is one of the bounding points that was added
  //      // at the beginning of the algorithm, then grab the points
  //      // from the variable "points" (this list has the boundary
  //      // points and the original points have been transformed by the
  //      // input transform).  if none of the points are bounding points,
  //      // then grab the points from the variable "inPoints" so the alpha
  //      // criterion is applied in the nontransformed space.
  //      if(triPts[0]<numPoints && triPts[1]<numPoints && triPts[2]<numPoints)
  //      {
  //        inPoints->GetPoint(triPts[0], x1);
  //        inPoints->GetPoint(triPts[1], x2);
  //        inPoints->GetPoint(triPts[2], x3);
  //      }
  //      else
  //      {
  //        points->GetPoint(triPts[0], x1);
  //        points->GetPoint(triPts[1], x2);
  //        points->GetPoint(triPts[2], x3);
  //      }

  //      // evaluate the alpha criterion in 3D
  //      vtkTriangle::ProjectTo2D(x1, x2, x3, xx1, xx2, xx3);
  //      if(vtkTriangle::Circumcircle(xx1, xx2, xx3, center) > alpha2)
  //      {
  //        triUse[i] = 0;
  //      }
  //      else
  //      {
  //        for(int j = 0; j<3; j++)
  //        {
  //          pointUse[triPts[j]] = 1;
  //        }
  //      }
  //    }//if non-deleted triangle
  //  }//for all triangles

  //  //traverse all edges see whether we need to create some
  //  for(cellId = 0, triangles->InitTraversal();
  //      triangles->GetNextCell(npts, triPts); cellId++)
  //  {
  //    if(!triUse[cellId])
  //    {
  //      for(i = 0; i < npts; i++)
  //      {
  //        ap1 = triPts[i];
  //        ap2 = triPts[(i + 1) % npts];

  //        if(this->BoundingTriangulation || (ap1<numPoints && ap2<numPoints))
  //        {
  //          this->Mesh->GetCellEdgeNeighbors(cellId, ap1, ap2, neighbors);
  //          numNei = neighbors->GetNumberOfIds();

  //          if(numNei < 1 || ((neighbor = neighbors->GetId(0)) > cellId
  //            && !triUse[neighbor]))
  //          {//see whether edge is shorter than Alpha

  //            // same argument as above, if one is a boundary point, get
  //            // it using this->GetPoint() which are transformed points. if
  //            // neither of the points are boundary points, get the from
  //            // inPoints (untransformed points) so alpha comparison done
  //            // untransformed space
  //            if(ap1 < numPoints && ap2 < numPoints)
  //            {
  //              inPoints->GetPoint(ap1, x1);
  //              inPoints->GetPoint(ap2, x2);
  //            }
  //            else
  //            {
  //              this->GetPoint(ap1, x1);
  //              this->GetPoint(ap2, x2);
  //            }
  //            if((vtkMath::Distance2BetweenPoints(x1, x2)*0.25) <= alpha2)
  //            {
  //              pointUse[ap1] = 1; pointUse[ap2] = 1;
  //              pts[0] = ap1;
  //              pts[1] = ap2;
  //              alphaLines->InsertNextCell(2, pts);
  //            }//if passed test
  //          }//test edge
  //        }//if valid edge
  //      }//for all edges of this triangle
  //    }//if triangle not output
  //  }//for all triangles

  //  //traverse all points, create vertices if none used
  //  for(ptId = 0; ptId<(numPoints + 8); ptId++)
  //  {
  //    if((ptId < numPoints || this->BoundingTriangulation)
  //       && !pointUse[ptId])
  //    {
  //      pts[0] = ptId;
  //      alphaVerts->InsertNextCell(1, pts);
  //    }
  //  }

  //  // update output
  //  delete[] pointUse;
  //  output->SetVerts(alphaVerts);
  //  alphaVerts->Delete();
  //  output->SetLines(alphaLines);
  //  alphaLines->Delete();
  //}
  ////alpha end

  fixupBoundaryTriangles(numVerts, projectedVertices, triUse);

  // Remove the 8 bounding triangle vertices
  TriMesh::VertexList delVerts = m_Delaunay->getVertices();
  for(size_t i = numVerts; i < 8; i++)
  {
    delVerts.pop_back();
  }

  // std::vector<int64_t> vertsToRemove(8);
  // std::iota(vertsToRemove.begin(), vertsToRemove.end(), numVerts);
  // m_Delaunay->removeVertices(vertsToRemove);

  size_t numGoodTris = 0;

  for(size_t i = 0; i < triUse.size(); i++)
  {
    if(triUse[i])
    {
      numGoodTris++;
    }
  }

  std::vector<std::vector<float>> untransformedVerts;
  for(auto i = 0; i < numVerts; i++)
  {
    untransformedVerts.push_back(delVerts[i].vert);
  }

  SharedVertexList::Pointer vertices = TriangleGeom::CreateSharedVertexList(untransformedVerts.size());
  TriangleGeom::Pointer triangles = TriangleGeom::CreateGeometry(numGoodTris, vertices, SIMPL::Geometry::TriangleGeometry);
  float* vertPtr = triangles->getVertexPointer(0);
  int64_t* triPtr = triangles->getTriPointer(0);

  TriMesh::TriList triList = m_Delaunay->getTriangles();

  size_t triIter = 0;
  int64_t triVerts[3];

  for(int64_t i = 0; i < int64_t(triList.size()); i++)
  {
    if(triUse[i])
    {
      m_Delaunay->getTriangleVertices(i, triVerts);
      triPtr[3 * triIter + 0] = triVerts[0];
      triPtr[3 * triIter + 1] = triVerts[1];
      triPtr[3 * triIter + 2] = triVerts[2];
      triIter++;
    }
  }

  for(auto i = 0; i < untransformedVerts.size(); i++)
  {
    Eigen::Vector3d point(untransformedVerts[i][0], untransformedVerts[i][1], untransformedVerts[i][2]);
    // Eigen::Vector3d transformedPoint = transform.inverse() * point;
    // vertPtr[3 * i + 0] = transformedPoint(0);
    // vertPtr[3 * i + 1] = transformedPoint(1);
    // vertPtr[3 * i + 2] = transformedPoint(2);
    vertPtr[3 * i + 0] = point(0);
    vertPtr[3 * i + 1] = point(1);
    vertPtr[3 * i + 2] = point(2);
  }

  return triangles;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
int64_t DelaunayTriangulation::findTriangle(double x[3], int64_t tri, double tol, int64_t nei[3], int64_t pts[3])
{
  int i, j, ir, ic, inside, i2, i3;
  int64_t newNei;
  double p[3][3], n[2], vp[2], vx[2], dp, minProj;

  m_Delaunay->getTriangleVertices(tri, pts);
  m_Delaunay->getVertexCoordinates(pts[0], p[0]);
  m_Delaunay->getVertexCoordinates(pts[1], p[1]);
  m_Delaunay->getVertexCoordinates(pts[2], p[2]);

  srand(tri);
  ir = rand() % 3;
  const double del2D_tolerance = 1.0e-014;

  for(inside = 1, minProj = del2D_tolerance, ic = 0; ic < 3; ic++)
  {
    i = (ir + ic) % 3;
    i2 = (i + 1) % 3;
    i3 = (i + 2) % 3;

    // create a 2D edge normal to define a "half-space"; evaluate points (i.e.,
    // candidate point and other triangle vertex not on this edge).
    n[0] = -(p[i2][1] - p[i][1]);
    n[1] = p[i2][0] - p[i][0];
    Normalize2x1(n);

    // compute local vectors
    for(j = 0; j < 2; j++)
    {
      vp[j] = p[i3][j] - p[i][j];
      vx[j] = x[j] - p[i][j];
    }

    // check for duplicate point
    Normalize2x1(vp);
    if(Normalize2x1(vx) <= tol)
    {
      m_NumDuplicatePoints++;
      return -1;
    }

    // see if two points are in opposite half spaces
    dp = Dot2D(n, vx) * (Dot2D(n, vp) < 0 ? -1.0 : 1.0);
    if(dp < del2D_tolerance)
    {
      if(dp < minProj) // track edge most orthogonal to point direction
      {
        inside = 0;
        nei[1] = pts[i];
        nei[2] = pts[i2];
        minProj = dp;
      }
    } // outside this edge
  }   // for each edge

  if(inside) // all edges have tested positive
  {
    nei[0] = (-1);
    return tri;
  }

  else if(!inside && (fabs(minProj) < del2D_tolerance)) // on edge
  {
    nei[0] = m_Delaunay->getTriangleEdgeNeighbor(nei[1], nei[2], tri);
    return tri;
  }

  else // walk towards point
  {
    newNei = m_Delaunay->getTriangleEdgeNeighbor(nei[1], nei[2], tri);
    if(newNei == nei[0])
    {
      m_NumDegeneracies++;
      return -1;
    }
    else
    {
      nei[0] = tri;
      return findTriangle(x, newNei, tol, nei, pts);
    }
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DelaunayTriangulation::checkEdge(int64_t point, double x[3], int64_t p1, int64_t p2, int64_t tri, bool recursive)
{
  double x1[3], x2[3], x3[3];
  int64_t neighbor;

  m_Delaunay->getVertexCoordinates(p1, x1);
  m_Delaunay->getVertexCoordinates(p2, x2);

  neighbor = m_Delaunay->getTriangleEdgeNeighbor(p1, p2, tri);

  if(neighbor > 0) // i.e., not a boundary edge
  {
    // get neighbor info including opposite point
    int64_t oppositeVert = m_Delaunay->getOppositeVertex(p1, p2, neighbor);
    if(oppositeVert < 0)
    {
      return;
    }
    m_Delaunay->getVertexCoordinates(oppositeVert, x3);

    // see whether point is in circumcircle
    if(inCircumcircle(x3, x, x1, x2))
    { // swap diagonal
      m_Delaunay->removeLinkFromTriangle(p1, tri);
      m_Delaunay->removeLinkFromTriangle(p2, neighbor);
      m_Delaunay->addLinkToTriangle(point, neighbor);
      m_Delaunay->addLinkToTriangle(oppositeVert, tri);

      m_Delaunay->replaceTriangleVertices(point, oppositeVert, p2, tri);
      m_Delaunay->replaceTriangleVertices(point, p1, oppositeVert, neighbor);

      if(recursive)
      {
        // two new edges become suspect
        checkEdge(point, x, oppositeVert, p2, tri, true);
        checkEdge(point, x, p1, oppositeVert, neighbor, true);
      }
    } // in circle
  }   // interior edge
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DelaunayTriangulation::findPointBounds(TriMesh::VertexCoordList& vertices)
{
  for(auto&& vert : vertices)
  {
    if(vert[0] < m_PointBounds[0])
    {
      m_PointBounds[0] = static_cast<double>(vert[0]);
    }
    if(vert[0] > m_PointBounds[1])
    {
      m_PointBounds[1] = static_cast<double>(vert[0]);
    }
    if(vert[1] < m_PointBounds[2])
    {
      m_PointBounds[2] = static_cast<double>(vert[1]);
    }
    if(vert[1] > m_PointBounds[3])
    {
      m_PointBounds[3] = static_cast<double>(vert[1]);
    }
    if(vert[2] < m_PointBounds[4])
    {
      m_PointBounds[4] = static_cast<double>(vert[2]);
    }
    if(vert[2] > m_PointBounds[5])
    {
      m_PointBounds[5] = static_cast<double>(vert[2]);
    }
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DelaunayTriangulation::fixupBoundaryTriangles(int64_t numVerts, TriMesh::VertexCoordList& points, std::vector<int64_t>& triUse)
{
  bool isConnected;
  int64_t numSwaps = 0;
  int64_t p1;
  int64_t p2;
  int64_t p3;
  int64_t triPts[3];
  int64_t neiPts[3];
  int64_t pts[3];
  int64_t swapPts[3];
  double n1[3];
  double n2[3];

  for(int64_t ptId = 0; ptId < numVerts; ptId++)
  {
    // check if point is only connected to triangles scheduled for
    // removal
    std::vector<int64_t> neighbors = m_Delaunay->getTrianglesToVertex(ptId);
    auto ncells = neighbors.size();

    isConnected = false;

    for(size_t i = 0; i < ncells; i++)
    {
      if(triUse[neighbors[i]])
      {
        isConnected = true;
        break;
      }
    }

    // this point will be connected in the output
    if(isConnected)
    {
      // point is connected: continue
      continue;
    }

    // This point is only connected to triangles scheduled for removal.
    // Therefore it will not be connected in the output triangulation.
    // Let's swap edges to create a triangle with 3 inner points.
    // - inner points have an id < numPoints
    // - boundary point ids are, numPoints <= id < numPoints+8.

    // visit every edge connected to that point.
    // check the 2 triangles touching at that edge.
    // if one triangle is connected to 2 non-boundary points

    for(auto i = 0; i < ncells; i++)
    {
      int64_t tri1 = neighbors[i];
      m_Delaunay->getTriangleVertices(tri1, triPts);

      if(triPts[0] == ptId)
      {
        p1 = triPts[1];
        p2 = triPts[2];
      }
      else if(triPts[1] == ptId)
      {
        p1 = triPts[2];
        p2 = triPts[0];
      }
      else
      {
        p1 = triPts[0];
        p2 = triPts[1];
      }

      // if both p1 & p2 are boundary points,
      // we skip them.
      if(p1 >= numVerts && p2 >= numVerts)
      {
        continue;
      }

      int64_t tri2 = m_Delaunay->getTriangleEdgeNeighbor(p1, p2, tri1);

      // get the 3 points of the neighbor triangle
      m_Delaunay->getTriangleVertices(tri2, neiPts);

      // locate the point different from p1 and p2
      if(neiPts[0] != p1 && neiPts[0] != p2)
      {
        p3 = neiPts[0];
      }
      else if(neiPts[1] != p1 && neiPts[1] != p2)
      {
        p3 = neiPts[1];
      }
      else
      {
        p3 = neiPts[2];
      }

      // create the two new triangles.
      // we just need to replace their pt ids.
      pts[0] = ptId;
      pts[1] = p1;
      pts[2] = p3;

      swapPts[0] = ptId;
      swapPts[1] = p3;
      swapPts[2] = p2;

      findTriangleNormal(pts, n1);
      findTriangleNormal(swapPts, n2);

      // the normals must be along the same direction,
      // or one triangle is upside down.
      if(MatrixMath::DotProduct3x1(n1, n2) < 0.0)
      {
        // do not swap diagonal
        continue;
      }

      // swap edge [p1 p2] and diagonal [ptId p3]

      // it's ok to swap the diagonal
      m_Delaunay->removeLinkFromTriangle(p1, tri2);
      m_Delaunay->removeLinkFromTriangle(p2, tri1);
      m_Delaunay->addLinkToTriangle(ptId, tri2);
      m_Delaunay->addLinkToTriangle(p3, tri1);

      m_Delaunay->replaceTriangleVertices(pts[0], pts[1], pts[2], tri1);
      m_Delaunay->replaceTriangleVertices(swapPts[0], swapPts[1], swapPts[2], tri2);

      triUse[tri1] = (p1 < numVerts && p3 < numVerts);
      triUse[tri2] = (p3 < numVerts && p2 < numVerts);

      // update the 'scheduled for removal' flag of the first triangle.
      // The second triangle was not scheduled for removal anyway.
      numSwaps++;
    }
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
double DelaunayTriangulation::circumcircle(double a[2], double b[2], double c[2], double center[2])
{
  double n12[2] = {0.0, 0.0};
  double n13[2] = {0.0, 0.0};
  double x12[2] = {0.0, 0.0};
  double x13[2] = {0.0, 0.0};
  double sum = 0.0;
  double diff = 0.0;
  size_t i;

  for(i = 0; i < 2; i++)
  {
    n12[i] = b[i] - a[i];
    n13[i] = c[i] - a[i];
    x12[i] = (b[i] + a[i]) / 2.0;
    x13[i] = (c[i] + a[i]) / 2.0;
  }

  Eigen::Matrix2d A;
  A << n12[0], n12[1], n13[0], n13[1];
  Eigen::Vector2d rhs;
  rhs << Dot2D(n12, x12), Dot2D(n13, x13);
  Eigen::Vector2d solution = A.colPivHouseholderQr().solve(rhs);

  center[0] = solution(0);
  center[1] = solution(1);

  for(sum = 0, i = 0; i < 2; i++)
  {
    diff = a[i] - center[i];
    sum += diff * diff;
    diff = b[i] - center[i];
    sum += diff * diff;
    diff = c[i] - center[i];
    sum += diff * diff;
  }

  if((sum /= 3.0) > std::numeric_limits<double>::max())
  {
    return std::numeric_limits<double>::max();
  }
  else
  {
    return sum;
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
int32_t DelaunayTriangulation::inCircumcircle(double p[3], double a[3], double b[3], double c[3])
{
  double radius2 = 0.0;
  double center[2] = {0.0, 0.0};
  double dist2 = 0.0;

  radius2 = circumcircle(a, b, c, center);

  dist2 = (p[0] - center[0]) * (p[0] - center[0]) + (p[1] - center[1]) * (p[1] - center[1]);

  if(dist2 < (0.999999999999 * radius2))
  {
    return 1;
  }
  else
  {
    return 0;
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
double DelaunayTriangulation::Determinant3x3(double g[3][3])
{
  return (g[0][0] * (g[1][1] * g[2][2] - g[1][2] * g[2][1])) - (g[0][1] * (g[1][0] * g[2][2] - g[1][2] * g[2][0])) + (g[0][2] * (g[1][0] * g[2][1] - g[1][1] * g[2][0]));
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
double DelaunayTriangulation::Dot2D(double a[2], double b[2])
{
  return (a[0] * b[0] + a[1] * b[1]);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
double DelaunayTriangulation::Normalize2x1(double n[2])
{
  double denom;
  denom = sqrt(((n[0] * n[0]) + (n[1] * n[1])));
  if(denom != 0)
  {
    n[0] /= denom;
    n[1] /= denom;
  }
  return denom;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DelaunayTriangulation::findTriangleNormal(int64_t triPoints[3], double n[3])
{
  double length;

  double ax, ay, az, bx, by, bz;

  double a[3];
  double b[3];
  double c[3];

  m_Delaunay->getVertexCoordinates(triPoints[0], a);
  m_Delaunay->getVertexCoordinates(triPoints[1], b);
  m_Delaunay->getVertexCoordinates(triPoints[2], c);

  // order is important!!! maintain consistency with triangle vertex order
  ax = c[0] - b[0];
  ay = c[1] - b[1];
  az = c[2] - b[2];
  bx = a[0] - b[0];
  by = a[1] - b[1];
  bz = a[2] - b[2];

  n[0] = (ay * bz - az * by);
  n[1] = (az * bx - ax * bz);
  n[2] = (ax * by - ay * bx);

  if((length = sqrt((n[0] * n[0] + n[1] * n[1] + n[2] * n[2]))) != 0.0)
  {
    n[0] /= length;
    n[1] /= length;
    n[2] /= length;
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
Eigen::Transform<double, 3, Eigen::Affine> DelaunayTriangulation::findProjectionPlane()
{
  auto numVertices = m_Vertices.size();
  double matrix[3][3] = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}};
  double normal[3] = {0.0, 0.0, 0.0};
  double origin[3] = {0.0, 0.0, 0.0};
  double v[3] = {0.0, 0.0, 0.0};

  double m[9];
  double *c1, *c2, *c3, det;

  const double tolerance = 1.0e-03;

  for(auto&& vert : m_Vertices)
  {
    v[0] += vert[0] * vert[2];
    v[1] += vert[1] * vert[2];
    v[2] += vert[2];

    matrix[0][0] += vert[0] * vert[0];
    matrix[0][1] += vert[0] * vert[1];
    matrix[0][2] += vert[0];

    matrix[1][0] += vert[0] * vert[1];
    matrix[1][1] += vert[1] * vert[1];
    matrix[1][2] += vert[1];

    matrix[2][0] += vert[0];
    matrix[2][1] += vert[1];
  }

  matrix[2][2] = numVertices;

  origin[0] = matrix[0][2] / numVertices;
  origin[1] = matrix[1][2] / numVertices;
  origin[2] = v[2] / numVertices;

  c1 = m;
  c2 = m + 3;
  c3 = m + 6;

  double tmpMatrixInv[3][3] = {{matrix[0][0], matrix[1][0], matrix[2][0]}, {matrix[0][1], matrix[1][1], matrix[2][1]}, {matrix[0][2], matrix[1][2], matrix[2][2]}};

  double tmpMatrix0[3][3] = {{v[0], matrix[1][0], matrix[2][0]}, {v[1], matrix[1][1], matrix[2][1]}, {v[2], matrix[1][2], matrix[2][2]}};

  double tmpMatrix1[3][3] = {{matrix[0][0], v[0], matrix[2][0]}, {matrix[0][1], v[1], matrix[2][1]}, {matrix[0][2], v[2], matrix[2][2]}};

  if((det = Determinant3x3(tmpMatrixInv)) > tolerance)
  {
    normal[0] = Determinant3x3(tmpMatrix0) / det;
    normal[1] = Determinant3x3(tmpMatrix1) / det;
    normal[2] = -1.0;
  }

  double zAxis[3] = {0.0, 0.0, 1.0};
  double rotationAxis[3] = {0.0, 0.0, 0.0};

  MatrixMath::Normalize3x1(normal);
  MatrixMath::CrossProduct(normal, zAxis, rotationAxis);
  MatrixMath::Normalize3x1(rotationAxis);

  double rotationAngle = acos(MatrixMath::DotProduct3x1(zAxis, normal));

  Eigen::Transform<double, 3, Eigen::Affine> transform =
      Eigen::AngleAxisd(rotationAngle, Eigen::Vector3d(rotationAxis[0], rotationAxis[1], rotationAxis[2])) * Eigen::Translation3d(-origin[0], -origin[1], -origin[2]);

  return transform;
}


#include <cmath>
#include <map>
#include <math.h>  // pow
#include <sys/time.h>
#include <sys/resource.h>   // check the memory usage

#include <iostream>
#include <stdio.h>
#include <thread>
#include <vector>

#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>

#include <HELR/TestScheme.h>
#include <HELR/Cipher.h>
#include <HELR/CZZ.h>
#include <HELR/EvaluatorUtils.h>
#include <HELR/Message.h>
#include <HELR/Params.h>
#include <HELR/PubKey.h>
#include <HELR/Scheme.h>
#include <HELR/SchemeAlgo.h>
#include <HELR/SchemeAux.h>
#include <HELR/SecKey.h>
#include <HELR/StringUtils.h>

#include "LRtest.h"
#include "HELR.h"

#define debug 0

long LogReg::getctlvl(Cipher& ctxt){
    long nBits = NumBits(ctxt.mod)-1;
    return nBits;
}

//---------------------------------------------------------------------------------------------------

//! @ output ztrain = (2*y[k]-1) * (1, x[k])  -> encryption: zTrainCipher
void LogReg::EncryptData(Cipher*& zTrainCipher, dMat zTrain){

    //long slots = LRparams.n_training;

    CZZ**  mvec= new CZZ*[LRparams.dim1];

    // zTrainCipher = (2*y[k]-1) * (1, x[k])
    for (long i = 0; i < LRparams.dim1; ++i){
        mvec[i]= new CZZ[LRparams.nslots];
        for(int k=0; k< LRparams.nslots; ++k){
            if(zTrain[k][i]!=0){
                mvec[i][k].r = scaleup(zTrain[k][i], LRparams.logp-3);  // logp/8
            }
            else{
                mvec[i][k].r = to_ZZ("0");
            }
            mvec[i][k].i = to_ZZ("0");
        }
    }

    NTL_EXEC_RANGE(LRparams.dim1, first, last);
    for (long i = first; i < last; ++i){
        zTrainCipher[i] = scheme.encrypt(mvec[i], LRparams.nslots);
    }
    NTL_EXEC_RANGE_END;

    delete[] mvec;

}

void LogReg::show_and_compare(dVec& theta, dMat zTrain, CZZ*& dtheta){

    LR_poly(theta, zTrain, LRparams);

    double maxErrorBit = 0.0;
    double minRelativeBit= 20.0;

    for(int i=0; i< LRparams.dim1; i++){
        ZZ msg_scaleup= scaleup(theta[i], LRparams.logp);

        cout << "m " << i << " : ["  <<  msg_scaleup << "], theta: " << theta[i] << endl;   // unencrypted

        double theta;
        conv(theta, dtheta[i].r);
        theta = scaledown(theta, LRparams.logp);

        cout << "d " << i << " : ["  << dtheta[i].r << "], (HE)theta: " << theta << endl;      // encrypted
        cout << "e " << i << " : ["  << (msg_scaleup - dtheta[i].r)  << "],  ";    // error

        // Msg-bit
        ZZ msgbnd= dtheta[i].r;
        double MsgBit = 0.0;
        if(msgbnd!=0)
            MsgBit= (log(abs(msgbnd))/log(2)) ;
        cout << "Msg: " << MsgBit << ", ";

        // Error-bit
        ZZ error = (msg_scaleup - dtheta[i].r);
        double ErrorBit = 0.0;
        if(error!=0)
            ErrorBit= (log(abs(error))/log(2)) ;
        cout << "Error: " << ErrorBit << endl;

        double RelativeBit= MsgBit- ErrorBit;

        if(maxErrorBit < ErrorBit) maxErrorBit= ErrorBit;
        if(minRelativeBit > RelativeBit)  minRelativeBit= RelativeBit;
        cout << "-------------------------------------------------------------" << endl;

    }

    cout << "MAX error bit : " << maxErrorBit << ", Min relative-error bit: " << minRelativeBit<< endl;
    cout << "-------------------------------------------------------------" << endl;

}

void LogReg::HElogreg(Cipher*& thetaCipher, Cipher*& zTrainCipher,  dMat zTrain){
    struct rusage usage;
    auto start= chrono::steady_clock::now();

    // zSumCipher = 1/(n/polyscale) * sum (2y[k]-1) x[k] = 1/n * sum z[k]
    // At the first round, we just take zTrainCipher as theta (lvl= 2)
    Cipher* zSumCipher= new Cipher[LRparams.dim1];

    //! rot and sum
    Cipher* ctemp= new Cipher[LRparams.dim1];

    NTL_EXEC_RANGE(LRparams.dim1, first, last);
    for (long i = first; i < last; ++i){
        zSumCipher[i]= zTrainCipher[i];

        for(long j= 0; j< LRparams.logn; ++j){
            int l = (1<<j);
            ctemp[i] = scheme.leftRotate(zSumCipher[i], l);
            scheme.addAndEqual(zSumCipher[i], ctemp[i]);
        }

        scheme.modSwitchAndEqual(zSumCipher[i], LRparams.logn - LRparams.log2polyscale);
        thetaCipher[i] = zSumCipher[i];
    }
    NTL_EXEC_RANGE_END;

    auto end = std::chrono::steady_clock::now();
    auto diff = end - start;
    double timeElapsed= chrono::duration <double, milli> (diff).count()/1000.0;
    double totaltime = timeElapsed;

    int ret = getrusage(RUSAGE_SELF,&usage);

    cout << "-------------------------------------------------------------" << endl;
    cout << "1-iter : mod(theta)= " << getctlvl(thetaCipher[0]) << ", running time= "  << timeElapsed << "s, " ;
    cout<<  "Mem= " << usage.ru_maxrss/(1024)  << "MB" << endl;
    cout << "-------------------------------------------------------------" << endl;

    dVec mtheta(LRparams.dim1, 0.0);
    CZZ* dtheta = new CZZ[LRparams.dim1];
    for(int i = 0; i< LRparams.dim1; ++i){
        dtheta[i] = (scheme.decrypt(secretKey, thetaCipher[i]))[0];
    }

    show_and_compare(mtheta, zTrain, dtheta);

    //--------------------------------------------------------------
    double zlvl =  getctlvl(zSumCipher[0]);

    for(int j= 1; j< LRparams.max_iter; ++j){
        auto start= chrono::steady_clock::now();

        Cipher* gradCipher= new Cipher[LRparams.dim1];

        switch(LRparams.polydeg){
            case 3:
                gradCipher= getgrad_deg3(thetaCipher, zTrainCipher);
                break;
            case 7:
                gradCipher= getgrad_deg7(thetaCipher, zTrainCipher);
                break;

        }

        double tlvl = getctlvl(thetaCipher[0]);
        double glvl = getctlvl(gradCipher[0]);

        NTL_EXEC_RANGE(LRparams.dim1, first, last);
        for (long i = first; i < last; ++i){
            Cipher ztemp = scheme.modEmbed(zSumCipher[i], zlvl- tlvl);
            scheme.addAndEqual(thetaCipher[i], ztemp);

            scheme.modEmbedAndEqual(thetaCipher[i], tlvl- glvl);
            scheme.subAndEqual(thetaCipher[i], gradCipher[i]);
        }
        NTL_EXEC_RANGE_END;

        auto end = std::chrono::steady_clock::now();
        auto diff = end - start;
        double timeElapsed= chrono::duration <double, milli> (diff).count()/1000.0;
        totaltime += timeElapsed;

        ret = getrusage(RUSAGE_SELF,&usage);

        delete[] gradCipher;

        cout << "-------------------------------------------------------------" << endl;
        cout << j+1 << "-iter : mod(theta)= " << getctlvl(thetaCipher[0]) << ", running time= "  << timeElapsed << "s, " ;
        cout<<  "Mem= " << usage.ru_maxrss/1024  << "MB" << endl;
        cout << "-------------------------------------------------------------" << endl;

        for(int i=0; i< LRparams.dim1; ++i){
            dtheta[i] = (scheme.decrypt(secretKey, thetaCipher[i]))[0];
        }

        show_and_compare(mtheta, zTrain, dtheta);
    }

    cout << "Total Evaluation Time = " << totaltime << " s" << endl;

    delete[] zSumCipher;
    delete[] ctemp;
    delete[] dtheta;

}

Cipher* LogReg::getgrad_deg7(Cipher*& thetaCipher, Cipher*& zTrainCipher){

    //! compute ztheta =  (p* z[k]/8) (p* theta)/p  : mod(theta)+ logp
    double tlvl =  getctlvl(thetaCipher[0]);
    double zlvl =  getctlvl(zTrainCipher[0]);

    Cipher* ctemp= new Cipher[LRparams.dim1];

    NTL_EXEC_RANGE(LRparams.dim1, first, last);
    for (long i = first; i < last; i++){
        ctemp[i] = scheme.modEmbed(zTrainCipher[i], zlvl - tlvl);
        scheme.multAndEqual(ctemp[i], thetaCipher[i]);
    }
    NTL_EXEC_RANGE_END;

    Cipher ztheta = ctemp[0];
    for (long i = 1; i < LRparams.dim1; i++){
        scheme.addAndEqual(ztheta, ctemp[i]);
    }

    scheme.modSwitchAndEqual(ztheta, LRparams.logp);  // ztheta.lvl : tlvl - logp

    //-----------------------------------------------------------------------------------------
    // ctemp  =  alpha * (ztheta) * (a7*ztheta^6 + a5*ztheta^4 +a3*ztheta^2 + a1) * z[i] for 0<=i<=dim
    //        =  ((alpha*a7*z[i]) * (ztheta)) * (ztheta^6 + a5/a7*ztheta^4 +a3/a7*ztheta^2 + a1/a7)
    //-----------------------------------------------------------------------------------------

    // zSquare = p * (theat*z[k]/8)^2  with  tlvl - 2*logp
    Cipher zSquare= scheme.square(ztheta);
    scheme.modSwitchAndEqual(zSquare, LRparams.logp);

    // zQuartic = p * (theat*z[k]/8)^4 with  tlvl - 3*logp
    Cipher zQuartic = scheme.square(zSquare);
    scheme.modSwitchAndEqual(zQuartic, LRparams.logp);

    // zQuartic = p * (a7*ztheta^4 + a5*ztheta^2 + a3)  with  tlvl - 3*logp
    Cipher ctemp1= scheme.multByConst(zSquare, LRparams.evalcoeff[5]);
    scheme.modSwitchAndEqual(ctemp1, LRparams.logp);     // lvl(theta)+3
    scheme.addConstAndEqual(ctemp1, LRparams.evalcoeff[3]);

    scheme.multByConst(zQuartic, LRparams.evalcoeff[7]);
    scheme.addAndEqual(zQuartic, ctemp1);

    Cipher* res= new Cipher[LRparams.dim1];

    NTL_EXEC_RANGE(LRparams.dim1, first, last);
    for (long i = first; i < last; ++i){
        res[i] = scheme.modEmbed(zTrainCipher[i], zlvl - tlvl + LRparams.logp);
        scheme.multAndEqual(res[i], ztheta);
        scheme.modSwitchAndEqual(res[i], LRparams.logp);       // res: tlvl - logp

        ctemp[i]= scheme.multByConst(res[i], LRparams.evalcoeff[1]);
        scheme.modSwitchAndEqual(ctemp[i], LRparams.logp);     // res: tlvl - 2*logp

        scheme.multAndEqual(res[i], zSquare);
        scheme.modSwitchAndEqual(res[i], LRparams.logp);      // res: tlvl - 3 *logp

        scheme.multAndEqual(res[i], zQuartic);
        scheme.modSwitchAndEqual(res[i], LRparams.logp);
        scheme.multByConst(res[i], LRparams.evalcoeff[7]);    // res: tlvl - 4 *logp

        scheme.addAndEqual(res[i], ctemp[i]);
    }
    NTL_EXEC_RANGE_END;

    //! rot and sum

    NTL_EXEC_RANGE(LRparams.dim1, first, last);
    for (long i = first; i < last; ++i){
        for(long j= 0; j< LRparams.logn; ++j){
            long l = (1<<j);
            ctemp[i] = scheme.leftRotate(res[i], l);
            scheme.addAndEqual(res[i], ctemp[i]);
        }
        scheme.modSwitchAndEqual(res[i], LRparams.logn - LRparams.log2polyscale);
    }
    NTL_EXEC_RANGE_END;

    delete[] ctemp;
    return res;
}

//-----------------------------------------------------------------------------------------
// res[i]  =  1/n * sum_k (ztheta) * (a3*ztheta^3 + a1*ztheta) * z[i]/8 for 0<=i<=dim
//         =  1/n * sum_k ((z[i]/8) * (ztheta)) * (ztheta^2 + a1/a3) * a3)
//-----------------------------------------------------------------------------------------

Cipher* LogReg::getgrad_deg3(Cipher*& thetaCipher, Cipher*& zTrainCipher){

    double tlvl =  getctlvl(thetaCipher[0]);
    double zlvl =  getctlvl(zTrainCipher[0]);

    Cipher* ctemp= new Cipher[LRparams.dim1];

    NTL_EXEC_RANGE(LRparams.dim1, first, last);
    for (long i = first; i < last; i++){
        ctemp[i] = scheme.modEmbed(zTrainCipher[i], zlvl - tlvl);
        scheme.multAndEqual(ctemp[i], thetaCipher[i]);
    }
    NTL_EXEC_RANGE_END;

    Cipher ztheta = ctemp[0];
    for (long i = 1; i < LRparams.dim1; i++){
        scheme.addAndEqual(ztheta, ctemp[i]);
    }

    scheme.modSwitchAndEqual(ztheta, LRparams.logp);  // ztheta.lvl : tlvl - logp

    //---------------------------------------------------
    //! zSquare= p * (theat*z[k]/8)^2 + p (a1/a3) with  tlvl - 2*logp
    Cipher zSquare= scheme.square(ztheta);
    scheme.modSwitchAndEqual(zSquare, LRparams.logp);
    scheme.addConstAndEqual(zSquare, LRparams.evalcoeff[1]);

    //! res[i]= ((z[i]/8) * a3 ) * (ztheta)) * (ztheta^2 + a1/a3))
    Cipher* res= new Cipher[LRparams.dim1];

    NTL_EXEC_RANGE(LRparams.dim1, first, last);
    for (long i = first; i < last; ++i){
        res[i]= scheme.multByConst(zTrainCipher[i], LRparams.evalcoeff[3]);
        scheme.modSwitchAndEqual(res[i], LRparams.logp);  // ((z[i]/8) * a3 ) with zlvl - logp

        scheme.modEmbedAndEqual(res[i], zlvl - tlvl);
        scheme.multAndEqual(res[i], ztheta);
        scheme.modSwitchAndEqual(res[i], LRparams.logp);  // ((z[i]/8) * a3 ) * (ztheta)) with zlvl - 2*logp

        scheme.multAndEqual(res[i], zSquare);
        scheme.modSwitchAndEqual(res[i], LRparams.logp);  // with zlvl - 3*logp
    }
    NTL_EXEC_RANGE_END;

    //! (sum res[i]) / (n/polyscale)

    NTL_EXEC_RANGE(LRparams.dim1, first, last);
    for (long i = first; i < last; ++i){
        for(long j= 0; j< LRparams.logn; ++j){
            long l = (1<<j);
            ctemp[i] = scheme.leftRotate(res[i], l);
            scheme.addAndEqual(res[i], ctemp[i]);
        }

        scheme.modSwitchAndEqual(res[i], LRparams.logn - LRparams.log2polyscale);
    }
    NTL_EXEC_RANGE_END;

    delete[] ctemp;
    return res;
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __MODULES_NUMERIC_AD_CD_ADCT_HPP__
#define __MODULES_NUMERIC_AD_CD_ADCT_HPP__

#include <Eigen/Core>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/utility/enable_if.hpp>

#ifdef USE_TOON
#include <TooN/TooN.h>
#include <libv/lma/lm/container/tag.hpp>
#endif
#include <libv/lma/ttt/traits/naming.hpp>

namespace adct
{
  template<class T> struct Expr
  {
    T const& cast() const {return static_cast<T const &>(*this); }
  };

  template<class T, int N, class Default=void> class Ad : public Expr<Ad<T,N,Default>>
  {
    public:
      typedef Eigen::Matrix<T, N, 1/*, Eigen::DontAlign*/> Array;
      Ad(const T& val = T()):value_(val),infinite_(Array::Zero()) {}
      Ad(const T& val , int i):value_(val),infinite_(Array::Zero()) { infinite_[i] = 1; }
      template<class X> Ad(const Expr<X>& expr):value_(expr.cast().value()),infinite_(expr.cast().infinite()) {}

      const T&     value()    const { return value_; }
      const Array& infinite() const { return infinite_; }

    private:
      T     value_;
      Array infinite_;
  };

#ifdef USE_TOON
  template<class T, int N> class Ad<T,N,lma::Toon> : public Expr<Ad<T,N,lma::Toon>>
  {
    public:
      typedef TooN::Vector<N,T> Array;
      Ad(const T& val = T()):value_(val),infinite_(TooN::Zeros) {}
      Ad(const T& val , int i):value_(val),infinite_(TooN::Zeros) { infinite_[i] = 1; }
      template<class X> Ad(const Expr<X>& expr):value_(expr.cast().value()),infinite_(expr.cast().infinite()) {}

      const T&     value()    const { return value_; }
      const Array& infinite() const { return infinite_; }

    private:
      T     value_;
      Array infinite_;
  };
#endif

  template<class T, class = void> struct Traits { typedef const T& type; };

  template<class A, class Op> struct Unary : Expr<Unary<A,Op>>
  {
    typename Traits<A>::type a;
    Unary(const A& a_):a(a_){}
    auto value()    const { return Op::value(a); }
    auto infinite() const { return Op::infinite(a); }
  };

  template<class A, class B, class Op> struct Binary : Expr<Binary<A,B,Op>>
  {
    typename Traits<A>::type a;
    typename Traits<B>::type b;
    Binary(const A& a_, const B& b_):a(a_),b(b_){}
    auto value()    const { return Op::value(a,b); }
    auto infinite() const { return Op::infinite(a,b); }
  };

  template<class T> struct Traits<T, typename boost::enable_if< boost::is_floating_point<T> >::type > { typedef T type; };
  template<class A, class B, class Op>  struct Traits<Binary<A,B,Op>> { typedef Binary<A,B,Op> type; };
  template<class A, class Op>           struct Traits<Unary<A,Op>>    { typedef Unary<A,Op> type; };

  struct Minus
  {
    template<class A> static auto value   (const A& a) { return - a.value();}
    template<class A> static auto infinite(const A& a) { return - a.infinite();}
  };

  struct Sqrt
  {
    template<class A> static auto value   (const A& a) { return sqrt(a.value()); }
    template<class A> static auto infinite(const A& a) { return a.infinite() / (2.0 * sqrt(a.value())); }
  };

  struct Cos
  {
    template<class A> static auto value   (const A& a) { return cos(a.value()); }
    template<class A> static auto infinite(const A& a) { return - sin(a.value()) * a.infinite(); }
  };

  struct Sin
  {
    template<class A> static auto value   (const A& a) { return sin(a.value()); }
    template<class A> static auto infinite(const A& a) { return cos(a.value()) * a.infinite(); }
  };

  struct Addition
  {
    template<class A, class B> static auto value   (const A& a, const B& b) { return a.value() + b.value() ;}
    template<class A, class B> static auto infinite(const A& a, const B& b) { return a.infinite() + b.infinite() ;}
  };

  struct Substract
  {
    template<class A, class B> static auto value   (const A& a, const B& b) { return a.value() - b.value() ;}
    template<class A, class B> static auto infinite(const A& a, const B& b) { return a.infinite() - b.infinite() ;}
  };

  struct Multiply
  {
    template<class A, class B> static auto value   (const A& a, const B& b) { return a.value() * b.value() ;}
    template<class A, class B> static auto infinite(const A& a, const B& b) { return a.value() * b.infinite() + a.infinite() * b.value(); }
  };

  struct AdditionScalar
  {
    template<class A> static auto value   (const A& a, double b) { return a.value() + b ;}
    template<class A> static auto infinite(const A& a, double  ) { return a.infinite() ;}
  };

  struct SubstractScalar
  {
    template<class A> static auto value   (const A& a, double b) { return a.value() - b ;}
    template<class A> static auto infinite(const A& a, double  ) { return a.infinite() ;}
  };

  struct ScalarSubstract
  {
    template<class A> static auto value   (double b, const A& a) { return b - a.value() ;}
    template<class A> static auto infinite(double  , const A& a) { return -a.infinite() ;}
  };

  struct MultiplyScalar
  {
    template<class A> static auto value   (const A& a, double b) { return a.value() * b ;}
    template<class A> static auto infinite(const A& a, double b) { return a.infinite() * b;}
  };

  struct DivideScalar
  {
    template<class A> static auto value   (const A& a, double b) { return a.value() / b ;}
    template<class A> static auto infinite(const A& a, double b) { return a.infinite() / b;}
  };

  struct ScalarDivide
  {
    static auto sqr(double t) { return t * t; }
    template<class B> static auto value   (double a, const B& b) { return a / b.value() ;}
    template<class B> static auto infinite(double a, const B& b) { return - b.infinite() * a / sqr(b.value()); }
  };

  struct Divide
  {
    template<class A, class B> static auto value(const A& a, const B& b) { return a.value() / b.value() ; }
    template<class A, class B> static auto infinite(const A& a, const B& b)
    {
      const double val_inv = 1.0 / b.value();
      return (a.infinite() - a.value() * val_inv * b.infinite()) * val_inv;
    }
  };

  template<class A, class B> auto operator+(const Expr<A>& a, const Expr<B>& b) { return Binary<A,B,Addition>(a.cast(),b.cast()); }
  template<class A, class B> auto operator-(const Expr<A>& a, const Expr<B>& b) { return Binary<A,B,Substract>(a.cast(),b.cast()); }
  template<class A, class B> auto operator*(const Expr<A>& a, const Expr<B>& b) { return Binary<A,B,Multiply>(a.cast(),b.cast()); }
  template<class A, class B> auto operator/(const Expr<A>& a, const Expr<B>& b) { return Binary<A,B,Divide>(a.cast(),b.cast()); }

  template<class A>          auto operator+(const Expr<A>& a, double b)         { return Binary<A,double,AdditionScalar>(a.cast(),b); }
  template<class A>          auto operator+(double b, const Expr<A>& a)         { return a+b; }
  template<class A>          auto operator-(const Expr<A>& a, double b)         { return Binary<A,double,SubstractScalar>(a.cast(),b); }
  template<class A>          auto operator-(double b, const Expr<A>& a)         { return Binary<double,A,ScalarSubstract>(b,a.cast()); }

  template<class A>          auto operator*(const Expr<A>& a, double b)         { return Binary<A,double,MultiplyScalar>(a.cast(),b); }
  template<class A>          auto operator*(double b, const Expr<A>& a)         { return a*b; }
  template<class A>          auto operator/(const Expr<A>& a, double b)         { return Binary<A,double,DivideScalar>(a.cast(),b); }
  template<class B>          auto operator/(double a, const Expr<B>& b)         { return Binary<double,B,ScalarDivide>(a,b.cast()); }

  template<class A>          auto operator-(const Expr<A>& a)                   { return Unary<A,Minus>(a.cast()); }
  template<class A>          auto sqrt     (const Expr<A>& a)                   { return Unary<A,Sqrt>(a.cast()); }
  template<class A>          auto cos      (const Expr<A>& a)                   { return Unary<A,Cos>(a.cast()); }
  template<class A>          auto sin      (const Expr<A>& a)                   { return Unary<A,Sin>(a.cast()); }

  template<class A, class B> bool operator>(const Expr<A>& a, const Expr<B>& b) { return a.cast().value() > b.cast().value() ; }
  template<class A, class B> bool operator<(const Expr<A>& a, const Expr<B>& b) { return a.cast().value() < b.cast().value() ; }
}

#include <libv/lma/ttt/traits/naming.hpp>
#include <libv/lma/string/string_utiliy.hpp>

namespace ttt
{
  template<class T, int N> struct Name<adct::Ad<T,N>>
  {
    static std::string name(){ return std::string("Ad<") + ttt::name<T>() + "," + lma::to_string(N) + ">"; }
  };

}

#endif


#include <iostream>
#include <stdexcept>
// Boost random generator
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
// Eigen dense matrices
#include <Eigen/Dense>

#include "factory.h"
#include "normal_multivar.h"

namespace numeric_utils {

NormalMultiVar::NormalMultiVar()
  : RandomGenerator()
{
  generator_ = boost::random::mt19937(seed_);
  distribution_ = boost::random::normal_distribution<double>();
}

NormalMultiVar::NormalMultiVar(int seed)
  : RandomGenerator()
{
  seed_ = seed;
  generator_ = boost::random::mt19937(seed_);
  distribution_ = boost::random::normal_distribution<double>();
}

bool NormalMultiVar::generate(
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& random_numbers,
    const Eigen::VectorXd& means, const Eigen::MatrixXd& cov,
    unsigned int cases) {

  bool success = true;
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> lower_cholesky;

  try {
    auto llt = cov.llt();
    lower_cholesky = llt.matrixL();

    if (llt.info() == Eigen::NumericalIssue) {
      throw std::runtime_error(
          "\nERROR: In NormalMultivar::generate method: Input covariance matrix is not "
          "positive semi-definite\n");
    }
  } catch (const std::exception& e) {
    std::cerr << "\nERROR: In normal multivariate random number generation: "
              << e.what() << std::endl;
    success = false;
  }

  random_numbers.resize(cov.rows(), cases);

  // Generate random numbers based on distribution and generator type for
  // requested number of cases
  for (unsigned int i = 0; i < random_numbers.cols(); ++i) {
    for (unsigned int j = 0; j < random_numbers.rows(); ++j) {
      random_numbers(j, i) = distribution_(generator_);
    }
  }

  // Transform from unit normal distribution based on covariance and mean values
  for (unsigned int i = 0; i < random_numbers.cols(); ++i) {
    random_numbers.col(i) = lower_cholesky * random_numbers.col(i) + means;
  }

  return success;
}

std::string NormalMultiVar::name() const {
  return "NormalMultiVar";
}
}  // namespace numeric_utils


//
// $Id$
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// 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.
//
#ifndef DETERMINEBINWIDTH_H
#define DETERMINEBINWIDTH_H

#include <cmath>
#include <boost/assert.hpp>
#include <boost/range/algorithm_ext.hpp>
#include <boost/bind.hpp>
#include <algorithm>

#include "pwiz/utility/findmf/base/base/diff.hpp"
#include "pwiz/utility/findmf/base/resample/utilities/determinebinwidth.hpp"

namespace ralab
{
	namespace base
	{
		namespace resample
		{
			template<typename TReal>
			struct SquareRoot{
				TReal operator()(TReal x) const{
					return(sqrt(x));
				}
			};

			struct SamplingWith{
				std::vector<double> diff_;
				std::vector<double> summ_;
				std::vector<double> am_;

				//expects a sorted sequence
				template<typename TRealI>
				double operator()(TRealI begin, TRealI end)
				{
					//BOOST_ASSERT(!boost::range::is_sorted(begin,end));
					typedef typename std::iterator_traits<TRealI>::value_type TReal;
					std::size_t N = std::distance(begin,end);
					double am;
					if(N > 1){
						diff_.resize(N-1);
						summ_.resize(N-1);
						am_.resize(N-1);
						ralab::base::base::diff(begin,end,diff_.begin(),1);

						utilities::summ( begin , end, summ_.begin(),1);
						//square the sum
						//std::transform(summ_.begin(),summ_.end(),summ_.begin(),boost::bind(sqrt,_1));
						std::transform(summ_.begin(),summ_.end(),summ_.begin(),SquareRoot<TReal>());
						std::transform(diff_.begin(),diff_.end(),summ_.begin(),am_.begin(),std::divides<double>());
						std::sort(am_.begin(),am_.end());
						am = utilities::determine(am_.begin(),am_.end());
					}else{
						am = 0.;
					}
					return am;
				}
			};

		}
	}
}

#endif // DETERMINEBINWIDTH_H


#ifndef BACKTRACKING_LINE_SEARCH_HPP
#define BACKTRACKING_LINE_SEARCH_HPP

#include <functional>
#include <Eigen/Core>

namespace mathtoolbox
{
    namespace optimization
    {
        // Procedure 3.1: Backtracking Line Search
        //
        // This algoritmh tries to find an appropriate step size that satisfies
        // the Armijo condition (i.e., the safficient decreasing condition).
        // This algorithm runs faster than the line search algorithm for the
        // strong Wolfe conditions, but it does not guarantee the curvature
        // condition, which is required to stabilize the overall optimization.
        inline double RunBacktrackingLineSearch(const std::function<double(const Eigen::VectorXd&)>& f,
                                                const Eigen::VectorXd& grad,
                                                const Eigen::VectorXd& x,
                                                const Eigen::VectorXd& p,
                                                const double alpha_init,
                                                const double rho,
                                                const double c)
        {
            constexpr unsigned int num_max_iterations = 50;

            unsigned counter = 0;
            double alpha = alpha_init;
            while (true)
            {
                // Equation 3.6a
                const bool armijo_condition = f(x + alpha * p) <= f(x) + c * alpha * grad.transpose() * p;

                if (armijo_condition || counter == num_max_iterations) { break; }

                alpha *= rho;

                ++ counter;
            }
            return alpha;
        }
    }
}

#endif /* BACKTRACKING_LINE_SEARCH_HPP */


#include <possumwood_sdk/node_implementation.h>

#define EIGEN_STACK_ALLOCATION_LIMIT 0

#include <actions/traits.h>
#include <tbb/task_group.h>

#include <Eigen/Sparse>
#include <mutex>
#include <opencv2/opencv.hpp>

#include "frame.h"

namespace {

class Triplets;

class Row {
  public:
	Row() = default;

	void addValue(int64_t row, int64_t col, double value) {
		if(value != 0.0)
			m_values[(row << 32) + col] += value;
	}

  private:
	Row(const Row&) = delete;
	Row& operator=(const Row&) = delete;

	std::map<int64_t, double> m_values;

	friend class Triplets;
};

class Triplets {
  public:
	Triplets(int rows, int cols) : m_rowCount(0), m_rows(rows), m_cols(cols) {
	}

	void addRow(const Row& r) {
		for(auto& v : r.m_values) {
			int32_t row = v.first >> 32;
			int32_t col = v.first & 0xffffffff;

			assert(row < m_rows);
			assert(col < m_cols);

			m_triplets.push_back(Eigen::Triplet<double>(m_rowCount, row * m_cols + col, v.second));
		}

		++m_rowCount;
	}

	std::size_t rows() const {
		return m_rowCount;
	}

	const std::vector<Eigen::Triplet<double>>& triplets() const {
		return m_triplets;
	}

  private:
	std::vector<Eigen::Triplet<double>> m_triplets;

	int m_rowCount, m_rows, m_cols;

	friend class Row;
};

static const cv::Mat kernel = (cv::Mat_<double>(3, 3) << 0.0, -1.0, 0.0, -1.0, 4.0, -1.0, 0.0, -1.0, 0.0);

// static const cv::Mat kernel = (cv::Mat_<double>(3,3) <<
// 	-1.0, -1.0, -1.0,
// 	-1.0,  8.0, -1.0,
// 	-1.0, -1.0, -1.0
// );

// static const cv::Mat kernel = (cv::Mat_<double>(3,3) <<
// 	-1.0, -2.0, -1.0,
// 	-2.0, 12.0, -2.0,
// 	-1.0, -2.0, -1.0
// );

// static const cv::Mat kernel = (cv::Mat_<double>(5,5) <<
// 	 0.0,  0.0,  1.0,  0.0,  0.0,
// 	 0.0,  2.0, -8.0,  2.0,  0.0,
// 	 1.0, -8.0, 20.0, -8.0,  1.0,
// 	 0.0,  2.0, -8.0,  2.0,  0.0,
// 	 0.0,  0.0,  1.0,  0.0,  0.0
// );

float buildMatrices(const cv::Mat& image, const cv::Mat& mask, Eigen::SparseMatrix<double>& A, Eigen::VectorXd& b,
                    const cv::Rect2i& roi) {
	Triplets triplets(roi.height, roi.width);
	std::vector<double> values;

	std::size_t validCtr = 0, interpolatedCtr = 0;

	for(int y = roi.y; y < roi.y + roi.height; ++y)
		for(int x = roi.x; x < roi.x + roi.width; ++x) {
			Row row;

			// masked and/or edge
			if(mask.at<unsigned char>(y, x) > 128) {
				values.push_back(0.0f);

				// convolution
				for(int yi = 0; yi < kernel.rows; ++yi)
					for(int xi = 0; xi < kernel.cols; ++xi) {
						int ypos = y + yi - kernel.rows / 2;
						int xpos = x + xi - kernel.cols / 2;

						// handling of edges - "clip" (or "mirror", commented out for now)
						if(ypos < roi.y)
							// ypos = -ypos;
							ypos = roi.y;
						if(ypos >= roi.y + roi.height)
							// ypos = (image.rows-1) - (ypos-image.rows);
							ypos = roi.y + roi.height - 1;

						if(xpos < roi.x)
							// xpos = -xpos;
							xpos = roi.x;
						if(xpos >= roi.x + roi.width)
							// xpos = (image.cols-1) - (xpos-image.cols);
							xpos = roi.x + roi.width - 1;

						row.addValue(ypos - roi.y, xpos - roi.x, kernel.at<double>(yi, xi));
					}

				++interpolatedCtr;
			}

			// non-masked
			if(mask.at<unsigned char>(y, x) <= 128) {
				values.push_back(image.at<float>(y, x));
				row.addValue(y - roi.y, x - roi.x, 1);

				++validCtr;
			}

			triplets.addRow(row);
		}

	// initialise the sparse matrix
	A = Eigen::SparseMatrix<double>(triplets.rows(), roi.height * roi.width);
	A.setFromTriplets(triplets.triplets().begin(), triplets.triplets().end());

	// and the "b" vector
	assert(values.size() == triplets.rows());
	b = Eigen::VectorXd(values.size());
	for(std::size_t i = 0; i < values.size(); ++i)
		b[i] = values[i];

	return (float)validCtr / ((float)validCtr + (float)interpolatedCtr);
}

dependency_graph::InAttr<possumwood::opencv::Frame> a_inFrame, a_inMask;
dependency_graph::InAttr<unsigned> a_mosaic;
dependency_graph::OutAttr<possumwood::opencv::Frame> a_outFrame;

dependency_graph::State compute(dependency_graph::Values& data) {
	dependency_graph::State state;

	const cv::Mat& input = *data.get(a_inFrame);
	const cv::Mat& mask = *data.get(a_inMask);
	const unsigned mosaic = data.get(a_mosaic);

	if(input.depth() != CV_32F)
		throw std::runtime_error("Laplacian inpainting - input image type has to be CV_32F.");
	if(mask.type() != CV_8UC1 && mask.type() != CV_8UC3)
		throw std::runtime_error("Laplacian inpainting - mask image type has to be CV_8UC1 or CV_8UC3.");
	if(input.empty() || mask.empty())
		throw std::runtime_error("Laplacian inpainting - empty input image and/or mask.");
	if(input.size != mask.size)
		throw std::runtime_error("Laplacian inpainting - input and mask image size have to match.");
	if(input.cols % mosaic != 0 || input.rows % mosaic != 0)
		throw std::runtime_error(
		    "Laplacian inpainting - image size is not divisible by mosaic count - invalid mosaic?.");

	std::vector<std::vector<float>> x(input.channels(), std::vector<float>(input.rows * input.cols, 0.0f));

	tbb::task_group tasks;
	std::mutex solve_mutex;

	// split the inputs and masks per channel
	std::vector<cv::Mat> inputs, masks;
	cv::split(input, inputs);
	cv::split(mask, masks);

	assert((int)inputs.size() == input.channels());
	assert((int)masks.size() == mask.channels());

	const unsigned mosaic_rows = input.rows / mosaic;
	const unsigned mosaic_cols = input.cols / mosaic;

	for(unsigned yi = 0; yi < mosaic; ++yi) {
		for(unsigned xi = 0; xi < mosaic; ++xi) {
			cv::Rect2i roi;
			roi.y = yi * mosaic_rows;
			roi.x = xi * mosaic_cols;
			roi.height = mosaic_rows;
			roi.width = mosaic_cols;

			for(int channel = 0; channel < input.channels(); ++channel) {
				tasks.run([channel, &inputs, &masks, &x, &state, &solve_mutex, roi]() {
					cv::Mat inTile = inputs[channel];

					cv::Mat inMask;
					if(masks.size() == 1)
						inMask = masks[0];
					else
						inMask = masks[channel];

					Eigen::SparseMatrix<double> A;
					Eigen::VectorXd b, tmp;

					const float ratio = buildMatrices(inTile, inMask, A, b, roi);

					if(ratio > 0.003) {
						const char* stage = "solver construction";

						Eigen::SparseLU<Eigen::SparseMatrix<double> /*, Eigen::NaturalOrdering<int>*/> chol(A);

						if(chol.info() == Eigen::Success) {
							stage = "analyze pattern";

							chol.analyzePattern(A);

							if(chol.info() == Eigen::Success) {
								stage = "factorize";

								chol.factorize(A);

								if(chol.info() == Eigen::Success) {
									stage = "solve";

									tmp = chol.solve(b);

									assert(tmp.size() == roi.height * roi.width);
									for(int i = 0; i < tmp.size(); ++i) {
										const int row = i / roi.width;
										const int col = i % roi.width;
										const int index = (row + roi.y) * masks[0].cols + col + roi.x;

										assert((std::size_t)index < x[channel].size());

										x[channel][index] = tmp[i];
									}
								}
							}
						}

						std::lock_guard<std::mutex> guard(solve_mutex);

						if(chol.info() == Eigen::NumericalIssue)
							state.addWarning("Decomposition failed - Eigen::NumericalIssue at stage " +
							                 std::string(stage));
						else if(chol.info() == Eigen::NoConvergence)
							state.addWarning("Decomposition failed - Eigen::NoConvergence at stage " +
							                 std::string(stage));
						else if(chol.info() == Eigen::InvalidInput)
							state.addWarning("Decomposition failed - Eigen::InvalidInput at stage " +
							                 std::string(stage));
						else if(chol.info() != Eigen::Success)
							state.addWarning("Decomposition failed - unknown error at stage " + std::string(stage));
					}
				});
			}
		}
	}

	tasks.wait();

	cv::Mat result = input.clone();
	for(int yi = 0; yi < result.rows; ++yi)
		for(int xi = 0; xi < result.cols; ++xi)
			for(int c = 0; c < input.channels(); ++c)
				result.ptr<float>(yi, xi)[c] = x[c][yi * result.cols + xi];

	data.set(a_outFrame, possumwood::opencv::Frame(result));

	return state;
}

void init(possumwood::Metadata& meta) {
	meta.addAttribute(a_inFrame, "frame", possumwood::opencv::Frame(), possumwood::AttrFlags::kVertical);
	meta.addAttribute(a_inMask, "mask", possumwood::opencv::Frame(), possumwood::AttrFlags::kVertical);
	meta.addAttribute(a_mosaic, "mosaic", 1u);
	meta.addAttribute(a_outFrame, "out_frame", possumwood::opencv::Frame(), possumwood::AttrFlags::kVertical);

	meta.addInfluence(a_inFrame, a_outFrame);
	meta.addInfluence(a_inMask, a_outFrame);
	meta.addInfluence(a_mosaic, a_outFrame);

	meta.setCompute(compute);
}

possumwood::NodeImplementation s_impl("opencv/inpaint_laplacian", init);

}  // namespace