#include <cassert>
#include <cmath>

#include <functional>
#include <numeric>
#include <sstream>

#include <boost/multiprecision/gmp.hpp>

#include <QtDebug>

using s64 = int64_t;

using namespace std;

using boost::multiprecision::mpq_rational;

QDebug operator<<(QDebug d, const mpq_rational& r) {
    d.nospace();
    d.noquote();
    stringstream s;
    s << r;
    d << QString::fromStdString(s.str());
    return d.resetFormat();
}

QMap<int /*roll*/, s64 /*count*/> rolls(int sides, int repetitions) {

    std::function<void(int , int, QMap<int, s64>*)> traverse = [&traverse, sides, repetitions](int sum, int rep, QMap<int, s64>* ret) {
        if (rep == repetitions) {
            if (ret->contains(sum) == false) {
                (*ret)[sum] = 0;
            }
            (*ret)[sum] += 1;
            return;
        }

        for (int i = 1; i <= sides; i += 1) {
            traverse(sum + i, rep + 1, ret);
        }
    };

    QMap<int, s64> ret;

    traverse(0, 0, &ret);

    return ret;
}

mpq_rational winProbability(int sides1, int reps1, int sides2, int reps2) {
    auto rolls1 = rolls(sides1, reps1);
    auto rolls2 = rolls(sides2, reps2);

    auto values1 = rolls1.values();
    auto values2 = rolls2.values();

    s64 sum1 = accumulate(begin(values1), end(values1), 0);
    s64 sum2 = accumulate(begin(values2), end(values2), 0);

    mpq_rational ret(0);

    QMap<int, s64>::const_iterator i = rolls2.constBegin();
    while (i != rolls2.constEnd()) {
        mpq_rational pickProbability(i.value(), sum1);

        s64 winNumerator = 0;

        QMap<int, s64>::const_iterator j = rolls1.constBegin();
        while (j != rolls1.constEnd()) {

            if (j.key() < i.key()) {
                winNumerator += j.value();
                //qDebug() << "  +" << j.key() << j.value() << winNumerator;
            } else {
                //qDebug() << "  -" << j.key();
            }

            ++j;
        }

        ret += pickProbability*mpq_rational(winNumerator, sum2);

        //qDebug() << "sum" << i.key() << ", pick" << pickProbability << ", win this" << mpq_rational(winNumerator, sum2) << ", total" << ret;

        ++i;
    }

    return ret;
}

void testRolls();
void test1();

int main(int argc, char **args) {
    Q_UNUSED(argc); Q_UNUSED(args);

    testRolls();
    test1();

    return 0;
}

void testRolls() {

    auto a31 = rolls(3, 1);
        assert(a31.size() == 3);
        assert(a31[1] == 1);
        assert(a31[2] == 1);
        assert(a31[3] == 1);

    auto a22 = rolls(2, 2);
        assert(a22.size() == 3);
        assert(a22[2] == 1);
        assert(a22[3] == 2);
        assert(a22[4] == 1);

    auto a49 = rolls(4, 9).values(); assert(accumulate(begin(a49), end(a49), 0) == 262144);
    auto a66 = rolls(6, 6).values(); assert(accumulate(begin(a66), end(a66), 0) ==  46656);
}

void test1() {
    qDebug() << winProbability(6, 6, 4, 9);
}


#include <SFML/Graphics.hpp>
#include <SFML/Window.hpp>
#include <iostream>
#include <Eigen/Dense>
#include <algorithm>
using namespace sf;
using namespace std;
using namespace Eigen;
typedef Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic > Mat;

void reset(Mat& mat) {
	//initialize matrix to zero matrix
	//행렬을 영행렬로 초기화합니다.
	for (int i = 0; i < mat.rows(); ++i)
		for (int j = 0; j < mat.cols(); ++j)
			mat(i, j) = 0;
}

bool compare(pair<double, Mat> a, pair<double, Mat> b) {
	//compare function for sorting eigen values in descending order
	//고유값을 내림차순으로 정렬하기 위한 비교함수
	return a.first * a.first > b.first * b.first;
}

class RGB {
public:
	enum { R, G, B };
	RGB() {
	}
	RGB(Mat red, Mat green, Mat blue) {
		size = red.rows();
		mat[R] = red;	mat[G] = green;	mat[B] = blue;
		empty = false;
	}
	Mat mat[3];
	int size;
	bool empty = true;

	vector<pair<double, Mat>> lists[3];
	bool processed[3] = { false,false,false };
	int added[3] = { -1,-1,-1 };
	void getEVDColor(int color, int rate) {
		if (!processed[color]) {
			SelfAdjointEigenSolver<Mat> eigensolver(mat[color]);
			if (eigensolver.info() != Success) abort();
			Mat eigenValues = eigensolver.eigenvalues();
			Mat eigenVectors = eigensolver.eigenvectors();
			eigenVectors = eigenVectors.householderQr().householderQ();//정규직교화
			int num = eigenValues.rows();//고유값의 개수

			for (int i = 0; i < num; ++i) {
				pair<double, Mat> element;
				element.first = eigenValues(i);
				element.second = eigenVectors.col(i);
				lists[color].push_back(element);
			}
			sort(lists[color].begin(), lists[color].end(), compare);
			processed[color] = true;
			mat[color] = Mat(size, size);
			reset(mat[color]);
			cout << endl;
		}
		if (added[color] <= rate) {
			for (int i = added[color] + 1; i < rate; ++i) {
				mat[color] += (lists[color][i].first * (lists[color][i].second * lists[color][i].second.transpose()));
			}
		}
		else {
			for (int i = added[color]; i >= rate; --i) {
				mat[color] -= (lists[color][i].first * (lists[color][i].second * lists[color][i].second.transpose()));
			}
		}
		added[color] = rate - 1;
	}
};

RGB getMatrixFromImage(Image& img) {
	//only square image available
	//it uses upper triangle part of the image only.
	if (img.getSize().x != img.getSize().y) return RGB();
	int size = img.getSize().x;

	Mat red(size, size);
	Mat green(size, size);
	Mat blue(size, size);
	for (int i = 0; i < size; ++i) {
		for (int j = 0; j < size; ++j) {
			if (j < i) {
				red(i, j) = img.getPixel(i, j).r;
				green(i, j) = img.getPixel(i, j).g;
				blue(i, j) = img.getPixel(i, j).b;
			}
			else if (i == j) red(i, j) = 0;
			else {
				red(i, j) = img.getPixel(j, i).r;
				green(i, j) = img.getPixel(j, i).g;
				blue(i, j) = img.getPixel(j, i).b;
			}
		}
	}
	return RGB(red, green, blue);
}

Image getImageFromRGB(RGB& rgb) {
	Image  result;
	result.create(rgb.size, rgb.size);
	for (int i = 0; i < rgb.size; ++i) {
		for (int j = 0; j < rgb.size; ++j) {
			Color color;
			color.r = rgb.mat[RGB::R](i, j);
			color.g = rgb.mat[RGB::G](i, j);
			color.b = rgb.mat[RGB::B](i, j);
			result.setPixel(i, j, color.toInteger() < 0 ? Color(0) : color);
		}
	}
	return result;
}

int main() {

	string input;
	cout << "input image file name including extension>";
	cin >> input;

	Image original;
	original.loadFromFile(input);
	if (original.getSize().x != original.getSize().y) {
		cout << "only square image available";
		return -1;
	}

	RenderWindow window(VideoMode(original.getSize().x, original.getSize().y), "Math channel Ssootube Eigen Value Decomposition");
	RGB mat;
	mat = getMatrixFromImage(original);
	Image applied = getImageFromRGB(mat);
	Texture t; t.loadFromImage(applied);
	Sprite output(t);
	int k = 0;
	cout << "only upper triangular part of the image will be used." << endl;
	while (window.isOpen()) {
		Event e;
		while (window.pollEvent(e)) {
			if (e.type == Event::Closed)
				window.close();
			if (e.type == Event::KeyPressed)
				if (e.key.code == Keyboard::Enter)
				{
					window.clear();
					window.draw(output);
					window.display();
					if (k <= mat.size) {
						mat.getEVDColor(RGB::R, k);
						mat.getEVDColor(RGB::G, k);
						mat.getEVDColor(RGB::B, k++);
					}
					applied = getImageFromRGB(mat);
					t.loadFromImage(applied);
					output.setTexture(t);
					cout << "press enter to continue. current rank:" << ((k - 2 == -1) ? 0 : k - 2) << endl;
					break;
				}
		}

	}
	return 0;
}


#include <math.h>
#include <iostream>

#include <Eigen/Dense>

// From a graph's adjacency matrix, generate its probability matrix A
Eigen::MatrixXf probMatrix(Eigen::MatrixXf &R, float alpha);

// Main body of improved Pagerank
Eigen::MatrixXf improvedPageRank(Eigen::MatrixXf &R, float alpha, float epsilon);


/**
 * @file pfasst/quadrature/interface.hpp
 * @since v0.3.0
 */
#ifndef _PFASST__QUADRATURE__INTERFACE_HPP_
#define _PFASST__QUADRATURE__INTERFACE_HPP_

#include <cassert>
#include <vector>
using namespace std;

#include <Eigen/Dense>
template<typename scalar>
using Matrix = Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

template<typename scalar>
using Index = typename Matrix<scalar>::Index;

#include "pfasst/globals.hpp"
#include "pfasst/interfaces.hpp"
#include "pfasst/quadrature/polynomial.hpp"

namespace pfasst
{
  namespace quadrature
  {
    /**
     * Quadrature type descriptors.
     * @since v0.3.0
     */
    enum class QuadratureType : int {
        GaussLegendre   =  0  //!< @ref pfasst::quadrature::GaussLegendre "Gauss-Legendre" quadrature
      , GaussLobatto    =  1  //!< @ref pfasst::quadrature::GaussLobatto "Gauss-Lobatto" quadrature
      , GaussRadau      =  2  //!< @ref pfasst::quadrature::GaussRadau "Gauss-Radau" quadrature
      , ClenshawCurtis  =  3  //!< @ref pfasst::quadrature::ClenshawCurtis "Clenshaw-Curtis" quadrature
      , Uniform         =  4  //!< Uniform quadrature
      , UNDEFINED       = -1
    };

    template<typename scalar>
    static Polynomial<scalar> build_polynomial(const size_t node, const vector<scalar>& nodes)
    {
      const size_t num_nodes = nodes.size();
      Polynomial<scalar> p(num_nodes + 1), p1(num_nodes + 1);
      p[0] = 1.0;

      for (size_t m = 0; m < num_nodes; ++m) {
        if (m == node) { continue; }

        // p_{m+1}(x) = (x - x_j) * p_m(x)
        p1[0] = scalar(0.0);
        for (size_t j = 0; j < num_nodes;     ++j) { p1[j + 1]  = p[j]; }
        for (size_t j = 0; j < num_nodes + 1; ++j) { p1[j]     -= p[j] * nodes[m]; }
        for (size_t j = 0; j < num_nodes + 1; ++j) { p[j]       = p1[j]; }
      }

      return p;
    }

    /**
     * Compute quadrature matrix \\( Q \\) between two sets of nodes.
     *
     * Computing the quadrature matrix \\( Q \\) for polynomial-based integration from one set of
     * quadrature nodes (@p from) to another set of quadrature nodes (@p to).
     *
     * @tparam scalar precision of quadrature (i.e. `double`)
     * @param[in] from first set of quadrature nodes
     * @param[in] to second set of quadrature nodes
     * @returns quadrature matrix \\( Q \\) with `to.size()` rows and `from.size()` colums
     *
     * @pre For correctness of the algorithm it is assumed, that both sets of nodes are in the range
     *   \\( [0, 1] \\).
     *
     * @since v0.3.0
     */
    template<typename scalar>
    static Matrix<scalar> compute_q_matrix(const vector<scalar>& from, const vector<scalar>& to)
    {
      const size_t to_size = to.size();
      const size_t from_size = from.size();
      assert(to_size >= 1 && from_size >= 1);

      Matrix<scalar> q_mat = Matrix<scalar>::Zero(to_size, from_size);

      for (size_t m = 0; m < from_size; ++m) {
        Polynomial<scalar> p = build_polynomial(m, from);
        // evaluate integrals
        auto den = p.evaluate(from[m]);
        auto P = p.integrate();
        for (size_t j = 0; j < to_size; ++j) {
          q_mat(j, m) = (P.evaluate(to[j]) - P.evaluate(scalar(0.0))) / den;
        }
      }

      return q_mat;
    }

    /**
     * Compute quadrature matrix \\( Q \\) for one set of nodes.
     *
     * @tparam scalar precision of quadrature (i.e. `double`)
     * @param[in] nodes quadrature nodes to compute \\( Q \\) matrix for
     *
     * @since v0.3.0
     *
     * @overload
     */
    template<typename scalar>
    static Matrix<scalar> compute_q_matrix(const vector<scalar>& nodes)
    {
      return compute_q_matrix<scalar>(nodes, nodes);
    }

    /**
     * Compute quadrature matrix \\( Q \\) from a given node-to-node quadrature matrix \\( S \\).
     *
     * @tparam scalar precision of quadrature (i.e. `double`)
     * @param[in] s_mat \\( S \\) matrix to compute \\( Q \\) from
     * @see pfasst::quadrature::compute_s_matrix
     *
     * @since v0.3.0
     *
     * @overload
     */
    template<typename scalar>
    static Matrix<scalar> compute_q_matrix(const Matrix<scalar>& s_mat)
    {
      Matrix<scalar> q_mat = Matrix<scalar>::Zero(s_mat.rows(), s_mat.cols());
      q_mat.col(0) = s_mat.col(0);
      for (Index<scalar> q_mat_col = 1; q_mat_col < q_mat.cols(); ++q_mat_col) {
        q_mat.col(q_mat_col) = q_mat.col(q_mat_col - 1) + s_mat.col(q_mat_col);
      }
      return q_mat;
    }

    /**
     * Compute node-to-node quadrature matrix \\( S \\) from a given quadrature matrix \\( Q \\).
     *
     * The \\( S \\) matrix provides a node-to-node quadrature where the \\( i \\)-th row of
     * \\( S \\) represents a quadrature from the \\( i-1 \\)-th node to the \\( i \\)-th node.
     *
     * The procedure is simply subtracting the \\( i-1 \\)-th row of \\( Q \\) from the
     * \\( i \\)-th row of \\( Q \\).
     *
     * @tparam scalar precision of quadrature (i.e. `double`)
     * @param[in] q_mat \\( Q \\) matrix to compute \\( S \\) of
     * @returns \\( S \\) matrix
     *
     * @since v0.3.0
     */
    template<typename scalar>
    static Matrix<scalar> compute_s_matrix(const Matrix<scalar>& q_mat)
    {
      Matrix<scalar> s_mat = Matrix<scalar>::Zero(q_mat.rows(), q_mat.cols());
      s_mat.row(0) = q_mat.row(0);
      for (Index<scalar> row = 1; row < s_mat.rows(); ++row) {
        s_mat.row(row) = q_mat.row(row) - q_mat.row(row - 1);
      }
      return s_mat;
    }

    /**
     * Compute node-to-node quadrature matrix \\( S \\) from two given sets of nodes
     *
     * @tparam scalar precision of quadrature (i.e. `double`)
     * @param[in] from first set of quadrature nodes
     * @param[in] to second set of quadrature nodes
     *
     * @since v0.3.0
     *
     * @overload
     */
    template<typename scalar>
    static Matrix<scalar> compute_s_matrix(const vector<scalar>& from, const vector<scalar>& to)
    {
      return compute_s_matrix(compute_q_matrix(from, to));
    }

    /**
     * Compute vector \\( q \\) for integration from \\( 0 \\) to \\( 1 \\) for given set of nodes.
     *
     * This equals to the last row of the quadrature matrix \\( Q \\) for the given set of nodes.
     *
     * @tparam scalar precision of quadrature (i.e. `double`)
     * @param[in] nodes quadrature nodes to compute \\( Q \\) matrix for
     * @pre For correctness of the algorithm it is assumed, that the nodes are in the range
     *   \\( [0, 1] \\).
     *
     * @since v0.3.0
     */
    template<typename scalar>
    static vector<scalar> compute_q_vec(const vector<scalar>& nodes)
    {
      const size_t num_nodes = nodes.size();
      assert(num_nodes >= 1);

      vector<scalar> q_vec = vector<scalar>(num_nodes, scalar(0.0));

      for (size_t m = 0; m < num_nodes; ++m) {
        Polynomial<scalar> p = build_polynomial(m, nodes);
        // evaluate integrals
        auto den = p.evaluate(nodes[m]);
        auto P = p.integrate();
        q_vec[m] = (P.evaluate(scalar(1.0)) - P.evaluate(scalar(0.0))) / den;
      }

      return q_vec;
    }

    /**
     * Interface for quadrature handlers.
     *
     * Quadrature handlers provide \\( Q \\), \\( S \\) and \\( B \\) matrices respecting the left
     * and right nodes, i.e. whether \\( 0 \\) and \\( 1 \\) are part of the nodes or not.
     *
     * Computation of the quadrature nodes and matrices (i.e. quadrature weights) is done on
     * initialization.
     *
     * @tparam scalar precision of quadrature (i.e. `double`)
     *
     * @since v0.3.0
     */
    template<typename precision = pfasst::time_precision>
    class IQuadrature
    {
      protected:
        //! @{
        static const bool LEFT_IS_NODE = false;
        static const bool RIGHT_IS_NODE = false;

        size_t num_nodes;
        Matrix<precision> q_mat;
        Matrix<precision> s_mat;
        vector<precision> q_vec; // XXX: black spot
        Matrix<precision> b_mat;
        vector<precision> nodes;
        //! @}

      public:
        //! @{
        /**
         * @throws invalid_argument if number of nodes is invalid for quadrature type
         */
        explicit IQuadrature(const size_t num_nodes);
        /**
         * @throws invalid_argument if number of nodes is invalid for quadrature type
         */
        IQuadrature();
        virtual ~IQuadrature() = default;
        //! @}

        //! @{
        virtual const Matrix<precision>& get_q_mat() const;
        virtual const Matrix<precision>& get_s_mat() const;
        virtual const Matrix<precision>& get_b_mat() const;
        virtual const vector<precision>& get_q_vec() const;
        virtual const vector<precision>& get_nodes() const;
        virtual size_t get_num_nodes() const;

        /**
         * @throws pfasst::NotImplementedYet if not overwritten by implementation;
         *   required for quadrature of any kind
         */
        virtual bool left_is_node() const;
        /**
         * @throws pfasst::NotImplementedYet if not overwritten by implementation;
         *   required for quadrature of any kind
         */
        virtual bool right_is_node() const;
        //! @}

        /**
         * Compute a rough estimate of the numerical error... XXX
         */
        precision expected_error() const;

      protected:
        //! @{
        /**
         * @throws pfasst::NotImplementedYet if not overwritten by implementation;
         *   required for quadrature of any kind
         */
        virtual void compute_nodes();
        virtual void compute_weights();
        //! @}
    };
  }  // ::pfasst::quadrature
}  // ::pfasst

#include "pfasst/quadrature/interface_impl.hpp"

#endif  // _PFASST__QUADRATURE__INTERFACE_HPP_


/*
 * VELOCITY_RELATIONS_TEST.CPP Test fixtures for analytical profiles
 *
 * Author:                   Tom Clark (thclark @ github)
 *
 * Copyright (c) 2016-9 Octue Ltd. All Rights Reserved.
 *
 */

#include <Eigen/Dense>
#include <Eigen/Core>
#include "gtest/gtest.h"
#include <unsupported/Eigen/AutoDiff>

#include "definitions.h"
#include "relations/stress.h"
#include "relations/veer.h"
#include "relations/velocity.h"

using namespace es;
using namespace Eigen;

// Test fixture for generating analytical profiles
class VelocityRelationsTest : public ::testing::Test {};

TEST_F(VelocityRelationsTest, test_power_law_profile) {

    // Basic test parameters
    double z_ref = 60;
    double u_ref = 10;
    double alpha = 0.3;

    // Check that it works for a z value of type double
    double z_doub = 1.;
    double speed2 = power_law_speed(z_doub, u_ref, z_ref, alpha);
    std::cout << "checked scalar double operation (U = " << speed2 << " m/s)" << std::endl;

    // Check that it works for a VectorXd input (vertically spaced z)
    double low = 1;
    double high = 100;
    size_t n_bins = 100;
    VectorXd z = VectorXd::LinSpaced(n_bins, low, high);
    VectorXd speed = power_law_speed(z, u_ref, z_ref, alpha);
    std::cout << "checked VectorXd operation" << std::endl;

    // Check that it works for an AutoDiffScalar
    // Also provides minimal example of how to get the derivative through the profile
    typedef Eigen::AutoDiffScalar<Eigen::VectorXd> ADScalar;
    ADScalar ads_z;
    ADScalar ads_speed;
    VectorXd dspeed_dz;
    dspeed_dz.setZero(n_bins);

    typedef Eigen::AutoDiffScalar<Eigen::VectorXd> ADScalar;
    for (int k = 0; k < n_bins; k++) {
        ads_z.value() = z[k];
        ads_z.derivatives() = Eigen::VectorXd::Unit(1, 0);  // Also works once outside the loop without resetting the
                                                            // derivative guess each step
        ads_speed = power_law_speed(ads_z, u_ref, z_ref, alpha);
        dspeed_dz[k] = ads_speed.derivatives()[0];
    }
    std::cout << "checked AutoDiffScalar operation" << std::endl;

    // Print useful diagnostics values
    std::cout << "speed = ["     << speed.transpose()     << "];" << std::endl;
    std::cout << "dspeed_dz = [" << dspeed_dz.transpose() << "];" << std::endl;
}

//TEST_F(VelocityRelationsTest, test_most_profile) {
//
//    // von karman constant
//    double kappa = 0.41;
//    // zero plane offset distance (e.g. for forest canopies)
//    double d = 0;
//    // roughness length
//    double z0 = 0.5;
//    // Monin-Obukhov length
//    double L = 20.;
//
//    // Check that it works for a z value of type double
//    double z_doub = 1.;
//    double speed2 = most_law_speed(z_doub, kappa, d, z0, L);
//    std::cout << "checked scalar double operation (U = " << speed2 << " m/s)" << std::endl;
//
//    // Check that it works for a VectorXd input (vertically spaced z)
//    double low = 1;
//    double high = 100;
//    size_t n_bins = 100;
//    VectorXd z = VectorXd::LinSpaced(n_bins, low, high);
//    VectorXd speed = most_law_speed(z, kappa, d, z0, L);
//    std::cout << "checked VectorXd operation" << std::endl;
//
//    // Check that it works for an AutoDiffScalar
//    // Also provides minimal example of how to get the derivative through the profile
//    typedef Eigen::AutoDiffScalar<Eigen::VectorXd> ADScalar;
//    ADScalar ads_z;
//    ADScalar ads_speed;
//    VectorXd dspeed_dz;
//    dspeed_dz.setZero(n_bins);
//    for (int k = 0; k < n_bins; k++) {
//        ads_z.value() = z[k];
//        ads_z.derivatives() = Eigen::VectorXd::Unit(1, 0);  // Also works once outside the loop without resetting the
//        // derivative guess each step
//        ads_speed = most_law_speed(ads_z, kappa, d, z0, L);
//        dspeed_dz[k] = ads_speed.derivatives()[0];
//    }
//    std::cout << "checked AutoDiffScalar operation" << std::endl;
//
//    // Print useful diagnostics values
//    std::cout << "speed = ["     << speed.transpose()     << "];" << std::endl;
//    std::cout << "dspeed_dz = [" << dspeed_dz.transpose() << "];" << std::endl;
//}

TEST_F(VelocityRelationsTest, test_marusic_jones_profile) {

    // Basic test parameters
    double pi_j = 0.42;
    double kappa = 0.41;
    double delta = 1000.0;
    double u_inf = 20.0;
    double s = 23.6;
    double u_tau = u_inf / s;
    double z_0 = 0.0;

    // Check that it works for a z value of type double
    double z_doub = 1.0;
    double speed1 = marusic_jones_speed(z_doub, pi_j, kappa, z_0, delta, u_inf, u_tau);
    std::cout << "checked scalar double operation (U = " << speed1 << " m/s)" << std::endl;

    // Check that it works for a VectorXd input (vertically spaced z)
    double low = 1;
    double high = 100;
    size_t n_bins = 100;
    VectorXd z = VectorXd::LinSpaced(n_bins, low, high);
    VectorXd speed = marusic_jones_speed(z, pi_j, kappa, z_0, delta, u_inf, u_tau);
    std::cout << "checked VectorXd operation" << std::endl;

    // Check that it works for an AutoDiffScalar in z
    // Also provides minimal example of how to get the derivative through the profile
    typedef Eigen::AutoDiffScalar<Eigen::VectorXd> ADScalar;
    ADScalar ads_z;
    ADScalar ads_speed;
    VectorXd dspeed_dz;
    dspeed_dz.setZero(n_bins);
    for (int k = 0; k < n_bins; k++) {
        ads_z.value() = z[k];
        ads_z.derivatives() = Eigen::VectorXd::Unit(1, 0);  // Also works once outside the loop without resetting the derivative guess each step
        ads_speed = marusic_jones_speed(ads_z, pi_j, kappa, z_0, delta, u_inf, u_tau);
//        std::cout << ads_speed.value() << std::endl;
        dspeed_dz[k] = ads_speed.derivatives()[0];
    }
    std::cout << "checked AutoDiffScalar operation" << std::endl;

    // Print useful diagnostics values
    std::cout << "speed = ["     << speed.transpose()     << "];" << std::endl;
    std::cout << "dspeed_dz = [" << dspeed_dz.transpose() << "];" << std::endl;

    // Check it works for an AutoDiffScalar in z and pi_j
    VectorXd partial_dspeed_dz, partial_dspeed_dpi, partial_dspeed_dpi_check;
    ADScalar ads_pi_j, ads_speed_check1, ads_speed_check2;
    partial_dspeed_dz.setZero(n_bins);
    partial_dspeed_dpi.setZero(n_bins);
    partial_dspeed_dpi_check.setZero(n_bins);
    for (int k = 0; k < n_bins; k++) {

        // Set the current values
        ads_z.value() = z[k];
        ads_pi_j.value() = pi_j;

        // Initialise both derivative terms to unit vectors, telling us which term relates to which derivative
        ads_z.derivatives() = Eigen::VectorXd::Unit(2,0);
        ads_pi_j.derivatives() = Eigen::VectorXd::Unit(2,1);
        ads_speed = marusic_jones_speed(ads_z, ads_pi_j, kappa, z_0, delta, u_inf, u_tau);
        partial_dspeed_dz[k] = ads_speed.derivatives()[0];
        partial_dspeed_dpi[k] = ads_speed.derivatives()[1];

        // Run a check with a crude central differencer
        ads_speed_check1 = marusic_jones_speed(ads_z, pi_j-0.01, kappa, z_0, delta, u_inf, u_tau);
        ads_speed_check2 = marusic_jones_speed(ads_z, pi_j+0.01, kappa, z_0, delta, u_inf, u_tau);
        ads_speed_check2 = (ads_speed_check2 - ads_speed_check1) / 0.02;
        partial_dspeed_dpi_check[k] = ads_speed_check2.value();

    }
    std::cout << "partial_dspeed_dpi       = [" << partial_dspeed_dpi.transpose() << "];" << std::endl;
    std::cout << "partial_dspeed_dpi_check = [" << partial_dspeed_dpi_check.transpose() << "];" << std::endl;
}

TEST_F(VelocityRelationsTest, test_lewkowicz_profile) {

    // Basic test parameters
    double pi_coles = 0.42;
    double kappa = 0.41;
    double delta = 1000.0;
    double u_inf = 20.0;
    double shear_ratio = 23.6;
    double z_0 = 0.0;

    // Check that it works for a z value of type double
    double z_doub = 1.0;
    double speed1 = lewkowicz_speed(z_doub, pi_coles, kappa, u_inf, shear_ratio, delta);
    std::cout << "checked scalar double operation (U = " << speed1 << " m/s)" << std::endl;

    // Check that it works for a ArrayXd input (vertically spaced z)
    double low = 1;
    double high = 100;
    int n_bins = 10;
    VectorXd z = VectorXd::LinSpaced(n_bins, low, high);
    VectorXd speed = lewkowicz_speed(z, pi_coles, kappa, u_inf, shear_ratio, delta);
    std::cout << "checked VectorXd operation: "<< speed.transpose() << std::endl;

    // Check that it works for an AutoDiffScalar
    // Also provides minimal example of how to get the derivative through the profile
    typedef Eigen::AutoDiffScalar<Eigen::VectorXd> ADScalar;
    ADScalar ads_z;
    ADScalar ads_speed;
    VectorXd dspeed_dz;
    dspeed_dz.setZero(n_bins);
    for (int k = 0; k < n_bins; k++) {
        ads_z.value() = z[k];
        ads_z.derivatives() = Eigen::VectorXd::Unit(1, 0);  // Also works once outside the loop without resetting the derivative guess each step
        ads_speed = lewkowicz_speed(ads_z, pi_coles, kappa, u_inf, shear_ratio, delta);
        dspeed_dz[k] = ads_speed.derivatives()[0];
    }
    std::cout << "checked AutoDiffScalar<VectorXd> operation" << std::endl;

    // Print useful diagnostics values
    std::cout << "speed = ["     << speed.transpose()     << "];" << std::endl;
    std::cout << "dspeed_dz = [" << dspeed_dz.transpose() << "];" << std::endl;

    // Print variables to plot comparison with MATLAB based equivalent calculation
    //    std::cout << "pi_j = " << pi_j << ";" << std::endl;
    //    std::cout << "kappa = " << kappa << ";" << std::endl;
    //    std::cout << "delta = " << delta << ";" << std::endl;
    //    std::cout << "s = " << s << ";" << std::endl;
    //    std::cout << "u_inf = " << u_inf << ";" << std::endl;
    //    std::cout << "z_0 = " << z_0 << ";" << std::endl;
    //    std::cout << "z = [" << z << "];" << std::endl;

}

class VeerRelationsTest : public ::testing::Test {};

TEST_F(VeerRelationsTest, test_veer_profile) {

//    // Elevation of site in degrees latitude
//    double phi_latitude = 52;
//
//    Vector3d k;
//    k << 0, 0, 2*omega_world*sind(phi_latitude);
//
//    VectorXd v_bar;
//    v_bar << 10., 3.;

    // Assume homogeneous, isotropic turbulence such that R13 = R23. Not valid, but OK for the sake of the unit test.

}


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

/**
 * @file utils_test.cpp
 * @brief Unit tests using Google Test framework to validate class
 *  implementation.
 * @author Sean Crutchlow <sean.GH1231@gmail.com>
 * @version 1.0
 */

/// System
#include <gtest/gtest.h>

#include <cmath>
#include <tuple>
#include <type_traits>
#include <vector>

/// Library
#include <Eigen/Dense>

/// Project
#include "kalman_filter/utils.h"

namespace kalman_filter {
class UtilsTest : public ::testing::Test {
 protected:
  double error;

  virtual void SetUp() {
    error = 1e-6;
  }

  virtual void TearDown() {}

  const bool Compare(const coord2D_t& _lhs,
                     const coord2D_t& _rhs,
                     const double _epsilon) {
    if (std::abs(std::get<0>(_lhs) - std::get<0>(_rhs)) > _epsilon)
      return false;
    if (std::abs(std::get<1>(_lhs) - std::get<1>(_rhs)) > _epsilon)
      return false;

    return true;
  }

  const bool Compare(const coord3D_t& _lhs,
                     const coord3D_t& _rhs,
                     const double _epsilon) {
    if (std::abs(std::get<0>(_lhs) - std::get<0>(_rhs)) > _epsilon)
      return false;
    if (std::abs(std::get<1>(_lhs) - std::get<1>(_rhs)) > _epsilon)
      return false;
    if (std::abs(std::get<2>(_lhs) - std::get<2>(_rhs)) > _epsilon)
      return false;

    return true;
  }
};

TEST_F(UtilsTest, Cartesian2Polar) {
  coord2D_t expected(5.0, 0.92729521800161);     /// r, theta
  coord2D_t result = Cartesian2Polar(3.0, 4.0);  /// x, y

  EXPECT_TRUE(Compare(result, expected, error));
}

TEST_F(UtilsTest, Polar2Cartesian) {
  coord2D_t expected(-2.0807341827357, 4.5464871341284);  /// x, y
  coord2D_t result = Polar2Cartesian(5.0, 2.0);           /// r, theta

  EXPECT_TRUE(Compare(result, expected, error));
}

TEST_F(UtilsTest, Cartesian2Spherical) {
  coord3D_t expected(7.6426435217142,
                     0.74847023342654,
                     0.90806681890191);                     /// rho, theta, phi
  coord3D_t result = Cartesian2Spherical(3.2, 4.1, 5.6);    /// x, y, z

  EXPECT_TRUE(Compare(result, expected, error));
}

TEST_F(UtilsTest, Cartesian2Cylindrical) {
  coord3D_t expected(5.3413481444295,
                     0.90482708941579,
                     5.7);                                    /// rho, theta, z
  coord3D_t result = Cartesian2Cylindrical(3.3, 4.2, 5.7);    /// x, y, z

  EXPECT_TRUE(Compare(result, expected, error));
}

TEST_F(UtilsTest, Spherical2Cartesian) {
  coord3D_t expected(-6.352477916,
                      0.9055237668,
                      3.265892074);                         /// x, y, z
  coord3D_t result = Spherical2Cartesian(7.2, 3.0, 1.1);    /// rho, theta, phi

  EXPECT_TRUE(Compare(result, expected, error));
}

TEST_F(UtilsTest, Cylindrical2Cartesian) {
  coord3D_t expected(-2.802096119,
                      2.566760086,
                      4.8);                                 /// x, y, z
  coord3D_t result = Cylindrical2Cartesian(3.8, 2.4, 4.8);  /// r, theta, z

  EXPECT_TRUE(Compare(result, expected, error));
}

TEST_F(UtilsTest, Deg2Rad) {
  double expected = 0.021537363;   /// radians
  double result = Deg2Rad(1.234);  /// degrees

  EXPECT_NEAR(result, expected, error);
}

TEST_F(UtilsTest, Rad2Deg) {
  double expected = 70.702992;     /// degrees
  double result = Rad2Deg(1.234);  /// radians

  EXPECT_NEAR(result, expected, error);
}

TEST_F(UtilsTest, Feet2Meters) {
  double expected = 0.3761232;         /// meters
  double result = Feet2Meters(1.234);  /// feet

  EXPECT_NEAR(result, expected, error);
}

TEST_F(UtilsTest, Meters2Feet) {
  double expected = 4.0485564;         /// feet
  double result = Meters2Feet(1.234);  /// meters

  EXPECT_NEAR(result, expected, error);
}

TEST_F(UtilsTest, CalcRMSE) {
  Eigen::VectorXd e1(2), e2(2), a1(2), a2(2);
  e1 << 0.00001234, 0.12340000;
  e2 << 0.00999900, 0.99990000;
  a1 << 0.00987600, 0.12000000;
  a2 << 0.00001200, 0.98760000;

  std::vector<Eigen::VectorXd> estimated = {e1, e2};
  std::vector<Eigen::VectorXd> actual = {a1, a2};

  Eigen::VectorXd expected(2);
  expected << 0.009925522,
              0.00902358;

  Eigen::VectorXd result = CalcRMSE(estimated, actual);  /// meters

  EXPECT_TRUE((result - expected).norm() < error);
}
}   //  namespace kalman_filter

int main(int argc, char **argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt

/*
    This is an example illustrating the use of the matrix object
    from the dlib C++ Library.
*/

#include <iostream>
#include <dlib/matrix.h>

using namespace dlib;
using namespace std;

// ----------------------------------------------------------------------------------------

#if defined(BUILD_MONOLITHIC)
#define main(cnt, arr)      dlib_matrix_ex_main(cnt, arr)
#endif

int main(int argc, const char** argv)
{
    // Let's begin this example by using the library to solve a simple
    // linear system.
    //
    // We will find the value of x such that y = M*x where
    //
    //      3.5
    // y =  1.2
    //      7.8
    //
    // and M is
    //
    //      54.2   7.4   12.1
    // M =  1      2     3
    //      5.9    0.05  1

    // First let's declare these 3 matrices.
    // This declares a matrix that contains doubles and has 3 rows and 1 column.
    // Moreover, its size is a compile time constant since we put it inside the <>.
    matrix<double,3,1> y;
    // Make a 3 by 3 matrix of doubles for the M matrix.  In this case, M is
    // sized at runtime and can therefore be resized later by calling M.set_size().
    matrix<double> M(3,3);

    // You may be wondering why someone would want to specify the size of a
    // matrix at compile time when you don't have to.  The reason is two fold.
    // First, there is often a substantial performance improvement, especially
    // for small matrices, because it enables a number of optimizations that
    // otherwise would be impossible.  Second, the dlib::matrix object checks
    // these compile time sizes to ensure that the matrices are being used
    // correctly.  For example, if you attempt to compile the expression y*y you
    // will get a compiler error since that is not a legal matrix operation (the
    // matrix dimensions don't make sense as a matrix multiplication).  So if
    // you know the size of a matrix at compile time then it is always a good
    // idea to let the compiler know about it.

    // Now we need to initialize the y and M matrices and we can do so like this:
    M = 54.2,  7.4,  12.1,
        1,     2,    3,
        5.9,   0.05, 1;

    y = 3.5,
        1.2,
        7.8;

    // The solution to y = M*x can be obtained by multiplying the inverse of M
    // with y.  As an aside, you should *NEVER* use the auto keyword to capture
    // the output from a matrix expression.  So don't do this: auto x = inv(M)*y;
    // To understand why, read the matrix_expressions_ex.cpp example program.
    matrix<double> x = inv(M)*y;

    cout << "x: \n" << x << endl;

    // We can check that it really worked by plugging x back into the original equation
    // and subtracting y to see if we get a column vector with values all very close
    // to zero (Which is what happens.  Also, the values may not be exactly zero because
    // there may be some numerical error and round off).
    cout << "M*x - y: \n" << M*x - y << endl;

    // Also note that we can create run-time sized column or row vectors like so
    matrix<double,0,1> runtime_sized_column_vector;
    matrix<double,1,0> runtime_sized_row_vector;
    // and then they are sized by saying
    runtime_sized_column_vector.set_size(3);

    // Similarly, the x matrix can be resized by calling set_size(num rows, num columns).  For example
    x.set_size(3,4);  // x now has 3 rows and 4 columns.

    // The elements of a matrix are accessed using the () operator like so:
    cout << M(0,1) << endl;
    // The above expression prints out the value 7.4.  That is, the value of
    // the element at row 0 and column 1.

    // If we have a matrix that is a row or column vector.  That is, it contains either
    // a single row or a single column then we know that any access is always either
    // to row 0 or column 0 so we can omit that 0 and use the following syntax.
    cout << y(1) << endl;
    // The above expression prints out the value 1.2

    // Let's compute the sum of elements in the M matrix.
    double M_sum = 0;
    // loop over all the rows
    for (long r = 0; r < M.nr(); ++r)
    {
        // loop over all the columns
        for (long c = 0; c < M.nc(); ++c)
        {
            M_sum += M(r,c);
        }
    }
    cout << "sum of all elements in M is " << M_sum << endl;

    // The above code is just to show you how to loop over the elements of a matrix.  An
    // easier way to find this sum is to do the following:
    cout << "sum of all elements in M is " << sum(M) << endl;

    // Note that you can always print a matrix to an output stream by saying:
    cout << M << endl;
    // which will print:
    //   54.2  7.4 12.1
    //      1    2    3
    //    5.9 0.05    1

    // However, if you want to print using comma separators instead of spaces you can say:
    cout << csv << M << endl;
    // and you will instead get this as output:
    //   54.2, 7.4, 12.1
    //   1, 2, 3
    //   5.9, 0.05, 1

    // Conversely, you can also read in a matrix that uses either space, tab, or comma
    // separated values by uncommenting the following:
    // cin >> M;

    // -----------------------------  Comparison with MATLAB ------------------------------
    // Here I list a set of Matlab commands and their equivalent expressions using the dlib
    // matrix.  Note that there are a lot more functions defined for the dlib::matrix.  See
    // the HTML documentation for a full listing.

    matrix<double> A, B, C, D, E;
    matrix<int> Aint;
    matrix<long> Blong;

    // MATLAB: A = eye(3)
    A = identity_matrix<double>(3);

    // MATLAB: B = ones(3,4)
    B = ones_matrix<double>(3,4);

    // MATLAB: B = rand(3,4)
    B = randm(3,4);

    // MATLAB: C = 1.4*A
    C = 1.4*A;

    // MATLAB: D = A.*C
    D = pointwise_multiply(A,C);

    // MATLAB: E = A * B
    E = A*B;

    // MATLAB: E = A + C
    E = A + C;

    // MATLAB: E = A + 5
    E = A + 5;

    // MATLAB: E = E'
    E = trans(E);  // Note that if you want a conjugate transpose then you need to say conj(trans(E))

    // MATLAB: E = B' * B
    E = trans(B)*B;

    double var;
    // MATLAB: var = A(1,2)
    var = A(0,1); // dlib::matrix is 0 indexed rather than starting at 1 like Matlab.

    // MATLAB: C = round(C)
    C = round(C);

    // MATLAB: C = floor(C)
    C = floor(C);

    // MATLAB: C = ceil(C)
    C = ceil(C);

    // MATLAB: C = diag(B)
    C = diag(B);

    // MATLAB: B = cast(A, "int32")
    Aint = matrix_cast<int>(A);

    // MATLAB: A = B(1,:)
    A = rowm(B,0);

    // MATLAB: A = B([1:2],:)
    A = rowm(B,range(0,1));

    // MATLAB: A = B(:,1)
    A = colm(B,0);

    // MATLAB: A = [1:5]
    Blong = range(1,5);

    // MATLAB: A = [1:2:5]
    Blong = range(1,2,5);

    // MATLAB: A = B([1:3], [1:2])
    A = subm(B, range(0,2), range(0,1));
    // or equivalently
    A = subm(B, rectangle(0,0,1,2));

    // MATLAB: A = B([1:3], [1:2:4])
    A = subm(B, range(0,2), range(0,2,3));

    // MATLAB: B(:,:) = 5
    B = 5;
    // or equivalently
    set_all_elements(B,5);

    // MATLAB: B([1:2],[1,2]) = 7
    set_subm(B,range(0,1), range(0,1)) = 7;

    // MATLAB: B([1:3],[2:3]) = A
    set_subm(B,range(0,2), range(1,2)) = A;

    // MATLAB: B(:,1) = 4
    set_colm(B,0) = 4;

    // MATLAB: B(:,[1:2]) = 4
    set_colm(B,range(0,1)) = 4;

    // MATLAB: B(:,1) = B(:,2)
    set_colm(B,0) = colm(B,1);

    // MATLAB: B(1,:) = 4
    set_rowm(B,0) = 4;

    // MATLAB: B(1,:) = B(2,:)
    set_rowm(B,0) = rowm(B,1);

    // MATLAB: var = det(E' * E)
    var = det(trans(E)*E);

    // MATLAB: C = pinv(E)
    C = pinv(E);

    // MATLAB: C = inv(E)
    C = inv(E);

    // MATLAB: [A,B,C] = svd(E)
    svd(E,A,B,C);

    // MATLAB: A = chol(E,'lower')
    A = chol(E);

    // MATLAB: var = min(min(A))
    var = min(A);
}

// ----------------------------------------------------------------------------------------


#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include<cstdlib>
#include <time.h>

#include <Eigen/Dense>

// Splits a string into a vector, with a delimiter char
std::vector<std::string> split(std::string str, char delimeter);

// Reads a neighbours list and generates the adjacency matrix
Eigen::MatrixXf parseNeighboursList(std::string path);

// Reads an edges list and generates the adjacency matrix
Eigen::MatrixXf parseEdgesList(std::string path);

// Generates a graph of size n with a probability p of edge spawn,
// and saves it in path (as a neighbours list)
void genNeighboursList(std::string path, unsigned n, float p);


// g++ -O3 -DNDEBUG benchmarkX.cpp -o benchmarkX && time ./benchmarkX

#include <iostream>
#include <Eigen/Core>

using namespace std;
using namespace Eigen;

#ifndef VECTYPE
#define VECTYPE VectorXLd
#endif

#ifndef VECSIZE
#define VECSIZE 1000000
#endif

#ifndef REPEAT
#define REPEAT 1000
#endif

int main(int argc, char *argv[])
{
	VECTYPE I = VECTYPE::Ones(VECSIZE);
	VECTYPE m(VECSIZE,1);
	for(int i = 0; i < VECSIZE; i++)
	{
		m[i] = 0.1 * i/VECSIZE;
	}
	for(int a = 0; a < REPEAT; a++)
	{
		m = VECTYPE::Ones(VECSIZE) + 0.00005 * (m.cwise().square() + m/4);
	}
	cout << m[0] << endl;
	return 0;
}


//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2011
//
//============================================================================

#ifndef __Thea_Algorithms_Zernike2_hpp__
#define __Thea_Algorithms_Zernike2_hpp__

// Currently Visual Studio 2010 fails to compile Boost multi_array in Debug mode: https://svn.boost.org/trac/boost/ticket/4874
#if defined(_MSC_VER) && _MSC_VER >= 1600
#  define THEA_NO_ZERNIKE
#endif

#ifndef THEA_NO_ZERNIKE

#include "../Common.hpp"
#include "../IAddressableMatrix.hpp"
#include "../MatVec.hpp"
#include <boost/multi_array.hpp>
#include <complex>

namespace Thea {
namespace Algorithms {

/**
 * Compute Zernike moments of a 2D distribution, represented as a matrix of density values. The density values may be
 * multidimensional, i.e. the matrix elements may be vectors or colors.
 *
 * This class adapts code for the LightField Descriptor from Ding-Yun Chen et al.,
 * http://3d.csie.ntu.edu.tw/~dynamic/3DRetrieval/index.html .
 */
class THEA_API Zernike2
{
  public:
    /** The matrix type storing N-dimensional moments. Each column is a moment. */
    template <int N, typename ScalarT> using MomentMatrix = Matrix< N, Eigen::Dynamic, std::complex<ScalarT> >;

    /** %Options for generating Zernike moments. */
    struct THEA_API Options
    {
      int angular_steps;  ///< Number of angular steps.
      int radial_steps;   ///< Number of radial steps.
      int lut_radius;     ///< Radius of Zernike basis function for lookup table.

      /** Constructor. */
      Options(int angular_steps_ = 12, int radial_steps_ = 3, int lut_radius_ = 50)
      : angular_steps(angular_steps_), radial_steps(radial_steps_), lut_radius(lut_radius_)
      {}

      /** Get the set of default options. */
      static Options const & defaults() { static Options const def; return def; }

    }; // struct Options

    /** Constructor. */
    Zernike2(Options const & opts_ = Options::defaults());

    /** Get the number of moments generated by a call to compute(). */
    intx numMoments() const { return opts.angular_steps * opts.radial_steps; }

    /**
     * Compute Zernike moments of a 2D distribution, represented as a matrix of single- or multi-dimensional density values
     * (such as reals, vectors or colors). If the density values have more than one dimension, they should allow array
     * addressing (operator[](intx i)). The template parameter N, inferred from the last argument, must be the same
     * as the number of dimensions of the input.
     *
     * @param distrib The distribution represented as an addressable matrix of density values.
     * @param center_x The x-coordinate (column) of the center of the non-zero region of the distribution, in matrix
     *   coordinates.
     * @param center_y The y-coordinate (row) of the center of the non-zero region of the distribution, in matrix coordinates.
     * @param radius The radius of the non-zero region of the distribution, measured from the center, in matrix coordinates. All
     *   zero elements can be ignored when specifying this number.
     * @param moments Used to return the Zernike moments, specified in "angle-major, radius-minor" order. Each moment is a
     *   column of the matrix.
     *
     * @return The number of pixels that have non-zero values and were used to compute the moments.
     */
    template <int N, typename T, typename ScalarT>
    intx compute(IAddressableMatrix<T> const & distrib, double center_x, double center_y, double radius,
                 MomentMatrix<N, ScalarT> & moments) const;

  private:
    /** Generate lookup table for moments. */
    void generateBasisLUT() const;

    /**
     * Check if an input element (type T) is zero. This is the generic implementation for multi-channel inputs. The
     * single-channel case is separately specialized.
    */
    template <int N> struct IsZero
    {
      template <typename T> static bool check(T const & t)
      {
        for (int i = 0; i < N; ++i)
          if (t[i] != 0) return false;

        return true;
      }
    };

    /**
     * Add a scaled increment to a moment. This is the generic implementation for multi-channel inputs. The single-channel case
     * is separately specialized.
     */
    template <int N, typename ScalarT> struct Accum
    {
      template <typename T>
      static void add(T const & t, std::complex<double> const & x, typename MomentMatrix<N, ScalarT>::ColXpr acc)
      {
        for (intx i = 0; i < acc.size(); ++i)
        {
          acc[i].real(acc[i].real() + static_cast<ScalarT>(x.real() * t[i]));
          acc[i].imag(acc[i].imag() - static_cast<ScalarT>(x.imag() * t[i]));
        }
      }
    };

    typedef boost::multi_array< std::complex<double>, 4 > LUT;  ///< Lookup table class.

    Options opts;                ///< Set of options.
    mutable LUT lut;             ///< Coefficient lookup table.
    mutable bool lut_generated;  ///< Has the LUT been generated?

}; // class Zernike2

// Specializations for single-channel input.
template <>
struct Zernike2::IsZero<1>
{
  template <typename T> static bool check(T const & t)
  {
    return t != 0;
  }
};

template <typename ScalarT>
struct Zernike2::Accum<1, ScalarT>
{
  template <typename T>
  static void add(T const & t, std::complex<double> const & x, typename MomentMatrix<1, ScalarT>::ColXpr & acc)
  {
    acc[0].real(acc[0].real() + static_cast<ScalarT>(x.real() * t));
    acc[0].imag(acc[0].imag() - static_cast<ScalarT>(x.imag() * t));
  }
};

template <int N, typename T, typename ScalarT>
intx
Zernike2::compute(IAddressableMatrix<T> const & distrib, double center_x, double center_y, double radius,
                  Zernike2::MomentMatrix<N, ScalarT> & moments) const
{
  alwaysAssertM(radius > 0, "Zernike2: Radius must be greater than zero");

  this->generateBasisLUT();

  moments.resize(Eigen::NoChange, numMoments());
  moments.setZero();

  intx ncols = distrib.cols();
  intx nrows = distrib.rows();

  // Don't go outside the specified radius
  intx min_x = std::max(0L,        (intx)std::ceil (center_x - radius));
  intx max_x = std::min(ncols - 1, (intx)std::floor(center_x + radius));

  intx min_y = std::max(0L,        (intx)std::ceil (center_y - radius));
  intx max_y = std::min(nrows - 1, (intx)std::floor(center_y + radius));

  double r_radius = opts.lut_radius / radius;

  std::complex<double> x1, x2, x3;
  double dx, dy, tx, ty;
  intx ix, iy;
  intx count = 0;
  for (intx y = min_y; y <= max_y; ++y)
  {
    for (intx x = min_x; x <= max_x; ++x)
    {
      T const & density = distrib.at(y, x);
      if (!IsZero<N>::check(density))
      {
        dx = x - center_x;
        dy = y - center_y;
        tx = dx * r_radius + opts.lut_radius;
        ty = dy * r_radius + opts.lut_radius;
        ix = (intx)tx;
        iy = (intx)ty;
        dx = tx - ix;
        dy = ty - iy;

        // Summation of basis function
        for (intx p = 0; p < opts.angular_steps; ++p)
        {
          for (intx r = 0; r < opts.radial_steps; ++r)
          {
            x1 = lut[p][r][ix][iy    ] + (lut[p][r][ix + 1][iy    ] - lut[p][r][ix][iy    ]) * dx;
            x2 = lut[p][r][ix][iy + 1] + (lut[p][r][ix + 1][iy + 1] - lut[p][r][ix][iy + 1]) * dx;
            x3 = x1 + (x2 - x1) * dy;

            Accum<N, ScalarT>::add(density, x3, moments.col(p * opts.radial_steps + r));
          }
        }

        count++;
      }
    }
  }

  if (count > 0)
    moments /= (ScalarT)count;

  return count;
}

} // namespace Algorithms
} // namespace Thea

#endif // !defined(THEA_NO_ZERNIKE)

#endif


//var_model.cpp

#include <math.h>
#include <algorithm>
#include <functional>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

#include "var_model.h"

using namespace std;
using namespace boost::accumulators;

//Define container for muliple criteria sorting
struct Container{
                double _return; //returns
                double _prob; //weighted prob,
            };

bool sortByReturn(const Container &lhs, const Container &rhs) { return lhs._return < rhs._return; }

VaR::VaR(double _alpha):alpha(_alpha){}

//copy constructor implementation
VaR::VaR(const VaR& other):
	alpha(other.alpha)
{}

double VaR::getAlpha() const {return alpha;}

void VaR::setAlpha(double _alpha) {alpha = _alpha;}

ParametricVaR::ParametricVaR(double _alpha, bool _logNormal):VaR(_alpha),logNormal(_logNormal)
{}

//copy constructor implementation
ParametricVaR::ParametricVaR(const ParametricVaR& other):
	VaR(other),logNormal(other.logNormal)
{}

double ParametricVaR::NormalVaR(double u, double sigma) const{

		double za;

		za =  boost::math::quantile(snd, getAlpha());

		return (u + sigma * za);
}

double ParametricVaR::LogNormalVaR(double u, double sigma) const {

		double za;

		za =  boost::math::quantile(snd, getAlpha());

		return -(1. - exp(u + sigma * za));
}

RiskMetricsVaR::RiskMetricsVaR(double _alpha, double _lambda, bool _logNormal)
:ParametricVaR(_alpha, _logNormal), lambda(_lambda){}

//copy constructor implementation
RiskMetricsVaR::RiskMetricsVaR(const RiskMetricsVaR& other):
	ParametricVaR(other), lambda(other.lambda)
{}

double RiskMetricsVaR::operator()(double _meanReturn,
                                  double _sigmatminus1, double _returnt) const{

    double sigmat = sqrt(lambda * _sigmatminus1 * _sigmatminus1 + (1. - lambda) * _returnt * _returnt);

	double tmp = logNormal ? LogNormalVaR(_meanReturn, sigmat) : NormalVaR(_meanReturn, sigmat);

	return tmp >= 0 ? 0. : tmp;

}

double RiskMetricsVaR::operator()(double _meanReturn, const Vec& returns) const
{
	double sigmaT = 0.;
    size_t T(returns.size());

    for(size_t i = 1; i < T;++i){
        sigmaT = sqrt(lambda * sigmaT * sigmaT + (1. - lambda) * returns[i] * returns[i]);
    }

	double tmp = logNormal ? LogNormalVaR(_meanReturn, sigmaT) : NormalVaR(_meanReturn, sigmaT);

	return tmp >= 0 ? 0. : tmp;
}

double RiskMetricsVaR::operator()(const Vec& returns) const
{
    double sigmaT = 0.;
    double _meanReturn =0;
    size_t T(returns.size());

    for(size_t i = 1; i < T;++i){
        sigmaT = sqrt(lambda * sigmaT * sigmaT + (1. - lambda) * returns[i] * returns[i]);
        _meanReturn += returns[i];
    }

    _meanReturn =_meanReturn/(T-1);

    double tmp = logNormal ? LogNormalVaR(_meanReturn, sigmaT) : NormalVaR(_meanReturn, sigmaT);

	return tmp >= 0 ? 0. : tmp;
}

GarchVaR::GarchVaR(double _alpha, double _a, double _b,  double _c,bool _logRtns)
:ParametricVaR(_alpha, _logRtns), a(_a), b(_b), c(_c){}

//copy constructor implementation
GarchVaR::GarchVaR(const GarchVaR& other):
	ParametricVaR(other), a(other.a), b(other.b),c(other.c)
{}

double GarchVaR::operator()(double _meanReturn,
                            double _sigmatminus1, double _returntminus1) const{

    double sigmat = sqrt(a + c * _sigmatminus1 * _sigmatminus1 + b * _returntminus1 * _returntminus1);

	double tmp = logNormal ? LogNormalVaR(_meanReturn, sigmat) : NormalVaR(_meanReturn, sigmat);

	return tmp >= 0 ? 0. : tmp;

}

double GarchVaR::operator()(double _meanReturn, const Vec& returns) const
{
	double sigmaT = 0.;

    for(size_t i = 0; i < returns.size();++i)
        sigmaT += pow(c,static_cast<double>(i)) * returns[i] * returns[i];

    sigmaT *= b;
    sigmaT += a/(1.-c);

	double tmp = logNormal ? LogNormalVaR(_meanReturn, sqrt(sigmaT)) : NormalVaR(_meanReturn, sqrt(sigmaT));

	return tmp >= 0 ? 0. : tmp;
}

double GarchVaR::operator()(const Vec& returns) const
{
    double sigmaT = 0.;
    double _meanReturn =0;
    size_t T(returns.size());

    for(size_t i = 0; i < T;++i){
        sigmaT += pow(c,static_cast<double>(i)) * returns[i] * returns[i];
        _meanReturn += returns[i];
    }

    sigmaT *= b;
    sigmaT += a/(1.-c);
    _meanReturn =_meanReturn/(T-1);

	double tmp = logNormal ? LogNormalVaR(_meanReturn, sigmaT) : NormalVaR(_meanReturn, sigmaT);

	return tmp >= 0 ? 0 : tmp;
}

NoneParametricVaR::NoneParametricVaR(double _alpha):VaR(_alpha){}

NoneParametricVaR::NoneParametricVaR(const NoneParametricVaR& other)
:VaR(other)
{}

HistoricalVaR::HistoricalVaR(double _alpha, double _lambda, WeightingScheme _ws)
:NoneParametricVaR(_alpha), lambda(_lambda), ws(_ws){}

//copy constructor implementation
HistoricalVaR::HistoricalVaR(const HistoricalVaR& other):
	NoneParametricVaR(other), lambda(other.lambda), ws(other.ws)
{}

double HistoricalVaR::operator()(double _sigmatminus1,
                                 const Vec& returns) const
{
 	double T = returns.size();

	double alpha(getAlpha());

	unsigned int c(alpha * T);
	c == 0 ? c : c-= 1;

	double tmp;

	int input(ws);

	std::vector<double> _returns(returns);

	switch(input){

	case 0:
        {
            // sort returns find cth arg - Histogram method
            std::sort(_returns.begin(), _returns.end());

            tmp = _returns[c];
        }

        break;

	case 1:
        {
            std::vector<Container> prob(T);

            double a = (1. - lambda)/(1. - pow(lambda, T));

            for(size_t i = 0; i < T; ++i){
                prob[i]._return = _returns[T -1 - i];
                prob[i]._prob = a * pow(lambda, i);
            }

            // Sort Container by return function
            std::sort(prob.begin(), prob.end(),sortByReturn);

            double sumProb(0);

            for(size_t i = 0; i < T; ++i){
                sumProb += prob[i]._prob;

                if(sumProb >= alpha){
                    tmp = prob[i]._return;
                    break;
                }
            }

        }

        break;

    case 2:

        {
            std::vector<double> sigma;

            sigma.push_back(_sigmatminus1 * _sigmatminus1);

            for(size_t i = 1; i < T;++i)
                sigma.push_back(lambda * sigma[i - 1] + (1. - lambda) * returns[i - 1] * returns[i - 1]);

            double sigmaT(sigma.back());

            std::vector<double> _wgthRtns;

            for(size_t i = 0; i < T; ++i){

                if(sigma[i] <= 0) _wgthRtns.push_back(_returns[i]);
                else _wgthRtns.push_back(sqrt(sigmaT / sigma[i]) * _returns[i] );
            }

            // sort returns find cth arg - Histogram method
            std::sort(_wgthRtns.begin(), _wgthRtns.end());

            tmp = _wgthRtns[c];
        }

        break;

    default:
        break;

	}

	return tmp >= 0 ? 0. : tmp;
}

ExtremeValueVaR::ExtremeValueVaR(double _alpha):VaR(_alpha){}

//copy constructor implementation
ExtremeValueVaR::ExtremeValueVaR(const ExtremeValueVaR& other):
	VaR(other)
{}

PoTVaR::PoTVaR(double _u,double _beta, double _xi,double _alpha)
: ExtremeValueVaR(_alpha),xi(_xi),beta(_beta),u(_u){

}

//copy constructor implementation
PoTVaR::PoTVaR(const PoTVaR& other):
	ExtremeValueVaR(other), xi(other.xi), beta(other.beta), u(other.u)
{}

double PoTVaR::operator()(const double& ratio) const
{
	double a = 1./ratio * getAlpha();

	return -(u +  beta * (pow(a,- xi) - 1.)/ xi );
}

double PoTVaR::expectedShortfall(const double& ratio) const
{
	return ((this->operator()(ratio) - (beta - xi * u)))/(1. - xi);

}

double PoTVaR::expectedShortfall(const Vec& returns) const{

	double ratio(0.);

	for(size_t i =0;i < returns.size();++i){
        if(returns[i] < 0)
            if(-returns[i] > u) ratio +=1;
	}

	ratio /= returns.size();
    if(ratio < .05) ratio = .05;

	return this->expectedShortfall(ratio);
}

double PoTVaR::operator()(const Vec& returns) const{

	double ratio(0.);

	for(size_t i =0;i < returns.size();++i){
        if(returns[i] < 0)
            if(-returns[i] > u) ratio +=1;
	}

	ratio /= returns.size();
    if(ratio < .05) ratio = .05;

	return this->operator()(ratio);
}


//
//  MathUtil.cpp
//  edgeRuntime
//
//  Created by Abdelrahaman Aly on 04/02/14.
//
//

//Library Headers
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>

//Custom Headers
#include "List.h"
#include "MathUtil.h"
using namespace NTL;
namespace Utilities
{
    //less significative bit on the left always most significative bit on the right always
    int * MathUtil::obtainBits(long number, int size)
    {
        int * bits= new int[size];
        for (int i=0; i<size; i++)
        {

            if (number&1)
            {

                bits[i]=1;
            }
            else
            {
                bits[i]=0;
            }

            number>>=1;

        }
        return bits;
    };

    //Calculates the lagrangian interpolation. Currently optimized for 3 players
    long MathUtil::lagrangianInterpolation( long * values, int size)
    {
        //TODO: build the lagrangian interpolation

        ZZ_p  result;
        result=2*conv<ZZ_p>(values[0]) -conv<ZZ_p>(values[1]);
        long resultLong =conv<long>(result);
        return  resultLong;
    };

    //calculates the complement 2 adding first aone and then decomposing into bits
    int* MathUtil::obtainComplementTwo(long  value, int size)
    {
        value=~value;
        value++;
        return MathUtil::obtainBits(value, size);
    };

    //obtains the additive of 2 mod values
    long MathUtil::additionMod(long a, long b)
    {
        ZZ_p ap= conv<ZZ_p>(a);
        ZZ_p bp= conv<ZZ_p>(b);
        return conv<long>(a+b);
    };

    //Obteins the multiplicative of 2 mod values
    long MathUtil::multiplyMod(long a, long b)
    {
        ZZ_p ap= conv<ZZ_p>(a);
        ZZ_p bp= conv<ZZ_p>(b);
        return conv<long>(a*b);
    };

    //An specification on generic multiplier of polynomials
    ZZ_pX MathUtil::multiplyLagrangePolynomials(vec_ZZ_p x_v, ZZ_p x_j)
    {

        ZZ_p x_d= x_j-x_v[0];
        ZZ_p a= 1/x_d;
        ZZ_pX p;
        p.SetLength(2);
        p[0]= (-1)*x_v[0]*a;
        p[1]= a;

        for (int k=1; k< x_v.length(); k++)
        {
            ZZ_p x_k =x_v[k];
            x_d= x_j-x_k;
            a= 1/x_d;

            ZZ_pX auxP;
            auxP.SetLength(2);
            auxP[0]= (-1)*x_k*a;
            auxP[1]=a;

            p=p*auxP;
        }
        return p;
    };

    //Interpolations:
    ZZ_pX MathUtil::multiplyLagrangePolynomials(int x_s, int x_k,int x_j)
    {
        vec_ZZ_p values;
        values.SetLength((x_k+1)-x_s -1);
        int j=0;
        for (int i = x_s; i<=x_k; i++)
        {
            if (i!= x_j) {
               values[j]= conv<ZZ_p>(i);
               j++;
            }

        }
        return MathUtil::multiplyLagrangePolynomials(values, conv<ZZ_p>(x_j));
    };

    int MathUtil::pow2roundup (int value)
    {
        int x=value;
        if (x < 0)
            return 0;
        --x;
        x |= x >> 1;
        x |= x >> 2;
        x |= x >> 4;
        x |= x >> 8;
        x |= x >> 16;
        return x+1;
    }
}


#include <Eigen/Eigenvalues>
#include <boost/timer/timer.hpp>
#include <iostream>

#include "eigen-qp.hpp"

using namespace std;
using namespace Eigen;
using namespace EigenQP;
/*
 *  min 0.5 x.Q.x + c.x
 *
 *  Ax <= b
 *  Ex = d
 *
 * See: http://etd.dtu.dk/thesis/220437/ep08_19.pdf
 */
// #define NV_FIXED 8
// #define NC_FIXED 16
// #define NE_FIXED 3

#define PROB_VARS 8
#define PROB_INEQ 16
#define PROB_EQ 3

#define N_TEST   (1<<16)

template<typename Scalar, int NV_FIXED, int NC_FIXED, int NE_FIXED>
void test()
{

    typedef Eigen::Matrix<Scalar,-1,-1> MatrixXs;
    typedef Eigen::Matrix<Scalar,-1,1> VectorXs;

    // Make a random problem
    int num_vars = NV_FIXED;
    int num_ineq = NC_FIXED;
    int num_eq   = NE_FIXED;

    // Random matrices
    MatrixXs Q = MatrixXs::Random(num_vars,num_vars);
    Q *= Q.adjoint()/sqrt(num_vars); // Make it pos def

    VectorXs c = VectorXs::Random(num_vars);

    MatrixXs A = MatrixXs::Random(num_ineq,num_vars);
    VectorXs b = VectorXs::Random(num_ineq);

    MatrixXs E = MatrixXs::Random(num_eq,num_vars);
    VectorXs f = VectorXs::Random(num_eq);

    VectorXs x_unc;
    // Solve unconstrainted system
    cout << "Unconstrained..." << endl;
    {
        boost::timer::auto_cpu_timer t;
        for (int ii=0; ii < N_TEST; ii++)
          x_unc = -Q.ldlt().solve(c);
    }
    VectorXs x(num_vars);
    // Generate inequality constraints
    b.array() = (A*x_unc).array() - 0.15;
    // Inequality constrained problem
    cout << "quadprog, dynamic code" << endl;
    {
        boost::timer::auto_cpu_timer t;
        for (int ii=0; ii < N_TEST; ii++)
            quadprog(Q,c,A,b,x);
    }
    cout << "    error: " << (x - x_unc).norm() << endl;
    {
        QPIneqSolver<Scalar,-1,-1> *solver;
        cout <<"QPIneqSolver, dynamic, obj creation" << endl;
        {
            boost::timer::auto_cpu_timer t;
            for (int ii=0; ii < N_TEST; ii++)
            {
                solver = new QPIneqSolver<Scalar,-1,-1>(num_vars,num_ineq);
                solver->solve(Q,c,A,b,x);
            }
        }
        cout << "    error: " << (x - x_unc).norm() << endl;
        cout << "QPIneqSolver, dynamic, object reuse" << endl;
        {
            boost::timer::auto_cpu_timer t;
            for (int ii=0; ii < N_TEST; ii++)
                solver->solve(Q,c,A,b,x);
        }
        cout << "    error: " << (x - x_unc).norm() << endl;
    }

    // Fixed size
    Matrix<Scalar, NV_FIXED,NV_FIXED> Q_fixed(Q);
    Matrix<Scalar, NV_FIXED, 1> c_fixed(c);
    Matrix<Scalar, NC_FIXED,NV_FIXED> A_fixed(A);
    Matrix<Scalar, NC_FIXED,1> b_fixed(b);

    // Q_fixed.setRandom();
    // c_fixed.setRandom();
    // A_fixed.setRandom();

    x_unc = -Q_fixed.ldlt().solve(c_fixed);

    b_fixed.array() = (A_fixed*x_unc).array() - 0.12;

    Matrix<Scalar, NV_FIXED, 1> x_fixed;
    cout << "quadprog, fixed code" << endl;
    {
        boost::timer::auto_cpu_timer t;
        for (int ii=0; ii < N_TEST; ii++)
            quadprog(Q_fixed,c_fixed,A_fixed,b_fixed,x_fixed);
    }
    cout << "    error: " << (x_fixed - x_unc).norm() << endl;
    {
        QPIneqSolver<Scalar,NV_FIXED,NC_FIXED> *solver;
        cout << "QPIneqSolver, fixed" << endl;
        {
            boost::timer::auto_cpu_timer t;
            for (int ii=0; ii < N_TEST; ii++)
            {
                solver = new QPIneqSolver<Scalar,NV_FIXED,NC_FIXED>(num_vars,num_ineq);
                solver->solve(Q_fixed,c_fixed,A_fixed,b_fixed,x_fixed);
            }
        }
        cout << "    error: " << (x_fixed - x_unc).norm() << endl;
        cout << "QPIneqSolver, fixed, object reuse" << endl;
        {
            boost::timer::auto_cpu_timer t;
            for (int ii=0; ii < N_TEST; ii++)
                solver->solve(Q_fixed,c_fixed,A_fixed,b_fixed,x_fixed);
        }
        cout << "    error: " << (x_fixed - x_unc).norm() << endl;
    }

    cout << "QPEqSolver, equality constraints, dynamic" << endl;
    {
        QPEqSolver<Scalar> solver(num_vars,num_eq);
        {
            boost::timer::auto_cpu_timer t;
            for (int ii=0; ii < N_TEST; ii++)
                solver.solve(Q,c,E,f,x);
        }
    }

    /*
    // known to be broken
    cout << "quadprog, ineq/eq constraints, dynamic" << endl;
    {
        //b = A*x - 0.12;
        QPGenSolver<Scalar> solver(num_vars,num_ineq,num_eq);
        {
            boost::timer::auto_cpu_timer t;
            for (int ii=0; ii < N_TEST; ii++)
                solver.solve(Q,c,A,b,E,f,x);
        }
    }
    */
}

int main(int argc, char ** argv)
{
    srand((unsigned int) time(0));
    cout << "TESTING DOUBLE" << endl;
    test<double,PROB_VARS,PROB_INEQ,PROB_EQ>();
    cout << "TESTING FLOAT" << endl;
    test<float,PROB_VARS,PROB_INEQ,PROB_EQ>();
    return 0;
}


/**
 * \file
 *
 * \author Nicholas J. Curtis
 * \date 04/29/2016
 *
 * \brief Defines an interface for boost's runge_kutta_fehlberg78 solver
 *
*/

#ifndef RK78_TYPEDEFS_HPP
#define RK78_TYPEDEFS_HPP

#include <boost/numeric/odeint.hpp>
using namespace boost::numeric::odeint;

//our code
extern "C" {
	#include "dydt.h"
	#include "header.h"
	#include "solver_options.h"
}

#ifdef GENERATE_DOCS
namespace rk78 {
#endif

//! state vector
typedef std::vector< double > state_type;

//! solver type
typedef runge_kutta_fehlberg78< state_type , double > stepper;

//! controller type
typedef controlled_runge_kutta< stepper > controller;

//stiffness measure for project
//do a binary search to find the maximum available stepsize
#ifdef LOG_OUTPUT
//#define STIFFNESS_MEASURE
#endif
#ifdef STIFFNESS_MEASURE
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#endif

/**
   \brief A wrapper class to evaluate the rhs function y' = f(y)
   stores the state variable, and provides to dydt
*/
class rhs_eval {
	double m_statevar;
public:
	rhs_eval() {
		this->m_statevar = -1;
	}

	void set_state_var(const double state_var)
	{
		this->m_statevar = state_var;
	}

	//wrapper for the pyJac RHS fn
	void operator() (const state_type &y , state_type &fy , const double t) const
	{
		dydt(t, this->m_statevar, &y[0], &fy[0]);
	}
};

#ifdef GENERATE_DOCS
}
#endif

#endif


// Based on minimal example in https://github.com/CGAL/cgal/wiki/How-to-use-CGAL-with-CMake-or-your-own-build-system
// but with Qt dependency removed

#include <CGAL/Exact_predicates_exact_constructions_kernel_with_sqrt.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/helpers.h>

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Boolean_set_operations_2.h>

// Just to ensure that Eigen's CMake configuration is correct
#include <Eigen/Dense>

typedef CGAL::Exact_predicates_exact_constructions_kernel_with_sqrt K;
typedef K::Point_3 Point;
typedef CGAL::Surface_mesh<Point> Mesh;

typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;
typedef Kernel::Point_2                                   Point_2;
typedef CGAL::Polygon_2<Kernel>                           Polygon_2;

int main()
{
  Mesh m;
  CGAL::make_icosahedron<Mesh, Point>(m);

  // This part is based on Boolean_set_operations_2/do_intersect.cpp
  Polygon_2 P;
  P.push_back (Point_2 (-1,1));
  P.push_back (Point_2 (0,-1));
  P.push_back (Point_2 (1,1));
  Polygon_2 Q;
  Q.push_back(Point_2 (-1,-1));
  Q.push_back(Point_2 (1,-1));
  Q.push_back(Point_2 (0,1));

  if ((CGAL::do_intersect (P, Q))) {
    std::cout << "The two polygons intersect in their interior." << std::endl;
    return 0;
  } else {
    std::cout << "The two polygons do not intersect, but should." << std::endl;
    return 1;
  }
}


#include "NuiMatrixUtilities.h"

#include <Eigen/Geometry>

namespace NuiMatrixUtilities
{
	Vector3f rodrigues2(const Eigen::Matrix3f& matrix)
	{
		Eigen::JacobiSVD<Eigen::Matrix3f> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);
		Eigen::Matrix3f R = svd.matrixU() * svd.matrixV().transpose();

		double rx = R(2, 1) - R(1, 2);
		double ry = R(0, 2) - R(2, 0);
		double rz = R(1, 0) - R(0, 1);

		double s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
		double c = (R.trace() - 1) * 0.5;
		c = c > 1. ? 1. : c < -1. ? -1. : c;

		double theta = acos(c);

		if( s < 1e-5 )
		{
			double t;

			if( c > 0 )
				rx = ry = rz = 0;
			else
			{
				t = (R(0, 0) + 1)*0.5;
				rx = sqrt( std::max(t, 0.0) );
				t = (R(1, 1) + 1)*0.5;
				ry = sqrt( std::max(t, 0.0) ) * (R(0, 1) < 0 ? -1.0 : 1.0);
				t = (R(2, 2) + 1)*0.5;
				rz = sqrt( std::max(t, 0.0) ) * (R(0, 2) < 0 ? -1.0 : 1.0);

				if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R(1, 2) > 0) != (ry*rz > 0) )
					rz = -rz;
				theta /= sqrt(rx*rx + ry*ry + rz*rz);
				rx *= theta;
				ry *= theta;
				rz *= theta;
			}
		}
		else
		{
			double vth = 1/(2*s);
			vth *= theta;
			rx *= vth; ry *= vth; rz *= vth;
		}
		return Eigen::Vector3d(rx, ry, rz).cast<float>();
	}
}


//
// Copyright (c) 2012 Juan Palacios juan.palacios.puyana@gmail.com
// This file is part of minimathlibs.
// Subject to the BSD 2-Clause License
// - see < http://opensource.org/licenses/BSD-2-Clause>
//

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestAxisAngle

#include <boost/test/unit_test.hpp>
#include <iostream>
#include <cmath>
#include "minimath/geom3d_ops.hpp"
#include "minimath/point3d.hpp"
#include "minimath/rotation3d.hpp"
#include "TestRotation3DUtils.h"

using minimath::axisangle;
using minimath::rotation3d;
using minimath::rotation3dx;
using minimath::rotation3dy;
using minimath::rotation3dz;

BOOST_AUTO_TEST_SUITE(TestAxisAngle)

BOOST_AUTO_TEST_CASE(testInstantiation)
{
  axisangle<double> rot1, rot2;
}

BOOST_AUTO_TEST_CASE(testDefaultEquality)
{
  BOOST_CHECK(axisangle<double>() == axisangle<double>());
}

BOOST_AUTO_TEST_CASE(testCopyConstruction)
{
  axisangle<double> rot1;
  axisangle<double> rot2(rot1);
  BOOST_CHECK(rot1 == rot2);
}

BOOST_AUTO_TEST_CASE(testAssignment)
{
  axisangle<double> rot1, rot2;
  rot2 = rot1;
  BOOST_CHECK(rot1 == rot2);
}

BOOST_AUTO_TEST_CASE(testNullRotation)
{
  axisangle<double> rot;
  pointxyzd pTest = rotation3d<double>(rot)*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rotation3d<double>(rot)*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rotation3d<double>(rot)*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint45X)
{
  axisangle<double> rot(p100, PI/4.); // 45 degree rotation about X
  pointxyzd pTest = rotation3d<double>(rot)*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rotation3d<double>(rot)*p010;
  BOOST_CHECK(pTest == pointxyzd(0., cos45, cos45));

  pTest = rotation3d<double>(rot)*p001;
  BOOST_CHECK(pTest == pointxyzd(0., -cos45, cos45));
}

BOOST_AUTO_TEST_CASE(testRotatePoint90X)
{
  axisangle<double> rot(p100, PI/2.); // 90 degree rotation about X
  pointxyzd pTest = rotation3d<double>(rot)*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rotation3d<double>(rot)*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));

  pTest = rotation3d<double>(rot)*p001;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint180X)
{
  axisangle<double> rot(p100, PI); // 180 degree rotation about X

  pointxyzd pTest = rotation3d<double>(rot)*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rotation3d<double>(rot)*p010;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));

  pTest = rotation3d<double>(rot)*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint45Y)
{
  axisangle<double> rot(p010, PI/4.); // 45 degree rotation about Y
  pointxyzd pTest = rotation3d<double>(rot)*p100;
  BOOST_CHECK(pTest == pointxyzd(cos45, 0., -cos45));

  pTest = rotation3d<double>(rot)*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0));

  pTest = rotation3d<double>(rot)*p001;
  BOOST_CHECK(pTest == pointxyzd(cos45, 0, cos45));
}

BOOST_AUTO_TEST_CASE(testRotatePoint90Y)
{
  axisangle<double> rot(p010, PI/2.); // 90 degree rotation about Y
  pointxyzd pTest = rotation3d<double>(rot)*p100;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));

  pTest = rotation3d<double>(rot)*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rotation3d<double>(rot)*p001;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint180Y)
{
  axisangle<double> rot(p010, PI); // 180 degree rotation about Y

  pointxyzd pTest = rotation3d<double>(rot)*p100;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = rotation3d<double>(rot)*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rotation3d<double>(rot)*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint45Z)
{
  axisangle<double> rot(p001, PI/4.); // 45 degree rotation about Z
  pointxyzd pTest = rotation3d<double>(rot)*p100;
  BOOST_CHECK(pTest == pointxyzd(cos45, cos45, 0.));

  pTest = rotation3d<double>(rot)*p010;
  BOOST_CHECK(pTest == pointxyzd(-cos45, cos45, 0));

  pTest = rotation3d<double>(rot)*p001;
  BOOST_CHECK(pTest == pointxyzd(0, 0, 1));
}

BOOST_AUTO_TEST_CASE(testRotatePoint90Z)
{
  axisangle<double> rot(p001, PI/2.); // 90 degree rotation about Z
  pointxyzd pTest = rotation3d<double>(rot)*p100;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rotation3d<double>(rot)*p010;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = rotation3d<double>(rot)*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint180Z)
{
  axisangle<double> rot(p001, PI); // 180 degree rotation about Z

  pointxyzd pTest = rotation3d<double>(rot)*p100;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = rotation3d<double>(rot)*p010;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));

  pTest = rotation3d<double>(rot)*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testAssignRotation3DX)
{
  for (int i = 1; i < 9; ++i) {
    BOOST_CHECK(axisangle<double>(p100, PI/i) == rotation3dx<double>(PI/i));
  }
}

BOOST_AUTO_TEST_CASE(testAssignRotation3DY)
{
  for (int i = 1; i < 9; ++i) {
    BOOST_CHECK(axisangle<double>(p010, PI/i) == rotation3dy<double>(PI/i));
  }
}

BOOST_AUTO_TEST_CASE(testAssignRotation3DZ)
{
  for (int i = 1; i < 9; ++i) {
    BOOST_CHECK(axisangle<double>(p001, PI/i) == rotation3dz<double>(PI/i));
  }
}

BOOST_AUTO_TEST_CASE(testInverse)
{
  BOOST_CHECK(TestUtils::testInverseAxisAngle(p100));
  BOOST_CHECK(TestUtils::testInverseAxisAngle(p010));
  BOOST_CHECK(TestUtils::testInverseAxisAngle(p001));
}

BOOST_AUTO_TEST_CASE(testInvert)
{
  BOOST_CHECK(TestUtils::testInvertAxisAngle(p100));
  BOOST_CHECK(TestUtils::testInvertAxisAngle(p010));
  BOOST_CHECK(TestUtils::testInvertAxisAngle(p001));
}

BOOST_AUTO_TEST_SUITE_END()


#include "gtest/gtest.h"

#include <Eigen/Dense>

TEST (Eigen, dotProduct) {
  Eigen::Vector3d v {0.1, 0.2, 0.3};
  double expected = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
  ASSERT_EQ(v.norm(), expected);
}


#ifndef EXPSUM_LSQR_HPP
#define EXPSUM_LSQR_HPP

#include <armadillo>

namespace expsum
{
///
/// lsqr
///
/// `lsqr` solves a least squares problem
///
/// ``` math
///   \min \| A\bm{x} - \bm{b} \|_{2},
/// ```
///
/// where ``$A \in \mathcal{C}^{m \times n}, \, \bm{b} \in \mathcal{C}^{n} $``
/// with ``$m \geq n$``.
///
/// @param[in]  matvec A function object that compute matrix-vector product
///   `y += A * x`
/// @param[in]  matvec_trans A function object that compute matrix-vector
///   product `y += A.t() * x`
/// @param[inout] u A vector of length m. On input the right hand side vector
///     ``$b$``, on exit, `u` is overwritten.
/// @param[out] x A vector of length n. On input and initial guess of solution,
///    on exit, the computed solution.
/// @param[in] precond_solve Functor for applying preconditioner to a vector.
/// @param[out] work A matrix of size ``$n \times 3$`` used as workspace.
/// @param[inout] iterations  On input the max number of iteration, on exit the
///    number of performed iterations.
/// @param[inout] tol_error  On input the tolerance error, on exit as estimation
///    of the relative error.
///

template <typename MatVec, typename MatVecTrans, typename VectorU,
          typename VectorX, typename Preconditioner, typename MatrixWork>
void lsqr(MatVec matvec, MatVecTrans matvec_trans, VectorU& u, VectorX& x,
          const Preconditioner& apply_preconditioner, MatrixWork& work,
          arma::uword& iterations, typename VectorX::pod_type& tol_error)
{
    using value_type = typename VectorX::elem_type;
    using real_type  = typename VectorX::pod_type;
    using size_type  = arma::uword;

    using std::sqrt;
    using std::real;
    using std::abs;
    using std::norm;

    const real_type atol = tol_error;
    const real_type btol = tol_error;

    auto p = work.col(0);
    auto v = work.col(1);
    auto w = work.col(2);
    //
    // --- Initialization
    //
    x.zeros();
    real_type alpha = real_type();
    real_type beta  = arma::norm(u);
    if (beta > real_type())
    {
        u *= real_type(1) / beta;         // normalize
        matvec_trans(u, value_type(), p); // p <-- A.t() * u;
        apply_preconditioner(p, v);       // v <-- M.inv() * p
        alpha = sqrt(real(arma::cdot(v, p)));
        if (alpha > real_type())
        {
            v *= real_type(1) / alpha; // normalize
            w = v;
        }
    }

    // Estimation of the norm of `A.t() * r`
    real_type norm_Atr = alpha * beta;
    if (norm_Atr == real_type())
    {
        // x = 0 is the exact solution
        return;
    }

    real_type rhobar = alpha;
    real_type phibar = beta;

    // Estimations of the Frobenius norm of matrix A
    real_type norm_A = real_type();
    // Norm of r.h.s. vector b
    real_type norm_b = beta;
    // Estimation of the norm of residual vector `r = b - A * x`.
    real_type norm_r = beta;
    // Estimation of the norm of solution vector x.
    real_type norm_x = real_type();

    real_type cs2     = real_type(-1);
    real_type sn2     = real_type();
    real_type z       = real_type();
    real_type norm_bb = real_type();
    real_type norm_xx = real_type();

    size_type iter = 0;
    while (++iter <= iterations)
    {
        //
        // --- Bidiagonalization
        //
        // Perform the next step of the bidiagonalization with M-weighted inner
        // product.  `beta, u, alpha, v`. These satisfy the relations
        //
        // ``` math
        //  \beta_{i+1} \bm{u}_{i+1}
        //    &= A \bm{v}_{i} - \alpha_{i} \bm{u}_{i},
        //  \alpha_{i+1} \bm{v}_{i+1}
        //    &= M^{-1} A^{H} \bm{u}_{i+1} - \beta_{i+1} \bm{v}_{i},
        //  \alpha_{i+1} &= (\bm{v}_{i+1}^{H} M \bm{v}_{i+1}^{})^{1/2}
        // ```
        //
        matvec(v, -alpha, u); // u <-- A * v - alpha * u
        beta = arma::norm(u);
        norm_bb += norm(alpha) + norm(beta);

        if (beta > real_type())
        {
            u *= real_type(1) / beta;
            matvec_trans(u, -beta, p);  // p <-- A.t() * u - beta * p
            apply_preconditioner(p, v); // v <-- M.inv() * p
            alpha = sqrt(real(arma::cdot(v, p)));
            if (alpha > real_type())
            {
                v *= real_type(1) / alpha;
            }
        }
        //
        // --- Orthogonal transformation
        //
        // Construct and apply next orthogonal transformation (plane rotation)
        // to eliminate the subdiagonal element (beta) of the lower-bidiagonal
        // matrix, giving an upper-bidiagonal matrix. The explicit form of the
        // transformation is given as
        //
        // [cs  sn][rhobar      0  phibar]   [rho   theta     phi ]
        // [sn -cs][  beta  alpha       0] = [  0  rhobar' phibar']
        //
        real_type rho   = sqrt(norm(rhobar) + norm(beta));
        real_type cs    = rhobar / rho;
        real_type sn    = beta / rho;
        real_type theta = sn * alpha;
        rhobar          = -cs * alpha;
        real_type phi   = cs * phibar;
        phibar *= sn;
        //
        // Update vectors
        //
        // norm_dd += (w / rho).squaredNorm();
        x += (phi / rho) * w;
        w = v - (theta / rho) * w;
        //
        // Estimate norm of x using the result of plane rotation
        //
        real_type delta     = sn2 * rho;
        real_type gamma_bar = -cs2 * rho;
        real_type rhs       = phi - delta * z;
        real_type zbar      = rhs / gamma_bar;
        norm_x              = sqrt(norm_xx + norm(zbar));
        real_type gamma     = sqrt(norm(gamma_bar) + norm(theta));
        cs2                 = gamma_bar / gamma;
        sn2                 = theta / gamma;
        z                   = rhs / gamma;
        norm_xx += norm(z);

        //
        // Test for convergence.
        //
        norm_A = sqrt(norm_bb);
        // cond_A   = norm_A * sqrt(norm_dd);
        norm_r   = phibar;
        norm_Atr = phibar * alpha * abs(cs);
        //
        // Stop iteration if
        //
        // |A^{T} \bm{r}|_{F} / |A^{T}|_F |\bm{r}| < tol
        //
        // if (norm_Atr <= norm_A * norm_r * tol_error)
        // {
        //     break;
        // }
        if (norm_r <= btol * norm_b + atol * norm_A * norm_x)
        {
            break;
        }
    }

    tol_error  = norm_r;
    iterations = iter;
}

} // namespace expsum

#endif /* EXPSUM_LSQR_HPP */


#include <iostream>
#include <iomanip>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/constants/constants.hpp>
typedef boost::multiprecision::cpp_dec_float_50 decfloat;

int main()
{
    const decfloat ln_two = boost::math::constants::ln_two<decfloat>();
    decfloat numerator = 1, denominator = ln_two;

    for(int n = 1; n <= 17; n++) {
        decfloat h = (numerator *= n) / (denominator *= ln_two) / 2;
        decfloat tenths_dig = floor((h - floor(h)) * 10);
        std::cout << "h(" << std::setw(2) << n << ") = " << std::setw(25) << std::fixed << h <<
            (tenths_dig == 0 || tenths_dig == 9 ? " is " : " is NOT ") << "an almost-integer.\n";
    }
}


/**
 * @file uniformcubicspline.cc
 * @brief NPDE exam problem summer 2019 "CLEmpiricFlux" code
 * @author Oliver Rietmann
 * @date 18.07.2019
 * @copyright Developed at ETH Zurich
 */

#include "uniformcubicspline.h"

#include <Eigen/Core>
#include <cassert>

namespace {

template <typename T>
constexpr T Square(T x) {
  return x * x;
}

template <typename T>
constexpr T Cube(T x) {
  return x * x * x;
}

constexpr int getJ(double a, double b, unsigned int n, double u) {
  return u < b ? (int)(n * ((u - a) / (b - a)) + 1.0) : n;
}

constexpr double zeta(double a, double b, unsigned int n, double j) {
  return a + j * (b - a) / n;
}

}  // namespace

namespace CLEmpiricFlux {

UniformCubicSpline::UniformCubicSpline(double a, double b, Eigen::VectorXd f,
                                       Eigen::VectorXd M)
    : _n(f.size() - 1), _a(a), _b(b), _f(std::move(f)), _M(std::move(M)) {
  assert(b >= a);
  assert(_f.size() >= 2);
  assert(_f.size() == _M.size());
}

double UniformCubicSpline::operator()(double u) const {
  assert((u >= _a) && (u <= _b));
  int j = getJ(_a, _b, _n, u);
  double h = (_b - _a) / _n;
  double tau = (u - zeta(_a, _b, _n, j - 1)) / h;

  return _f(j) * tau + _f(j - 1) * (1.0 - tau) +
         Square(h) / 6.0 *
             (_M(j) * (Cube(tau) - tau) +
              _M(j - 1) * (-Cube(tau) + 3.0 * Square(tau) - 2.0 * tau));
}

double UniformCubicSpline::derivative(double u) const {
  assert((u >= _a) && (u <= _b));
  int j = getJ(_a, _b, _n, u);
  double h = (_b - _a) / _n;
  double tau = (u - zeta(_a, _b, _n, j - 1)) / h;

  return (_f(j) - _f(j - 1) +
          Square(h) / 6.0 *
              (_M(j) * (3.0 * Square(tau) - 1.0) +
               _M(j - 1) * (-3.0 * Square(tau) + 6.0 * tau - 2.0))) *
         (1.0 / h);
}

}  // namespace CLEmpiricFlux


/*
	The code below is a test case for a compressive sensing reconstruction algorithm applied to a single image from the MNIST dataset.
	Eigen template library is used for easier matrix handling. URL: http://eigen.tuxfamily.org/index.php?title=Main_Page
								... for some examples see: eigen_testing.cpp
	Written by: Karlo Sever
*/

//#include "images_training_set.h"		// MNIST dataset
//#include "labels_training_set.h"

#include <iostream>
#include "test_img.h"					// Digit: 0
#include "eig_vect_matrix.h"			// Eigenvectors taken from the '0' dictionary
#include "measurement_matrix.h"

#include <Eigen/Dense>

using namespace Eigen;

#define DIM 784

//Test image of digit '0' is of dimensions X_test[784,1] stored in X_test_img[784]
//Measurement matrix is of dimensions phi[78,784] stored in phi_vect[78*784]
//Measured signal y = phi*X_test is of dimensions [78,1]

int main(void) {
	VectorXd X_test(DIM);
	for (int i = 0; i < DIM; i++)
		X_test(i) = X_test_img[i];

	MatrixXd phi(78, DIM);
	int indx = 0;
	for (int i = 0; i < 78; i++) {
		for (int j = 0; j < DIM; j++) {
			phi(i, j) = phi_vect[indx++];
			printf("%.6f", phi(i, j));
		}
		printf("\n");
	}

	MatrixXd v0(DIM, DIM);
	indx = 0;
	for (int i = 0; i < DIM; i++) {
		for (int j = 0; j < DIM; j++) {
			v0(i, j) = eigv[indx++];
		}
	}

	for (int i = 0; i < DIM; i++) {
		printf("%6.2f", X_test[i]);
		if (i % 28 == 0)
			printf("\n");
	}
	std::cout << "\nMeasured signal y... \n" << phi * X_test << std::endl;

	//MatrixXd A(DIM, DIM), B(DIM, 78);
	//A = v0.transpose()*phi.transpose()*phi*v0;
	//B = v0.transpose()*phi.transpose();
	//alpha_rec = A.inverse()*B*(phi*X_test);

	VectorXd alpha_rec(DIM), X_rec(DIM), recon(DIM);
	X_rec = phi.transpose()*(phi*X_test);
	alpha_rec = v0.transpose()*X_rec;					// Reconstruction coefficients

	recon = v0 * alpha_rec;								// Reconstructed data
	std::cout << "\n\nReconstructed data...\n" << std::endl;
	for (int i = 0; i < DIM; i++) {
		printf("%6.2f ", recon[i]);
		if (i % 28 == 0) {
			printf("\n");
		}
	}

	return 0;
}


#include <Rcpp.h>

//[[Rcpp::depends(RcppEigen)]]
#include <RcppEigen.h>

//[[Rcpp::depends(BH)]]
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/digamma.hpp>

#include "logLikelihoods.hpp"
#include "AuxiliaryFunctions.hpp"

double logPoissonGammaDistribution(const double & x, const double & mean, const double & dispersion)
{
    double eta = dispersion;
    if (!(dispersion > 0))
    {
        eta = 2e-16;
    }

    const double logProbability = boost::math::lgamma(x + eta) - boost::math::lgamma(eta) -
        boost::math::lgamma(x + 1) + x * (std::log(mean) - std::log(mean + eta)) + eta * (std::log(eta) -
        std::log(mean + eta));

    return logProbability;
}

double logInflatedTruncatedPoissonGammaDistribution(const double & x, const double & mean, const double & dispersion,
                                                    const double & p, const double & inflation, const double & truncation)
{
    const double logTruncatedProbability = (logPoissonGammaDistribution(x, mean, dispersion) -
        std::log(1.0 - std::exp(dispersion * (std::log(dispersion) - std::log(mean + dispersion)))));

    double logProbability;
    if (x == inflation)
    {
        logProbability = std::log(p + std::exp(std::log(1.0 - p) + logTruncatedProbability));
    }
    else
    {
        logProbability = std::log(1.0 - p) + logTruncatedProbability;
    }

    return logProbability;
}

//[[Rcpp::export(.devianceResidualPoissonGammaDistribution)]]
double devianceResidualPoissonGammaDistribution(const double & x, const double & mean, const double & dispersion)
{
	// if (std::abs(x - mean) < 2e-8)
	// {
	// 	return 0.0;
	// }

	double eta = dispersion;
	if (!(dispersion > 0))
	{
	    eta = 2e-16;
	}

	double deviance_ma = (x + eta) * (std::log(mean + eta) - std::log(x + eta));
    if (x > 0)
    {
        deviance_ma += x * (std::log(x) - std::log(mean));
    }

    return boost::math::sign(x - mean) * std::pow(2.0 * deviance_ma, 0.5);
}

double devianceResidualPG1(const double & x, const double & mean, const double & dispersion)
{
    double eta = dispersion;
    if (!(dispersion > 0))
    {
        eta = 2e-16;
    }

    double deviance_ma = (x - mean) * std::log(eta + 1) / eta -
        boost::math::lgamma((eta * x + mean) / eta) +
        boost::math::lgamma(x * (eta + 1) / eta);

    if (x > 0)
    {
        deviance_ma += boost::math::lgamma(x / eta) - boost::math::lgamma(mean / eta);
    }

    return boost::math::sign(x - mean) * std::pow(2.0 * deviance_ma, 0.5);
}

double logMultinomialCoefficient(const int & totalCounts, const Eigen::VectorXd & counts)
{
    std::size_t N = counts.size();

    double logGammaCounts = 0;
    for (std::size_t n = 0; n < N; n++)
    {
        logGammaCounts += lgamma(counts[n] + 1);
    }
    return lgamma(totalCounts + 1) - logGammaCounts;
}

double logDirichletMultinomial(const Eigen::VectorXd & counts, const Eigen::VectorXd & alleleFrequencies)
{
    std::size_t N = alleleFrequencies.size();
    int sumCounts = counts.sum();

    double groupSum = 0.0;
    for (std::size_t n = 0; n < N; n++)
    {
        groupSum += counts[n] * std::log(alleleFrequencies[n]);

    }

    double resLogDirichletMultinomial = logMultinomialCoefficient(sumCounts, counts) + groupSum;
    return resLogDirichletMultinomial;
}

double logDirichletMultinomialTheta(const Eigen::VectorXd & counts, const double & theta, const Eigen::VectorXd & alleleFrequencies)
{
    std::size_t N = alleleFrequencies.size();
    int sumCounts = counts.sum();

    double groupSum = 0.0;
    double gamma_plus = 1/theta - 1;
    for (std::size_t n = 0; n < N; n++)
    {
        double gamma_j = gamma_plus * alleleFrequencies[n];
        groupSum += lgamma(counts[n] + gamma_j) - lgamma(gamma_j);
    }

    groupSum += (lgamma(gamma_plus) - lgamma(sumCounts + gamma_plus));

    double resLogDirichletMultinomial = logMultinomialCoefficient(sumCounts, counts) + groupSum;
    return resLogDirichletMultinomial;
}

Eigen::VectorXd logLikelihoodAlleleCoverage(const Eigen::VectorXd & coverage, const std::vector<Eigen::MatrixXd> & expectedContributionMatrix,
                                            const std::vector<Eigen::VectorXd> & alleleIndex, const Eigen::VectorXd partialSumAlleles,
                                            const Eigen::VectorXd & sampleParameters, const Eigen::VectorXd & mixtureProportions,
                                            const Eigen::VectorXd & markerImbalances)
{
    const std::size_t & M = alleleIndex.size();

    const double & referenceMarkerAverage = sampleParameters[0];
    const double & dispersion = sampleParameters[1];

    Eigen::VectorXd logLikelihood = Eigen::VectorXd::Zero(M);
    for (std::size_t m = 0; m < M; m++)
    {
        const Eigen::VectorXd & alleleIndex_m = alleleIndex[m];
        const Eigen::MatrixXd & expectedContributionMatrix_m = expectedContributionMatrix[m];
        const Eigen::VectorXd & expectedContribution = expectedContributionMatrix_m * mixtureProportions;

        double logLikelihood_m = 0.0;
        for (std::size_t a = 0; a < alleleIndex_m.size(); a++)
        {
            const std::size_t & n = partialSumAlleles[m] + alleleIndex_m[a];

            double mu_ma = referenceMarkerAverage * markerImbalances[m] * expectedContribution[a];
            if (expectedContribution[a] > 0)
            {
                logLikelihood_m += logPoissonGammaDistribution(coverage[n], mu_ma, mu_ma / dispersion);
            }
        }

        logLikelihood[m] = logLikelihood_m;
    }

    return logLikelihood;
}

Eigen::VectorXd logLikelihoodNoiseCoverage(const Eigen::VectorXd & coverage, const std::vector<Eigen::VectorXd> & noiseIndex,
                                           const Eigen::VectorXd & partialSumAlleles,
                                           const double & noiseLevel, const double & noiseDispersion, const double & p)
{
    const std::size_t & M = noiseIndex.size();

    Eigen::VectorXd logLikelihood = Eigen::VectorXd::Zero(M);
    for (std::size_t m = 0; m < M; m++)
    {
        const Eigen::VectorXd & noiseIndex_m = noiseIndex[m];
        for (std::size_t a = 0; a < noiseIndex_m.size(); a++)
        {
            const std::size_t & n = partialSumAlleles[m] + noiseIndex_m[a];
            logLikelihood[m] += logInflatedTruncatedPoissonGammaDistribution(coverage[n], noiseLevel, noiseDispersion, p, 1.0, 0.0);
        }
    }

    return logLikelihood;
}

Eigen::VectorXd logGenotypeProbabilityHWE(const Eigen::VectorXd & alleleFrequencies, const Eigen::MatrixXd & unknownProfiles, const std::size_t & numberOfMarkers, const Eigen::VectorXd & numberOfAlleles)
{
    Eigen::VectorXd partialNumberOfAlleles = partialSumEigen(numberOfAlleles);

    Eigen::VectorXd logPriorProbability = Eigen::VectorXd::Zero(numberOfMarkers);
    for (std::size_t m = 0; m < numberOfMarkers; m++)
    {
        Eigen::MatrixXd profiles_m = unknownProfiles.block(partialNumberOfAlleles[m], 0, numberOfAlleles[m], unknownProfiles.cols());

        Eigen::VectorXd profileCounts_m = profiles_m.rowwise().sum();
        Eigen::VectorXd alleleFractions_m = alleleFrequencies.segment(partialNumberOfAlleles[m], numberOfAlleles[m]);

        logPriorProbability[m] = logDirichletMultinomial(profileCounts_m, alleleFractions_m);
    }

    return logPriorProbability;
}

Eigen::VectorXd logGenotypeProbabilityThetaCorrection(const Eigen::VectorXd & alleleFrequencies, const double & theta, const Eigen::MatrixXd & unknownProfiles, const Eigen::MatrixXd & knownProfiles, const std::size_t & numberOfMarkers, const Eigen::VectorXd & numberOfAlleles)
{
    Eigen::MatrixXd profiles = bindColumns(knownProfiles, unknownProfiles);
    std::size_t numberOfContributors = profiles.cols();
    std::size_t numberOfKnownContributors = numberOfContributors - unknownProfiles.cols();

    Eigen::VectorXd partialNumberOfAlleles = partialSumEigen(numberOfAlleles);
    Eigen::VectorXd thetaCorrectedProbabilty = Eigen::VectorXd::Zero(numberOfMarkers);
    for (std::size_t m = 0; m < numberOfMarkers; m++)
    {
        Eigen::MatrixXd profiles_m = profiles.block(partialNumberOfAlleles[m], 0, numberOfAlleles[m], numberOfContributors);

        Eigen::VectorXd profileCounts = profiles_m.rowwise().sum();
        Eigen::VectorXd alleleFractions_m = alleleFrequencies.segment(partialNumberOfAlleles[m], numberOfAlleles[m]);

        double logProbabilityOfUnknownProfiles = logDirichletMultinomialTheta(profileCounts, theta, alleleFractions_m);
        double logProbabilty_m = logProbabilityOfUnknownProfiles;

        if (numberOfKnownContributors != 0)
        {
            Eigen::MatrixXd knownProfiles_m = knownProfiles.block(partialNumberOfAlleles[m], 0, numberOfAlleles[m], numberOfKnownContributors);

            Eigen::VectorXd knownCounts = knownProfiles_m.rowwise().sum();

            double logProbabilityOfKnownProfiles = logDirichletMultinomialTheta(knownCounts, theta, alleleFractions_m);
            logProbabilty_m -= logProbabilityOfKnownProfiles;
        }

        thetaCorrectedProbabilty[m] = logProbabilty_m;
    }

    return thetaCorrectedProbabilty;
}

Eigen::VectorXd logPriorGenotypeProbability(const Eigen::VectorXd & alleleFrequencies, const double & theta, const Eigen::MatrixXd & unknownProfiles, const Eigen::MatrixXd & knownProfiles, const std::size_t & numberOfMarkers, const Eigen::VectorXd & numberOfAlleles)
{
    Eigen::VectorXd logPriorProbability;
    if (theta == 0.0)
    {
        logPriorProbability = logGenotypeProbabilityHWE(alleleFrequencies, unknownProfiles, numberOfMarkers, numberOfAlleles);
    }
    else
    {
        logPriorProbability = logGenotypeProbabilityThetaCorrection(alleleFrequencies, theta, unknownProfiles, knownProfiles, numberOfMarkers, numberOfAlleles);
    }

    return logPriorProbability;
}