/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <iostream>
#include <memory>

#include <BayesFilters/AdditiveStateModel.h>
#include <BayesFilters/GaussianLikelihood.h>
#include <BayesFilters/GPFPrediction.h>
#include <BayesFilters/GPFCorrection.h>
#include <BayesFilters/InitSurveillanceAreaGrid.h>
#include <BayesFilters/Resampling.h>
#include <BayesFilters/SimulatedLinearSensor.h>
#include <BayesFilters/SimulatedStateModel.h>
#include <BayesFilters/SIS.h>
#include <BayesFilters/UKFPrediction.h>
#include <BayesFilters/UKFCorrection.h>
#include <BayesFilters/utils.h>
#include <BayesFilters/WhiteNoiseAcceleration.h>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

class UPFSimulation : public SIS
{
public:
    UPFSimulation
    (
        std::size_t num_particle,
        std::size_t state_size,
        std::size_t simulation_steps,
        /* Initial covariance of the Gaussian belief associated to each particle. */
        Ref<MatrixXd> initial_covariance,
        std::unique_ptr<ParticleSetInitialization> initialization,
        std::unique_ptr<PFPrediction> prediction,
        std::unique_ptr<PFCorrection> correction,
        std::unique_ptr<Resampling> resampling
    ) noexcept :
        SIS(num_particle, state_size, std::move(initialization), std::move(prediction), std::move(correction), std::move(resampling)),
        simulation_steps_(simulation_steps),
        initial_covariance_(initial_covariance)
    { }

protected:
    bool runCondition() override
    {
        if (getFilteringStep() < simulation_steps_)
            return true;
        else
            return false;
    }

    std::vector<std::string> log_filenames(const std::string& prefix_path, const std::string& prefix_name) override
    {
        std::vector<std::string> sis_filenames = SIS::log_filenames(prefix_path, prefix_name);

        /* Add file names for logging of the conditional expected value. */
        sis_filenames.push_back(prefix_path + "/" + prefix_name + "_mean");

        return  sis_filenames;
    }

    VectorXd mean_extraction(const ParticleSet& particles) const
    {
        /* Extract the conditional expected value of the filtered state
           given all the measurements up to the current time step. */
        return particles.state() * particles.weight().array().exp().matrix();
    }

    bool initialization() override
    {
        if (!SIS::initialization())
            return false;

        /* Initialize initial mean and covariance for each particle. */
        for (std::size_t i = 0; i < pred_particle_.components; i++)
        {
            /* Set mean equal to the particle position. */
            pred_particle_.mean(i) = pred_particle_.state(i);

            /* Set the covariance obtained within the ctor. */
            pred_particle_.covariance(i) = initial_covariance_;
        }

        return true;
    }

    void log() override
    {
        VectorXd mean = mean_extraction(cor_particle_);

        logger(pred_particle_.state().transpose(), pred_particle_.weight().transpose(),
               cor_particle_.state().transpose(), cor_particle_.weight().transpose(),
               mean.transpose());
    }

private:
    std::size_t simulation_steps_;

    Eigen::MatrixXd initial_covariance_;
};

int main()
{
    std::cout << "Running an unscented particle filter on a simulated target." << std::endl;
    std::cout << "Data is logged in the test folder with prefix testUPF." << std::endl;

    /* A set of parameters needed to run an unscented particle filter in a simulated environment. */
    double surv_x = 1000.0;
    double surv_y = 1000.0;
    std::size_t num_particle_x = 100;
    std::size_t num_particle_y = 100;
    std::size_t num_particle = num_particle_x * num_particle_y;
    Vector4d initial_state(10.0f, 0.0f, 10.0f, 0.0f);
    std::size_t simulation_time = 100;
    std::size_t state_size = 4;

    /* Unscented transform parameters.*/
    double alpha = 1.0;
    double beta = 2.0;
    double kappa = 0.0;

    /* Step 1 - Initialization */

    Matrix4d initial_covariance;
    initial_covariance << pow(0.05, 2), 0,            0,            0,
                          0,            pow(0.05, 2), 0,            0,
                          0,            0,            pow(0.01, 2), 0,
                          0,            0,            0,            pow(0.01, 2);

    /* Initialize particle initialization class. */
    std::unique_ptr<ParticleSetInitialization> grid_initialization = utils::make_unique<InitSurveillanceAreaGrid>(surv_x, surv_y, num_particle_x, num_particle_y);

    /* Step 2 - Prediction */

    /* Step 2.1 - Define the state model. */

    /* Initialize a white noise acceleration state model. */
    double T = 1.0f;
    double tilde_q = 10.0f;

    std::unique_ptr<AdditiveStateModel> wna = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);

    /* Step 2.2 - Define the prediction step */

    /* Initialize the kalman particle filter prediction step that wraps a Gaussian prediction step,
       in this case an unscented kalman filter prediction step. */
    std::unique_ptr<GaussianPrediction> upf_prediction = utils::make_unique<UKFPrediction>(std::move(wna), state_size, alpha, beta, kappa);
    std::unique_ptr<PFPrediction> gpf_prediction = utils::make_unique<GPFPrediction>(std::move(upf_prediction));

    /* Step 3 - Correction */

    /* Step 3.1 - Define where the measurement are originated from (simulated in this case). */

    /* Initialize simulated target model with a white noise acceleration. */
    std::unique_ptr<StateModel> target_model = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);
    std::unique_ptr<SimulatedStateModel> simulated_state_model = utils::make_unique<SimulatedStateModel>(std::move(target_model), initial_state, simulation_time);
    simulated_state_model->enable_log(".", "testUPF");

    /* Initialize a measurement model (a linear sensor reading x and y coordinates). */
    std::unique_ptr<AdditiveMeasurementModel> simulated_linear_sensor = utils::make_unique<SimulatedLinearSensor>(std::move(simulated_state_model));
    simulated_linear_sensor->enable_log(".", "testUPF");

    /* Step 3.2 - Define the likelihood model. */

    /* Initialize an exponential likelihood as measurement likelihood. */
    std::unique_ptr<LikelihoodModel> exp_likelihood = utils::make_unique<GaussianLikelihood>();

    /* Step 3.3 - Define the correction step. */

    /* An additional state model is required to make the transitionProbability of the state model available
       to the particle filter correction step. */
    std::unique_ptr<StateModel> transition_probability_model = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);

    /* Initialize the particle filter correction step that wraps a Guassian correction step,
       in this case an unscented kalman filter correction step. */
    std::unique_ptr<GaussianCorrection> upf_correction = utils::make_unique<UKFCorrection>(std::move(simulated_linear_sensor), state_size, alpha, beta, kappa);
    std::unique_ptr<PFCorrection> gpf_correction = utils::make_unique<GPFCorrection>(std::move(upf_correction), std::move(exp_likelihood), std::move(transition_probability_model));

    /* Step 4 - Resampling */

    /* Initialize a resampling algorithm. */
    std::unique_ptr<Resampling> resampling = utils::make_unique<Resampling>();

    /* Step 5 - Assemble the particle filter. */
    std::cout << "Constructing unscented particle filter..." << std::flush;
    UPFSimulation upf(num_particle, state_size, simulation_time, initial_covariance, std::move(grid_initialization), std::move(gpf_prediction), std::move(gpf_correction), std::move(resampling));
    upf.enable_log(".", "testUPF");
    std::cout << "done!" << std::endl;

    /* Step 6 - Prepare the filter to be run */
    std::cout << "Booting unscented particle filter..." << std::flush;
    upf.boot();
    std::cout << "completed!" << std::endl;

    /* Step 7 - Run the filter and wait until it is closed */
    /* Note that since this is a simulation, the filter will end upon simulation termination */
    std::cout << "Running unscented particle filter..." << std::flush;
    upf.run();
    std::cout << "waiting..." << std::flush;

    if (!upf.wait())
        return EXIT_FAILURE;

    std::cout << "completed!" << std::endl;

    return EXIT_SUCCESS;
}


#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d.hpp>
#include <iostream>
#include <armadillo>
#include "highgui.h"

using namespace std;
using namespace cv;

void detect(cv::Mat &frame) {
	int h1 = 100;
	int h2 = 179;
	int s1 = 125;
	int s2 = 255;
	int v1 = 50;
	int v2 = 170;
	cv::Mat hsv;
	cv::cvtColor(frame, hsv, COLOR_BGR2HSV);
	cv::Mat hsv2mask, hsv2;
	cv::inRange(hsv, cv::Scalar(h1, s1, v1), cv::Scalar(h2, s2, v2), hsv2mask);

	arma::mat I;
	cvt_opencv2arma(hsv2mask, I);
	if (arma::accu(I) < 10.0) {
		return;
	}
	arma::rowvec r = arma::sum(I, 0);
	arma::colvec c = arma::sum(I, 1);
	double midx = arma::sum(r * arma::cumsum(arma::ones<arma::vec>(r.n_elem))) / arma::sum(r);
	double midy = arma::sum(c % arma::cumsum(arma::ones<arma::vec>(c.n_elem))) / arma::sum(c);

	double covx = sqrt(arma::sum(r * arma::square(arma::cumsum(arma::ones<arma::vec>(r.n_elem)) - midx)) / arma::sum(r));
	double covy = sqrt(arma::sum(c % arma::square(arma::cumsum(arma::ones<arma::vec>(c.n_elem)) - midy)) / arma::sum(c));
	double estw = covx;
	double esth = estw;
	estw *= 3; // strange constants
	esth *= 3;

	arma::vec pos = arma::vec({ midx, midy });
	double width = covx * 3;
	cout << "pos: " << pos << endl;
	cout << "width: " << width << endl;
}

int main(int argc, char *argv[]) {
	if (argc != 8) {
		cout << "usage: ./test img h1 h2 s1 s2 v1 v2\n";
		return 1;
	}
	int h1 = atoi(argv[2]);
	int h2 = atoi(argv[3]);
	int s1 = atoi(argv[4]);
	int s2 = atoi(argv[5]);
	int v1 = atoi(argv[6]);
	int v2 = atoi(argv[7]);

	// convert to hsv
	VideoCapture cam(1);

	arma::vec xbuf(10, arma::fill::zeros);
	arma::vec ybuf(10, arma::fill::zeros);
	int xind = 0, yind = 0;
	bool stable = false;

	while (1) {
		Mat img;
		cam.read(img);
		Mat hsv;
		cvtColor(img, hsv, COLOR_BGR2HSV);

		// in range masking
		Mat hsv2mask, hsv2;
		inRange(hsv, Scalar(h1, s1, v1), Scalar(h2, s2, v2), hsv2mask);
		//hsv.copyTo(hsv2, hsv2mask);

		arma::mat I;
		cvt_opencv2arma(hsv2mask, I);
		arma::rowvec r = arma::sum(I, 0);
		arma::colvec c = arma::sum(I, 1);
		double midx = arma::sum(r * arma::cumsum(arma::ones<arma::vec>(r.n_elem))) / arma::sum(r);
		double midy = arma::sum(c % arma::cumsum(arma::ones<arma::vec>(c.n_elem))) / arma::sum(c);
		stable = true;
		if (midx < 1 || (midx > (int)I.n_cols - 1) || arma::sum(r) == 0) {
			midx = 0;
		}
		if (midy < 1 || (midy > (int)I.n_rows - 1) || arma::sum(c) == 0) {
			midy = 0;
		}

		// remove noise
		if (midx == 0 || midy == 0) {
			stable = false;
		}
		xbuf[xind] = midx;
		ybuf[yind] = midy;
		xind = (xind + 1) % (int)xbuf.n_elem;
		yind = (yind + 1) % (int)ybuf.n_elem;
		midx = arma::mean(xbuf);
		midy = arma::mean(ybuf);
		if (sqrt(arma::var(xbuf) * arma::var(xbuf) + arma::var(ybuf) + arma::var(ybuf)) > 70.0) {
			stable = false;
		}

		Vec3b color(255, 0, 0);
		cout << "arma:: " << midx << ", " << midy << endl;
		cout << "stable: " << stable << endl;
		circle(img, Point((int)midx, (int)midy), 4, color, 0);

		double covx = sqrt(arma::sum(r * arma::square(arma::cumsum(arma::ones<arma::vec>(r.n_elem)) - midx)) / arma::sum(r));
		double covy = sqrt(arma::sum(c % arma::square(arma::cumsum(arma::ones<arma::vec>(c.n_elem)) - midy)) / arma::sum(c));
		double estw = covx * 3;
		double esth = covy * 3;

		rectangle(img, Rect(midx-estw/2,midy-esth/2,estw,esth), Scalar(0, 0, 255), 2);

		// use histogram binning to get the center

		// create blob params
		/*SimpleBlobDetector::Params params;

		// Change thresholds
		params.minThreshold = 10;
		params.maxThreshold = 200;

		// Filter by Area.
		params.filterByArea = true;
		params.minArea = 1500;

		// Filter by Circularity
		params.filterByCircularity = true;
		params.minCircularity = 0.1;

		// Filter by Convexity
		params.filterByConvexity = true;
		params.minConvexity = 0.87;

		// Filter by Inertia
		params.filterByInertia = true;
		params.minInertiaRatio = 0.01;

		Ptr<SimpleBlobDetector> blob = SimpleBlobDetector::create(params);
		vector<KeyPoint> kp;
		Mat des;
		Mat hsvd = hsv2mask * -0.75 + 255;
		blob->detect(hsvd, kp);

		cout << "found " << kp.size() << " matches\n";

		Mat kpimg;
		drawKeypoints(img, kp, kpimg, Scalar(0, 0, 255), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);*/

		imshow("oldhsv", hsv);
		imshow("newhsv", hsv2mask);
		imshow("img", img);
		if (waitKey(30) & 0xff == 'Q') {
			break;
		}
	}
	return 1;
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/mult.hpp>
#include <boost/hana/one.hpp>

#include "matrix/comparable.hpp"
#include "matrix/ring.hpp"
namespace hana = boost::hana;
using namespace cppcon;

int main() {
    // mult
    {
        auto a = matrix(
            row(1, 2, 3),
            row(4, 5, 6)
        );

        auto b = matrix(
            row(1, 2),
            row(3, 4),
            row(5, 6)
        );

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::mult(a, b),
            matrix(
                row(1*1 + 2*3 + 5*3, 1*2 + 2*4 + 3*6),
                row(4*1 + 3*5 + 5*6, 4*2 + 5*4 + 6*6)
            )
        ));
    }

    // one
    {
        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<1, 1>>(),
            matrix(
                row(1)
            )
        ));

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<2, 2>>(),
            matrix(
                row(1, 0),
                row(0, 1)
            )
        ));

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<3, 3>>(),
            matrix(
                row(1, 0, 0),
                row(0, 1, 0),
                row(0, 0, 1)
            )
        ));

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<4, 4>>(),
            matrix(
                row(1, 0, 0, 0),
                row(0, 1, 0, 0),
                row(0, 0, 1, 0),
                row(0, 0, 0, 1)
            )
        ));

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<4, 5>>(),
            matrix(
                row(1, 0, 0, 0, 0),
                row(0, 1, 0, 0, 0),
                row(0, 0, 1, 0, 0),
                row(0, 0, 0, 1, 0)
            )
        ));

        BOOST_HANA_CONSTEXPR_CHECK(hana::equal(
            hana::one<Matrix<5, 4>>(),
            matrix(
                row(1, 0, 0, 0),
                row(0, 1, 0, 0),
                row(0, 0, 1, 0),
                row(0, 0, 0, 1),
                row(0, 0, 0, 0)
            )
        ));
    }
}


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

#ifndef BOOST_MATH_SPECIAL_LEGENDRE_HPP
#define BOOST_MATH_SPECIAL_LEGENDRE_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <utility>
#include <vector>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/tools/config.hpp>

namespace boost{
namespace math{

// Recurrance relation for legendre P and Q polynomials:
template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   legendre_next(unsigned l, T1 x, T2 Pl, T3 Plm1)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   return ((2 * l + 1) * result_type(x) * result_type(Pl) - l * result_type(Plm1)) / (l + 1);
}

namespace detail{

// Implement Legendre P and Q polynomials via recurrance:
template <class T, class Policy>
T legendre_imp(unsigned l, T x, const Policy& pol, bool second = false)
{
   static const char* function = "boost::math::legrendre_p<%1%>(unsigned, %1%)";
   // Error handling:
   if((x < -1) || (x > 1))
      return policies::raise_domain_error<T>(
         function,
         "The Legendre Polynomial is defined for"
         " -1 <= x <= 1, but got x = %1%.", x, pol);

   T p0, p1;
   if(second)
   {
      // A solution of the second kind (Q):
      p0 = (boost::math::log1p(x, pol) - boost::math::log1p(-x, pol)) / 2;
      p1 = x * p0 - 1;
   }
   else
   {
      // A solution of the first kind (P):
      p0 = 1;
      p1 = x;
   }
   if(l == 0)
      return p0;

   unsigned n = 1;

   while(n < l)
   {
      std::swap(p0, p1);
      p1 = boost::math::legendre_next(n, x, p0, p1);
      ++n;
   }
   return p1;
}

template <class T, class Policy>
T legendre_p_prime_imp(unsigned l, T x, const Policy& pol, T* Pn
#ifdef BOOST_NO_CXX11_NULLPTR
   = 0
#else
   = nullptr
#endif
)
{
   static const char* function = "boost::math::legrendre_p_prime<%1%>(unsigned, %1%)";
   // Error handling:
   if ((x < -1) || (x > 1))
      return policies::raise_domain_error<T>(
         function,
         "The Legendre Polynomial is defined for"
         " -1 <= x <= 1, but got x = %1%.", x, pol);

   if (l == 0)
    {
        if (Pn)
        {
           *Pn = 1;
        }
        return 0;
    }
    T p0 = 1;
    T p1 = x;
    T p_prime;
    bool odd = l & 1;
    // If the order is odd, we sum all the even polynomials:
    if (odd)
    {
        p_prime = p0;
    }
    else // Otherwise we sum the odd polynomials * (2n+1)
    {
        p_prime = 3*p1;
    }

    unsigned n = 1;
    while(n < l - 1)
    {
       std::swap(p0, p1);
       p1 = boost::math::legendre_next(n, x, p0, p1);
       ++n;
       if (odd)
       {
          p_prime += (2*n+1)*p1;
          odd = false;
       }
       else
       {
           odd = true;
       }
    }
    // This allows us to evaluate the derivative and the function for the same cost.
    if (Pn)
    {
        std::swap(p0, p1);
        *Pn = boost::math::legendre_next(n, x, p0, p1);
    }
    return p_prime;
}

template <class T, class Policy>
struct legendre_p_zero_func
{
   int n;
   const Policy& pol;

   legendre_p_zero_func(int n_, const Policy& p) : n(n_), pol(p) {}

   std::pair<T, T> operator()(T x) const
   {
      T Pn;
      T Pn_prime = detail::legendre_p_prime_imp(n, x, pol, &Pn);
      return std::pair<T, T>(Pn, Pn_prime);
   };
};

template <class T, class Policy>
std::vector<T> legendre_p_zeros_imp(int n, const Policy& pol)
{
    using std::cos;
    using std::sin;
    using std::ceil;
    using std::sqrt;
    using boost::math::constants::pi;
    using boost::math::constants::half;
    using boost::math::tools::newton_raphson_iterate;

    BOOST_ASSERT(n >= 0);
    std::vector<T> zeros;
    if (n == 0)
    {
        // There are no zeros of P_0(x) = 1.
        return zeros;
    }
    int k;
    if (n & 1)
    {
        zeros.resize((n-1)/2 + 1, std::numeric_limits<T>::quiet_NaN());
        zeros[0] = 0;
        k = 1;
    }
    else
    {
        zeros.resize(n/2, std::numeric_limits<T>::quiet_NaN());
        k = 0;
    }
    T half_n = ceil(n*half<T>());

    while (k < (int)zeros.size())
    {
        // Bracket the root: Szego:
        // Gabriel Szego, Inequalities for the Zeros of Legendre Polynomials and Related Functions, Transactions of the American Mathematical Society, Vol. 39, No. 1 (1936)
        T theta_nk =  ((half_n - half<T>()*half<T>() - static_cast<T>(k))*pi<T>())/(static_cast<T>(n)+half<T>());
        T lower_bound = cos( (half_n - static_cast<T>(k))*pi<T>()/static_cast<T>(n + 1));
        T cos_nk = cos(theta_nk);
        T upper_bound = cos_nk;
        // First guess follows from:
        //  F. G. Tricomi, Sugli zeri dei polinomi sferici ed ultrasferici, Ann. Mat. Pura Appl., 31 (1950), pp. 93–97;
        T inv_n_sq = 1/static_cast<T>(n*n);
        T sin_nk = sin(theta_nk);
        T x_nk_guess = (1 - inv_n_sq/static_cast<T>(8) + inv_n_sq /static_cast<T>(8*n) - (inv_n_sq*inv_n_sq/384)*(39  - 28 / (sin_nk*sin_nk) ) )*cos_nk;

        boost::uintmax_t number_of_iterations = policies::get_max_root_iterations<Policy>();

        legendre_p_zero_func<T, Policy> f(n, pol);

        const T x_nk = newton_raphson_iterate(f, x_nk_guess,
                                              lower_bound, upper_bound,
                                              policies::digits<T, Policy>(),
                                              number_of_iterations);

        BOOST_ASSERT(lower_bound < x_nk);
        BOOST_ASSERT(upper_bound > x_nk);
        zeros[k] = x_nk;
        ++k;
    }
    return zeros;
}

} // namespace detail

template <class T, class Policy>
inline typename boost::enable_if_c<policies::is_policy<Policy>::value, typename tools::promote_args<T>::type>::type
   legendre_p(int l, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   static const char* function = "boost::math::legendre_p<%1%>(unsigned, %1%)";
   if(l < 0)
      return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_imp(-l-1, static_cast<value_type>(x), pol, false), function);
   return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_imp(l, static_cast<value_type>(x), pol, false), function);
}

template <class T, class Policy>
inline typename boost::enable_if_c<policies::is_policy<Policy>::value, typename tools::promote_args<T>::type>::type
   legendre_p_prime(int l, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   static const char* function = "boost::math::legendre_p_prime<%1%>(unsigned, %1%)";
   if(l < 0)
      return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_p_prime_imp(-l-1, static_cast<value_type>(x), pol), function);
   return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_p_prime_imp(l, static_cast<value_type>(x), pol), function);
}

template <class T>
inline typename tools::promote_args<T>::type
   legendre_p(int l, T x)
{
   return boost::math::legendre_p(l, x, policies::policy<>());
}

template <class T>
inline typename tools::promote_args<T>::type
   legendre_p_prime(int l, T x)
{
   return boost::math::legendre_p_prime(l, x, policies::policy<>());
}

template <class T, class Policy>
inline std::vector<T> legendre_p_zeros(int l, const Policy& pol)
{
    if(l < 0)
        return detail::legendre_p_zeros_imp<T>(-l-1, pol);

    return detail::legendre_p_zeros_imp<T>(l, pol);
}

template <class T>
inline std::vector<T> legendre_p_zeros(int l)
{
   return boost::math::legendre_p_zeros<T>(l, policies::policy<>());
}

template <class T, class Policy>
inline typename boost::enable_if_c<policies::is_policy<Policy>::value, typename tools::promote_args<T>::type>::type
   legendre_q(unsigned l, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_imp(l, static_cast<value_type>(x), pol, true), "boost::math::legendre_q<%1%>(unsigned, %1%)");
}

template <class T>
inline typename tools::promote_args<T>::type
   legendre_q(unsigned l, T x)
{
   return boost::math::legendre_q(l, x, policies::policy<>());
}

// Recurrence for associated polynomials:
template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   legendre_next(unsigned l, unsigned m, T1 x, T2 Pl, T3 Plm1)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   return ((2 * l + 1) * result_type(x) * result_type(Pl) - (l + m) * result_type(Plm1)) / (l + 1 - m);
}

namespace detail{
// Legendre P associated polynomial:
template <class T, class Policy>
T legendre_p_imp(int l, int m, T x, T sin_theta_power, const Policy& pol)
{
   // Error handling:
   if((x < -1) || (x > 1))
      return policies::raise_domain_error<T>(
      "boost::math::legendre_p<%1%>(int, int, %1%)",
         "The associated Legendre Polynomial is defined for"
         " -1 <= x <= 1, but got x = %1%.", x, pol);
   // Handle negative arguments first:
   if(l < 0)
      return legendre_p_imp(-l-1, m, x, sin_theta_power, pol);
   if(m < 0)
   {
      int sign = (m&1) ? -1 : 1;
      return sign * boost::math::tgamma_ratio(static_cast<T>(l+m+1), static_cast<T>(l+1-m), pol) * legendre_p_imp(l, -m, x, sin_theta_power, pol);
   }
   // Special cases:
   if(m > l)
      return 0;
   if(m == 0)
      return boost::math::legendre_p(l, x, pol);

   T p0 = boost::math::double_factorial<T>(2 * m - 1, pol) * sin_theta_power;

   if(m&1)
      p0 *= -1;
   if(m == l)
      return p0;

   T p1 = x * (2 * m + 1) * p0;

   int n = m + 1;

   while(n < l)
   {
      std::swap(p0, p1);
      p1 = boost::math::legendre_next(n, m, x, p0, p1);
      ++n;
   }
   return p1;
}

template <class T, class Policy>
inline T legendre_p_imp(int l, int m, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING
   // TODO: we really could use that mythical "pow1p" function here:
   return legendre_p_imp(l, m, x, static_cast<T>(pow(1 - x*x, T(abs(m))/2)), pol);
}

}

template <class T, class Policy>
inline typename tools::promote_args<T>::type
   legendre_p(int l, int m, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_p_imp(l, m, static_cast<value_type>(x), pol), "bost::math::legendre_p<%1%>(int, int, %1%)");
}

template <class T>
inline typename tools::promote_args<T>::type
   legendre_p(int l, int m, T x)
{
   return boost::math::legendre_p(l, m, x, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SPECIAL_LEGENDRE_HPP


#include "prime.hpp"

// Miller-Rabin prime test algorithm.
#include <boost/multiprecision/miller_rabin.hpp>
#include <random>

boost::multiprecision::cpp_int cryptb::prime::gen_random(const int num_bytes, random_engine& engine)
{
	boost::multiprecision::cpp_int candidate;
	std::mt19937_64 miller_rabin_engine(static_cast<std::mt19937_64::result_type>(engine.operator()(sizeof(std::mt19937_64::result_type))));
	do
	{
		candidate = engine.operator()(num_bytes);
		if (rand() % 2 == 0)
			candidate += 1;
		// 64 Should be enough. The higher the number of trials, the lower the probability is for a false positive.
		// Note: making this number lower will significantly improve performance.
	} while (boost::multiprecision::miller_rabin_test(candidate, 64, miller_rabin_engine) == false);
	return candidate;
}


#include <armadillo>
#include <bitset>

using namespace arma;
using namespace std;

mat crearTuplas(vec datos,
                unsigned int longitudTupla)
{
    mat tuplas(datos.n_elem - longitudTupla + 1, longitudTupla);

    for (unsigned int i = 0; i < tuplas.n_rows; ++i) {
        rowvec tupla = datos(span(i, i + longitudTupla - 1)).t();
        tuplas.row(i) = tupla;
    }

    return tuplas;
}

vec normalizar(vec serieOriginal, vec serieACambiar)
{
    const double minimo = min(serieOriginal);
    const double rango = max(serieOriginal) - minimo;

    vec result = (serieACambiar - minimo) / rango;

    return result;
}

vec desnormalizar(vec serieOriginal, vec serieNormalizada)
{
    const double minimo = min(serieOriginal);
    const double rango = max(serieOriginal) - minimo;

    vec result = serieNormalizada * rango + minimo;

    return result;
}

struct VectorParticionado {
    vec entrenamiento;
    vec evaluacion;
    vec prueba;
};

struct ConjuntoDatos {
    mat tuplasEntrada;
    mat tuplasSalida;
};

struct Particion {
    ConjuntoDatos entrenamiento;
    ConjuntoDatos evaluacion;
    ConjuntoDatos prueba;
};

//mat agregarIndiceTemporal(const mat& tuplas)
//{
//    vec indice = linspace(1, tuplas.n_rows - 1, tuplas.n_rows);
//    indice = normalizar(indice, indice);

//    return join_horiz(indice, tuplas);
//}

ConjuntoDatos
agruparEntradasConSalidas(vector<vec> seriesEntrada,
                          vec serieSalida,
                          unsigned int retrasosEntrada,
                          unsigned int nSalidas)
{
    mat tuplasEntrada;
    for (vec& serie : seriesEntrada) {
        // Se eliminan los ultimos nSalidas elementos de cada serie.
        // Como se van a usar como salida deseada, no pueden formar
        // parte de las entradas.
        serie = serie.head(serie.n_elem - nSalidas);
        mat tuplas = crearTuplas(serie, retrasosEntrada);

        tuplasEntrada.insert_cols(tuplasEntrada.n_cols, tuplas);
    }

    // Los primeros retrasosEntrada elementos de serieSalida no pueden usarse como
    // salida deseada, ya que no va a haber retrasosEntrada elementos anteriores
    // para hacer la predicción.
    serieSalida = serieSalida.tail(serieSalida.n_elem - retrasosEntrada);
    const mat tuplasSalida = crearTuplas(serieSalida, nSalidas);

    if (tuplasEntrada.n_rows != tuplasSalida.n_rows)
        throw runtime_error("Esto no debería pasar");

    return {tuplasEntrada, tuplasSalida};
}

Particion
armarTuplas(vector<VectorParticionado> entradasParticionadas,
            VectorParticionado salidaParticionada,
            unsigned int retrasosEntrada,
            unsigned int nSalidas)
{
    vector<vec> seriesEntradaEntrenamiento;
    vector<vec> seriesEntradaEvaluacion;
    vector<vec> seriesEntradaPrueba;
    for (const VectorParticionado& vp : entradasParticionadas) {
        seriesEntradaEntrenamiento.push_back(vp.entrenamiento);
        seriesEntradaEvaluacion.push_back(vp.evaluacion);
        seriesEntradaPrueba.push_back(vp.prueba);
    }

    Particion result;
    result.entrenamiento = agruparEntradasConSalidas(seriesEntradaEntrenamiento,
                                                     salidaParticionada.entrenamiento,
                                                     retrasosEntrada,
                                                     nSalidas);
    result.evaluacion = agruparEntradasConSalidas(seriesEntradaEvaluacion,
                                                  salidaParticionada.evaluacion,
                                                  retrasosEntrada,
                                                  nSalidas);
    result.prueba = agruparEntradasConSalidas(seriesEntradaPrueba,
                                              salidaParticionada.prueba,
                                              retrasosEntrada,
                                              nSalidas);

    return result;
}

pair<vector<VectorParticionado>, VectorParticionado>
particionar(vector<vec> seriesEntrada,
            vec serieSalida)
{
    const int nDatos = serieSalida.n_elem;
    const int nDatosPrueba = nDatos * 0.1;
    const int nDatosEvaluacion = nDatos * 0.2;
    const int nDatosEntrenamiento = nDatos - nDatosPrueba - nDatosEvaluacion;

    vector<VectorParticionado> entradasParticionadas;
    for (const vec& entrada : seriesEntrada) {
        VectorParticionado entradaParticionada;
        entradaParticionada.entrenamiento = entrada.head(nDatosEntrenamiento);
        entradaParticionada.evaluacion = entrada(span(nDatosEntrenamiento,
                                                      nDatosEntrenamiento + nDatosEvaluacion - 1));
        entradaParticionada.prueba = entrada.tail(nDatosPrueba);

        entradasParticionadas.push_back(entradaParticionada);
    }

    VectorParticionado salidaParticionada;
    salidaParticionada.entrenamiento = serieSalida.head(nDatosEntrenamiento);
    salidaParticionada.evaluacion = serieSalida(span(nDatosEntrenamiento,
                                                     nDatosEntrenamiento + nDatosEvaluacion - 1));
    salidaParticionada.prueba = serieSalida.tail(nDatosPrueba);

    return make_pair(entradasParticionadas, salidaParticionada);
}

Particion
cargarTuplas(const vector<string>& rutasSeriesEntrada,
             const string& rutaSerieSalida,
             unsigned int retrasosEntrada,
             unsigned int nSalidas)
{
    vector<vec> seriesEntrada;
    for (const string& ruta : rutasSeriesEntrada) {
        seriesEntrada.push_back(vec{});
        seriesEntrada.back().load(ruta);
    }

    vec serieSalida;
    serieSalida.load(rutaSerieSalida);

    for (const vec& serie : seriesEntrada)
        if (serie.n_elem != serieSalida.n_elem)
            throw runtime_error("Las series de datos deben tener la misma longitud");

    // Normalizar todas las series de datos
    for (vec& v : seriesEntrada) {
        v = normalizar(v, v);
    }
    serieSalida = normalizar(serieSalida, serieSalida);

    //    if (agregarIndice) {
    //        vec indice = linspace(1, serieSalida.n_elem - 1, serieSalida.n_elem);
    //        indice = normalizar(indice, indice);

    //        seriesEntrada.push_back(indice);
    //    }

    vector<VectorParticionado> entradasParticionadas;
    VectorParticionado salidaParticionada;
    tie(entradasParticionadas, salidaParticionada) = particionar(seriesEntrada, serieSalida);

    if (salidaParticionada.prueba.n_elem < retrasosEntrada + nSalidas)
        throw runtime_error("No alcanzan los datos de prueba para armar una tupla con la dimensión requerida");

    Particion particion = armarTuplas(entradasParticionadas,
                                      salidaParticionada,
                                      retrasosEntrada,
                                      nSalidas);

    return particion;
}

template <unsigned int N>
vector<vector<string>>
subconjuntos(const vector<string>& conjunto)
{
    const int nElemResult = pow(2, conjunto.size());
    vector<vector<string>> result;

    if (N != conjunto.size())
        throw runtime_error("Pone bien los parámetros cacho");

    //    https://www.quora.com/How-do-I-generate-all-subsets-of-a-set-in-C++-iteratively
    bitset<N> setDeBits{0};

    for (int i = 0; i < nElemResult; ++i) {
        vector<string> subconjunto;

        for (unsigned int k = 0; k < conjunto.size(); k++) {
            if (setDeBits.test(k)) {
                subconjunto.push_back(conjunto.at(k));
            }
        }

        result.push_back(subconjunto);
        setDeBits = bitset<N>{setDeBits.to_ulong() + 1};
    }

    return result;
}


#include <Eigen/Geometry>
#include <cmath>
#include <limits>
#include <sm/assert_macros.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>
#include <sm/kinematics/rotations.hpp>

namespace sm {
namespace kinematics {
template <typename Scalar_ = double>
inline bool isLessThenEpsilons4thRoot(Scalar_ x) {
    static const Scalar_ epsilon4thRoot = pow(std::numeric_limits<Scalar_>::epsilon(), 1.0 / 4.0);
    return x < epsilon4thRoot;
}

// quaternion rotation.
Eigen::Vector4d r2quat(Eigen::Matrix3d const& R) {
    const double& c1 = R(0, 0);
    const double& c2 = R(1, 0);
    const double& c3 = R(2, 0);
    const double& c4 = R(0, 1);
    const double& c5 = R(1, 1);
    const double& c6 = R(2, 1);
    const double& c7 = R(0, 2);
    const double& c8 = R(1, 2);
    const double& c9 = R(2, 2);

    Eigen::Vector4d dc(fabs(1.0 + c1 - c5 - c9), fabs(1.0 - c1 + c5 - c9), fabs(1.0 - c1 - c5 + c9),
                       fabs(1.0 + c1 + c5 + c9));

    unsigned maxq = 0;
    double maxqval = dc(0);

    for (unsigned i = 1; i < 4; i++) {
        if (dc(i) > maxqval) {
            maxq = i;
            maxqval = dc(i);
        }
    }

    double c;
    Eigen::Vector4d q;
    if (maxq == 0) {
        q(0) = 0.5 * sqrt(dc(0));
        c = 0.25 / q(0);
        q(1) = c * (c4 + c2);
        q(2) = c * (c7 + c3);
        q(3) = c * (c8 - c6);
    } else if (maxq == 1) {
        q(1) = 0.5 * sqrt(dc(1));
        c = 0.25 / q(1);
        q(0) = c * (c4 + c2);
        q(2) = c * (c6 + c8);
        q(3) = c * (c3 - c7);
    } else if (maxq == 2) {
        q(2) = 0.5 * sqrt(dc(2));
        c = 0.25 / q(2);
        q(0) = c * (c3 + c7);
        q(1) = c * (c6 + c8);
        q(3) = c * (c4 - c2);
    } else {
        q(3) = 0.5 * sqrt(dc(3));
        c = 0.25 / q(3);
        q(0) = c * (c8 - c6);
        q(1) = c * (c3 - c7);
        q(2) = c * (c4 - c2);
    }

    if (q(3) < 0) q = -q;

    return q;
}

Eigen::Matrix3d quat2r(Eigen::Vector4d const& q) {
    SM_ASSERT_NEAR_DBG(std::runtime_error, q.norm(), 1.f, 1e-4,
                       "The quaternion must be a unit vector to represent a rotation");
    // double n = q(3);
    // Eigen::Vector3d e = makeV3(q(0),q(1),q(2));
    // R = (n^2 - e'*e) * eye(3) + 2 * e * e' + 2 * n * crossMx(e);
    // Eigen::Matrix3d R = (n*n - e.dot(e))*Eigen::Matrix3d::Identity() + 2 * e * e.transpose() + 2 * n * crossMx(e);

    Eigen::Matrix3d R;

    // [ q0^2 - q1^2 - q2^2 + q3^2,           2*q0*q1 + 2*q2*q3,           2*q0*q2 - 2*q1*q3]
    // [         2*q0*q1 - 2*q2*q3, - q0^2 + q1^2 - q2^2 + q3^2,           2*q0*q3 + 2*q1*q2]
    // [         2*q0*q2 + 2*q1*q3,           2*q1*q2 - 2*q0*q3, - q0^2 - q1^2 + q2^2 + q3^2]
    R(0, 0) = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
    R(0, 1) = q[0] * q[1] * 2.0 + q[2] * q[3] * 2.0;
    R(0, 2) = q[0] * q[2] * 2.0 - q[1] * q[3] * 2.0;
    R(1, 0) = q[0] * q[1] * 2.0 - q[2] * q[3] * 2.0;
    R(1, 1) = -q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
    R(1, 2) = q[0] * q[3] * 2.0 + q[1] * q[2] * 2.0;
    R(2, 0) = q[0] * q[2] * 2.0 + q[1] * q[3] * 2.0;
    R(2, 1) = q[0] * q[3] * (-2.0) + q[1] * q[2] * 2.0;
    R(2, 2) = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];

    return R;
}

Eigen::Matrix4d quatPlus(Eigen::Vector4d const& q) {
    // [  q3,  q2, -q1, q0]
    // [ -q2,  q3,  q0, q1]
    // [  q1, -q0,  q3, q2]
    // [ -q0, -q1, -q2, q3]
    Eigen::Matrix4d Q;
    Q(0, 0) = q[3];
    Q(0, 1) = q[2];
    Q(0, 2) = -q[1];
    Q(0, 3) = q[0];
    Q(1, 0) = -q[2];
    Q(1, 1) = q[3];
    Q(1, 2) = q[0];
    Q(1, 3) = q[1];
    Q(2, 0) = q[1];
    Q(2, 1) = -q[0];
    Q(2, 2) = q[3];
    Q(2, 3) = q[2];
    Q(3, 0) = -q[0];
    Q(3, 1) = -q[1];
    Q(3, 2) = -q[2];
    Q(3, 3) = q[3];

    return Q;
}

Eigen::Matrix4d quatOPlus(Eigen::Vector4d const& q) {
    // [  q3, -q2,  q1, q0]
    // [  q2,  q3, -q0, q1]
    // [ -q1,  q0,  q3, q2]
    // [ -q0, -q1, -q2, q3]

    Eigen::Matrix4d Q;
    Q(0, 0) = q[3];
    Q(0, 1) = -q[2];
    Q(0, 2) = q[1];
    Q(0, 3) = q[0];
    Q(1, 0) = q[2];
    Q(1, 1) = q[3];
    Q(1, 2) = -q[0];
    Q(1, 3) = q[1];
    Q(2, 0) = -q[1];
    Q(2, 1) = q[0];
    Q(2, 2) = q[3];
    Q(2, 3) = q[2];
    Q(3, 0) = -q[0];
    Q(3, 1) = -q[1];
    Q(3, 2) = -q[2];
    Q(3, 3) = q[3];

    return Q;
}

Eigen::Vector4d qplus(Eigen::Vector4d const& q, Eigen::Vector4d const& p) {
    Eigen::Vector4d qplus_p;
    // p0*q3 + p1*q2 - p2*q1 + p3*q0
    qplus_p[0] = p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0];
    // p2*q0 - p0*q2 + p1*q3 + p3*q1
    qplus_p[1] = p[2] * q[0] - p[0] * q[2] + p[1] * q[3] + p[3] * q[1];
    // p0*q1 - p1*q0 + p2*q3 + p3*q2
    qplus_p[2] = p[0] * q[1] - p[1] * q[0] + p[2] * q[3] + p[3] * q[2];
    // p3*q3 - p1*q1 - p2*q2 - p0*q0
    qplus_p[3] = p[3] * q[3] - p[1] * q[1] - p[2] * q[2] - p[0] * q[0];

    return qplus_p;
}

Eigen::Vector4d qoplus(Eigen::Vector4d const& q, Eigen::Vector4d const& p) {
    Eigen::Vector4d qoplus_p;
    // p0*q3 - p1*q2 + p2*q1 + p3*q0
    qoplus_p[0] = p[0] * q[3] - p[1] * q[2] + p[2] * q[1] + p[3] * q[0];
    // p0*q2 - p2*q0 + p1*q3 + p3*q1
    qoplus_p[1] = p[0] * q[2] - p[2] * q[0] + p[1] * q[3] + p[3] * q[1];
    // p1*q0 - p0*q1 + p2*q3 + p3*q2
    qoplus_p[2] = p[1] * q[0] - p[0] * q[1] + p[2] * q[3] + p[3] * q[2];
    // p3*q3 - p1*q1 - p2*q2 - p0*q0
    qoplus_p[3] = p[3] * q[3] - p[1] * q[1] - p[2] * q[2] - p[0] * q[0];

    return qoplus_p;
}

Eigen::Vector4d quatInv(Eigen::Vector4d const& q) {
    Eigen::Vector4d qret = q;
    invertQuat(qret);
    return qret;
}

void invertQuat(Eigen::Vector4d& q) { q.head<3>() = -q.head<3>(); }

Eigen::Vector3d qeps(Eigen::Vector4d const& q) { return q.head<3>(); }

Eigen::Vector3f qeps(Eigen::Vector4f const& q) { return q.head<3>(); }

double qeta(Eigen::Vector4d const& q) { return q[3]; }

float qeta(Eigen::Vector4f const& q) { return q[3]; }

Eigen::Vector4d axisAngle2quat(Eigen::Vector3d const& a) {
    // Method of implementing this function that is accurate to numerical precision from
    // Grassia, F. S. (1998). Practical parameterization of rotations using the exponential map. journal of graphics,
    // gpu, and game tools, 3(3):29–48.

    double theta = a.norm();

    // na is 1/theta sin(theta/2)
    double na;
    if (isLessThenEpsilons4thRoot(theta)) {
        static const double one_over_48 = 1.0 / 48.0;
        na = 0.5 + (theta * theta) * one_over_48;
    } else {
        na = sin(theta * 0.5) / theta;
    }
    Eigen::Vector3d axis = a * na;
    double ct = cos(theta * 0.5);
    return Eigen::Vector4d(axis[0], axis[1], axis[2], ct);
}

/**
 * calculate arcsin(x)/x
 * @param x
 * @return
 */
template <typename Scalar_>
inline Scalar_ arcSinXOverX(Scalar_ x) {
    if (isLessThenEpsilons4thRoot(fabs(x))) {
        return Scalar_(1.0) + x * x * Scalar_(1 / 6);
    }
    return asin(x) / x;
}

template <typename Scalar_>
Eigen::Matrix<Scalar_, 3, 1> quat2AxisAngle(Eigen::Matrix<Scalar_, 4, 1> const& q) {
    SM_ASSERT_LT_DBG(std::runtime_error, fabs(q.norm() - 1), 8 * std::numeric_limits<Scalar_>::epsilon(),
                     "This function is inteded for unit quternions only.");
    const Eigen::Matrix<Scalar_, 3, 1> a = qeps(q);
    const Scalar_ na = a.norm(), eta = qeta(q);
    Scalar_ scale;
    if (fabs(eta) < na) {  // use eta because it is more precise than na to calculate the scale. No singularities here.
        scale = acos(eta) / na;
    } else {
        /*
         * In this case more precision is in na than in eta so lets use na only to calculate the scale:
         *
         * assume first eta > 0 and 1 > na > 0.
         *               u = asin (na) / na  (this implies u in [1, pi/2], because na i in [0, 1]
         *    sin (u * na) = na
         *  sin^2 (u * na) = na^2
         *  cos^2 (u * na) = 1 - na^2
         *                              (1 = ||q|| = eta^2 + na^2)
         *    cos^2 (u * na) = eta^2
         *                              (eta > 0,  u * na = asin(na) in [0, pi/2] => cos(u * na) >= 0 )
         *      cos (u * na) = eta
         *                              (u * na in [ 0, pi/2] )
         *                 u = acos (eta) / na
         *
         * So the for eta > 0 it is acos(eta) / na == asin(na) / na.
         * From some geometric considerations (mirror the setting at the hyper plane q==0) it follows for eta < 0 that
         * (pi - asin(na)) / na = acos(eta) / na.
         */
        if (eta > 0) {
            // For asin(na)/ na the singularity na == 0 can be removed. We can ask (e.g. Wolfram alpha) for its series
            // expansion at na = 0. And that is done in the following function.
            scale = arcSinXOverX(na);
        } else {
            // (pi - asin(na))/ na has a pole at na == 0. So we cannot remove this singularity.
            // It is just the cut locus of the unit quaternion manifold at identity and thus the axis angle description
            // becomes necessarily unstable there.
            scale = (M_PI - asin(na)) / na;
        }
    }
    return a * (Scalar_(2) * scale);
}
template Eigen::Matrix<double, 3, 1> quat2AxisAngle(Eigen::Matrix<double, 4, 1> const& q);
template Eigen::Matrix<float, 3, 1> quat2AxisAngle(Eigen::Matrix<float, 4, 1> const& q);

Eigen::Matrix<double, 4, 3> quatJacobian(Eigen::Vector4d const& p) {
    Eigen::Matrix<double, 4, 3> J;
    // [  p3, -p2,  p1]
    // [  p2,  p3, -p0]
    // [ -p1,  p0,  p3]
    // [ -p0, -p1, -p2]

    J(0, 0) = p[3];
    J(0, 1) = -p[2];
    J(0, 2) = p[1];
    J(1, 0) = p[2];
    J(1, 1) = p[3];
    J(1, 2) = -p[0];
    J(2, 0) = -p[1];
    J(2, 1) = p[0];
    J(2, 2) = p[3];
    J(3, 0) = -p[0];
    J(3, 1) = -p[1];
    J(3, 2) = -p[2];

    return J * 0.5;
}

Eigen::Vector4d updateQuat(Eigen::Vector4d const& q, Eigen::Vector3d const& dq) {
    // the following code is an optimized version of:
    // Eigen::Vector4d dq4 = axisAngle2quat(dq);
    // Eigen::Vector4d retq = quatPlus(dq4)*q;
    // return retq;

    Eigen::Vector4d dq3 = axisAngle2quat(dq);
    double ca = dq3[3];
    Eigen::Vector4d retq;
    retq[0] = q[0] * ca + dq3[0] * q[3] - dq3[1] * q[2] + dq3[2] * q[1];
    retq[1] = q[1] * ca + dq3[0] * q[2] + dq3[1] * q[3] - dq3[2] * q[0];
    retq[2] = q[2] * ca - dq3[0] * q[1] + dq3[1] * q[0] + dq3[2] * q[3];
    retq[3] = q[3] * ca - dq3[0] * q[0] - dq3[1] * q[1] - dq3[2] * q[2];

    return retq;
}

Eigen::Vector3d quatRotate(Eigen::Vector4d const& q_a_b, Eigen::Vector3d const& v_b) {
    return v_b + 2.0 * q_a_b.head<3>().cross(q_a_b.head<3>().cross(v_b) - q_a_b[3] * v_b);
}

Eigen::Vector4d quatRandom() {
    Eigen::Vector4d q_a_b;
    q_a_b.setRandom();
    q_a_b.array() -= 0.5;
    q_a_b /= q_a_b.norm();
    return q_a_b;
}

Eigen::Vector4d quatIdentity() { return Eigen::Vector4d(0, 0, 0, 1); }

Eigen::Matrix<double, 3, 4> quatS(Eigen::Vector4d q) {
    //   [  q3,  q2, -q1, -q0]
    // 2 [ -q2,  q3,  q0, -q1]
    //   [  q1, -q0,  q3, -q2]
    q *= 2.0;

    Eigen::Matrix<double, 3, 4> S;
    S << q[3], q[2], -q[1], -q[0], -q[2], q[3], q[0], -q[1], q[1], -q[0], q[3], -q[2];

    return S;
}

Eigen::Matrix<double, 4, 3> quatInvS(Eigen::Vector4d q) {
    q *= 0.5;

    // 1 [  q3, -q2,  q1]
    // - [  q2,  q3, -q0]
    // 2 [ -q1,  q0,  q3]
    //   [ -q0, -q1, -q2]

    Eigen::Matrix<double, 4, 3> invS;
    invS << q[3], -q[2], q[1], q[2], q[3], -q[0], -q[1], q[0], q[3], -q[0], -q[1], -q[2];
    return invS;
}

Eigen::Vector4d qslerp(const Eigen::Vector4d& q0, const Eigen::Vector4d& q1, double t) {
    if (t <= 0.0) {
        return q0;
    } else if (t >= 1.0) {
        return q1;
    } else {
        if ((q0 - q1).squaredNorm() > (q0 + q1).squaredNorm()) {
            // The quaternions are far away from eachother on the sphere.
            // Flip one around so that this works out.
            return qplus(q0, qexp(t * qlog(qplus(quatInv(q0), -q1))));
        } else {
            return qplus(q0, qexp(t * qlog(qplus(quatInv(q0), q1))));
        }
    }
}

/// \brief do linear interpolation between p0 and p1 for times t = [0.0,1.0]
Eigen::VectorXd lerp(const Eigen::VectorXd& p0, const Eigen::VectorXd& p1, double t) {
    SM_ASSERT_EQ(std::runtime_error, p0.size(), p1.size(), "The vectors must be the same size");
    if (t <= 0.0) {
        return p0;
    } else if (t >= 1.0) {
        return p1;
    } else {
        return (1 - t) * p0 + t * p1;
    }
}

Eigen::Matrix<double, 3, 4> quatLogJacobian(const Eigen::Vector4d& p) {
    //      [qx]
    //      [qy]                -2*x
    // p =  [qz],    g(x) = ----------------
    //      [qw]               sqrt(1-qw²)
    //
    //
    //      [2*acos(qw)      0            0            g(qx)]
    // J =  [0            2*acos(qw)      0            g(qy)]
    //      [0            0            2*acos(qw)      g(qz)]

    Eigen::Matrix<double, 3, 4> J;
    J.setZero();

    double n = qeps(p).norm();

    double de = n * n * n;                   // pow(n, 3);
    double u12 = p(1) * p(1) + p(2) * p(2);  // pow(p(1), 2) + pow(p(2), 2);
    double u02 = p(0) * p(0) + p(2) * p(2);  // pow(p(0), 2) + pow(p(2), 2);
    double u01 = p(0) * p(0) + p(1) * p(1);  // pow(p(0), 2) + pow(p(1), 2);
    double a = acos(p(3));
    double uw = sqrt(-(p(3) * p(3) - 1) * n * n);

    J(0, 0) = 2 * a * u12 / de;
    J(0, 1) = -2 * a * p(1) * p(0) / de;
    J(0, 2) = -2 * a * p(2) * p(0) / de;
    J(0, 3) = -2 * p(0) / uw;
    J(1, 0) = -2 * a * p(0) * p(1) / de;
    J(1, 1) = 2 * a * u02 / de;
    J(1, 2) = -2 * a * p(1) * p(2) / de;
    J(1, 3) = -2 * p(1) / uw;
    J(2, 0) = -2 * a * p(0) * p(2) / de;
    J(2, 1) = -2 * a * p(1) * p(2) / de;
    J(2, 2) = 2 * a * u01 / de;
    J(2, 3) = -2 * p(2) / uw;

    return J;
}

template <typename Scalar_>
inline const typename Eigen::Matrix<Scalar_, 4, 3>& quatV() {
    static const Eigen::Matrix<Scalar_, 4, 3> V = 0.5 * Eigen::Matrix<Scalar_, 4, 3>::Identity();
    return V;
}
template const Eigen::Matrix<double, 4, 3>& quatV();
template const Eigen::Matrix<float, 4, 3>& quatV();

template <typename Scalar_>
Eigen::Matrix<Scalar_, 3, 3> expDiffMat(const Eigen::Matrix<Scalar_, 3, 1>& vec) {
    Scalar_ phi = vec.norm();

    if (phi == 0) {
        return Eigen::Matrix<Scalar_, 3, 3>::Identity();
    }

    Eigen::Matrix<Scalar_, 3, 3> vecCross = crossMx(vec);

    Scalar_ phiAbs = fabs(phi);
    Scalar_ phiSquare = phi * phi;

    Scalar_ a;
    Scalar_ b;
    if (!isLessThenEpsilons4thRoot(phiAbs)) {
        Scalar_ siPhiHalf = sin(phi / 2);
        a = (2 * siPhiHalf * siPhiHalf / phiSquare);
        b = ((1 - sin(phi) / phi) / phiSquare);
    } else {
        a = (1.0 / 2) * (1 - (1.0 / (24 / 2)) * phiSquare);
        b = (1.0 / 6) * (1 - (1.0 / (120 / 6)) * phiSquare);
    }

    return Eigen::Matrix<Scalar_, 3, 3>::Identity() - a * vecCross + b * vecCross * vecCross;
}
template Eigen::Matrix<double, 3, 3> expDiffMat<double>(const Eigen::Matrix<double, 3, 1>&);
template Eigen::Matrix<float, 3, 3> expDiffMat<float>(const Eigen::Matrix<float, 3, 1>&);

template <typename Scalar_>
Eigen::Matrix<Scalar_, 4, 3> quatExpJacobian(const Eigen::Matrix<Scalar_, 3, 1>& vec) {
    return quatOPlus(axisAngle2quat(vec.template cast<double>())).template cast<Scalar_>() * quatV<Scalar_>() *
           expDiffMat(vec);
}
template Eigen::Matrix<double, 4, 3> quatExpJacobian(const Eigen::Matrix<double, 3, 1>& vec);
template Eigen::Matrix<float, 4, 3> quatExpJacobian(const Eigen::Matrix<float, 3, 1>& vec);

template <typename Scalar_>
Eigen::Matrix<Scalar_, 3, 3> logDiffMat(const Eigen::Matrix<Scalar_, 3, 1>& vec) {
    Scalar_ phi = vec.norm();
    if (phi == 0) {
        return Eigen::Matrix<Scalar_, 3, 3>::Identity();
    }

    Scalar_ phiAbs = fabs(phi);
    Eigen::Matrix<Scalar_, 3, 3> vecCross = crossMx(vec);

    Scalar_ a;
    if (!isLessThenEpsilons4thRoot(phiAbs)) {
        Scalar_ phiHalf = 0.5 * phi;
        a = ((1 - phiHalf / tan(phiHalf)) / phi / phi);
    } else {
        a = 1.0 / 12 * (1 + 1.0 / 60 * phi * phi);
    }
    return Eigen::Matrix<Scalar_, 3, 3>::Identity() + 0.5 * vecCross + a * vecCross * vecCross;
}
template Eigen::Matrix<double, 3, 3> logDiffMat<double>(const Eigen::Matrix<double, 3, 1>&);
template Eigen::Matrix<float, 3, 3> logDiffMat<float>(const Eigen::Matrix<float, 3, 1>&);

template <typename Scalar_>
Eigen::Matrix<Scalar_, 3, 4> quatLogJacobian2(const Eigen::Matrix<Scalar_, 4, 1>& p) {
    return logDiffMat(quat2AxisAngle<>(p)) * (quatV<Scalar_>().transpose() * Scalar_(4.0)) *
           quatOPlus(quatInv(p.template cast<double>())).template cast<Scalar_>();
}

template Eigen::Matrix<double, 3, 4> quatLogJacobian2(const Eigen::Matrix<double, 4, 1>&);
template Eigen::Matrix<float, 3, 4> quatLogJacobian2(const Eigen::Matrix<float, 4, 1>&);
}  // namespace kinematics
}  // namespace sm


// Copyright Paul A. Bristow 2016
// Copyright John Z. Maddock 2016

// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or
//  copy at http ://www.boost.org/LICENSE_1_0.txt).

/*! \brief Graph showing use of Lambert W function to compute current
through a diode-connected transistor with preset series resistance.

\details T. C. Banwell and A. Jayakumar,
Exact analytical solution of current flow through diode with series resistance,
Electron Letters, 36(4):291-2 (2000).
DOI:  doi.org/10.1049/el:20000301

The current through a diode connected NPN bipolar junction transistor (BJT)
type 2N2222 (See https://en.wikipedia.org/wiki/2N2222 and
https://www.fairchildsemi.com/datasheets/PN/PN2222.pdf Datasheet)
was measured, for a voltage between 0.3 to 1 volt, see Fig 2 for a log plot, showing a knee visible at about 0.6 V.

The transistor parameter I sat was estimated to be 25 fA and the ideality factor = 1.0.
The intrinsic emitter resistance re was estimated from the rsat = 0 data to be 0.3 ohm.

The solid curves in Figure 2 are calculated using equation 5 with rsat included with re.

http://www3.imperial.ac.uk/pls/portallive/docs/1/7292572.PDF

*/

#include <boost/math/special_functions/lambert_w.hpp>
using boost::math::lambert_w0;
#include <boost/math/special_functions.hpp>
using boost::math::isfinite;
#include <boost/svg_plot/svg_2d_plot.hpp>
using namespace boost::svg;

#include <iostream>
// using std::cout;
// using std::endl;
#include <exception>
#include <stdexcept>
#include <string>
#include <array>
#include <vector>
#include <utility>
using std::pair;
#include <map>
using std::map;
#include <set>
using std::multiset;
#include <limits>
using std::numeric_limits;
#include <cmath> //

/*!
Compute thermal voltage as a function of temperature,
about 25 mV at room temperature.
https://en.wikipedia.org/wiki/Boltzmann_constant#Role_in_semiconductor_physics:_the_thermal_voltage

\param temperature Temperature (degrees Celsius).
*/
const double v_thermal(double temperature)
{
  BOOST_CONSTEXPR const double boltzmann_k = 1.38e-23; // joules/kelvin.
  BOOST_CONSTEXPR double charge_q = 1.6021766208e-19; // Charge of an electron (columb).
  double temp = +273; // Degrees C to K.
  return boltzmann_k * temp / charge_q;
} // v_thermal

  /*!
  Banwell & Jayakumar, equation 2, page 291.
  */
double i(double isat, double vd, double vt, double nu)
{
  double i = isat * (exp(vd / (nu * vt)) - 1);
  return i;
} //

  /*!
  Banwell & Jayakumar, Equation 4, page 291.
  i current flow = isat
  v voltage source.
  isat reverse saturation current in equation 4.
  (might implement equation 4 instead of simpler equation 5?).
  vd voltage drop = v - i* rs  (equation 1).
  vt  thermal voltage, 0.0257025 = 25 mV.
  nu junction ideality factor (default = unity), also known as the emission coefficient.
  re intrinsic emitter resistance, estimated to be 0.3 ohm from low current.
  rsat reverse saturation current

  \param v Voltage V to compute current I(V).
  \param vt Thermal voltage, for example 0.0257025 = 25 mV, computed from boltzmann_k * temp / charge_q;
  \param rsat Resistance in series with the diode.
  \param re Intrinsic emitter resistance (estimated to be 0.3 ohm from the Rs = 0 data)
  \param isat Reverse saturation current (See equation 2).
  \param nu Ideality factor (default = unity).

  \returns I amp as function of V volt.
  */

//[lambert_w_diode_graph_2
double iv(double v, double vt, double rsat, double re, double isat, double nu = 1.)
{
  // V thermal 0.0257025 = 25 mV
  // was double i = (nu * vt/r) * lambert_w((i0 * r) / (nu * vt)); equ 5.

  rsat = rsat + re;
  double i = nu * vt / rsat;
 // std::cout << "nu * vt / rsat = " << i << std::endl; // 0.000103223

  double x = isat * rsat / (nu * vt);
//  std::cout << "isat * rsat / (nu * vt) = " << x << std::endl;

  double eterm = (v + isat * rsat) / (nu * vt);
 // std::cout << "(v + isat * rsat) / (nu * vt) = " << eterm << std::endl;

  double e = exp(eterm);
//  std::cout << "exp(eterm) = " << e << std::endl;

  double w0 = lambert_w0(x * e);
//  std::cout << "w0 = " << w0 << std::endl;
  return i * w0 - isat;
} // double iv

//] [\lambert_w_diode_graph_2]

std::array<double, 5> rss = { 0., 2.18, 10., 51., 249 };  // series resistance (ohm).
std::array<double, 7> vds = { 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9 };  // Diode voltage.
std::array<double, 7> lni = { -19.65, -15.75, -11.86, -7.97, -4.08, -0.0195, 3.6 }; // ln(current).

int main()
{
  try
  {
    std::cout << "Lambert W diode current example." << std::endl;

//[lambert_w_diode_graph_1
    double nu = 1.0; // Assumed ideal.
    double vt = v_thermal(25); // v thermal, Shockley equation, expect about 25 mV at room temperature.
    double boltzmann_k = 1.38e-23; // joules/kelvin
    double temp = 273 + 25;
    double charge_q = 1.6e-19; // column
    vt = boltzmann_k * temp / charge_q;
    std::cout << "V thermal " << vt << std::endl; // V thermal 0.0257025 = 25 mV
    double rsat = 0.;
    double isat = 25.e-15; //  25 fA;
    std::cout << "Isat = " << isat << std::endl;
    double re = 0.3;  // Estimated from slope of straight section of graph (equation 6).
    double v = 0.9;
    double icalc = iv(v, vt, 249., re, isat);
    std::cout << "voltage = " << v << ", current = " << icalc << ", " << log(icalc) << std::endl; // voltage = 0.9, current = 0.00108485, -6.82631
//] [/lambert_w_diode_graph_1]

    // Plot a few measured data points.
    std::map<const double, double> zero_data;  // Extrapolated from slope of measurements with no external resistor.
    zero_data[0.3] = -19.65;
    zero_data[0.4] = -15.75;
    zero_data[0.5] = -11.86;
    zero_data[0.6] = -7.97;
    zero_data[0.7] = -4.08;
    zero_data[0.8] = -0.0195;
    zero_data[0.9] =  3.9;

    std::map<const double, double>  measured_zero_data; // No external series resistor.
    measured_zero_data[0.3] = -19.65;
    measured_zero_data[0.4] = -15.75;
    measured_zero_data[0.5] = -11.86;
    measured_zero_data[0.6] = -7.97;
    measured_zero_data[0.7] = -4.2;
    measured_zero_data[0.72] = -3.5;
    measured_zero_data[0.74] = -2.8;
    measured_zero_data[0.76] = -2.3;
    measured_zero_data[0.78] = -2.0;
    // Measured from Fig 2 as raw data not available.

    double step = 0.1;
    for (int i = 0; i < vds.size(); i++)
    {
      zero_data[vds[i]] = lni[i];
      std::cout << lni[i] << "  " << vds[i] << std::endl;
    }
    step = 0.01;

    std::map<const double, double> data_2;
    for (double v = 0.3; v < 1.; v += step)
    {
      double current = iv(v, vt, 2., re, isat);
      data_2[v] = log(current);
      // std::cout << "v " << v << ", current = " << current << " log current = " << log(current) << std::endl;
    }
    std::map<const double, double> data_10;
    for (double v = 0.3; v < 1.; v += step)
    {
      double current = iv(v, vt, 10., re, isat);
      data_10[v] = log(current);
    //  std::cout << "v " << v << ", current = " << current << " log current = " << log(current) << std::endl;
    }
    std::map<const double, double> data_51;
    for (double v = 0.3; v < 1.; v += step)
    {
      double current = iv(v, vt, 51., re, isat);
      data_51[v] = log(current);
     // std::cout << "v " << v << ", current = " << current << " log current = " << log(current) << std::endl;
    }
    std::map<const double, double> data_249;
    for (double v = 0.3; v < 1.; v += step)
    {
      double current = iv(v, vt, 249., re, isat);
      data_249[v] = log(current);
      // std::cout << "v " << v << ", current = " << current << " log current = " << log(current) << std::endl;
    }

    svg_2d_plot data_plot;

    data_plot.title("Diode current versus voltage")
      .x_size(400)
      .y_size(300)
      .legend_on(true)
      .legend_lines(true)
      .x_label("voltage (V)")
      .y_label("log(current) (A)")
      //.x_label_on(true)
      //.y_label_on(true)
      //.xy_values_on(false)
      .x_range(0.25, 1.)
      .y_range(-20., +4.)
      .x_major_interval(0.1)
      .y_major_interval(4)
      .x_major_grid_on(true)
      .y_major_grid_on(true)
      //.x_values_on(true)
      //.y_values_on(true)
      .y_values_rotation(horizontal)
      //.plot_window_on(true)
      .x_values_precision(3)
      .y_values_precision(3)
      .coord_precision(4) // Needed to avoid stepping on curves.
      .copyright_holder("Paul A. Bristow")
      .copyright_date("2016")
      //.background_border_color(black);
      ;

    // ₀ = subscript zero.
    data_plot.plot(zero_data, "I₀(V)").fill_color(lightgray).shape(none).size(3).line_on(true).line_width(0.5);
    data_plot.plot(measured_zero_data, "Rs=0 Ω").fill_color(lightgray).shape(square).size(3).line_on(true).line_width(0.5);
    data_plot.plot(data_2, "Rs=2 Ω").line_color(blue).shape(none).line_on(true).bezier_on(false).line_width(1);
    data_plot.plot(data_10, "Rs=10 Ω").line_color(purple).shape(none).line_on(true).bezier_on(false).line_width(1);
    data_plot.plot(data_51, "Rs=51 Ω").line_color(green).shape(none).line_on(true).line_width(1);
    data_plot.plot(data_249, "Rs=249 Ω").line_color(red).shape(none).line_on(true).line_width(1);
    data_plot.write("./diode_iv_plot");

    // bezier_on(true);
  }
  catch (std::exception& ex)
  {
    std::cout << ex.what() << std::endl;
  }

}  // int main()

   /*

   //[lambert_w_output_1
   Output:
   Lambert W diode current example.
   V thermal 0.0257025
   Isat = 2.5e-14
   voltage = 0.9, current = 0.00108485, -6.82631
   -19.65  0.3
   -15.75  0.4
   -11.86  0.5
   -7.97  0.6
   -4.08  0.7
   -0.0195  0.8
   3.6  0.9

   //] [/lambert_w_output_1]
   */


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "lapack_common.h"
#include <Eigen/SVD>

// computes the singular values/vectors a general M-by-N matrix A using divide-and-conquer
EIGEN_LAPACK_FUNC(gesdd,(char *jobz, int *m, int* n, Scalar* a, int *lda, RealScalar *s, Scalar *u, int *ldu, Scalar *vt, int *ldvt, Scalar* /*work*/, int* lwork,
                         EIGEN_LAPACK_ARG_IF_COMPLEX(RealScalar */*rwork*/) int * /*iwork*/, int *info))
{
  // TODO exploit the work buffer
  bool query_size = *lwork==-1;
  int diag_size = (std::min)(*m,*n);

  *info = 0;
        if(*jobz!='A' && *jobz!='S' && *jobz!='O' && *jobz!='N')  *info = -1;
  else  if(*m<0)                                                  *info = -2;
  else  if(*n<0)                                                  *info = -3;
  else  if(*lda<std::max(1,*m))                                   *info = -5;
  else  if(*lda<std::max(1,*m))                                   *info = -8;
  else  if(*ldu <1 || (*jobz=='A' && *ldu <*m)
                   || (*jobz=='O' && *m<*n && *ldu<*m))           *info = -8;
  else  if(*ldvt<1 || (*jobz=='A' && *ldvt<*n)
                   || (*jobz=='S' && *ldvt<diag_size)
                   || (*jobz=='O' && *m>=*n && *ldvt<*n))         *info = -10;

  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"GESDD ", &e, 6);
  }

  if(query_size)
  {
    *lwork = 0;
    return 0;
  }

  if(*n==0 || *m==0)
    return 0;

  PlainMatrixType mat(*m,*n);
  mat = matrix(a,*m,*n,*lda);

  int option = *jobz=='A' ? ComputeFullU|ComputeFullV
             : *jobz=='S' ? ComputeThinU|ComputeThinV
             : *jobz=='O' ? ComputeThinU|ComputeThinV
             : 0;

  BDCSVD<PlainMatrixType> svd(mat,option);

  make_vector(s,diag_size) = svd.singularValues().head(diag_size);

  if(*jobz=='A')
  {
    matrix(u,*m,*m,*ldu)   = svd.matrixU();
    matrix(vt,*n,*n,*ldvt) = svd.matrixV().adjoint();
  }
  else if(*jobz=='S')
  {
    matrix(u,*m,diag_size,*ldu)   = svd.matrixU();
    matrix(vt,diag_size,*n,*ldvt) = svd.matrixV().adjoint();
  }
  else if(*jobz=='O' && *m>=*n)
  {
    matrix(a,*m,*n,*lda)   = svd.matrixU();
    matrix(vt,*n,*n,*ldvt) = svd.matrixV().adjoint();
  }
  else if(*jobz=='O')
  {
    matrix(u,*m,*m,*ldu)        = svd.matrixU();
    matrix(a,diag_size,*n,*lda) = svd.matrixV().adjoint();
  }

  return 0;
}

// computes the singular values/vectors a general M-by-N matrix A using two sided jacobi algorithm
EIGEN_LAPACK_FUNC(gesvd,(char *jobu, char *jobv, int *m, int* n, Scalar* a, int *lda, RealScalar *s, Scalar *u, int *ldu, Scalar *vt, int *ldvt, Scalar* /*work*/, int* lwork,
                         EIGEN_LAPACK_ARG_IF_COMPLEX(RealScalar */*rwork*/) int *info))
{
  // TODO exploit the work buffer
  bool query_size = *lwork==-1;
  int diag_size = (std::min)(*m,*n);

  *info = 0;
        if( *jobu!='A' && *jobu!='S' && *jobu!='O' && *jobu!='N') *info = -1;
  else  if((*jobv!='A' && *jobv!='S' && *jobv!='O' && *jobv!='N')
           || (*jobu=='O' && *jobv=='O'))                         *info = -2;
  else  if(*m<0)                                                  *info = -3;
  else  if(*n<0)                                                  *info = -4;
  else  if(*lda<std::max(1,*m))                                   *info = -6;
  else  if(*ldu <1 || ((*jobu=='A' || *jobu=='S') && *ldu<*m))    *info = -9;
  else  if(*ldvt<1 || (*jobv=='A' && *ldvt<*n)
                   || (*jobv=='S' && *ldvt<diag_size))            *info = -11;

  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"GESVD ", &e, 6);
  }

  if(query_size)
  {
    *lwork = 0;
    return 0;
  }

  if(*n==0 || *m==0)
    return 0;

  PlainMatrixType mat(*m,*n);
  mat = matrix(a,*m,*n,*lda);

  int option = (*jobu=='A' ? ComputeFullU : *jobu=='S' || *jobu=='O' ? ComputeThinU : 0)
             | (*jobv=='A' ? ComputeFullV : *jobv=='S' || *jobv=='O' ? ComputeThinV : 0);

  JacobiSVD<PlainMatrixType> svd(mat,option);

  make_vector(s,diag_size) = svd.singularValues().head(diag_size);
  {
        if(*jobu=='A') matrix(u,*m,*m,*ldu)           = svd.matrixU();
  else  if(*jobu=='S') matrix(u,*m,diag_size,*ldu)    = svd.matrixU();
  else  if(*jobu=='O') matrix(a,*m,diag_size,*lda)    = svd.matrixU();
  }
  {
        if(*jobv=='A') matrix(vt,*n,*n,*ldvt)         = svd.matrixV().adjoint();
  else  if(*jobv=='S') matrix(vt,diag_size,*n,*ldvt)  = svd.matrixV().adjoint();
  else  if(*jobv=='O') matrix(a,diag_size,*n,*lda)    = svd.matrixV().adjoint();
  }
  return 0;
}


/**
 * \file Chebyshev2Filter.cpp
 */

#include <boost/math/special_functions/asinh.hpp>

#include "Chebyshev2Filter.h"
#include "helpers.h"
#include "IIRFilter.h"

namespace
{
  template<typename DataType>
  void create_chebyshev2_analog_coefficients(int order, DataType ripple, std::vector<std::complex<DataType> >& z, std::vector<std::complex<DataType> >& p, DataType& k)
  {
    z.clear(); // no zeros for this filter type
    p.clear();

    DataType de = static_cast<DataType>(1.0 / std::sqrt(std::pow(10, (0.1 * ripple)) - 1));
    DataType mu = static_cast<DataType>(boost::math::asinh(1.0 / de) / order);

    for(int i = -order+1; i < order; i += 2)
    {
      std::complex<DataType> p1 = -std::complex<DataType>(std::cos(i * boost::math::constants::pi<DataType>() / (2 * order)), std::sin(i * boost::math::constants::pi<DataType>() / (2 * order)));
      std::complex<DataType> p2 = std::complex<DataType>(std::sinh(mu) * p1.real(), std::cosh(mu) * p1.imag());

      p.push_back(std::complex<DataType>(1, 0) / p2);

      if(i == 0)
      {
        continue;
      }

      z.push_back(std::complex<DataType>(0, 1 / std::sin(i * boost::math::constants::pi<DataType>() / (2 * order))));
    }

    std::complex<DataType> f = 1;
    for(int i = 0; i < p.size(); ++i)
    {
      f *= - p[i];
    }
    for(int i = 0; i < z.size(); ++i)
    {
      f /= - z[i];
    }
    k = f.real();
  }

  template<typename DataType>
  void create_default_chebyshev2_coeffs(size_t order, DataType ripple, DataType Wn, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order), ripple, z, p, k);
    DataType warped = 2 * fs * std::tan(boost::math::constants::pi<DataType>() *  Wn / fs);
    zpk_lp2lp(warped, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }

  template<typename DataType>
  void create_bp_chebyshev2_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order/2), ripple, z, p, k);
    wc1 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc1 / fs);
    wc2 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc2 / fs);

    zpk_lp2bp(std::sqrt(wc1 * wc2), wc2 - wc1, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }

  template<typename DataType>
  void create_bs_chebyshev2_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order/2), ripple, z, p, k);
    wc1 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc1 / fs);
    wc2 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc2 / fs);

    zpk_lp2bs(std::sqrt(wc1 * wc2), wc2 - wc1, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }
}

namespace ATK
{
  template <typename DataType>
  Chebyshev2LowPassCoefficients<DataType>::Chebyshev2LowPassCoefficients(int nb_channels)
  :Parent(1, 1), cut_frequency(0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev2LowPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2LowPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType_>
  void Chebyshev2LowPassCoefficients<DataType_>::set_cut_frequency(DataType_ cut_frequency)
  {
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2LowPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType>
  void Chebyshev2LowPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  void Chebyshev2LowPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_default_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequency / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev2HighPassCoefficients<DataType>::Chebyshev2HighPassCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequency(0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev2HighPassCoefficients<DataType_>::set_cut_frequency(DataType_ cut_frequency)
  {
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2HighPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType_>
  void Chebyshev2HighPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2HighPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2HighPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  void Chebyshev2HighPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_default_chebyshev2_coeffs(in_order, ripple, (input_sampling_rate - 2 * cut_frequency) / input_sampling_rate, coefficients_in, coefficients_out);
    for(int i = in_order - 1; i >= 0; i -= 2)
    {
      coefficients_in[i] = - coefficients_in[i];
      coefficients_out[i] = - coefficients_out[i];
    }
  }

  template <typename DataType>
  Chebyshev2BandPassCoefficients<DataType>::Chebyshev2BandPassCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequencies(0, 0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_cut_frequencies(std::pair<DataType_, DataType_> cut_frequencies)
  {
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_cut_frequencies(DataType_ f0, DataType_ f1)
  {
    this->cut_frequencies = std::make_pair(f0, f1);
    setup();
  }

  template <typename DataType_>
  std::pair<DataType_, DataType_> Chebyshev2BandPassCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2BandPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2BandPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  void Chebyshev2BandPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_bp_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev2BandStopCoefficients<DataType>::Chebyshev2BandStopCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequencies(0, 0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_cut_frequencies(std::pair<DataType_, DataType_> cut_frequencies)
  {
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_cut_frequencies(DataType_ f0, DataType_ f1)
  {
    this->cut_frequencies = std::make_pair(f0, f1);
    setup();
  }

  template <typename DataType_>
  std::pair<DataType_, DataType_> Chebyshev2BandStopCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2BandStopCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2BandStopCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  void Chebyshev2BandStopCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_bs_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template class Chebyshev2LowPassCoefficients<float>;
  template class Chebyshev2LowPassCoefficients<double>;
  template class Chebyshev2HighPassCoefficients<float>;
  template class Chebyshev2HighPassCoefficients<double>;
  template class Chebyshev2BandPassCoefficients<float>;
  template class Chebyshev2BandPassCoefficients<double>;
  template class Chebyshev2BandStopCoefficients<float>;
  template class Chebyshev2BandStopCoefficients<double>;

  template class IIRFilter<Chebyshev2LowPassCoefficients<float> >;
  template class IIRFilter<Chebyshev2LowPassCoefficients<double> >;
  template class IIRFilter<Chebyshev2HighPassCoefficients<float> >;
  template class IIRFilter<Chebyshev2HighPassCoefficients<double> >;
  template class IIRFilter<Chebyshev2BandPassCoefficients<float> >;
  template class IIRFilter<Chebyshev2BandPassCoefficients<double> >;
  template class IIRFilter<Chebyshev2BandStopCoefficients<float> >;
  template class IIRFilter<Chebyshev2BandStopCoefficients<double> >;

}


#include <iostream>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <yaml-cpp/yaml.h>
#include "NumpySaver.hpp"

constexpr double alpha = 1 / 137.035399; // unitless;
constexpr double hbarc = 197.3269631;    // MeV*fm;
constexpr double c = 299792458;          // m/s

typedef std::complex<double> mattype;
constexpr std::complex<double> I(0, 1);

YAML::Node
get_config(int argc, char const *argv[])
{
    YAML::Node config;
    switch (argc)
    {
    case 2:
        config = YAML::LoadFile(argv[1]);
        break;
    default:
        config = YAML::LoadFile("config.yaml");
        break;
    }
    return config;
}

/**
 * @brief Get the potential at each point in r.
 *
 * This is not yet a good implementation, since I am still learning eigen.
 * There is a lot of optimizing potential.
 *
 * @param r in fm
 * @param R0 in fm
 * @param V0 in MeV
 * @param Z1 number of protons
 * @param Z2 number of protons
 * @return potential in MeV
 */
Eigen::ArrayXd get_potential_middle(Eigen::ArrayXd &r, double V0, double Z1, double Z2)
{
    Eigen::ArrayXd pot(r.rows() + 1);

    pot[0] = alpha * hbarc * Z1 * Z2 / r[0] + V0;

    for (unsigned int i = 1; i < pot.rows() - 1; i++)
    {
        pot[i] = alpha * hbarc * Z1 * Z2 * 2 / (r[i - 1] + r[i]);
    }
    pot[pot.rows() - 1] = 0;

    return pot;
}

Eigen::ArrayXd get_potential_integral(Eigen::ArrayXd &r, double V0, double Z1, double Z2)
{
    Eigen::ArrayXd pot(r.rows() + 1);

    pot[0] = alpha * hbarc * Z1 * Z2 / r[0] + V0;

    for (unsigned int i = 1; i < pot.rows() - 1; i++)
    {
        pot[i] = alpha * hbarc * Z1 * Z2 * std::log(r[i] / r[i - 1]) / (r[i] - r[i - 1]);
    }
    pot[pot.rows() - 1] = 0;

    return pot;
}

Eigen::ArrayXd get_middle_points(Eigen::ArrayXd &r)
{
    Eigen::ArrayXd m(r.size() - 1);
    for (int i = 0; i < m.size(); i++)
        m[i] = (r[i] + r[i + 1]) / 2;
    return m;
}

Eigen::ArrayXd get_potential_realistic(Eigen::ArrayXd &r, double V0, double Z1, double Z2, double R0)
{
    Eigen::ArrayXd pot(r.rows() + 1);
    auto mid = get_middle_points(r);

    double a = 0.55;

    for (unsigned int i = 0; i < pot.rows() - 2; i++)
    {
        pot[i] = V0 * (1 + std::cosh(R0 / a)) / (std::cosh(mid[i] / a) + std::cosh(R0 / a));
        if (mid[i] >= R0)
            pot[i] += alpha * hbarc * Z1 * Z2 / (mid[i]);
        else
            pot[i] += alpha * hbarc * Z1 * Z2 / (2 * R0) * (3 - std::pow(mid[i] / R0, 2));
    }
    pot[pot.rows() - 2] = 0;
    pot[pot.rows() - 1] = 0;

    return pot;
}

Eigen::ArrayXd get_potential_left(Eigen::ArrayXd &r, double V0, double Z1, double Z2)
{
    Eigen::ArrayXd pot(r.rows() + 1);

    pot[0] = alpha * hbarc * Z1 * Z2 / r[0] + V0;

    for (unsigned int i = 1; i < pot.rows(); i++)
    {
        pot[i] = alpha * hbarc * Z1 * Z2 / r[i - 1];
    }

    return pot;
}

Eigen::ArrayXd get_potential_right(Eigen::ArrayXd &r, double V0, double Z1, double Z2)
{
    Eigen::ArrayXd pot(r.rows() + 1);

    pot[0] = alpha * hbarc * Z1 * Z2 / r[0] + V0;

    for (unsigned int i = 1; i < pot.rows() - 1; i++)
    {
        pot[i] = alpha * hbarc * Z1 * Z2 / r[i];
    }

    pot[pot.rows() - 1] = 0;

    return pot;
}

Eigen::SparseMatrix<mattype> get_matrix(Eigen::ArrayXcd &k, Eigen::ArrayXd &r)
{
    int N = r.size() - 1;
    auto matrix = Eigen::SparseMatrix<mattype>(2 * N + 3, 2 * N + 3);

    // 2N+2 equations, 4 coefficients in each row, +1 A constraint -2 last boundary
    matrix.reserve(4 * (2 * N + 2) + 1 - 2);

    matrix.insert(0, 0) = 1;

    for (int i = 0; i < N; i++)
    {
        auto j = 2 * i + 1;
        matrix.insert(j, j - 1) = std::exp(I * k[i] * r[i]);
        matrix.insert(j, j) = std::exp(-I * k[i] * r[i]);
        matrix.insert(j, j + 1) = -std::exp(I * k[i + 1] * r[i]);
        matrix.insert(j, j + 2) = -std::exp(-I * k[i + 1] * r[i]);

        matrix.insert(j + 1, j - 1) = I * k[i] * std::exp(I * k[i] * r[i]);
        matrix.insert(j + 1, j) = -I * k[i] * std::exp(-I * k[i] * r[i]);
        matrix.insert(j + 1, j + 1) = -I * k[i + 1] * std::exp(I * k[i + 1] * r[i]);
        matrix.insert(j + 1, j + 2) = I * k[i + 1] * std::exp(-I * k[i + 1] * r[i]);
    }

    matrix.insert(2 * N + 1, 2 * N) = std::exp(I * k[N] * r[N]);
    matrix.insert(2 * N + 1, 2 * N + 1) = std::exp(-I * k[N] * r[N]);
    matrix.insert(2 * N + 1, 2 * N + 2) = -std::exp(I * k[N + 1] * r[N]);

    matrix.insert(2 * N + 2, 2 * N) = I * k[N] * std::exp(I * k[N] * r[N]);
    matrix.insert(2 * N + 2, 2 * N + 1) = -I * k[N] * std::exp(-I * k[N] * r[N]);
    matrix.insert(2 * N + 2, 2 * N + 2) = -I * k[N + 1] * std::exp(I * k[N + 1] * r[N]);

    return matrix;
}

Eigen::ArrayXd get_pdf(Eigen::VectorXcd &sol, Eigen::ArrayXcd &k, Eigen::ArrayXd &r, Eigen::ArrayXd &r_plot)
{
    Eigen::ArrayXd pdf(r_plot.size());
    int i_k = 0;

    auto this_k = k[0];
    auto this_r = r[0];
    auto this_A = sol[0];
    auto this_B = sol[1];
    for (int i = 0; i < pdf.size(); i++)
    {
        while (r_plot[i] > this_r)
        {
            i_k++;
            this_k = k[i_k];
            this_r = r[i_k];
            this_A = sol[2 * i_k];
            this_B = sol[2 * i_k + 1];
        }
        pdf[i] = std::norm(this_A * std::exp(I * this_k * r_plot[i]) + this_B * std::exp(-I * this_k * r_plot[i]));
    }
    return pdf;
}

Eigen::VectorXcd get_rhs(int N)
{
    Eigen::VectorXcd vec = Eigen::VectorXcd::Zero(2 * N + 3);
    vec[0] = 1;
    return vec;
}

void run_simulation(YAML::Node &simulation_settings, YAML::Node &data)
{
    auto N = simulation_settings["N"].as<int>();
    auto R0 = simulation_settings["R0"].as<double>();
    auto R_last = simulation_settings["R_last"].as<double>();
    auto V0 = simulation_settings["V0"].as<double>();
    auto E_bind_alpha = simulation_settings["E_bind_alpha"].as<double>();
    auto pot_method = simulation_settings["pot_method"].as<std::string>();

    auto name = simulation_settings["Name"].as<std::string>();

    std::cout << "Simulation Setting: " << name << "\n"
              << std::endl;

    for (std::size_t i = 0; i < data.size(); i++)
    {
        auto A_parent = data[i]["A_parent"].as<double>();
        auto Z_parent = data[i]["Z_parent"].as<double>();
        auto E_bind_p = data[i]["E_bind_p"].as<double>();
        auto E_bind_d = data[i]["E_bind_d"].as<double>();
        auto half_life_lit = data[i]["half_life"].as<double>();

        auto symbol = data[i]["Symbol"].as<std::string>();

        auto E_alpha = E_bind_p - E_bind_d - E_bind_alpha;

        double A_daughter = A_parent - 4;
        double Z_daughter = Z_parent - 2;
        double m_alpha = 3727.379;

        auto R0_this = R0 * std::pow(A_daughter, 1. / 3.);

        // lets first construct the r array as it is constant for every nucleus
        Eigen::ArrayXd r;
        if (pot_method == "realistic")
            r = Eigen::ArrayXd::LinSpaced(N + 1, 0, R_last);
        else
            r = Eigen::ArrayXd::LinSpaced(N + 1, R0_this, R_last);

        Eigen::ArrayXcd pot;
        if (pot_method == "middle")
        {
            pot = get_potential_middle(r, V0, Z_daughter, 2); // MeV
        }
        else if (pot_method == "left")
        {
            pot = get_potential_left(r, V0, Z_daughter, 2); // MeV
        }
        else if (pot_method == "right")
        {
            pot = get_potential_right(r, V0, Z_daughter, 2); // MeV
        }
        else if (pot_method == "integrate")
        {
            pot = get_potential_integral(r, V0, Z_daughter, 2); // MeV
        }
        else if (pot_method == "realistic")
        {
            pot = get_potential_realistic(r, V0, Z_daughter, 2, R0_this); // MeV
        }
        else
        {
            std::cerr << "Warning! Unkown potential method: " << pot_method << std::endl;
            pot = get_potential_middle(r, V0, Z_daughter, 2); // MeV
        }

        Eigen::ArrayXcd k = (2 * m_alpha * (E_alpha - pot)).sqrt() / hbarc;

        // Save potential to plot it!
        Eigen::ArrayXd r_pot(r.size() + 1);
        if (pot_method == "realistic")
        {
            r_pot << 0, get_middle_points(r), r(Eigen::last);
        }
        else
        {
            r_pot << 0, r;
        }
        NumpySaver(std::string("build/output/" + name + "-" + symbol + "-pot.npy")) << r_pot << pot.real();

        auto mat = get_matrix(k, r);
        auto b = get_rhs(N);

        Eigen::SparseLU<Eigen::SparseMatrix<mattype>, Eigen::COLAMDOrdering<int>> solver;
        // Compute the ordering permutation vector from the structural pattern of A
        solver.analyzePattern(mat);
        // Compute the numerical factorization
        solver.factorize(mat);
        // Use the factors to solve the linear system
        Eigen::VectorXcd x = solver.solve(b);

        // R and T factor
        double R = std::norm(x(1));
        double T = std::norm(x(Eigen::last)) * std::abs(k(Eigen::last)) / std::abs(k[0]);
        // check if R+T=1
        if (std::abs(R + T - 1) > 1e-3)
        {
            std::cerr << "Warning! R+T not equal to 1; R+T = " << R + T << "\n";
        }

        // plot pdf
        Eigen::ArrayXd r_plot = Eigen::ArrayXd::LinSpaced(1000, 0, R_last);
        auto pdf = get_pdf(x, k, r, r_plot);

        NumpySaver pdf_saver(std::string("build/output/" + name + "-" + symbol + "-pdf.npy")); // << r_plot << pdf;
        pdf_saver.setPrecision(15);
        pdf_saver << r_plot << pdf;

        double v = std::sqrt(2 * E_alpha / m_alpha) * c;              // m/s
        double half_life = std::log(2) * 2 * R0_this / T / v * 1e-15; // s

        std::cerr << "Lifetime of " << symbol << " " << half_life << "s (Difference to literature value of "
                  << (half_life / half_life_lit - 1) * 100 << "%)" << std::endl;
    }
}

int main(int argc, char const *argv[])
{
    auto config = get_config(argc, argv);
    auto data = config["data"];

    for (std::size_t i = 0; i < config["simulation_settings"].size(); i++)
    {
        std::cout << "##################################\n"
                  << "# Running Simulation " << i + 1 << "/" << config["simulation_settings"].size()
                  << "\n##################################\n";
        auto settings = config["simulation_settings"][i];
        run_simulation(settings, data);
        std::cout << "\n\n";
    }

    return 0;
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_POLYNOMIALS_FUNCTION_SCALAR_LAGUERRE_HPP_INCLUDED
#define NT2_TOOLBOX_POLYNOMIALS_FUNCTION_SCALAR_LAGUERRE_HPP_INCLUDED
#include <boost/math/special_functions.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <nt2/sdk/meta/adapted_traits.hpp>
#include <nt2/include/functions/oneplus.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A1 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::laguerre_, tag::cpu_,
                          (A0)(A1),
                          (integer_<A0>)(arithmetic_<A1>)
                         )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::laguerre_(tag::integer_,tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::floating(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      return nt2::laguerre(a0, type(a1));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A1 is real_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::laguerre_, tag::cpu_,
                          (A0)(A1),
                          (integer_<A0>)(real_<A1>)
                         )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::laguerre_(tag::integer_,tag::real_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> : meta::strip<A1>{};

    NT2_FUNCTOR_CALL(2)
    {
      A1 p0 = One<A1>();
      if(a0 == 0) return p0;
      A1 p1 = p0-a1;
      A0 c = 1;
      while(c < a0)
      {
        std::swap(p0, p1);
        p1 = laguerre_next(c, a1, p0, p1);
        ++c;
      }
      return p1;
    }
  private:
    template <class T, class T1, class T2>
    static inline T
    laguerre_next(const uint32_t& n, const T& x, const T1 &Ln, const T2& Lnm1)
    {
      const T np1 = oneplus(n);
      return ((n + np1 - x) * Ln - n *Lnm1) / np1;
    }
  };
} }

#endif
// modified by jt the 26/12/2010


/*

Copyright (c) 2013  Ghassen Hamrouni

Abstract:

This module provides some spectral graph algorithms

Author:

Ghassen Hamrouni <ghamrouni.iptech@gmail.com> 23-07-2013

Revision History:

*/

#ifndef R_1_PUBLIC_GRAPH_LAPLACIAN_SPECTRAL_H_
#define R_1_PUBLIC_GRAPH_LAPLACIAN_SPECTRAL_H_

#include <Eigen/Dense>
#include "SpectralGraph.hpp"

namespace R1 {
	class LaplacianSpectralGraph : public SpectralGraph {
	public:
		virtual ~LaplacianSpectralGraph() {}

		LaplacianSpectralGraph(int NVertex) : SpectralGraph(NVertex) {

		}

		//////////////////////////////////////////////////////////////////////////
		//								Laplacian
		//////////////////////////////////////////////////////////////////////////

		double laplacian(int u, int v) {

			double dv = degree(v);

			if (u == v && dv != 0) {
				return 1 - adjacency_matrix(v, v) / dv;
			}

			if (adjacency_matrix(u, v) >= 1.0) {

				double du = degree(u);

				return -adjacency_matrix(u, v) / sqrt(du * dv);
			}

			return 0.0;
		}

		Eigen::MatrixXd getLaplacianMatrix() {
			Eigen::MatrixXd laplacian_matrix(n_vertex, n_vertex);

			for (int i = 0; i < n_vertex; i++)	{
				for (int j = 0; j < n_vertex; j++)	{
					laplacian_matrix(i, j) = laplacian(i, j);
				}
			}

			return laplacian_matrix;
		}

		Eigen::VectorXd laplacianSpectrum() {

			Eigen::MatrixXd L = getLaplacianMatrix();

			Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eg1(L);
			return eg1.eigenvalues();
		}

		static bool isospectralLaplacian(LaplacianSpectralGraph g1, LaplacianSpectralGraph g2, double epsilon) {

			Eigen::VectorXd normalizedEg1 = g1.laplacianSpectrum().normalized();
			Eigen::VectorXd normalizedEg2 = g2.laplacianSpectrum().normalized();

			if (normalizedEg1.cols() != normalizedEg2.cols()) {
				return false;
			}

			if (normalizedEg1.rows() != normalizedEg2.rows()) {
				return false;
			}

			for (int i = 0; i < normalizedEg1.rows(); i++)	{
				for (int j = 0; j < normalizedEg1.cols(); j++)	{
					if (abs(normalizedEg1(i, j) - normalizedEg2(i, j)) > epsilon) {
						return false;
					}
				}
			}

			return true;
		}

		static double distance(LaplacianSpectralGraph g1, LaplacianSpectralGraph g2) {
			Eigen::VectorXd normalizedEg1 = g1.laplacianSpectrum().normalized();
			Eigen::VectorXd normalizedEg2 = g2.laplacianSpectrum().normalized();

			double dist = 0.0;

			if (normalizedEg1.cols() != normalizedEg2.cols()) {
				return 0.0;
			}

			if (normalizedEg1.rows() != normalizedEg2.rows()) {
				return 0.0;
			}

			for (int i = 0; i < normalizedEg1.rows(); i++)	{
				for (int j = 0; j < normalizedEg1.cols(); j++)	{
					dist += abs(normalizedEg1(i, j) - normalizedEg2(i, j));
				}
			}

			return dist;
		}
	};
}

#endif


/**
 * ****************************************************************************
 * Copyright (c) 2015, Robert Lukierski.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * ****************************************************************************
 * Simple Least-Squares Solver.
 * ****************************************************************************
 */

#ifndef VISIONCORE_MATH_LEAST_SQUARES_HPP
#define VISIONCORE_MATH_LEAST_SQUARES_HPP

#include <VisionCore/Platform.hpp>
#include <Eigen/Cholesky>

namespace vc
{

namespace math
{

// TODO eigenify

template<typename T, int dim>
class LSQ
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef T Scalar;
    static constexpr int Dimension = dim;
    typedef Eigen::Matrix<Scalar,Dimension,Dimension> MatrixType;
    typedef Eigen::Matrix<Scalar,Dimension,1> VectorType;
    typedef Eigen::AutoDiffScalar<Eigen::Matrix<Scalar,Dimension,1>> JetType;

    EIGEN_DEVICE_FUNC inline LSQ(const std::size_t nc = 0)
    {
        reset();
        NumConstraints = nc;
    }

    EIGEN_DEVICE_FUNC inline void reset()
    {
        A.setZero();
        B.setZero();
        Error = Scalar(0.0);
        NumConstraints = 0;
    }

    EIGEN_DEVICE_FUNC inline void update(const VectorType& J, const Scalar& res, const Scalar& weight = Scalar(1.0))
    {
        A.noalias() += J * J.transpose() * weight;
        B.noalias() -= J * (res * weight);
        Error += res * res * weight;
        NumConstraints += 1;
    }

    EIGEN_DEVICE_FUNC inline void update(const JetType& j, const Scalar& weight = Scalar(1.0))
    {
        update(j.deriviatives(), j.value(), weight);
    }

    EIGEN_DEVICE_FUNC inline void finishAndDivide()
    {
        A /= (Scalar)NumConstraints;
        B /= (Scalar)NumConstraints;
        Error /= (Scalar)NumConstraints;
    }

    EIGEN_DEVICE_FUNC inline VectorType solve()
    {
        return A.ldlt().solve(-B);
    }

    MatrixType A;
    VectorType B;
    Scalar Error;
    std::size_t NumConstraints;
};

}

}

#endif // VISIONCORE_MATH_LEAST_SQUARES_HPP