// Header
#include <polyvec/curve-tracer/curve_bezier.hpp>

// C++ stl
#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <limits>
#include <stdexcept>

// Eigen
#include <Eigen/Core>

// Polyvec
#include <polyvec/geom.hpp>
#include <polyvec/geometry/smooth_curve.hpp>
#include <polyvec/geometry/line.hpp>

namespace polyvec {

	// Taken from https://github.com/inkscape/lib2geom: src/2geom/bezier.h
	// Compute the value of a Bernstein-Bezier polynomial.
	// This method uses a Horner-like fast evaluation scheme.
	// param t Time value
	// param control_points control points (one per column)
	//
	template <typename Derived>
	Eigen::Matrix<double, Derived::RowsAtCompileTime,1>
		_bezier_value_at(const Eigen::MatrixBase<Derived>& control_points, const double t) {

		assert_break(t <= 1.);
		assert_break(t >= 0);

		double u = 1.0 - t;
		int degree = control_points.cols() - 1;

		Eigen::Matrix<double, Derived::RowsAtCompileTime, 1> ans;

		double bc = 1;
		double tn = 1;

		ans = control_points.col(0) * u;

		for (unsigned i = 1; i < degree; i++) {
			tn = tn * t;
			bc = bc * (degree - i + 1) / i;
			ans = (ans + tn * bc * control_points.col(i)) * u;
		}
		ans = (ans + tn * t * control_points.col(degree));

		return ans;
	}

	// From https://github.com/inkscape/lib2geom/: src/2geom/bezier.cpp
	Eigen::Matrix2Xd
		_derivative_bezier_curve(const Eigen::Matrix2Xd& control_points) {
		assert_break(control_points.cols() >= 2);
		Eigen::Matrix2Xd ans(2, control_points.cols() - 1);

		const int order = (int)control_points.cols() - 1;

		for (unsigned i = 0; i < ans.cols(); ++i) {
			ans.col(i) = order * (control_points.col(i + 1) - control_points.col(i));
		}

		return ans;
	}

	Eigen::Matrix3Xd
		_tesselate(const Eigen::Matrix2Xd& control_points, const int n_sampling) {
		Eigen::Matrix3Xd ans(3, n_sampling);

		for (int i = 0; i < n_sampling; ++i) {
			const double t = 1. / (n_sampling - 1) * i;
			ans.col(i) (0) = t;
			ans.col(i).tail<2>() = _bezier_value_at(control_points, t);
		}

		return ans;
	}

	const std::vector<double>&
		_get_gauss_quad_locs() {
		static std::vector<double> ans;
		static bool is_init = false;

		if (!is_init) {
			ans.resize(6);
			ans[0] = -9.3246951420315202781230155449399e-01L;
			ans[1] = -6.6120938646626451366139959501991e-01L;
			ans[2] = -2.3861918608319690863050172168071e-01L;
			ans[3] = -ans[2];
			ans[4] = -ans[1];
			ans[5] = -ans[0];

			for (double& a : ans) {
				a = a / 2. + 0.5;
			}

			is_init = true;
		}

		return ans;
	}

	const std::vector<double>&
		_get_gauss_quad_weights() {
		static std::vector<double> ans;
		static bool is_init = false;

		if (!is_init) {
			ans.resize(6);
			ans[0] = 1.7132449237917034504029614217273e-01L;
			ans[1] = 3.6076157304813860756983351383772e-01L;
			ans[2] = 4.6791393457269104738987034398955e-01L;
			ans[3] = ans[2];
			ans[4] = ans[1];
			ans[5] = ans[0];

			for (double& a : ans) {
				a = a / 2.;
			}

			is_init = true;
		}

		return ans;
	}

// ===================================================================
//                            Bezier Curve
// ===================================================================

    Eigen::Vector2d
    BezierCurve::pos ( const double t ) const {
        return _bezier_value_at ( _control_points_d0, t );
    }

    Eigen::Vector2d
    BezierCurve::dposdt ( const double t ) const {
        return _bezier_value_at ( _control_points_d1, t );
    }

    Eigen::Vector2d
    BezierCurve::dposdtdt ( const double t ) const {
        return _bezier_value_at ( _control_points_d2, t );
    }

	Eigen::Vector2d BezierCurve::dposdtdtdt(const double t) const
	{
		return _bezier_value_at(_control_points_d3, t);
	}

    Eigen::Matrix2Xd
    BezierCurve::dposdparams ( const double t ) const {
        const int n_points = n_control_points();

        // Eigen::Matrix2Xd ans(2, n_points);

        // Coefficient of bernstein monomials in a bernstein polynomial
        const double b0[] = {1., 0., 0., 0.};
        const double b1[] = {0., 1., 0., 0.};
        const double b2[] = {0., 0., 1., 0.};
        const double b3[] = {0., 0., 0., 1.};

        // Value of derivatives
		Eigen::MatrixXd control(4, 4);
		control <<
			1., 0., 0., 0.,
			0., 1., 0., 0.,
			0., 0., 1., 0.,
			0., 0., 0., 1.;

		auto dxdp = _bezier_value_at(control, t);

        Eigen::Matrix2Xd ans = Eigen::Matrix2Xd::Zero ( 2, n_points * 2 );
        ans.row ( 0 ) << dxdp(0), 0, dxdp(1), 0, dxdp(2), 0, dxdp(3), 0;
        ans.row ( 1 ) << 0, dxdp(0), 0, dxdp(1), 0, dxdp(2), 0, dxdp(3);

        return ans;
    }

    Eigen::Matrix2Xd
    BezierCurve::dposdtdparams ( const double t ) const {
        const int order = 3;
        const double drder = order;
        const int n_points = n_control_points();

        // Eigen::Matrix2Xd ans(2, n_points);

        // Coefficient of bernstein monomials in a bernstein polynomial
        const double b0_deriv[] = {-drder, 0., 0.};
        const double b1_deriv[] = {drder, -drder, 0.};
        const double b2_deriv[] = {0., drder, -drder};
        const double b3_deriv[] = {0., 0., drder};

		Eigen::MatrixXd control(4, 3);
		control <<
			-drder, 0., 0.,
			drder, -drder, 0.,
			0., drder, -drder,
			0., 0., drder;

        // Value of derivatives
		auto dxdtdp = _bezier_value_at(control, t);

        Eigen::Matrix2Xd ans = Eigen::Matrix2Xd::Zero ( 2, n_points * 2 );
        ans.row ( 0 ) << dxdtdp(0), 0, dxdtdp(1), 0, dxdtdp(2), 0, dxdtdp(3), 0;
        ans.row ( 1 ) << 0, dxdtdp(0), 0, dxdtdp(1), 0, dxdtdp(2), 0, dxdtdp(3);

        return ans;
    }

    Eigen::Matrix2Xd
    BezierCurve::dposdtdtdparams ( const double t ) const {
        const int order = 3;
        const double drder = order*(order-1);
        const int n_points = n_control_points();

        // Eigen::Matrix2Xd ans(2, n_points);

        // Coefficient of bernstein monomials in a bernstein polynomial
		Eigen::MatrixXd control(4, 2);
		control <<
			drder, 0.,
			-2 * drder, drder,
			drder, -2 * drder,
			0., drder;

        // Value of derivatives
		auto dxdtdtdp = _bezier_value_at(control, t);

        Eigen::Matrix2Xd ans = Eigen::Matrix2Xd::Zero ( 2, n_points * 2 );
        ans.row ( 0 ) << dxdtdtdp(0), 0, dxdtdtdp(1), 0, dxdtdtdp(2), 0, dxdtdtdp(3), 0;
        ans.row ( 1 ) << 0, dxdtdtdp(0), 0, dxdtdtdp(1), 0, dxdtdtdp(2), 0, dxdtdtdp(3);

        return ans;
    }

	Eigen::Matrix2Xd BezierCurve::dposdtdtdtdparams(const double t) const
	{
		const int order = 3;
		const double factor = 6;
		const int n_points = n_control_points();

		// Coefficient of bernstein monomials in a bernstein polynomial
		Eigen::MatrixXd control(4, 1);
		control <<
			-factor,
			3 * factor,
			-3 * factor,
			factor;

		// Value of derivatives
		auto dxdtdp = _bezier_value_at(control, t);

		Eigen::Matrix2Xd ans = Eigen::Matrix2Xd::Zero(2, n_points * 2);
		ans.row(0) << dxdtdp(0), 0, dxdtdp(1), 0, dxdtdp(2), 0, dxdtdp(3), 0;
		ans.row(1) << 0, dxdtdp(0), 0, dxdtdp(1), 0, dxdtdp(2), 0, dxdtdp(3);

		return ans;
	}

    double
    BezierCurve::project ( const Eigen::Vector2d& point ) const {

        // int closest_idx = -1;
        double closest_t = -1;
        double closest_dist2 = std::numeric_limits<double>::max();

        // First find an initial guess for the closest point
        for ( int i = 0; i < _tesselation.cols(); ++i ) {
            double dist2 = ( point - _tesselation.col ( i ).tail<2>() ).squaredNorm();

            if ( dist2 < closest_dist2 ) {
                // closest_idx = i;
                closest_t = _tesselation.col ( i ) ( 0 );
                closest_dist2 = dist2;
            }
        }

        // Now refine your guess using newton iterations
        auto newton_iteration = [&] ( double guess ) -> double {
            Eigen::Vector2d post, dert, der2t;
            post = this->pos ( guess );
            dert = this->dposdt ( guess );
            der2t = this->dposdtdt ( guess );

            double dot = dert.dot ( point - post );
            double dotDer = der2t.dot ( point - post ) - dert.squaredNorm();

            // Make sure the iteration does not make things worse
            if ( dotDer >= -1e-30 ) {
                return guess;
            }

            // Make sure the iteration does not shoot us out of the range
            return std::max ( 0., std::min ( 1., guess - dot / dotDer ) );
        };
        closest_t = newton_iteration ( closest_t );
        closest_t = newton_iteration ( closest_t );
        closest_t = newton_iteration ( closest_t );
        closest_t = newton_iteration ( closest_t );

        return closest_t;
    }

    Eigen::VectorXd
    BezierCurve::dtprojectdparams ( const double time, const Eigen::Vector2d& point ) const {
        const int n_params = n_control_points() * 2;
        const double tend = 1.;

        return SmoothCurveUtil::projection_derivatives (
                   n_params,
                   time,
                   tend,
                   point,
                   pos ( time ),
                   dposdt ( time ),
                   dposdtdt ( time ),
                   dposdparams ( time ),
                   dposdtdparams ( time ) );
    }

    Eigen::Matrix2Xd
    BezierCurve::dposprojectdparams ( const double t, const Eigen::VectorXd& dtprojectdparams ) const {
        return dposdparams ( t ) + dposdt ( t ) * dtprojectdparams.transpose();
    }

    Eigen::Matrix2Xd
    BezierCurve::dposdtprojectdparams ( const double t, const Eigen::VectorXd& dtprojectdparams ) const {
        return dposdtdparams ( t ) + dposdtdt ( t ) * dtprojectdparams.transpose();
    }

    void
    BezierCurve::set_control_points ( const Eigen::Matrix2Xd& control_points_d0_in ) {
        assert_break ( control_points_d0_in.cols() == n_control_points() );
        _control_points_d0 = control_points_d0_in;
        _control_points_d1 = _derivative_bezier_curve ( _control_points_d0 );
        _control_points_d2 = _derivative_bezier_curve ( _control_points_d1 );
		_control_points_d3 = _derivative_bezier_curve ( _control_points_d2 );
        _tesselation = _tesselate ( _control_points_d0, _n_tesselation );
    }

    const Eigen::Matrix2Xd&
    BezierCurve::get_control_points() const {
        return _control_points_d0;
    }

	void BezierCurve::set_params(const Eigen::VectorXd & params)
	{
		Eigen::Matrix2Xd controlPoints(2, 4);
		controlPoints << params[0], params[2], params[4], params[6], params[1], params[3], params[5], params[7];
		set_control_points(controlPoints);
	}

	Eigen::VectorXd BezierCurve::get_params() const
	{
		Eigen::VectorXd params(n_params());
		params <<
			_control_points_d0.coeff(0, 0), _control_points_d0.coeff(1, 0),
			_control_points_d0.coeff(0, 1), _control_points_d0.coeff(1, 1),
			_control_points_d0.coeff(0, 2), _control_points_d0.coeff(1, 2),
			_control_points_d0.coeff(0, 3), _control_points_d0.coeff(1, 3);
		return params;
	}

	//const Eigen::Matrix3Xd&
    //BezierCurve::get_tesselation3() {
    //    return _tesselation;
    //}

    Eigen::VectorXd
    BezierCurve::get_tesselationt() const {
        return _tesselation.row(0).transpose();
    }

    Eigen::Matrix2Xd
    BezierCurve::get_tesselation2() const {
        return _tesselation.bottomRows<2>();
    }

    double
    BezierCurve::length() const {
        const std::vector<double>& ww = _get_gauss_quad_weights();
        const std::vector<double>& tt = _get_gauss_quad_locs();
        double len = 0;

        for ( unsigned i = 0; i < tt.size(); ++i ) {
            len += ww[i] * dposdt ( tt[i] ).norm();
        }

        return len;
    }

    Eigen::VectorXd
    BezierCurve::dlengthdparams() {
        const int n_points = n_control_points();
        const double tol = 1e-10;
        const std::vector<double>& ww = _get_gauss_quad_weights();
        const std::vector<double>& tt = _get_gauss_quad_locs();

        Eigen::VectorXd dlendprams = Eigen::VectorXd ( n_points * 2 );
        dlendprams.setZero();

        for ( unsigned i = 0; i < tt.size(); ++i ) {
            Eigen::Vector2d tang = dposdt ( tt[i] );
            double tang_norm = std::max ( tang.norm(), tol );
            dlendprams += ww[i] * 1. / tang_norm * dposdtdparams ( tt[i] ).transpose() * tang;
        }

        return dlendprams;
    }

	geom::aabb BezierCurve::get_bounding_box() const
	{
		geom::aabb b;
		for (int i = 0; i < 4; ++i)
			b.add(_control_points_d0.col(i));
		return b;
	}

	BezierCurve::BezierCurve(const Eigen::Matrix2Xd& C) {
		set_control_points(C);
	}

	std::pair<GlobFitCurve*, GlobFitCurve*> BezierCurve::split(double t) const
	{
		Eigen::Matrix2Xd cLeft(2, 4), cRight(2, 4);

		auto& p = _control_points_d0;

		// left curve
		cLeft.col(0) = p.col(0);
		cLeft.col(1) = _bezier_value_at(p.leftCols<2>(), t);
		cLeft.col(2) = _bezier_value_at(p.leftCols<3>(), t);
		cLeft.col(3) = _bezier_value_at(p.leftCols<4>(), t);

		// right curve
		cRight.col(0) = _bezier_value_at(p.rightCols<4>(), t);
		cRight.col(1) = _bezier_value_at(p.rightCols<3>(), t);
		cRight.col(2) = _bezier_value_at(p.rightCols<2>(), t);
		cRight.col(3) = p.col(3);

		return std::make_pair(new BezierCurve(cLeft), new BezierCurve(cRight));
	}
}


// Copyright University of Warwick 2014
// 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)

// Authors:
// John Back

/*! \file LpcConvexHull.cc
    \brief Class that calculates the "convex hull" extent of a cluster of hits
*/

#include "LACE/LpcConvexHull.hh"

#include "LACE/LpcCluster.hh"

#include <Eigen/Dense>

LpcConvexHull::LpcConvexHull()
{
}

LpcConvexHull::~LpcConvexHull()
{
}

void LpcConvexHull::findLengths(LpcCluster* theCluster)
{

    // Find the convex hull lengths along the spatial dimensions of
    // the cluster and store them in the cluster pointer. Originally,
    // this method would require the calculation of the convex hull
    // points with QHull. But, the extent of the hull is simply given
    // by the size of the rectangular box that will enclose the
    // cluster "ellipsoid", i.e. the lengths (max-min range) along
    // each of the principal axes

    if (!theCluster) {return;}

    //Retrieve the matrix of cluster hit positions
    Eigen::MatrixXd hitCoords = theCluster->getHitPositions();

    // We need to transform the point co-ordinates to lie along the principal axes.
    // Find the eigenvectors of the covariance matrix of the hit co-ordinates
    Eigen::MatrixXd covMatrix = functions_.getCovarianceMatrix(hitCoords);

    std::pair<Eigen::VectorXd, Eigen::MatrixXd> pcaEigen =
	functions_.findNormEigenVectors(covMatrix);

    Eigen::MatrixXd eVectors = pcaEigen.second;

    Eigen::MatrixXd eV = eVectors.transpose();

    // Transform the hit co-ordinates. Row = point, col = x,y,z,...
    Eigen::MatrixXd transCoords = hitCoords*eV;

    // Get the lengths along each axis
    int nDim = hitCoords.cols();
    Eigen::VectorXd lengths = Eigen::VectorXd::Zero(nDim);

    for (int i = 0; i < nDim; i++) {

	Eigen::VectorXd coordCol = transCoords.col(i);
	double maxVal = coordCol.maxCoeff();
	double minVal = coordCol.minCoeff();
	double range = fabs(maxVal - minVal);

	lengths(i) = range;

    }

    theCluster->storeConvexHull(lengths);

}


#include "utils.hpp"
#include <cmath>
#include <fstream>
#include <boost/range/numeric.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/median.hpp>

using namespace std;

namespace delphi::utils {

/**
 * Returns the square of a number.
 */
// double sqr(double x) { return x * x; }

/**
 * Returns the sum of a vector of doubles.
 */
double sum(const std::vector<double> &v) { return boost::accumulate(v, 0.0); }

/**
 * Returns the arithmetic mean of a vector of doubles.
 * Updated based on:
 * https://codereview.stackexchange.com/questions/185450/compute-mean-variance-and-standard-deviation-of-csv-number-file
 */
double mean(const std::vector<double> &v) {
    if (v.empty()) {
        return std::numeric_limits<double>::quiet_NaN();
    }

    return sum(v) / v.size();
}

/**
 * Returns the sample standard deviation of a vector of doubles.
 * Based on:
 * https://codereview.stackexchange.com/questions/185450/compute-mean-variance-and-standard-deviation-of-csv-number-file
 */
double standard_deviation(const double mean, const std::vector<double>& v)
{
    if (v.size() <= 1u)
        return std::numeric_limits<double>::quiet_NaN();

    auto const add_square = [mean](double sum, int i) {
        auto d = i - mean;
        return sum + d*d;
    };
    double total = std::accumulate(v.begin(), v.end(), 0.0, add_square);
    return sqrt(total / (v.size() - 1));
}

/**
 * Returns the median of a vector of doubles.
 */
double median(const std::vector<double> &xs) {
  using namespace boost::accumulators;
  accumulator_set<double, features<tag::median>> acc;
  for (auto x : xs) {
    acc(x);
  }
  return boost::accumulators::median(acc);
}

double log_normpdf(double x, double mean, double sd) {
  double var = pow(sd, 2);
  double log_denom = -0.5 * log(2 * M_PI) - log(sd);
  double log_nume = pow(x - mean, 2) / (2 * var);

  return log_denom - log_nume;
}

nlohmann::json load_json(string filename) {
  ifstream i(filename);
  nlohmann::json j = nlohmann::json::parse(i);
  return j;
}

} // namespace delphi::utils


#include <armadillo>
#include <math.h>
#include <fruitpunch/Graphics/Matrices.h>
#include <fruitpunch/Graphics/Vertex.h>
#include <fruitpunch/Graphics/Renderer.h>
#include <vector>

#define PI180 0.0174532925

using namespace arma;
using namespace std;

namespace fp_core {

/**
 * Returns
 */
fmat33 rotation_matrix(float angle) {
  float rad = angle * PI180;

  fmat33 rot;
  rot << cos(rad) << -sin(rad) << 0 << endr
      << sin(rad) << cos(rad) << 0 << endr
      << 0 << 0 << 1 << endr;
  return rot;
}

fmat33 translation_matrix(point p) {
  // generates the translation matrix
  fmat33 trans;
  trans.eye();
  trans.at(0, 2) = p.x;
  trans.at(1, 2) = p.y;

  return trans;
}

fmat33 scale_matrix(point p) {
  fmat33 trans;
  trans.eye();
  trans.at(0, 0) = p.x;
  trans.at(1, 1) = p.y;

  return trans;
}

fmat33 to_local_space_matrix(point center) {
  center.x = -center.x;
  center.y = -center.y;
  return translation_matrix(center);
}

fmat33 to_global_space_matrix(point center) {
  return translation_matrix(center);
}

void apply_matrix_transformation(point * points, int size,
    Renderer& renderer, fmat33 transformation) {

  // create point array
  for (int i = 0; i < size; i++) {
    // gets the point

    point& p = points[i];
    //printf("Added %d,%d", m_vertices[i].position().x, m_vertices[i].position().y);
    // transforms the point
    points[i] = renderer.transform_point(p, transformation);
    //printf("Transformed (%f,%f) \n", parr[i].x, parr[i].y);
  }

}

}


/**
 * @file kruskal.cpp
 * @author prakash sellathurai
 * @brief minimum spanning tree using Kruskal algorithm
 * @version 0.1
 * @date 2021-07-16
 *
 * @copyright Copyright (c) 2021
 *
 */

#include <algorithm>
#include <boost/pending/disjoint_sets.hpp>
#include <iostream>
#include <vector>

using namespace std;

/**
 * @brief Edge pairs are stored in a vector
 *
 */
class EdgePair {
public:
  int x, y, weight;
  EdgePair() {}
  EdgePair(int x, int y, int weight) : x(x), y(y), weight(weight) {}
};

/**
 * @brief Weighted Graph
 *
 */
class Graph {
private:
  int V;                                // No. of vertices
  int E;                                // No. of edges
  vector<vector<pair<int, int>>> edges; // Graph represented as adjacency list

public:
  Graph(int V) : V(V+1), E(0) { edges.resize(V+1); }
  void addEdge(int v, int w, int weight) {
    edges[v].push_back(make_pair(w, weight));
    edges[w].push_back(make_pair(v, weight));
    E++;
  }

  static bool sortbysec(const EdgePair &e1, const EdgePair &e2) {
    return e1.weight < e2.weight;
  }
  vector<EdgePair> to_edgearray() {
    vector<EdgePair> res;
    for (int i = 0; i < V; i++) {
      for (auto edge : edges[i]) {
        res.push_back(EdgePair(i, edge.first, edge.second));
      }
    }
    return res;
  }

  /**
   * @brief kruskal  algorithm, for finding minimum spanning tree weight
   *
   * @return int
   */
  int kruskal() {
    int weight = 0; /*cost of minimum spanning tree*/
    vector<EdgePair> e = to_edgearray();
    sort(e.begin(), e.end(), sortbysec);
    int vectorlist[V + 1];
    int parentlist[V + 1];
    boost::disjoint_sets<int *, int *> ds(vectorlist, parentlist);
    for (int i = 0; i < V; i++) {
      ds.make_set(i);
    }

    std::cout << std::endl;
    for (auto edge:e) {
      auto u = ds.find_set(edge.x);
      auto v = ds.find_set(edge.y);
      if (u != v) {
        std::cout << "Edge : " << edge.x << " " << edge.y << std::endl;
        weight += edge.weight;
        ds.link(edge.x, edge.y);
      }
    }
    return weight;
  }
};

int main(int argc, const char **argv) {
  Graph g(5);
  g.addEdge(0, 1, 1);
  g.addEdge(1, 2, 2);
  g.addEdge(2, 3, 3);
  std::cout << "Minimum spanning tree weight is : " << g.kruskal() << std::endl;
  return 0;
}


//
// Created by Hamza El-Kebir on 4/17/21.
//

#ifndef LODESTAR_BILINEARTRANSFORMATION_HPP
#define LODESTAR_BILINEARTRANSFORMATION_HPP

#include <Eigen/Dense>
#include "Lodestar/systems/StateSpace.hpp"
#include "Lodestar/aux/CompileTimeQualifiers.hpp"

namespace ls {
    namespace analysis {
        /**
         * @brief Routines for converting a state space system from continuous-
         * to discrete-time and vice versa.
         *
         * @note
         * Corresponds to SLICOT Routine AB04MD
         * (Discrete-time <-> continuous-time conversion by bilinear transformation).
         */
        class BilinearTransformation {
        public:
            template<typename TScalar = double, int TStateDim = Eigen::Dynamic, int TInputDim = Eigen::Dynamic, int TOutputDim = Eigen::Dynamic>
            struct mallocStructC2D {
                Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>> HH;
                Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>> HH2;
                Eigen::Matrix<TScalar, TStateDim, TStateDim> I;
            };

            template<typename TScalar = double, int TStateDim = Eigen::Dynamic, int TInputDim = Eigen::Dynamic, int TOutputDim = Eigen::Dynamic>
            struct mallocStructD2C {
                Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>> HH;
                Eigen::Matrix<TScalar, TStateDim, TStateDim> IMAC;
                Eigen::Matrix<TScalar, TStateDim, TStateDim> I;
            };

            /**
             * @brief Generates generalized bilinear transform of a
             * continuous-time state space system.
             *
             * @param A TState matrix.
             * @param B Input matrix.
             * @param C Output matrix.
             * @param D Feedforward matrix.
             * @param dt Sampling period.
             * @param alpha Generalized bilinear transformation parameter; default
             * parameter corresponds to backward differencing transform.
             *
             * @return Transformed discrete-time state space system.
             */
            static systems::StateSpace<>
            c2d(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
                const Eigen::MatrixXd &C, const Eigen::MatrixXd &D,
                double dt,
                double alpha = 1);

            /**
             * @brief Generates generalized bilinear transform of a
             * continuous-time state space system.
             *
             * @param A Pointer to state matrix.
             * @param B Pointer to input matrix.
             * @param C Pointer to output matrix.
             * @param D Pointer to feedforward matrix.
             * @param dt Sampling period.
             * @param alpha Generalized bilinear transformation parameter; default
             * parameter corresponds to backward differencing transform.
             *
             * @return Transformed discrete-time state space system.
             */
            static systems::StateSpace<>
            c2d(const Eigen::MatrixXd *A, const Eigen::MatrixXd *B, const Eigen::MatrixXd *C, const Eigen::MatrixXd *D,
                double dt,
                double alpha);

            /**
             * @brief Generates generalized bilinear transform of a
             * continuous-time state space system.
             *
             * @param ss TState space system.
             * @param dt Sampling period.
             * @param alpha Generalized bilinear transformation parameter; default
             * parameter corresponds to backward differencing transform.
             *
             * @return Transformed discrete-time state space system.
             */
            static systems::StateSpace<>
            c2d(const systems::StateSpace<> &ss, double dt, double alpha = 1);

            template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
            static void
            c2d(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt, double alpha,
                systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                mallocStructC2D<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                LS_IS_DYNAMIC_DEFAULT(TStateDim, TInputDim, TOutputDim));

            template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
            static void
            c2d(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt, double alpha,
                systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                mallocStructC2D<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                LS_IS_STATIC_DEFAULT(TStateDim, TInputDim, TOutputDim));

            /**
             * @brief Generates generalized bilinear transform of a
             * discrete-time state space system.
             *
             * @param ss TState space system.
             * @param dt Sampling period.
             * @param alpha Generalized bilinear transformation parameter; default
             * parameter corresponds to backward differencing transform.
             *
             * @return Transformed continuous-time state space system.
             */
            static systems::StateSpace<>
            d2c(const systems::StateSpace<> &ss, double dt,
                double alpha = 1);

            template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
            static void
            d2c(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt, double alpha,
                systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                mallocStructD2C<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                LS_IS_DYNAMIC_DEFAULT(TStateDim, TInputDim, TOutputDim));

            template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
            static void
            d2c(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt, double alpha,
                systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                mallocStructD2C<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                LS_IS_STATIC_DEFAULT(TStateDim, TInputDim, TOutputDim));

            /**
             * @brief Generates generalized bilinear transform of a
             * discrete-time state space system.
             *
             * This method retrieves the sampling period from the state space
             * object.
             *
             * @param ss TState space system.
             * @param alpha Generalized bilinear transformation parameter; default
             * parameter corresponds to backward differencing transform.
             *
             * @return Transformed continuous-time state space system.
             */
            static systems::StateSpace<>
            d2c(const systems::StateSpace<> &ss, double alpha = 1);

            /**
             * @brief Generates generalized bilinear transform of a
             * discrete-time state space system.
             *
             * @param A TState matrix.
             * @param B Input matrix.
             * @param C Output matrix.
             * @param D Feedforward matrix.
             * @param dt Sampling period.
             * @param alpha Generalized bilinear transformation parameter; default
             * parameter corresponds to backward differencing transform.
             *
             * @return Transformed continuous-time state space system.
             */
            static systems::StateSpace<>
            d2c(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
                const Eigen::MatrixXd &C, const Eigen::MatrixXd &D, double dt,
                double alpha = 1);

            /**
             * @brief Generates generalized bilinear transform of a
             * discrete-time state space system.
             *
             * @param A Pointer to state matrix.
             * @param B Pointer to input matrix.
             * @param C Pointer to output matrix.
             * @param D Pointer to feedforward matrix.
             * @param dt Sampling period.
             * @param alpha Generalized bilinear transformation parameter; default
             * parameter corresponds to backward differencing transform.
             *
             * @return Transformed continuous-time state space system.
             */
            static systems::StateSpace<>
            d2c(const Eigen::MatrixXd *A, const Eigen::MatrixXd *B, const Eigen::MatrixXd *C, const Eigen::MatrixXd *D,
                double dt,
                double alpha);

            /**
             * @brief Generates Tustin transform of a continuous-time state
             * space system.
             *
             * @param ss TState space system.
             * @param dt Sampling period.
             *
             * @return Transformed discrete-time state space system.
             */
            static systems::StateSpace<>
            c2dTustin(const systems::StateSpace<> &ss, double dt);

            /**
             * @brief Generates Tustin transform of a discrete-time state space
             * system.
             *
             * @param ss TState space system.
             * @param dt Sampling period.
             *
             * @return Transformed continuous-time state space system.
             */
            static systems::StateSpace<>
            d2cTustin(const systems::StateSpace<> &ss, double dt);

            /**
             * @brief Generates Euler transform of a continuous-time state space
             * system.
             *
             * @param ss TState space system.
             * @param dt Sampling period.
             *
             * @return Transformed discrete-time state space system.
             */
            static systems::StateSpace<>
            c2dEuler(const systems::StateSpace<> &ss, double dt);

            /**
             * @brief Generates Euler transform of a discrete-time state space
             * system.
             *
             * @param ss TState space system.
             * @param dt Sampling period.
             *
             * @return Transformed continuous-time state space system.
             */
            static systems::StateSpace<>
            d2cEuler(const systems::StateSpace<> &ss, double dt);

            /**
             * @brief Generates backward differencing transform of a
             * continuous-time state space system.
             *
             * @param ss TState space system.
             * @param dt Sampling period.
             *
             * @return Transformed discrete-time state space system.
             */
            static systems::StateSpace<>
            c2dBwdDiff(const systems::StateSpace<> &ss, double dt);

            /**
             * @brief Generates backward differencing transform of a
             * discrete-time state space system.
             *
             * @param ss TState space system.
             * @param dt Sampling period.
             *
             * @return Transformed continuous-time state space system.
             */
            static systems::StateSpace<>
            d2cBwdDiff(const systems::StateSpace<> &ss, double dt);
        };
    }
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void
ls::analysis::BilinearTransformation::c2d(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
                                          double dt, double alpha,
                                          ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                                          mallocStructC2D<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                                          LS_IS_DYNAMIC(TStateDim, TInputDim, TOutputDim))
{
    if (alpha < 0 || alpha > 1) alpha = 0;
    dt = abs(dt);

    memStruct->I.setIdentity(ss->stateDim(), ss->stateDim());
    memStruct->HH = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
            memStruct->I - alpha * dt * (ss->getA()));
    memStruct->HH2 = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
            (memStruct->I - alpha * dt * (ss->getA())).transpose());

    out->setA(memStruct->HH.template solve(memStruct->I - (1 - alpha) * dt * (ss->getA())));
    out->setB(memStruct->HH.template solve(dt * (ss->getB())));
    out->setC(memStruct->HH2.template solve((ss->getC()).transpose()).transpose());
    out->setD((ss->getD()) + alpha * (ss->getC()) * (out->getB()));
    out->setDiscreteParams(dt, true);
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void
ls::analysis::BilinearTransformation::c2d(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
                                          double dt, double alpha,
                                          ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                                          mallocStructC2D<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                                          LS_IS_STATIC(TStateDim, TInputDim, TOutputDim))
{
    if (alpha < 0 || alpha > 1) alpha = 0;
    dt = abs(dt);

    memStruct->I.setIdentity();
    memStruct->HH = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
            memStruct->I - alpha * dt * (ss->getA()));
    memStruct->HH2 = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
            (memStruct->I - alpha * dt * (ss->getA())).transpose());

    out->setA(memStruct->HH.template solve(memStruct->I - (1 - alpha) * dt * (ss->getA())));
    out->setB(memStruct->HH.template solve(dt * (ss->getB())));
    out->setC(memStruct->HH2.template solve((ss->getC()).transpose()).transpose());
    out->setD((ss->getD()) + alpha * (ss->getC()) * (*out->getB()));
    out->setDiscreteParams(dt, true);
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void
ls::analysis::BilinearTransformation::d2c(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
                                          double dt, double alpha,
                                          ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                                          mallocStructD2C<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                                          LS_IS_DYNAMIC(TStateDim, TInputDim, TOutputDim))
{
    if (alpha < 0 || alpha > 1) alpha = 0;
    dt = abs(dt);

    memStruct->I.setIdentity(ss->stateDim(), ss->stateDim());
    memStruct->HH = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
            alpha * dt * (ss->getA()).transpose() + (1 - alpha) * dt * memStruct->I);
    out->setA(memStruct->HH.template solve((ss->getA()).transpose() - memStruct->I));
    memStruct->IMAC = memStruct->I - alpha * dt * (out->getA());

    out->setB(memStruct->IMAC * (ss->getB()) / dt);
    out->setC((ss->getC()) * memStruct->IMAC);
    out->setD((ss->getD()) - alpha * (out->getC()) * (ss->getB()));
    out->setDiscreteParams(-1, false);
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void
ls::analysis::BilinearTransformation::d2c(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
                                          double dt, double alpha,
                                          ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                                          mallocStructD2C<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                                          LS_IS_STATIC(TStateDim, TInputDim, TOutputDim))
{
    if (alpha < 0 || alpha > 1) alpha = 0;
    dt = abs(dt);

    memStruct->I.setIdentity();
    memStruct->HH = Eigen::ColPivHouseholderQR<Eigen::Matrix<TScalar, TStateDim, TStateDim>>(
            alpha * dt * (ss->getA()).transpose() + (1 - alpha) * dt * memStruct->I);
    out->setA(memStruct->HH.template solve((ss->getA()).transpose() - memStruct->I));
    memStruct->IMAC = memStruct->I - alpha * dt * (out->getA());

    out->setB(memStruct->IMAC * (ss->getB()) / dt);
    out->setC((ss->getC()) * memStruct->IMAC);
    out->setD((ss->getD()) - alpha * (out->getC()) * (ss->getB()));
    out->setDiscreteParams(-1, false);
}

#endif //LODESTAR_BILINEARTRANSFORMATION_HPP


#include <iostream>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <fstream>
#include <vector>
#include <iomanip>
#include <math.h>

void load_csv(std::string filename, Eigen::ArrayXXd &output);

void writeToCSVfile(std::string name,Eigen::MatrixXd matrix)
{
  std::ofstream file(name.c_str());

  for(int  i = 0; i < matrix.rows(); i++){
      for(int j = 0; j < matrix.cols(); j++){
         std::string str = std::to_string(matrix(i,j));
         if(j+1 == matrix.cols()){
             file<<str;
         }else{
             file<<str<<',';
         }
      }
      file<<'\n';
  }
}

int main(int argc, char const *argv[])
{
	Eigen::ArrayXXd Input;
	Eigen::ArrayXXd Feature;

	// Choose how many samples to skip at each iteration
	int samplingTime = 3;

	// Load Data
	// Input has dimension Time X 1
	// Feature has dimension Time X N_Feature
	load_csv("Feature.csv", Feature);
	load_csv("TrainData.csv", Input);

	// Calculate effective dimension
	int dimension = ceil(Input.rows()/(double) samplingTime);

	// Create structures
	Eigen::ArrayXXd distances_feature(dimension, dimension);
	Eigen::ArrayXXd distances_input_corr(dimension,dimension);

	Eigen::ArrayXXd distances_input(dimension, dimension);
	Eigen::ArrayXXd distances_mixed(dimension, dimension);

	//Compute Kernels for 1D and 2D data
	double h_1D = 0.9 * pow((double)dimension, -1.0/5.0);
	h_1D = pow(h_1D,2);

	double h_2D =  pow((double)dimension, -1.0/6.0);
	h_2D = pow(h_2D,2);

	// Structures for variance
    double var_correction = dimension/(double)(dimension-1);
	double var_input;
	double var_feature;
	double extra_diagonal;
	Eigen::MatrixXd CovMatrix(2,2);

    var_input= var_correction * (Input.pow(2).mean()-pow(Input.mean(),2));

    // Compute distance matrix for input
	int i_aux = 0, j_aux;
	for (int i = 0; i < Input.rows(); i = i+samplingTime)
	{
		j_aux = i_aux;
		for (int j = i; j < Input.rows(); j = j+samplingTime)
		{
			distances_input(i_aux,j_aux) = Input(i) - Input(j);
			distances_input(j_aux,i_aux) = distances_input(i_aux,j_aux);
			j_aux++;
		}
		i_aux++;
	}

	// Compute Kernel Distances
	distances_input_corr = -1.0 * distances_input.pow(2) / (2*var_input * h_1D);
	distances_input_corr = distances_input_corr.exp() / std::sqrt(2*M_PI*var_input * h_1D);
	// Compute input pdf
	Eigen::ArrayXd C_Input = distances_input_corr.rowwise().mean();

	// Analyze Feature Matrix
	for (int z = 0; z < Feature.cols(); ++z)
	{
		// Isolate signal from one feature
		Eigen::ArrayXd signal = Feature.col(z);

		// Compute variance
		var_feature = var_correction * (signal.pow(2).mean()-pow(signal.mean(),2));
		//var_feature = var_feature;

		// extra_diagonal =( ( signal - signal.mean()) * (Input - Input.mean()) ).mean() / (double) (dimension-1);
		// Compute Covariance Matrix. Kernel width is 0 for extra-diagonal term
		CovMatrix(0,0) = var_input * h_2D;
		CovMatrix(0,1) = 0;
		CovMatrix(1,0) = 0;
		CovMatrix(1,1) = var_feature * h_2D;

		double determinant = std::sqrt(CovMatrix.determinant());

		// Compute inverse of diagonal matrix
		CovMatrix(0,0) = 1/(var_input * h_2D);
		CovMatrix(1,1) = 1/(var_feature * h_2D);

		Eigen::MatrixXd Support(2,1);

		int i_aux = 0, j_aux;
		for (int i = 0; i < Input.rows(); i = i+samplingTime)
		{
			j_aux = i_aux;
			for (int j = i; j < Input.rows(); j = j+samplingTime)
			{
				// Compute distance feature
				distances_feature(i_aux,j_aux) = signal(i) - signal(j);
				distances_feature(j_aux, i_aux) = distances_feature(i_aux,j_aux);

				Support(0,0) = distances_input(i_aux,j_aux);
				Support(1,0) = distances_feature(i_aux, j_aux);
				double result = (- ( (Support.transpose() * CovMatrix) * Support))(0,0);
				distances_mixed(i_aux,j_aux) = exp( result/(2) );
				distances_mixed(j_aux, i_aux) = distances_mixed(i_aux, j_aux);

				j_aux++;
			}
		i_aux++;
		}
	// Compute Kernel Feature Matrix and Kernel Joint Distance Matrix
	distances_mixed = distances_mixed / (2 * M_PI * determinant);
	distances_feature = -1.0 * distances_feature.pow(2) / (2*var_feature* h_1D);
	distances_feature = distances_feature.exp()/std::sqrt(2*M_PI * var_feature * h_1D);

	// Compute Feature pdf
	Eigen::ArrayXd C_Feature = distances_feature.rowwise().mean();

	// Compute Mutual Information
	Eigen::ArrayXd num =  distances_mixed.rowwise().mean();
	Eigen::ArrayXd den = C_Feature * C_Input;
	num = num.cwiseQuotient(den);
	num = num.log() / log(2);
	std::cout << "Feature: " << z << std::endl;
	std::cout <<"MI: " << num.mean() << std::endl;

	// Single Entropies
	auto feature_pdf = distances_feature.rowwise().mean().log() / log(2);
	auto input_pdf = distances_input_corr.rowwise().mean().log()/log(2);
	auto joint_pdf = distances_mixed.rowwise().mean().log()/log(2);

	std::cout << "Entropy Feature H(Y): " << -feature_pdf.mean() << std::endl;
	std::cout << "Entropy Input H(X): " << -input_pdf.mean() << std::endl;
	std::cout << "Entropy Joint H(X|Y): " << -joint_pdf.mean() << std::endl;

	}
	return 0;
}

void load_csv(std::string filename, Eigen::ArrayXXd &output)
{
    std::vector<double> vec;
    std::string buffer;
    char *tokens;
    int i, j;
    int cols = 0, line = 0;

    std::ifstream input_stream(filename.c_str());

    if (input_stream.is_open())
    {
        // // Read header
        // if (!input_stream.eof())
        // {
        //     getline(input_stream, buffer, '\n');
        //     cols = std::count(buffer.begin(), buffer.end(), ',') + 1;
        // }

        // Read data
        while (!input_stream.eof())
        {

            getline(input_stream, buffer, '\n');
            if(line == 0)
	        	cols = std::count(buffer.begin(), buffer.end(), ',') + 1;

            tokens = strtok(strdup(buffer.c_str()), ",");
            for (i = 0; (i < cols) && (tokens != NULL); i++)
            {
                vec.push_back(atof(tokens));
                tokens = strtok(NULL, ",");
            }
            line++;
        }
        // Close file
        input_stream.close();

        // Place data in matrix
        if (line > 1)
        {
            output.conservativeResize(line - 1, cols);
            for (i = 0; i < line - 1; i++)
            {
                for (j = 0; j < cols; j++)
                {
                    output(i, j) = vec[i * cols + j];
                }
            }
        }
    }
    else
    {
        std::cerr << "File " << filename << " not found" << std::endl;
        throw;
    }
}


#include <Eigen/Dense>
#include <algorithm>
#include <cfloat>
#include <random>
#include <iostream>
#include <simpleClusterization_common.hpp>
#include <simpleClusterization.hpp>

using namespace Eigen;

float euclideanNorm(
        const VectorXf &v1,
        const VectorXf &v2
    ){
    return (v1 - v2).squaredNorm();
}

void calculateFuzzyWeights(
        const Ref<const MatrixXf>   &entities,
        const Ref<const MatrixXf>   &centroids,
        Ref<MatrixXfR>              weights,
        squaredNorm_t               *norm
        ){
    const int entitiesNumber  = entities.rows();
    const int centroidsNumber = centroids.rows();
    for(int i=0;i<centroidsNumber;++i){
        for(int j=0;j<entitiesNumber;++j){
            weights(i, j) = (*norm)(centroids.row(i), entities.row(j));
            assert(weights(i, j)> FCM_THRESHOLD && "A centroid and an entity coincide, this leads to infinite weights, correct\n");
        }
    }
    weights.array() = 1.0 / weights.array();
}

void FCMGenerator(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        Ref<MatrixXfR>              weights,
        squaredNorm_t               *norm
    ){
    //The loop is as following:
    //1 - We initialize weights to random values from 0 to 1 (to check if they need to sum up to 1)
    //2 - We calculate the Centroids of each cluster (what is gonna end up in entities as a Statistical Entity according to the following formula: c_j = (Sum_i w_ij^m * x_i)/(Sum_i w_ij^m)
    //3 - We update weights according to this formula:  w_ij = 1 / (Sum_k (distance(x_i, c_j)/distance(x_i, c_k)) ^ (2 / m-1)) where m is a fuzziness parameter that's usually put equal to 2
    // Loop until Norm(W_i+1 - W_i) < Epsilon where Epsilon is a threshold decided by the coder
    assert(weights.cols()==entities.rows() && centroids.rows()==weights.rows() && centroids.cols()==entities.cols() && "Matrix sizes for FCMGenerator not compatibles\n");
    const int centroidsNumber = centroids.rows();
    MatrixXfR weightsOld(weights.rows(), weights.cols());
    MatrixXfR weights2(weights.rows(), weights.cols());
    MatrixXfR weightsOld2(weights.rows(), weights.cols());
    //Initialization of the weights at random values, might be improved if we could get a reasonable initial guess
    weightsOld = MatrixXf::Zero(weights.rows(), weights.cols());
    weights = MatrixXf::Random(weights.rows(), weights.cols());
    for(int loopIndex=0;loopIndex<FCM_MAX_ITERATIONS && (weights - weightsOld).squaredNorm() > FCM_THRESHOLD;++loopIndex){
        weightsOld = weights;
        weights2 = weights.array().square();
        //Calculation of the Centroids
        centroids = weights2 * entities;
        for(int i=0;i<centroidsNumber;++i){
            float multiplier  = 1.0 / weights2.row(i).sum();
            centroids.row(i) *= multiplier;
        }
        //Update of the weights with the new Centroids
        calculateFuzzyWeights(entities, centroids, weights, norm);
        weights.rowwise().normalize();
    }
}

float daviesBouldinIndex(
        const Ref<const MatrixXf>    &entities,
        const Ref<const MatrixXf>    &centroids,
        const Ref<const MatrixXb>    &weights,
        squaredNorm_t                *norm
    ){
    const int clustersNumber = centroids.rows();
    const int startingEntitiesNumber = entities.rows();
    VectorXf scatterVector = VectorXf::Zero(clustersNumber);
    for(int i=0;i<clustersNumber;++i){
        for(int j=0;j<startingEntitiesNumber;++j){
            if(weights(i, j)){
                scatterVector(i)+=(*norm)(centroids.row(i), entities.row(j));
            }
        }
        float multiplier  = 1.0 / (float) weights.row(i).count();
        scatterVector(i) *= multiplier;
    }
    scatterVector = scatterVector.cwiseSqrt();
    MatrixXf clusterSeparationMatrix(clustersNumber, clustersNumber);
    for(int i=0;i<clustersNumber;++i){
        for(int j=0;j<clustersNumber;++j){
            clusterSeparationMatrix(i, j) = (*norm)(centroids.row(i), centroids.row(j));
        }
    }
    clusterSeparationMatrix = clusterSeparationMatrix.cwiseSqrt();
    float dbIndex = 0;
    for(int i=0;i<clustersNumber;++i){
        float dbIndexCluster = 0;
        for(int j=0;j<clustersNumber;++j){
            if(i!=j){
                dbIndexCluster = std::max((scatterVector(i) + scatterVector(j))/clusterSeparationMatrix(i, j), dbIndexCluster);
            }
        }
        dbIndex+=dbIndexCluster;
    }
    return dbIndex/float(clustersNumber);
}

float silhouetteTest(
        const Ref<const MatrixXf>   &entities,
        const Ref<const MatrixXf>   &clusters,
        const Ref<const MatrixXfR>  &weights
        ){
    //TODO
    return 1.;
}

/*!
 * @brief      Takes data points and a k-long cluster matrix, generates initial values for k centroids.
 * @note       entities is copied and not referenced, as it needs to be altered for simplicity's sake
 * @param[in]  entities    The datapoints
 * @param[in-out] centroids   The centroids of the clusters generated by this function
 * @param[in]  norm        A pointer to the norm function you want to use
*/
void kmeansInitializer(
        const Ref<const MatrixXf>    &entities,
        Ref<MatrixXf>                centroids,
        squaredNorm_t                *norm
    ){
    const int statsNumber = entities.cols();
    const int startingEntitiesNumber = entities.rows();
    const int clustersNumber = centroids.rows();
    int currentClustersNumber = 0;
    MatrixXfR mEntities = entities;
    VectorXf squaredDistances(startingEntitiesNumber);
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<> intDistribution(0, startingEntitiesNumber - 1);
//   1. Choose one center uniformly at random among the data points.
    int randomIndex = intDistribution(gen);
    while(true){
        centroids.row(currentClustersNumber) = mEntities.row(randomIndex);
        mEntities.row(randomIndex) = mEntities.row(startingEntitiesNumber - currentClustersNumber - 1);
        mEntities.row(startingEntitiesNumber - currentClustersNumber - 1) = VectorXf::Zero(statsNumber);
        ++currentClustersNumber;
        if(currentClustersNumber==clustersNumber){
            break;
        }
        //   2. For each data point x not chosen yet, compute D(x), the distance between x and the nearest center that has already been chosen.
        for(int j=0;j<startingEntitiesNumber - currentClustersNumber;++j){
            squaredDistances(j) = (*norm)(centroids.row(0), mEntities.row(j));
            for(int i=1;i<currentClustersNumber;++i){
                squaredDistances(j) = std::min((*norm)(centroids.row(i), mEntities.row(j)), squaredDistances(j));
            }
        }
        //   3. Choose one new data point at random as a new center, using a weighted probability distribution where a point x is chosen with probability proportional to D(x)2.
        std::partial_sum(squaredDistances.data(), squaredDistances.data() + startingEntitiesNumber - currentClustersNumber - 1, squaredDistances.data());
        std::uniform_real_distribution<> floatDistribution(squaredDistances(0), squaredDistances(startingEntitiesNumber - currentClustersNumber - 1));
        float randomFloat = floatDistribution(gen);
        randomIndex = std::upper_bound(squaredDistances.data(), squaredDistances.data() + startingEntitiesNumber - currentClustersNumber - 1, randomFloat) - squaredDistances.data();
        //   4. Repeat Steps 2 and 3 until k centers have been chosen.
    }
//   5. Now that the initial centers have been chosen, proceed using standard k-means clustering.
}

void calculateBooleanWeights(
        const Ref<const MatrixXf>   &entities,
        const Ref<const MatrixXf>   &centroids,
        Ref<MatrixXbR>              weights,
        squaredNorm_t               *norm
    ){
    const int entitiesNumber = entities.rows();
    const int clustersNumber = centroids.rows();
    weights.setConstant(false);
    for(int j=0;j<entitiesNumber;j++){
        float minDistance = (*norm)(centroids.row(0), entities.row(j));
        int minIndex = 0;
        for(int i=1;i<clustersNumber;i++){
            float currentDistance = (*norm)(centroids.row(i), entities.row(j));
            if(currentDistance < minDistance){
                minDistance = currentDistance;
                minIndex = i;
            }
        }
        weights(minIndex, j) = true;
    }
}

void kmeansGenerator(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        Ref<MatrixXbR>              weights,
        squaredNorm_t               *norm
    ){
    assert(entities.cols()==centroids.cols() && "Called cmeansGenerator with entities and centroids having different dimensions\n");
    const int entitiesNumber = entities.rows();
    const int clustersNumber = centroids.rows();
    //We initialize the centroids with some datapoints that are spread out across the dataset, according to the kmeans++ algorithm
    kmeansInitializer(entities, centroids, norm);
    calculateBooleanWeights(entities, centroids, weights, norm);
    MatrixXb oldWeights(clustersNumber, entitiesNumber);
    oldWeights.setConstant(false);
    while(oldWeights!=weights){
        oldWeights = weights;
        centroids = (weights.cast<float>()) * entities;
        for(int i=0;i<clustersNumber;++i){
            float multiplier  = 1.0 / (float) weights.row(i).count();
            centroids.row(i) *= multiplier;
        }
        calculateBooleanWeights(entities, centroids, weights, norm);
    }
}

int clusterGeneratorApproximate(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        Ref<MatrixXfR>              weights,
        Ref<MatrixXbR>              boolWeights,
        squaredNorm_t               *norm
    ){
    assert(centroids.cols()==entities.cols() && "clusterGeneratorApproximate: called with entities and centroids having different sizes");
    const int statsNumber = entities.cols();
    const int entitiesNumber = entities.rows();
    const int maxClustersNumber = centroids.rows();
    float fitnessCandidate = 50.;
    float newFitness = 50.;
    MatrixXf currentClustersCandidate(maxClustersNumber, statsNumber);
    MatrixXbR currentBoolWeightsCandidate(maxClustersNumber, entitiesNumber);
    int clustersNumber = 2;
    //The minimum amount of clusters is 2 because otherwise the Davies-Bouldin index fails
    for(int currentClustersNumber = 2; currentClustersNumber<=maxClustersNumber; ++currentClustersNumber){
        for(int i=0;i<attemptsPerClustersNumber;++i){
            int iterations = 0;
            do{
                currentClustersCandidate.topLeftCorner(currentClustersNumber, statsNumber).setZero();
                currentBoolWeightsCandidate.topLeftCorner(currentClustersNumber, entitiesNumber).setZero();
                kmeansGenerator(entities, currentClustersCandidate.topLeftCorner(currentClustersNumber, statsNumber), currentBoolWeightsCandidate.topLeftCorner(currentClustersNumber, entitiesNumber), norm);
                newFitness = daviesBouldinIndex(entities, currentClustersCandidate.topLeftCorner(currentClustersNumber, statsNumber), currentBoolWeightsCandidate.topLeftCorner(currentClustersNumber, entitiesNumber),  norm);
                ++iterations;
            }while(std::isnan(newFitness) && iterations < maxIterationPerClustersNumber);
            if (newFitness < fitnessCandidate){
                centroids.topLeftCorner(currentClustersNumber, statsNumber)            = currentClustersCandidate.topLeftCorner(currentClustersNumber, statsNumber);
                boolWeights.topLeftCorner(currentClustersNumber, entitiesNumber)       = currentBoolWeightsCandidate.topLeftCorner(currentClustersNumber, entitiesNumber);
                fitnessCandidate                                                = newFitness;
                clustersNumber                                                 = currentClustersNumber;
            }
        }
    }
    //Single-datapoint clusters lead to infinite fuzzy weights, so we offset them by a small vector.
    //The risk in doing this is that we might end up moving the centroid too much, so that its datapoint ends up in another cluster.
    //So to avoid this, we scale our offsetConstant by the dataset's dimensionality.
    //Additionally, we choose as a direction the one defined by the current vector and the average one the result is that we're pushing the centroid towards the center of the whole dataset,
    //which makes it slightly harder for the worst case scenario to happen.
    const float shiftMultiplier = offsetConstant / float(statsNumber);
    const RowVectorXf averageEntity = entities.colwise().mean();
    for(int i=0;i<clustersNumber;++i){
        if(boolWeights.row(i).count()==1){
            centroids.row(i) += shiftMultiplier * (centroids.row(i) - averageEntity);
        }
    }
    calculateFuzzyWeights(entities, centroids.topRightCorner(clustersNumber, statsNumber), weights.topLeftCorner(clustersNumber, entitiesNumber), norm);
    return clustersNumber;
}

int clusterGeneratorExact(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        MatrixXfR                   &weights,
        squaredNorm_t               *norm
    ){
    assert(weights.rows()==weights.cols() && "Called clusterGenerator with a non-square weights matrix\n");
    const int statsNumber = entities.cols();
    const int entitiesNumber = entities.rows();
    const int maxClustersNumber = centroids.rows();
    float fitnessCandidate = 0;
    float newFitness = 0;
    int centroidsNumber = 2;
    MatrixXf currentClustersCandidate(maxClustersNumber, statsNumber);
    //Initialized to catch the improbable case of silhouetteTest() always returning 0
    MatrixXfR currentWeightsCandidate, weightsCandidate;
    //Initialized to catch the improbable case of silhouetteTest() always returning 0
    for(int clustersNumber = 2; clustersNumber<=maxClustersNumber; ++clustersNumber){
        currentClustersCandidate.topLeftCorner(clustersNumber, statsNumber).setZero();
        currentWeightsCandidate.topLeftCorner(clustersNumber, entitiesNumber).setZero();
        FCMGenerator(entities, currentClustersCandidate, currentWeightsCandidate, norm);
        newFitness = silhouetteTest(entities, currentClustersCandidate, currentWeightsCandidate);
        if (newFitness > fitnessCandidate){
            centroids.topLeftCorner(clustersNumber, statsNumber) = currentClustersCandidate.topLeftCorner(clustersNumber, statsNumber);
            weights.topLeftCorner(clustersNumber, entitiesNumber) = currentWeightsCandidate.topLeftCorner(clustersNumber, entitiesNumber);
            fitnessCandidate = newFitness;
            centroidsNumber = clustersNumber;
        }
    }
    return centroidsNumber;
}


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)
/*
    boost::math::fft example 06

    Fast Polynomial Multiplication
*/

#include <boost/math/fft/bsl_backend.hpp>
#include <iostream>
#include <vector>
#include <exception>
#include <boost/core/demangle.hpp>

template<class T>
void print(const std::vector<T>& V)
{
  std::cout << "size(V) = " << V.size() << "\n";
  std::cout << "[";
  for(auto x: V)
  {
    std::cout << x << ", ";
  }
  std::cout << "]\n";
}

template<class T>
std::vector<T> multiply_halfcomplex(const std::vector<T>& A, const std::vector<T>& B)
// I strongly discourage to use this function that depends on the halfcomplex representation
// TODO: create a class to "view" the contents of a halfcomplex array
{
  std::cout << "Polynomial multiplication using halfcomplex with: "<< boost::core::demangle(typeid(T).name()) <<"\n";
  const std::size_t N = A.size();
  std::vector<T> TA(N),TB(N);
  boost::math::fft::bsl_rdft<T> P(N);
  P.real_to_halfcomplex(A.begin(),A.end(),TA.begin());
  P.real_to_halfcomplex(B.begin(),B.end(),TB.begin());

  std::vector<T> C(N);

  C[0]=TA[0]*TB[0];
  for(unsigned int i=1;i+1<N;i+=2)
  {
    C[i] = TA[i]*TB[i]-TA[i+1]*TB[i+1];
    C[i+1] = TA[i]*TB[i+1] + TA[i+1]*TB[i];
  }
  if(N%2==0)
  {
    C.back() = TA.back()*TB.back();
  }

  P.halfcomplex_to_real(C.begin(),C.end(),C.begin());
  std::transform(C.begin(), C.end(), C.begin(),
                 [N](T x) { return x / N; });

  print(C);
  return C;
}
template<class T>
std::vector<T> multiply_complex(const std::vector<T>& A, const std::vector<T>& B)
{
  std::cout << "Polynomial multiplication using complex: "<< boost::core::demangle(typeid(T).name()) <<"\n";
  const std::size_t N = A.size();
  std::vector< boost::multiprecision::complex<T> > TA(N),TB(N);
  boost::math::fft::bsl_rdft<T> P(N);
  P.real_to_complex(A.begin(),A.end(),TA.begin());
  P.real_to_complex(B.begin(),B.end(),TB.begin());

  std::vector<T> C(N);

  for(unsigned int i=0;i<N;++i)
  {
    TA[i]*=TB[i];
  }

  P.complex_to_real(TA.begin(),TA.end(),C.begin());
  std::transform(C.begin(), C.end(), C.begin(),
                 [N](T x) { return x / N; });

  print(C);
  return C;
}

template<class T>
T difference(const std::vector<T>& A, const std::vector<T>& B)
{
  using std::abs;
  T diff{};
  if(A.size()!=B.size()) return -1;
  for(unsigned int i=0;i<A.size();++i)
  {
    diff += abs(A[i]-B[i]);
  }
  return diff;
}

template<typename Real>
void multiply() {
  using std::abs;
  std::vector<Real> A{1.,4.,-5.,1.,0.,0.,0.,0.};
  std::vector<Real> B{-1.,1.,2.,3.,0.,0.,0.,0.};
  std::vector<Real> C{-1,-3,11,5,3,-13,3,0};

  std::vector<Real> result;
  Real diff;

  // result = multiply_halfcomplex(A,B);
  // diff = difference(result,C);
  // if(abs(diff)>1e-6)
  //   throw std::runtime_error("wrong result");
  result = multiply_complex(A,B);
  diff = difference(result,C);
  if(abs(diff)>1e-3)
    throw std::runtime_error("wrong result");
}

int main()
{
  multiply<float>();
  multiply<double>();
  multiply<long double>();
  return 0;
}


#ifndef EXPSUM_KERNEL_FUNCTIONS_POW_KERNEL_HPP
#define EXPSUM_KERNEL_FUNCTIONS_POW_KERNEL_HPP

#include <cassert>
#include <sstream>
#include <stdexcept>
#include <tuple>

#include <armadillo>

#include "expsum/constants.hpp"
#include "expsum/exponential_sum.hpp"
#include "expsum/kernel_functions/gamma.hpp"
#include "expsum/kernel_functions/gauss_quadrature.hpp"

namespace expsum
{

//
// Approximate power function by an exponential sum.
//
// For given accuracy ``$\epsilon > 0$`` and distance to the singularity
// ``$\delta > 0$``, this find the approximation of power function
// ``$f(r)=r^{-\beta}$`` with a linear combination of exponential functions
// such that
//
// ``` math
// \left| r^{-\beta}-\sum_{m=1}^{M}w_{m}e^{-a_{m}r} \right|
//    \leq r^{-\beta}\epsilon
// ```
//
// for ``$r\in [\delta,1]$.
//
// @beta   power factor ``$beta > 0$``
// @delta  distance to the singularity ``$0 < \delta < 1$``
// @eps    required accuracy ``$0 < \epsilon < e^{-1}$``
// @return pair of vectors holding expnents ``$a_{m}$`` and weights ``$w_{m}$``
//

template <typename T>
struct pow_kernel
{
public:
    using size_type   = arma::uword;
    using real_type   = T;
    using vector_type = arma::Col<T>;
    using matrix_type = arma::Mat<T>;

private:
    vector_type exponent_;
    vector_type weight_;
    real_type beta_;
    real_type delta_;
    real_type eps_;

public:
    void compute(real_type beta__, real_type delta__, real_type eps__);

    size_type size() const
    {
        return exponent_.size();
    }

    const vector_type& exponents() const
    {
        return exponent_;
    }

    const vector_type& weights() const
    {
        return weight_;
    }

    real_type beta() const
    {
        return beta_;
    }

    real_type delta() const
    {
        return delta_;
    }

    real_type eps() const
    {
        return eps_;
    }

private:
    static void check_params(real_type beta__, real_type delta__,
                             real_type eps__);
    static size_type get_num_intervals(real_type beta__, real_type delta__,
                                       real_type eps__);
    static std::tuple<size_type, real_type>
    get_num_gauss_jacobi(real_type beta__, real_type eps__);
    static std::tuple<size_type, real_type>
    get_num_gauss_legendre(real_type beta__, real_type eps__);
};

template <typename T>
void pow_kernel<T>::compute(real_type beta__, real_type delta__,
                            real_type eps__)
{
    check_params(beta__, delta__, eps__);

    beta_  = beta__;
    delta_ = delta__;
    eps_   = eps__;

    //
    // Find the sub-optimal sum-of-exponential approximation by discretizing the
    // integral representation of power function
    //
    // ``` math
    //  r^{-\beta} = \frac{1}{\Gamma(\beta)}
    //               \int_{0}^{\infty} e^{-rx} x^{\beta-1} dx \quad (t > 0).
    // ```
    //
    // Split the integral into $J+1$ sub-intervals as
    //
    // ``` math
    //  r^{-\beta}
    //     = \frac{1}{\Gamma(\beta)}
    //       \left( \int_{0}^{2} + \int_{4}^{2} + \int_{8}^{4} + \cdots
    //             + \int_{2^{J-1}}^{2^J} + \int_{2^J}^{\infty} \right)
    //       e^{-ry} x^{\beta-1} dy.
    // ```
    //
    // The integral over the first interval $[0,2]$ is approximated by the $N_1$
    // point Gauss-Jacobi rule, while the integral over the inteval
    // $[2^{j-1},2^j]$ is approximated by the $N_2$ point Gauss-Legendre rule.
    //
    // The number of intervals $J$ and quadrature points $N_1,N_2$ are
    // determined so as to the relative error of the truncation becomes smaller
    // than $\epsilon$.
    //
    size_type J, N1, N2;
    T b1, b2;
    J = get_num_intervals(beta__, delta__ / 2, eps__);
    std::tie(N1, b1) = get_num_gauss_jacobi(beta__, eps__ / (J + 1));
    std::tie(N2, b2) = get_num_gauss_legendre(beta__, eps__ / (J + 1));
    //
    // Discritization of the integral representation by applying the quadrature
    // rule to each interval
    //
    const size_type N = N1 + (J - 1) * N2;
    gauss_jacobi_rule<T> gaujac(N1, T(), beta__ - 1);
    gauss_legendre_rule<T> gauleg(N2);

    assert(gaujac.size() == N1);
    assert(gauleg.size() == N2);

    vector_type a(N);
    vector_type w(N);

    auto ait = std::begin(a);
    auto wit = std::begin(w);

    const auto scale = T(1) / std::tgamma(beta__);

    for (size_type i = 0; i < gaujac.size(); ++i)
    {
        *ait = gaujac.x(i) + T(1);
        *wit = scale * gaujac.w(i);
        ++ait;
        ++wit;
    }

    // The upper/lower bound of interval
    auto lower = T(1);
    auto upper = T(2);
    for (size_type k = 1; k < J; ++k)
    {
        lower *= 2; // lower = 2^(k-1)
        upper *= 2; // upper = 2^k
        const auto a1 = (upper - lower) / 2;
        const auto b1 = (upper + lower) / 2;
        for (size_type i = 0; i < gauleg.size(); ++i)
        {
            const auto y = a1 * gauleg.x(i) + b1;
            *ait         = y;
            *wit         = a1 * scale * std::pow(y, beta__ - 1) * gauleg.w(i);
            ++ait;
            ++wit;
        }
    }

    std::swap(exponent_, a);
    std::swap(weight_, w);
    return;
}

//------------------------------------------------------------------------------
// Private member functions
//------------------------------------------------------------------------------
template <typename T>
void pow_kernel<T>::check_params(real_type beta__, real_type delta__,
                                 real_type eps__)
{
    if (!(beta__ > real_type()))
    {
        std::ostringstream msg;
        msg << "Invalid value for the argument `beta': "
               "beta > 0 expected, but beta = "
            << beta__ << " is given";
        throw std::invalid_argument(msg.str());
    }

    if (!(real_type() < delta__ && delta__ < real_type(1)))
    {
        std::ostringstream msg;
        msg << "Invalid value for the argument `delta': "
               "0 < delta < 1 expected, but delta = "
            << delta__ << " is given";
        throw std::invalid_argument(msg.str());
    }

    if (!(real_type() < eps__ &&
          eps__ < real_type(1) / arma::Datum<real_type>::e))
    {
        std::ostringstream msg;
        msg << "Invalid value for the argument `eps': "
               "0 < eps < 1/e expected, but "
            << eps__ << " is given";
        throw std::invalid_argument(msg.str());
    }
}

template <typename T>
typename pow_kernel<T>::size_type
pow_kernel<T>::get_num_intervals(real_type beta__, real_type delta__,
                                 real_type eps__)
{
    //
    // Find minimal integer J, such that
    //
    //  Gamma(beta, delta * 2^J) / Gamma(beta) <= eps / (J + 1)
    //
    size_type J1 = 0;
    size_type J2 = 20;

    while (true)
    {
        auto x  = delta__ * std::pow(T(2), J2);
        auto fj = gamma_q(beta__, x);

        if (fj * (J2 + 1) <= eps__)
        {
            if (J2 - J1 <= 1)
            {
                break;
            }

            J2 = (J1 + J2) / 2;
        }
        else
        {
            J1 = J2;
            J2 = J2 + 5;
        }
    }

    return J2;
}

template <typename T>
std::tuple<typename pow_kernel<T>::size_type, T>
pow_kernel<T>::get_num_gauss_jacobi(real_type beta__, real_type eps__)
{
    //
    // Find N such that R(N, b) <= eps with
    //
    // R(N, x) = 2^(beta + 2) / Gamma(beta + 1) * exp(cosh(x)-1)
    //         * exp(-2*N*x) / (1 - exp(-2*x)).
    //
    // Here, the parameter b is chosen to minimize R(N, x) for each N.
    //

    // Logarithm of pre-factor of R(N, x),
    const auto ln_pre = (beta__ + 2) * std::log(T(2)) - std::lgamma(beta__ + 1);

    const size_type max_iter = 100;
    const auto ln_eps        = std::log(eps__);

    // Initial guesses of parameters
    size_type n1 = 0;
    size_type n2 = 4;
    auto x       = T(4);

    while (true)
    {
        // For given N = n2, minimize R(N, x) w.r.t. x
        for (size_type i = 0; i < max_iter; ++i)
        {
            // R'(N, x) without pre-factor
            auto df = std::sinh(x) + 2 / std::expm1(2 * x) - 2 * n2;
            // R''(N, x) without pre-factor
            auto t   = std::expm1(2 * x);
            auto ddf = std::cosh(x) - 4 * std::exp(2 * x) / (t * t);
            auto dx  = -df / ddf;

            if (x + dx < T()) // ensure x > 0
            {
                x *= T(0.5);
            }
            else
            {
                x += dx;
            }

            if (std::abs(dx) <= x * eps__)
            {
                break;
            }
        }

        // ln(R(N, b))
        auto ln_resid = (std::expm1(x) + std::expm1(-x)) / 2 // cosh(x) - 1
                        - 2 * n2 * x                         // ln(exp(-2*n*x))
                        - std::log(-std::expm1(-2 * x)); // log(1 - exp(-2*x))

        if (ln_pre + ln_resid <= ln_eps) // equiv to R(N, b) <= eps
        {
            if (n2 - n1 <= 1)
            {
                break;
            }

            n2 = (n1 + n2) / 2;
        }
        else
        {
            n1 = n2;
            n2 = n2 + 2;
        }
    }

    return {n2, x};
}

template <typename T>
std::tuple<typename pow_kernel<T>::size_type, T>
pow_kernel<T>::get_num_gauss_legendre(real_type beta__, real_type eps__)
{
    //
    // Find N such that R(N, b) <= eps with
    //
    // R(N, x) = 8 / Gamma(beta) * (beta / (e*x))^(beta)
    //         * rho^(-2*N) / (1 - rho^(-2)) * g(x),
    //
    // for 0 < x < 2, where rho(x) = 3 - x + sqrt((2 - x) * (4 - x)), and
    //
    //  g(x) = x^(beta-1)        if 0 < beta <= 1,
    //  g(x) = (6 - x)^(beta-1)  if beta > 1
    //

    // Logarithm of pre-factor of R(N, x),
    const auto ln_pre = std::log(T(8)) - std::lgamma(beta__) +
                        beta__ * std::log(beta__ / constant<T>::e);

    const size_type max_iter = 100;
    const auto ln_eps        = std::log(eps__);

    // Initial guesses of parameters

    size_type n1 = 0;
    size_type n2 = 4;
    auto x       = T(1);

    // function log(g(x))
    auto fn_g = [=](T z) {
        return beta__ <= T(1)
                   ? -std::log(z)
                   : (beta__ - 1) * std::log(6 - z) - beta__ * std::log(z);
    };

    // first derivative of log(g(x))
    auto fn_dg = [=](T z) {
        return beta__ <= T(1) ? -1 / z : (1 - beta__) / (6 - z) - beta__ / z;
    };

    // second derivative of log(g(x))
    auto fn_d2g = [=](T z) {
        return beta__ <= T(1)
                   ? 1 / (z * z)
                   : (1 - beta__) / ((6 - z) * (6 - z)) + beta__ / (z * z);
    };

    while (true)
    {
        // For given N = n2, minimize R(N, x) w.r.t. x
        for (size_type i = 0; i < max_iter; ++i)
        {
            auto p    = std::sqrt((2 - x) * (4 - x));
            auto pinv = 1 / p;
            // rho(x) = 3 - x + sqrt((2 - x) * (4 - x))
            auto rho = 3 - x + p;
            // d rho(x) / d x = -1 - (3 - x) / sqrt((2 - x) * (4 - x))
            //                = - rho / p
            // auto drho = -rho * pinv;
            // d^2 rho(x) / d x^2
            // auto d2rho = rho * pinv * pinv * (1 - (3 - x) * pinv);

            //
            // Let f(x) = log(rho^{-2N}(x) / (1 -rho^{2}(x))) and compute its
            // first and second derivatives
            //
            auto t   = 1 / (rho + 1) * (rho - 1);
            auto df  = 2 * (n2 - t) * pinv;
            auto d2f = (df * (3 - x) - 2 * rho * rho * t * t) * pinv * pinv;

            //
            // Compute first and second derivatives of log(g(x))
            //
            auto dg  = fn_dg(x);
            auto d2g = fn_d2g(x);

            // d log(R(N, x)) / d x without pre-factor
            auto dr = df + dg;
            // d^2 log(R(N, x)) / d x^2  without pre-factor
            auto d2r = d2f + d2g;

            // Update x
            auto dx = -dr / d2r;

            if (x + dx < T()) // ensure x > 0
            {
                x *= T(0.5);
            }
            else if (x + dx > T(2)) // ensure x < 2
            {
                x += (2 - x) / T(2);
            }
            else
            {
                x += dx;
            }

            if (std::abs(dx) <= x * eps__)
            {
                break;
            }
        }

        // ln(R(N, b))
        const auto rho = 3 - x + std::sqrt((2 - x) * (4 - x));
        const auto fx =
            -T(2 * n2) * std::log(rho) - std::log1p(-T(1) / rho / rho);
        const auto ln_resid = fx + fn_g(x);

        if (ln_pre + ln_resid <= ln_eps) // equiv to R(N, b) <= eps
        {
            if (n2 - n1 <= 1)
            {
                break;
            }

            n2 = (n1 + n2) / 2;
        }
        else
        {
            n1 = n2;
            n2 = n2 + 2;
        }
    }

    return {n2, std::acosh(3 - x)};
}
} // namespace: expsum

#endif /* EXPSUM_KERNEL_FUNCTIONS_POW_KERNEL_HPP */


/*
 * genstudenttab.cpp:
 *
 */
#include <iostream>
#include <iomanip>
#include <boost/math/distributions/students_t.hpp>

using namespace std;
using namespace boost::math;

const int samplesize_max = 100;

int main()
{
    double alpha[] = { 0.1, 0.05, 0.01 }; /* 90%, 95%, 99% */

    cout << "int tstud_tab_size = " << samplesize_max << ";\n";

    for(int i = 0; i < sizeof(alpha) / sizeof(alpha[0]); ++i) {

        students_t d(99999);
        double t = quantile(complement(d, alpha[i] / 2));
        cout << "double tstud_p" << fixed << setprecision(0) << 100 * (1 - alpha[i])
             << "_ninf = " << fixed << setprecision(3) << t << ";\n";

        cout << "double tstud_tab_p" << setprecision(0) << 100 * (1 - alpha[i])
             << "[] = {\n";
        cout << "    ";
        for (int n = 2; n <= samplesize_max; ++n) {
            students_t dist(n - 1);
            double t = quantile(complement(dist, alpha[i] / 2));
            cout << fixed << setprecision(3) << left << t << ", ";
            if (n % 8 == 0) {
                cout << endl << "    ";
            }
        }
        cout << "\n};\n";
    }
    return 0;
}


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

// Free energy, energy, and specific heat of square lattice Ising model

#include <iomanip>
#include <iostream>
#include <boost/math/differentiation/autodiff.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "ising/mp_wrapper.hpp"
#include "ising/tc/square.hpp"
#include "options.hpp"
#include "square.hpp"

template<typename T>
void calc0(const options2& opt) {
  using namespace ising::free_energy;
  typedef T real_t;
  real_t Jx = convert<real_t>(opt.Jx);
  real_t Jy = convert<real_t>(opt.Jy);
  real_t Tmin, Tmax, dT;
  if (opt.Tmin == "tc" || opt.Tmin == "Tc") {
    Tmin = Tmax = dT = ising::tc::square(Jx, Jy);
  } else {
    Tmin = convert<real_t>(opt.Tmin);
    Tmax = convert<real_t>(opt.Tmax);
    dT = convert<real_t>(opt.dT);
  }
  if (Tmin < 0 || Tmax < 0) throw(std::invalid_argument("Temperature should be positive"));
  if (Tmin > Tmax) throw(std::invalid_argument("Tmax should be larger than Tmin"));
  if (dT <= 0) throw(std::invalid_argument("dT should be positive"));
  std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10)
            << "# lattice: square\n"
            << "# precision: " << std::numeric_limits<real_t>::digits10 << std::endl
            << "# Lx Ly Jx Jy T 1/T F/N E/N C/N\n";
  for (auto t = Tmin; t < Tmax + 1e-4 * dT; t += dT) {
    real_t beta = 1 / t;
    auto f = square::infinite(Jx, Jy, beta);
    std::cout << "inf inf " << Jx << ' ' << Jy << ' ' << t << ' ' << beta << ' '
              << f << " N/A N/A" << std::endl;
  }
}

template<typename T>
void calc(const options2& opt) {
  using namespace ising::free_energy;
  typedef T real_t;
  real_t Jx = convert<real_t>(opt.Jx);
  real_t Jy = convert<real_t>(opt.Jy);
  real_t Tmin, Tmax, dT;
  if (opt.Tmin == "tc" || opt.Tmin == "Tc") {
    Tmin = Tmax = dT = ising::tc::square(Jx, Jy);
  } else {
    Tmin = convert<real_t>(opt.Tmin);
    Tmax = convert<real_t>(opt.Tmax);
    dT = convert<real_t>(opt.dT);
  }
  if (Tmin < 0 || Tmax < 0) throw(std::invalid_argument("Temperature should be positive"));
  if (Tmin > Tmax) throw(std::invalid_argument("Tmax should be larger than Tmin"));
  if (dT <= 0) throw(std::invalid_argument("dT should be positive"));
  std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10)
            << "# lattice: square\n"
            << "# precision: " << std::numeric_limits<real_t>::digits10 << std::endl
            << "# Lx Ly Jx Jy T 1/T F/N E/N C/N\n";
  for (auto t = Tmin; t < Tmax + 1e-4 * dT; t += dT) {
    auto beta = boost::math::differentiation::make_fvar<real_t, 2>(1 / t);
    auto f = square::infinite(Jx, Jy, beta);
    std::cout << "inf inf " << Jx << ' ' << Jy << ' ' << t << ' ' << (1 / t) << ' '
              << free_energy(f, beta) << ' ' << energy(f, beta) << ' '
              << specific_heat(f, beta) << std::endl;
  }
}

int main(int argc, char **argv) {
  using namespace boost::multiprecision;
  options2 opt(argc, argv);
  if (!opt.valid) return 127;
  if (opt.prec <= std::numeric_limits<float>::digits10) {
    calc<float>(opt);
  } else if (opt.prec <= std::numeric_limits<double>::digits10) {
    calc<double>(opt);
  } else if (opt.prec <= std::numeric_limits<mp_wrapper<cpp_dec_float_50>>::digits10) {
    calc0<mp_wrapper<cpp_dec_float_50>>(opt);
    // calc<mp_wrapper<cpp_dec_float_50>>(opt);
  } else if (opt.prec <= std::numeric_limits<mp_wrapper<cpp_dec_float_100>>::digits10) {
    calc0<mp_wrapper<cpp_dec_float_100>>(opt);
    // calc<mp_wrapper<cpp_dec_float_100>>(opt);
  } else {
    std::cerr << "Error: Required precision is too high\n"; return 127;
  }
}


// Example: exercise2-advanced
//
// Goal: implement a simple adaptive strategy based on exact solution
//
// Reference: see https://fusionforge.zih.tu-dresden.de/plugins/mediawiki/wiki/amdis/index.php/AMDiS::Expressions
//            for a detailed description of possible expression terms.
//
// Compile-and-run:
// > cd build
// > make exercise2b
// > cd ..
// > build/exercise2b init/exercise2.dat.2d
//

#include "AMDiS.h"

#include <array>
#include <boost/numeric/mtl/mtl.hpp>

using namespace AMDiS;

void convergence_factor(std::vector<std::array<double,2>> const& error)
{
  mtl::dense_vector<double> rhs_res(error.size());
  mtl::dense2D<double> A_res(error.size(), 2);
  for (size_t i = 0; i < error.size(); ++i) {
    rhs_res(i) = std::log(error[i][1]);
    A_res(i, 0) = 1;
    A_res(i, 1) = std::log(error[i][0]);
  }

  mtl::dense_vector<double> convergence(2);

  mtl::dense2D<double> Q(num_rows(A_res), num_rows(A_res)), R(num_rows(A_res), num_cols(A_res));
  boost::tie(Q, R)= mtl::matrix::qr(A_res);

  mtl::dense_vector<double> b(trans(Q)*rhs_res);
  mtl::irange rows(0,num_cols(A_res));

  mtl::dense2D<double> R_ = R[rows][rows];
  convergence = mtl::matrix::upper_trisolve(R_, b[rows]);

  std::cout << "\n|u - u_h| = C * h^k\n   C = " << std::exp(convergence(0)) << "\n   k = " << convergence(1) << "\n\n";
}

inline double calcMeshSizes(Mesh* mesh)
{
  TraverseStack stack;
  ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_LEAF_EL | Mesh::FILL_COORDS);
  double maxH = 0.0;
  while (elInfo) {
    auto coords = elInfo->getCoords();
    for (int i = 0; i < coords.getSize(); i++)
      for (int j = i+1; j < coords.getSize(); j++)
	  maxH = std::max(maxH, norm(coords[i] - coords[j]));
    elInfo = stack.traverseNext(elInfo);
  }
  return maxH;
}

struct G : AbstractFunction<double, WorldVector<double> >
{
  double operator()(WorldVector<double> const& x) const
  {
    return std::exp(-10.0*(x*x));
  }
};

// solve: -laplace(u) = f(x) in Omega,    u = g on Gamma
int main(int argc, char* argv[])
{ FUNCNAME("Main");

  AMDiS::init(argc, argv);

  // ===== create and init the scalar problem =====
  ProblemStat prob("poisson");
  prob.initialize(INIT_ALL);

  // ===== define operators =====
  Operator opLaplace(prob.getFeSpace(), prob.getFeSpace());
  addSOT(opLaplace, 1.0);

  Operator opF(prob.getFeSpace());
  auto f = -(400.0*(X()*X()) - 40.0)*exp(-10.0*(X()*X()));
  addZOT(opF, f); // f(x)

  // ===== add operators to problem =====
  prob.addMatrixOperator(opLaplace, 0, 0);   // -laplace(u)
  prob.addVectorOperator(opF, 0);            // f(x)

  // ===== add boundary conditions =====
  BoundaryType nr = 1;
  prob.addDirichletBC(nr, 0, 0, new G); // g(x)

  // ===== create info-object, that holds parameters ===
  AdaptInfo adaptInfo("adapt");

  DOFVector<double>& U = *prob.getSolution(0);
  DOFVector<double> ErrVec(U), UExact(U);

  auto u_exact = exp(-10.0*(X()*X()));

  std::vector<std::array<double, 2>> error_vec_L2;
  std::vector<std::array<double, 2>> error_vec_H1;
  std::vector<std::array<double, 2>> error_vec_P;

  double error = 1.e10;
  for (int i = 0; i < adaptInfo.getMaxSpaceIteration(); ++i)
  {
    // ===== assemble and solve linear system =====
    prob.assemble(&adaptInfo);
    prob.solve(&adaptInfo);

    ErrVec << absolute(valueOf(U) - u_exact);
    io::writeFile(ErrVec, "error_" + std::to_string(i) + ".vtu");

    UExact << u_exact;
    double errorL2 = std::sqrt( integrate( pow<2>(valueOf(U) - valueOf(UExact)) ) );
    double errorH1 = std::sqrt( integrate( pow<2>(valueOf(U) - valueOf(UExact)) + unary_dot(gradientOf(U) - gradientOf(UExact)) ) );

    double h_max = calcMeshSizes(prob.getMesh());

    WorldVector<double> p; p[0] = 0.5; p[1] = 0.5;
    double errorP = std::abs(U(p) - exp(-10.0*(p*p)));
    MSG("h(%d) = %f\n", i, h_max);
    MSG("errorL2(%d) = %e\n", i, errorL2);
    MSG("errorH1(%d) = %e\n", i, errorH1);
    MSG("errorP(%d) = %e\n", i, errorP);

    error_vec_L2.push_back({h_max, errorL2});
    error_vec_H1.push_back({h_max, errorH1});
    error_vec_P.push_back({h_max, errorP});

    error = errorL2;
    if (error < adaptInfo.getSpaceTolerance(0))
      break;

    // refine mesh
    RefinementManager* refManager = prob.getRefinementManager();
    Flag f = refManager->globalRefine(prob.getMesh(), 1);
  }

  std::cout << "L2-norm:\n";
  convergence_factor(error_vec_L2);

  std::cout << "H1-norm:\n";
  convergence_factor(error_vec_H1);

  std::cout << "pointwise-norm:\n";
  convergence_factor(error_vec_P);

  AMDiS::finalize();
}


/*
Copyright 2009-2021 Nicolas Colombe

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include <math/principalaxis.hpp>

#include <armadillo>

namespace eXl
{

  bool ComputePrincipalAxis::operator()(Polygoni const& iPoly)
  {
    return Compute(iPoly);
  }

  bool ComputePrincipalAxis::operator()(Polygonf const& iPoly)
  {
    return Compute(iPoly);
  }

  bool ComputePrincipalAxis::operator()(Polygond const& iPoly)
  {
    return Compute(iPoly);
  }

  bool ComputePrincipalAxis::operator()(Vector<Vector2f> const& iPoints)
  {
    return Compute(iPoints.data(), iPoints.size());
  }

  bool ComputePrincipalAxis::operator()(Vector<Vector2d> const& iPoints)
  {
    return Compute(iPoints.data(), iPoints.size());
  }

  template <typename Real>
  bool ComputePrincipalAxis::Compute(Polygon<Real> const& iPoly)
  {
    if (iPoly.Border().size() == 0)
    {
      return false;
    }
    uint32_t numPt = iPoly.Border().size();
    Vector2<Real> const* points = iPoly.Border().data();
    if (numPt > 1 && points[0] == points[numPt - 1])
    {
      --numPt;
    }

    return Compute(points, numPt);
  }

  template <typename Real>
  bool ComputePrincipalAxis::Compute(Vector2<Real> const* iPoints, uint32_t iNumPt)
  {
    if (iNumPt == 0)
    {
      return false;
    }
    arma::mat covarianceMatrix(2, 2, arma::fill::zeros);
    m_Center = Vector2d::ZERO;

    for (unsigned int i = 0; i < iNumPt; ++i)
    {
      Vector2<Real> const& curValue = iPoints[i];
      m_Center.X() += curValue.X();
      m_Center.Y() += curValue.Y();
    }

    m_Center.X() /= iNumPt;
    m_Center.Y() /= iNumPt;

    for (unsigned int i = 0; i < iNumPt; ++i)
    {
      Vector2<Real> const& curValue = iPoints[i];
      double centeredX = curValue.X() - m_Center.X();
      double centeredY = curValue.Y() - m_Center.Y();
      covarianceMatrix.at(0,0) += centeredX * centeredX;
      covarianceMatrix.at(1,1) += centeredY * centeredY;
      covarianceMatrix.at(0,1) += centeredX * centeredY;
    }

    covarianceMatrix.at(0,0) /= iNumPt;
    covarianceMatrix.at(1,1) /= iNumPt;
    covarianceMatrix.at(0,1) /= iNumPt;
    covarianceMatrix.at(1,0) = covarianceMatrix.at(0,1);

    arma::colvec eigval;
    arma::mat eigvect;

    bool res = arma::eig_sym(eigval, eigvect, covarianceMatrix);

    eXl_ASSERT_REPAIR_RET(res, false);

    unsigned int maxIdx = 0;
    double maxVal = eigval.at(0);
    if (eigval.at(1) > maxVal)
    {
      maxIdx = 1;
      maxVal = eigval.at(1);
    }

    m_PrimaryAxis.X() = eigvect.at(maxIdx, 0);
    m_PrimaryAxis.Y() = eigvect.at(maxIdx, 1);

    return true;
  }
}