/*
 * <one line to give the library's name and an idea of what it does.>
 * Copyright (C) 2015  Guillaume L. <guillaume.lozenguez@mines-douai.fr>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "float2.h"
#include "tools.h"
#include <boost/concept_check.hpp>

using namespace mia;
using namespace std;

Float2 Float2 :: middle( const Float2 & A, const Float2 & B )
{
  return (A+B)*0.5f;
}

Float2 Float2 :: mean( const std::list<Float2> & lFloat2 )
{
    Float2 mean;
    int size(0);

    for( std::list<Float2>::const_iterator it= lFloat2.begin() ; it != lFloat2.end(); ++it ){
        mean+= *it;
        ++size;
    }
    return mean / (float)size;
}

std::array<Float2, 2> Float2::simpleLinearRegression( const std::list<Float2> & point )
{
    int size(0);
    Float2 mean(0.f, 0.f), origin(0.f, 0.f);

    for( std::list<Float2>::const_iterator it(point.begin()), itEnd(point.end()) ; it != itEnd ; ++it )
    {
        mean+= Float2(it->x, it->y);
        ++size;
    }
    mean/= (float)size;

    float slopeNum(0.f), yxSlopeDenum(0.f), xySlopeDenum(0.f);
    for( std::list<Float2>::const_iterator it(point.begin()), itEnd(point.end()) ; it != itEnd ; ++it )
    {
        float dx= (it->x-mean.x);
        float dy= (it->y-mean.y);

        slopeNum+= dx*dy;

        yxSlopeDenum+= dx*dx;
        xySlopeDenum+= dy*dy;

        ++size;
    }

    if( yxSlopeDenum == 0 && xySlopeDenum == 0 )
    {
        origin.x= mean.x;
        origin.y= 0.f;
    }
    else if( yxSlopeDenum*yxSlopeDenum > xySlopeDenum*xySlopeDenum )
    {
        float slope= slopeNum / yxSlopeDenum;
        origin.x= 0;
        origin.y= mean.y - slope * mean.x;
    }
    else
    {
        float slope= slopeNum / xySlopeDenum;
        origin.y= 0;
        origin.x= mean.x - slope * mean.y;
    }

    std::array<Float2, 2> descriptor= {mean, Float2(origin, mean)};
    descriptor[1].normalize();

    return descriptor;
}

std::array<Float2, 2> Float2::projectionSegment( const std::list<Float2> & point, const Float2 &mean, const Float2 &normDir )
{
    list<float> projection;
    for( list<Float2>::const_iterator it(point.begin()), itEnd(point.end()) ; it!=itEnd ; ++it )
        projection.push_back( dotProduct( (*it) - mean, normDir ) );
    projection.sort();

    array<Float2, 2> segment= { normDir * *(projection.begin()) + mean, normDir * *(projection.rbegin()) + mean };
    return segment;
}

bool Float2::validSegmentRegression( const std::list<Float2> & lFloat2, const std::array<Float2, 2> & normSegment, float treshold )
{
    Float2 normDir= normSegment[1].orthogonal();
    bool valid(true);

    for( list<Float2>::const_iterator it(lFloat2.begin()), itEnd(lFloat2.end()) ; valid && it!=itEnd ; ++it )
        valid= dotProduct( (*it) - normSegment[0], normDir ) < treshold;

    return valid;
}

list<Float2> Float2 :: polarSort(const list<Float2> & lFloat2)
{
    list< valued<Float2> > toSort;
    for(list<Float2>::const_iterator it = lFloat2.begin(); it != lFloat2.end() ; ++it )
        toSort.push_back( valued<Float2>( *it, it->angle() ) );
    toSort.sort();

    list<Float2> ret;
    for(list<valued<Float2>>::const_iterator it = toSort.begin(); it != toSort.end() ; ++it )
        ret.push_back( it->item );

    return ret;
}

Transform Transform::from_match ( std::list<std::pair<Float2, Float2>>::const_iterator itBegin,
                                     std::list<std::pair<Float2, Float2>>::const_iterator itEnd )
{ // Which translation / rotation to transfom second to first ?
    Transform t;
    t.translation= Float2(0.f, 0.f);
    t.center= Float2(0.f, 0.f);
    t.rotation= 0.f;
    int size(0);

    for( std::list<std::pair<Float2, Float2>>::const_iterator it= itBegin ; it != itEnd ; ++it )
    {// Get center :
        t.center+= it->first;
        t.translation+= it->second;
        ++size;
    }
    t.center/= (float)size; // Center first
    t.translation/= (float)size; // Center second
    t.translation= t.center - t.translation; // translation from second to first.

    // Rotation :
    for( std::list<std::pair<Float2, Float2>>::const_iterator it= itBegin ; it != itEnd ; ++it )
    {
        t.rotation+= angle( it->second + t.translation, t.center, it->first );
    }
    t.rotation= reduceRadian( t.rotation/(float)size );

    return t;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the krls object
    from the dlib C++ Library.

    The krls object allows you to perform online regression.  This
    example will use the krls object to perform filtering of a signal
    corrupted by uniformly distributed noise.
*/

#include <iostream>

#include <dlib/svm.h>
#include <dlib/rand.h>

using namespace std;
using namespace dlib;

// Here is the function we will be trying to learn with the krls
// object.
double sinc(double x)
{
    if (x == 0)
        return 1;

    // also add in x just to make this function a little more complex
    return sin(x)/x + x;
}

int main()
{
    // Here we declare that our samples will be 1 dimensional column vectors.  The reason for
    // using a matrix here is that in general you can use N dimensional vectors as inputs to the
    // krls object.  But here we only have 1 dimension to make the example simple.
    typedef matrix<double,1,1> sample_type;

    // Now we are making a typedef for the kind of kernel we want to use.  I picked the
    // radial basis kernel because it only has one parameter and generally gives good
    // results without much fiddling.
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Here we declare an instance of the krls object.  The first argument to the constructor
    // is the kernel we wish to use.  The second is a parameter that determines the numerical
    // accuracy with which the object will perform part of the regression algorithm.  Generally
    // smaller values give better results but cause the algorithm to run slower (because it tries
    // to use more "dictionary vectors" to represent the function it is learning.
    // You just have to play with it to decide what balance of speed and accuracy is right
    // for your problem.  Here we have set it to 0.001.
    //
    // The last argument is the maximum number of dictionary vectors the algorithm is allowed
    // to use.  The default value for this field is 1,000,000 which is large enough that you
    // won't ever hit it in practice.  However, here we have set it to the much smaller value
    // of 7.  This means that once the krls object accumulates 7 dictionary vectors it will
    // start discarding old ones in favor of new ones as it goes through the training process.
    // In other words, the algorithm "forgets" about old training data and focuses on recent
    // training samples. So the bigger the maximum dictionary size the longer its memory will
    // be.  But in this example program we are doing filtering so we only care about the most
    // recent data.  So using a small value is appropriate here since it will result in much
    // faster filtering and won't introduce much error.
    krls<kernel_type> test(kernel_type(0.05),0.001,7);

    dlib::rand rnd;

    // Now let's loop over a big range of values from the sinc() function.  Each time
    // adding some random noise to the data we send to the krls object for training.
    sample_type m;
    double mse_noise = 0;
    double mse = 0;
    double count = 0;
    for (double x = -20; x <= 20; x += 0.01)
    {
        m(0) = x;
        // get a random number between -0.5 and 0.5
        const double noise = rnd.get_random_double()-0.5;

        // train on this new sample
        test.train(m, sinc(x)+noise);

        // once we have seen a bit of data start measuring the mean squared prediction error.
        // Also measure the mean squared error due to the noise.
        if (x > -19)
        {
            ++count;
            mse += pow(sinc(x) - test(m),2);
            mse_noise += pow(noise,2);
        }
    }

    mse /= count;
    mse_noise /= count;

    // Output the ratio of the error from the noise and the mean squared prediction error.
    cout << "prediction error:                   " << mse << endl;
    cout << "noise:                              " << mse_noise << endl;
    cout << "ratio of noise to prediction error: " << mse_noise/mse << endl;

    // When the program runs it should print the following:
    //    prediction error:                   0.00735201
    //    noise:                              0.0821628
    //    ratio of noise to prediction error: 11.1756

    // And we see that the noise has been significantly reduced by filtering the points
    // through the krls object.

}


#include <ctime>
#include <vector>
#include <math.h>

#include <Eigen/Dense>
#include <Eigen/Core>

#include "mex.h"
#include "nlopt.hpp"

int counter = 0;

using namespace std;
using namespace Eigen;
using Eigen::MatrixXd;

struct mFuncData
{
	int numDims, numSamples;
	MatrixXd X, Y;
	MatrixXd XX, YX;
};

double myfunc(unsigned n, const double *inW, double *grad, void *inData)
{
	++counter;

	mFuncData* data = (mFuncData*)inData;
	MatrixXd W = Map<MatrixXd>((double*)inW, data->numDims, data->numDims);

	// 2 * W*(X*X') - Y*X' - Y*X';
	if (grad)
	{
		clock_t beginM = clock();

		mwSize size[2]; size[0] = data->numDims;  size[1] = data->numDims;
		mxArray* prhs[2];
		prhs[0] = mxCreateNumericArray(2, size, mxDOUBLE_CLASS, mxREAL);
		double* WMatlab = (double*)mxGetData(prhs[0]);
		Map<MatrixXd>(WMatlab, data->numDims, data->numDims) = W;
		prhs[1] = mxCreateNumericArray(2, size, mxDOUBLE_CLASS, mxREAL);
		double* XMatlab = (double*)mxGetData(prhs[1]);
		Map<MatrixXd>(XMatlab, data->numDims, data->numDims) = data->XX;

		mxArray* plhs[1];
		int output = mexCallMATLAB(1, plhs, 2, prhs, "mtimes");
		double* res = (double*)mxGetData(plhs[0]);
		MatrixXd newW = Map<MatrixXd>(res, data->numDims, data->numDims);
		MatrixXd gradient = 2.0*newW - data->YX - data->YX;

		Map<MatrixXd>(grad, data->numDims, data->numDims) = gradient;

		clock_t endM = clock();
		// mexPrintf("time step newgrad: %f\n", difftime(endM, beginM));
	}

	// val = norm(W*X - Y);
	clock_t begin1 = clock();
	MatrixXd res = W*data->X - data->Y;
	clock_t end1 = clock();
	//mexPrintf("time step main: %f\n", difftime(end1, begin1));

	// Matlab call
	clock_t beginM = clock();
	mxArray* prhs[1];
	mwSize size[2]; size[0] = data->numDims;  size[1] = data->numSamples;
	prhs[0] = mxCreateNumericArray(2, size, mxDOUBLE_CLASS, mxREAL);
	double* resMatlab = (double*)mxGetData(prhs[0]);
	Map<MatrixXd>(resMatlab, data->numDims, data->numSamples) = res;
	mxArray* plhs[1];
	mexCallMATLAB(1, plhs, 1, prhs, "norm");
	double norm = (double)mxGetScalar(plhs[0]);
	clock_t endM = clock();
	// mexPrintf("time step norm: %f\n", difftime(endM, beginM));
	mexPrintf("[iter %d] energy: %f\n", counter, norm);

	/*
	clock_t begin2 = clock();
	double energy = res.operatorNorm();
	clock_t end2 = clock();
	mexPrintf("time step norm: %f\n", difftime(end2, begin2));
	mexPrintf("[iter %d] energy: %f\n", counter, energy);
	return energy;
	*/

	return norm;
}

typedef struct
{
	double a, b;
} mConstraintData;

double myconstraint(unsigned n, const double *x, double *grad, void *data)
{
	mConstraintData *d = (mConstraintData *)data;
	double a = d->a, b = d->b;
	if (grad) {
		grad[0] = 3 * a * (a*x[0] + b) * (a*x[0] + b);
		grad[1] = -1.0;
	}
	return ((a*x[0] + b) * (a*x[0] + b) * (a*x[0] + b) - x[1]);
}

void run_optimiser(double* x, double* residual, int numDims, int numSamples, double* srcCentres, double* tgtCentres)
{
	nlopt_opt opt;

	opt = nlopt_create(NLOPT_LD_MMA, numDims*numDims); /* algorithm and dimensionality */
	// opt = nlopt_create(NLOPT_LN_COBYLA, numDims*numDims); /* algorithm and dimensionality */

	mFuncData funcData;
	funcData.numDims = numDims;
	funcData.numSamples = numSamples;
	funcData.X = Map<MatrixXd>(srcCentres, numDims, numSamples);
	funcData.Y = Map<MatrixXd>(tgtCentres, numDims, numSamples);

	// Precomputation
	funcData.XX = funcData.X * funcData.X.transpose();
	funcData.YX = funcData.Y * funcData.X.transpose();

	nlopt_set_min_objective(opt, myfunc, &funcData);
	nlopt_set_xtol_rel(opt, 1e-4);
	nlopt_set_maxeval(opt, 25);

	// Constraints:
	//mConstraintData data[2] = { { 2, 0 }, { -1, 1 } };
	//nlopt_add_inequality_constraint(opt, myconstraint, &data[0], 1e-4);
	//nlopt_add_inequality_constraint(opt, myconstraint, &data[1], 1e-4);

	if (nlopt_optimize(opt, x, residual) < 0)
		printf("nlopt failed!\n");
	else
		printf("found minimum after %d evaluations with residual %f\n", counter, *residual);

	nlopt_destroy(opt);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	/* Macros for the input arguments */
	#define srcCentres_IN prhs[0]
	#define tgtCentres_IN prhs[1]

	/* Macros for the output arguments */
	#define x_OUT plhs[0]
	#define residual_OUT plhs[1]

	/* Check correctness of input/output arguments */
	if (nrhs < 2 || nrhs > 2)
		mexErrMsgTxt("Wrong number of input arguments.");
	else if (nlhs > 2)
		mexErrMsgTxt("Too many output arguments.");

	/* Get input data */
	double* srcCentres = (double*)mxGetData(srcCentres_IN);
	double* tgtCentres = (double*)mxGetData(tgtCentres_IN);
	int numDims = (int)mxGetM(srcCentres_IN);
	int numSamples = (int)mxGetN(srcCentres_IN);

	/* Create output data */
	x_OUT = mxCreateNumericMatrix(numDims, numDims, mxDOUBLE_CLASS, mxREAL);
	double* x = (double*)mxGetData(x_OUT);
	residual_OUT = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL);
	double* residual = (double*)mxGetData(residual_OUT);

	// Initial guess (Id matrix)
	for (int idx = 0; idx < numDims*numDims; ++idx)
	{
		if (idx % (numDims + 1) == 0)
			x[idx] = 1.0;
		else
			x[idx] = 0.0;
	}

	// Reset counter
	counter = 0;

	/* Call method */
	run_optimiser(x, residual, numDims, numSamples, srcCentres, tgtCentres);

	return;
}


#include <frovedis.hpp>
#include <frovedis/matrix/rowmajor_matrix.hpp>
#include <frovedis/matrix/tsne.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;

template <class T>
void call_tsne(const std::string& data_p, const std::string& out_p, double perplexity,
               double early_exaggeration, double min_grad_norm, double learning_rate,
               size_t n_components, size_t max_iter, size_t n_iter_without_progress,
               const std::string& metric, const std::string& method,
               const std::string& init, bool verbose) {
  time_spent load_t(INFO);
  load_t.lap_start();
  auto mat = make_rowmajor_matrix_load<T>(data_p);
  load_t.lap_stop();
  load_t.show_lap("data loading time: ");
  std::cout << "n_samples = " << mat.num_row
            << ", n_features = " << mat.num_col
            << std::endl;
  time_spent tsne_t(INFO);
  tsne_t.lap_start();

  TSNE<T> t1;
  t1.set_perplexity(perplexity).
     set_early_exaggeration(early_exaggeration).
     set_min_grad_norm(min_grad_norm).
     set_learning_rate(learning_rate).
     set_n_components(n_components).
     set_n_iter(max_iter).
     set_n_iter_without_progress(n_iter_without_progress).
     set_metric(metric).
     set_method(method).
     set_init(init).
     set_verbose(verbose);

  auto Y_mat = t1.fit_transform(mat);
  auto n_iter = t1.get_n_iter_();
  auto kl_divergence = t1.get_kl_divergence_();

  tsne_t.lap_stop();
  tsne_t.show_lap("Overall computation time: ");
  Y_mat.save(out_p);
  std::cout << "n_iter_ = " << n_iter << std::endl;
  std::cout << "kl_divergence_ = " << kl_divergence << std::endl;
}

int main(int argc, char* argv[]) {
  use_frovedis use(argc, argv);

  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
      ("help,h", "produce help message")
      ("input,i" , value<std::string>(), "input data path containing input data for T-SNE")
      ("dtype,t" , value<std::string>(), "input data type (float or double) [default: double]")
      ("output,o" , value<std::string>(), "output data path to save output embeddings")
      ("max_iter,k", value<size_t>(), "maximum no. of iterations (default: 1000)")
      ("perplexity,p", value<double>(), "number of nearest neighbors for each point (default: 30.0)")
      ("early_exaggeration,e", value<double>(), "controls the space between natural clusters in the embedded space (default: 12.0)")
      ("min_grad_norm,g", value<double>(), "gradient norm threshold (default: 1e-7)")
      ("learning_rate,l", value<double>(), "learning rate for t-SNE (default: 200.0)")
      ("n_components,n", value<size_t>(), "dimension of the embedded space (default: 2)")
      ("niter_without_progress", value<size_t>(), "maximum number of iterations without progress before we abort the optimization (default: 300)")
      ("metric,m" , value<std::string>(), "the metric (euclidean or precomputed) to use when calculating distance (default: euclidean)")
      ("method" , value<std::string>(), "the method (exact) to use for TSNE computation (default: exact)")
      ("init" , value<std::string>(), "the init (random) to use for initializing Y mat (default: random)")
      ("verbose", "set loglevel to DEBUG");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  std::string dtype =  "double";
  std::string data_p =  "";
  std::string out_p =  "";
  size_t max_iter = 1000;
  double perplexity = 30.0;
  double early_exaggeration = 12.0;
  double min_grad_norm = 1e-7;
  double learning_rate = 200.0;
  size_t n_components = 2;
  size_t n_iter_without_progress = 300;
  std::string metric = "euclidean"; //possible values = ["euclidean", "precomputed"]
  std::string method = "exact"; //possible values = ["exact"]
  std::string init = "random"; //possible values = ["random"]
  bool verbose = false;

  if(argmap.count("help")){
    std::cerr << opt << std::endl;
    exit(1);
  }
  if(argmap.count("input")){
    data_p = argmap["input"].as<std::string>();
  } else {
    std::cerr << "input path is not specified" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }
  if(argmap.count("dtype")){
    dtype = argmap["dtype"].as<std::string>();
  }
  if(argmap.count("output")){
    out_p = argmap["output"].as<std::string>();
  } else {
    std::cerr << "output path is not specified" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }
  if(argmap.count("max_iter")){
     max_iter = argmap["max_iter"].as<size_t>();
  }
  if(argmap.count("perplexity")){
     perplexity = argmap["perplexity"].as<double>();
  }
  if(argmap.count("early_exaggeration")){
     early_exaggeration = argmap["early_exaggeration"].as<double>();
  }
  if(argmap.count("min_grad_norm")){
     min_grad_norm = argmap["min_grad_norm"].as<double>();
  }
  if(argmap.count("learning_rate")){
     learning_rate = argmap["learning_rate"].as<double>();
  }
  if(argmap.count("n_components")){
     n_components = argmap["n_components"].as<size_t>();
  }
  if(argmap.count("niter_without_progress")){
     n_iter_without_progress = argmap["niter_without_progress"].as<size_t>();
  }
  if(argmap.count("metric")){
    metric = argmap["metric"].as<std::string>();
  }
  if(argmap.count("method")){
    method = argmap["method"].as<std::string>();
  }
  if(argmap.count("init")){
    init = argmap["init"].as<std::string>();
  }
  if(argmap.count("verbose")){
    set_loglevel(DEBUG);
    verbose = true;
  }

  try {
    if (dtype == "float") {
      call_tsne<float>(data_p, out_p, perplexity, early_exaggeration, min_grad_norm,
                       learning_rate, n_components, max_iter,
                       n_iter_without_progress, metric, method, init, verbose);
    }
    else if (dtype == "double") {
      call_tsne<double>(data_p, out_p, perplexity, early_exaggeration, min_grad_norm,
                        learning_rate, n_components, max_iter,
                        n_iter_without_progress, metric, method, init, verbose);
    }
    else {
      std::cerr << "Supported dtypes are only float and double!\n";
      std::cerr << opt << std::endl;
      exit(1);
    }
  }
  catch(std::exception& e) {
    std::cout << "exception caught: " << e.what() << std::endl;
  }
  return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_NTHROOT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_NTHROOT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-exponential
    Function object implementing nthroot capabilities

    nth root function: \f$\sqrt[n]{x}\f$
    \arg n must be of integer type
    \arg if n is even and x negative the result is @ref Nan
    \arg if x is null the result is @ref Zero
    \arg if x is one  the result is @ref One

    @par Semantic:

    For every parameters of  floating type T and integral type N:

    @code
    T r = nthroot(x, n);
    @endcode

    is similar to:

    @code
    T r = n >= 0 ? pow(x, rec(tofloat(n))) : Nan;
    @endcode

    @par Note:
    nthroot is more expansive than pow(x, rec(tofloat(n))) because
    it takes care of some limits issues that @ref pow does not mind of.

    See if it suits you better.

    @see pow, rec, sqrt, cbrt

  **/
  const boost::dispatch::functor<tag::nthroot_> nthroot = {};
} }
#endif

#include <boost/simd/function/scalar/nthroot.hpp>
#include <boost/simd/function/simd/nthroot.hpp>

#endif


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

#include <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <math.h>       /* sqrt */
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>

namespace votca { namespace tools {

using namespace std;

/**
*
* ublas binding for gsl_eigen_symmv
* note that the eigenvalues/eigenvectors are UNSORTED
*
*/
bool linalg_eigenvalues_symmetric( ub::symmetric_matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V)
{
	gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        // gsl does not handle conversion of a symmetric_matrix
        ub::matrix<double> _A( N,N );
        _A = A;

	E.resize(N, false);
	V.resize(N, N, false);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(N);

	int status = gsl_eigen_symmv(&A_view.matrix, &E_view.vector, &V_view.matrix, w);
	//gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_ABS_ASC);
	gsl_eigen_symmv_free(w);
	gsl_set_error_handler(handler);

	return (status != 0);
};

/**
*
* ublas binding for gsl_eigen_symmv
* input matrix type general matrix!
* wrapping gsl_eigen_symmv
*
*/
bool linalg_eigenvalues( ub::matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V)
{
	gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        // gsl does not handle conversion of a symmetric_matrix
        ub::matrix<double> _A( N,N );
        _A = A;

	E.resize(N, false);
	V.resize(N, N, false);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(N);

	int status = gsl_eigen_symmv(&A_view.matrix, &E_view.vector, &V_view.matrix, w);
	gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_VAL_ASC);
	gsl_eigen_symmv_free(w);
	gsl_set_error_handler(handler);

	return (status != 0);
};

/**
*
* ublas binding for gsl_eigen_symmv
* input matrix type general matrix single precision!
* wrapping gsl_eigen_symmv
*
*/
bool linalg_eigenvalues( ub::matrix<float> &A, ub::vector<float> &E, ub::matrix<float> &V)
{
	gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        // gsl does not handle symmetric_matrix and floats, so this is super stupid
        ub::matrix<double> _A( N,N );
        _A = A;
        ub::vector<double> _E(N);
        ub::matrix<double> _V(N,N);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&_E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&_V(0,0), N, N);
	gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(N);

	int status = gsl_eigen_symmv(&A_view.matrix, &E_view.vector, &V_view.matrix, w);
	gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_VAL_ASC);
	gsl_eigen_symmv_free(w);
	gsl_set_error_handler(handler);

	//E.resize(N, false);
	//V.resize(N, N, false);
        E = _E;
        V = _V;

	return (status != 0);
};

bool linalg_eigenvalues(  ub::vector<float> &E, ub::matrix<float> &V)
{
        /* on input V is the matrix that shall be diagonalized
         * GSL does not provide an in-place routine, so we wrap
         * gsl_eigen_symmv for compatibility
         */

         // make a copy of E
         ub::matrix<float> A = V;

         // now call wrapper for gsl_eigen_symmv
         bool status = linalg_eigenvalues( A , E, V );
	return (status != 0);
};

/**
*
* ublas binding for gsl_eigen_symm
* input matrix type general matrix!
* wrapping gsl_eigen_symm leaves input matrix
*
*/
bool linalg_eigenvalues( ub::vector<double> &E, ub::matrix<double> &V)
{
        /* on input V is the matrix that shall be diagonalized
         * GSL does not provide an in-place routine, so we wrap
         * gsl_eigen_symmv for compatibility
         */

         // make a copy of E
         ub::matrix<double> A = V;

         // now call wrapper for gsl_eigen_symmv
         bool status = linalg_eigenvalues( A , E, V );

         return status;
};

/*
 * use expert routine to calculate only a subrange of eigenvalues
 */
bool linalg_eigenvalues( ub::matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V , int nmax)
{
    throw std::runtime_error("linalg_eigenvalues is not compiled-in due to disabling of MKL - recompile Votca Tools with MKL support");
}

/*
 * use expert routine to calculate only a subrange of eigenvalues single precision
 */
bool linalg_eigenvalues( ub::matrix<float> &A, ub::vector<float> &E, ub::matrix<float> &V , int nmax)
{
    // now call wrapper for gsl_eigen_symmv
    bool status = linalg_eigenvalues( A , E, V );

    return status;
}

bool linalg_eigenvalues_general( ub::matrix<double> &A,ub::matrix<double> &B, ub::vector<double> &E, ub::matrix<double> &V)
{
	gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        // gsl destroys A so use copy
        ub::matrix<double> _A( N,N );
        _A = A;

        ub::matrix<double> _B( N,N );
        _B=B;

	E.resize(N, false);
	V.resize(N, N, false);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
        gsl_matrix_view B_view = gsl_matrix_view_array(&_B(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_eigen_gensymmv_workspace *w = gsl_eigen_gensymmv_alloc(N);

	int status = gsl_eigen_gensymmv(&A_view.matrix,&B_view.matrix, &E_view.vector, &V_view.matrix, w);
	gsl_eigen_gensymmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_VAL_ASC);
	gsl_eigen_gensymmv_free(w);
	gsl_set_error_handler(handler);

        ub::matrix<double> _temp= ub::prod(B,V);
        ub::matrix<double> n=ub::prod(ub::trans(V),_temp);
      /*
        for (int i=0;i<n.size1();i++){

                for (int j=0;j<n.size2();j++){
                cout <<"n("<< i << ":"<< j <<")= " <<n(i,j)<< endl;
                }}

       */

        for (unsigned int i=0;i<n.size1();i++){
            ub::matrix_range<ub::matrix<double> > column=ub::subrange( V, 0, V.size2(),i, i+1 );
            //cout <<"n("<< i << ":"<< i <<")= " <<n(i,i) <<":" <<sqrt(n(i,i))<< endl;
            //for (int j=0;j<column.size1();j++){

            //cout <<"V("<<i<<":"<<j<<")="<<column(j,0)<< endl;

              //}
            column=column/sqrt(n(i,i));

        }

	return (status != 0);
};

}}


#ifndef WAGNER_COMMON_H_
#define WAGNER_COMMON_H_

#include <cmath>

#ifndef WAGNER_NOBOOST
  #include <boost/container/flat_set.hpp>
  #include <boost/container/flat_map.hpp>
#else
  #include <set>
  #include <map>
#endif

namespace wagner {

#ifndef WAGNER_NOBOOST
  template<typename Key>
  using set = boost::container::flat_set<Key>;

  template<typename Key, typename Value>
  using map = boost::container::flat_map<Key, Value>;
#else
  template<typename Key>
  using set = std::set<Key>;

  template<typename Key, typename Value>
  using map = std::map<Key, Value>;
#endif

constexpr size_t wagner_version = 2;
constexpr size_t wagner_revision = 0;

/** Mathematical constant e. */
#define math_e 2.71828182845904523536

/** Mathematical constant pi. */
#define math_pi 3.14159265358979323846

/** Log 2 (very important in information theory). */
#define log2(x) (log(x) / log(2.0))

/** Check if a number is a power of two. */
#define power_of_two(n) (((n) != 0) && !((n) & ((n)-1)))

/** Cubic root. */
#define cbrt(x) (pow((x), 1.0 / 3.0))

/** Max of two values. */
#define max2(a, b) ((a) > (b) ? (a) : (b))

/** Min of two values. */
#define min2(a, b) ((a) < (b) ? (a) : (b))

/** Max of three values. */
#define max3(a, b, c) (max2(a, b) > (c) ? max2(a, b) : (c))

/** Min of three values. */
#define min3(a, b, c) (min2(a, b) < (c) ? min2(a, b) : (c))

/** Max of four values. */
#define max4(a, b, c, d) (max3(a, b, c) > (d) ? max3(a, b, c) : (d))

/** Min of four values. */
#define min4(a, b, c, d) (min3(a, b, c) < (d) ? min3(a, b, c) : (d))

/** Max of five values. */
#define max5(a, b, c, d, e) (max4(a, b, c, d) > (e) ? max4(a, b, c, d) : (e))

/** Min of five values. */
#define min5(a, b, c, d, e) (min4(a, b, c, d) < (e) ? min4(a, b, c, d) : (e))

}

#endif


#pragma once

#include <Eigen/Core>
#include <cppad/cppad.hpp> // the CppAD package http://www.coin-or.org/CppAD/
#include <vector>

template <typename Type, typename Derived>
std::vector<Type> Eigen2AD(const Eigen::MatrixBase<Derived>& M)
{
    std::vector<Type> out;
    out.resize(M.size());

    Eigen::Index p = 0;
    for (Eigen::Index j = 0; j < M.cols(); ++j)
        for (Eigen::Index i = 0; i < M.rows(); ++i)
            out[p++] = M(i, j);

    return out;
}

template <typename Type>
std::vector<Type> SumAD(const std::vector<Type>& M1, const std::vector<Type>& M2)
{
    std::vector<Type> out(M1.size());

    for (size_t i = 0; i < M1.size(); ++i)
        out[i] = M1[i] + M2[i];

    return out;
}

template <typename Type>
std::vector<Type> SubAD(const std::vector<Type>& M1, const std::vector<Type>& M2)
{
    std::vector<Type> out(M1.size());

    for (size_t i = 0; i < M1.size(); ++i)
        out[i] = M1[i] - M2[i];

    return out;
}

template <typename Type>
std::vector<Type> ProductAD(const std::vector<Type>& M1, const std::vector<Type>& M2, size_t rowsOut, size_t colsOut = 1)
{
    std::vector<Type> out(rowsOut * colsOut, Type(0));

    size_t m = rowsOut;
    size_t n = M1.size() / rowsOut;
    size_t p = colsOut;
    for (size_t j = 0; j < p; ++j)
        for (size_t i = 0; i < m; ++i)
            for (size_t k = 0; k < n; ++k)
                out[i + j * p] += M1[i + k * m] * M2[k + j * p];

    return out;
}

template <typename Type>
Type DotProductAD(const std::vector<Type>& v1, const std::vector<Type>& v2)
{
    Type sum;

    sum = 0.;
    for (size_t i = 0; i < size_t(v1.size()); ++i)
        sum += v1[i] * v2[i];

    return sum;
}

template <typename Type>
std::vector<Type> CrossAD(const std::vector<Type>& v1, const std::vector<Type>& v2)
{
    std::vector<Type> out(3);
    out[0] = v1[1] * v2[2] - v1[2] * v2[1];
    out[1] = v1[2] * v2[0] - v1[0] * v2[2];
    out[2] = v1[0] * v2[1] - v1[1] * v2[0];
    return out;
}

template <typename Type>
std::vector<Type> Cross6AD(const std::vector<Type>& v1, const std::vector<Type>& v2)
{
    std::vector<Type> out(6);
    out[0] = v1[1] * v2[2] - v1[2] * v2[1];
    out[1] = v1[2] * v2[0] - v1[0] * v2[2];
    out[2] = v1[0] * v2[1] - v1[1] * v2[0];
    out[3] = v1[4] * v2[2] - v1[5] * v2[1] + v1[1] * v2[5] - v1[2] * v2[4];
    out[4] = v1[5] * v2[0] - v1[3] * v2[2] + v1[2] * v2[3] - v1[0] * v2[5];
    out[5] = v1[3] * v2[1] - v1[4] * v2[0] + v1[0] * v2[4] - v1[1] * v2[3];

    return out;
}

template <typename Type>
std::vector<Type> Cross6DAD(const std::vector<Type>& v1, const std::vector<Type>& v2)
{
    std::vector<Type> out(6);
    out[0] = v1[1] * v2[2] - v1[2] * v2[1] + v1[4] * v2[5] - v1[5] * v2[4];
    out[1] = v1[2] * v2[0] - v1[0] * v2[2] + v1[5] * v2[3] - v1[3] * v2[5];
    out[2] = v1[0] * v2[1] - v1[1] * v2[0] + v1[3] * v2[4] - v1[4] * v2[3];
    out[3] = v1[1] * v2[5] - v1[2] * v2[4];
    out[4] = v1[2] * v2[3] - v1[0] * v2[5];
    out[5] = v1[0] * v2[4] - v1[1] * v2[3];

    return out;
}

template <typename Type>
std::vector<Type> CrossAD(const std::vector<Type>& v)
{
    std::vector<Type> out(9);
    out[0] = Type(0);
    out[1] = v[2];
    out[2] = -v[1];
    out[3] = -v[2];
    out[4] = Type(0);
    out[5] = v[0];
    out[6] = v[1];
    out[7] = -v[0];
    out[8] = Type(0);
    return out;
}

template <typename Type>
std::vector<Type> TransposeAD(const std::vector<Type>& M, int rows)
{
    std::vector<Type> out(M.size());
    size_t cols = M.size() / rows;
    size_t p = 0;
    for (size_t j = 0; j < cols; ++j)
        for (size_t i = 0; i < rows; ++i)
            out[p++] = M[j + cols * i];

    return out;
}

template <typename Type>
std::vector<Type> IdMatAD(size_t size)
{
    std::vector<Type> out(size * size, Type(0));
    for (size_t i = 0; i < size; ++i)
        out[i + i * size] = Type(1);

    return out;
}

template <typename Type>
std::vector<Type> exp3x3AD(const std::vector<Type>& ax)
{
    std::vector<Type> out(9, Type(0));
    Type angle = CppAD::sqrt(DotProductAD(ax, ax));
    if (CppAD::abs(angle) < std::numeric_limits<double>::epsilon()) {
        out[0] = 1;
        out[4] = 1;
        out[8] = 1;
    } else {
        Type x = ax[0] / angle;
        Type y = ax[1] / angle;
        Type z = ax[2] / angle;
        Type sa = CppAD::sin(angle);
        Type ca = CppAD::cos(angle);

        out[0] = ca + (1 - ca) * x * x;
        out[1] = (1 - ca) * y * x + sa * z;
        out[2] = (1 - ca) * z * x - sa * y;
        out[3] = (1 - ca) * x * y - sa * z;
        out[4] = ca + (1 - ca) * y * y;
        out[5] = (1 - ca) * z * y + sa * x;
        out[6] = (1 - ca) * x * z + sa * y;
        out[7] = (1 - ca) * y * z - sa * x;
        out[8] = ca + (1 - ca) * z * z;
    }

    return out;
}

template <template <class> typename ADType, typename Type>
std::vector<Type> CastOutAD(const std::vector<ADType<Type>>& in)
{
    size_t s = in.size();
    std::vector<Type> out(s);
    for (size_t i = 0; i < s; ++i)
        out[i] = CppAD::Value(in[i]);

    return out;
}


#ifndef TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP
#define TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP

#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <cmath>
#include <triumf/bnmr/slr/common.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// spin-lattice relaxation (SLR)
namespace slr {

/// pulsed Gaussian distribution of exponentials integral
/// (from 0 to time_p <= time)
template <typename T = double>
T pulsed_gauss_dist_exp_integral(T time, T time_p, T nuclear_lifetime,
                                 T slr_rate, T sigma) {
  // make sure that
  assert(time >= time_p);
  // integrand for the numeric integral
  auto integrand = [=](T t_p) {
    // translated from Maxima
    T expo1 = std::exp(
        boost::math::constants::half<T>() * time * time * sigma * sigma +
        boost::math::constants::half<T>() * t_p * t_p * sigma * sigma +
        t_p * slr_rate);
    T expo2 = std::exp(-time / nuclear_lifetime + t_p / nuclear_lifetime -
                       t_p * time * sigma * sigma);
    T expo3 = std::exp(time * slr_rate);
    return -expo2 *
           (expo1 * std::erf(((time - t_p) * sigma * sigma - slr_rate) /
                             boost::math::constants::root_two<T>() / sigma) -
            expo1) /
           (expo3 * std::erf(slr_rate / sigma /
                             boost::math::constants::root_two<T>()) +
            expo3);
  };
  // create the integrator for tanh-sinh quadrature
  static boost::math::quadrature::tanh_sinh<T> integrator;
  // evaluate the integral from 0 to time_p
  T Q = integrator.integrate(integrand, 0.0, time_p);
  return Q;
}

/// pulsed Gaussian distribution of exponentials
template <typename T = double>
T pulsed_gauss_dist_exp(T time, T nuclear_lifetime, T pulse_length, T asymmetry,
                        T slr_rate, T sigma) {
  if (time == 0.0) {
    return asymmetry;
  } else if (time > 0.0 and time <= pulse_length) {
    return asymmetry *
           pulsed_gauss_dist_exp_integral(time, time, nuclear_lifetime,
                                          slr_rate, sigma) /
           normalization(time, nuclear_lifetime);
  } else if (time > pulse_length) {
    return (asymmetry *
            pulsed_gauss_dist_exp_integral(time, pulse_length, nuclear_lifetime,
                                           slr_rate, sigma) /
            normalization(pulse_length, nuclear_lifetime)) /
           std::exp(-(time - pulse_length) / nuclear_lifetime);
  } else {
    return 0.0;
  }
}

/// pulsed Gaussian distribution of exponentials (ROOT)
template <typename T = double>
T pulsed_gauss_dist_exp(const T *x, const T *par) {
  return pulsed_gauss_dist_exp<T>(*x, par[0], par[1], par[2], par[3], par[4]);
}

} // namespace slr

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP


#include "mesh_reader.hpp"
#include "newton_raphson.hpp"
#include "constants.hpp"

#include <tuple>
#include <iostream>
#include <math.h>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_blocked.hpp>
#include <chrono>

using namespace boost::numeric::ublas;

/* Configure boost-numeric-bindings in /usr/local to be able to use the compile statement on the next line. */
/* COMPILE WITH: g++ -Wall -std=c++14 -O3 -lstdc++ -o fem.o fem.cpp
mesh_reader.hpp needs those two flags for reading files */

class F {
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    matrix<double> Au_;
    matrix<double> Av_;
    matrix<double> B_;
    matrix<double> C_;
    vector<double> D_;

  public:
    F(matrix<double> Au, matrix<double> Av, matrix<double> B, matrix<double> C, vector<double> D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {

    }

    vector<double> operator()(vector<double> x) {
      /* x = [uT vT]T */
      int n = Au_.size1();
      vector<double> u = project(x,range(0,n));
      vector<double> v = project(x,range(n,2*n));
      vector<double> func(2*n);
      project(func,range(0,n)) = prod(Au_,u) + prod(B_,Ru(u,v)) + hu*(prod(C_,u) - D_*uamb);
      project(func,range(n,2*n)) = -prod(Av_,v) + prod(B_,Rv(u,v)) - hv*(prod(C_,v) - D_*vamb);
      return func;
    }
};

class J {
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    matrix<double> Au_;
    matrix<double> Av_;
    matrix<double> B_;
    matrix<double> C_;
    vector<double> D_;

  public:
    J(matrix<double> Au, matrix<double> Av, matrix<double> B, matrix<double> C, vector<double> D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {

    }

    matrix<double> operator()(vector<double> x) {
      /* x = [uT vT]T */
      int n = Au_.size1();
      vector<double> u = project(x,range(0,n));
      vector<double> v = project(x,range(n,2*n));
      matrix<double> JAC(2*n, 2*n);
      project(JAC,range(0,n),range(0, n)) = Au_ + prod(B_,dRudu(u,v)) + hu*C_;
      project(JAC,range(n,2*n),range(0,n)) = prod(B_,dRudv(u,v));
      project(JAC,range(n,2*n),range(0,n)) = prod(B_,dRvdu(u,v));
      project(JAC,range(n,2*n),range(n,2*n)) = -1*Av_ + prod(B_,dRvdv(u,v)) - hv*C_;
      return JAC;
    }
};

int main() {
  /* Read and store mesh information */
  auto t1 = std::chrono::high_resolution_clock::now();
  matrix<double> vertices = mesh::read_vertices();
  matrix<double> triangles = mesh::read_triangles(vertices);
  matrix<int> boundaries = mesh::read_boundaries(vertices);
  auto t2 = std::chrono::high_resolution_clock::now();
  std::cout << "Input data successfully read:" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate righthand side vector integrals */
  t1 = std::chrono::high_resolution_clock::now();
  matrix<double> init_B(vertices.size1(), vertices.size1());
  symmetric_adaptor<matrix<double>, lower> B(init_B);
  B = set_zero(B);
  for (unsigned t = 0; t < triangles.size1(); ++t) {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    //std::cout << std::endl;
    B(a, a) += area*(6.*vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    //std::cout<< B(a,a) << std::endl;
    B(b, a) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + vertices(c, 0));
    B(c, a) += area*(2.*vertices(a, 0) + vertices(b, 0) + 2.*vertices(c, 0));
    B(b, b) += area*(2.*vertices(a, 0) + 6.*vertices(b, 0) + 2.*vertices(c, 0));
    //std::cout << "B(b,c): " << B(b,c) << "deel1: "<< area*(2.*vertices(a, 0) + 6.*vertices(b, 0) + 2.*vertices(c, 0)) << std::endl;
    B(b, c) += area*(vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B(c, c) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + 6.*vertices(c, 0));
    //std::cout << B << std::endl;
  }
  //std::cout << "before division: " << B << std::endl;
  B *= (1./60.);
  //std::cout << "after divison: " << B << std::endl;
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << "B matrix successfully assembled" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate first part of stiffness matrix A */
  t1 = std::chrono::high_resolution_clock::now();
  matrix<double> init_A_U(vertices.size1(), vertices.size1());
  matrix<double> init_A_V(vertices.size1(), vertices.size1());
  matrix<double> I_U(2,2);
  matrix<double> I_V(2,2);
  matrix<double> G(3, 2);
  matrix<double> GGT_U(3, 3);
  matrix<double> GGT_V(3, 3);
  matrix<double> temp(3, 3);
  symmetric_adaptor<matrix<double>, lower> A_U(init_A_U);
  symmetric_adaptor<matrix<double>, lower> A_V(init_A_V);
  I_U = set_zero(I_U);
  I_V = set_zero(I_V);
  A_U = set_zero(A_U);
  A_V = set_zero(A_V);
  I_U(0,0) = DU_R;
  I_U(1,1) = DU_Z;
  I_V(0,0) = DV_R;
  I_V(1,1) = DV_Z;
  for (unsigned t = 0; t < triangles.size1(); ++t) {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    G(0, 0) = (vertices(b, 1) - vertices(c, 1));
    G(1, 0) = (vertices(c, 1) - vertices(a, 1));
    G(2, 0) = (vertices(a, 1) - vertices(b, 1));
    G(0, 1) = (vertices(c, 0) - vertices(b, 0));
    G(1, 1) = (vertices(a, 0) - vertices(c, 0));
    G(2, 1) = (vertices(b, 0) - vertices(a, 0));
    temp = prod(G,I_U);
    GGT_U = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*prod(temp, trans(G));
    temp = prod(G,I_V);
    GGT_V = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*prod(temp, trans(G));
    // std::cout << "factor: " << (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6) << std::endl;
    // std::cout << "G: " << G << std::endl;
    // std::cout << "temp: " << temp << std::endl;
    // std::cout << "tempGT: " << prod(temp, trans(G)) << std::endl;
    // std::cout << "GGT_V: " << GGT_V << std::endl;
    A_U(a, a) += GGT_U(0, 0);
    A_U(b, a) += GGT_U(1, 0);
    A_U(c, a) += GGT_U(2, 0);
    A_U(b, b) += GGT_U(1, 1);
    A_U(b, c) += GGT_U(1, 2);
    A_U(c, c) += GGT_U(2, 2);
    A_V(a, a) += GGT_V(0, 0);
    A_V(b, a) += GGT_V(1, 0);
    A_V(c, a) += GGT_V(2, 0);
    A_V(b, b) += GGT_V(1, 1);
    A_V(b, c) += GGT_V(1, 2);
    A_V(c, c) += GGT_V(2, 2);
  }

  t2 = std::chrono::high_resolution_clock::now();
  std::cout << "A matrices assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Boundary condition integrals: second part of A and a constant vector term */
  t1 = std::chrono::high_resolution_clock::now();
  matrix<double> init_C(vertices.size1(), vertices.size1());
  symmetric_adaptor<matrix<double>, lower> C(init_C);
  C = set_zero(C);
  vector<double> D(vertices.size1());
  D = set_zero(D);
  // std::cout << std::endl;
  // std::cout << std::endl;
  // std::cout << std::endl;
  for (unsigned b = 0; b < boundaries.size1(); ++b) {
    double len = sqrt(pow(vertices(boundaries(b, 0), 0) - vertices(boundaries(b, 1), 0), 2) +
      pow(vertices(boundaries(b, 0), 1) - vertices(boundaries(b, 1), 1), 2));
    C(boundaries(b, 0), boundaries(b, 0)) += len*(vertices(boundaries(b, 0), 0)/4 + vertices(boundaries(b, 1), 0)/12);
    C(boundaries(b, 0), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/12);
    C(boundaries(b, 1), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/4);
    // Nakijken -> 1 vergeten
    D(boundaries(b,0)) += len*(vertices(boundaries(b,0),0)/3.+vertices(boundaries(b,1),0)/6.);
    // std::cout << "D(boundaries(b,0)): " << len*(vertices(boundaries(b,0),0)/3.+vertices(boundaries(b,1),0)/6.) << std::endl;
    // std::cout << "b: " << b << std::endl;
    D(boundaries(b,1)) += len*(vertices(boundaries(b,0),0)/6.+vertices(boundaries(b,1),0)/3.);
    // std::cout << "D(boundaries(b,1)): " << len*(vertices(boundaries(b,0),0)/6.+vertices(boundaries(b,1),0)/3.) << std::endl;
    // std::cout << "b: " << len << std::endl;
  }
  // std::cout << "C: "<< C << std::endl;
  // std::cout << std::endl;
  // std::cout << std::endl;
  // std::cout << std::endl;
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << "C matrix and D vector assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Root finding for nonlinear system of equations */
  t1 = std::chrono::high_resolution_clock::now();
  F F_funct(A_U, A_V, B, C, D);
  J J_funct(A_U, A_V, B, C, D);
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << "Functors are created" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  t1 = std::chrono::high_resolution_clock::now();

  std::cout << "A_U: " << A_U << std::endl;
  std::cout << std::endl;
  std::cout << "A_V: " << A_V << std::endl;
  std::cout << std::endl;
  std::cout << "B: " << B << std::endl;
  std::cout << std::endl;
  std::cout << "C: " << C << std::endl;
  std::cout << std::endl;
  std::cout << "D: " << D << std::endl;
  std::cout << std::endl;
  vector<double> guess = vector<double>(vertices.size1()*2,5.);
  matrix<double> inverse_u_0 (vertices.size1(),vertices.size1());
  inverse_u_0 = set_zero(inverse_u_0);
  InvertMatrix<double>(A_U+(Vmu/Kmu)*B+hu*C, inverse_u_0);
  std::cout << "V/K" << Vmu/Kmu << std::endl;
  std::cout << std::endl;
  std::cout << "hu" << hu << std::endl;
  std::cout << std::endl;
  vector<double> u_0 = prod(inverse_u_0, hu*D*uamb);

  std::cout << std::endl;
  std::cout << std::endl;
  matrix<double> inverse_v_0 (vertices.size1(),vertices.size1());
  inverse_v_0 = set_zero(inverse_v_0);
  InvertMatrix<double>(A_V+hv*C, inverse_v_0);
  vector<double> v_0 = prod(inverse_v_0, rq*(Vmu/Kmu)*prod(B,u_0)+hv*vamb*D);
  std::cout << "v_0" << v_0 << std::endl;
  std::cout << std::endl;
  std::cout << "inverse_v_0" << inverse_v_0 << std::endl;
  std::cout << std::endl;
  std::cout << "second factor" << rq*(Vmu/Kmu)*prod(B,u_0)+hv*vamb*D << std::endl;
  std::cout << std::endl;
  std::cout << std::endl;
  std::cout << "uamb: " << uamb << std::endl;
  std::cout << std::endl;
  std::cout << std::endl;

  project(guess,range(0,vertices.size1())) = u_0;
  project(guess,range(vertices.size1(),2*vertices.size1())) = v_0;
  std::cout << "Initial guess" << guess << std::endl;
  t2 = std::chrono::high_resolution_clock::now();
  std::cout<< "Initial guess calculated" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  newton_raphson(F_funct,J_funct,guess,pow(10,-15));
  std::cout << guess << std::endl;

  return 0;
}


#include <cstdio>
#include <iostream>
#include <vector>
#include <NTL/ZZ.h>
#include <NTL/tools.h>

using namespace std;
using namespace NTL;

#define REPORT_INTERVAL	100000
#define MAX_Q_BITS 255

/*
  This code modifies [Algorithm 1, BN06] to search for candidate parameters of
  Barreto-Naehrig curves with high 2-adicity. (The curve itself can be explicitly
  constructed by following the second half of [Algorithm 1, BN06].)

  This code was used to find the BN curve in [BCTV13].

  [BCTV13] = "Succinct Non-Interactive Arguments for a von Neumann Architecture"
  [BN06]   = "Pairing-Friendly Elliptic Curves of Prime Order"
*/

int main(int argc, char **argv)
{
    if (argc != 4)
    {
        cout << "usage: " << argv[0] << " [rand_seed even_offset wanted_two_adicity]\n";
        return 0;
    }

    /* Collect inputs */
    ZZ seed;
    conv(seed, atoi(argv[1]));
    long even_offset = atoi(argv[2]);
    long wanted_two_adicity = atoi(argv[3]);

    /* |x| ~ 64 bits so that |q| = 4 * |x| ~ 256 bits */
    long num_x_bits = 63;

    long num_iters = 0;
    long num_found = 0;

    SetSeed(seed);
    while (1)
    {
        ++num_iters;

        /* Sample x */
        ZZ x = RandomLen_ZZ(num_x_bits);

        /**
         * Make x even and divisible by a large power of 2 to improve two adicity.
         * The resulting q is s.t. -1 is a square in Fq.
         */
        x = ((x >> even_offset) << even_offset);

        /* Uncomment to make x odd and ensure that -1 is a nonsquare in Fq. */
        // SetBit(x, 0);

        /**
         * Compute candidate BN parameters using the formulas:
         * t = 6*x^2 + 1,
         * q = 36*x^4 + 36*x^3 + 24*x^2 + 6*x + 1
         * r = q - t + 1
         * (see [BN06])
         */
        ZZ x2 = x * x;
        ZZ x3 = x2 * x;
        ZZ x4 = x3 * x;
        ZZ t = 6 * x2 + 1;
        ZZ q = 36 * x4 + 36 * x3 + 24 * x2 + 6 * x + 1;
        ZZ r = q - t + 1;

        long num_q_bits = NumBits(q);
        long two_adicity = NumTwos(r-1);

        if (num_q_bits > MAX_Q_BITS)
        {
            continue;
        }

        if (ProbPrime(r) && ProbPrime(q) && (two_adicity >= wanted_two_adicity))
        {
            cout << "x = " << x << "\n";
            cout << "q = " << q << "\n";
            cout << "r = " << r << "\n";
            cout << "log2(q) =" << num_q_bits << "\n";
            cout << "ord_2(r-1) = " << two_adicity << "\n";
            cout.flush();
            ++num_found;
        }

        if (num_iters % REPORT_INTERVAL == 0)
        {
            printf("[ num_iters = %ld , num_found = %0.2f per %d ]\n", num_iters, 1.*REPORT_INTERVAL*num_found/num_iters, REPORT_INTERVAL);
            fflush(stdout);
        }
    }
}


#pragma once

#include <boost/multiprecision/cpp_int.hpp>

#include "Algorithms.hh"
#include "IsPrimeGenerator.hh"
#include "Typedefs.hh"

namespace cml {

struct RsaPublicKey {
    UnboundedInt e{ 0 };
    UnboundedInt n{ 0 };
};

struct RsaPrivateKey {
    UnboundedInt d{ 0 };
    UnboundedInt n{ 0 };
};

template <typename PrimeGeneratorType>
struct RsaProtocol {
    static_assert(IsPrimeGenerator<PrimeGeneratorType>::value,
                  "Invalid template argument for cml::RsaProtocol: PrimeGeneratorType "
                  "interface is not suitable");

    using PublicKey      = RsaPublicKey;
    using PrivateKey     = RsaPrivateKey;
    using PrimeGenerator = PrimeGeneratorType;

    RsaProtocol();
    explicit RsaProtocol(const PrimeGenerator& primeGenerator);

    void generate();

    UnboundedInt encrypt(const Uint64& source, const PublicKey& anotherPublicKey);
    std::vector<UnboundedInt> encrypt(const std::vector<Uint64>& source, const PublicKey& anotherPublicKey);

    Uint64 decrypt(const UnboundedInt& source);
    std::vector<Uint64> decrypt(const std::vector<UnboundedInt>& source);

    PublicKey publicKey{};
    PrivateKey privateKey{};
    PrimeGenerator primeGenerator{};
};

template <typename PrimeGeneratorType>
RsaProtocol<PrimeGeneratorType>::RsaProtocol() = default;

template <typename PrimeGeneratorType>
RsaProtocol<PrimeGeneratorType>::RsaProtocol(const PrimeGenerator& primeGenerator) : primeGenerator(primeGenerator)
{}

template <typename PrimeGeneratorType>
void RsaProtocol<PrimeGeneratorType>::generate()
{
    UnboundedInt p{ primeGenerator() };
    UnboundedInt q{ primeGenerator() };

    // Compute phi
    UnboundedInt phi = (p - 1) * (q - 1);

    // Compute 'n'
    UnboundedInt n = p * q;
    publicKey.n    = n;
    privateKey.n   = n;

    // Mersenne prime number
    publicKey.e = 65537;

    // Compute 'd'
    privateKey.d = invmod(publicKey.e, phi);
}

template <typename PrimeGeneratorType>
UnboundedInt RsaProtocol<PrimeGeneratorType>::encrypt(const Uint64& source, const PublicKey& anotherPublicKey)
{
    return modexp<UnboundedInt>(UnboundedInt{ source }, anotherPublicKey.e, anotherPublicKey.n);
}

template <typename PrimeGeneratorType>
std::vector<UnboundedInt> RsaProtocol<PrimeGeneratorType>::encrypt(const std::vector<Uint64>& source,
                                                                   const PublicKey& anotherPublicKey)
{
    std::vector<UnboundedInt> result{};
    result.resize(source.size());

    for (std::size_t i = 0; i < source.size(); ++i) {
        result[i] = encrypt(source[i], anotherPublicKey);
    }

    return result;
}

template <typename PrimeGeneratorType>
Uint64 RsaProtocol<PrimeGeneratorType>::decrypt(const UnboundedInt& source)
{
    return static_cast<Uint64>(modexp<UnboundedInt>(source, privateKey.d, privateKey.n));
}

template <typename PrimeGeneratorType>
std::vector<Uint64> RsaProtocol<PrimeGeneratorType>::decrypt(const std::vector<UnboundedInt>& source)
{
    std::vector<Uint64> result{};
    result.resize(source.size());

    for (std::size_t i = 0; i < source.size(); ++i) {
        result[i] = decrypt(source[i]);
    }

    return result;
}

} // namespace cml


#include <Eigen/Dense>
#include <Eigen/SVD>
#include <cmath>
#include "metrics.h"
#include <iostream>

using namespace Eigen;

namespace dmaps
{
	f_type rmsd(const vector_t& ri, const vector_t& rj, const vector_t& w)
	{
		// Copy vectors for manipulation.
		vector_t r1 = ri, r2 = rj;

		// Get subset of weights just to make sure the size is proper.
		const vector_t& ws = w.segment(0, ri.size()/3);

		// Maps for ease of use.
		Map<matrix3_t> xi(r1.data(), r1.size()/3, 3);
		Map<matrix3_t> xj(r2.data(), r2.size()/3, 3);

		// Subtract out centers of mass.
		f_type wtot = ws.sum();
		vector3_t comi = (ws.asDiagonal()*xi).colwise().sum()/wtot;
		vector3_t comj = (ws.asDiagonal()*xj).colwise().sum()/wtot;
		xi.rowwise() -= comi.transpose();
		xj.rowwise() -= comj.transpose();

		// SVD of covariance matrix.
		matrix_t cov = xi.transpose()*ws.asDiagonal()*xj;
		JacobiSVD<matrix_t> svd(cov, ComputeThinU | ComputeThinV);

		// Find rotation.
		f_type d = (svd.matrixV()*svd.matrixU().transpose()).determinant() > 0 ? 1 : -1;

		matrix33_t eye = matrix33_t::Identity(3, 3);
		eye(2, 2) = d;
		matrix33_t R = svd.matrixV()*eye*svd.matrixU().transpose();

		// Return rmsd.
		return std::sqrt((ws.asDiagonal()*(xi - xj*R).array().square().matrix()).sum()/wtot);
	}

	f_type euclidean(const vector_t& ri, const vector_t& rj, const vector_t& w)
	{
		return std::sqrt((w.array()*(ri-rj).array().square()).sum());
	}

	f_type contact_map(const vector_t& ri, const vector_t& rj, const vector_t&)
	{
		Map<const matrix3_t> xi(ri.data(), ri.size()/3, 3);
		Map<const matrix3_t> xj(rj.data(), rj.size()/3, 3);

		int irows = xi.rows(), jrows = xj.rows();
		vector_t dxi(irows*irows/2 - irows/2), dxj(jrows*jrows/2 - jrows/2);

		// Compute pairwise distances.
		int k = 0;
		for(int i = 0; i < irows - 1; ++i)
		{
			int nrows = irows - i - 1;
			dxi.segment(k, nrows) = (xi.bottomRows(nrows).rowwise() - xi.row(i)).matrix().rowwise().norm();
			k += nrows;
		}

		k = 0;
		for(int i = 0; i < jrows - 1; ++i)
		{
			int nrows = jrows - i - 1;
			dxj.segment(k, nrows) = (xj.bottomRows(nrows).rowwise() - xj.row(i)).matrix().rowwise().norm();
			k += nrows;
		}

		// Calculate distance metric normalization constants.
		f_type r0 = 0.35, n = 8, m = 12;

		dxi.array() = (1. - (dxi.array()/r0).pow(n))/(1. - (dxi.array()/r0).pow(m));
		dxj.array() = (1. - (dxj.array()/r0).pow(n))/(1. - (dxj.array()/r0).pow(m));

		f_type norm = std::sqrt(dxi.sum()*dxj.sum());

		return std::sqrt(1.0/norm*(dxi - dxj).array().square().sum());
	}
}


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

// Implementation File
// Look at polynomial_basis.h for documentation
#include <traj_opt_pro/polynomial_basis.h>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/pointer_cast.hpp>
#include <boost/range/irange.hpp>

#include <iostream>
#include <stdexcept>
#include <vector>

namespace traj_opt {

Poly PolyCalculusPro::bernstein_polynomial(typename Poly::size_type n,
                                           typename Poly::size_type i) {
  std::vector<decimal_t> t_dat({0, 1});
  Poly t(t_dat.data(), 1);  // t
  std::vector<decimal_t> omt_dat({1, -1});
  Poly omt(omt_dat.data(), 1);  // 1-t

  Poly t_rasied_i(omt_dat.data(), 0);  // t to the ith power
  Poly omt_rasied_ni(t_rasied_i);      // (1-t) to the (n-i)th power

  for (uint c = 0; c < i; c++) t_rasied_i *= t;
  for (uint c = 0; c < n - i; c++) omt_rasied_ni *= omt;

  Poly result(t_rasied_i);
  result *= omt_rasied_ni;
  result *= boost::math::binomial_coefficient<decimal_t>(n, i);
  return result;
}
Poly PolyCalculusPro::chebyshev_polynomial(typename Poly::size_type n) {
  std::vector<decimal_t> omt_dat({1, -1});
  Poly omt(omt_dat.data(), 1);  //  1 - t
  decimal_t one = 1;
  decimal_t zero = 0;

  Poly omt_raised_k(&one, 0);  // (1-t) to the kth power
  if (n == 0) return omt_raised_k;

  Poly result(&zero, 0);

  for (uint k = 0; k <= n; k++) {
    decimal_t coeff = std::pow(-2.0, k) * decimal_t(n) / decimal_t(n + k);
    coeff *= boost::math::binomial_coefficient<decimal_t>(n + k, 2 * k);
    // std::cout << "sub chevy " << coeff*omt_raised_k << std::endl;
    result += coeff * omt_raised_k;
    omt_raised_k *= omt;
  }

  return result;
}
Poly PolyCalculusPro::shifted_legendre(typename Poly::size_type n) {
  // shifted legensdre polynomial of order n
  typename std::vector<decimal_t> v;
  for (typename Poly::size_type k = 0; k <= n; k++) {
    v.push_back(boost::math::binomial_coefficient<decimal_t>(n, k) *
                boost::math::binomial_coefficient<decimal_t>(n + k, k) *
                std::pow(-1.0, k + n));
  }
  Poly result(v.data(), n);
  return result;
}

uint Basis::dim() { return n_p; }

// // switched these constructors to use new generic one
// BasisBundle::BasisBundle(uint n_p_, uint k_r_)
//     : BasisBundle(LEGENDRE, n_p_, k_r_) {}
// BasisBundle::BasisBundle(int n) : BasisBundle(BEZIER, n, 0) {}

LegendreBasis::LegendreBasis(uint n_p_, uint k_r_) : StandardBasis(0) {
  orthogonal_ = true;
  n_p = n_p_;
  k_r = k_r_;
  std::vector<decimal_t> simple;
  simple.push_back(1.0);
  for (uint i = 0; i < k_r; i++) {
    Poly poly(simple.data(), simple.size() - 1);
    polys.push_back(poly);
    simple.back() = 0.0;
    simple.push_back(1.0);
  }
  // does this line break ooqp? yes it does
  if (n_p == k_r) return;
  for (uint p = 0; p <= n_p - k_r; p++) {
    Poly p_cur = PolyCalculusPro::shifted_legendre(p);
    // std::cout << "shifted p " << p_cur << std::endl;
    for (uint i = 0; i < k_r; i++) {
      p_cur = PolyCalculus::integrate(p_cur);
    }
    polys.push_back(p_cur);
  }
}

BezierBasis::BezierBasis(uint n_p_) : StandardBasis(0) {
  n_p = n_p_;  // this was commented out, why?
  type_ = PolyType::BEZIER;
  for (int i = 0; i <= static_cast<int>(n_p); i++)
    polys.push_back(PolyCalculusPro::bernstein_polynomial(n_p, i));
}
ChebyshevBasis::ChebyshevBasis(uint n_p_) : StandardBasis(0) {
  n_p = n_p_;
  type_ = PolyType::CHEBYSHEV;
  std::vector<decimal_t> simple;
  simple.push_back(1.0);
  for (uint i = 0; i <= n_p; i++) {
    polys.push_back(PolyCalculusPro::chebyshev_polynomial(i));
  }

  // std::cout << "Chebyshev "  << *this << std::endl;
}
EndPointBasis::EndPointBasis(uint n_p_) : StandardBasis(0) {
  n_p = n_p_;  // this was commented out, why?
  type_ = PolyType::ENDPOINT;
  traj_opt::MatD coeffs = MatD::Zero(n_p_ + 1, n_p + 1);
  for (int i = 0; i <= static_cast<int>(n_p); i++) {
    if (i % 2 == 0) {
      coeffs(i, i / 2) = boost::math::factorial<decimal_t>(i / 2);

    } else {
      for (int j = 0; j <= static_cast<int>(n_p - i / 2); j++) {
        if (i < 2)
          coeffs(i, j) = 1;
        else
          coeffs(i, j + i / 2) = coeffs(i - 2, j + i / 2) * (j + 1);
      }
    }
  }
  traj_opt::MatD coeffsi = coeffs.inverse();

  for (int i = 0; i <= static_cast<int>(n_p); i++) {
    std::vector<decimal_t> data;
    for (int j = 0; j <= static_cast<int>(n_p); j++) {
      //      data.push_back(coeffsi(i, j));
      if (std::abs(coeffsi(j, i)) > 1e-12) data.push_back(coeffsi(j, i));
      //            data.push_back(coeffsi(i, j));
      else
        data.push_back(0.0);
    }
    polys.push_back(Poly(data.data(), n_p));
  }
  //  std::cout << "Endpoint M " << coeffs << std::endl;
  //  std::cout << "Endpoint Minv " << coeffsi << std::endl;
  //  std::cout << "Basis " << *this << std::endl;

  //  std::cout << "Endpoint np " << n_p << " poly size " << polys.size()
  //  <<std::endl;
}

decimal_t LegendreBasis::innerproduct(uint i, uint j) const {
  if (i != j)
    return 0;
  else
    return StandardBasis::innerproduct(i, j);
}
Poly StandardBasis::getPoly(uint i) const { return polys.at(i); }
StandardBasis::StandardBasis(uint n) : Basis(n) {
  type_ = PolyType::STANDARD;
  if (n == 0) return;
  std::vector<decimal_t> simple;
  simple.push_back(1.0);
  for (uint i = 0; i <= n_p; i++) {
    Poly poly(simple.data(), simple.size() - 1);
    polys.push_back(poly);
    simple.back() = 0.0;
    simple.push_back(1.0);
  }
}
BasisTransformer::BasisTransformer(boost::shared_ptr<StandardBasis> from,
                                   int derr) {
  int n = from->dim();
  boost::shared_ptr<StandardBasis> to = boost::make_shared<StandardBasis>(n);
  *this = BasisTransformer(from, to, derr);
}
BasisTransformer::BasisTransformer(boost::shared_ptr<StandardBasis> from,
                                   boost::shared_ptr<StandardBasis> to,
                                   int derr)
    : to_(to), from_(from) {
  //  assert(to_->dim() == from_->dim());
  n = static_cast<int>(from_->dim()) + 1;
  A = MatD::Zero(n - derr, n);
  //  std::cout << "Rows " << A.rows() << " , " << A.cols() << std::endl;
  for (int i = 0; i < n - derr; i++) {
    Poly pi = from_->getPoly(i + derr);
    for (int j = 0; j < static_cast<int>(pi.size()); j++) {
      //      std::cout << "i,j " << i << " , " << j << std::endl;
      A(i, j + derr) = pi[j];
    }
  }
  B = MatD::Zero(n - derr, n - derr);
  for (int i = 0; i < n - derr; i++) {
    Poly qi = to_->getPoly(i);
    for (int j = 0; j < static_cast<int>(qi.size()); j++) {
      //      std::cout << "i,j " << i << " , " << j << std::endl;
      B(i, j) = qi[j];
    }
  }
  //  Ainv = A.inverse();
  Binv = B.inverse();
  // draw your communitive diagram to see where these come from
  //  basisbasis_ = Binv * A;
  basisbasis_ = (A * Binv).transpose();

  //   std::cout << "Debug A: " << A << std::endl;
  //   std::cout << "Debug B: " << B << std::endl;
  //   std::cout << "Debug Ainv: " << Ainv << std::endl;
  //   std::cout << "Debug Binv: " << Binv << std::endl;
  //   std::cout << "BB " << basisbasis_ << std::endl;
}
const MatD &BasisTransformer::getBasisBasisTransform() {
  //  std::cout << "BB " << basisbasis_ << std::endl;
  return basisbasis_;
}
const MatD &BasisTransformer::getLinearTransform(decimal_t a, decimal_t b) {
  decimal_t data[2];
  data[0] = b;
  data[1] = a;  // remember boost's convention is backward from matlabs
  Poly fac(data, 1);
  decimal_t one = 1;
  Poly base(&one, 0);
  MatD mat = MatD::Zero(n, n);
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < static_cast<int>(base.size()); j++) {
      mat(i, j) = base[j];
    }
    base = base * fac;
  }

  //  std::cout << "Debug mat: " << mat << std::endl;
  scaledtranform_ = Ainv * mat * A;  // more communitive diagrams
  return scaledtranform_;
}
boost::shared_ptr<Basis> BasisBundle::getBasis(int i) {
  if (i >= 0)
    return derrivatives.at(i);
  else
    return integrals.at(-i - 1);
}
BasisBundlePro::BasisBundlePro(PolyType type, uint n_p_, uint k_r_) {
  n_p = n_p_;
  k_r = k_r_;
  // only computer the first 4 derrivatives
  derrivatives.reserve(10);
  // computer the integral too, preferable use i7 to do computering
  integrals.reserve(1);

  for (auto i : boost::irange(0, 11)) {
    boost::shared_ptr<Basis> base;
    //    std::cout << "np " << n_p << std::endl;
    //    std::cout << "np2 " << n_p_ << std::endl;

    if (type == LEGENDRE)
      base = boost::make_shared<LegendreBasis>(n_p, k_r);
    else if (type == STANDARD)
      base = boost::make_shared<StandardBasis>(n_p);
    else if (type == BEZIER)
      base = boost::make_shared<BezierBasis>(n_p);
    else if (type == ENDPOINT)
      base = boost::make_shared<EndPointBasis>(n_p);
    else if (type == CHEBYSHEV)
      base = boost::make_shared<ChebyshevBasis>(n_p);
    else
      throw std::runtime_error("Unknown basis type");

    if (i == 11) {
      base->integrate();
      integrals.push_back(base);
    } else {
      for (int j = 0; j < i; j++) base->differentiate();
      derrivatives.push_back(base);
    }
  }
}
}  // namespace traj_opt


#include <Eigen/Dense>
#include <fmt/core.h>
#include <fmt/ranges.h>

#include <algorithm>
#include <array>
#include <fstream>
#include <iostream>
#include <numeric>
#include <optional>
#include <sstream>
#include <stack>
#include <string>
#include <vector>

using Mat = Eigen::MatrixXi;

auto makeMat(std::vector<std::string> const &lines) {
  Mat m(10, 10);
  m.setZero();

  for (auto i = 0; i < lines.size(); ++i) {
    auto row = lines[i];
    for (auto j = 0; j < row.size(); ++j) {
      std::string val;
      val += row[j];
      m(i, j) = std::stoi(val);
    }
  }

  return m;
}

auto parseFile(char const *file_name) {
  auto file = std::ifstream(file_name);
  std::string line;
  std::vector<std::string> out;
  while (std::getline(file, line)) {
    out.push_back(line);
  }

  return makeMat(out);
}

std::size_t absorb(Mat &m, int i, int j) {
  if (i < 0 || i >= m.rows() || j < 0 || j >= m.cols() || m(i, j) == 0) {
    return 0; // Don't absorb if we don't exist or already flashed
  }

  m(i, j) += 1;
  std::size_t out = 0;
  if (m(i, j) > 9) { // Flash
    m(i, j) = 0;
    out += 1; // Count our flash
    out += absorb(m, i + 1, j);
    out += absorb(m, i - 1, j);
    out += absorb(m, i, j + 1);
    out += absorb(m, i, j - 1);
    out += absorb(m, i + 1, j + 1);
    out += absorb(m, i - 1, j + 1);
    out += absorb(m, i - 1, j - 1);
    out += absorb(m, i + 1, j - 1);
  }

  return out;
}

std::size_t step(Mat &m) {
  // Increase energy levels by one
  for (auto r = 0; r < m.rows(); ++r) {
    for (auto c = 0; c < m.cols(); ++c) {
      m(r, c) += 1;
    }
  }

  std::size_t out = 0;
  for (auto r = 0; r < m.rows(); ++r) {
    for (auto c = 0; c < m.cols(); ++c) {
      if (m(r, c) > 9) {
        m(r, c) = 0;
        out += 1;
        out += absorb(m, r + 1, c);
        out += absorb(m, r - 1, c);
        out += absorb(m, r, c + 1);
        out += absorb(m, r, c - 1);
        out += absorb(m, r + 1, c + 1);
        out += absorb(m, r - 1, c + 1);
        out += absorb(m, r - 1, c - 1);
        out += absorb(m, r + 1, c - 1);
      }
    }
  }

  return out;
}

int main(int _, char **argv) {
  auto M = parseFile(argv[1]);
  std::cout << "M:\n" << M << "\n\n";

  auto Mp1 = M;
  std::size_t sum = 0;
  for(auto i = 0; i < 100; ++i){
    sum += step(Mp1);
  }
  fmt::print("Part1 score: {}\n", sum);

  auto day = 0;
  while(M.sum() != 0){
    ++day;
    step(M);
  }
  fmt::print("Part2 day: {}\n", day);
}


/// @file
/// Interpolation for Lie groups.

#pragma once

#include <Eigen/Eigenvalues>

#include "interpolate_details.hpp"

namespace Sophus {

/// This function interpolates between two Lie group elements ``foo_T_bar``
/// and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1].
///
/// It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar``
/// and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns
/// ``foo_T_baz``.
///
/// (Since interpolation on Lie groups is inverse-invariant, we can equivalently
/// think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the
/// return value being ``quiz_T_foo``.)
///
/// Precondition: ``p`` must be in [0, 1].
///
template <class G, class Scalar2 = typename G::Scalar>
enable_if_t<interp_details::Traits<G>::supported, G> interpolate(
    G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) {
  using Scalar = typename G::Scalar;
  Scalar inter_p(p);
  SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1),
                "p ({}) must in [0, 1].", inter_p);
  return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log());
}

}  // namespace Sophus


// Copyright (C) 2018 Thanaphon Chavengsaksongkram <as12production@gmail.com>, He Sun <he.sun@ed.ac.uk>
// This file is subject to the license terms in the LICENSE file
// found in the top-level directory of this distribution.

/**
 * Example-5.cpp
 *
 * This example perform Spectral Sparsification, and use Spectra to calcualte Eigenvalue of Graph Laplacians
 *
 * */

#include <gSparse/gSparse.hpp>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/SparseGenMatProd.h>

Eigen::VectorXcd GetTopEigenValue(const gSparse::SparsePrecisionMatrix & M)
{
    std::cout << "Calculating Eigen Value"<<std::endl;
    using namespace Spectra;
    SparseGenMatProd<double> op(M);

    // Construct eigen solver object, requesting the largest three eigenvalues
    GenEigsSolver< double, LARGEST_MAGN, SparseGenMatProd<double> > eigs(&op, 3, 6);

    // Initialize and compute
    eigs.init();
    int nconv = eigs.compute();

    // Retrieve results
    Eigen::VectorXcd evalues;
    if(eigs.info() == SUCCESSFUL)
        evalues = eigs.eigenvalues();

    std::cout << "Eigenvalues found:\n" << evalues << std::endl;
    return evalues;
}
int main()
{
    // Generate 100x100 Complete Graph
    auto graph = gSparse::Builder::buildUnitCompleteGraph(100);
    // Creating Sparsifier Object
    gSparse::SpectralSparsifier::ERSampling sparsifier(graph);
    // Set Hyper-parameters
    // Approximate the Effective Weight Resistance (Faster)
    // C = 100 and Epsilon = 0.5
    sparsifier.SetERPolicy(gSparse::SpectralSparsifier::APPROXIMATE_ER);
    sparsifier.SetC(100.0);
    sparsifier.SetEpsilon(0.5);
    // Compute Effective Weight Resistance using ApproxER
    sparsifier.Compute();
    // Get a sparsified graph
    auto sparseGraph1 = sparsifier.GetSparsifiedGraph();
    // Set to EXACT ER
    sparsifier.SetERPolicy(gSparse::SpectralSparsifier::EXACT_ER);
    // Re-calcuate ER using ExactER
    sparsifier.Compute();
    // Get a sparsified graph
    auto sparseGraph2 = sparsifier.GetSparsifiedGraph();

    // Use Spectra to calculate top Eigen values.
    std::cout<<"Original Eigen Value " <<std::endl;
    GetTopEigenValue(graph->GetLaplacianMatrix());
    std::cout<<"---------------------------"<<std::endl;
    std::cout<<"Top Eigen Value for ApproxER "<<std::endl;
    GetTopEigenValue(sparseGraph1->GetLaplacianMatrix());
    std::cout<<"Top Eigen Value for ApproxER - Original"<<std::endl;
    GetTopEigenValue(sparseGraph1->GetLaplacianMatrix() - graph->GetLaplacianMatrix());
    std::cout<<"---------------------------"<<std::endl;
    std::cout<<"Top Eigen Value for ExactER "<<std::endl;
    GetTopEigenValue(sparseGraph2->GetLaplacianMatrix());
    std::cout<<"Top Eigen Value for ApproxER - ExactER"<<std::endl;
    GetTopEigenValue(sparseGraph1->GetLaplacianMatrix() - graph->GetLaplacianMatrix());
    return 0;
}


/**
 * @file nested_cylinders.cc
 * @brief Compute the convergence of the nested cylinders experiment
 */

#define _USE_MATH_DEFINES
#include <annulus_triag_mesh_builder.h>
#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/utils.h>
#include <lf/quad/quad.h>
#include <mesh_function_velocity.h>
#include <norms.h>
#include <piecewise_const_element_matrix_provider.h>
#include <piecewise_const_element_vector_provider.h>
#include <solution_to_mesh_data_set.h>

#include <algorithm>
#include <boost/program_options.hpp>
#include <cstring>
#include <filesystem>
#include <iomanip>
#include <iostream>
#include <string>

using lf::uscalfe::operator-;

/**
 * @brief Solve the nested cylinders problem with zero potential at the boundary
 * @param mesh A shared pointer to the mesh on which to solve the PDE
 * @param dofh The dofhandler used for the simulation
 * @param r The radius of the inner cylinder
 * @param R The radius of the outer cylinder
 * @param omega1 The angular velocity of the inner cylinder
 * @param omega2 The angular velocity of the outer cylinder
 * @param modified_penalty If true, use the modified penalty term instead of the
 * original one
 * @returns A vector of basis function coefficients for the solution
 */
Eigen::VectorXd solveNestedCylindersZeroBC(
    const std::shared_ptr<const lf::mesh::Mesh> &mesh,
    const lf::assemble::DofHandler &dofh, double r, double R, double omega1,
    double omega2, bool modified_penalty) {
  // The volume forces are equal to zero everywhere
  auto f = [](const Eigen::Vector2d & /*unused*/) {
    return Eigen::Vector2d::Zero();
  };
  // Drive the inner and outer cylinder with omega1 and omega2
  const double eps = 1e-10;
  auto dirichlet_funct = [&](const lf::mesh::Entity &edge) -> Eigen::Vector2d {
    const auto *const geom = edge.Geometry();
    const auto vertices = geom->Global(edge.RefEl().NodeCoords());
    if (vertices.col(0).norm() <= R + eps &&
        vertices.col(0).norm() >= R - eps &&
        vertices.col(1).norm() <= R + eps &&
        vertices.col(1).norm() >= R - eps) {
      return omega2 * R * (vertices.col(1) - vertices.col(0)).normalized();
    }
    if (vertices.col(0).norm() <= r + eps &&
        vertices.col(0).norm() >= r - eps &&
        vertices.col(1).norm() <= r + eps &&
        vertices.col(1).norm() >= r - eps) {
      return omega1 * r * (vertices.col(0) - vertices.col(1)).normalized();
    }
    return Eigen::Vector2d::Zero();
  };

  lf::mesh::utils::CodimMeshDataSet<Eigen::Vector2d> dirichlet(mesh, 1);
  for (const auto *ep : mesh->Entities(1)) {
    dirichlet(*ep) = dirichlet_funct(*ep);
  }

  // Asemble the LSE
  const auto boundary = lf::mesh::utils::flagEntitiesOnBoundary(mesh);
  // Assemble the Matrix
  lf::assemble::COOMatrix<double> A(dofh.NumDofs(), dofh.NumDofs());
  const projects::ipdg_stokes::assemble::PiecewiseConstElementMatrixProvider
      elem_mat_provider(100, boundary, modified_penalty);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elem_mat_provider, A);
  // Assemble the right hand side
  Eigen::VectorXd rhs = Eigen::VectorXd::Zero(dofh.NumDofs());
  const projects::ipdg_stokes::assemble::PiecewiseConstElementVectorProvider
      elem_vec_provider(100, f, lf::quad::make_TriaQR_MidpointRule(), boundary,
                        dirichlet);
  lf::assemble::AssembleVectorLocally(0, dofh, elem_vec_provider, rhs);

  // Enforce the no-flow boundary conditions
  auto selector = [&](lf::base::size_type idx) -> std::pair<bool, double> {
    const auto &entity = dofh.Entity(idx);
    if (entity.RefEl() == lf::base::RefElType::kPoint && boundary(entity)) {
      return {true, 0};
    }
    return {false, 0};
  };
  lf::assemble::FixFlaggedSolutionComponents(selector, A, rhs);

  // Solve the LSE using sparse LU
  Eigen::SparseMatrix<double> As = A.makeSparse();
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(As);
  return solver.solve(rhs);
}

/**
 * @brief Solve the nested cylinders problem with constant potential at the
 * boundary
 * @param mesh A shared pointer to the mesh on which to solve the PDE
 * @param dofh The dofhandler used for the simulation
 * @param r The radius of the inner cylinder
 * @param R The radius of the outer cylinder
 * @param omega1 The angular velocity of the inner cylinder
 * @param omega2 The angular velocity of the outer cylinder
 * @param modified_penalty If true, use the modified penalty term instead of the
 * original one
 * @returns A vector of basis function coefficients for the solution
 */
Eigen::VectorXd solveNestedCylindersNonzeroBC(
    const std::shared_ptr<const lf::mesh::Mesh> &mesh,
    const lf::assemble::DofHandler &dofh, double r, double R, double omega1,
    double omega2, bool modified_penalty) {
  // The volume forces are equal to zero everywhere
  auto f = [](const Eigen::Vector2d & /*unused*/) {
    return Eigen::Vector2d::Zero();
  };
  // Drive the inner and outer cylinder with omega1 and omega2
  const double eps = 1e-10;
  auto dirichlet_funct = [&](const lf::mesh::Entity &edge) -> Eigen::Vector2d {
    const auto *const geom = edge.Geometry();
    const auto vertices = geom->Global(edge.RefEl().NodeCoords());
    if (vertices.col(0).norm() <= R + eps &&
        vertices.col(0).norm() >= R - eps &&
        vertices.col(1).norm() <= R + eps &&
        vertices.col(1).norm() >= R - eps) {
      return omega2 * R * (vertices.col(1) - vertices.col(0)).normalized();
    }
    if (vertices.col(0).norm() <= r + eps &&
        vertices.col(0).norm() >= r - eps &&
        vertices.col(1).norm() <= r + eps &&
        vertices.col(1).norm() >= r - eps) {
      return omega1 * r * (vertices.col(0) - vertices.col(1)).normalized();
    }
    return Eigen::Vector2d::Zero();
  };

  lf::mesh::utils::CodimMeshDataSet<Eigen::Vector2d> dirichlet(mesh, 1);
  for (const auto *ep : mesh->Entities(1)) {
    dirichlet(*ep) = dirichlet_funct(*ep);
  }

  // Asemble the LSE
  const auto boundary = lf::mesh::utils::flagEntitiesOnBoundary(mesh);
  // Assemble the Matrix
  lf::assemble::COOMatrix<double> A(dofh.NumDofs(), dofh.NumDofs());
  const projects::ipdg_stokes::assemble::PiecewiseConstElementMatrixProvider
      elem_mat_provider(100, boundary, modified_penalty);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elem_mat_provider, A);
  // Assemble the right hand side
  Eigen::VectorXd rhs = Eigen::VectorXd::Zero(dofh.NumDofs());
  const projects::ipdg_stokes::assemble::PiecewiseConstElementVectorProvider
      elem_vec_provider(100, f, lf::quad::make_TriaQR_MidpointRule(), boundary,
                        dirichlet);
  lf::assemble::AssembleVectorLocally(0, dofh, elem_vec_provider, rhs);

  // Combine the basis functions on the inside boundary to a single one
  const lf::base::size_type M_outer = 0;
  const lf::base::size_type M_inner = 1;
  // Create a mapping of DOF indices to remove the afterwards unused DOFs
  std::vector<lf::base::size_type> dofmap(dofh.NumDofs());
  lf::base::size_type idx = 2;
  for (lf::base::size_type dof = 0; dof < dofh.NumDofs(); ++dof) {
    const auto &entity = dofh.Entity(dof);
    const auto *const geom = entity.Geometry();
    if (entity.RefEl() == lf::base::RefElType::kPoint && boundary(entity)) {
      if (geom->Global(entity.RefEl().NodeCoords()).norm() > R - eps) {
        dofmap[dof] = M_outer;
      } else {
        dofmap[dof] = M_inner;
      }
    } else {
      dofmap[dof] = idx++;
    }
  }
  // Apply this mapping to the triplets of the matrix
  std::for_each(A.triplets().begin(), A.triplets().end(),
                [&](Eigen::Triplet<double> &trip) {
                  trip = Eigen::Triplet<double>(
                      dofmap[trip.row()], dofmap[trip.col()], trip.value());
                });
  // Apply the mapping to the right hand side vector
  Eigen::VectorXd rhs_mapped = Eigen::VectorXd::Zero(idx);
  for (lf::base::size_type dof = 0; dof < dofh.NumDofs(); ++dof) {
    rhs_mapped[dofmap[dof]] += rhs[dof];
  }

  // Set the potential on the outer boundary to zero
  auto selector = [&](lf::base::size_type idx) -> std::pair<bool, double> {
    return {idx == M_outer, 0};
  };
  lf::assemble::FixFlaggedSolutionComponents(selector, A, rhs);

  // Solve the LSE using sparse LU
  Eigen::SparseMatrix<double> As_mapped = A.makeSparse().block(0, 0, idx, idx);
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(As_mapped);
  const Eigen::VectorXd sol_mapped = solver.solve(rhs_mapped);

  // Apply the inverse mapping to recovr the basis function coefficients for the
  // original basis functions
  Eigen::VectorXd sol = Eigen::VectorXd::Zero(dofh.NumDofs());
  for (lf::base::size_type dof = 0; dof < dofh.NumDofs(); ++dof) {
    sol[dof] = sol_mapped[dofmap[dof]];
  }

  return sol;
}

/**
 * @brief Concatenate objects defining an operator<<(std::ostream&)
 * @param args A variadic pack of objects implementing
 * `operator<<(std::ostream&)`
 * @returns A string with the objects concatenated
 */
template <typename... Args>
static std::string concat(Args &&... args) {
  std::ostringstream ss;
  (ss << ... << args);
  return ss.str();
}

/**
 * @brief outputs the L2 and DG norm errors for the nested cylinders experiment
 *
 * Three different command line arguments can be provided:
 *   - builder
 *   - files
 *   - irregular
 *
 * Providing builder as a command line argument will build the meshes with
 * #AnnulusTriagMeshBuilder. Providing files as a command line argument will use
 * uniform meshes generated by GMSH. Providing irregular as a command line
 * argument will use meshes with a sudden jump in mesh resolution.
 */
int main(int argc, char *argv[]) {
  const double r = 0.25;
  const double R = 1;
  const double omega1 = 0;
  const double omega2 = 1;

  // Parse the command line options
  std::string mesh_selection;
  boost::program_options::options_description desc{"Options"};
  desc.add_options()("help,h", "Help Screen")(
      "type", boost::program_options::value<std::string>(&mesh_selection),
      "Type of mesh to use. Either 'builder', 'files' or 'irregular'");
  boost::program_options::positional_options_description pos_desc;
  pos_desc.add("type", 1);
  boost::program_options::command_line_parser parser{argc, argv};
  parser.options(desc).positional(pos_desc).allow_unregistered();
  boost::program_options::parsed_options po = parser.run();
  boost::program_options::variables_map vm;
  boost::program_options::store(po, vm);
  boost::program_options::notify(vm);
  if (vm.count("help") != 0U) {
    std::cout << desc << std::endl;
  }

  std::vector<std::shared_ptr<lf::mesh::Mesh>> meshes;
  if (mesh_selection == "files") {
    // Read the mesh from the gmsh file
    std::filesystem::path meshpath = __FILE__;
    meshpath = meshpath.parent_path();
    for (int i = 0; i <= 4; ++i) {
      const auto meshfile = meshpath / concat("annulus", std::setw(2),
                                              std::setfill('0'), i, ".msh");
      std::unique_ptr<lf::mesh::MeshFactory> factory =
          std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
      lf::io::GmshReader reader(std::move(factory), meshfile.string());
      meshes.push_back(reader.mesh());
    }
  } else if (mesh_selection == "builder") {
    // Build a sequence of meshes
    for (unsigned i = 0U; i < 8U; ++i) {
      const unsigned nx = 4U << i;
      const double dx = 2 * M_PI * (r + R) / 2 / nx;
      const unsigned ny = std::max(static_cast<unsigned>((R - r) / dx), 1U);

      // Build the mesh
      std::unique_ptr<lf::mesh::MeshFactory> factory =
          std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
      projects::ipdg_stokes::mesh::AnnulusTriagMeshBuilder builder(
          std::move(factory));
      builder.setInnerRadius(r);
      builder.setOuterRadius(R);
      builder.setNumAngularCells(nx);
      builder.setNumRadialCells(ny);
      meshes.push_back(builder.Build());
    }
  } else if (mesh_selection == "irregular") {
    std::filesystem::path meshpath = __FILE__;
    const auto mesh_irregular_path =
        meshpath.parent_path() / "annulus_irregular.msh";
    const auto mesh_irregular_inverted_path =
        meshpath.parent_path() / "annulus_irregular_inverted.msh";
    std::unique_ptr<lf::mesh::MeshFactory> factory_irregular =
        std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
    std::unique_ptr<lf::mesh::MeshFactory> factory_irregular_inverted =
        std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
    lf::io::GmshReader reader_irregular(std::move(factory_irregular),
                                        mesh_irregular_path.string());
    lf::io::GmshReader reader_irregular_inverted(
        std::move(factory_irregular_inverted),
        mesh_irregular_inverted_path.string());
    meshes.push_back(reader_irregular.mesh());
    meshes.push_back(reader_irregular_inverted.mesh());
  } else {
    std::cout << desc << std::endl;
    exit(1);
  }

  // Compute the analytic solution of the problem
  const double C1 = 2 * (omega1 * r * r - omega2 * R * R) / (r * r - R * R);
  const double C2 = ((omega1 - omega2) * r * r * R * R) / (R * R - r * r);
  auto analytic_velocity = [&](const Eigen::Vector2d &x) -> Eigen::Vector2d {
    const double radius = x.norm();
    Eigen::Vector2d vec;
    vec << x[1], -x[0];
    vec.normalize();
    return -(0.5 * C1 * radius + C2 / radius) * vec;
  };
  auto analytic_gradient = [&](const Eigen::Vector2d &x) -> Eigen::Matrix2d {
    const double r2 = x.squaredNorm();
    Eigen::Matrix2d g;
    g << 2 * C2 * x[0] * x[1] / r2 / r2,
        -C1 / 2 - (C2 * r2 - 2 * C2 * x[1] * x[1]) / r2 / r2,
        C1 / 2 + (C2 * r2 - 2 * C2 * x[0] * x[0]) / r2 / r2,
        -2 * C2 * x[0] * x[1] / r2 / r2;
    return g;
  };

  // Solve the problem on each mesh and compute the error
  for (const auto &mesh : meshes) {
    lf::assemble::UniformFEDofHandler dofh(
        mesh,
        {{lf::base::RefEl::kPoint(), 1}, {lf::base::RefEl::kSegment(), 1}});
    const auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh);
    const Eigen::VectorXd solution_zero =
        solveNestedCylindersZeroBC(mesh, dofh, r, R, omega1, omega2, false);
    const Eigen::VectorXd solution_nonzero =
        solveNestedCylindersNonzeroBC(mesh, dofh, r, R, omega1, omega2, false);
    const Eigen::VectorXd solution_zero_modified =
        solveNestedCylindersZeroBC(mesh, dofh, r, R, omega1, omega2, true);
    const Eigen::VectorXd solution_nonzero_modified =
        solveNestedCylindersNonzeroBC(mesh, dofh, r, R, omega1, omega2, true);
    // Create mesh functions for the analytic and numerical solutions
    const auto velocity_exact =
        lf::mesh::utils::MeshFunctionGlobal(analytic_velocity);
    const auto grad_exact =
        lf::mesh::utils::MeshFunctionGlobal(analytic_gradient);
    const auto velocity_zero =
        projects::ipdg_stokes::post_processing::MeshFunctionVelocity<double,
                                                                     double>(
            fe_space, solution_zero);
    const auto velocity_nonzero =
        projects::ipdg_stokes::post_processing::MeshFunctionVelocity<double,
                                                                     double>(
            fe_space, solution_nonzero);
    const auto velocity_zero_modified =
        projects::ipdg_stokes::post_processing::MeshFunctionVelocity<double,
                                                                     double>(
            fe_space, solution_zero_modified);
    const auto velocity_nonzero_modified =
        projects::ipdg_stokes::post_processing::MeshFunctionVelocity<double,
                                                                     double>(
            fe_space, solution_nonzero_modified);
    // Store the solution
    lf::io::VtkWriter writer_zero(
        mesh, concat("result_zero_", dofh.NumDofs(), ".vtk"));
    lf::io::VtkWriter writer_nonzero(
        mesh, concat("result_nonzero_", dofh.NumDofs(), ".vtk"));
    writer_zero.WriteCellData("velocity", velocity_zero);
    writer_zero.WriteCellData("velocity_modified", velocity_zero_modified);
    writer_nonzero.WriteCellData("velocity", velocity_nonzero);
    writer_nonzero.WriteCellData("velocity_modified",
                                 velocity_nonzero_modified);
    writer_zero.WriteCellData(
        "analytic", lf::mesh::utils::MeshFunctionGlobal(analytic_velocity));
    writer_nonzero.WriteCellData(
        "analytic", lf::mesh::utils::MeshFunctionGlobal(analytic_velocity));
    // Compute the difference between the numerical and the analytical solution
    auto diff_velocity_zero = velocity_zero - velocity_exact;
    auto diff_velocity_zero_modified = velocity_zero_modified - velocity_exact;
    auto diff_velocity_nonzero = velocity_nonzero - velocity_exact;
    auto diff_velocity_nonzero_modified =
        velocity_nonzero_modified - velocity_exact;
    auto diff_gradient_zero = -grad_exact;
    auto diff_gradient_zero_modified = -grad_exact;
    auto diff_gradient_nonzero = -grad_exact;
    auto diff_gradient_nonzero_modified = -grad_exact;
    const auto qr_provider = [](const lf::mesh::Entity &e) {
      return lf::quad::make_QuadRule(e.RefEl(), 0);
    };
    const double L2_zero = projects::ipdg_stokes::post_processing::L2norm(
        mesh, diff_velocity_zero, qr_provider);
    const double L2_nonzero = projects::ipdg_stokes::post_processing::L2norm(
        mesh, diff_velocity_nonzero, qr_provider);
    ;
    const double DG_zero = projects::ipdg_stokes::post_processing::DGnorm(
        mesh, diff_velocity_zero, diff_gradient_zero, qr_provider);
    const double DG_nonzero = projects::ipdg_stokes::post_processing::DGnorm(
        mesh, diff_velocity_nonzero, diff_gradient_nonzero, qr_provider);
    const double L2_zero_modified =
        projects::ipdg_stokes::post_processing::L2norm(
            mesh, diff_velocity_zero_modified, qr_provider);
    const double L2_nonzero_modified =
        projects::ipdg_stokes::post_processing::L2norm(
            mesh, diff_velocity_nonzero_modified, qr_provider);
    const double DG_zero_modified =
        projects::ipdg_stokes::post_processing::DGnorm(
            mesh, diff_velocity_zero_modified, diff_gradient_zero_modified,
            qr_provider);
    const double DG_nonzero_modified =
        projects::ipdg_stokes::post_processing::DGnorm(
            mesh, diff_velocity_nonzero_modified,
            diff_gradient_nonzero_modified, qr_provider);
    std::cout << mesh->NumEntities(2) << ' ' << L2_zero << ' ' << DG_zero << ' '
              << L2_nonzero << ' ' << DG_nonzero << ' ' << L2_zero_modified
              << ' ' << DG_zero_modified << ' ' << L2_nonzero_modified << ' '
              << DG_nonzero_modified << std::endl;
  }

  return 0;
}


// 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.

/**
 * @file odeint.cpp ODE numerical integration example.
 */

#include <boost/numeric/odeint.hpp>

#include <matplot/matplot.h>

#include "smooth/bundle.hpp"
#include "smooth/compat/odeint.hpp"
#include "smooth/so3.hpp"
#include "smooth/tn.hpp"

#include "plot_tools.hpp"

using matplot::plot;
using std::views::transform;

/**
 * @brief Numerically solve the following ODE on \f$ \mathbb{SO}(3) \times \mathbb{R}^3 \f$:
 *
 * \f[
 * \mathrm{d}^r X_t = v(t)  \\
 * \mathrm{d}^r v_t = -k_p * (X(t) \ominus X_{des}(t)) - k_d * v(t)
 * \f]
 */
int main(int argc, char const * argv[])
{
  using state_t = smooth::Bundle<smooth::SO3d, Eigen::Vector3d>;
  using deriv_t = typename state_t::Tangent;

  std::srand(5);

  // equilibrium point
  const smooth::SO3d Xdes = smooth::SO3d::Identity();

  // "control" proportional and derivative gains
  constexpr double kp = 1;
  constexpr double kd = 1;

  auto ode = [&](const state_t & state, deriv_t & deriv, double t) {
    deriv.template head<3>() = state.part<1>();
    deriv.template tail<3>() = -kp * (state.part<0>() - Xdes) - kd * state.part<1>();
  };

  state_t state;
  state.part<0>() = smooth::SO3d(Eigen::Quaterniond(0, 0.8, 0, 0.1));
  state.part<1>() = Eigen::Vector3d(0, 0, 2);

  std::vector<double> tvec;
  std::vector<state_t> gvec;

  auto stepper = boost::numeric::odeint::
    runge_kutta4<state_t, double, deriv_t, double, boost::numeric::odeint::vector_space_algebra>();

  boost::numeric::odeint::integrate_const(
    stepper, ode, state, 0., 10., 0.01, [&tvec, &gvec](const state_t & s, double t) {
      tvec.push_back(t);
      gvec.push_back(s);
    });

  matplot::figure();
  matplot::hold(matplot::on);
  // plot a sphere
  auto phi = matplot::linspace(0, 2 * M_PI, 200);
  for (double h = -0.9; h < 0.95; h += 0.2) {
    auto xsph = r2v(phi | transform([&](double p) { return std::sqrt(1. - h * h) * std::cos(p); }));
    auto ysph = r2v(phi | transform([&](double p) { return std::sqrt(1. - h * h) * std::sin(p); }));
    auto zsph = r2v(phi | transform([&](double p) { return h; }));
    matplot::plot3(xsph, ysph, zsph)->line_width(0.25).color("gray");
    matplot::plot3(ysph, zsph, xsph)->line_width(0.25).color("gray");
    matplot::plot3(zsph, xsph, ysph)->line_width(0.25).color("gray");
  }
  // plot the trajectory
  auto xyz =
    gvec | transform([](auto s) { return s.template part<0>() * Eigen::Vector3d::UnitZ(); });
  matplot::plot3(r2v(xyz | transform([](auto s) { return s.x(); })),
    r2v(xyz | transform([](auto s) { return s.y(); })),
    r2v(xyz | transform([](auto s) { return s.z(); })))
    ->line_width(4)
    .color("blue");
  matplot::title("Attitude");

  matplot::figure();
  matplot::hold(matplot::on);
  plot(tvec, r2v(gvec | transform([](auto s) { return s.template part<1>()[0]; })), "r")
    ->line_width(2);
  plot(tvec, r2v(gvec | transform([](auto s) { return s.template part<1>()[1]; })), "g")
    ->line_width(2);
  plot(tvec, r2v(gvec | transform([](auto s) { return s.template part<1>()[2]; })), "b")
    ->line_width(2);
  matplot::title("Velocity");

  matplot::show();

  return 0;
}