/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    smallExample.cpp
 * @brief   Create small example with two poses and one landmark
 * @brief   smallExample
 * @author  Carlos Nieto
 * @author  Frank dellaert
 */

#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/base/Matrix.h>

#include <tests/smallExample.h>

#include <boost/optional.hpp>
#include <boost/shared_ptr.hpp>

#include <iostream>
#include <string>

using namespace std;

namespace gtsam {
namespace example {

	using namespace gtsam::noiseModel;

	typedef boost::shared_ptr<NonlinearFactor> shared;

	static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
	static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
	static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
	static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);

	static const Index _l1_=0, _x1_=1, _x2_=2;
	static const Index _x_=0, _y_=1, _z_=2;

	// Convenience for named keys
	using symbol_shorthand::X;
	using symbol_shorthand::L;

	/* ************************************************************************* */
	boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
		// Create
		boost::shared_ptr<Graph> nlfg(
				new Graph);

		// prior on x1
		Point2 mu;
		shared f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
		nlfg->push_back(f1);

		// odometry between x1 and x2
		Point2 z2(1.5, 0);
		shared f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2)));
		nlfg->push_back(f2);

		// measurement between x1 and l1
		Point2 z3(0, -1);
		shared f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1)));
		nlfg->push_back(f3);

		// measurement between x2 and l1
		Point2 z4(-1.5, -1.);
		shared f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1)));
		nlfg->push_back(f4);

		return nlfg;
	}

	/* ************************************************************************* */
	Graph createNonlinearFactorGraph() {
		return *sharedNonlinearFactorGraph();
	}

	/* ************************************************************************* */
	Values createValues() {
		Values c;
		c.insert(X(1), Point2(0.0, 0.0));
		c.insert(X(2), Point2(1.5, 0.0));
		c.insert(L(1), Point2(0.0, -1.0));
		return c;
	}

	/* ************************************************************************* */
	VectorValues createVectorValues() {
		VectorValues c(vector<size_t>(3, 2));
		c[_l1_] = Vector_(2, 0.0, -1.0);
		c[_x1_] = Vector_(2, 0.0, 0.0);
		c[_x2_] = Vector_(2, 1.5, 0.0);
		return c;
	}

	/* ************************************************************************* */
	boost::shared_ptr<const Values> sharedNoisyValues() {
		boost::shared_ptr<Values> c(new Values);
		c->insert(X(1), Point2(0.1, 0.1));
		c->insert(X(2), Point2(1.4, 0.2));
		c->insert(L(1), Point2(0.1, -1.1));
		return c;
	}

	/* ************************************************************************* */
	Values createNoisyValues() {
		return *sharedNoisyValues();
	}

	/* ************************************************************************* */
	VectorValues createCorrectDelta(const Ordering& ordering) {
		VectorValues c(vector<size_t>(3,2));
		c[ordering[L(1)]] = Vector_(2, -0.1, 0.1);
		c[ordering[X(1)]] = Vector_(2, -0.1, -0.1);
		c[ordering[X(2)]] = Vector_(2, 0.1, -0.2);
		return c;
	}

	/* ************************************************************************* */
	VectorValues createZeroDelta(const Ordering& ordering) {
		VectorValues c(vector<size_t>(3,2));
		c[ordering[L(1)]] = zero(2);
		c[ordering[X(1)]] = zero(2);
		c[ordering[X(2)]] = zero(2);
		return c;
	}

	/* ************************************************************************* */
	GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
		// Create empty graph
	  GaussianFactorGraph fg;

		SharedDiagonal unit2 = noiseModel::Unit::Create(2);

		// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2));

		// odometry between x1 and x2: x2-x1=[0.2;-0.1]
		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2));

    // measurement between x1 and l1: l1-x1=[0.0;0.2]
		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2));

		// measurement between x2 and l1: l1-x2=[-0.2;0.3]
		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2));

		return fg;
	}

	/* ************************************************************************* */
	/** create small Chordal Bayes Net x <- y
	 * x y d
	 * 1 1 9
	 *   1 5
	 */
	GaussianBayesNet createSmallGaussianBayesNet() {
		Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
		Matrix R22 = Matrix_(1, 1, 1.0);
		Vector d1(1), d2(1);
		d1(0) = 9;
		d2(0) = 5;
		Vector tau(1);
		tau(0) = 1.0;

		// define nodes and specify in reverse topological sort (i.e. parents last)
		GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau));
		GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau));
		GaussianBayesNet cbn;
		cbn.push_back(Px_y);
		cbn.push_back(Py);

		return cbn;
	}

	/* ************************************************************************* */
	// Some nonlinear functions to optimize
	/* ************************************************************************* */
	namespace smallOptimize {

		Point2 h(const Point2& v) {
			return Point2(cos(v.x()), sin(v.y()));
		}

		Matrix H(const Point2& v) {
			return Matrix_(2, 2,
					-sin(v.x()), 0.0,
					 0.0, cos(v.y()));
		}

		struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {

			Point2 z_;

			UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
				gtsam::NoiseModelFactor1<Point2>(model, key), z_(z) {
			}

			Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
				if (A) *A = H(x);
				return (h(x) - z_).vector();
			}

		};

	}

	/* ************************************************************************* */
	boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() {
		boost::shared_ptr<Graph> fg(new Graph);
		Vector z = Vector_(2, 1.0, 0.0);
		double sigma = 0.1;
		boost::shared_ptr<smallOptimize::UnaryFactor> factor(
				new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
		fg->push_back(factor);
		return fg;
	}

	Graph createReallyNonlinearFactorGraph() {
		return *sharedReallyNonlinearFactorGraph();
	}

	/* ************************************************************************* */
	pair<Graph, Values> createNonlinearSmoother(int T) {

		// Create
		Graph nlfg;
		Values poses;

		// prior on x1
		Point2 x1(1.0, 0.0);
		shared prior(new simulated2D::Prior(x1, sigma1_0, X(1)));
		nlfg.push_back(prior);
		poses.insert(X(1), x1);

		for (int t = 2; t <= T; t++) {
			// odometry between x_t and x_{t-1}
			Point2 odo(1.0, 0.0);
			shared odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t)));
			nlfg.push_back(odometry);

			// measurement on x_t is like perfect GPS
			Point2 xt(t, 0);
			shared measurement(new simulated2D::Prior(xt, sigma1_0, X(t)));
			nlfg.push_back(measurement);

			// initial estimate
			poses.insert(X(t), xt);
		}

		return make_pair(nlfg, poses);
	}

	/* ************************************************************************* */
	pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering) {
		Graph nlfg;
		Values poses;
		boost::tie(nlfg, poses) = createNonlinearSmoother(T);

		if(!ordering) ordering = *poses.orderingArbitrary();
		return make_pair(*nlfg.linearize(poses, *ordering), *ordering);
	}

	/* ************************************************************************* */
	GaussianFactorGraph createSimpleConstraintGraph() {
		// create unary factor
		// prior on _x_, mean = [1,-1], sigma=0.1
		Matrix Ax = eye(2);
		Vector b1(2);
		b1(0) = 1.0;
		b1(1) = -1.0;
		JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));

		// create binary constraint factor
		// between _x_ and _y_, that is going to be the only factor on _y_
		// |1 0||x_1| + |-1  0||y_1| = |0|
		// |0 1||x_2|   | 0 -1||y_2|   |0|
		Matrix Ax1 = eye(2);
		Matrix Ay1 = eye(2) * -1;
		Vector b2 = Vector_(2, 0.0, 0.0);
		JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
				constraintModel));

		// construct the graph
		GaussianFactorGraph fg;
		fg.push_back(f1);
		fg.push_back(f2);

		return fg;
	}

	/* ************************************************************************* */
	VectorValues createSimpleConstraintValues() {
		VectorValues config(vector<size_t>(2,2));
		Vector v = Vector_(2, 1.0, -1.0);
		config[_x_] = v;
		config[_y_] = v;
		return config;
	}

	/* ************************************************************************* */
	GaussianFactorGraph createSingleConstraintGraph() {
		// create unary factor
		// prior on _x_, mean = [1,-1], sigma=0.1
		Matrix Ax = eye(2);
		Vector b1(2);
		b1(0) = 1.0;
		b1(1) = -1.0;
		//GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1));
		JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));

		// create binary constraint factor
		// between _x_ and _y_, that is going to be the only factor on _y_
		// |1 2||x_1| + |10 0||y_1| = |1|
		// |2 1||x_2|   |0 10||y_2|   |2|
		Matrix Ax1(2, 2);
		Ax1(0, 0) = 1.0;
		Ax1(0, 1) = 2.0;
		Ax1(1, 0) = 2.0;
		Ax1(1, 1) = 1.0;
		Matrix Ay1 = eye(2) * 10;
		Vector b2 = Vector_(2, 1.0, 2.0);
		JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
				constraintModel));

		// construct the graph
		GaussianFactorGraph fg;
		fg.push_back(f1);
		fg.push_back(f2);

		return fg;
	}

	/* ************************************************************************* */
	VectorValues createSingleConstraintValues() {
		VectorValues config(vector<size_t>(2,2));
		config[_x_] = Vector_(2, 1.0, -1.0);
		config[_y_] = Vector_(2, 0.2, 0.1);
		return config;
	}

	/* ************************************************************************* */
	GaussianFactorGraph createMultiConstraintGraph() {
		// unary factor 1
		Matrix A = eye(2);
		Vector b = Vector_(2, -2.0, 2.0);
		JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1));

		// constraint 1
		Matrix A11(2, 2);
		A11(0, 0) = 1.0;
		A11(0, 1) = 2.0;
		A11(1, 0) = 2.0;
		A11(1, 1) = 1.0;

		Matrix A12(2, 2);
		A12(0, 0) = 10.0;
		A12(0, 1) = 0.0;
		A12(1, 0) = 0.0;
		A12(1, 1) = 10.0;

		Vector b1(2);
		b1(0) = 1.0;
		b1(1) = 2.0;
		JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
				constraintModel));

		// constraint 2
		Matrix A21(2, 2);
		A21(0, 0) = 3.0;
		A21(0, 1) = 4.0;
		A21(1, 0) = -1.0;
		A21(1, 1) = -2.0;

		Matrix A22(2, 2);
		A22(0, 0) = 1.0;
		A22(0, 1) = 1.0;
		A22(1, 0) = 1.0;
		A22(1, 1) = 2.0;

		Vector b2(2);
		b2(0) = 3.0;
		b2(1) = 4.0;
		JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
				constraintModel));

		// construct the graph
		GaussianFactorGraph fg;
		fg.push_back(lf1);
		fg.push_back(lc1);
		fg.push_back(lc2);

		return fg;
	}

	/* ************************************************************************* */
	VectorValues createMultiConstraintValues() {
		VectorValues config(vector<size_t>(3,2));
		config[_x_] = Vector_(2, -2.0, 2.0);
		config[_y_] = Vector_(2, -0.1, 0.4);
		config[_z_] = Vector_(2, -4.0, 5.0);
		return config;
	}

	/* ************************************************************************* */
	// Create key for simulated planar graph
	Symbol key(int x, int y) {
		return X(1000*x+y);
	}

	/* ************************************************************************* */
	boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {

		// create empty graph
		NonlinearFactorGraph nlfg;

		// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
		shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), Isotropic::Sigma(2,1e-3), key(1,1)));
		nlfg.push_back(constraint);

		// Create horizontal constraints, 1...N*(N-1)
		Point2 z1(1.0, 0.0); // move right
		for (size_t x = 1; x < N; x++)
			for (size_t y = 1; y <= N; y++) {
				shared f(new simulated2D::Odometry(z1, Isotropic::Sigma(2,0.01), key(x, y), key(x + 1, y)));
				nlfg.push_back(f);
			}

		// Create vertical constraints, N*(N-1)+1..2*N*(N-1)
		Point2 z2(0.0, 1.0); // move up
		for (size_t x = 1; x <= N; x++)
			for (size_t y = 1; y < N; y++) {
				shared f(new simulated2D::Odometry(z2, Isotropic::Sigma(2,0.01), key(x, y), key(x, y + 1)));
				nlfg.push_back(f);
			}

		// Create linearization and ground xtrue config
		Values zeros;
    for (size_t x = 1; x <= N; x++)
      for (size_t y = 1; y <= N; y++)
        zeros.insert(key(x, y), Point2());
		Ordering ordering(planarOrdering(N));
		VectorValues xtrue(zeros.dims(ordering));
		for (size_t x = 1; x <= N; x++)
			for (size_t y = 1; y <= N; y++)
				xtrue[ordering[key(x, y)]] = Point2(x,y).vector();

		// linearize around zero
		boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros, ordering);
		return boost::make_tuple(*gfg, xtrue);
	}

	/* ************************************************************************* */
	Ordering planarOrdering(size_t N) {
		Ordering ordering;
		for (size_t y = N; y >= 1; y--)
			for (size_t x = N; x >= 1; x--)
				ordering.push_back(key(x, y));
		return ordering;
	}

	/* ************************************************************************* */
	pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
			const GaussianFactorGraph& original) {
		GaussianFactorGraph T, C;

		// Add the x11 constraint to the tree
		T.push_back(original[0]);

		// Add all horizontal constraints to the tree
		size_t i = 1;
		for (size_t x = 1; x < N; x++)
			for (size_t y = 1; y <= N; y++, i++)
				T.push_back(original[i]);

		// Add first vertical column of constraints to T, others to C
		for (size_t x = 1; x <= N; x++)
			for (size_t y = 1; y < N; y++, i++)
				if (x == 1)
					T.push_back(original[i]);
				else
					C.push_back(original[i]);

		return make_pair(T, C);
	}

/* ************************************************************************* */

} // example
} // namespace gtsam


#include <math.h>
#include <cmath>
#include <RcppEigen.h>
#include <Rcpp.h>
#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <iostream>
// [[Rcpp::depends(RcppEigen)]]

typedef Eigen::Triplet<float> T; //tripleta para rellenar matrices sparse
typedef Eigen::MappedSparseMatrix<float> MSpMat; //alias para matriz sparse
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorXd; //alias para vector sparse
using namespace Rcpp;

// [[Rcpp::export]]
Eigen::SparseMatrix<float> Kernel_float ( NumericMatrix M, int h, int w, double distancia, double sigJ, double sigd)
{
  // Funcion para calcular  la matriz de similaridades de todos los puntos de una imagen
  //  input: w: altura en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         h: ancho en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         distancia: limite de la vecindad del pixel a considerar
  //         M: imagen de 2 dimensiones (EN ESCALA DE GRISES)
  //         sigJ: sigma para kernel gaussiano, de la imagen
  //         sigd: sigma para kernell gaussiano, de las distancias
  //output: W, similaridad calculada, sparse
  int i, j, k, l = 0 ;
  float vecindad = (float)distancia;
  float d2 = 0.;
  float calculo = 0.;
  std::vector<T> tripletList; //inicializamos tripleta
  tripletList.reserve((int)floor(h*h*w*w*.1));
  //comienza calculo de matriz de similaridades
  for(i=0; i < h; i++)
  {
    for(j=0; j< w; j++ )
    {
      for(k=0; k<h; k++)
      {
        for(l=0; l<w; l++)
        {
          d2 = (float)(pow(i-k,2)+pow(j-l,2));
          if(d2 <= vecindad)
          {
            calculo = (float) (exp(-pow(M(j,i)-M(l,k),2)/(2*pow(sigJ,2))-d2/(2*pow(sigd,2))));
            tripletList.push_back(T((j)*h+i, (l)*h+k, calculo ));
          }
        }
      }
    }
  }
  //termina calculo de matriz de similaridades
  Eigen::SparseMatrix<float>  W(h*w, h*w); //construccion de matriz de similaridades
  W.setFromTriplets(tripletList.begin(), tripletList.end());
  return(W);
}


// Std includes
#include <cmath>
#include <iostream>
#include <memory>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "s0s/runge_kutta_fehlberg.h"
#include "sl0/point.h"
#include "sl0/group/dynamic.h"
// Simple includes
#include "flow.h"

using TypeScalar = double;
// State
template<int Size>
using TypeVector = Eigen::Matrix<TypeScalar, Size, 1>;
// Space
constexpr unsigned int DIM = 3;
using TypeSpaceVector = Eigen::Matrix<TypeScalar, DIM, 1>;
// Ref and View
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
// Group Parameters
using TypeStepPoint = sl0::StepPoint<TypeVector, DIM, TypeView, Flow>;
// Solver
using TypeSolver = s0s::SolverRungeKuttaFehlberg<TypeVector<Eigen::Dynamic>, TypeView>;

int main () {
    // Parameters
    TypeSpaceVector x0 = TypeSpaceVector::Constant(1.0);
    TypeScalar t0 = 0.0;
    TypeScalar dt = 1e-3;
    unsigned int nt = std::round(1.0 / dt);
    unsigned int np = 1000;
    // Create group
    sl0::GroupDynamic<TypeVector, DIM, TypeView, TypeSolver> group;
    // Ceate points
    std::cout << "Point creation" << "\n";
    std::shared_ptr<TypeStepPoint> sStepPoint = std::make_shared<TypeStepPoint>(std::make_shared<Flow>());
    for(std::size_t i = 0; i < np; i++) {
        group.sStep->addMember(group.state, std::make_shared<TypeStepPoint>(*sStepPoint), DIM);
        sStepPoint->x(group.sStep->memberState(group.state.data(), i)) = x0;
    }
    // Set initial state
    std::cout << "Point init" << "\n";
    for(std::size_t n = 0; n < group.sStep->size(); n++) {
        sStepPoint->x(group.sStep->memberState(group.state.data(), n)) = x0;
    }
    group.t = t0;
    // Computation
    std::cout << "Computing" << "\n";
    for(std::size_t i = 0; i < nt; i++) {
        group.update(dt);
    }
    // out
    std::cout << "\n";
    std::cout << "Group advected following a an exponential flow, exp(" << group.t << ") = " << "\n";
    std::cout << "\n";
    for(std::size_t n = 0; n < group.sStep->size(); n++) {
        std::cout << "Group Final Position : " << "\n" << sStepPoint->x(group.sStep->memberState(group.state.data(), n)) << "\n";
    }
    std::cout << std::endl;
}


#include "LinearCodeGenerator.h"
#include "Math.h"

#include <unordered_map>
#include <gurobi_c++.h>
#include <armadillo>
#include <random>

/* Configuration */
constexpr bool SHOW_GUROBI_OUTPUT = true;
constexpr bool SHOW_MATRIX_OUTPUT = false;
constexpr uint32_t TIMEOUT_MATRIX_REDUCE = 1;

LinearCodeGenerator::LinearCodeGenerator(uint16_t q, uint16_t k, uint16_t b)
	: m_q(q)
	, m_k(k)
	, m_b(b)
{

}

auto LinearCodeGenerator::generateCode() const -> LinearCode
{
	std::cout << "Generating canonical represents" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;

	auto canonialRepresents = Math::generateCanonialRepresents(m_q, m_k);
	if (SHOW_MATRIX_OUTPUT)
	{
		for (arma::uword i = 0; i < canonialRepresents.size(); ++i)
		{
			std::cout << "r" << i << "\t";
			canonialRepresents.at(i).t().raw_print();
		}
	}
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	std::cout << "Generating matrix A" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;
	const auto matrixA = generateMatrixA(canonialRepresents);
	if (SHOW_MATRIX_OUTPUT)
	{
		matrixA.raw_print();
	}
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	std::cout << "Generating producer e0" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;
	const auto producerE0 = generateProducerE0(canonialRepresents, matrixA);

	const auto colCycles = cycleCheck(canonialRepresents, producerE0, m_q);
	const auto rowCycles = cycleCheck(canonialRepresents, producerE0.t(), m_q);

	auto colGroups = generateRepresentGroups(colCycles, canonialRepresents.size());
	auto rowGroups = generateRepresentGroups(rowCycles, canonialRepresents.size());

	std::cout << "Found Producer e0 - reducing to [" << colGroups.size() << "] groups" << std::endl;
	if (SHOW_MATRIX_OUTPUT)
	{
		producerE0.raw_print();
		std::cout << "----------------------------------------------------" << std::endl << std::endl;
		std::cout << "Found cycles for columns" << std::endl;
		std::cout << "----------------------------------------------------" << std::endl;
		for (auto& group : colGroups)
		{
			std::cout << "S([r" << group.at(0) << "]) = {";
			for (arma::uword i = 0; i < group.size(); i++)
			{
				std::cout << "[r" << group.at(i) << "]";
				if (i != group.size() - 1) std::cout << ", ";
			}
			std::cout << "}" << std::endl;
		}

		std::cout << std::endl;
		std::cout << "Found cycles for rows" << std::endl;
		std::cout << "----------------------------------------------------" << std::endl;
		for (auto& group : rowGroups)
		{
			std::cout << "ST([r" << group.at(0) << "]) = {";
			for (arma::uword i = 0; i < group.size(); i++)
			{
				std::cout << "[r" << group.at(i) << "]";
				if (i != group.size() - 1) std::cout << ", ";
			}
			std::cout << "}" << std::endl;
		}
	}
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	std::cout << "Re-calculate matrix A with cylces" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;
	const auto optMatrixA = generateOptimizedMatrixA(matrixA, colCycles, rowCycles);
	if (SHOW_MATRIX_OUTPUT)
	{
		optMatrixA.raw_print();
	}
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	std::cout << "Calculate vector c" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;
	const auto vectorC = generateVectorC(colGroups);
	vectorC.t().raw_print();
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	std::cout << "Calculating vector x" << std::endl;
	std::cout << "----------------------------------------------------" << std::endl;
	auto vectorX = generateVectorX(optMatrixA, vectorC, m_b);
	vectorX.t().raw_print();
	std::cout << "----------------------------------------------------" << std::endl << std::endl;

	const uint16_t n = dot(vectorX, vectorC);
	const uint16_t d = n - m_b;
	auto G = generateMatrixG(vectorX, colGroups, canonialRepresents);

	return { n, m_q, m_k, d, G };
}

auto LinearCodeGenerator::generateProducerE0(const std::vector<arma::s32_vec>& canonialRepresents, const arma::s32_mat& matrixA) const -> arma::s32_mat
{
	arma::s32_mat e0(m_k, m_k);
	arma::s32_mat test_e0(m_k, m_k);
	test_e0.fill(0);
	auto colGroupCount = std::numeric_limits<uint32_t>::max();
	uint32_t timeout = 0;

	std::chrono::high_resolution_clock::time_point start;
	do
	{
		auto colCycles = cycleCheck(canonialRepresents, test_e0, m_q);
		auto colGroups = generateRepresentGroups(colCycles, canonialRepresents.size());

		uint32_t maxElements = 0;
		for (auto& colGroup : colGroups)
		{
			if (colGroup.size() > maxElements)
			{
				maxElements = colGroup.size();
			}
		}
		if (maxElements < m_q && colGroups.size() < colGroupCount)
		{
			colGroupCount = colGroups.size();
			e0 = test_e0;
			timeout = 0;
		}

		const auto end = std::chrono::high_resolution_clock::now();
		const auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
		if (duration > 1000)
		{
			std::cout << "Current producer e0 reduces A from [" << matrixA.n_cols << "] to [" << colGroupCount << "] columns." << std::endl;
			start = end;
			++timeout;
		}

		test_e0 = generateMatrixE0();
	} while (colGroupCount == matrixA.n_cols || timeout < TIMEOUT_MATRIX_REDUCE);

	return e0;
}

auto LinearCodeGenerator::generateOptimizedMatrixA(const arma::s32_mat& matrixA,
	const std::unordered_map<arma::uword, std::vector<arma::uword>>& colCycles,
	const std::unordered_map<arma::uword, std::vector<arma::uword>>& rowCycles) const -> arma::s32_mat
{
	auto optMatrixA = matrixA;
	std::vector<arma::uword> colsDelete;
	std::vector<arma::uword> rowsDelete;

	for (auto& c : colCycles)
	{
		for (arma::uword i = 1; i < c.second.size(); ++i)
		{
			for (arma::uword j = 0; j < optMatrixA.n_rows; j++)
			{
				optMatrixA(j, c.second.at(0)) += optMatrixA(j, c.second.at(i));
			}

			colsDelete.push_back(c.second.at(i));
		}
	}

	for (auto& r : rowCycles)
	{
		for (arma::uword i = 1; i < r.second.size(); ++i)
		{
			rowsDelete.push_back(r.second.at(i));
		}
	}

	sort(colsDelete.begin(), colsDelete.end());
	reverse(colsDelete.begin(), colsDelete.end());
	for (auto& c : colsDelete)
	{
		optMatrixA.shed_col(c);
	}

	sort(rowsDelete.begin(), rowsDelete.end());
	reverse(rowsDelete.begin(), rowsDelete.end());
	for (auto& r : rowsDelete)
	{
		optMatrixA.shed_row(r);
	}

	return optMatrixA;
}

auto LinearCodeGenerator::generateMatrixG(const arma::s32_vec& xVec, const std::vector<std::vector<arma::uword>>& colGroups,
	const std::vector<arma::s32_vec>& rVector) -> arma::s32_mat
{
	uint32_t colSize = 0;
	for (uint32_t i = 0; i < xVec.size(); ++i)
	{
		colSize += xVec.at(i) * colGroups.at(i).size();
	}

	const auto rowSize = rVector.at(0).size();
	arma::s32_mat G(rowSize, colSize, arma::fill::zeros);

	int colIndex = 0;
	for (uint32_t x = 0; x < xVec.size(); ++x)
	{
		for (auto i = 0; i < xVec.at(x); ++i)
		{
			for (unsigned int j : colGroups.at(x))
			{
				for (uint32_t k = 0; k < rowSize; ++k)
				{
					G(k, colIndex) = rVector.at(j).at(k);
				}
				colIndex++;
			}
		}
	}

	return G;
}

auto LinearCodeGenerator::generateVectorC(const std::vector<std::vector<arma::uword>>& colGroups) -> arma::s32_vec
{
	arma::s32_vec vectorC(colGroups.size());

	for (arma::uword i = 0; i < colGroups.size(); ++i)
	{
		vectorC.at(i) = colGroups.at(i).size();
	}

	return vectorC;
}

auto LinearCodeGenerator::generateVectorX(const arma::s32_mat& matrixA, const arma::s32_vec& vectorC, uint16_t b) -> arma::s32_vec
{
	const auto env = GRBEnv();
	auto model = GRBModel(env);
	model.getEnv().set(GRB_IntParam_OutputFlag, SHOW_GUROBI_OUTPUT);

	std::vector<GRBVar> modelVars;
	modelVars.reserve(vectorC.size());
	for (uint32_t i = 0; i < vectorC.size(); i++)
	{
		modelVars.push_back(model.addVar(0.0, GRB_INFINITY, 0.0, GRB_INTEGER));
	}

	GRBLinExpr expr = 0.0;
	for (uint32_t i = 0; i < vectorC.size(); i++)
	{
		expr += modelVars.at(i) * vectorC.at(i);
	}

	model.setObjective(expr, GRB_MAXIMIZE);

	for (arma::uword x = 0; x < matrixA.n_rows; x++)
	{
		expr = 0.0;
		for (arma::uword y = 0; y < matrixA.n_cols; y++)
		{
			expr += modelVars.at(y) * matrixA(x, y);
		}
		model.addConstr(expr, GRB_LESS_EQUAL, b);
	}

	//model.update();
	//model.write("debug.lp");
	model.optimize();

	arma::s32_vec vectorX(modelVars.size());
	for (uint32_t i = 0; i < modelVars.size(); ++i)
	{
		vectorX.at(i) = static_cast<int>(modelVars.at(i).get(GRB_DoubleAttr_X));
	}

	return vectorX;
}

auto LinearCodeGenerator::generateMatrixA(const std::vector<arma::s32_vec>& canonialRepresents) const -> arma::s32_mat
{
	auto matrixA = arma::s32_mat(canonialRepresents.size(), canonialRepresents.size(), arma::fill::zeros);

	for (arma::uword row = 0; row < canonialRepresents.size(); ++row)
	{
		for (arma::uword col = 0; col < canonialRepresents.size(); ++col)
		{
			if (static_cast<int>(dot(canonialRepresents.at(row), canonialRepresents.at(col))) % m_q == 0)
			{
				matrixA(row, col) = 1;
			}
		}
	}

	return matrixA;
}

auto sortHelper(const std::vector<arma::uword>& i, const std::vector<arma::uword>& j) -> bool
{
	return i.at(0) < j.at(0);
}

auto LinearCodeGenerator::generateRepresentGroups(const std::unordered_map<arma::uword, std::vector<arma::uword>>& cycle,
	int vecCount) const -> std::vector<std::vector<arma::uword>>
{
	std::vector<std::vector<arma::uword>> repGroups;
	repGroups.reserve(cycle.size());
	for (auto& c : cycle)
	{
		repGroups.push_back(c.second);
	}

	for (auto i = 0; i < vecCount; ++i)
	{
		auto found = false;
		for (auto& repGroup : repGroups)
		{
			for (int k : repGroup)
			{
				if (k == i)
				{
					found = true;
				}
			}
		}

		if (!found)
		{
			repGroups.push_back(std::vector<arma::uword>{static_cast<arma::uword>(i)});
		}
	}

	std::sort(repGroups.begin(), repGroups.end(), sortHelper);

	return repGroups;
}

auto LinearCodeGenerator::generateMatrixE0() const -> arma::s32_mat
{
	std::default_random_engine generator;
	const std::uniform_int_distribution<int> distribution(0, m_q - 1);

	arma::mat e(m_k, m_k);
	do
	{
		for (auto i = 0; i < m_k; ++i)
		{
			for (auto j = 0; j < m_k; ++j)
			{
				generator.seed(std::random_device()());
				e(i, j) = distribution(generator);
			}
		}
	} while (0 == arma::det(e));

	arma::s32_mat e0(m_k, m_k);
	for (auto i = 0; i < m_k; ++i)
	{
		for (auto j = 0; j < m_k; ++j)
		{
			e0(i, j) = e(i, j);
		}
	}

	return e0;
}

auto LinearCodeGenerator::cycleCheck(const std::vector<arma::s32_vec>& rVector, const arma::s32_mat& E, uint16_t q) const
-> std::unordered_map<arma::uword, std::vector<arma::uword>>
{
	std::unordered_map<arma::uword, arma::uword> convert;
	for (arma::uword i = 0; i < rVector.size(); i++)
	{
		arma::s32_vec tmpVec = E * rVector.at(i);
		Math::normalizeVector(tmpVec, q);
		if (!Math::isLinDependent(rVector.at(i), tmpVec, q))
		{
			const auto vectorIndex = Math::getVectorIndex(rVector, tmpVec, q);
			if (-1 != vectorIndex)
			{
				//cout << "Found conversion: r" << i << " -> r" << vectorIndex << endl;
				convert.insert({ i, vectorIndex });
			}
		}
	}

	std::unordered_map<arma::uword, std::vector<arma::uword>> cycles;
	for (arma::uword i = 0; i < rVector.size(); i++)
	{
		const auto key = convert.find(i);
		if (key != convert.end())
		{
			std::vector<arma::uword> tmp;
			tmp.push_back(key->first);
			tmp.push_back(key->second);

			bool search;
			do
			{
				search = false;
				const auto next = convert.find(tmp.back());
				if (next != convert.end())
				{
					const auto loop = find(tmp.begin() + 1, tmp.end(), next->second);
					if (loop != tmp.end())
					{
						tmp.erase(tmp.begin(), loop);
					}

					tmp.push_back(next->second);
					search = true;
				}
			} while (search && tmp.front() != tmp.back());

			if (tmp.front() == tmp.back())
			{
				tmp.pop_back();
				std::sort(tmp.begin(), tmp.end());
				if (cycles.find(tmp.front()) == cycles.end())
				{
					cycles.insert({ tmp.front(), tmp });
				}
			}
		}
	}

	return cycles;
}


/***************************************************************************
                          meshalgs.cc  -  description
                             -------------------
    begin                : Mon Mar 31 2008
    copyright            : (C) 2005 by Knut-Helge Vik
    email                : knuthelv@ifi.uio.no
 ***************************************************************************/
#include "meshalgs.h"
#include "../treealgs/treestructure.h"
#include "../simdefs.h"
#include "../treealgs/fheap.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/graph_utility.hpp>
#include <functional>
#include <iostream>
#include <fstream>
#include <cmath>

using namespace std;
using namespace boost;
using namespace TreeAlgorithms;

//typedef TreeAlgorithms::COLOR COLOR;

namespace GraphAlgorithms
{

// dijkstra's shortest path tree
void maxDistance(const GraphN &g, const VertexSet &V, int src, DistanceVector &zdistance, DistanceVector &hops, ParentVector &zparent, double &eccentricity, int &node_in_eccentricity_path)
{
    out_edge_iteratorN oit, oit_end;
    //enum COLOR { WHITE = 0, GRAY, BLACK};
    std::vector<TreeAlgorithms::COLOR> colorVector(num_vertices(g));

    HeapD<FHeap> heapD;
    Heap *heap = heapD.newInstance(num_vertices(g));

    for(VertexSet::iterator vit = V.begin(), vit_end = V.end(); vit != vit_end; ++vit)
    {
        zdistance[*vit] = (std::numeric_limits<double>::max)();
        zparent[*vit] = *vit;
        hops[*vit] = 0;
        colorVector[*vit] = WHITE;
    }

    colorVector[src] = GRAY;
    zdistance[src] = 0;
    heap->insert(src, 0.0);

    while(heap->nItems() > 0)
    {
        int u = heap->deleteMin();
        for(boost::tuples::tie(oit, oit_end) = out_edges(u, g); oit != oit_end; ++oit)
        {
            int v = target(*oit, g);
            double path_weight = zdistance[u] + g[*oit].weight;

            if((path_weight < zdistance[v]))
            {
                zdistance[v] = path_weight;
                zparent[v] = u;
                hops[v] = hops[u] + 1;

                if(colorVector[v] == WHITE)
                {
                    colorVector[v] = GRAY;
                    heap->insert(v, zdistance[v]);
                }
                else if(colorVector[v] == GRAY)
                {
                    heap->decreaseKey(v, zdistance[v]);
                }

                if(zdistance[v] >= eccentricity)
                {
                    eccentricity = zdistance[v];
                    node_in_eccentricity_path = v;
                }
            }
        }
        colorVector[u] = BLACK;
    }
    delete heap;
}

void maxDistance(const TreeStructure &T, int src, DistanceVector &zdistance, DistanceVector &hops, ParentVector &zparent, double &eccentricity, int &node_in_eccentricity_path)
{
    maxDistance(T.g, T.V, src, zdistance, hops, zparent, eccentricity, node_in_eccentricity_path);
}

// dijkstra's shortest path tree
void maxDistance(const TreeStructure &T, int src, DistanceVector &zdistance, DistanceVector &hops, ParentVector &zparent)
{
    double ecc = 0;
    int node = -1;

    maxDistance(T, src, zdistance, hops, zparent,ecc, node);
}

double eccentricity_distance(const TreeStructure &T, int src)
{
    return eccentricity_distance(T.g, T.V, src);
}

double eccentricity_distance(const GraphN &g, const VertexSet &V, int src)
{
    DistanceVector 	distance(num_vertices(g));
    DistanceVector 	hops(num_vertices(g));
    ParentVector	parent(num_vertices(g));
    double ecc = 0;
    int node = -1;

    maxDistance(g, V, src, distance, hops, parent, ecc, node);

    return ecc;
}

double eccentricity_distance(const GraphN &g, const VertexSet &V, int src, DistanceVector &distance, DistanceVector &hops, ParentVector &parent)
{
    double ecc = 0;
    int node = -1;

    maxDistance(g, V, src, distance, hops, parent, ecc, node);
    return ecc;
}

// breadth first search tree
void maxDistanceHops(const TreeStructure &T, int src, DistanceVector &distance, DistanceVector &hops, ParentVector &parent)
{
    const GraphN &g = T.g;
    ASSERTING(T.V.contains(src));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        distance[*vit] = (std::numeric_limits<double>::max)();   // set to maximum distance
        hops[*vit] = (std::numeric_limits<double>::max)();   // set to maximum distance
        parent[*vit] = *vit;
    }

    distance[src] = 0;
    hops[src] = 0;
    parent[src] = src;

    typedef list<int> IntList;
    IntList neighbors;
    neighbors.push_back(src);

    while(!neighbors.empty())
    {
        int v = neighbors.front();
        neighbors.pop_front();

        ASSERTING(T.V.contains(v));
        out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(v, g); oit != oit_end; ++oit)
        {
            int u = target(*oit, g);
            ASSERTING(T.V.contains(u));

            if(hops[u] == (std::numeric_limits<double>::max()))
            {
                hops[u] = hops[v] + 1;
                distance[u] = distance[v] + g[*oit].weight;
                parent[u] = v;
                neighbors.push_back(u);
            }
        }
    }
}

// breadth first search tree
double maxDistanceHops(const TreeStructure &T, int src, DistanceVector &distance)
{
    const GraphN &g = T.g;
    ASSERTING(T.V.contains(src));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
        distance[*vit] = (std::numeric_limits<double>::max)();

    distance[src] = 0;
    double diameter = 0;

    typedef list<int> IntList;
    IntList neighbors;
    neighbors.push_back(src);

    while(!neighbors.empty())
    {
        int v = neighbors.front();
        neighbors.pop_front();

        out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(v, g); oit != oit_end; ++oit)
        {
            int u = target(*oit, g);
            ASSERTING(T.V.contains(u));

            if(distance[u] == (std::numeric_limits<double>::max()))
            {
                distance[u] = distance[v] + 1;
                if(distance[u] > diameter) diameter = distance[u];

                neighbors.push_back(u);
            }
        }
    }
    return diameter;
}

double eccentricity_hops(const TreeStructure &T, int src)
{
    DistanceVector 	distance(num_vertices(T.g));

    return maxDistanceHops(T, src, distance);
}

double diameter_hops(const TreeStructure &T)
{
    double max = 0;
    DistanceVector distance(num_vertices(T.g));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        int v = *vit;
        double d = maxDistanceHops(T, v, distance);
        max = d > max ? d : max;
    }

    return max;
}

double diameter_distance(const TreeStructure &T)
{
    double diameter = 0;
    PathList diameterPath;
    diameter_distance(T, diameterPath, diameter);

    return diameter;
}

void diameter_distance(const TreeStructure &T, PathList &diameterPath, double &diameter)
{
    DistanceVector 	distance(num_vertices(T.g));
    DistanceVector 	hops(num_vertices(T.g));
    ParentVector	parent(num_vertices(T.g));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        double eccentricity = 0;
        int node_in_eccentricity_path = -1;
        int v = *vit;
        maxDistance(T, v, distance, hops, parent, eccentricity, node_in_eccentricity_path);

        if(diameter < eccentricity)
        {
            diameter = eccentricity;
            diameterPath.clear();

            ASSERTING(node_in_eccentricity_path != v);
            int path_node = node_in_eccentricity_path;
            while(path_node != v)
            {
                diameterPath.push_back(path_node);
                path_node = parent[path_node];
            }

            diameterPath.push_back(v);

            //cerr << WRITE_FUNCTION << " diameter : " << diameter << endl;
        }
    }
}

// matrix of the shortest distances between any two vertices.
void distanceMatrix(const TreeStructure &T, DistanceVectorMatrix &distance, DistanceVectorMatrix &hops, ParentVectorMatrix &parent)
{
    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        int v = *vit;
        maxDistance(T, v, distance[v], hops[v], parent[v]);
    }
}

void distanceMatrixHops(const TreeStructure &T, DistanceVectorMatrix &distance)
{
    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        int v = *vit;
        maxDistanceHops(T, v, distance[v]);
    }
}

void printAllPairsSP(ParentMatrix &parent, int src, int targ, PathList &path)
{
    if(src == targ)
    {
        cerr << src << " ";
        path.push_back(src);
    }
    else if(parent[src][targ] == src)
    {
        cerr << " no path from " << src << " to " << targ << " exists " << endl;
    }
    else
    {
        printAllPairsSP(parent, src, parent[src][targ], path);
        cerr << targ << " ";
        path.push_back(targ);
    }
}

};

/*
double maxDistanceHops(const TreeStructure &T, int src, DistanceVector &distance)
{
    const GraphN &g = T.g;
    ASSERTING(T.V.contains(src));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
        distance[*vit] = (std::numeric_limits<double>::max)();   // set to maximum distance

    distance[src] = 0;
    double depth = 1;                       // next distance in BFS
    int cnt = 0;

    VertexSet neighborSet;
    neighborSet.insert(src);

    while(!neighborSet.empty())
    {
        VertexSet nextNeighbors;
        for(VertexSet::iterator vit = neighborSet.begin(), vit_end = neighborSet.end(); vit != vit_end; ++vit)
        {
            int v = *vit;
            out_edge_iteratorN oit, oit_end;
            ASSERTING(T.V.contains(v));

            for(boost::tuples::tie(oit, oit_end) = out_edges(v, g); oit != oit_end; ++oit)
            {
                int u = target(*oit, g);
                if(distance[u] == (std::numeric_limits<double>::max)()) // first time reachable?
                {
                    cnt++;
                    distance[u] = depth;
                    nextNeighbors.insert(u);
                }
            }
        }
        // next try set of vertices further away
        neighborSet = nextNeighbors;
        depth++;
    }

    return cnt == T.V.size() ? depth-1 : T.V.size();
}
*/

/*void maxDistance(const TreeStructure &T, int src, DistanceVector &distance, DistanceVector &hops, ParentVector &parent, double &eccentricity, int &node_in_eccentricity_path)
{
    const GraphN &g = T.g;
    ASSERTING(T.V.contains(src));

    for(VertexSet::iterator vit = T.V.begin(), vit_end = T.V.end(); vit != vit_end; ++vit)
    {
        distance[*vit] = (std::numeric_limits<double>::max)();   // set to maximum distance
        hops[*vit] = (std::numeric_limits<double>::max)();   // set to maximum distance
        parent[*vit] = *vit;
    }

    distance[src] = 0;
    hops[src] = 0;
    parent[src] = src;

    typedef list<int> IntList;
    IntList neighbors;
    neighbors.push_back(src);

    while(!neighbors.empty())
    {
        int v = neighbors.front();
        neighbors.pop_front();

        ASSERTING(T.V.contains(v));
        out_edge_iteratorN oit, oit_end;
        for(boost::tuples::tie(oit, oit_end) = out_edges(v, g); oit != oit_end; ++oit)
        {
            int u = target(*oit, g);
            ASSERTING(T.V.contains(u));

            if(distance[u] > distance[v] + g[*oit].weight)
            {
                hops[u] = hops[v] + 1;
                distance[u] = distance[v] + g[*oit].weight;
                parent[u] = v;
                neighbors.push_back(u);

                if(distance[u] >= eccentricity)
                {
                    eccentricity = distance[u];
                    node_in_eccentricity_path = u;
                }
                //cerr << u << " hops " << hops[u] << " distance " << distance[u] << " parent " << parent[u] << endl;
            }
        }
    }
}*/


/* Copyright (C) 2012-2019 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
/* EncryptedArray.cpp - Data-movement operations on arrays of slots
 */
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <helib/EncryptedArray.h>
#include <helib/polyEval.h>
#include <helib/debugging.h>

namespace helib {

// Compute a degree-p polynomial poly(x) s.t. for any t<e and integr z of the
// form z = z0 + p^t*z1 (with 0<=z0<p), we have poly(z) = z0 (mod p^{t+1}).
//
// We get poly(x) by interpolating a degree-(p-1) polynomial poly'(x)
// s.t. poly'(z0)=z0 - z0^p (mod p^e) for all 0<=z0<p, and then setting
// poly(x) = x^p + poly'(x).
static void buildDigitPolynomial(NTL::ZZX& result, long p, long e)
{
  if (p<2 || e<=1) return; // nothing to do
  FHE_TIMER_START;
  long p2e = NTL::power_long(p,e); // the integer p^e

  // Compute x - x^p (mod p^e), for x=0,1,...,p-1
  NTL::vec_long x(NTL::INIT_SIZE, p);
  NTL::vec_long y(NTL::INIT_SIZE, p);
  long bottom = -(p/2);
  for (long j=0; j<p; j++) {
    long z = bottom+j;
    x[j] = z;
    y[j] = z-NTL::PowerMod((z < 0 ? z + p2e : z), p, p2e);  // x - x^p (mod p^e)

    while (y[j] > p2e/2)         y[j] -= p2e;
    while (y[j] < -(p2e/2))      y[j] += p2e;
  }
  interpolateMod(result, x, y, p, e);
  //OLD: assert(deg(result)<p); // interpolating p points, should get deg<=p-1
  helib::assertTrue(deg(result)<p, "Interpolation error - degree too high");
  SetCoeff(result, p);   // return result = x^p + poly'(x)
  //  cerr << "# digitExt mod "<<p<<"^"<<e<<"="<<result<<endl;
  FHE_TIMER_STOP;
}

// extractDigits assumes that the slots of *this contains integers mod p^r
// i.e., that only the free terms are nonzero. (If that assumptions does
// not hold then the result will not be a valid ciphertext anymore.)
//
// It returns in the slots of digits[j] the j'th-lowest gigits from the
// integers in the slots of the input. Namely, the i'th slot of digits[j]
// contains the j'th digit in the p-base expansion of the integer in the
// i'th slot of the *this. The plaintext space of digits[j] is mod p^{r-j},
// and all the digits are at the same level.

int fhe_watcher = 0;

void extractDigits(std::vector<Ctxt>& digits, const Ctxt& c, long r)
{
  const Context& context = c.getContext();
  long rr = c.effectiveR();
  if (r<=0 || r>rr) r = rr; // how many digits to extract

  long p = context.zMStar.getP();

  NTL::ZZX x2p;
  if (p>3) {
    buildDigitPolynomial(x2p, p, r);
  }

  Ctxt tmp(c.getPubKey(), c.getPtxtSpace());
  digits.resize(r, tmp);      // allocate space

#ifdef DEBUG_PRINTOUT
  fprintf(stderr, "***\n");
#endif
  for (long i=0; i<r; i++) {
    tmp = c;
    for (long j=0; j<i; j++) {

      if (p==2) digits[j].square();
      else if (p==3) digits[j].cube();
      else polyEval(digits[j], x2p, digits[j]);
      // "in spirit" digits[j] = digits[j]^p

#ifdef DEBUG_PRINTOUT
      fprintf(stderr, "%5ld", digits[j].bitCapacity());
#endif

      tmp -= digits[j];
      tmp.divideByP();
    }
    digits[i] = tmp; // needed in the next round

#ifdef DEBUG_PRINTOUT
    if (dbgKey) {
       double ratio =
          log(embeddingLargestCoeff(digits[i], *dbgKey)/digits[i].getNoiseBound())/log(2.0);
       fprintf(stderr, "%5ld [%f]", digits[i].bitCapacity(), ratio);
       if (ratio > 0) fprintf(stderr, " BAD-BOUND");
       fprintf(stderr, "\n");
    }
    else {
       fprintf(stderr, "%5ld\n", digits[i].bitCapacity());
    }
#endif
  }

#ifdef DEBUG_PRINTOUT
  fprintf(stderr, "***\n");
#endif
}

static
void compute_a_vals(NTL::Vec<NTL::ZZ>& a, long p, long e)
// computes a[m] = a(m)/m! for m = p..(e-1)(p-1)+1,
// as defined by Chen and Han.
// a.length() is set to (e-1)(p-1)+2

{
   NTL::ZZ p_to_e = NTL::power_ZZ(p, e);
   NTL::ZZ p_to_2e = NTL::power_ZZ(p, 2*e);

   long len = (e-1)*(p-1)+2;

   NTL::ZZ_pPush push(p_to_2e);

   NTL::ZZ_pX x_plus_1_to_p = power(NTL::ZZ_pX(NTL::INIT_MONO, 1) + 1, p);
   NTL::ZZ_pX denom = InvTrunc(x_plus_1_to_p - NTL::ZZ_pX(NTL::INIT_MONO, p), len);
   NTL::ZZ_pX poly = MulTrunc(x_plus_1_to_p, denom, len);
   poly *= p;

   a.SetLength(len);

   NTL::ZZ m_fac(1);
   for (long m = 2; m < p; m++) {
      m_fac = MulMod(m_fac, m, p_to_2e);
   }

   for (long m = p; m < len; m++) {
      m_fac = MulMod(m_fac, m, p_to_2e);
      NTL::ZZ c = rep(coeff(poly, m));
      NTL::ZZ d = GCD(m_fac, p_to_2e);
      if (d == 0 || d > p_to_e || c % d != 0) throw helib::RuntimeError("cannot divide");
      NTL::ZZ m_fac_deflated = (m_fac / d) % p_to_e;
      NTL::ZZ c_deflated = (c / d) % p_to_e;
      a[m] = MulMod(c_deflated, InvMod(m_fac_deflated, p_to_e), p_to_e);
   }

}

// This computes Chen and Han's magic polynomial G, which
// has the property that G(x) = (x mod p) (mod p^e).
// Here, (x mod p) is in the interval [0,1] if p == 2,
// and otherwise, is in the interval (-p/2, p/2).
static
void compute_magic_poly(NTL::ZZX& poly1, long p, long e)
{
   FHE_TIMER_START;

   NTL::Vec<NTL::ZZ> a;

   compute_a_vals(a, p, e);

   NTL::ZZ p_to_e = NTL::power_ZZ(p, e);
   long len = (e-1)*(p-1)+2;

   NTL::ZZ_pPush push(p_to_e);

   NTL::ZZ_pX poly(0);
   NTL::ZZ_pX term(1);
   NTL::ZZ_pX X(NTL::INIT_MONO, 1);

   poly = 0;
   term = 1;

   for (long m = 0; m < p; m++) {
      term *= (X-m);
   }

   for (long m = p; m < len; m++) {
      poly += term * NTL::conv<NTL::ZZ_p>(a[m]);
      term *= (X-m);
   }

   // replace poly by poly(X+(p-1)/2) for odd p
   if (p % 2 == 1) {
      NTL::ZZ_pX poly2(0);

      for (long i = deg(poly); i >= 0; i--)
         poly2 = poly2*(X+(p-1)/2) + poly[i];

      poly = poly2;
   }

   poly = X - poly;
   poly1 = NTL::conv<NTL::ZZX>(poly);
}

// extendExtractDigits assumes that the slots of *this contains integers mod
// p^{r+e} i.e., that only the free terms are nonzero. (If that assumptions
// does not hold then the result will not be a valid ciphertext anymore.)
//
// It returns in the slots of digits[j] the j'th-lowest digits from the
// integers in the slots of the input. Namely, the i'th slot of digits[j]
// contains the j'th digit in the p-base expansion of the integer in the i'th
// slot of the *this.  The plaintext space of digits[j] is mod p^{e+r-j}.

void extendExtractDigits(std::vector<Ctxt>& digits, const Ctxt& c, long r, long e)
{
  const Context& context = c.getContext();

  long p = context.zMStar.getP();
  NTL::ZZX x2p;
  if (p>3) {
    buildDigitPolynomial(x2p, p, r);
  }

  // we should pre-compute this table
  // for i = 0..r-1, entry i is G_{e+r-i} in Chen and Han
  NTL::Vec<NTL::ZZX> G;
  G.SetLength(r);
  for (long i: range(r)) {
    compute_magic_poly(G[i], p, e+r-i);
  }

  std::vector<Ctxt> digits0;

  Ctxt tmp(c.getPubKey(), c.getPtxtSpace());

  digits.resize(r, tmp);      // allocate space
  digits0.resize(r, tmp);

#ifdef DEBUG_PRINTOUT
  fprintf(stderr, "***\n");
#endif
  for (long i: range(r)) {
    tmp = c;
    for (long j: range(i)) {
      if (digits[j].capacity() >= digits0[j].capacity()) {
         // optimization: digits[j] is better than digits0[j],
         // so just use it

         tmp -= digits[j];
#ifdef DEBUG_PRINTOUT
      fprintf(stderr, "%5ld*", digits[j].bitCapacity());
#endif
      }
      else {
	if (p==2) digits0[j].square();
	else if (p==3) digits0[j].cube();
	else polyEval(digits0[j], x2p, digits0[j]); // "in spirit" digits0[j] = digits0[j]^p

	tmp -= digits0[j];
#ifdef DEBUG_PRINTOUT
      fprintf(stderr, "%5ld ", digits0[j].bitCapacity());
#endif
      }
      tmp.divideByP();
    }
    digits0[i] = tmp; // needed in the next round
    polyEval(digits[i], G[i], tmp);

#ifdef DEBUG_PRINTOUT
    if (dbgKey) {
      double ratio =
        log(embeddingLargestCoeff(digits[i], *dbgKey)/digits[i].getNoiseBound())/log(2.0);
      fprintf(stderr, "%5ld  --- %5ld", digits0[i].bitCapacity(), digits[i].bitCapacity());
      fprintf(stderr, " [%f]", ratio);
      if (ratio > 0) fprintf(stderr, " BAD-BOUND");
      fprintf(stderr, "\n");
    }
    else {
      fprintf(stderr, "%5ld  --- %5ld\n", digits0[i].bitCapacity(), digits[i].bitCapacity());
    }
#endif
  }
}

}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*!
 Copyright (C) 2016 Andres Hernandez

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/qldefines.hpp>
#ifdef BOOST_MSVC
#  include <ql/auto_link.hpp>
#endif
#include <ql/math/optimization/differentialevolution.hpp>
#include <ql/math/optimization/simulatedannealing.hpp>
#include <ql/experimental/math/fireflyalgorithm.hpp>
#include <ql/experimental/math/hybridsimulatedannealing.hpp>
#include <ql/experimental/math/particleswarmoptimization.hpp>
#include <ql/functional.hpp>

#include <boost/timer.hpp>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <iomanip>

using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {

    Integer sessionId() { return 0; }

}
#endif

unsigned long seed = 127;

/*
    Some benchmark functions taken from
    https://en.wikipedia.org/wiki/Test_functions_for_optimization

    Global optimizers have generally a lot of hyper-parameters, and one
    * usually requires some hyper-parameter optimization to find appropriate values
*/

Real ackley(const Array& x) {
    //Minimum is found at 0
    Real p1 = 0.0, p2 = 0.0;

    for (Size i = 0; i < x.size(); i++) {
        p1 += x[i] * x[i];
        p2 += std::cos(M_TWOPI*x[i]);
    }
    p1 = -0.2*std::sqrt(0.5*p1);
    p2 *= 0.5;
    return M_E + 20.0 - 20.0*std::exp(p1)-std::exp(p2);
}

Disposable<Array> ackleyValues(const Array& x) {
    Array y(x.size());
    for (Size i = 0; i < x.size(); i++) {
        Real p1 = x[i] * x[i];
        p1 = -0.2*std::sqrt(0.5*p1);
        Real p2 = 0.5*std::cos(M_TWOPI*x[i]);
        y[i] = M_E + 20.0 - 20.0*std::exp(p1)-std::exp(p2);
    }
    return y;
}

Real sphere(const Array& x) {
    //Minimum is found at 0
    return DotProduct(x, x);
}

Disposable<Array> sphereValues(const Array& x) {
    Array y(x.size());
    for (Size i = 0; i < x.size(); i++) {
        y[i] = x[i]*x[i];
    }
    return y;
}

Real rosenbrock(const Array& x) {
    //Minimum is found at f(1, 1, ...)
    QL_REQUIRE(x.size() > 1, "Input size needs to be higher than 1");
    Real result = 0.0;
    for (Size i = 0; i < x.size() - 1; i++) {
        Real temp = (x[i + 1] - x[i] * x[i]);
        result += (x[i] - 1.0)*(x[i] - 1.0) + 100.0*temp*temp;
    }
    return result;
}

Real easom(const Array& x) {
    //Minimum is found at f(\pi, \pi, ...)
    Real p1 = 1.0, p2 = 0.0;
    for (Size i = 0; i < x.size(); i++) {
        p1 *= std::cos(x[i]);
        p2 += (x[i] - M_PI)*(x[i] - M_PI);
    }
    return -p1*std::exp(-p2);
}

Disposable<Array> easomValues(const Array& x) {
    Array y(x.size());
    for (Size i = 0; i < x.size(); i++) {
        Real p1 = std::cos(x[i]);
        Real p2 = (x[i] - M_PI)*(x[i] - M_PI);
        y[i] = -p1*std::exp(-p2);
    }
    return y;
}

Real eggholder(const Array& x) {
    //Minimum is found at f(512, 404.2319)
    QL_REQUIRE(x.size() == 2, "Input size needs to be equal to 2");
    Real p = (x[1] + 47.0);
    return -p*std::sin(std::sqrt(std::abs(0.5*x[0] + p))) -
        x[0] * std::sin(std::sqrt(std::abs(x[0] - p)));
}

Real printFunction(Problem& p, const Array& x) {
    std::cout << " f(" << x[0];
    for (Size i = 1; i < x.size(); i++) {
        std::cout << ", " << x[i];
    }
    Real val = p.value(x);
    std::cout << ") = " << val << std::endl;
    return val;
}

class TestFunction : public CostFunction {
public:
    typedef ext::function<Real(const Array&)> RealFunc;
    typedef ext::function<Disposable<Array>(const Array&)> ArrayFunc;
    TestFunction(const RealFunc & f, const ArrayFunc & fs = ArrayFunc()) : f_(f), fs_(fs) {}
    TestFunction(Real(*f)(const Array&), Disposable<Array>(*fs)(const Array&) = NULL) : f_(f), fs_(fs) {}
    virtual ~TestFunction(){}
    virtual Real value(const Array& x) const {
        return f_(x);
    }
    virtual Disposable<Array> values(const Array& x) const {
        if(!fs_)
            throw std::runtime_error("Invalid function");
        return fs_(x);
    }
private:
    RealFunc f_;
    ArrayFunc fs_;
};

int test(OptimizationMethod& method, CostFunction& f, const EndCriteria& endCriteria,
          const Array& start, const Constraint& constraint = Constraint(),
          const Array& optimum = Array()) {
    QL_REQUIRE(start.size() > 0, "Input size needs to be at least 1");
    std::cout << "Starting point: ";
    Constraint c;
    if (!constraint.empty())
        c = constraint;
    Problem p(f, c, start);
    printFunction(p, start);
    method.minimize(p, endCriteria);
    std::cout << "End point: ";
    Real val = printFunction(p, p.currentValue());
    if(!optimum.empty())
    {
        std::cout << "Global optimium: ";
        Real optimVal = printFunction(p, optimum);
        if(std::abs(optimVal) < 1e-13)
            return std::abs(val-optimVal) < 1e-6;
        else
            return std::abs((val-optimVal)/optimVal) < 1e-6;
    }
    return 1;
}

void testFirefly() {
    /*
    The Eggholder function is only in 2 dimensions, it has a multitude
    * of local minima, and they are not symmetric necessarily
    */
    Size n = 2;
    NonhomogeneousBoundaryConstraint constraint(Array(n, -512.0), Array(n, 512.0));
    Array x(n, 0.0);
    Array optimum(n);
    optimum[0] = 512.0;
    optimum[1] = 404.2319;
    Size agents = 150;
    Real vola = 1.5;
    Real intense = 1.0;
    ext::shared_ptr<FireflyAlgorithm::Intensity> intensity =
        ext::make_shared<ExponentialIntensity>(10.0, 1e-8, intense);
    ext::shared_ptr<FireflyAlgorithm::RandomWalk> randomWalk =
        ext::make_shared<LevyFlightWalk>(vola, 0.5, 1.0, seed);
    std::cout << "Function eggholder, Agents: " << agents
            << ", Vola: " << vola << ", Intensity: " << intense << std::endl;
    TestFunction f(eggholder);
    FireflyAlgorithm fa(agents, intensity, randomWalk, 40);
    EndCriteria ec(5000, 1000, 1.0e-8, 1.0e-8, 1.0e-8);
    test(fa, f, ec, x, constraint, optimum);
    std::cout << "================================================================" << std::endl;
}

void testSimulatedAnnealing(Size dimension, Size maxSteps, Size staticSteps){

    /*The ackley function has a large amount of local minima, but the
      structure is symmetric, so if one could simply just ignore the
      walls separating the local minima, it would look like almost
      like a parabola

      Andres Hernandez: I could not find a configuration that was able
      to fix the problem
    */

    //global minimum is at 0.0
    TestFunction f(ackley, ackleyValues);

    //Starting point
    Array x(dimension, 1.5);
    Array optimum(dimension, 0.0);

    //Constraint for local optimizer
    Array lower(dimension, -5.0);
    Array upper(dimension, 5.0);
    NonhomogeneousBoundaryConstraint constraint(lower, upper);

    Real lambda = 0.1;
    Real temperature = 350;
    Real epsilon = 0.99;
    Size ms = 1000;
    std::cout << "Function ackley, Lambda: " << lambda
            << ", Temperature: " << temperature
            << ", Epsilon: " << epsilon
            << ", Iterations: " << ms
            << std::endl;

    MersenneTwisterUniformRng rng(seed);
    SimulatedAnnealing<MersenneTwisterUniformRng> sa(lambda, temperature, epsilon, ms, rng);
    EndCriteria ec(maxSteps, staticSteps, 1.0e-8, 1.0e-8, 1.0e-8);
    test(sa, f, ec, x, constraint, optimum);
    std::cout << "================================================================" << std::endl;
}

void testGaussianSA(Size dimension, Size maxSteps, Size staticSteps, Real initialTemp,
                    Real finalTemp,
                    GaussianSimulatedAnnealing::ResetScheme resetScheme = GaussianSimulatedAnnealing::ResetToBestPoint,
                    Size resetSteps = 150,
                    GaussianSimulatedAnnealing::LocalOptimizeScheme optimizeScheme = GaussianSimulatedAnnealing::EveryBestPoint,
                    ext::shared_ptr<OptimizationMethod> localOptimizer = ext::make_shared<LevenbergMarquardt>()){

    /*The ackley function has a large amount of local minima, but the
     * structure is symmetric, so if one could simply just ignore the
     * walls separating the local minima, it would look like almost like
     * a parabola*/

    //global minimum is at 0.0
    TestFunction f(ackley, ackleyValues);

    std::cout << "Function: ackley, Dimensions: " << dimension
              << ", Initial temp:" << initialTemp
              << ", Final temp:" << finalTemp
              << ", Reset scheme:" << resetScheme
              << ", Reset steps:" << resetSteps
              << std::endl;
    //Starting point
    Array x(dimension, 1.5);
    Array optimum(dimension, 0.0);

    //Constraint for local optimizer
    Array lower(dimension, -5.0);
    Array upper(dimension, 5.0);
    NonhomogeneousBoundaryConstraint constraint(lower, upper);

    //Simulated annealing setup
    SamplerGaussian sampler(seed);
    ProbabilityBoltzmannDownhill probability(seed);
    TemperatureExponential temperature(initialTemp, dimension);
    GaussianSimulatedAnnealing sa(sampler, probability, temperature, ReannealingTrivial(),
                                  initialTemp, finalTemp, 50, resetScheme,
                                  resetSteps, localOptimizer,
                                  optimizeScheme);

    EndCriteria ec(maxSteps, staticSteps, 1.0e-8, 1.0e-8, 1.0e-8);
    test(sa, f, ec, x, constraint, optimum);
    std::cout << "================================================================" << std::endl;
}

void testPSO(Size n){
    /*The Rosenbrock function has a global minima at (1.0, ...) and a local minima at (-1.0, 1.0, ...)
    The difficulty lies in the weird shape of the function*/
    NonhomogeneousBoundaryConstraint constraint(Array(n, -1.0), Array(n, 4.0));
    Array x(n, 0.0);
    Array optimum(n, 1.0);
    Size agents = 100;
    Size kneighbor = 25;
    Size threshold = 500;
    std::cout << "Function: rosenbrock, Dimensions: " << n
            << ", Agents: " << agents << ", K-neighbors: " << kneighbor
            << ", Threshold: " << threshold << std::endl;
    ext::shared_ptr<ParticleSwarmOptimization::Topology> topology =
        ext::make_shared<KNeighbors>(kneighbor);
    ext::shared_ptr<ParticleSwarmOptimization::Inertia> inertia =
        ext::make_shared<LevyFlightInertia>(1.5, threshold, seed);
    TestFunction f(rosenbrock);
    ParticleSwarmOptimization pso(agents, topology, inertia, 2.05, 2.05, seed);
    EndCriteria ec(10000, 1000, 1.0e-8, 1.0e-8, 1.0e-8);
    test(pso, f, ec, x, constraint, optimum);
    std::cout << "================================================================" << std::endl;
}

void testDifferentialEvolution(Size n, Size agents){
    /*The Rosenbrock function has a global minima at (1.0, ...) and a local minima at (-1.0, 1.0, ...)
    The difficulty lies in the weird shape of the function*/
    NonhomogeneousBoundaryConstraint constraint(Array(n, -4.0), Array(n, 4.0));
    Array x(n, 0.0);
    Array optimum(n, 1.0);

    TestFunction f(rosenbrock);

    Real probability = 0.3;
    Real stepsizeWeight = 0.6;
    DifferentialEvolution::Strategy strategy = DifferentialEvolution::BestMemberWithJitter;

    std::cout << "Function: rosenbrock, Dimensions: " << n << ", Agents: " << agents
              << ", Probability: " << probability
              << ", StepsizeWeight: " << stepsizeWeight
              << ", Strategy: BestMemberWithJitter" << std::endl;
    DifferentialEvolution::Configuration config;
    config.withBounds(true)
          .withCrossoverProbability(probability)
          .withPopulationMembers(agents)
          .withStepsizeWeight(stepsizeWeight)
          .withStrategy(strategy)
          .withSeed(seed);

    DifferentialEvolution de(config);
    EndCriteria ec(5000, 1000, 1.0e-8, 1.0e-8, 1.0e-8);
    test(de, f, ec, x, constraint, optimum);
    std::cout << "================================================================" << std::endl;
}

void printTime(double seconds){
    Integer hours = int(seconds/3600);
    seconds -= hours * 3600;
    Integer minutes = int(seconds/60);
    seconds -= minutes * 60;
    std::cout << " \nRun completed in ";
    if (hours > 0)
        std::cout << hours << " h ";
    if (hours > 0 || minutes > 0)
        std::cout << minutes << " m ";
    std::cout << std::fixed << std::setprecision(0)
              << seconds << " s\n" << std::endl;
}

int main(int, char* []) {

    try {
        std::cout << std::endl;
        boost::timer timer;

        std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
        std::cout << "Firefly Algorithm Test" << std::endl;
        std::cout << "----------------------------------------------------------------" << std::endl;
        testFirefly();

        printTime(timer.elapsed());

        std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
        std::cout << "Hybrid Simulated Annealing Test" << std::endl;
        std::cout << "----------------------------------------------------------------" << std::endl;
        testGaussianSA(3, 500, 200, 100.0, 0.1, GaussianSimulatedAnnealing::ResetToBestPoint, 150, GaussianSimulatedAnnealing::EveryNewPoint);
        testGaussianSA(10, 500, 200, 100.0, 0.1, GaussianSimulatedAnnealing::ResetToBestPoint, 150, GaussianSimulatedAnnealing::EveryNewPoint);
        testGaussianSA(30, 500, 200, 100.0, 0.1, GaussianSimulatedAnnealing::ResetToBestPoint, 150, GaussianSimulatedAnnealing::EveryNewPoint);

        printTime(timer.elapsed());

        std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
        std::cout << "Particle Swarm Optimization Test" << std::endl;
        std::cout << "----------------------------------------------------------------" << std::endl;
        testPSO(3);
        testPSO(10);
        testPSO(30);

        printTime(timer.elapsed());

        std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
        std::cout << "Simulated Annealing Test" << std::endl;
        std::cout << "----------------------------------------------------------------" << std::endl;
        testSimulatedAnnealing(3, 10000, 4000);
        testSimulatedAnnealing(10, 10000, 4000);
        testSimulatedAnnealing(30, 10000, 4000);

        printTime(timer.elapsed());

        std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
        std::cout << "Differential Evolution Test" << std::endl;
        std::cout << "----------------------------------------------------------------" << std::endl;
        testDifferentialEvolution(3, 50);
        testDifferentialEvolution(10, 150);
        testDifferentialEvolution(30, 450);

        printTime(timer.elapsed());

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


/**
 * @file calc-characteristic.cpp
 *
 * @brief calculate characteristic polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <inttypes.h>
#include <stdint.h>
#include "dSFMText.hpp"
#include "dSFMT-calc-jump.hpp"
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>

using namespace dsfmt;
using namespace NTL;
using namespace std;

void calc_minimal(GF2X& minimal, dSFMText& dsfmt, int bitpos)
{
    uint64_t mask[2];
    if (bitpos >= 52) {
	mask[0] = 0;
	mask[1] = UINT64_C(1) << (bitpos - 52);
    } else {
	mask[0] = UINT64_C(1) << bitpos;
	mask[1] = 0;
    }
    int maxdegree = dsfmt.get_mamaxdegree();
    vec_GF2 seq;
    seq.SetLength(2 * maxdegree);
    for (int i = 0; i < 2 * maxdegree; i++) {
	seq[i] = dsfmt.next(mask);
    }
    MinPolySeq(minimal, seq, maxdegree);
#if defined(DEBUG)
    cout << "deg(minimal) = " << dec << deg(minimal) << endl;
#endif
}

void LCM(GF2X& lcm, const GF2X& x, const GF2X& y) {
    GF2X gcd;
    mul(lcm, x, y);
    GCD(gcd, x, y);
    lcm /= gcd;
}

void get_lcm(GF2X& lcmpoly, dSFMText& dsfmt) {
    GF2X minimal;
    GF2X tmp;
    int maxdegree = dsfmt.get_mamaxdegree();
    dsfmt.seeding(1234);
//    dsfmt.set_high()
//    dsfmt.seeding(1234, 1);
    for (int bitpos = 0; bitpos < 52 * 2; bitpos++) {
	calc_minimal(minimal, dsfmt, bitpos);
	LCM(tmp, lcmpoly, minimal);
	lcmpoly = tmp;
#if defined(DEBUG)
    cout << "deg(lcm) = " << dec << deg(lcmpoly) << endl;
#endif
#if 0
	if (deg(lcmpoly) == maxdegree) {
	    return;
	}
	if (deg(lcmpoly) > maxdegree) {
	    return;
	}
#endif
    }
#if defined(DEBUG)
    cout << "deg(lcm) = " << dec << deg(lcmpoly) << endl;
#endif
#if 1
    for(int i = 0; i < maxdegree; i++) {
	dsfmt.init_basis();
	for (int bitpos = 0; bitpos < 52 * 2; bitpos++) {
	    calc_minimal(minimal, dsfmt, bitpos);
	    LCM(tmp, lcmpoly, minimal);
	    lcmpoly = tmp;
#if 0
	    if (deg(lcmpoly) == maxdegree) {
		return;
	    }
#endif
	}
    }
#endif
#if 0
    cerr << "can't find lcm deg=" << dec << deg(lcmpoly) << endl;
    cerr << "maxdegree = " << dec << maxdegree << endl;
    throw new logic_error("can't find lcm");
#endif
}

static int has_large_irreducible(GF2X& fpoly, int degree) {
    static const GF2X t2(2, 1);
    static const GF2X t1(1, 1);
    GF2X t2m;
    GF2X t;
    GF2X alpha;
    int m;

    t2m = t2;
    if (deg(fpoly) < degree) {
	return 0;
    }
    t = t1;
    t += t2m;

    for (m = 1; deg(fpoly) > degree; m++) {
	for(;;) {
	    GCD(alpha, fpoly, t);
	    if (IsOne(alpha)) {
		break;
	    }
	    fpoly /= alpha;
	    if (deg(fpoly) < degree) {
		return 0;
	    }
	}
	t2m *= t2m;
	t2m %= fpoly;
	add(t, t2m, t1);
    }
    if (deg(fpoly) != degree) {
	return 0;
    }
    return IterIrredTest(fpoly);
}

void check_fix(dSFMText & fix)
{
    fix.setup_high();
    cout << "before:";
    fix.print(cout);
    fix.next();
    cout << "after 1 step:";
    fix.print(cout);
    fix.next();
    cout << "after 2 step:";
    fix.print(cout);
    fix.next();
    cout << "after 3 step:";
    fix.print(cout);
}

void check_fix2(dSFMText & fix)
{
    cout << "before:";
    fix.print(cout);
    fix.next_add();
    cout << "after 1 step:";
    fix.print(cout);
    fix.next_add();
    cout << "after 2 step:";
    fix.print(cout);
    fix.next_add();
    cout << "after 3 step:";
    fix.print(cout);
}

void calc_fix(GF2X& lcm, int mexp, int pos1, int sl1,
	      uint64_t mask1, uint64_t mask2)
{
    dSFMText fix(mexp, pos1, sl1, mask1, mask2);
    //fix.setup_high();
    //fix.next();
    //cout << "setup0:";
    //fix.print(cout);
    fix.setup_constants();
    cout << "setup:";
    fix.print(cout);
    dSFMText work(mexp, pos1, sl1, mask1, mask2);
    cout << "zero:";
    work.print(cout);
    for (int i = 0; i <= deg(lcm); i++) {
	if (IsOne(coeff(lcm, i))) {
	    work.add(fix);
#if defined(DEBUG) && 0
    cout << "work:";
    work.print(cout);
#endif
	}
	fix.next();
    }
    cout << "fix:";
    work.print(cout);
    check_fix2(work);
#if 0
    cout << "* (t+1)" << endl;
    GF2X t1(1,1);
    dSFMText work2(mexp, pos1, sl1, mask1, mask2);
    cout << "work2:";
    work2.print(cout);
    for (int i = 0; i <= deg(t1); i++) {
	if (IsOne(coeff(t1, i))) {
	    work2.add(work);
#if defined(DEBUG)
    cout << "work2:";
    work2.print(cout);
#endif
	}
	work.next();
    }
    cout << "work2:";
    work2.print(cout);
#endif
}

void check_const(GF2X& lcm, int mexp, int pos1, int sl1,
	      uint64_t mask1, uint64_t mask2)
{
    dSFMText normal(mexp, pos1, sl1, mask1, mask2);
    dSFMText add(mexp, pos1, sl1, mask1, mask2);
    normal.seeding(123);
    add.seeding(123);

    normal.setup_high();
    normal.next();
    add.next_add();
    cout << "normal:";
    normal.print(cout);
    cout << "add:";
    add.print(cout);
    for (int i = 0; i < 5; i++) {
	normal.next();
	add.next_add();
    }
    cout << "normal:";
    normal.print(cout);
    cout << "add:";
    add.print(cout);
}

int main(int argc, char *argv[]) {
    if (argc < 6) {
	cout << argv[0] << " mexp pos1 sl1 mask1 mask2" << endl;
	return -1;
    }
    int mexp = strtol(argv[1], NULL, 10);
    int pos1 = strtol(argv[2], NULL, 10);
    int sl1 = strtol(argv[3], NULL, 10);
    uint64_t mask[2];
    mask[0] = strtoull(argv[4], NULL, 16);
    mask[1] = strtoull(argv[5], NULL, 16);
#if defined(DEBUG)
    cout << "mexp:" << dec << mexp << endl;
    cout << "pos1:" << dec << pos1 << endl;
    cout << "sl1:" << dec << sl1 << endl;
    cout << "mask1:" << hex<< mask[0] << endl;
    cout << "mask2:" << hex << mask[1] << endl;
#endif
    dSFMText dsfmt(mexp, pos1, sl1, mask[0], mask[1]);
    GF2X characteristic(0,1);
    get_lcm(characteristic, dsfmt);
#if defined(DEBUG)
    cout << "degree:" << dec << deg(characteristic) << endl;
    cerr << "maxdegree = " << dec << dsfmt.get_mamaxdegree() << endl;
    cout << characteristic << endl;
#endif
    GF2X work;
    work = characteristic;
#if defined(DEBUG)
    cout << "degree:" << deg(characteristic) << endl;
    cout << characteristic << endl;
#endif
    if (!has_large_irreducible(characteristic, mexp)) {
        cout << "error?" << endl;
        return -1;
    }
#if 0
    GF2X remain = work / characteristic;
    vec_pair_GF2X_long factors;
    CanZass(factors, remain);
    cout << "=== factor of remain ===" << endl;
    for (int i = 0; i < factors.length(); i++) {
	cout << factors[i].a;
	cout << ":";
	cout << factors[i].b << endl;
    }
    cout << "=== factor of remain ===" << endl;
#endif
    GF2X d, s, inv;
    GF2X t1(1,1);
    XGCD(d, s, inv, work, t1);
// d = gcd(a,b), a s + b t = d
    MulMod(s, inv, t1, work);
    if (deg(s) != 0) {
	cout << "inv is not inv." << endl;
    }

    string x;
//    cout << "# deg = " << dec << deg(work) << endl;
    polytostring(x, work);
    cout << "#" << dec << mexp;
    cout << "," << dec << pos1;
    cout << "," << dec << sl1;
    cout << "," << hex << mask[0];
    cout << "," << hex << mask[1];
    cout << dec << endl;
    cout << x << endl;
    cout << dec << flush;
#if 0
    string y;
    polytostring(y, inv);
    if (deg(d) == 0) {
	cout << y << endl;
    } else {
	cout << "can't finf inv d=" << d << endl;
    }
    calc_fix(inv, mexp, pos1, sl1, mask[0], mask[1]);
    //check_const(inv, mexp, pos1, sl1, mask[0], mask[1]);
#endif
}


#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <strstream>
#include <fstream>
#include <set>
#include <map>
using namespace Eigen;

MatrixXd V;
MatrixXi F;
std::vector<std::vector<int> > IrregularF;

void Upsampling()
{
	std::map<std::pair<int, int>, int> edgeid;
	for (int i = 0; i < F.cols(); ++i) {
		for (int j = 0; j < 4; ++j) {
			int v1 = F(j, i);
			int v2 = F((j + 1) % 4, i);
			if (v1 > v2)
				std::swap(v1, v2);
			auto key = std::make_pair(v1, v2);
			int s = edgeid.size();
			if (edgeid.count(key) == 0)
				edgeid[key] = s;
		}
	}
	std::vector<Vector4i> faces(F.cols() * 4);
	std::vector<Vector3d> vertices(V.cols() + edgeid.size() + F.cols());
	for (int i = 0; i < V.cols(); ++i) {
		vertices[i] = V.col(i);
	}
	for (auto info : edgeid)
		vertices[V.cols() + info.second] = 0.5 * (V.col(info.first.first) + V.col(info.first.second));
	for (int i = 0; i < F.cols(); ++i) {
		Vector3d p(0, 0, 0);
		for (int j = 0; j < 4; ++j) {
			p += V.col(F(j, i));
		}
		p *= 0.25;
		vertices[V.cols() + edgeid.size() + i] = p;
	}
	for (int i = 0; i < F.cols(); ++i) {
		int eid[4];
		for (int j = 0; j < 4; ++j) {
			int v1 = F(j, i);
			int v2 = F((j + 1) % 4, i);
			if (v1 > v2)
				std::swap(v1, v2);
			auto key = std::make_pair(v1, v2);
			if (edgeid.count(key) == 0) {
				printf("OMG!\n");
			}
			eid[j] = edgeid[key];
		}
		for (int j = 0; j < 4; ++j) {
			faces[i * 4 + j] =
				Vector4i(F(j, i), eid[j] + V.cols(), V.cols() + edgeid.size() + i, eid[(j + 3) % 4] + V.cols());
		}
	}
	V.resize(3, vertices.size());
	memcpy(V.data(), vertices.data(), sizeof(double) * 3 * vertices.size());
	F.resize(4, faces.size());
	memcpy(F.data(), faces.data(), sizeof(int) * 4 * faces.size());
}
void Load(const char* filename) {
	std::vector<Vector3d> positions;
	std::vector<Vector4i> faces;
	std::ifstream is(filename);
	char buffer[2048];
	while (is.getline(buffer, 2048)) {
		std::strstream str;
		str << buffer;
		str >> buffer;
		if (strcmp(buffer, "v") == 0) {
			double x, y, z;
			str >> x >> y >> z;
			positions.push_back(Vector3d(x, y, z));
		}
		else if (strcmp(buffer, "f") == 0) {
			std::vector<int> face;
			while (str >> buffer) {
				std::strstream fstr;
				fstr << buffer;
				int f;
				fstr >> f;
				face.push_back(f-1);
			}
			if (face.size() == 4) {
				Vector4i f(face[0], face[1], face[2], face[3]);
				faces.push_back(f);
			} else {
				IrregularF.push_back(face);
			}
		}
	}
	V.resize(3, positions.size());
	F.resize(4, faces.size());
	memcpy(F.data(), faces.data(), sizeof(int) * 4 * faces.size());

	std::vector<std::set<int> > VF(positions.size());
	for (int i = 0; i < F.cols(); ++i) {
		for (int j = 0; j < 4; ++j) {
			int x = F(j, i);
			int y = F((j + 1) % 4, i);
			VF[x].insert(y);
			VF[y].insert(x);
		}
	}
	std::vector<Vector3d> new_positions(positions.size());
	for (int i = 0; i < positions.size(); ++i) {
		new_positions[i] = Eigen::Vector3d(0, 0, 0);
	}
	for (int i = 0; i < VF.size(); ++i) {
		for (auto& ind : VF[i]) {
			new_positions[i] += positions[ind];
		}
		new_positions[i] /= VF[i].size();
	}

	memcpy(V.data(), new_positions.data(), sizeof(double) * 3 * positions.size());
}

void ReportFaceInfo()
{
	printf("Vertex: %d      Regular Faes: %d       Irregular faces %d\n", V.cols(), F.cols(), IrregularF.size());
}

void ReportManifold()
{
	std::set<std::pair<int, int> > dedges;
	bool isManifold = true;
	for (int i = 0; i < F.cols(); ++i) {
		for (int j = 0; j < 4; ++j) {
			int v1 = F(j, i);
			int v2 = F((j + 1) % 4, i);
			auto key = std::make_pair(v1, v2);
			if (dedges.count(key)) {
				isManifold = false;
			} else {
				dedges.insert(key);
			}
		}
	}
	for (int i = 0; i < IrregularF.size(); ++i) {
		for (int j = 0; j < IrregularF[i].size(); ++j) {
			int v1 = IrregularF[i][j];
			int v2 = IrregularF[i][(j + 1) % IrregularF[i].size()];
			auto key = std::make_pair(v1, v2);
			if (dedges.count(key)) {
				isManifold = false;
			} else {
				dedges.insert(key);
			}
		}
	}
	if (isManifold) {
		printf("Is Manifold: True.\n");
	} else {
		printf("Is Manifold: False.\n");
	}
}

void ReportSingularity()
{
	std::vector<std::set<int> > links(V.cols());
	for (int i = 0; i < F.cols(); ++i) {
		for (int j = 0; j < 4; ++j) {
			int v1 = F(j, i);
			int v2 = F((j + 1) % 4, i);
			links[v1].insert(v2);
			links[v2].insert(v1);
		}
	}
	for (int i = 0; i < IrregularF.size(); ++i) {
		for (int j = 0; j < IrregularF[i].size(); ++j) {
			int v1 = IrregularF[i][j];
			int v2 = IrregularF[i][(j + 1) % IrregularF[i].size()];
			links[v1].insert(v2);
			links[v2].insert(v1);
		}
	}
	std::map<int, int> valences;
	for (int i = 0; i < links.size(); ++i) {
		int v = links[i].size();
		if (v != 0) {
			if (v > 5) {
				//printf("Weird %d\n", i);
			}
			if (valences.count(v)) {
				valences[v] += 1;
			} else {
				valences[v] = 1;
			}
		}
	}
	for (auto& p : valences) {
		printf("Valence %d: %d\n", p.first, p.second);
	}
}

void ReportAngleDifference()
{
	double e = 0, max_angle = 0, min_angle = 360;
	for (int i = 0; i < F.cols(); ++i) {
		for (int j = 0; j < 4; ++j) {
			int v0 = F(j, i);
			int v1 = F((j+1)%4,i);
			int v2 = F((j+3)%4,i);
			Vector3d d1 = V.col(v1) - V.col(v0);
			Vector3d d2 = V.col(v2) - V.col(v0);
			d1.normalize();
			d2.normalize();
			double angle = 180.0/3.141592654*atan2(d1.cross(d2).norm(), d1.dot(d2));
			e += (angle-90)*(angle-90);
			max_angle = std::max(angle, max_angle);
			min_angle = std::min(angle, min_angle);
		}
	}
	printf("min max angle: %lf %lf\n", min_angle, max_angle);
	printf("angle average error: %lf\n", sqrt(e / 4 / F.cols()));
	std::vector<double> len;
	double sum_area = 0;
	for (int i = 0; i < F.cols(); ++i) {
		Eigen::Vector3d a1 = V.col(F(1, i)) - V.col(F(0, i));
		Eigen::Vector3d a2 = V.col(F(3, i)) - V.col(F(0, i));
		Eigen::Vector3d a3 = V.col(F(1, i)) - V.col(F(2, i));
		Eigen::Vector3d a4 = V.col(F(3, i)) - V.col(F(2, i));
		double t1 = a1.cross(a2).norm();
		double t2 = a3.cross(a4).norm();
		len.push_back(t1 + t2);
		sum_area += t1 + t2;
	}
	double med_area = sum_area / F.cols();
	for (int i = 0; i < len.size(); ++i) {
		len[i] = (len[i] - med_area) / med_area;
	}
	double t = 0;
	for (int i = 0; i < len.size(); ++i) {
		t += len[i] * len[i];
	}
	printf("area %lf\n", sqrt(t / len.size()));
}

void Analyze()
{
	ReportFaceInfo();
	ReportManifold();
	ReportSingularity();
	ReportAngleDifference();
}

int main(int argc, char** argv) {
	if (argc < 2) {
		printf("./analyzer input.obj [scale] [output.txt]\n");
		return 0;
	}

	Load(argv[1]);

	int scale = 1;
	if (argc >= 3) {
		sscanf(argv[2], "%d", &scale);
	}
	for (int i = 1; i < scale; ++i) {
		Upsampling();
	}
	if (argc >= 4) {
		freopen(argv[3],"w",stdout);
	}

	Analyze();

	fclose(stdout);
}


#include <iostream>
#include <algorithm>
#include <vector>
#include <queue>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/tuple/tuple.hpp>

typedef  boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
  boost::property<boost::edge_capacity_t, long,
    boost::property<boost::edge_residual_capacity_t, long,
      boost::property<boost::edge_reverse_t, traits::edge_descriptor> > > >  graph;
typedef  boost::graph_traits<graph>::edge_descriptor      edge_desc;
typedef  boost::graph_traits<graph>::out_edge_iterator      out_edge_it;

class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

using namespace std;

// Modeling it as a flow problem.
// The source connects to all vertices with a positive balance, all other vertices connect to the sink.
// Since an unions total value needs to be strictly greater than zero, we know there needs to be at
// least one province that does not need its full balance to support it. I no such province exists,
// there can not be an union. If there is a province that does not need its full balance, the flow is
// less than the sum of positive balances
void solve() {
  int n, m;
  cin >> n >> m;

  // Set up the graph
  graph G(n);
  edge_adder adder(G);
  auto rc_map = boost::get(boost::edge_residual_capacity, G);

  auto source = boost::add_vertex(G);
  auto target = boost::add_vertex(G);

  // Connect all provinces to source/target depending on balance
  int sumPositive = 0;
  int b;
  for (int i = 0; i < n; ++i) {
    cin >> b;
    if (b > 0) {
      sumPositive += b;
      adder.add_edge(source, i, b);
    }
    else {
      adder.add_edge(i, target, -b);
    }
  }

  // Connect provinces
  int u, v, d;
  for (int i = 0; i < m; ++i) {
    cin >> u >> v >> d;
    adder.add_edge(u, v, d);
  }

  // Compute flow & output
  int flow = boost::push_relabel_max_flow(G, source, target);
  cout << (flow < sumPositive ? "yes" : "no") << endl;
}

int main() {
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  for (int i = 0; i < t; ++i) {
    solve();
  }
  return 0;
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/core/exceptions.h>
#include <mrpt/core/lock_helper.h>
#include <mrpt/gui/CDisplayWindowGUI.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/Lie/SO.h>

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

static void AppRotationConverter()
{
	nanogui::init();

	// Create main window:
	mrpt::gui::CDisplayWindowGUI_Params cp;
	// cp.fullscreen = true;

	// Input variables (they are bound to the GUI controls):
	mrpt::math::CQuaternionDouble in_quat;
	mrpt::math::CMatrixFixed<double, 3, 3> in_rot;
	in_rot.setIdentity();
	std::array<double, 3> in_ypr = {0, 0, 0};
	mrpt::math::TVector3D in_axisangle_ax(0, 0, 0);
	double in_axisangle_ang = 0;
	auto in_lie_log = mrpt::math::CVectorFixed<double, 3>::Zero();
	bool units_radians = true;

	mrpt::opengl::CSetOfObjects::Ptr gl_corner_user =
		mrpt::opengl::stock_objects::CornerXYZ(1.0f);
	mrpt::opengl::CAxis::Ptr gl_corner_reference = mrpt::opengl::CAxis::Create(
		-1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 0.2f, 1.0f, true);
	gl_corner_reference->setTextScale(0.04);

	// In/out UI control declarations (declared here so we can use them in the
	// lambda below):
	nanogui::TabWidget* tabWidget = nullptr;
	nanogui::TextBox* ed_in_rot[3][3];
	nanogui::TextBox *edOutMatrix = nullptr, *edOutQuat = nullptr,
					 *edOutAxisAngle_Ax = nullptr, *edOutAxisAngle_An = nullptr,
					 *edOutLogSO3 = nullptr;
	nanogui::TextBox* ed_out_rot[3][3];
	nanogui::Slider* sl_in_ypr[3] = {nullptr, nullptr, nullptr};

	// The main function: update all calculations:
	auto lambdaRecalcAll = [&]() {
		mrpt::poses::CPose3D userPose;
		switch (tabWidget->activeTab())
		{
			// YPR
			case 0:
			{
				const double K = units_radians ? 1.0 : (M_PI / 180.0);
				userPose.setFromValues(
					0, 0, 0, in_ypr[0] * K, in_ypr[1] * K, in_ypr[2] * K);
			}
			break;
			// Rot matrix:
			case 1:
			{
				// Run a SVD to ensure we take the closest SO(3) matrix to the
				// user input:
				Eigen::Matrix3d M = in_rot.asEigen();
				Eigen::JacobiSVD<Eigen::Matrix3d> svd(
					M, Eigen::ComputeFullU | Eigen::ComputeFullV);

				auto R = mrpt::math::CMatrixDouble33(
					(svd.matrixU() * svd.matrixV().transpose()).eval());
				userPose.setRotationMatrix(R);
				for (int r = 0; r < 3; r++)
					for (int c = 0; c < 3; c++)
						ed_in_rot[r][c]->setValue(
							mrpt::format("%.05f", R(r, c)));
			}
			break;
			// Quaternion
			case 2:
			{
				userPose = mrpt::poses::CPose3D(in_quat, 0, 0, 0);
			}
			break;
			// axis+angle
			case 3:
			{
				const double K = units_radians ? 1.0 : (M_PI / 180.0);
				mrpt::math::TVector3D v =
					in_axisangle_ax * (in_axisangle_ang * K);
				mrpt::math::CVectorFixed<double, 3> vn;
				for (int i = 0; i < 3; i++)
					vn[i] = v[i];

				userPose.setRotationMatrix(mrpt::poses::Lie::SO<3>::exp(vn));
			}
			break;
			// log(SO(3))
			case 4:
			{
				userPose.setRotationMatrix(
					mrpt::poses::Lie::SO<3>::exp(in_lie_log));
			}
			break;
		};

		// Set 3D view corner:
		gl_corner_user->setPose(userPose);

		// Update text output:
		const mrpt::math::CMatrixDouble33 Rout = userPose.getRotationMatrix();

		// matrix:
		edOutMatrix->setValue(Rout.inMatlabFormat());
		for (int r = 0; r < 3; r++)
			for (int c = 0; c < 3; c++)
				ed_out_rot[r][c]->setValue(mrpt::format("%.05f", Rout(r, c)));

		// quat:
		mrpt::math::CQuaternionDouble q;
		userPose.getAsQuaternion(q);
		edOutQuat->setValue(q.asString());

		// SO(3) log:
		const auto log_R = mrpt::poses::Lie::SO<3>::log(Rout);
		edOutLogSO3->setValue(
			mrpt::format("[%.05f %.05f %.05f]", log_R[0], log_R[1], log_R[2]));

		// axis-angle:
		mrpt::math::TVector3D axis(log_R[0], log_R[1], log_R[2]);
		if (axis.norm() > 1e-20) axis = axis.unitarize();

		edOutAxisAngle_Ax->setValue(
			mrpt::format("[%.05f %.05f %.05f]", axis[0], axis[1], axis[2]));
		edOutAxisAngle_An->setValue(mrpt::format(
			"%.05f %s", log_R.norm() * (units_radians ? 1.0 : 180.0 / M_PI),
			(units_radians ? "rad" : "deg")));
	};

	// Create GUI:
	mrpt::gui::CDisplayWindowGUI win("3D rotation converter", 900, 700, cp);

	// Add INPUT window:
	// -----------------------------
	nanogui::Window* winInput = new nanogui::Window(&win, "Rotation input");
	winInput->setPosition(nanogui::Vector2i(10, 50));
	winInput->setLayout(new nanogui::GroupLayout());
	winInput->setFixedWidth(350);

	tabWidget = winInput->add<nanogui::TabWidget>();

	{  // Yaw/pitch/roll  tab:
		nanogui::Widget* layer = tabWidget->createTab("Yaw-pitch-roll");
		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		const char* lb[3] = {"yaw (Z)=", "pitch (Y)=", "roll (x)="};

		for (int i = 0; i < 3; i++)
		{
			layer->add<nanogui::Label>(lb[i]);
			nanogui::TextBox* ed =
				layer->add<nanogui::TextBox>(mrpt::format("%.2f", in_quat[i]));
			ed->setEditable(true);
			ed->setFormat("[-+]?[0-9.e+-]*");
			ed->setCallback([&in_ypr, i, lambdaRecalcAll, &sl_in_ypr,
							 &units_radians](const std::string& s) {
				in_ypr[i] = std::stod(s);
				sl_in_ypr[i]->setValue(
					in_ypr[i] / (units_radians ? M_PIf : 180.0f));
				lambdaRecalcAll();
				return true;
			});
			nanogui::Slider* sl = layer->add<nanogui::Slider>();
			sl_in_ypr[i] = sl;
			sl->setRange({-1.0f, 1.0f});
			sl->setCallback([&, i, ed](float val) {
				val *= units_radians ? M_PIf : 180.0f;
				if (i == 1) val *= 0.5f;  // Pitch
				ed->setValue(mrpt::format("%.03f", val));
				ed->callback()(ed->value());
			});
		}
	}

	{  // rotation matrix tab:
		nanogui::Widget* layer = tabWidget->createTab("SO(3) matrix");

		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		for (int r = 0; r < 3; r++)
		{
			for (int c = 0; c < 3; c++)
			{
				nanogui::TextBox* ed = layer->add<nanogui::TextBox>(
					mrpt::format("%.04f", in_rot(r, c)));
				ed->setEditable(true);
				ed->setFormat("[-+]?[0-9.e+-]*");
				ed->setCallback([&in_rot, r, c](const std::string& s) {
					in_rot(r, c) = std::stod(s);
					return true;
				});
				ed_in_rot[r][c] = ed;
			}
		}
	}

	{  // Quaternion tab:
		nanogui::Widget* layer = tabWidget->createTab("Quaternion");
		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		const char* lb[4] = {"w (real)=", "x=", "y=", "z="};

		for (int i = 0; i < 4; i++)
		{
			layer->add<nanogui::Label>(lb[i]);

			nanogui::TextBox* ed =
				layer->add<nanogui::TextBox>(mrpt::format("%.2f", in_quat[i]));
			ed->setEditable(true);
			ed->setFormat("[-+]?[0-9.e+-]*");
			ed->setCallback([&in_quat, i](const std::string& s) {
				in_quat[i] = std::stod(s);
				return true;
			});
		}
	}

	{  // Axis-angle tab:
		nanogui::Widget* layer = tabWidget->createTab("Axis-angle");
		layer->setLayout(new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5,
			0));
		layer->add<nanogui::Label>("Axis:");

		{
			nanogui::Widget* panel = layer->add<nanogui::Widget>();
			panel->setLayout(new nanogui::GridLayout(
				nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill,
				5, 0));

			for (int r = 0; r < 3; r++)
			{
				nanogui::TextBox* ed = panel->add<nanogui::TextBox>(
					mrpt::format("%.04f", in_axisangle_ax[r]));
				ed->setEditable(true);
				ed->setFormat("[-+]?[0-9.e+-]*");
				ed->setCallback([&in_axisangle_ax, r](const std::string& s) {
					in_axisangle_ax[r] = std::stod(s);
					return true;
				});
			}
		}
		{
			layer->add<nanogui::Label>("Angle:");
			nanogui::TextBox* ed = layer->add<nanogui::TextBox>(
				mrpt::format("%.04f", in_axisangle_ang));
			ed->setEditable(true);
			ed->setFormat("[-+]?[0-9.e+-]*");
			ed->setCallback([&in_axisangle_ang](const std::string& s) {
				in_axisangle_ang = std::stod(s);
				return true;
			});
		}
	}

	{  // axis with magnitude = log(R) in Lie group
		nanogui::Widget* layer = tabWidget->createTab("log(SO(3))");

		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 1, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		layer->add<nanogui::Label>("Axis with magnitude", "sans-bold");
		layer->add<nanogui::Label>("(i.e. vee(log(R)) in SO(3))");
		{
			nanogui::Widget* panel = layer->add<nanogui::Widget>();
			panel->setLayout(new nanogui::GridLayout(
				nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill,
				5, 0));

			for (int r = 0; r < 3; r++)
			{
				nanogui::TextBox* ed = layer->add<nanogui::TextBox>(
					mrpt::format("%.04f", in_lie_log[r]));
				ed->setEditable(true);
				ed->setFormat("[-+]?[0-9.e+-]*");
				ed->setCallback([&in_lie_log, r](const std::string& s) {
					in_lie_log[r] = std::stod(s);
					return true;
				});
			}
		}
	}

	tabWidget->setActiveTab(0);

	// Apply button:
	winInput->add<nanogui::Button>("Apply", ENTYPO_ICON_CALCULATOR)
		->setCallback(lambdaRecalcAll);

	// Add top menu subwindow:
	// -----------------------------
	{
		nanogui::ref<nanogui::Window> winMenu = new nanogui::Window(&win, "");
		winMenu->setPosition(nanogui::Vector2i(0, 0));
		winMenu->setLayout(new nanogui::BoxLayout(
			nanogui::Orientation::Horizontal, nanogui::Alignment::Middle, 5));
		nanogui::Theme* modTheme =
			new nanogui::Theme(win.screen()->nvgContext());
		modTheme->mWindowHeaderHeight = 1;
		winMenu->setTheme(modTheme);

		winMenu->add<nanogui::Button>("Quit", ENTYPO_ICON_ARROW_BOLD_LEFT)
			->setCallback([&win]() { win.setVisible(false); });

		winMenu->add<nanogui::Label>("      ");	 // separator

		winMenu
			->add<nanogui::CheckBox>(
				"Show reference frame",
				[&](bool b) { gl_corner_reference->setVisibility(b); })
			->setChecked(true);

		winMenu
			->add<nanogui::CheckBox>(
				"Show rotated frame",
				[&](bool b) { gl_corner_user->setVisibility(b); })
			->setChecked(true);
		winMenu
			->add<nanogui::CheckBox>(
				"Ortho. view",
				[&](bool b) { win.camera().setCameraProjective(!b); })
			->setChecked(false);

		winMenu->add<nanogui::Label>("Units:");
		winMenu
			->add<nanogui::ComboBox>(
				std::vector<std::string>({"radians", "degrees"}))
			->setCallback([&](int index) {
				// On units update:
				units_radians = (index == 0);
				lambdaRecalcAll();
			});
	}

	// Add OUTPUT window:
	// -----------------------------
	{
		nanogui::Window* winOutput =
			new nanogui::Window(&win, "Rotation output");
		winOutput->setPosition(nanogui::Vector2i(10, 320));
		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 1, nanogui::Alignment::Fill, 5,
			0);
		winOutput->setLayout(layout);
		winOutput->setFixedWidth(350);

		winOutput->add<nanogui::Label>("SO(3) rotation matrix", "sans-bold");
		{
			nanogui::Widget* panel = winOutput->add<nanogui::Widget>();
			panel->setLayout(new nanogui::GridLayout(
				nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill,
				5, 0));
			for (int r = 0; r < 3; r++)
				for (int c = 0; c < 3; c++)
					ed_out_rot[r][c] = panel->add<nanogui::TextBox>("");
		}

		winOutput->add<nanogui::Label>("In MATLAB-like notation:");
		edOutMatrix = winOutput->add<nanogui::TextBox>("");
		edOutMatrix->setEditable(true);

		winOutput->add<nanogui::Label>("Quaternion (r,x,y,z)", "sans-bold");
		edOutQuat = winOutput->add<nanogui::TextBox>("");
		edOutQuat->setEditable(true);

		winOutput->add<nanogui::Label>("Axis-angle (r,x,y,z)", "sans-bold");
		edOutAxisAngle_Ax = winOutput->add<nanogui::TextBox>("");
		edOutAxisAngle_Ax->setEditable(true);
		edOutAxisAngle_An = winOutput->add<nanogui::TextBox>("");
		edOutAxisAngle_An->setEditable(true);

		winOutput->add<nanogui::Label>("Axis with angle (log(R))", "sans-bold");
		edOutLogSO3 = winOutput->add<nanogui::TextBox>("");
		edOutLogSO3->setEditable(true);
	}

	// Add a background scene:
	// -----------------------------
	{
		auto scene = mrpt::opengl::COpenGLScene::Create();
		scene->insert(mrpt::opengl::CGridPlaneXY::Create());

		scene->insert(gl_corner_user);
		scene->insert(gl_corner_reference);

		auto lck = mrpt::lockHelper(win.background_scene_mtx);
		win.background_scene = std::move(scene);
	}

	win.performLayout();

	win.camera().setZoomDistance(5.0f);

	// Update view and process events:
	win.drawAll();
	win.setVisible(true);
	nanogui::mainloop();

	nanogui::shutdown();
}

int main()
{
	try
	{
		AppRotationConverter();
		return 0;
	}
	catch (const std::exception& e)
	{
		std::cerr << mrpt::exception_to_str(e) << std::endl;
		return -1;
	}
}