#include <Eigen/Dense>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkDelaunay2D.h>
#include <vtkDoubleArray.h>
#include <vtkSmartPointer.h>
#include <vtkIdFilter.h>
#include <vtkPointData.h>

typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

int main(){
    clock_t t1;
    t1 = clock();
    for(auto i=0; i < 10000; ++i){
        vtkNew<vtkPolyDataReader> reader;
        reader->SetFileName("T7.vtk");
        reader->Update();
        auto poly = reader->GetOutput();
        auto N = poly->GetNumberOfPoints();
        auto pts = (double*) poly->GetPoints()->GetData()->GetVoidPointer(0);
        Map3Xd points(pts,3,N);

        // Project points to unit sphere
        points.colwise().normalize();

        // Reset the center of the sphere to origin by translating
        Vector3d center = points.rowwise().mean();
        points = points.colwise() - center;

        // Rotate all points so that the point in 0th column is along z-axis
        Vector3d c = points.col(0);
        double_t cos_t = c(2);
        double_t sin_t = std::sqrt( 1 - cos_t*cos_t );
        Vector3d axis;
        axis << c(1), -c(0), 0.;
        Matrix3d rotMat, axis_cross, outer;
        axis_cross << 0. , -axis(2), axis(1),
                        axis(2), 0., -axis(0),
                        -axis(1), axis(0), 0.;

        outer.noalias() = axis*axis.transpose();

        rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross + (1-cos_t)*outer;
        Matrix3Xd rPts(3,N);
        rPts = rotMat*points; // The points on a sphere rotated

        // Calculate the stereographic projections
        Vector3d p0;
        Map3Xd l0( &(rPts(0,1)), 3, N-1 );
        Matrix3Xd l(3,N-1), proj(3,N-1);
        p0 << 0,0,-1;
        c = rPts.col(0);
        l = (l0.colwise() - c).colwise().normalized();
        for( auto j=0; j < N-1; ++j ){
            proj.col(j) = ((p0(2) - l0(2,j))/l(2,j))*l.col(j) + l0.col(j);
            proj(j,2) = 0.0;
        }
        // Calculate the 2d delaunay triangulations
        vtkNew<vtkDoubleArray> pts2dArr;
        pts2dArr->SetVoidArray((void*)proj.data(), 3*(N-1), 1);
        pts2dArr->SetNumberOfComponents(3);
        vtkNew<vtkPoints> pts2d;
        pts2d->SetData(pts2dArr);
        vtkNew<vtkPolyData> poly2d;
        poly2d->SetPoints(pts2d);
        vtkNew<vtkIdFilter> idf;
        idf->PointIdsOn();
        idf->SetIdsArrayName("OrigIds");
        idf->SetInputData(poly2d);
        vtkNew<vtkDelaunay2D> d2d;
        d2d->SetInputConnection(idf->GetOutputPort());
        d2d->Update();

        // Write the triangulation to file
        vtkNew<vtkCellArray> final;
        auto stereoTris = d2d->GetOutput()->GetPolys();
        auto idArr = d2d->GetOutput()->GetPointData()->GetArray("OrigIds");
        vtkNew<vtkIdList> idL;
        stereoTris->InitTraversal();
        while( stereoTris->GetNextCell(idL) ){
            final->InsertNextCell(3);
            for(auto j=0; j < idL->GetNumberOfIds(); ++j)
                final->InsertCellPoint( int(
                                        idArr->GetTuple1(idL->GetId(j))) );
        }

    }
    float diff((float)clock() - (float)t1);
    std::cout << "Time elapsed : " << diff / CLOCKS_PER_SEC
              << " seconds" << std::endl;
    return 0;
}


#include <iostream>
#include <vector>
#include <fstream>
#include <string> // for string class
#include <math.h>
#include <iostream>
#include <cmath>
#include <array>
#include <complex>
#include<time.h>
#include "mex.h"

//#define EIGEN_USE_MKL_ALL
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <unsupported/Eigen/CXX11/Tensor>
#include <Eigen/LU>
#include <unsupported/Eigen/KroneckerProduct>
#include <Eigen/Core>
#include <Eigen/SVD>
#include<unsupported/Eigen/SparseExtra>

using namespace Eigen;
using complex_sparse_matrix = Eigen::SparseMatrix<std::complex<double>>;
using complex_vector = Eigen::VectorXcd;
using complex_matrix = Eigen::MatrixXcd;

#define PI acos(-1.0)

//*******************Changing***************************
//Without damping matrix
//full geometry, no symmetric boundarycondition
//********************************************************

//#include "mkl_lapacke.h"
//#include "lapack.h"

//typedef size_t INT;
//#define MKL_INT INT
//#ifndef lapack_int
//#define lapack_int MKL_INT
//#endif
//written by Chau Nguyen Khanh
#define MIN(a, b) ((a) < (b) ? (a) : (b))
//#ifdef __cplusplus
//extern "C" bool utIsInterruptPending();
//#else
//extern bool utIsInterruptPending();
//#endif

#if defined(NAN_EQUALS_ZERO)
#define IsNonZero(d) ((d)!=0.0 || mxIsNaN(d))
#else
#define IsNonZero(d) ((d)!=0.0)
#endif

//Dynamic force
// double force(double x)
// {
// 	double i;
// 	if ((1 - x) >= 0)
// 	{
// 		i = 1.0;
// 	}
// 	else
// 	{
// 		i = 0.0;
// 	}
// 	return -4 * (1.0 - pow((2.0 * x - 1.0), 2.0)) * i;
// }
double force(double x)
{
	return 1e9*sin(460 * x);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	//*********************************************
	/* get the values from the struct 1x1 */
	// int NoTimeStep = (int) mxGetScalar(mxGetField(prhs[0], 0, "NoTimeStep"));
	// int N_DoF = (int) mxGetScalar(mxGetField(prhs[0], 0, "N_DoF")); // number of dof per element
	// double dt = (double) mxGetScalar(mxGetField(prhs[0], 0, "dt")); // number of dof per element
	//*********************************************

	/* get the values from the struct Matrix m x 1 *//*only use the pointer*/
	// double *TIME_vec = (double *) mxGetPr(mxGetField(prhs[0], 0, "TIME_vec"));
	// int *TIMEPLOT = (int*) mxGetPr(mxGetField(prhs[0], 0, "TIMEPLOT"));
	// int LengthTIMEPLOT = (int) mxGetScalar(mxGetField(prhs[0], 0, "LengthTIMEPLOT")); // number of dof per element

	// 	double *val_v0 = (double *) mxGetPr(mxGetField(prhs[0], 0, "v0")); //
	// 	Eigen::VectorXd v0 = Map < VectorXd > (val_v0, N_DoF);
	//*********************************************
	double *val_rhs_matrix = (double *) mxGetPr(mxGetField(prhs[0], 0, "rhs_matrix"));
	int m_rhs_matrix = mxGetM(mxGetField(prhs[0], 0, "rhs_matrix"));
	int n_rhs_matrix = mxGetN(mxGetField(prhs[0], 0, "rhs_matrix"));
	Eigen::MatrixXd rhs_matrix = Eigen::Map < MatrixXd > (val_rhs_matrix, m_rhs_matrix, n_rhs_matrix);

	double *nnzval_K = (double *) mxGetPr(mxGetField(prhs[0], 0, "nnzval_K"));
	int m_K = (int) mxGetScalar(mxGetField(prhs[0], 0, "m_K"));
	int nnz_K = (int) mxGetScalar(mxGetField(prhs[0], 0, "nnz_K"));
	int *Ir_K = (int *) mxGetPr(mxGetField(prhs[0], 0, "Ir_K"));
	int *Jc_K = (int *) mxGetPr(mxGetField(prhs[0], 0, "Jc_K"));
	std::vector < Eigen::Triplet<double> > trip_K(3 * nnz_K);
	for (int i = 0; i < nnz_K; ++i)
	{
		trip_K.push_back(Eigen::Triplet<double>(Ir_K[i] - 1, Jc_K[i] - 1, nnzval_K[i]));
	}
	Eigen::SparseMatrix<double> K(m_K, m_K);
	K.setFromTriplets(trip_K.begin(), trip_K.end());

        std::cout << "finishing read matrix from matlab" << std::endl;

	//	* Out put *//* Out put *//* Out put *//* Out put *//* Out put *//* Out put */
	plhs[0] = mxCreateDoubleMatrix((mwSize) m_rhs_matrix, (mwSize) n_rhs_matrix, mxREAL);
	double *u0_out = mxGetPr(plhs[0]); // pointer pr_out will manage data in COLUMN Major.
	// plhs[1] = mxCreateDoubleMatrix((mwSize) N_DoF, (mwSize) LengthTIMEPLOT, mxREAL);
	// double *v0_out = mxGetPr(plhs[1]); // pointer pr_out will manage data in COLUMN Major.
	// plhs[2] = mxCreateDoubleMatrix((mwSize) N_DoF, (mwSize) LengthTIMEPLOT, mxREAL);
	// double *a0_out = mxGetPr(plhs[2]); // pointer pr_out will manage data in COLUMN Major.

	Eigen::SparseLU < Eigen::SparseMatrix<double> > solver_LHS;
	solver_LHS.analyzePattern(K); // for this step the numerical values of A are not used
	solver_LHS.factorize(K);
	if (solver_LHS.info() != Success)
	{
		// decomposition failed
		std::cout << "decomposition failed" << std::endl;
		return;
	} else {
		std::cout << "decomposition successful" << std::endl;
	}

	Eigen::MatrixXd u_n1(m_rhs_matrix,n_rhs_matrix);
	u_n1 = solver_LHS.solve(rhs_matrix);

	//Update solution to Final matrix

	Eigen::Map < MatrixXd > (u0_out + (m_rhs_matrix * 0), m_rhs_matrix,n_rhs_matrix) = u_n1;

}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>

#include <OpenTissue/core/math/optimization/optimization_make_constant_bounds.h>
#include <OpenTissue/core/math/optimization/optimization_project.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

template<typename bound_function>
inline void test_projection( bound_function const & lower, bound_function const & upper, ublas::vector<double> const & x)
{
  double tol =0.01;
  {
    ublas::vector<double>  y;
    y = x;
    OpenTissue::math::optimization::project(x,lower,upper,y);
    BOOST_CHECK_CLOSE( y(0) , 1.0 , tol );
    BOOST_CHECK_CLOSE( y(1) , -2.0 , tol );
    BOOST_CHECK_CLOSE( y(2) , 3.0 , tol );
    BOOST_CHECK_CLOSE( y(3) , -4.0 , tol );
    BOOST_CHECK_CLOSE( y(4) , 5.0 , tol );
    BOOST_CHECK_CLOSE( y(5) , -6.0 , tol );
    BOOST_CHECK_CLOSE( y(6) , 7.0 , tol );
    BOOST_CHECK_CLOSE( y(7) , -8.0 , tol );
    BOOST_CHECK_CLOSE( y(8) , 9.0 , tol );
    BOOST_CHECK_CLOSE( y(9) , -10.0 , tol );
  }
  {
    ublas::vector<double>  y;
    y = x;

    OpenTissue::math::optimization::project(y,lower,upper);

    BOOST_CHECK_CLOSE( y(0) , 1.0 , tol );
    BOOST_CHECK_CLOSE( y(1) , -2.0 , tol );
    BOOST_CHECK_CLOSE( y(2) , 3.0 , tol );
    BOOST_CHECK_CLOSE( y(3) , -4.0 , tol );
    BOOST_CHECK_CLOSE( y(4) , 5.0 , tol );
    BOOST_CHECK_CLOSE( y(5) , -6.0 , tol );
    BOOST_CHECK_CLOSE( y(6) , 7.0 , tol );
    BOOST_CHECK_CLOSE( y(7) , -8.0 , tol );
    BOOST_CHECK_CLOSE( y(8) , 9.0 , tol );
    BOOST_CHECK_CLOSE( y(9) , -10.0 , tol );
  }
  {
    ublas::vector<double>  y;
    y = x;
    OpenTissue::math::optimization::Projection<double, bound_function> P(lower,upper);
    P(x,y);
    BOOST_CHECK_CLOSE( y(0) , 1.0 , tol );
    BOOST_CHECK_CLOSE( y(1) , -2.0 , tol );
    BOOST_CHECK_CLOSE( y(2) , 3.0 , tol );
    BOOST_CHECK_CLOSE( y(3) , -4.0 , tol );
    BOOST_CHECK_CLOSE( y(4) , 5.0 , tol );
    BOOST_CHECK_CLOSE( y(5) , -6.0 , tol );
    BOOST_CHECK_CLOSE( y(6) , 7.0 , tol );
    BOOST_CHECK_CLOSE( y(7) , -8.0 , tol );
    BOOST_CHECK_CLOSE( y(8) , 9.0 , tol );
    BOOST_CHECK_CLOSE( y(9) , -10.0 , tol );
  }
  {
    ublas::vector<double>  y;
    y = x;
    OpenTissue::math::optimization::Projection<double, bound_function> P(lower,upper);
    P(y);
    BOOST_CHECK_CLOSE( y(0) , 1.0 , tol );
    BOOST_CHECK_CLOSE( y(1) , -2.0 , tol );
    BOOST_CHECK_CLOSE( y(2) , 3.0 , tol );
    BOOST_CHECK_CLOSE( y(3) , -4.0 , tol );
    BOOST_CHECK_CLOSE( y(4) , 5.0 , tol );
    BOOST_CHECK_CLOSE( y(5) , -6.0 , tol );
    BOOST_CHECK_CLOSE( y(6) , 7.0 , tol );
    BOOST_CHECK_CLOSE( y(7) , -8.0 , tol );
    BOOST_CHECK_CLOSE( y(8) , 9.0 , tol );
    BOOST_CHECK_CLOSE( y(9) , -10.0 , tol );
  }
  {
    ublas::vector<double>  y;
    y = x;
    OpenTissue::math::optimization::NoProjection<double> P;
    P(x,y);
    BOOST_CHECK_CLOSE( y(0) , x(0) , tol );
    BOOST_CHECK_CLOSE( y(1) , x(1) , tol );
    BOOST_CHECK_CLOSE( y(2) , x(2) , tol );
    BOOST_CHECK_CLOSE( y(3) , x(3) , tol );
    BOOST_CHECK_CLOSE( y(4) , x(4) , tol );
    BOOST_CHECK_CLOSE( y(5) , x(5) , tol );
    BOOST_CHECK_CLOSE( y(6) , x(6) , tol );
    BOOST_CHECK_CLOSE( y(7) , x(7) , tol );
    BOOST_CHECK_CLOSE( y(8) , x(8) , tol );
    BOOST_CHECK_CLOSE( y(9) , x(9) , tol );
  }
  {
    ublas::vector<double>  y;
    y = x;
    OpenTissue::math::optimization::NoProjection<double> P;
    P(y);
    BOOST_CHECK_CLOSE( y(0) , x(0) , tol );
    BOOST_CHECK_CLOSE( y(1) , x(1) , tol );
    BOOST_CHECK_CLOSE( y(2) , x(2) , tol );
    BOOST_CHECK_CLOSE( y(3) , x(3) , tol );
    BOOST_CHECK_CLOSE( y(4) , x(4) , tol );
    BOOST_CHECK_CLOSE( y(5) , x(5) , tol );
    BOOST_CHECK_CLOSE( y(6) , x(6) , tol );
    BOOST_CHECK_CLOSE( y(7) , x(7) , tol );
    BOOST_CHECK_CLOSE( y(8) , x(8) , tol );
    BOOST_CHECK_CLOSE( y(9) , x(9) , tol );
  }
}

BOOST_AUTO_TEST_SUITE(opentissue_math_big_projection);

BOOST_AUTO_TEST_CASE(test_case)
{
  ublas::vector<double> lower;
  ublas::vector<double> upper;
  ublas::vector<double> x;

  lower.resize(10,false);
  upper.resize(10,false);
  x.resize(10,false);

  upper(0) = 1.0;
  upper(1) = 2.0;
  upper(2) = 3.0;
  upper(3) = 4.0;
  upper(4) = 5.0;
  upper(5) = 6.0;
  upper(6) = 7.0;
  upper(7) = 8.0;
  upper(8) = 9.0;
  upper(9) = 10.0;

  lower = -upper;

  x(0) =  2.0;
  x(1) = -3.0;
  x(2) =  4.0;
  x(3) = -5.0;
  x(4) =  6.0;
  x(5) = -7.0;
  x(6) =  8.0;
  x(7) = -9.0;
  x(8) =  10.0;
  x(9) = -11.0;

  test_projection(
    OpenTissue::math::optimization::make_constant_bounds( lower )
    , OpenTissue::math::optimization::make_constant_bounds( upper )
    , x
    );

}

BOOST_AUTO_TEST_SUITE_END();


#include <AES.h>
#include <NTL/ZZ.h>
#include <RSA.h>
#include <UI.h>
#include <iostream>
using namespace std;
using namespace NTL;
void saveRSAKey(int bits) {
	ui::printTitle("  生成RSA密钥  ");

	cout << endl << "生成密钥中……" << endl;
	RSA::privateKey priK;
	RSA::publicKey pubK;
	RSA::makeKey(priK, pubK, bits);

	cout << endl;
	cout << "private key:" << endl << endl;
	cout << "p=" << priK.p << endl << endl;
	cout << "q=" << priK.q << endl << endl;
	cout << "d=" << priK.d << endl << endl;
	cout << endl;
	cout << "public key:" << endl << endl;
	cout << "n=" << pubK.n << endl << endl;
	cout << "e=" << pubK.e << endl << endl;
	cout << endl;
	ui::pause("\n任意键继续……");
	ui::OptionC opt[] = {{'1', "保存到文件"}, {'0', "不保存"}, ui::END_OPTC};
	ui::showOptionC(opt);
	int choice = ui::inputOptionC(opt);
	if (opt[choice].c == '1') {
		string priFile = "rsa.pri", pubFile = "rsa.pub";
		cout << "请输入私钥文件文件名（默认 rsa.pri ）：" << endl;
		ui::getLine(priFile, "rsa.pri");
		cout << "请输入公钥文件文件名（默认 rsa.pub ）：" << endl;
		ui::getLine(pubFile, "rsa.pub");
		RSA::savePublicKey(pubK, pubFile);
		RSA::savePrivateKey(priK, priFile);
	}
	return;
}

void makeRSAKey() {
	ui::printTitle("  选择生成密钥位数  ");
	ui::OptionC const opt[] = {
		{'1', "512"}, {'2', "1024"}, {'0', "离开"}, ui::END_OPTC};
	ui::showOptionC(opt);
	int choice = ui::inputOptionC(opt);
	if (opt[choice].c == '0')
		return;

	int m;
	if (opt[choice].c == '1')
		m = 512;
	if (opt[choice].c == '2')
		m = 1024;
	saveRSAKey(m);
	return;
}
void title() {
	ui::OptionC const opt[] = {{'1', "生成RSA密钥"}, {'2', "文件加密"},
							   {'3', "文件解密"},
							   {'0', "离开"},		 ui::END_OPTC};
	while (1) {
		ui::printTitle("  AES加密工具  ");
		ui::showOptionC(opt);
		int choice = ui::inputOptionC(opt);
		if (opt[choice].c == '0')
			break;
		if (opt[choice].c == '1')
			makeRSAKey();
		if (opt[choice].c == '2')
			AES::encipher();
		if (opt[choice].c == '3')
			AES::decipher();
	}
	return;
}

int main() {
	ui::setcoder("utf-8");
	title();
	return 0;
}


#include "sco/optimizers.hpp"
#include "sco/solver_interface.hpp"
#include "sco/expr_op_overloads.hpp"
#include "sco/modeling_utils.hpp"
#include "sco/sco_common.hpp"
#include "utils/logging.hpp"
#include <cmath>
#include <boost/assign/list_of.hpp>
#include <boost/format.hpp>
#include <iostream>
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <sstream>
#include "utils/stl_to_string.hpp"
using namespace util;
using namespace std;
using namespace boost::assign;
using namespace sco;
using namespace Eigen;

void setupProblem(OptProbPtr& probptr, size_t nvars) {
  probptr.reset(new OptProb());
  vector<string> var_names;
  for (size_t i=0; i < nvars; ++i) {
    var_names.push_back( (boost::format("x_%i")%i).str() );
  }
  probptr->createVariables(var_names);
}

void expectAllNear(const DblVec& x, const DblVec& y, double abstol) {
  EXPECT_EQ(x.size(), y.size());
  stringstream ss;
  LOG_INFO("checking %s ?= %s", CSTR(x), CSTR(y));
  for (size_t i=0; i < x.size(); ++i) EXPECT_NEAR(x[i], y[i], abstol);
}

double f_QuadraticSeparable(const VectorXd& x) {
  return x(0)*x(0) + sq(x(1) - 1) + sq(x(2)-2);
}
TEST(SQP, QuadraticSeparable)  {
  // if the problem is exactly a QP, it should be solved in one iteration
  OptProbPtr prob;
  setupProblem(prob, 3);
  prob->addCost(CostPtr(new CostFromFunc(ScalarOfVector::construct(&f_QuadraticSeparable), prob->getVars(), "f")));
  BasicTrustRegionSQP solver(prob);
  solver.trust_box_size_ = 100;
  vector<double> x = list_of(3)(4)(5);
  solver.initialize(x);
  OptStatus status = solver.optimize();
  ASSERT_EQ(status, OPT_CONVERGED);
  expectAllNear(solver.x(), list_of(0)(1)(2), 1e-3);
  // todo: checks on number of iterations and function evaluates
}
double f_QuadraticNonseparable(const VectorXd& x) {
  return sq(x(0) - x(1) + 3*x(2)) + sq(x(0)-1) + sq(x(2) - 2);
}
TEST(SQP, QuadraticNonseparable)  {
  OptProbPtr prob;
  setupProblem(prob, 3);
  prob->addCost(CostPtr(new CostFromFunc(ScalarOfVector::construct(&f_QuadraticNonseparable), prob->getVars(), "f",  true)));
  BasicTrustRegionSQP solver(prob);
  solver.trust_box_size_ = 100;
  solver.min_trust_box_size_ = 1e-5;
  solver.min_approx_improve_ = 1e-6;
  vector<double> x = list_of(3)(4)(5);
  solver.initialize(x);
  OptStatus status = solver.optimize();
  ASSERT_EQ(status, OPT_CONVERGED);
  expectAllNear(solver.x(), list_of(1)(7)(2), .01);
  // todo: checks on number of iterations and function evaluates
}

void testProblem(ScalarOfVectorPtr f, VectorOfVectorPtr g, ConstraintType cnt_type,
  const DblVec& init, const DblVec& sol) {
    OptProbPtr prob;
    size_t n = init.size();
    assert (sol.size() == n);
    setupProblem(prob, n);
    prob->addCost(CostPtr(new CostFromFunc(f, prob->getVars(), "f", true)));
    prob->addConstraint(ConstraintPtr(new ConstraintFromFunc(g, prob->getVars(), VectorXd(), cnt_type,"g")));
    BasicTrustRegionSQP solver(prob);
    solver.max_iter_ = 1000;
    solver.min_trust_box_size_ = 1e-5;
    solver.min_approx_improve_ = 1e-10;
    solver.merit_error_coeff_ = 1;

    solver.initialize(init);
    OptStatus status = solver.optimize();
    EXPECT_EQ(status, OPT_CONVERGED);
    expectAllNear(solver.x(), sol, .01);
}
// http://www.ai7.uni-bayreuth.de/test_problem_coll.pdf

double f_TP1(const VectorXd& x) {
  return 1*sq(x(1)-sq(x(0))) + sq(1-x(0));
}
VectorXd g_TP1(const VectorXd& x) {
  VectorXd out(1);
  out(0) = -1.5 - x(1);
  return out;
}
double f_TP2(const VectorXd& x) {
  return 100*sq(x(1)-sq(x(0))) + sq(1-x(0));
}
VectorXd g_TP2(const VectorXd& x) {
  VectorXd out(1);
  out(0) = -1.5 - x(1);
  return out;
}
double f_TP3(const VectorXd& x) {
  return (x(1) + 1e-5 * sq(x(1)-x(0)));
}
VectorXd g_TP3(const VectorXd& x) {
  VectorXd out(1);
  out(0) = 0 - x(1);
  return out;
}
double f_TP6(const VectorXd& x) {
  return sq(1-x(0));
}
VectorXd g_TP6(const VectorXd& x) {
  VectorXd out(1);
  out(0) = 10*(x(1)-sq(x(0)));
  return out;
}
double f_TP7(const VectorXd& x) {
  return log(1+sq(x(0))) - x(1);
}
VectorXd g_TP7(const VectorXd& x) {
  VectorXd out(1);
  out(0) = sq(1+sq(x(0))) + sq(x(1)) - 4;
  return out;
}

TEST(SQP, TP1) {
  testProblem(ScalarOfVector::construct(&f_TP1), VectorOfVector::construct(&g_TP1), INEQ, list_of(-2)(1), list_of(1)(1));
}
TEST(SQP, TP3) {
  testProblem(ScalarOfVector::construct(&f_TP3), VectorOfVector::construct(&g_TP3), INEQ, list_of(10)(1), list_of(0)(0));
}
TEST(SQP, TP6) {
  testProblem(ScalarOfVector::construct(&f_TP6), VectorOfVector::construct(&g_TP6), EQ, list_of(10)(1), list_of(1)(1));
}
TEST(SQP, TP7) {
  testProblem(ScalarOfVector::construct(&f_TP7), VectorOfVector::construct(&g_TP7), EQ, list_of(2)(2), list_of(0.)(sqrtf(3.)));
}


#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud) {
    // --------------------------------------------
    // -----Open 3D viewer and add point cloud-----
    // --------------------------------------------
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
        new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
    viewer->setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    // viewer->addCoordinateSystem (1.0);
    viewer->initCameraParameters();
    return (viewer);
}

int main(int argc, char **argv) {
    // initialize PointClouds
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
        new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr final(
        new pcl::PointCloud<pcl::PointXYZ>);

    // populate our PointCloud with points
    cloud->width = 500;
    cloud->height = 1;
    cloud->is_dense = false;
    cloud->points.resize(cloud->width * cloud->height);
    for (size_t i = 0; i < cloud->points.size(); ++i) {
        if (pcl::console::find_argument(argc, argv, "-s") >= 0 ||
            pcl::console::find_argument(argc, argv, "-sf") >= 0) {
            cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0);
            cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0);
            if (i % 5 == 0)
                cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0);
            else if (i % 2 == 0)
                cloud->points[i].z =
                    sqrt(1 - (cloud->points[i].x * cloud->points[i].x) -
                         (cloud->points[i].y * cloud->points[i].y));
            else
                cloud->points[i].z =
                    -sqrt(1 - (cloud->points[i].x * cloud->points[i].x) -
                          (cloud->points[i].y * cloud->points[i].y));
        } else {
            cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0);
            cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0);
            if (i % 2 == 0)
                cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0);
            else
                cloud->points[i].z =
                    -1 * (cloud->points[i].x + cloud->points[i].y);
        }
    }

    std::vector<int> inliers;

    // created RandomSampleConsensus object and compute the appropriated model
    pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(
        new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(
        new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
    if (pcl::console::find_argument(argc, argv, "-f") >= 0) {
        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
        ransac.setDistanceThreshold(.01);
        ransac.computeModel();
        ransac.getInliers(inliers);
    } else if (pcl::console::find_argument(argc, argv, "-sf") >= 0) {
        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
        ransac.setDistanceThreshold(.01);
        ransac.computeModel();
        ransac.getInliers(inliers);
    }

    // copies all inliers of the model computed to another PointCloud
    pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);

    // creates the visualization object and adds either our orignial cloud or
    // all of the inliers depending on the command line arguments specified.
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
    if (pcl::console::find_argument(argc, argv, "-f") >= 0 ||
        pcl::console::find_argument(argc, argv, "-sf") >= 0)
        viewer = simpleVis(final);
    else
        viewer = simpleVis(cloud);
    while (!viewer->wasStopped()) {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
    return 0;
}


#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_msgs/ModelCoefficients.h>
#include <visualization_msgs/Marker.h>

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/common/transforms.h>

#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <Eigen/Dense>

ros::Publisher obst_cluster_pub;
ros::Publisher model_pub;
bool visualize = true;

tf::Transform ros_tf;   // Transform to publish

void pointCloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& msg)
{
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.1);
    seg.setInputCloud (msg);

    // Coefficients of plane equation ax + by + cz + d = 0
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    // Indices of points in the plane
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    seg.segment (*inliers, *coefficients);

    // Output error if the plane found is less than 1/3 of the point cloud
    if (inliers->indices.size() < msg->points.size()/3.0)
        PCL_ERROR ("Could not estimate a planar model for the given dataset.\n");
    else
    {
        float a = coefficients->values[0];
        float b = coefficients->values[1];
        float c = coefficients->values[2];
        float d = coefficients->values[3];
        if (visualize)
        {
            // Publish plane visualization
            visualization_msgs::Marker plane_points;
            plane_points.type = visualization_msgs::Marker::SPHERE_LIST;
            plane_points.header.frame_id = "/stereo_camera";
            plane_points.header.stamp = ros::Time::now();
            plane_points.id = 0;
            plane_points.color.r = 0.0f;
            plane_points.color.g = 1.0f;
            plane_points.color.b = 0.0f;
            plane_points.color.a = 1.0;
            plane_points.scale.x = 0.02;
            plane_points.scale.y = 0.02;
            plane_points.scale.z = 0.02;
            plane_points.action = visualization_msgs::Marker::ADD;

            for(float x = -5; x < 5; x += 0.05)
            {
                for(float y = 0; y < 5; y += 0.05)
                {
                    float z = (-a*x - b*y - d)/c;
                    geometry_msgs::Point p;
                    p.x = x;
                    p.y = y;
                    p.z = z;
                    plane_points.points.push_back(p);
                }
            }

            obst_cluster_pub.publish(plane_points);
        }

        // Publish coefficients
        pcl_msgs::ModelCoefficients ros_coeff;
        pcl_conversions::fromPCL(*coefficients, ros_coeff);
        model_pub.publish(ros_coeff);

        // Calculate angle between the point-cloud ground plane and the world xy plane
        Eigen::Matrix<double, 1, 3> pcl_plane_normal, world_xy_normal, rotation_vector;
        pcl_plane_normal[0] = a;
        pcl_plane_normal[1] = b;
        pcl_plane_normal[2] = c;
        world_xy_normal[0] = 0.0;
        world_xy_normal[1] = 0.0;
        world_xy_normal[2] = 1.0;

        rotation_vector = (pcl_plane_normal.cross(world_xy_normal)).normalized();
        double theta = acos(pcl_plane_normal.dot(world_xy_normal)/sqrt(a*a + b*b + c*c));

        // Use the angle to create an affine transform
        Eigen::Affine3d tf_ground_plane = Eigen::Affine3d::Identity();
        tf_ground_plane.rotate(Eigen::AngleAxisd(theta, rotation_vector));

        // Apply the affine transform to the ground plane to determine translation
        Eigen::Vector4d original, transformed;
        original[0] = a*d;
        original[1] = b*d;
        original[2] = c*d;
        original[3] = 1.0;
        transformed = tf_ground_plane * original;
        tf_ground_plane.translation() << transformed[0], transformed[1], transformed[2];

        // Convert Eigen Affine transform to a ROS transform
        tf::transformEigenToTF(tf_ground_plane, ros_tf);
    }

    // Publish ROS transform
    static tf::TransformBroadcaster transform_br;
    transform_br.sendTransform(tf::StampedTransform(ros_tf, ros::Time::now(), "world", "stereo_camera"));
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ground_plane");
    ros::NodeHandle n;

    ros::param::get("~visualize", visualize);

    ros::Subscriber sub = n.subscribe<pcl::PointCloud<pcl::PointXYZ> >("/points2", 1, pointCloudCallback);
    model_pub = n.advertise<pcl_msgs::ModelCoefficients> ("ground_plane/model_coefficients", 1);
    if(visualize)
        obst_cluster_pub = n.advertise<visualization_msgs::Marker> ("ground_plane/visual_markers", 1);

    ros::spin();

    return 0;
}


#pragma once

#include <iostream>
#include <string>
#include <sstream>
#include <vector>
#include <algorithm>
#include <cmath>
#include <limits>

#include <eigen_conversions/eigen_msg.h>
#include <Eigen/StdVector>
#include <Eigen/SVD>

#include <boost/foreach.hpp>

#include <opencv2/opencv.hpp>

#include <image_geometry/pinhole_camera_model.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <ros/ros.h>

// #define SEGMENTATION_DEBUG

namespace nav {

/// UTILS

template <typename _Matrix_Type_>
bool pseudoInverse(
    const _Matrix_Type_ &a, _Matrix_Type_ &result,
    double epsilon =
        std::numeric_limits<typename _Matrix_Type_::Scalar>::epsilon());

typedef std::vector<cv::Point> Contour;

// Compute the centroid of an OpenCV contour (Not templated)
cv::Point contour_centroid(Contour &contour);

// Used as a comparison function for std::sort(), sorting in order of decreasing
// perimeters
// returns true if the contourArea(c1) > contourArea(c2)
bool larger_contour(const Contour &c1, const Contour &c2);

// Filter a histogram of type cv::MatND generated by cv::calcHist using a
// gaussian kernel
cv::MatND smooth_histogram(const cv::MatND &histogram,
                           size_t filter_kernel_size = 3, float sigma = 1.0);

// Generate a one-dimensional gaussian kernel given a kernel size and it's
// standard deviation
// (sigma)
std::vector<float> generate_gaussian_kernel_1D(size_t kernel_size = 3,
                                               float sigma = 1.0);

// Finds positive local maxima greater than (global maximum * thresh_multiplier)
std::vector<cv::Point> find_local_maxima(const cv::MatND &histogram,
                                         float thresh_multiplier);

// Finds negative local minima less than (global minimum * thresh_multiplier)
std::vector<cv::Point> find_local_minima(const cv::MatND &histogram,
                                         float thresh_multiplier);

// Selects the mode of a multi-modal distribution closest to a given target
// value
unsigned int select_hist_mode(std::vector<cv::Point> &histogram_modes,
                              unsigned int target);

// Takes in a grayscale image and segments out a semi-homogenous foreground
// object with pixel intensities close to <target>. Tuning of last three
// parameters may imrove results but default values should work well in
// most cases.
void statistical_image_segmentation(const cv::Mat &src, cv::Mat &dest,
                                    cv::Mat &debug_img, const int hist_size,
                                    const float **ranges, const int target,
                                    std::string image_name = "Unnamed Image",
                                    bool ret_dbg_img = false,
                                    const float sigma = 1.5,
                                    const float low_thresh_gain = 0.5,
                                    const float high_thresh_gain = 0.5);

cv::Mat triangulate_Linear_LS(cv::Mat mat_P_l, cv::Mat mat_P_r,
                              cv::Mat undistorted_l, cv::Mat undistorted_r);

Eigen::Vector3d kanatani_triangulation(const cv::Point2d &pt1,
                                       const cv::Point2d &pt2,
                                       const Eigen::Matrix3d &essential,
                                       const Eigen::Matrix3d &R);

Eigen::Vector3d lindstrom_triangulation(const cv::Point2d &pt1,
                                        const cv::Point2d &pt2,
                                        const Eigen::Matrix3d &essential,
                                        const Eigen::Matrix3d &R);

struct ImageWithCameraInfo {
  /**
          Packages corresponding  sensor_msgs::ImageConstPtr and
     sensor_msgs::CameraInfoConstPtr
     info_msg
          into one object. Containers of these objects can be sorted by their
     image_time attribute
  */
public:
  ImageWithCameraInfo() {}
  ImageWithCameraInfo(sensor_msgs::ImageConstPtr _image_msg_ptr,
                      sensor_msgs::CameraInfoConstPtr _info_msg_ptr);
  sensor_msgs::ImageConstPtr image_msg_ptr;
  sensor_msgs::CameraInfoConstPtr info_msg_ptr;
  ros::Time image_time;
  bool operator<(const ImageWithCameraInfo &right) const {
    return this->image_time < right.image_time;
  }
};

class FrameHistory {
  /**
          Object that subscribes itself to an image topic and stores up to a
     user defined
          number of ImageWithCameraInfo objects. The frame history can then be
     retrieved
          in whole or just a portion.
  */
public:
  FrameHistory(std::string img_topic, unsigned int hist_size);
  ~FrameHistory();
  void image_callback(const sensor_msgs::ImageConstPtr &image_msg,
                      const sensor_msgs::CameraInfoConstPtr &info_msg);
  std::vector<ImageWithCameraInfo>
  get_frame_history(unsigned int frames_requested);
  int frames_available();

  const std::string topic_name;
  const size_t history_size;

private:
  ros::NodeHandle nh;
  image_transport::CameraSubscriber _image_sub;
  image_transport::ImageTransport _image_transport;
  std::vector<ImageWithCameraInfo> _frame_history_ring_buffer;
  size_t frame_count;
};

/// Param Helpers

struct Range {
  cv::Scalar lower;
  cv::Scalar upper;
};

void range_from_param(std::string &param_root, Range &range);

void inParamRange(cv::Mat &src, Range &range, cv::Mat &dest);

/// Templated pseudoinverse function implementation
template <typename _Matrix_Type_>
bool pseudoInverse(
    const _Matrix_Type_ &a, _Matrix_Type_ &result,
    double epsilon =
        std::numeric_limits<typename _Matrix_Type_::Scalar>::epsilon()) {
  if (a.rows() < a.cols())
    return false;

  Eigen::JacobiSVD<_Matrix_Type_> svd = a.jacobiSvd();

  typename _Matrix_Type_::Scalar tolerance =
      epsilon * std::max(a.cols(), a.rows()) *
      svd.singularValues().array().abs().maxCoeff();

  result = svd.matrixV() *
           _Matrix_Type_(
               _Matrix_Type_((svd.singularValues().array().abs() > tolerance)
                                 .select(svd.singularValues().array().inverse(),
                                         0)).diagonal()) *
           svd.matrixU().adjoint();
}
} // namespace sub


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2016 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <algorithms/test_length.hpp>
#include <algorithms/length/linestring_cases.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/adapted/std_pair_as_segment.hpp>

#include <test_geometries/all_custom_linestring.hpp>
#include <test_geometries/wrapped_boost_array.hpp>

template <typename P>
void test_all_default() //test the default strategy
{
    double const pi = boost::math::constants::pi<double>();

    for(std::size_t i = 0; i < 2; ++i)
    {
        test_geometry<bg::model::linestring<P> >(Ls_data_sph[i], 2 * pi);
    }
    // Geometries with length zero
    test_geometry<P>("POINT(0 0)", 0);
    test_geometry<bg::model::polygon<P> >("POLYGON((0 0,0 1,1 1,1 0,0 0))", 0);
}

template <typename P>
void test_all_haversine(double const mean_radius)
{
    double const pi = boost::math::constants::pi<double>();
    bg::strategy::distance::haversine<float> haversine_strategy(mean_radius);

    for(std::size_t i = 0; i < 2; ++i)
    {
        test_geometry<bg::model::linestring<P> >(Ls_data_sph[i],
                                                 2 * pi * mean_radius,
                                                 haversine_strategy);
    }
    // Geometries with length zero
    test_geometry<P>("POINT(0 0)", 0, haversine_strategy);
    test_geometry<bg::model::polygon<P> >("POLYGON((0 0,0 1,1 1,1 0,0 0))",
                                          0, haversine_strategy);
}

template <typename P>
void test_empty_input()
{
    test_empty_input(bg::model::linestring<P>());
    test_empty_input(bg::model::multi_linestring<P>());
}

int test_main(int, char* [])
{
    //Earth radius estimation in Km
    //(see https://en.wikipedia.org/wiki/Earth_radius)
    double const mean_radius = 6371.0;

    test_all_default<bg::model::d2::point_xy<int,
            bg::cs::spherical_equatorial<bg::degree> > >();
    test_all_default<bg::model::d2::point_xy<float,
            bg::cs::spherical_equatorial<bg::degree> > >();
    test_all_default<bg::model::d2::point_xy<double,
            bg::cs::spherical_equatorial<bg::degree> > >();

    test_all_haversine<bg::model::d2::point_xy<int,
        bg::cs::spherical_equatorial<bg::degree> > >(mean_radius);
    test_all_haversine<bg::model::d2::point_xy<float,
        bg::cs::spherical_equatorial<bg::degree> > >(mean_radius);
    test_all_haversine<bg::model::d2::point_xy<double,
        bg::cs::spherical_equatorial<bg::degree> > >(mean_radius);

#if defined(HAVE_TTMATH)
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    //test_empty_input<bg::model::d2::point_xy<int> >();

    return 0;
}


#define __USE_MATH_DEFINES

#define BOOST_UBLAS_TYPE_CHECK (0)

#include <iostream>
#include <fstream>
#include <cmath>
#include <vector>

#include "JGTL_Ray2.h"
#include "JGTL_Vector2.h"
#include "JGTL_Quadratic.h"
#include "JGTL_StringConverter.h"

#include <boost/algorithm/string.hpp>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
using namespace std;
using namespace JGTL;
using namespace boost;
using namespace boost::numeric;

    typedef float (*regressionEquation2)(float,float,float);
    typedef void (*regressionEquationDerivative2)(float,float,float,float&,float&);

    typedef float (*regressionEquation3)(float,float,float,float);
    typedef void (*regressionEquationDerivative3)(float,float,float,float,float&,float&,float&);

	template<class T>
bool InvertMatrix(const ublas::matrix<T>& input, ublas::matrix<T>& inverse)
{
	using namespace boost::numeric::ublas;
	typedef permutation_matrix<std::size_t> pmatrix;
	// create a working copy of the input
	matrix<T> A(input);
	// create a permutation matrix for the LU-factorization
	pmatrix pm(A.size1());

	// perform LU-factorization
	int res = lu_factorize(A,pm);
	if( res != 0 ) return false;

	// create identity matrix of "inverse"
	inverse.assign(ublas::identity_matrix<T>(A.size1()));

	// backsubstitute to get the inverse
	lu_substitute(A, pm, inverse);

	return true;
}

void LoadPoints(string ptsString,vector< JGTL::Vector2<float> > &points)
{
	vector<string> splits;
	split( splits, ptsString, is_any_of(",") );

	for(size_t a=0;a<splits.size();a+=2)
	{
		if(splits[a].size()==0)
		{
			return;
		}

		points.push_back(
				JGTL::Vector2<float>(stringTo<float>(splits[a]),stringTo<float>(splits[a+1]))
				);

		//cout << "Adding point: " << points.back() << endl;
	}
}

float linear(float a,float b,float x)
{
	return a + b*x;
}
void linearDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = x;
}

float power(float a,float b,float x)
{
	return a*pow(x,b);
}
void powerDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	float tmp = pow(x,b);
	deriv_a = tmp;
	deriv_b = a*tmp*log(x);
}

float exponential(float a,float b,float x)
{
	return a*exp(b*x);
}
void exponentialDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	float tmp = exp(b*x);
	deriv_a = tmp;
	deriv_b = a*x*tmp;
}

float logarithmic(float a,float b,float x)
{
	return a + b*log(x);
}
void logarithmicDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = log(x);
}

float hyperbolic(float a,float b,float x)
{
	return a + b/x;
}
void hyperbolicDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = 1.0f/x;
}

float squared(float a,float b,float x)
{
	return a + b*x*x;
}
void squaredDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = x*x;
}

float taylor(float a,float b,float x)
{
	return exp(a + b*x*x);
}
void taylorDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	float tmp = exp(a + b*x*x);
	deriv_a = tmp;
	deriv_b = x*x*tmp;
}

float expE(float a,float b,float x)
{
	return a + b*pow(x,float(M_E));
}
void expEDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = pow(x,float(M_E));
}

float taylor2(float a,float b,float x)
{
	return exp(a + b*sqrt(x));
}
void taylor2Derivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	float tmp = exp(a + b*sqrt(x));
	deriv_a = tmp;
	deriv_b = sqrt(x)*tmp;
}

float hyperb2(float a,float b,float x)
{
	return a + (b / (x*x) );
}
void hyperb2Derivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = 1/(x*x);
}

float loglog(float a,float b,float x)
{
	return a + b*log(x)*log(x);
}
void loglogDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = log(x)*log(x);
}

float ehyperb(float a,float b,float x)
{
	return a*exp(b/x);
}
void ehyperbDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	float tmp = exp(b/x);
	deriv_a = tmp;
	deriv_b = (a*tmp)/x;
}

float hyperbSqrt(float a,float b,float x)
{
	return a + b/sqrt(x);
}
void hyperbSqrtDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = 1/sqrt(x);
}

float hyperbLn(float a,float b,float x)
{
	return a + b/log(x);
}
void hyperbLnDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = 1/log(x);
}

float logPower(float a,float b,float x)
{
	return a * pow(log(x),b);
}
void logPowerDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	float tmp = pow(log(x),b);
	deriv_a = tmp;
	deriv_b = a*tmp*log(log(x));
}

float sqrt(float a,float b,float x)
{
	return a + b*sqrt(x);
}
void sqrtDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = sqrt(x);
}

float xlogx(float a,float b,float x)
{
	return a + b*pow(x,log(x));
}
void xlogxDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = exp(log(x)*log(x));
}

float hblog(float a,float b,float x)
{
	return a + b/pow(x,log(x));
}
void hblogDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = exp(-1*log(x)*log(x));
}

float hbe(float a,float b,float x)
{
	return a + b/pow(x,float(M_E));
}
void hbeDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	deriv_a = 1;
	deriv_b = 1/pow(x,float(M_E));
}

float ehyperbsqrt(float a,float b,float x)
{
	return a*exp( b / sqrt(x) );
}
void ehyperbsqrtDerivatives(float a,float b,float x,float &deriv_a,float &deriv_b)
{
	float tmp = exp( b / sqrt(x) );
	deriv_a = tmp;
	deriv_b = a*tmp/sqrt(x);
}

float quadratic(float a,float b,float c,float x)
{
	return a*x*x + b*x + c;
}
void quadraticDerivatives(float a,float b,float c,float x,float &deriv_a,float &deriv_b,float &deriv_c)
{
	deriv_a = x*x;
	deriv_b = x;
	deriv_c = 1;
}

class Result2
{
public:
	float A,B;
	float r2;

	Result2(float Aset,float Bset,float r2set)
		:
			A(Aset),
			B(Bset),
			r2(r2set)
	{
	}

	Result2()
	{
	}
};

class Result3
{
public:
	float A,B,C;
	float r2;

	Result3(float Aset,float Bset,float Cset,float r2set)
		:
			A(Aset),
			B(Bset),
			C(Cset),
			r2(r2set)
	{
	}

	Result3()
	{
	}
};

Result2 tryRegression2(
		const vector< JGTL::Vector2<float> > &points,
		regressionEquation2 equation2,
		regressionEquationDerivative2 derivative2
		);

Result3 tryRegression3(
		const vector< JGTL::Vector2<float> > &points,
		regressionEquation3 equation3,
		regressionEquationDerivative3 derivative3
		);

int main()
{
	vector< JGTL::Vector2<float> > points;

	LoadPoints("2,59.49,3,40.81,4,32.64,5,26.10,6,24.18,7,22.01,8,17.13,9,16.77,10,18.26,11,13.38,12,13.93,13,10.99,14,11.21,15,10.61,16,13.16,17,12.14,18,11.53,19,8.54,20,8.16,21,7.21,22,7.28,23,10.15,24,7.21,25,8.31,26,8.44,27,6.19,28,6.29,29,4.34,30,7.13,31,5.51,32,7.54,33,5.16,34,6.98,35,4.76,36,5.67,37,7.85,38,2.94,39,6.54,40,3.22,41,5.59,42,6.51,43,2.52,44,7.07,45,4.32,46,6.88,47,6.97,48,3.53,49,2.51,50,4.38,51,2.56,",points);

	 vector<regressionEquation2> equations2;
	 vector<regressionEquationDerivative2> derivatives2;

    /* the ampersand is actually optional */
    equations2.push_back(&linear);
    derivatives2.push_back(&linearDerivatives);
    equations2.push_back(&power);
    derivatives2.push_back(&powerDerivatives);
    equations2.push_back(&exponential);
    derivatives2.push_back(&exponentialDerivatives);
    equations2.push_back(&logarithmic);
    derivatives2.push_back(&logarithmicDerivatives);
    equations2.push_back(&hyperbolic);
    derivatives2.push_back(&hyperbolicDerivatives);
    equations2.push_back(&squared);
    derivatives2.push_back(&squaredDerivatives);
    equations2.push_back(&taylor);
    derivatives2.push_back(&taylorDerivatives);
    equations2.push_back(&expE);
    derivatives2.push_back(&expEDerivatives);
    equations2.push_back(&taylor2);
    derivatives2.push_back(&taylor2Derivatives);
    equations2.push_back(&hyperb2);
    derivatives2.push_back(&hyperb2Derivatives);
    equations2.push_back(&loglog);
    derivatives2.push_back(&loglogDerivatives);
    equations2.push_back(&ehyperb);
    derivatives2.push_back(&ehyperbDerivatives);
    equations2.push_back(&hyperbSqrt);
    derivatives2.push_back(&hyperbSqrtDerivatives);
    equations2.push_back(&hyperbLn);
    derivatives2.push_back(&hyperbLnDerivatives);
    equations2.push_back(&logPower);
    derivatives2.push_back(&logPowerDerivatives);
    equations2.push_back(&sqrt);
    derivatives2.push_back(&sqrtDerivatives);
    equations2.push_back(&xlogx);
    derivatives2.push_back(&xlogxDerivatives);
    equations2.push_back(&hblog);
    derivatives2.push_back(&hblogDerivatives);
    equations2.push_back(&hbe);
    derivatives2.push_back(&hbeDerivatives);
    equations2.push_back(&ehyperbsqrt);
    derivatives2.push_back(&ehyperbsqrtDerivatives);

	 vector<regressionEquation3> equations3;
	 vector<regressionEquationDerivative3> derivatives3;

    equations3.push_back(&quadratic);
    derivatives3.push_back(&quadraticDerivatives);

	 Result3 bestResult;
	 int resultIndex;
	 int resultDim;

 	 for(size_t regressionTypes2=0;regressionTypes2<equations2.size();regressionTypes2++)
	{
		cout << "ON CASE: " << regressionTypes2 << endl;
		Result2 result = tryRegression2(
				points,
				equations2[regressionTypes2],
				derivatives2[regressionTypes2]
				);

		cout << "A: " << result.A << ", B: " << result.B
			<< ", r2: " << result.r2 << endl;

		if(!regressionTypes2 || result.r2>bestResult.r2)
		{
			bestResult.A = result.A;
			bestResult.B = result.B;
			bestResult.C = 0;
			resultIndex = regressionTypes2;
			resultDim=2;
		}
	}

 	 for(size_t regressionTypes3=0;regressionTypes3<equations3.size();regressionTypes3++)
	{
		cout << "ON CASE: " << regressionTypes3 << endl;
		Result3 result = tryRegression3(
				points,
				equations3[regressionTypes3],
				derivatives3[regressionTypes3]
				);

		cout << "A: " << result.A << ", B: " << result.B
			<< ", C: " << result.C
			<< ", r2: " << result.r2 << endl;

		if(!regressionTypes3 || result.r2>bestResult.r2)
		{
			bestResult = result;
			resultIndex = regressionTypes3;
			resultDim=3;
		}
	}

	 if(resultDim==2)
	 {
	 }
	 else //resultDim==3
	 {
	 }

	return 0;
}

Result2 tryRegression2(
		const vector< JGTL::Vector2<float> > &points,
		regressionEquation2 equation2,
		regressionEquationDerivative2 derivative2
		)
{
	Result2 result;

	/*
	points.push_back(JGTL::Vector2<float>(0,0));
	points.push_back(JGTL::Vector2<float>(1,1));
	points.push_back(JGTL::Vector2<float>(2,2));
	points.push_back(JGTL::Vector2<float>(3,3));
	points.push_back(JGTL::Vector2<float>(4,4));
	*/

	//Formula: f(x,A,B) = A + Bx
	//Derivative with respect to A: f(x,A,B) = 1
	//Derivative with respect to B: f(x,A,B) = x

	//First, pick an initial guess
	result.A = 1;
	result.B = 1;

	float prev_s_yx;

	for(int trials=0;trials<100;trials++)
	{
		//cout << "On trial: " << trials << endl;
		ublas::matrix<float> delta(int(points.size()),1);

		float s_yx = 0;
		float s_y = 0;
		//cout << "Deltas: ";
		float avg_y=0;
		for(int a=0;a<int(points.size());a++)
		{
			avg_y += points[a].y;
		}
		avg_y /= float(points.size());
		//cout << "Difference: ";
		for(int a=0;a<int(points.size());a++)
		{
			delta(a,0) = points[a].y - (equation2)(result.A,result.B,points[a].x);
			//if(a)
				//cout << ", ";
			//cout << delta(a,0);

			//Compute s_yx and s_y to see if we need to keep going
			s_yx += (delta(a,0)*delta(a,0));
			s_y += (points[a].y - avg_y)*(points[a].y - avg_y);
		}

		result.r2 = max(0.0f,1.0f - (s_yx / s_y));
		//cout << endl;

		//cout << "s_yx: " << s_yx << " s_y: " << s_y << endl;
		//cout << "R2: " << r2 << endl;

		if(trials)
		{
			if(fabs((prev_s_yx - s_yx)/(prev_s_yx)) < 0.0001)
			{
				//cout << "Found premature stopping condition!\n";
				break;
			}
		}
		prev_s_yx = s_yx;

		ublas::matrix<float> deriv(int(points.size()),2);

		//Compute estimate
		//cout << "Derivative: ";
		for(int testPoint=0;testPoint<int(points.size());testPoint++)
		{
			const JGTL::Vector2<float> &curPoint = points[testPoint];

			(derivative2)(result.A,result.B,curPoint.x,deriv(testPoint,0),deriv(testPoint,1));

			//cout << deriv(testPoint,0) << ", " << deriv(testPoint,1) << ", ";
		}
		//cout << endl;

		ublas::matrix<float> derivTranspose = trans(deriv);

		//mat_a = derivTranspose * deriv
		ublas::matrix<float> mat_a = prod(derivTranspose,deriv);

		//mat_b = derivTranspose * db
		ublas::matrix<float> mat_b = prod(derivTranspose,delta);

		//a*(delta_vector) = b, so solve for delta_vector
		ublas::matrix<float> mat_a_inverse(2,2);
		bool retval=false;
		try
		{
		    retval = InvertMatrix(mat_a,mat_a_inverse);
		}
		catch(...)
		{
			cout << "Error computing matrix inverse!\n";
		}

		if(retval)
		{
			ublas::matrix<float> delta_vector = prod(mat_a_inverse,mat_b);

			//cout << "Delta A: " << delta_vector(0,0) << endl;
			//cout << "Delta B: " << delta_vector(1,0) << endl;

			result.A += delta_vector(0,0);
			result.B += delta_vector(1,0);

			//cout << "New A: " << result.A << endl;
			//cout << "New B: " << result.B << endl;
		}
		else
		{
			cout << "INVERSE FAILED!\n";
			break;
		}
	}

	//cout << "R2: " << result.r2 << endl;

	return result;
}

Result3 tryRegression3(
		const vector< JGTL::Vector2<float> > &points,
		regressionEquation3 equation3,
		regressionEquationDerivative3 derivative3
		)
{
	Result3 result;

	/*
	points.push_back(JGTL::Vector2<float>(0,0));
	points.push_back(JGTL::Vector2<float>(1,1));
	points.push_back(JGTL::Vector2<float>(2,2));
	points.push_back(JGTL::Vector2<float>(3,3));
	points.push_back(JGTL::Vector2<float>(4,4));
	*/

	//Formula: f(x,A,B) = A + Bx
	//Derivative with respect to A: f(x,A,B) = 1
	//Derivative with respect to B: f(x,A,B) = x

	//First, pick an initial guess
	result.A = 1;
	result.B = 1;
	result.C = 1;

	float prev_s_yx;

	for(int trials=0;trials<100;trials++)
	{
		//cout << "On trial: " << trials << endl;
		ublas::matrix<float> delta(int(points.size()),1);

		float s_yx = 0;
		float s_y = 0;
		//cout << "Deltas: ";
		float avg_y=0;
		for(int a=0;a<int(points.size());a++)
		{
			avg_y += points[a].y;
		}
		avg_y /= float(points.size());
		for(int a=0;a<int(points.size());a++)
		{
			delta(a,0) = points[a].y - (equation3)(result.A,result.B,result.C,points[a].x);
			//if(a)
				//cout << ", ";
			//cout << delta(a,0);

			//Compute s_yx and s_y to see if we need to keep going
			s_yx += (delta(a,0)*delta(a,0));
			s_y += (points[a].y - avg_y)*(points[a].y - avg_y);
		}

		result.r2 = max(0.0f,1.0f - (s_yx / s_y));
		//cout << endl;

		//cout << "s_yx: " << s_yx << " s_y: " << s_y << endl;
		//cout << "R2: " << r2 << endl;

		if(trials)
		{
			if(fabs((prev_s_yx - s_yx)/(prev_s_yx)) < 0.0001)
			{
				//cout << "Found premature stopping condition!\n";
				break;
			}
		}
		prev_s_yx = s_yx;

		ublas::matrix<float> deriv(int(points.size()),3);

		//Compute estimate
		for(int testPoint=0;testPoint<int(points.size());testPoint++)
		{
			const JGTL::Vector2<float> &curPoint = points[testPoint];

			(derivative3)(result.A,result.B,result.C,curPoint.x,deriv(testPoint,0),deriv(testPoint,1),deriv(testPoint,2));
		}

		ublas::matrix<float> derivTranspose = trans(deriv);

		//mat_a = derivTranspose * deriv
		ublas::matrix<float> mat_a = prod(derivTranspose,deriv);

		//mat_b = derivTranspose * db
		ublas::matrix<float> mat_b = prod(derivTranspose,delta);

		//a*(delta_vector) = b, so solve for delta_vector
		ublas::matrix<float> mat_a_inverse(3,3);
		bool retval = InvertMatrix(mat_a,mat_a_inverse);

		if(retval)
		{
			ublas::matrix<float> delta_vector = prod(mat_a_inverse,mat_b);

			//cout << "Delta A: " << delta_vector(0,0) << endl;
			//cout << "Delta B: " << delta_vector(1,0) << endl;
			//cout << "Delta C: " << delta_vector(2,0) << endl;

			result.A += delta_vector(0,0);
			result.B += delta_vector(1,0);
			result.C += delta_vector(2,0);

			//cout << "New A: " << result.A << endl;
			//cout << "New B: " << result.B << endl;
			//cout << "New C: " << result.C << endl;
		}
		else
		{
			//cout << "INVERSE FAILED!\n";
			break;
		}
	}

	//cout << "R2: " << result.r2 << endl;

	return result;
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <gtest/gtest.h>

#include <Eigen/Sparse>

#include "smooth/optim.hpp"
#include "smooth/so3.hpp"

template<int N, int M>
void run_leastsquares_test(bool zero_d, bool sing)
{
  // static
  for (auto i = 0u; i != 10; ++i) {
    Eigen::Matrix<double, M, N> J;
    Eigen::Matrix<double, M, 1> r;
    Eigen::Matrix<double, N, 1> d;

    J.setRandom();
    if (sing) {
      J.col(N / 2).setZero();
      J.row(M / 2).setZero();
    }

    d.setRandom();
    d = (d + Eigen::Matrix<double, N, 1>::Ones()).cwiseMax(0);
    if (zero_d) { d.setZero(); }

    r.setRandom();

    // solve static
    Eigen::ColPivHouseholderQR<decltype(J)> J_qr(J);
    auto a1 = smooth::detail::solve_ls<N, M>(J_qr, d, r);

    // solve dynamic
    Eigen::Matrix<double, -1, -1> Jd = J;
    Eigen::Matrix<double, -1, 1> rd  = r;
    Eigen::Matrix<double, -1, 1> dd  = d;
    Eigen::ColPivHouseholderQR<decltype(Jd)> Jd_qr(Jd);
    auto a2 = smooth::detail::solve_ls<-1, -1>(Jd_qr, dd, rd);

    // solve sparse
    Eigen::SparseMatrix<double> Jsp;
    Jsp = J.sparseView();
    Eigen::SparseQR<decltype(Jsp), Eigen::COLAMDOrdering<int>> Jsp_qr(Jsp);
    auto a3 = smooth::detail::solve_ls<-1, -1>(Jsp_qr, dd, rd);

    // solve for precise solution
    Eigen::Matrix<double, N + M, N> lhs;
    lhs.template topLeftCorner<M, N>()    = J;
    lhs.template bottomLeftCorner<N, N>() = d.asDiagonal();

    Eigen::Matrix<double, N + M, 1> rhs;
    rhs.template head<M>() = -r;
    rhs.template tail<N>().setZero();
    Eigen::Matrix<double, N, 1> a_verif = lhs.fullPivHouseholderQr().solve(rhs);

    // verify that all methods gave the same result
    ASSERT_TRUE(a1.isApprox(a_verif));
    ASSERT_TRUE(a2.isApprox(a_verif));
    ASSERT_TRUE(a3.isApprox(a_verif));
  }
}

TEST(Optimization, LeastSquares)
{
  run_leastsquares_test<1, 1>(false, false);
  run_leastsquares_test<5, 1>(false, false);
  run_leastsquares_test<5, 10>(false, false);
  run_leastsquares_test<8, 16>(false, false);

  run_leastsquares_test<1, 1>(false, true);
  run_leastsquares_test<5, 1>(false, true);
  run_leastsquares_test<5, 10>(false, true);
  run_leastsquares_test<8, 16>(false, true);

  run_leastsquares_test<1, 1>(true, false);
  run_leastsquares_test<5, 10>(true, false);
  run_leastsquares_test<8, 16>(true, false);

  run_leastsquares_test<5, 10>(true, true);
  run_leastsquares_test<8, 16>(true, true);
}

TEST(Optimization, LmPar)
{
  constexpr int M = 4, N = 4;
  Eigen::Matrix<double, M, N> J;
  Eigen::Matrix<double, M, 1> r;
  Eigen::Matrix<double, N, 1> d;

  double Delta = 1;

  for (auto i = 0u; i != 10; ++i) {
    J.setRandom();

    d.setRandom();
    d = d + Eigen::Matrix<double, N, 1>::Ones();

    r.setRandom();

    // solve static
    auto [par1, x] = smooth::detail::lmpar<4, 4>(J, d, r, Delta);

    // solve dynamic
    Eigen::MatrixXd Jd = J;
    Eigen::VectorXd rd = r;
    Eigen::VectorXd dd = d;
    auto [par2, xd]    = smooth::detail::lmpar<-1, -1>(Jd, dd, rd, Delta);

    // solve sparse1
    Eigen::SparseMatrix<double> Jsp;
    Jsp               = J.sparseView();
    auto [par3, xsp1] = smooth::detail::lmpar<-1, -1>(Jsp, dd, rd, Delta);

    // solve sparse2
    auto [par4, xsp2] = smooth::detail::lmpar_sparse(Jsp, dd, rd, Delta);

    // check equality of static and dynamic
    ASSERT_NEAR(par1, par2, 1e-10);
    ASSERT_NEAR(par1, par3, 1e-10);
    ASSERT_NEAR(par1, par4, 1e-10);
    ASSERT_TRUE(x.isApprox(xd));
    ASSERT_TRUE(x.isApprox(xsp1));
    ASSERT_TRUE(x.isApprox(xsp2));

    // check that x solves resulting problem
    Eigen::ColPivHouseholderQR<decltype(J)> J_qr(J);
    auto x_test = smooth::detail::solve_ls<N, N>(J_qr, sqrt(par1) * d, r);
    ASSERT_TRUE(x_test.isApprox(x));

    // check that parameter satisfies conditions
    bool cond1 = (par1 == 0) && (d.asDiagonal() * x).norm() <= 1.1 * Delta;
    bool cond2 = (par1 > 0) && std::abs((d.asDiagonal() * x).norm() - Delta) <= 0.1 * Delta;
    ASSERT_TRUE(cond1 || cond2);
  }
}

TEST(Optimization, LmParSmall)
{
  constexpr int M = 4, N = 4;
  Eigen::Matrix<double, M, N> J;
  Eigen::Matrix<double, M, 1> r;
  Eigen::Matrix<double, N, 1> d;

  double Delta = 0.1;
  for (auto i = 0u; i != 10; ++i) {
    J.setRandom();

    d.setRandom();
    d = d + Eigen::Matrix<double, N, 1>::Ones();

    r.setRandom();

    // solve static
    auto [par1, x] = smooth::detail::lmpar<4, 4>(J, d, r, Delta);

    // solve dynamic
    Eigen::MatrixXd Jd = J;
    Eigen::VectorXd rd = r;
    Eigen::VectorXd dd = d;
    auto [par2, xd]    = smooth::detail::lmpar<-1, -1>(Jd, dd, rd, Delta);

    // solve sparse
    Eigen::SparseMatrix<double> Jsp;
    Jsp               = J.sparseView();
    auto [par3, xsp1] = smooth::detail::lmpar<-1, -1>(Jsp, dd, rd, Delta);

    // solve sparse2
    auto [par4, xsp2] = smooth::detail::lmpar_sparse(Jsp, dd, rd, Delta);

    // check equality of static and dynamic
    ASSERT_NEAR(par1, par2, 1e-10);
    ASSERT_NEAR(par1, par3, 1e-10);
    ASSERT_NEAR(par1, par4, 1e-10);
    ASSERT_TRUE(x.isApprox(xd));
    ASSERT_TRUE(x.isApprox(xsp1));
    ASSERT_TRUE(x.isApprox(xsp2));

    // check that x solves resulting problem
    Eigen::ColPivHouseholderQR<decltype(J)> J_qr(J);
    auto x_test = smooth::detail::solve_ls<N, N>(J_qr, sqrt(par1) * d, r);
    ASSERT_TRUE(x_test.isApprox(x));

    // check that parameter satisfies conditions
    bool cond1 = (par1 == 0) && (d.asDiagonal() * x).norm() <= 1.1 * Delta;
    bool cond2 = (par1 > 0) && std::abs((d.asDiagonal() * x).norm() - Delta) <= 0.1 * Delta;
    ASSERT_TRUE(cond1 || cond2);
  }
}

TEST(Optimization, LmParSing)
{
  constexpr int M = 4, N = 4;
  Eigen::Matrix<double, M, N> J;
  Eigen::Matrix<double, M, 1> r;
  Eigen::Matrix<double, N, 1> d;

  double Delta = 1;
  for (auto i = 0u; i != 10; ++i) {
    J.setRandom();
    J.col(3).setZero();

    d.setRandom();
    d = d + Eigen::Matrix<double, N, 1>::Ones();

    r.setRandom();

    // solve QR
    auto [par1, x] = smooth::detail::lmpar<4, 4>(J, d, r, Delta);

    // solve dynamic
    Eigen::MatrixXd Jd = J;
    Eigen::VectorXd rd = r;
    Eigen::VectorXd dd = d;
    auto [par2, xd]    = smooth::detail::lmpar<-1, -1>(Jd, dd, rd, Delta);

    // solve sparse1
    Eigen::SparseMatrix<double> Jsp;
    Jsp               = J.sparseView();
    auto [par3, xsp1] = smooth::detail::lmpar<-1, -1>(Jsp, dd, rd, Delta);

    // solve sparse2
    auto [par4, xsp2] = smooth::detail::lmpar_sparse(Jsp, dd, rd, Delta);

    // check equality of static and dynamic
    ASSERT_NEAR(par1, par2, 1e-10);
    ASSERT_NEAR(par1, par3, 1e-10);
    ASSERT_TRUE(x.isApprox(xd));
    ASSERT_TRUE(x.isApprox(xsp1));

    // check that x solves resulting problem
    Eigen::ColPivHouseholderQR<decltype(J)> J_qr(J);
    auto x_test = smooth::detail::solve_ls<N, N>(J_qr, sqrt(par1) * d, r);

    ASSERT_TRUE(x_test.isApprox(x));

    // check that parameter satisfies conditions
    bool cond1 = (par1 == 0) && (d.asDiagonal() * x).norm() <= 1.1 * Delta;
    bool cond2 = (par1 > 0) && std::abs((d.asDiagonal() * x).norm() - Delta) <= 0.1 * Delta;
    ASSERT_TRUE(cond1 || cond2);

    // don't expect sparse2 to be equal
    // check that parameter satisfies conditions
    bool cond3 = (par4 == 0) && (d.asDiagonal() * xsp2).norm() <= 1.1 * Delta;
    bool cond4 = (par4 > 0) && std::abs((d.asDiagonal() * xsp2).norm() - Delta) <= 0.1 * Delta;
    ASSERT_TRUE(cond3 || cond4);
  }
}

TEST(NLS, MultipleArgsStatic)
{
  smooth::SO3d g1, g2;
  g1.setRandom();
  g2.setRandom();

  auto f = [](auto v1, auto v2) {
    Eigen::Vector3d diff = (v1 - v2) - Eigen::Vector3d::Ones();
    Eigen::Matrix<double, 9, 1> ret;
    ret << v1.log(), v2.log(), diff;
    return ret;
  };

  smooth::MinimizeOptions opts;
  opts.ftol    = 1e-12;
  opts.ptol    = 1e-12;
  opts.verbose = true;

  smooth::minimize(f, smooth::wrt(g1, g2), opts);

  ASSERT_TRUE(g1.inverse().isApprox(g2, 1e-6));
}

TEST(NLS, MultipleArgsDynamic)
{
  smooth::SO3d g1, g2;
  g1.setRandom();
  g2.setRandom();

  auto f = [](auto v1, auto v2) -> Eigen::VectorXd {
    Eigen::VectorXd diff = (v1 - v2) - Eigen::Vector3d::Ones();
    Eigen::Matrix<double, 9, 1> ret;
    ret << v1.log(), v2.log(), diff;
    return ret;
  };

  smooth::MinimizeOptions opts;
  opts.ftol    = 1e-12;
  opts.ptol    = 1e-12;
  opts.verbose = true;

  smooth::minimize(f, smooth::wrt(g1, g2), opts);

  ASSERT_TRUE(g1.inverse().isApprox(g2, 1e-6));
}

TEST(NLS, MixedArgs)
{
  smooth::SO3d g0, g1;
  Eigen::VectorXd v(3);
  g0.setRandom();
  g1.setRandom();
  v.setRandom();

  auto f = [&](auto var_g, auto var_vec) -> Eigen::VectorXd {
    Eigen::Matrix<double, -1, 1> ret(6);
    ret << (var_g + var_vec.template head<3>()) - g0, var_vec - Eigen::Vector3d::Ones();
    return ret;
  };

  smooth::minimize(f, smooth::wrt(g1, v));

  auto g1_plus_v = g1 + v.head<3>();
  ASSERT_TRUE(g1_plus_v.isApprox(g0, 1e-6));
  ASSERT_TRUE(v.isApprox(Eigen::Vector3d::Ones(), 1e-6));
}

struct AnalyticSparseFunctor
{
  template<typename T>
  Eigen::VectorX<T>
  operator()(const smooth::SO3<T> & g1, const smooth::SO3<T> & g2, const smooth::SO3<T> & g3)
  {
    Eigen::VectorX<T> f(9);
    f.template segment<3>(0) = g1.log();
    f.template segment<3>(3) = (g3 - g2) - d23;
    f.template segment<3>(6) = (g1 - g3) - d31;
    return f;
  }

  Eigen::SparseMatrix<double>
  jacobian(const smooth::SO3d & g1, const smooth::SO3d & g2, const smooth::SO3d & g3) const
  {
    const Eigen::Matrix3d dr_f1_g1 = smooth::SO3d::dr_expinv(g1.log());

    const Eigen::Matrix3d dr_f2_g3 = smooth::SO3d::dr_expinv(g3 - g2);
    const Eigen::Matrix3d dr_f2_g2 = -smooth::SO3d::dl_expinv(g3 - g2);

    const Eigen::Matrix3d dr_f3_g1 = smooth::SO3d::dr_expinv(g1 - g3);
    const Eigen::Matrix3d dr_f3_g3 = -smooth::SO3d::dl_expinv(g1 - g3);

    Eigen::SparseMatrix<double> dr_f;
    dr_f.resize(9, 9);
    for (int i = 0; i != 3; ++i) {
      for (int j = 0; j != 3; ++j) {
        dr_f.insert(i, j) = dr_f1_g1(i, j);

        dr_f.insert(3 + i, 3 + j) = dr_f2_g2(i, j);
        dr_f.insert(3 + i, 6 + j) = dr_f2_g3(i, j);

        dr_f.insert(6 + i, 6 + j) = dr_f3_g3(i, j);
        dr_f.insert(6 + i, 0 + j) = dr_f3_g1(i, j);
      }
    }
    dr_f.makeCompressed();

    return dr_f;
  }

  Eigen::Vector3d d23, d31;
};

TEST(NLS, AnalyticSparse)
{
  auto f = AnalyticSparseFunctor{Eigen::Vector3d::Random(), Eigen::Vector3d::Random()};

  smooth::SO3d g1, g2, g3;
  g1.setRandom();
  g2.setRandom();
  g3.setRandom();

  auto g1c = g1;
  auto g2c = g2;
  auto g3c = g3;

  // solve with analytic diff
  smooth::minimize<smooth::diff::Type::Analytic>(f, smooth::wrt(g1, g2, g3));

  // solve with default autodiff
  smooth::minimize<smooth::diff::Type::Default>(f, smooth::wrt(g1c, g2c, g3c));

  ASSERT_TRUE(g1.isApprox(g1c, 1e-5));
  ASSERT_TRUE(g2.isApprox(g2c, 1e-5));
  ASSERT_TRUE(g3.isApprox(g3c, 1e-5));
}


/**
 * @file vortex.cc
 * @brief Solve the lid driven cavity experiment on a nonuniform mesh
 */

#include <build_system_matrix.h>
#include <lf/assemble/dofhandler.h>
#include <lf/io/gmsh_reader.h>
#include <lf/io/vtk_writer.h>
#include <lf/mesh/entity.h>
#include <lf/mesh/hybrid2d/mesh_factory.h>
#include <lf/mesh/utils/tp_triag_mesh_builder.h>
#include <lf/quad/quad.h>
#include <lf/refinement/refinement.h>
#include <piecewise_const_element_matrix_provider.h>
#include <piecewise_const_element_vector_provider.h>
#include <solution_to_mesh_data_set.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cassert>
#include <filesystem>

/**
 * @brief Solves the lid driven cavity problem on a domain [0,100]x[0,100]
 * @param mesh A shared pointer to the mesh on which to solve the PDE
 * @param dofh The dofhandler to use for the simulation
 * @param modified If true, use the modified penalty term
 * otherwise use the original one
 * @returns A vector containing the basis function coefficients of the solution
 */
Eigen::VectorXd solveLidDrivenCavity(
    const std::shared_ptr<const lf::mesh::Mesh> &mesh,
    const lf::assemble::DofHandler &dofh, bool modified = false) {
  // No volume forces are present in this experiment
  auto f = [](const Eigen::Vector2d & /*unused*/) -> Eigen::Vector2d {
    return Eigen::Vector2d::Zero();
  };
  // The top lid is driven with velocity 1
  auto dirichlet_funct = [](const lf::mesh::Entity &edge) -> Eigen::Vector2d {
    static constexpr double eps = 1e-10;
    const auto *const geom = edge.Geometry();
    const auto vertices = geom->Global(edge.RefEl().NodeCoords());
    Eigen::Vector2d v;
    v << 1. / 100, 0;
    if (vertices(1, 0) <= 100 + eps && vertices(1, 0) >= 100 - eps &&
        vertices(1, 1) <= 100 + eps && vertices(1, 1) >= 100 - eps) {
      return v;
    }
    return Eigen::Vector2d::Zero();
  };

  // Solve the LSE using sparse cholesky
  const auto [A, rhs] =
      projects::ipdg_stokes::assemble::buildSystemMatrixNoFlow(
          mesh, dofh, f, dirichlet_funct, 1,
          lf::quad::make_TriaQR_MidpointRule(), modified);
  Eigen::SparseMatrix<double> As = A.makeSparse();
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(As);
  return solver.solve(rhs);
}

/**
 * @brief stores the solution of the lid driven cavity experiment to a vtk file
 */
int main() {
  const double mu = 1;
  const double sigma = 1;
  const double rho = 1;

  // Load the mesh
  std::filesystem::path meshpath = __FILE__;
  meshpath = meshpath.parent_path() / "mesh.msh";
  std::unique_ptr<lf::mesh::MeshFactory> factory =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(factory), meshpath.string());
  auto mesh = reader.mesh();

  const auto boundary = lf::mesh::utils::flagEntitiesOnBoundary(mesh);

  lf::assemble::UniformFEDofHandler dofh(
      mesh, {{lf::base::RefEl::kPoint(), 1}, {lf::base::RefEl::kSegment(), 1}});
  std::cout << "solving original" << std::endl;
  const Eigen::VectorXd solution_original =
      solveLidDrivenCavity(mesh, dofh, false);
  std::cout << "extracting original" << std::endl;
  const auto c_original =
      projects::ipdg_stokes::post_processing::extractBasisFunctionCoefficients(
          mesh, dofh, solution_original);
  const auto v_original =
      projects::ipdg_stokes::post_processing::extractVelocity(
          mesh, dofh, solution_original);
  std::cout << "solving modified" << std::endl;
  const Eigen::VectorXd solution_modified =
      solveLidDrivenCavity(mesh, dofh, true);
  std::cout << "extracting modified" << std::endl;
  const auto c_modified =
      projects::ipdg_stokes::post_processing::extractBasisFunctionCoefficients(
          mesh, dofh, solution_modified);
  const auto v_modified =
      projects::ipdg_stokes::post_processing::extractVelocity(
          mesh, dofh, solution_modified);

  std::cout << "writing" << std::endl;
  lf::io::VtkWriter writer(mesh, "vortex.vtk");
  writer.WritePointData("coefficients_original", c_original);
  writer.WritePointData("coefficients_modified", c_modified);
  writer.WriteCellData("velocity_original", v_original);
  writer.WriteCellData("velocity_modified", v_modified);

  return 0;
}


/**
 * Author: Dominic Liao-McPherson
 * Contact: dliaomcp@umich.edu
 *
 * This file is part of the static-linalg library.
 * Copyright (C) 2018-2019 University of Michigan.
 *
 * This software is distributed under the BSD-3-Clause license.
 * You should have received a LICENSE file along with this program.
 * If not see: <https://opensource.org/licenses/BSD-3-Clause>
 */

#include "StaticMatrix.h"
#include "TestingUtils.h"
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;
using namespace testutils;

// TODO: Overhaul for its google test based
int main(){

	// test assignment *************************************
	double *a1 = new double[9];
	double d1[] = {0.5,2,3};
	StaticMatrix A(a1,3,3);
	StaticMatrix d(d1,3,1);
	A.rand();
	A(2,2) = 100;
	A.tril();

	// test reshaping and slicing *************************************

	cout << "A:\n";
	cout << A << endl;
	StaticMatrix Acol = A.col(0);
	cout << "First column of A:\n" << Acol << endl;
	StaticMatrix Arow = A.row(0);
	cout << "First row of A:\n" << Arow << endl;
	StaticMatrix ARE = A.getreshape(1,9);
	cout << "reshape(A, [1,9]) is:\n" << ARE << endl;
	Arow(2) = 4;
	Acol(1) = 6;
	cout << "After A(0,2) = 4, A(1,0) = 6 and column mods\n" << A << endl;

	A.ColScale(d);
	cout << "After column scaling\n" << A << endl;
	A.RowScale(d);
	cout << "After row scaling\n" << A << endl;
	MatrixXd A1(3,3);
	CopyEig(A,A1);

	// axpy *************************************
	double *b1 = new double[3];
	StaticMatrix x(b1,3);
	x.rand();

	double *b2 = new double[3];
	StaticMatrix y(b2,3);
	y.rand();

	double a = -1.0;

	MatrixXd x1(3,1);
	CopyEig(x,x1);

	MatrixXd y1(3,1);
	CopyEig(y,y1);

	y1 = y1 + a*x1;
	y.axpy(x,a);
	cout << "Testing y <- a*x + y, Error: \n";
	cout << DiffNorm(y,y1) << endl << endl;

	// gemv *************************************
	double b = 1;
	// not transposed
	A.rand();
	CopyEig(A,A1);
	x.rand();
	CopyEig(x,x1);
	y.rand();
	CopyEig(y,y1);

	cout << "Testing y <- a*A*x + b*y, Error: \n";
	y1 = a*A1*x1 + b*y1;
	y.gemv(A,x,a,b);

	cout << DiffNorm(y,y1) << "\n\n";

	// transposed
	cout << "Testing y <- a*A'*x + b*y, Error: \n";
	double *a2 = new double[6];
	StaticMatrix B(a2,2,3);
	B.rand();
	MatrixXd B1(2,3);
	CopyEig(B,B1);

	double *b3 = new double[2];
	StaticMatrix z(b3,2);
	z.rand();
	y.rand();
	CopyEig(y,y1);

	MatrixXd z1(2,1);
	CopyEig(z,z1);

	y1 = a*(B1.transpose())*z1 + b*y1;
	y.gemv(B,z,a,0.0,true);
	cout << DiffNorm(y,y1) << "\n\n";

	// test dot and norms *************************************
	z1 = x1.transpose()*y1;
	cout << "Testing dot(x,y), error is: ";
	cout << abs(StaticMatrix::dot(x,y) - z1(0)) << "\n\n";

	cout << "Testing 2 norm, error is :";
	cout << B.norm() - B1.norm() << "\n\n";
	cout << "Testing 1 norm: ";
	cout << B.asum() << "\n\n";

	// free memory
	delete[] a1;
	delete[] a2;
	delete[] b1;
	delete[] b2;
	delete[] b3;

	// gemm *************************************

	a1 = new double[9];
	b1 = new double[6];
	double* c1 = new double[6];
	A.map(a1,3,3);
	B.map(b1,3,2);
	StaticMatrix C(c1,3,2);

	MatrixXd AA(3,3);
	MatrixXd BB(3,2);
	MatrixXd CC(3,2);

	A.rand();
	B.rand();
	C.rand();
	CopyEig(C,CC);
	CopyEig(A,AA);
	CopyEig(B,BB);

	cout << "Testing C = aAB+bC, error:";
	C.gemm(A,B,a,b);
	CC = a*AA*BB + b*CC;
	cout << DiffNorm(C,CC) << "\n\n";

	// transA
	cout << "Testing C = a A'B + bC, error:";
	C.gemm(A,B,a,b,true);
	CC = a*(AA.transpose())*BB + b*CC;
	cout << DiffNorm(C,CC) << "\n\n";

	// trans B
	delete[] a1;
	delete[] b1;
	a1 = new double[3];
	b1 = new double[2];
	A.map(a1,3,1);
	B.map(b1,2,1);
	A.rand();
	B.rand();
	AA.resize(3,1);
	BB.resize(2,1);
	CopyEig(C,CC);
	CopyEig(A,AA);
	CopyEig(B,BB);

	CC = a*AA*(BB.transpose()) + b*CC;
	C.gemm(A,B,a,b,false,true);

	cout << "Testing C = a AB' + bC, Error: " << DiffNorm(C,CC) << "\n\n";

	delete[] a1;
	delete[] b1;

	// trans A,B
	a1 = new double[6];
	b1 = new double[4];
	A.map(a1,2,3);
	B.map(b1,2,2);
	A.rand();
	B.rand();

	AA.resize(2,3);
	BB.resize(2,2);
	CopyEig(C,CC);
	CopyEig(A,AA);
	CopyEig(B,BB);
	CC = a*(AA.transpose())*(BB.transpose()) + b*CC;
	C.gemm(A,B,a,b,true,true);

	cout << "Testing C = a A'B' + bC, Error: " << DiffNorm(C,CC) << "\n\n";

	delete[] a1;
	delete[] b1;
	delete[] c1;

	// Symmetric products *************************************

	a1 = new double[10];
	b1 = new double[5];
	c1 = new double[4];

	A.map(a1,5,2);
	C.map(c1,2,2);
	A.rand();
	AA.resize(5,2);
	CC.resize(2,2);

	CopyEig(C,CC);
	CopyEig(A,AA);

	CC = (AA.transpose())*AA + CC;
	C.gram(A);
	cout << "Testing C = A'*A, Error: " << DiffNorm(C,CC) << "\n\n";

	B.map(b1,5,1);
	B.rand();
	BB.resize(5,5);
	BB.fill(0);
	for(int i = 0; i< 5;i++)
		BB(i,i) = B(i);

	CC = (AA.transpose())*BB*AA + CC;
	C.gram(A,B);
	cout << "Testing C = A'*diag(B)*A + C, Error: " << DiffNorm(C,CC) << "\n\n";

	delete[] a1;
	delete[] b1;

	// Cholesky factorization *************************************
	double lmem[] = {3.8966,2.1881,1.1965,2.1551,
    2.1881,2.9966,0.6827,1.8861,
    1.1965,    0.6827,    1.7590,    0.5348,
    2.1551,    1.8861,    0.5348,    3.0955};

    A.map(lmem,4,4);
    Map<MatrixXd> AL(lmem,4,4);
    LLT<MatrixXd> LLTA(AL);
    A.llt();
    MatrixXd L = LLTA.matrixLLT();

    cout << "Testing Cholesky Factorizaton, Error: ";
    cout << DiffNorm(A,L) << endl;
    for(int j = 0;j<4;j++)
    	for(int i = 0;i< j;i++)
    		L(i,j) = 0.0;

	// Back solves *************************************

    double b4[] = {0.6541,    0.6892,    0.7482,    0.4505,
    0.0838,    0.2290,    0.9133,    0.1524,
    0.8258,    0.5383,    0.9961,    0.0782};
    B.map(b4,4,3);
    MatrixXd BE(3,4);
    BE << 0.6541,    0.6892,    0.7482,    0.4505,
    0.0838,    0.2290,    0.9133,    0.1524,
    0.8258,    0.5383,    0.9961,    0.0782;
    BE.transposeInPlace();

    //*************************************
    cout << "Testing B <- inv(L)*B, A = LL' \n";

    BE = (L.inverse())*BE;
    B.LeftCholApply(A);
    cout << "Error: " << DiffNorm(B,BE) << endl << endl;

    //*************************************
    cout << "Testing B <- inv(L)'*B, A = LL' \n";

    B.LeftCholApply(A,true);
    BE = ((L.inverse()).transpose())*BE;

    cout << "Error : " << DiffNorm(B,BE) << endl << endl;

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

    B.reshape(3,4);
    BE.resize(3,4);

    cout << "Testing B <- B*inv(L) \n";
    B.RightCholApply(A);
    BE = BE*(L.inverse());

    cout << "Error : " << DiffNorm(B,BE) << endl << endl;

    // *************************************
    cout << "Testing B <- B*inv(L)' \n";

    B.RightCholApply(A,true);
    BE = BE*((L.inverse()).transpose());

    cout << "Error : " << DiffNorm(B,BE) << endl << endl;

	return 0;
}


/**
 * \file dcs/math/detail/float.hpp
 *
 * \brief Utilities for floating-point comparison.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2012 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_DETAIL_FLOAT_HPP
#define DCS_MATH_DETAIL_FLOAT_HPP

#include <algorithm>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/utility/enable_if.hpp>
//#include <cfloat>
#include <cmath>
#include <cstdlib>
#include <limits>

namespace dcs { namespace math { namespace detail {

// See also:
// - https://adtmag.com/Articles/2000/03/16/Comparing-Floats-How-To-Determine-if-Floating-Quantities-Are-Close-Enough-Once-a-Tolerance-Has-Been.aspx
// - https://adtmag.com/articles/2000/03/16/comparing-floatshow-to-determine-if-floating-quantities-are-close-enough-once-a-tolerance-has-been-r.aspx
// - https://bitbashing.io/comparing-floats.html
// - http://code.google.com/p/googletest/source/browse/trunk/include/gtest/internal/gtest-internal.h
// - https://docs.python.org/3/tutorial/floatingpoint.html#tut-fp-issues
// - http://floating-point-gui.de/errors/comparison/
// - http://fcmp.sourceforge.net/
// - http://grouper.ieee.org/groups/754/
// - https://github.com/google/googletest/blob/master/googletest/docs/AdvancedGuide.md#Floating-Point_Comparison
// - http://learningcppisfun.blogspot.com/2010/04/comparing-floating-point-numbers.html
// - https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
// - https://stackoverflow.com/questions/1343890/rounding-number-to-2-decimal-places-in-c
// - https://twistedape.me.uk/2016/02/02/comparing-floating-point-numbers/
// . http://www.boost.org/doc/libs/release/libs/math/doc/html/math_toolkit/float_comparison.html
// - http://www.boost.org/doc/libs/release/libs/test/doc/html/boost_test/testing_tools/extended_comparison/floating_point.html
// - https://www.codeproject.com/Articles/383871/Demystify-Csharp-floating-point-equality-and-relat
// - https://www.gnu.org/software/libc/manual/html_node/Floating-Point-Parameters.html
// - https://www.gnu.org/software/gsl/doc/html/math.html
// - http://www.parashift.com/c++-faq-lite/newbie.html#faq-29.16
// - http://www.petebecker.com/js/js200012.html
// .
//

/**
 * \brief Perform approximate floating-point comparison.
 *
 * This function determines whether \a x and \a y are approximately equal to a
 * relative accuracy \a epsilon.
 *
 * To perform the approximate floating-point comparison, this function
 * implements the algorithm proposed by D.E. Knuth in Section 4.2.2 of (Knuth,1997) (see REFERENCES).
 *
 * The relative accuracy is measured using an interval of size \f$2 \delta\f$,
 * where \f$\delta = 2^k \epsilon\f$ and \f$k\f$ is the maximum base-2 exponent
 * of \a x and \a y as computed by the function `std::frexp`.
 *
 * If \a x and \a y lie within this interval, they are considered approximately
 * equal and the function returns `0`.
 * Otherwise if \a x < \a y, the function returns `-1`, or if \a x > \a y, the
 * function returns `+1`.
 *
 * \note \a x and \a y are compared to relative accuracy, so this function is
 *       not suitable for testing whether a value is approximately zero.
 *       Also, this function may not work correctly with degenerate cases.
 *       For instance, when both \a x and \a y are NaN, this function return 0,
 *       which is incorrect because, according to the IEEE 754 standard, NaN is
 *       always different from any floating-point number including itself
 *       (indeed, NaN is "unordered").
 *
 * The implementation is based on the one provided by the GNU Scientific Library
 * (GSL) which in turns is based on the package fcmp by T.C. Belding.
 *
 * \copyright GNU Scientific Library (GSL) 2.4 Copyright (c) 2002 Gert Van den Eynde (https://www.gnu.org/software/gsl)
 * \copyright fcmp 1.2.2 Copyright (c) 1998-2000 Theodore C. Belding, University of Michigan Center for the Study of Complex Systems (Ted.Belding@umich.edu)
 *
 * REFERENCES
 * - D.E. Knuth "The Art of Computer Programming, Volume 2: Seminumerical Algorithms, 3rd Edition," Addison-Wesley, 1997.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	int
>::type fcmp(const T x, const T y, const T epsilon)
{
	// Find exponent of largest absolute value

	const T max = (std::fabs(x) > std::fabs(y)) ? x : y;

	int exponent;

	std::frexp(max, &exponent);

	// Form a neighborhood of size  2 * delta

	const T delta = std::ldexp(epsilon, exponent);

	const T difference = x - y;

	if (difference > delta) // x > y
	{
		return 1;
	}
	if (difference < -delta) // x < y
	{
		return -1;
	}
	// -delta <= difference <= delta => x ~=~ y
	return 0;
}

/**
 * \brief x is approximately equal to y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \approx y\,\text{ if and only if } |y-x|\le\epsilon\max(e_x,e_y)
 * \f]
 * where \f$e_x\f$ and \f$e_y\f$ are the exponent of \f$x\f$ and \f$y\f$,
 * respectively.
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type approximately_equal(T x, T y, T tol)
{
	// Try first with standard comparison (handles the case when both x and y are zero or have other special values like inf or NaN)
	if (x == y)
	{
		// Tolerance is useless when both numbers are exactly the same
		return true;
	}

	// Handle degenerate cases
	//if (::std::isnan(x) || ::std::isinf(x) || ::std::isnan(y) || ::std::isinf(y))
	if (!::std::isfinite(x) || !::std::isfinite(y))
	{
		// Tolerance is useless when at least one number is not finite
		return x == y;
	}

	return fcmp(x, y, tol) == 0;
}

/**
 * \brief x is definitely equal to y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \sim y\,\text{ if and only if } |y-x|\le\epsilon\min(e_x,e_y)
 * \f]
 * where \f$e_x\f$ and \f$e_y\f$ are the exponent of \f$x\f$ and \f$y\f$,
 * respectively.
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type essentially_equal(T x, T y, T tol)
{
	// Try first with standard comparison (handles the case when both x and y are zero or have other special values like inf or NaN)
	if (x == y)
	{
		// Tolerance is useless when both numbers are exactly the same
		return true;
	}

	// Handle degenerate cases
	//if (::std::isnan(x) || ::std::isinf(x) || ::std::isnan(y) || ::std::isinf(y))
	if (!::std::isfinite(x) || !::std::isfinite(y))
	{
		// Tolerance is useless when at least one number is not finite
		return x == y;
	}

	// Check for numbers tha are very close to zero
	const T zero = 0;
	const T min_val = std::numeric_limits<T>::min();
	const T diff = ::std::fabs(x-y);
	if (x == zero || y == zero || diff < min_val)
	{
		// x or y is zero or both are extremely close to it
		// relative error is less meaningful here
		return diff < (tol*min_val);
	}

	// Otherwise, use the Knuth's method

	// - Find the min(x,y) and gets its exponent
	const T min = (std::fabs(x) < std::fabs(y)) ? x : y;
	int exponent = 0;
	std::frexp(min, &exponent);

	// - Form a neighborhood of size  2 * delta
	const T delta = std::ldexp(tol, exponent);
	const T difference = x - y;

	// - Now check if the number are very close to each other
	if (difference > delta      // x > y
		|| difference < -delta) // x < y
	{
		return false;
	}
	return true; // -delta <= difference <= delta => x ~=~ y
}

/**
 * \brief x is definitely greater than y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \succ y\,\text{ if and only if } x-y > \epsilon\max(e_x,e_y)
 * \f]
 * where \f$e_x\f$ and \f$e_y\f$ are the exponent of \f$x\f$ and \f$y\f$,
 * respectively.
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type definitely_greater(T x, T y, T tol)
{
	// Handle degenerate cases
	//if (::std::isnan(x) || ::std::isinf(x) || ::std::isnan(y) || ::std::isinf(y))
	if (!::std::isfinite(x) || !::std::isfinite(y))
	{
		// Tolerance is useless when at least one number is not finite
		return x > y;
	}

	return fcmp(x, y, tol) > 0;
}

/**
 * \brief x is definitely less than y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \prec y\,\text{ if and only if } y-x > \epsilon\max(e_x,e_y)
 * \f]
 * where \f$e_x\f$ and \f$e_y\f$ are the exponent of \f$x\f$ and \f$y\f$,
 * respectively.
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type definitely_less(T x, T y, T tol)
{
	// NOTE: don't use standard comparison operators because they do not take into account the given tolerance.
	//       For instance:
	//         x = 0.1233
	//         y = 0.1234
	//         -> x <  y if tol >= 1e-4
	//            but
	//            x == y if tol <  1e-3

	// Handle degenerate cases
	//if (::std::isnan(x) || ::std::isinf(x) || ::std::isnan(y) || ::std::isinf(y))
	if (!::std::isfinite(x) || !::std::isfinite(y))
	{
		// Tolerance is useless when at least one number is not finite
		return x < y;
	}

	return fcmp(x, y, tol) < 0;
}

}}} // Namespace dcs::math::detail

#endif // DCS_MATH_DETAIL_FLOAT_HPP


#pragma once

#include "definitions.hpp"

#include <Eigen/Dense>
#include <cmath>
#include <initializer_list>

inline RowVector vector_from_sequence(std::initializer_list<Real> seq)
{
    RowVector vec(seq.size());
    for (auto it = seq.begin(); it != seq.end(); ++it)
    {
        vec[std::distance(seq.begin(), it)] = *it;
    }
    return vec;
}

template <typename T>
inline Real squaredNorm(const T& vec)
{
    assert(vec.rows() == 1);
    return square(vec.array()).sum();
}
template <typename T>
inline Real norm(const T& vec)
{
    return std::sqrt(squaredNorm(vec));
}


#ifndef _UBLAS_
#define _UBLAS_

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

#endif

#ifndef _MY_UBLAS_
#define _MY_UBLAS_

using namespace boost::numeric::ublas;
using namespace boost::numeric;

template <class T>
unsigned int argmin(ublas::vector<T> v)
{ /* vの最小要素の添字を返す */
  if(v.empty()){ return -1; /* エラー */
  }else{
    unsigned int argmin = 0;
    T minval = v[argmin];
    for(unsigned int i = 0; i < v.size(); ++i){
      if(v[i] < minval){
	argmin = i; minval = v[i];
      }
    }
    return argmin;
  }
}

template <class T>
unsigned int argmin2 (ublas::vector<T> v)
{ /* vの2番目に小さい要素の添字を返す */
  if(v.size() < 2){ return -1; /* エラー */
  }else{
    unsigned int argmin1, argmin2;
    T min1, min2;
    if(v[0] <= v[1]){
      min1 = v[0]; argmin1 = 0;
      min2 = v[1]; argmin2 = 1;
    }else{
      min1 = v[1]; argmin1 = 1;
      min2 = v[0]; argmin2 = 0;
    }
    for(unsigned int i = 2; i < v.size(); ++i){
      if(v[i] <= min1){       /* 最小値が発見された */
	min2 = min1; argmin2 = argmin1;
	min1 = v[i]; argmin1 = i;
      }else if(v[i] <= min2){ /* 準最小値が発見された */
	min2 = v[i]; argmin2 = i;
      }
    }
    return argmin2;
  }
}

template <class T>
unsigned int argmax(ublas::vector<T> v){
  if(v.empty()){ return -1; /* エラー */
  }else{
    unsigned int argmax = 0; T maxval = v[argmax];
    for(unsigned int i = 0; i < v.size(); ++i){
      if(v[i] > maxval){
	argmax = i; maxval = v[i];
      }
    }
    return argmax;
  }
}

template <class T>
inline T min(ublas::vector<T> v){
  if(v.empty()){ return -1; /* エラー */
  }else{
    T minval = v[0];
    for(unsigned int i = 0; i < v.size(); ++i){
      if(v.at(i) < minval){
	minval = v.at(i);
      }
    }
    return minval;
  }
}

template <class T>
inline T min2(ublas::vector<T> v){
  /* vector vの要素で2番目に小さい要素を見つける */
  if(v.size() < 2){ return -1; /* エラー */
  }else{
    T min1 = v[0], min2 = v[1];
    if(min1 > min2){ /* swap する */
      T temp = min1; min1 = min2; min2 = temp;
    }
    for(unsigned int i = 2; i < v.size(); ++i){
      if(v[i] <= min1){       /* 最小値が発見された */
	min2 = min1; min1 = v[i];
      }else if(v[i] <= min2){ /* 準最小値が発見された */
	min2 = v[i];
      }
    }
    return min2;
  }
}

#if 0
ublas::vector<double> map /* ベクトルに関するmap */
(ublas::vector<double> source, function<double(double)> func)
{
  ublas::vector<double> result(source.size());
  for (unsigned int i = 0; i < source.size(); ++i){
    result[i]= func(source[i]);
  }
  return result;
}

ublas::matrix<double> map /* 行列に関するmap */
(ublas::matrix<double> source,
 function<ublas::vector<double>(ublas::vector<double>)> func)
{
  ublas::matrix<double> result(source.size1(), source.size2());
  for (unsigned int i = 0; i < source.size1(); ++i){
    row(result, i)= func(row(source,i));
  }
  return result;
}

int maptest()
{
  ublas::vector<double> vec(5, 0);
  vec[0] = 0.0;
  vec[1] = 1.1;
  vec[2] = -2.4;
  vec[3] = 3.5;
  vec[4] = 4.6;

  cout << vec << endl;

  cout <<  map(vec, [](double x){ return x + 1.0; }) << endl;
  return 0;
}
#endif

#endif


//  (C) Copyright John Maddock 2018.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/tools/series.hpp>
#include <iostream>
#include <complex>
#include <cassert>

//[series_log1p
template <class T>
struct log1p_series
{
   // we must define a result_type typedef:
   typedef T result_type;

   log1p_series(T x)
      : k(0), m_mult(-x), m_prod(-1) {}

   T operator()()
   {
      // This is the function operator invoked by the summation
      // algorithm, the first call to this operator should return
      // the first term of the series, the second call the second
      // term and so on.
      m_prod *= m_mult;
      return m_prod / ++k;
   }

private:
   int k;
   const T m_mult;
   T m_prod;
};
//]

//[series_log1p_func
template <class T>
T log1p(T x)
{
   // We really should add some error checking on x here!
   assert(std::fabs(x) < 1);

   // Construct the series functor:
   log1p_series<T> s(x);
   // Set a limit on how many iterations we permit:
   boost::uintmax_t max_iter = 1000;
   // Add it up, with enough precision for full machine precision:
   return boost::math::tools::sum_series(s, std::numeric_limits<T>::epsilon(), max_iter);
}
//]

//[series_clog1p_func
template <class T>
struct log1p_series<std::complex<T> >
{
   // we must define a result_type typedef:
   typedef std::complex<T> result_type;

   log1p_series(std::complex<T> x)
      : k(0), m_mult(-x), m_prod(-1) {}

   std::complex<T> operator()()
   {
      // This is the function operator invoked by the summation
      // algorithm, the first call to this operator should return
      // the first term of the series, the second call the second
      // term and so on.
      m_prod *= m_mult;
      return m_prod / T(++k);
   }

private:
   int k;
   const std::complex<T> m_mult;
   std::complex<T> m_prod;
};

template <class T>
std::complex<T> log1p(std::complex<T> x)
{
   // We really should add some error checking on x here!
   assert(abs(x) < 1);

   // Construct the series functor:
   log1p_series<std::complex<T> > s(x);
   // Set a limit on how many iterations we permit:
   boost::uintmax_t max_iter = 1000;
   // Add it up, with enough precision for full machine precision:
   return boost::math::tools::sum_series(s, std::complex<T>(std::numeric_limits<T>::epsilon()), max_iter);
}
//]

int main()
{
   using namespace boost::math::tools;

   std::cout << log1p(0.25) << std::endl;

   std::cout << log1p(std::complex<double>(0.25, 0.25)) << std::endl;

   return 0;
}


#ifndef KEY_GENERATOR_HPP
#define KEY_GENERATOR_HPP

#include <boost/multiprecision/cpp_int.hpp>
#include <ctime>
#include <iostream>
#include <random>
#include <stdexcept>

#include "big_int_type.hpp"
#include "euclidean.hpp"
#include "primality.hpp"

typedef struct key {
  big_int d_e;
  big_int n;
} Key;

typedef struct key_pair {
  Key pub_key;
  Key priv_key;
} Key_pair;

/*
 * Gera os valores de p, q, n, Phi(n), e, d.
 * @returns Um par de chaves, contendo uma chave pública e uma chave privada.
 */
Key_pair generate_keys(unsigned max_bits, bool print = false);

#endif