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

typedef boost::numeric::ublas::matrix<float> Matrix44;
typedef boost::numeric::ublas::identity_matrix<float> IdentityMtx44;

Matrix44 transposeMtx(const Matrix44 &matrix)
{
	Matrix44 matrix_ = IdentityMtx44(4);

	matrix_(0, 0) = matrix(0, 0);
	matrix_(0, 1) = matrix(1, 0);
	matrix_(0, 2) = matrix(2, 0);
	matrix_(0, 3) = matrix(3, 0);

	matrix_(1, 0) = matrix(0, 1);
	matrix_(1, 1) = matrix(1, 1);
	matrix_(1, 2) = matrix(2, 1);
	matrix_(1, 3) = matrix(3, 1);

	matrix_(2, 0) = matrix(0, 2);
	matrix_(2, 1) = matrix(1, 2);
	matrix_(2, 2) = matrix(2, 2);
	matrix_(2, 3) = matrix(3, 2);

	matrix_(3, 0) = matrix(0, 3);
	matrix_(3, 1) = matrix(1, 3);
	matrix_(3, 2) = matrix(2, 3);
	matrix_(3, 3) = matrix(3, 3);

	return matrix_;
}

int main()
{
	Matrix44 matrix(4, 4);

	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			matrix(i, j) = 4 * i + j;
		}
	}

	std::cout << matrix << std::endl;

	matrix = transposeMtx(matrix);

	std::cout << matrix << std::endl;

	return 0;
}


/**
 * @file matode_main.cc
 * @brief NPDE homework MatODE code
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Dense>
#include <iomanip>
#include <iostream>

#include "matode.h"

int main(int /*argc*/, char** /*argv*/) {
  /* SAM_LISTING_BEGIN_6 */
  double h = 0.01;  // stepsize
  Eigen::Vector3d norms;
  // Build M
  Eigen::Matrix3d M;
  M << 8, 1, 6, 3, 5, 7, 9, 9, 2;
  // Build A
  Eigen::Matrix3d A;
  A << 0, 1, 1, -1, 0, 1, -1, -1, 0;
  Eigen::MatrixXd I = Eigen::Matrix3d::Identity();

  auto QR = M.householderQr();

  Eigen::Matrix3d Q = QR.householderQ() * Eigen::Matrix3d::Identity();

  Eigen::Matrix3d explicit_euler, implicit_euler, implicit_midpoint;

  explicit_euler = Q;
  implicit_euler = Q;
  implicit_midpoint = Q;

	std::cout << (explicit_euler * explicit_euler.transpose() - I).norm() << "\t"
	          <<  (implicit_euler * implicit_euler.transpose() - I).norm() << "\t"
	          <<  (implicit_midpoint * implicit_midpoint.transpose() - I).norm() << std::endl;

	std::cout << 0 << "\t" <<(explicit_euler * explicit_euler.transpose() - I).norm() << "\t"
	          <<  (implicit_euler * implicit_euler.transpose() - I).norm() << "\t"
	          <<  (implicit_midpoint * implicit_midpoint.transpose() - I).norm() << std::endl;

  for(int i = 0; i < 20; i++) {
	explicit_euler = MatODE::eeulstep(A, explicit_euler, h);
	implicit_euler = MatODE::ieulstep(A, implicit_euler, h);
	implicit_midpoint = MatODE::impstep(A, implicit_midpoint, h);

	std::cout << i + 1 << "\t" <<(explicit_euler * explicit_euler.transpose() - I).norm() << "\t"
				<<  (implicit_euler * implicit_euler.transpose() - I).norm() << "\t"
				<<  (implicit_midpoint * implicit_midpoint.transpose() - I).norm() << std::endl;

  }

  /* SAM_LISTING_END_6 */
  return 0;
}


#include <gnuplot-iostream/gnuplot-iostream.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <iomanip>
#include <functional>
#include <vector>
#include <cmath>
#include "Tools.hpp"
using namespace std;

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

This program is for the first case of 1-Dimensional drag. The Euler1D
class is generalized to keep track of position and velocity. It accepts
a lambda as a generic euler step function. The only variable it depends
on is dt, generally used to change the accuracy and computation time.

The main function creates a Euler1D object called point with an initial
velocity of v_i and and an initial position (x) of 0. The main loop
increments t by dt each iteration, saving the x, v, and t in vectors and
applying the euler step. The program then plots v vs t and x vs t in
files VelTime.png and PosTime.png respectively. Then, the program uses
a few functions from Tools.hpp to find the percent error of the terminal
velocity and the rising and falling times of the object.

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

const double v_i = 5;			// Initial velocity of the object
const double g = 9.8;			// Gravitational acceleration (negative)
const double b = 1;				// Generalized drag constant
const double dt = 0.001;//0.255;		// Time step
const double SIM_TIME = 2;		// Time to run the simulation (Should be large enough to let v = v_t)

function<double(double,double)> dragStep = [](double x, double v){ return -g-b*v*abs(v); };	// This is the lambda function used as the euler step. The type isn't auto because\
	// it's passed as an argument to the Euler1D::update fuction and the compiler has to know parameter type.

class Euler1D{						// Euler1D definition
public:
	Euler1D(double x_0, double v_0);// Constructor
	void update(function<double(double,double)>& f);// Euler step and updating x
	double getX() const;	// Accessor methods
	double getV() const;
private:
	double x;	// Instance variables
	double v;
};

Euler1D::Euler1D(double x_0, double v_0){	// Constructor just initializes position and velocity
	x = x_0;
	v = v_0;
}

void Euler1D::update(function<double(double,double)>& f){	// Reference to Euler step lambda as argument for update function
	double eulerStep = f(x,v);	// Calculate the step before updating anything
	x += v*dt;					// Update x
	v += eulerStep*dt;			// Update v
}

double Euler1D::getX() const { return x; }	// Accessor methods for x and v
double Euler1D::getV() const { return v; }

void plotStuff(vector<double> x, vector<double> v, vector<double> t){	// Plots the velocity v. time and position v. time graphs
	Gnuplot gp;	// Instance of gnuplot terminal stream

	gp << setprecision(3);	// Velocity v. time graph...
	gp << "set xrange [0:" << SIM_TIME << "]\n";
	gp << "set yrange [" << v[0] << ":" << getMinVal(v)*1.1 << "]\n";
	gp << "set format y \"%.1f\"\n";
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set xlabel \"t (s)\"\n";
	gp << "set ylabel \"v (m/s)\"\n";
	gp << "set title \"Velocity vs. Time with Rayleigh's Drag Equation, b = " << b << "\"\n";
	gp << "set output \"VelTime.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(t,v));

	gp << "set xrange [0:" << SIM_TIME << "]\n";	// Position v. time graph...
	gp << "set yrange [0:" << getMaxVal(x)*1.1 << "]\n";
	gp << "set xlabel \"t (s)\"\n";
	gp << "set ylabel \"x (m)\"\n";
	gp << "set title \"Position vs. Time with Rayleigh's Drag Equation, b = " << b << "\"\n";
	gp << "set output \"PosTime.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(t,x));
}

int main(){					// Main program!
	Euler1D point(0,v_i);	// Create an Euler1D object, x=0 v=v_i
	vector<double> x_n;		// Vectors for x, v, and t
	vector <double> v_n;
	vector <double> t_n;

	for(double t = 0; t < SIM_TIME; t += dt){	// Main loop
		x_n.push_back(point.getX());	// Save point's x and v before modification
		v_n.push_back(point.getV());
		t_n.push_back(t);				// Save the value of t for plotting
		point.update(dragStep);			// Update the point using kinematic equations and Euler step
	}

	plotStuff(x_n, v_n, t_n);	// Make the plots!

	double diff = pErr(g,b,v_n.back());	// Calculate the percent error between accepted and estimated terminal velocities
	cout << "Difference between calculated and final value: " << diff*100 << "%\n";	// Display the percent error as a percentage

	double rising = sgnChangeTime(v_n, t_n);	// Find the time when velocity changes sign, marking the peak of the trajectory and rising time
	double hitground = sgnChangeTime(x_n, t_n);	// Find the time when position changes time, marking when the object returns to 0
	cout << "Rising time: " << rising << "\n";	// Output rising time
	cout << "Falling time: " << hitground-rising << "\n";	// Then falling time (they should be different if b!= 0)

	return 0;	// Everything went well, return 0!
}


#pragma once

#include <Eigen/Core>
#include <boost/static_assert.hpp>

namespace kt84 {

template <class _Func, int _DimIn, int _DimOut>
struct FiniteDifferentiator {
    typedef Eigen::Matrix<double, _DimIn , 1> Point;
    typedef Eigen::Matrix<double, _DimOut, _DimIn> Gradient;
    typedef Eigen::Matrix<double, _DimIn, _DimIn> Hessian;

    Gradient gradient_fd(const Point& point, double epsilon) const {
        Gradient result;
        const _Func& f = *reinterpret_cast<const _Func*>(this);
        for (int i = 0; i < _DimIn; ++i) {
            Point delta = Point::Unit(_DimIn, i) * epsilon;
            result.col(i) = (f(point + delta) - f(point - delta)) / (2 * epsilon);
        }
        return result;
    }
    Hessian hessian_fd(const Point& point, double epsilon) const {
        BOOST_STATIC_ASSERT(_DimOut == 1);
        Hessian result;
        for (int i = 0; i < _DimIn; ++i) {
            Point delta = Point::Unit(_DimIn, i) * epsilon;
            result.row(i) = (gradient_fd(point + delta, epsilon) - gradient_fd(point - delta, epsilon)) / (2 * epsilon);
        }
        return result;
    }
};

}


#pragma once

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <armadillo>

using namespace std;
using namespace arma;

// digamma function
double digamma(double x);
// multivariate digamma function
double digamma_mult(double x,uint32_t d);
// cateorical distribution (Multionomial for one word)
double Cat(uint32_t x, Row<double> pi);
// log cateorical distribution (Multionomial for one word)
double logCat(uint32_t x, Row<double> pi);
// evaluate beta distribution at x
double Beta(double x, double alpha, double beta);
// log beta function
double betaln(double alpha, double beta);
// evaluate log beta distribution at x
double logBeta(double x, double alpha, double beta);
double logBeta(const Row<double>& x, double alpha, double beta);
double logDir(const Row<double>& x, const Row<double>& alpha);

void betaMode(Col<double>& v, const Col<double>& alpha, const Col<double>& beta);
void betaMode(Row<double>& v, const Col<double>& alpha, const Col<double>& beta);
// stick breaking proportions;  truncated stickbreaking -> stick breaks will be dim longer than proportions v
void stickBreaking(Col<double>& prop, const Col<double>& v);
void stickBreaking(Row<double>& prop, const Row<double>& v);
uint32_t multinomialMode(const Row<double>& p);
void dirMode(Row<double>& mode, const Row<double>& alpha);
void dirMode(Col<double>& mode, const Col<double>& alpha);

template <class U>
Row<uint32_t> size(Mat<U> A)
{
  Row<uint32_t> s(2);
  s(0) = A.n_rows;
  s(1) = A.n_cols;
  return s;
};

template <class U>
Row<uint32_t> size(Col<U> A)
{
  Row<uint32_t> s(2);
  s(0) = A.n_rows;
  s(1) = 1;
  return s;
};

template <class U>
Row<uint32_t> size(Row<U> A)
{
  Row<uint32_t> s(2);
  s(0) = 1;
  s(1) = A.n_cols;
  return s;
};


#include "multibrot_opencl_calculator.h"

#include <boost/format.hpp>
#include <unordered_map>

namespace {
static const char* kMainProgram = R"(
/*
Requires a definition:
- REAL_T floating point type (acceptable types are float and, if device supports, half and double),
- RESULT_T - type of result pixel component (either uchar or ushort)
- RESULT_MAX - max value accepted by RESULT_T (either CHAR_MAX or USHRT_MAX)
- POWER_FUNC - name of a function that executes power of Z and adds C
  Function must take the following parameters:
  - Z = (zreal, zimg) - input and output number
  - zlen_sqr - square of absolute value of Z (square)
  - power
  - C = (real, img) - the second addend
  In general this function should do the following mathematical operation:
  (zreal, zimg) = (zreal, zimg) ^ power + (real, img)
  Different functions are used for optimisation purposes
*/

// Preprocessor magic based on https://stackoverflow.com/a/1489985
#define PASTER_2(x,y) x ## y
#define PASTER_3(x,y,z) x ## y ## z
#define EVALUATOR_2(x,y)  PASTER_2(x,y)
#define EVALUATOR_3(x,y,z)  PASTER_3(x,y,z)
#define CONVERT EVALUATOR_3(convert_, RESULT_T, _sat)
#define REAL_T4 EVALUATOR_2(REAL_T, 4)

// Universal complex number power function, supports all real powers but the slowest
void UniversalPowerOfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    // Formula for z^p is a multi-value function, by we use a basic plane only
    REAL_T multiplier = powr( (REAL_T)(zlen_sqr), (REAL_T)(0.5*power) );
    // TODO danger zone here - at first iteration atan2 gets zero as both arguments
    // Fix UB somehow
    REAL_T phi = atan2( *zimg, *zreal );
    *zreal = multiplier * cos( power * phi ) + real;
    *zimg = multiplier * sin( power * phi ) + img;
}

// Should be used when power is 1
void Power1OfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    *zreal += real;
    *zimg += img;
}

// Should be used when power is 2
void SquareOfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    REAL_T zreal_new = (*zreal) * (*zreal) - (*zimg) * (*zimg) + real;
    *zimg = 2 * (*zreal) * (*zimg) + img;
    *zreal = zreal_new;
}

// Should be used when power is 3
void CubeOfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    REAL_T zreal_new = pown(*zreal, 3) - 3 * (*zreal) * (*zimg) * (*zimg) + real;
    *zimg = 3 * (*zreal) * (*zreal) * (*zimg) - pown(*zimg, 3) + img;
    *zreal = zreal_new;
}

/*
    Calculate number of iterations that point after which point
    escaped the set.
    Returns raw number of iterations.
*/
REAL_T CalcPointOnMultibrotSet( REAL_T real, REAL_T img, REAL_T power, ushort max_iter_number )
{
    REAL_T iter_number = 0;
    REAL_T zreal = 0;
    REAL_T zimg = 0;
    REAL_T zlen_sqr = 0;
    while ( zlen_sqr < 2*2 && iter_number < max_iter_number )
    {
        POWER_FUNC( &zreal, &zimg, zlen_sqr, power, real, img );

        zlen_sqr = zreal*zreal + zimg*zimg;
        iter_number += 1;
    }
    return iter_number;
}

#ifdef COLOR_ENABLED
/*
    Scales iteration number and picks a color.
    May be used only when RESULT_T is uchar4 or ushort4.
    Useful for building color images.
*/
RESULT_T ProcessIterationNumber( REAL_T iter_number, ushort max_iter_number )
{
    // Calculate a color in HSV
    // This algorithm is based on https://www.codingame.com/playgrounds/2358/how-to-plot-the-mandelbrot-set/adding-some-colors
    REAL_T value = iter_number < max_iter_number ? RESULT_MAX : 0;
    REAL_T hue = ( RESULT_MAX / max_iter_number ) * iter_number;
    // Saturation is always max.
    // Convert to RGB
    REAL_T hue_section = hue / ( RESULT_MAX / 6 ); // hue_section is in range [0; 6]
    uchar i1 = convert_uchar_sat_rtz( hue_section / 2 );
    uchar i2 = convert_uchar_sat_rtz( remainder( hue_section, 2 ) );
    uchar ic = ( i1 + i2 ) % 3;
    uchar ix = ( i1 - i2 + 1 ) % 3;

    REAL_T rgb[3] = { 0 };
    const REAL_T chroma = value; // * saturation
    REAL_T second_color = chroma * ( 1 - fabs( remainder( hue_section, 2 ) - 1 ) );
    rgb[ic] = chroma;
    rgb[ix] = second_color;

    REAL_T4 result = (REAL_T4)( vload3( 0, rgb ), RESULT_MAX );
    return CONVERT( result );
}
#else
/*
    Scales iteration number so it uses all available values from 0 to max supported by a given
    number type.
    May be used only when RESULT_T is uchar or ushort.
    Useful for building grayscale images.
*/
RESULT_T ProcessIterationNumber( REAL_T iter_number, ushort max_iter_number )
{
    iter_number = ( RESULT_MAX / max_iter_number ) * iter_number;
    iter_number = RESULT_MAX - iter_number;
    return CONVERT( iter_number );
}
#endif

/*
    Build an image of Mandelbrot or Multibrot set.

    (rmin, imin) - complex value that corresponds to the left top corner of the image,
    (rmax, imax) - right bottom corner.

    Output is a pointer to buffer that will contain pixel data. Data format is defined by RESULT_T.

    Must be called with two-dimensional work assignment, where dimensions are used as image
    size (first dimension is width, second dimension is height).
    Pixels are stored in row-major order.
*/
__kernel void MultibrotSetKernel(
    REAL_T rmin, REAL_T imin,
    REAL_T rmax, REAL_T imax,
    REAL_T power,
    ushort max_iter_number,
    __global RESULT_T* restrict output
)
{
    REAL_T rstep = ( rmax - rmin ) / get_global_size(0);
    REAL_T istep = ( imax - imin ) / get_global_size(1);

    size_t row_width = get_global_size(0);
    size_t result_index = get_global_id(1) * row_width + get_global_id(0);

    REAL_T real = rmin + get_global_id(0) * rstep;
    REAL_T img = imin + get_global_id(1) * istep;
    REAL_T multibrot_val = CalcPointOnMultibrotSet( real, img, power, max_iter_number );
    RESULT_T result = ProcessIterationNumber( multibrot_val, max_iter_number );
    output[result_index] = result;
}
)";

template <typename T>
struct TempValueConstants {
    static const char* opencl_type_name;
    static const char* required_extension;
};

const char* TempValueConstants<half_float::half>::opencl_type_name = "half";
const char* TempValueConstants<half_float::half>::required_extension = "cl_khr_fp16";

const char* TempValueConstants<float>::opencl_type_name = "float";
const char* TempValueConstants<float>::required_extension = "";

const char* TempValueConstants<double>::opencl_type_name = "double";
const char* TempValueConstants<double>::required_extension = "cl_khr_fp64";

template <typename P>
struct ResultTypeConstants {
    static const char* result_type_name;
    static const char* result_max_val_macro;
    static const int result_max_val;
    static const bool color_enabled;
};

// Grayscale 8 bit
const char* ResultTypeConstants<cl_uchar>::result_type_name = "uchar";
const char* ResultTypeConstants<cl_uchar>::result_max_val_macro = "UCHAR_MAX";
const int ResultTypeConstants<cl_uchar>::result_max_val = CL_UCHAR_MAX;
const bool ResultTypeConstants<cl_uchar>::color_enabled = false;

// Grayscale 16 bit
const char* ResultTypeConstants<cl_ushort>::result_type_name = "ushort";
const char* ResultTypeConstants<cl_ushort>::result_max_val_macro = "USHRT_MAX";
const int ResultTypeConstants<cl_ushort>::result_max_val = CL_USHRT_MAX;
const bool ResultTypeConstants<cl_ushort>::color_enabled = false;

// RGB 8 bit
const char* ResultTypeConstants<cl_uchar4>::result_type_name = "uchar4";
const char* ResultTypeConstants<cl_uchar4>::result_max_val_macro = "UCHAR_MAX";
const int ResultTypeConstants<cl_uchar4>::result_max_val = CL_UCHAR_MAX;
const bool ResultTypeConstants<cl_uchar4>::color_enabled = true;

// RGB 16 bit
const char* ResultTypeConstants<cl_ushort4>::result_type_name = "ushort4";
const char* ResultTypeConstants<cl_ushort4>::result_max_val_macro = "USHRT_MAX";
const int ResultTypeConstants<cl_ushort4>::result_max_val = CL_USHRT_MAX;
const bool ResultTypeConstants<cl_ushort4>::color_enabled = true;
}  // namespace

template <typename T, typename P>
MultibrotOpenClCalculator<T, P>::MultibrotOpenClCalculator(
    const boost::compute::device& device, const boost::compute::context& context,
    size_t max_width_pix, size_t max_height_pix)
    : device_(device),
      context_(context),
      queue_(context, device, boost::compute::command_queue::enable_profiling),
      max_width_pix_(max_width_pix),
      max_height_pix_(max_height_pix),
      output_device_vector_(max_width_pix * max_height_pix, context) {
    BuildKernels();
}

template <typename T, typename P>
std::string MultibrotOpenClCalculator<T, P>::PrepareCompilerOptions(const std::string& power_func) {
    return (boost::format("-Werror -DREAL_T=%1% -DRESULT_T=%2% -DRESULT_MAX=%3% "
                          "-DPOWER_FUNC=%4% %5% ") %
            TempValueConstants<T>::opencl_type_name % ResultTypeConstants<P>::result_type_name %
            ResultTypeConstants<P>::result_max_val_macro % power_func %
            (ResultTypeConstants<P>::color_enabled ? "-DCOLOR_ENABLED" : ""))
        .str();
}

template <typename T, typename P>
void MultibrotOpenClCalculator<T, P>::BuildKernels() {
    // Collecting required extensions
    std::string required_extension = TempValueConstants<T>::required_extension;
    std::vector<std::string> extensions;
    if (!required_extension.empty()) {
        extensions.push_back(required_extension);
    }

    static const std::unordered_map<double /* power */, std::string> kFixedPowerFunctions = {
        {1.0, "Power1OfComplex"},
        {2.0, "SquareOfComplex"},
        {3.0, "CubeOfComplex"},
    };

    for (const auto& d : kFixedPowerFunctions) {
        specialized_kernels_.emplace(
            d.first, Utils::BuildKernel(
                         "MultibrotSetKernel", context_, kMainProgram,
                         PrepareCompilerOptions(d.second), extensions));
    }

    universal_kernel_ = Utils::BuildKernel(
        "MultibrotSetKernel", context_, kMainProgram,
        PrepareCompilerOptions("UniversalPowerOfComplex"), extensions);
}

template <typename T, typename P>
void MultibrotOpenClCalculator<T, P>::ExecutePrecalculateChecks(
    size_t width_pix, size_t height_pix, int max_iterations) {
    EXCEPTION_ASSERT(width_pix <= max_width_pix_);
    EXCEPTION_ASSERT(height_pix <= max_height_pix_);
    // Verify that given max iterations is valid for given pixel bit depth
    EXCEPTION_ASSERT(max_iterations <= ResultTypeConstants<P>::result_max_val);
}


/*
    This file is part of beautiful-bullet.

    Copyright (c) 2021, 2022 Bernardo Fichera <bernardo.fichera@gmail.com>

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

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

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

#include <iostream>

#include <Eigen/Core>

#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/parsers/urdf.hpp>

int main(int argc, char const* argv[])
{
    // Model
    pinocchio::Model model;
    pinocchio::urdf::buildModel("models/iiwa_bullet/model.urdf", model);
    pinocchio::Data data(model);

    // Frame ID
    const std::string name = "lbr_iiwa_link_7";
    const int id = model.getFrameId(name);

    // Random joint position and velocity
    Eigen::Matrix<double, 7, 1> q = Eigen::VectorXd::Random(7),
                                v = Eigen::VectorXd::Random(7);

    // Forward Kinematics
    pinocchio::forwardKinematics(model, data, q, v);
    pinocchio::framesForwardKinematics(model, data, q);

    // Jacobian
    pinocchio::Data::Matrix6x J(6, model.nv);
    J.setZero();
    pinocchio::computeFrameJacobian(model, data, q, id, J);

    // Pose
    pinocchio::SE3 pose = data.oMf[id];
    Eigen::Vector3d trans = pose.translation();
    Eigen::Matrix3d rot = pose.rotation();

    // Compare
    std::cout << "Jacobian derivation" << std::endl;
    std::cout << (J * v).transpose() << std::endl;

    std::cout << "Lie Algebra derivation" << std::endl;
    std::cout << pinocchio::log6(pose).toVector().transpose() << std::endl;

    return 0;
}


/*
 * SWE_Plane_Normal_Modes.hpp
 *
 *  Created on: 17 Nov 2019
 *      Author: Pedro Peixoto <pedrosp@ime.usp.br>
 *
 *      based on previous implementation by Martin Schreiber in swe_plane.cpp
 *
 */

#ifndef SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_
#define SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_

#include <rexi/EXPFunctions.hpp>
#include <sweet/plane/PlaneData_Spectral.hpp>
#include <sweet/plane/PlaneData_SpectralComplex.hpp>
#include <sweet/SimulationVariables.hpp>
#include <sweet/plane/PlaneOperators.hpp>
#include <functional>
#if SWEET_EIGEN
#include <Eigen/Eigenvalues>
#endif
/**
 * SWE Plane normal mode
 */
class SWE_Plane_Normal_Modes
{
public:

#if SWEET_QUADMATH && 0
	typedef __float128 T;
#else
	typedef double T;
#endif
	typedef std::complex<T> complex;

	static
	void add_normal_mode(
			std::size_t ik0,				//wavenumber in x
			std::size_t ik1,				// wavenumeber in y
			double geo_mode,    //Coeficient multiplying geostrophic mode
			double igwest_mode, //Coeficient multiplying west gravity mode
			double igeast_mode, //Coeficient multiplying east gravity mode
			PlaneData_Spectral &io_h, // h: surface height (perturbation)
			PlaneData_Spectral &io_u, // u: velocity in x-direction
			PlaneData_Spectral &io_v, // v: velocity in y-direction
			SimulationVariables &i_simVars // Simulation variables
	)
	{

		EXPFunctions<T> rexiFunctions;

		const PlaneDataConfig *planeDataConfig = io_h.planeDataConfig;

		if (i_simVars.disc.space_grid_use_c_staggering)
			SWEETError("Staggering not supported");

		//std::cout<<"Adding mode to fields"<<std::endl;

		//Check if k0 is in correct sprectral area
		//std::cout<< io_h.planeDataConfig->spectral_data_size[1] << std::endl;
		if( ik1<0 || ik1 >= planeDataConfig->spectral_data_size[1])
			SWEETError("Normal_mode: mode not within reach");

		if( ik0<0 || ik0 >= planeDataConfig->spectral_data_size[0])
			SWEETError("Normal_mode: mode not within reach");

		//Check for mirror effects
		T k0 = (T)ik0;
		T k1;
		if (ik1 < planeDataConfig->spectral_data_size[1]/2)
			k1 = (T)ik1;
		else
			k1 = (T)((int)ik1-(int)planeDataConfig->spectral_data_size[1]);

		complex v[3][3];
		complex lambda[3];

		SWE_Plane_Normal_Modes::sw_eigen_decomp(
				k0,				//wavenumber in x
				k1,				// wavenumeber in y
				i_simVars,  // Input Simulation variables
				false, // Direct EV matrix (not inverse)
				v , // EV matrix
				lambda // output eigen values */
		);

		complex U[3];
		// Set normal mode acording to desired wave type
		// These are weights for the modes
		U[0] = geo_mode;
		U[1] = igwest_mode;
		U[2] = igeast_mode;

		//Define normal mode as combination of eigen vectors
		complex UEV[3] = {0.0, 0.0, 0.0};
		for (int k = 0; k < 3; k++)
			for (int j = 0; j < 3; j++)
				UEV[k] += v[k][j] * U[j];

		//std::cout<<"spectral before"<< std::endl;
		//io_v.print_spectralIndex();

		complex h_add, u_add, v_add;
		h_add = io_h.spectral_get(ik1, ik0)+UEV[0];
		u_add = io_u.spectral_get(ik1, ik0)+UEV[1];
		v_add = io_v.spectral_get(ik1, ik0)+UEV[2];

		/* Add normal mode to data */
		io_h.spectral_set(ik1, ik0, h_add);
		io_u.spectral_set(ik1, ik0, u_add);
		io_v.spectral_set(ik1, ik0, v_add);

		io_h.spectral_zeroAliasingModes();
		io_u.spectral_zeroAliasingModes();
		io_v.spectral_zeroAliasingModes();

/*Debug output*/
#if 0

		std::cout<<"EV matrix"<<std::endl;
		for (int j = 0; j < 3; j++)	{
			for (int i = 0; i < 3; i++)
				std::cout<< v[j][i]<<" ";
			std::cout<<std::endl;
		}

		std::cout<<"Eigen values"<<std::endl;
		for (int j = 0; j < 3; j++)
			std::cout<< lambda[j]<<" ";
		std::cout<<std::endl;

		std::cout<<"Adding normal mode"<<std::endl;
		for (int j = 0; j < 3; j++)
			std::cout<< UEV[j]<<" ";
		std::cout<<std::endl;

		std::cout<<ik0<<" "<<ik1<< " "<< io_v.p_spectral_get(ik1, ik0) << UEV[2] << std::endl;

#endif
		return;
	}

	static
	void convert_allspectralmodes_to_normalmodes(
			PlaneData_Spectral &i_h, // h: surface height (perturbation)
			PlaneData_Spectral &i_u, // u: velocity in x-direction
			PlaneData_Spectral &i_v, // v: velocity in y-direction
			SimulationVariables &i_simVars, // Simulation variables
			PlaneData_Spectral &o_geo_mode,    //Output: Coeficients multiplying geostrophic mode
			PlaneData_Spectral &o_igwest_mode, //Output: Coeficients multiplying west gravity mode
			PlaneData_Spectral &o_igeast_mode //Output: Coeficients multiplying east gravity mode
	)
	{
		const PlaneDataConfig *planeDataConfig = i_h.planeDataConfig;

		o_geo_mode.spectral_set_zero();
		o_igwest_mode.spectral_set_zero();
		o_igeast_mode.spectral_set_zero();

		complex geo_mode_c;
		complex igwest_mode_c;
		complex igeast_mode_c;
		for (std::size_t ik1 = 0; ik1 < planeDataConfig->spectral_data_size[1]; ik1++)
		{
			for (std::size_t ik0 = 0; ik0 < planeDataConfig->spectral_data_size[0]; ik0++)
			{

				SWE_Plane_Normal_Modes::convert_spectralmode_to_normalmode(
									ik0, ik1,
									i_h,
									i_u,
									i_v,
									i_simVars,
									geo_mode_c,
									igwest_mode_c,
									igeast_mode_c
							);
				o_geo_mode.spectral_set(ik1, ik0, geo_mode_c);
				o_igwest_mode.spectral_set(ik1, ik0, igwest_mode_c);
				o_igeast_mode.spectral_set(ik1, ik0, igeast_mode_c);

			}
		}
		return;
	};

	static
	void convert_spectralmode_to_normalmode(
			std::size_t ik0,				//wavenumber in x
			std::size_t ik1,				// wavenumeber in y
			PlaneData_Spectral &i_h, // h: surface height (perturbation)
			PlaneData_Spectral &i_u, // u: velocity in x-direction
			PlaneData_Spectral &i_v, // v: velocity in y-direction
			SimulationVariables &i_simVars, // Simulation variables
			complex &o_geo_mode,    //Output: Coeficient multiplying geostrophic mode
			complex &o_igwest_mode, //Output: Coeficient multiplying west gravity mode
			complex &o_igeast_mode //Output: Coeficient multiplying east gravity mode
	)
	{

		EXPFunctions<T> rexiFunctions;

		const PlaneDataConfig *planeDataConfig = i_h.planeDataConfig;

		if (i_simVars.disc.space_grid_use_c_staggering)
			SWEETError("Staggering not supported");

		//std::cout<<"Adding mode to fields"<<std::endl;

		//Check if k0 is in correct sprectral area
		//std::cout<< io_h.planeDataConfig->spectral_data_size[1] << std::endl;
		if( ik1<0 || ik1 >= planeDataConfig->spectral_data_size[1])
			SWEETError("Normal_mode: mode not within reach");

		if( ik0<0 || ik0 >= planeDataConfig->spectral_data_size[0])
			SWEETError("Normal_mode: mode not within reach");

		//Check for mirror effects
		T k0 = (T)ik0;
		T k1;
		if (ik1 < planeDataConfig->spectral_data_size[1]/2)
			k1 = (T)ik1;
		else
			k1 = (T)((int)ik1-(int)planeDataConfig->spectral_data_size[1]);

		complex v[3][3];
		complex lambda[3];

		SWE_Plane_Normal_Modes::sw_eigen_decomp(
				k0,				//wavenumber in x
				k1,				// wavenumeber in y
				i_simVars,  // Input Simulation variables
				true, // Inverse ev matrix
				v , // inverse EV matrix
				lambda // output eigen values */
		);

		complex U[3];
		// Set (h,u,v) spectral coeficients
		// These are weights for the modes
		U[0] = i_h.spectral_get(ik1, ik0);
		U[1] = i_u.spectral_get(ik1, ik0);
		U[2] = i_v.spectral_get(ik1, ik0);

		//Apply inverse EV matrix to obtain data in EV space
		complex UEV[3] = {0.0, 0.0, 0.0};
		for (int k = 0; k < 3; k++)
			for (int j = 0; j < 3; j++)
				UEV[k] += v[k][j] * U[j];

		//Return the modes
		o_geo_mode=UEV[0];
		o_igwest_mode=UEV[1];
		o_igeast_mode=UEV[2];
		//std::cout<< " Geost: "<< o_geo_mode<<std::endl;
		//std::cout<< " IGWest: "<< o_igwest_mode<<std::endl;
		//std::cout<< " IGEast: "<< o_igeast_mode<<std::endl;

		return;
	}

	/* Get linear shallow water operator eigen decomposition */
	static
	void sw_eigen_decomp(
			T k0,				//wavenumber in x
			T k1,				// wavenumeber in y
			SimulationVariables &i_simVars, // Input Simulation variables
			bool i_inverse = false, // Input true, returns inverse matriz, false: returns direct
			complex o_v[3][3] = {0}, // output eigen vector (direct or inverse)
			complex o_evalues[3] =  0 // output eigen values (optional)
	)
	{
		EXPFunctions<T> rexiFunctions;
		bool i_evalues = false;
		if (o_evalues){
			i_evalues = true;
			//std::cout<<i_evalues<<" "<< o_evalues[0]   << std::endl;
		}
		else{
			i_evalues = false;
		}
		//std::cout<<o_evalues[1]<<std::endl;
		//std::cout<<i_inverse<<std::endl;

		if (i_simVars.disc.space_grid_use_c_staggering)
			SWEETError("Staggering not supported");

		complex I(0.0, 1.0);
		//std::cout<<"Calculating EV for mode (" << k0 << ", " << k1 << ")" << std::endl;
		//std::cout<<"hi"<< std::endl;
		T s0 = i_simVars.sim.plane_domain_size[0];
		T s1 = i_simVars.sim.plane_domain_size[1];

		T f = i_simVars.sim.plane_rotating_f0;
		T h = i_simVars.sim.h0;
		T g = i_simVars.sim.gravitation;

		T sqrt_h = rexiFunctions.l_sqrt(h);
		T sqrt_g = rexiFunctions.l_sqrt(g);

		complex b = -k0*I;	// d/dx exp(I*k0*x) = I*k0 exp(I*k0*x)
		complex c = -k1*I;

		b = b*rexiFunctions.pi2/s0;
		c = c*rexiFunctions.pi2/s1;

		/*
		 * Matrix with Eigenvectors (column-wise)
		 */
		complex v[3][3];
		complex v_inv[3][3];

		/*
		 * Eigenvalues
		 */
		complex lambda[3];

		if (i_simVars.sim.plane_rotating_f0 == 0)
		{
			/*
			 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,h*b,h*c%7D,%7Bg*b,0,0%7D,%7Bg*c,0,0%7D%7D
			 */
			if (k0 == 0 && k1 == 0)
			{
				v[0][0] = 1;
				v[1][0] = 0;
				v[2][0] = 0;

				v[0][1] = 0;
				v[1][1] = 1;
				v[2][1] = 0;

				v[0][2] = 0;
				v[1][2] = 0;
				v[2][2] = 1;

				if (i_evalues){
					lambda[0] = 0;
					lambda[1] = 0;
					lambda[2] = 0;
				}
			}
			else if (k0 == 0)
			{
				v[0][0] = 0;
				v[1][0] = 1;
				v[2][0] = 0;

				v[0][1] = -sqrt_h/sqrt_g;
				v[1][1] = 0;
				v[2][1] = 1;

				v[0][2] = sqrt_h/sqrt_g;
				v[1][2] = 0;
				v[2][2] = 1;

				if (i_evalues){
					lambda[0] = 0;
					lambda[1] = -c*sqrt_g*sqrt_h;
					lambda[2] = c*sqrt_g*sqrt_h;;
				}
			}
			else if (k1 == 0)
			{
				/*
				 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,h*b,h*c*0%7D,%7Bg*b,0,0%7D,%7Bg*c*0,0,0%7D%7D
				 */

				v[0][0] = 0;
				v[1][0] = 0;
				v[2][0] = 1;

				v[0][1] = -sqrt_h/sqrt_g;
				v[1][1] = 1;
				v[2][1] = 0;

				v[0][2] = sqrt_h/sqrt_g;
				v[1][2] = 1;
				v[2][2] = 0;

				if (i_evalues){
					lambda[0] = 0;
					lambda[1] = -b*sqrt_g*sqrt_h;
					lambda[2] = b*sqrt_g*sqrt_h;
				}
			}
			else
			{
				v[0][0] = 0;
				v[1][0] = -c/b;
				v[2][0] = 1.0;

				v[0][1] = -(sqrt_h*rexiFunctions.l_sqrtcplx(b*b + c*c))/(c*sqrt_g);
				v[1][1] = b/c;
				v[2][1] = 1.0;

				v[0][2] = (sqrt_h*rexiFunctions.l_sqrtcplx(b*b + c*c))/(c*sqrt_g);
				v[1][2] = b/c;
				v[2][2] = 1.0;

				if (i_evalues){
					lambda[0] = 0.0;
					lambda[1] = -rexiFunctions.l_sqrtcplx(b*b + c*c)*sqrt_h*sqrt_g;
					lambda[2] = rexiFunctions.l_sqrtcplx(b*b + c*c)*sqrt_h*sqrt_g;
				}
			}
		}
		else
		{
			if (k0 == 0 && k1 == 0)
			{
				/*
				 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,0,0%7D,%7B0,0,f%7D,%7B0,-f,0%7D%7D
				 */
				v[0][0] = 0;
				v[1][0] = -I;
				v[2][0] = 1;

				v[0][1] = 0;
				v[1][1] = I;
				v[2][1] = 1;

				v[0][2] = 1;
				v[1][2] = 0;
				v[2][2] = 0;

				if (i_evalues){
					lambda[0] = I*f;
					lambda[1] = -I*f;
					lambda[2] = 0;
				}
			}
			else if (k0 == 0)
			{
				/*
				 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,h*b*0,h*c%7D,%7Bg*b*0,0,f%7D,%7Bg*c,-f,0%7D%7D
				 */
				v[0][0] = f/(c*g);
				v[1][0] = 1;
				v[2][0] = 0;

				v[0][1] = -(c*h)/rexiFunctions.l_sqrtcplx(-f*f + c*c*g*h);
				v[1][1] =  -f/rexiFunctions.l_sqrtcplx(-f*f + c*c*g*h);
				v[2][1] = 1;

				v[0][2] = (c*h)/rexiFunctions.l_sqrtcplx(-f*f + c*c*g*h);
				v[1][2] = f/rexiFunctions.l_sqrtcplx(-f*f + c*c*g*h);
				v[2][2] = 1;

				if (i_evalues){
					lambda[0] = 0;
					lambda[1] = -rexiFunctions.l_sqrtcplx(c*c*g*h-f*f);
					lambda[2] = rexiFunctions.l_sqrtcplx(c*c*g*h-f*f);
				}
			}
			else if (k1 == 0)
			{
					/*
					 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,h*b,h*c*0%7D,%7Bg*b,0,f%7D,%7Bg*c*0,-f,0%7D%7D
					 */
				v[0][0] = -f/(b*g);
				v[1][0] = 0;
				v[2][0] = 1;

				v[0][1] = -(b*h)/f;
				v[1][1] = rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h)/f;
				v[2][1] = 1;

				v[0][2] = -(b*h)/f;
				v[1][2] = -rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h)/f;
				v[2][2] = 1;

				if (i_evalues){
					lambda[0] = 0;
					lambda[1] = -rexiFunctions.l_sqrtcplx(b*b*g*h-f*f);
					lambda[2] = rexiFunctions.l_sqrtcplx(b*b*g*h-f*f);
				}
			}
			else
				{
					/*
					 * Compute EV's of
					 * Linear operator
					 *
					 * [ 0  hb  hc ]
					 * [ gb  0   f ]
					 * [ gc -f   0 ]
					 *
					 * http://www.wolframalpha.com/input/?i=eigenvector%7B%7B0,h*b,h*c%7D,%7Bg*b,0,f%7D,%7Bg*c,-f,0%7D%7D
					 */

					v[0][0] = -f/(b*g);
					v[1][0] = -c/b;
					v[2][0] = 1.0;

					v[0][1] = -(c*f*h + b*h*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h))/(b*c*g*h + f*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h));
					v[1][1] = -(f*f - b*b*g*h)/(b*c*g*h + f*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h));
					v[2][1] = 1.0;

					v[0][2] = -(-c*f*h + b*h*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h))/(-b*c*g*h + f*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h));
					v[1][2] =  -(-f*f + b*b*g*h)/(-b*c*g*h + f*rexiFunctions.l_sqrtcplx(-f*f + b*b*g*h + c*c*g*h));
					v[2][2] = 1.0;

					if (i_evalues){
						lambda[0] = 0.0;
						lambda[1] = -rexiFunctions.l_sqrtcplx(b*b*g*h + c*c*g*h - f*f);
						lambda[2] =  rexiFunctions.l_sqrtcplx(b*b*g*h + c*c*g*h - f*f);
					}
				}
		}

			/*
			 * Invert Eigenvalue matrix
			 */

		if (i_inverse){
			v_inv[0][0] =  (v[1][1]*v[2][2] - v[1][2]*v[2][1]);
			v_inv[0][1] = -(v[0][1]*v[2][2] - v[0][2]*v[2][1]);
			v_inv[0][2] =  (v[0][1]*v[1][2] - v[0][2]*v[1][1]);

			v_inv[1][0] = -(v[1][0]*v[2][2] - v[1][2]*v[2][0]);
			v_inv[1][1] =  (v[0][0]*v[2][2] - v[0][2]*v[2][0]);
			v_inv[1][2] = -(v[0][0]*v[1][2] - v[0][2]*v[1][0]);

			v_inv[2][0] =  (v[1][0]*v[2][1] - v[1][1]*v[2][0]);
			v_inv[2][1] = -(v[0][0]*v[2][1] - v[0][1]*v[2][0]);
			v_inv[2][2] =  (v[0][0]*v[1][1] - v[0][1]*v[1][0]);

			complex s = v[0][0]*v_inv[0][0] + v[0][1]*v_inv[1][0] + v[0][2]*v_inv[2][0];

			for (int j = 0; j < 3; j++)	{
				for (int i = 0; i < 3; i++)
					v_inv[j][i] /= s;
			}
			//Return inverse matrix
			for (int j = 0; j < 3; j++)	{
				for (int i = 0; i < 3; i++)
					o_v[j][i] = v_inv[j][i] ;
			}
		}
		else{
			//Return direct matrix
			for (int j = 0; j < 3; j++)	{
				for (int i = 0; i < 3; i++)
					o_v[j][i] = v[j][i] ;
			}
		}
		if (i_evalues){
			for (int j = 0; j < 3; j++)	{
				o_evalues[j] = lambda[j] ;
			}
		}
		return;
}

	template <typename TCallbackClass>
	static
	void normal_mode_analysis(
			PlaneData_Spectral &io_prog_h_pert, // h: surface height (perturbation)
			PlaneData_Spectral &io_prog_u, // u: velocity in x-direction
			PlaneData_Spectral &io_prog_v, // v: velocity in y-direction
			int number_of_prognostic_variables,
			SimulationVariables &i_simVars, // Simulation variables
			TCallbackClass *i_class,
			void(TCallbackClass::* const i_run_timestep_method)(void)
	)
	{

		const PlaneDataConfig *planeDataConfig = io_prog_h_pert.planeDataConfig;

		// dummy time step to get time step size
		if (i_simVars.timecontrol.current_timestep_size <= 0)
			SWEETError("Normal mode analysis requires setting fixed time step size");

		/*
		 *
		 * Mode-wise normal mode analysis
		 *
		 *
		 */

		if (i_simVars.misc.normal_mode_analysis_generation == 4)
		{
#if SWEET_EIGEN
#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			SWEETError("SWE_Plane_Normal_Modes: This test was build for linear or linearized models, so please compile without dealising --plane-spectral-dealiasing=disable.");
#endif

			/*
			 * Setup all output files
			 */
			const char* filename; //general filename
			char buffer_real[1024];

			if (i_simVars.iodata.output_file_name == "")
				filename = "output_%s_t%020.8f.csv";
			else
				filename = i_simVars.iodata.output_file_name.c_str();

			sprintf(buffer_real, filename, "normal_modes_plane", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file(buffer_real, std::ios_base::trunc);
			std::cout << "Writing normal mode analysis to files of the form '" << buffer_real << "'" << std::endl;

			//Positive inertia-gravity modes
			sprintf(buffer_real, filename, "normal_modes_plane_igpos", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_igpos(buffer_real, std::ios_base::trunc);

			//Negative inertia-gravity modes
			sprintf(buffer_real, filename, "normal_modes_plane_igneg", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_igneg(buffer_real, std::ios_base::trunc);

			//Geostrophic modes
			sprintf(buffer_real, filename, "normal_modes_plane_geo", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_geo(buffer_real, std::ios_base::trunc);

			//std::cout << "WARNING: OUTPUT IS TRANSPOSED!" << std::endl;

			// use very high precision
			file << std::setprecision(20);
			file_igpos << std::setprecision(20);
			file_igneg << std::setprecision(20);
			file_geo << std::setprecision(20);

			file << "# dt " << i_simVars.timecontrol.current_timestep_size << std::endl;
			file << "# g " << i_simVars.sim.gravitation << std::endl;
			file << "# h " << i_simVars.sim.h0 << std::endl;
			file << "# r " << i_simVars.sim.sphere_radius << std::endl;
			file << "# f " << i_simVars.sim.plane_rotating_f0 << std::endl;

#if SWEET_USE_PLANE_SPECTRAL_SPACE
			int specmodes = planeDataConfig->get_spectral_iteration_range_area(0)+planeDataConfig->get_spectral_iteration_range_area(1);
			file << "# specnummodes " << specmodes << std::endl;
			file << "# specrealresx " << planeDataConfig->spectral_real_modes[0] << std::endl;
			file << "# specrealresy " << planeDataConfig->spectral_real_modes[1] << std::endl;
#endif

			file << "# physresx " << planeDataConfig->physical_res[0] << std::endl;
			file << "# physresy " << planeDataConfig->physical_res[1] << std::endl;
			file << "# normalmodegeneration " << i_simVars.misc.normal_mode_analysis_generation << std::endl;
			file << "# antialiasing ";
#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			file << 1;
#else
			file << 0;
#endif
			file << std::endl;

			PlaneData_Spectral* prog[3] = {&io_prog_h_pert, &io_prog_u, &io_prog_v};

			int number_of_prognostic_variables = 3;
			//The basic state is with zero in all variables
			// The only non zero variable in the basic state is the total height
			//    for which the constant is added within run_timestep()
			io_prog_h_pert.spectral_set_zero();
			io_prog_u.spectral_set_zero();
			io_prog_v.spectral_set_zero();

			//int num_timesteps = 1;

			// Timestep and perturbation
			double dt = i_simVars.timecontrol.current_timestep_size;
			double eps = dt;

			//Matrix representing discrete linear operator in spectral space
			Eigen::MatrixXcf A(3,3) ;
			//Eigen solver
			Eigen::ComplexEigenSolver<Eigen::MatrixXcf> ces;
			//Final eigenvalues
			std::complex<double> eval[3];

			//For each spectral mode
			//for (int r = 0; r < 2; r++) //only required to get the symmetric half of the spectrum
			//{
			int r = 0;

			for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
			{
				std::cout << "." << std::flush;
				for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
				{
					//This is the mode to be analysed
					//std::cout << "Mode (i,j)= (" << i << " , " << j <<")"<< std::endl;

					for (int outer_prog_id = 0; outer_prog_id < number_of_prognostic_variables; outer_prog_id++)
					{

						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog[inner_prog_id]->spectral_set_zero();

						// activate mode via real coefficient
						prog[outer_prog_id]->spectral_set(j, i, 1.0);
						//Activate the symetric couterpart of the mode (only needed if j>0 )
						if (j > 0)
							prog[outer_prog_id]->spectral_set(planeDataConfig->spectral_data_size[1]-j, i, 1.0);

						/*
						 * RUN timestep
						 */
						(i_class->*i_run_timestep_method)();

						/*
						 * compute
						 * 1/dt * (U(t+1) - U(t))
						 */

						std::complex<double> val = prog[outer_prog_id]->spectral_get(j, i);
						val = val - 1.0; //subtract U(0) from mode
						prog[outer_prog_id]->spectral_set(j, i, val);

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							(*prog[inner_prog_id]) /= eps;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							A(inner_prog_id,outer_prog_id)=prog[inner_prog_id]->spectral_get(j, i);;
						}

					}

					//std::cout << "Lik matrix" << std::endl;
					//std::cout << A << std::endl;

					//std::cout<<"Normal modes" << std::endl;
					ces.compute(A);
					for(int i=0; i<3; i++)
					{
						eval[i]=ces.eigenvalues()[i];
						//std::cout << "Eigenvalue "<< i << " : " << eval[i].real() <<" "<<eval[i].imag() << std::endl;

					}
					/* We will try to separate the modes in 3 types:
					 * -positive inertia-gravity (imag>f) - we will adopt coriolis f to test as if > zero, since the exact freq is sqrt(f^2+cK*K)
					 * -negative inertia-gravity (imag<-f)
					 * -negative inertia-gravity (imag aprox 0) - we will fit all other modes here
					 */
					int count_igpos=0;
					int count_igneg=0;
					int count_geo=0;
					for(int i=0; i<3; i++)
					{
						if(eval[i].imag() > 0.5 * i_simVars.sim.plane_rotating_f0)
						{
							//std::cout<< "IG pos mode: " << eval[i].imag() << std::endl;
							//file_igpos << eval[i].imag();
							file_igpos << eval[i].real()<< "\t" << eval[i].imag();
							file_igpos << "\t";
							count_igpos++;
						}
						if(eval[i].imag() < - 0.5 * i_simVars.sim.plane_rotating_f0)
						{
							//std::cout<< "IG neg mode: " << eval[i].imag() << std::endl;
							//file_igneg << eval[i].imag();
							file_igneg << eval[i].real()<< "\t" << eval[i].imag();
							file_igneg << "\t";
							count_igneg++;
						}
						if(eval[i].imag() >= - 0.5 * i_simVars.sim.plane_rotating_f0 && eval[i].imag() <=  0.5 * i_simVars.sim.plane_rotating_f0 )
						{
							//std::cout<< "IG geo mode: " << eval[i].imag() << std::endl;
							//file_geo << eval[i].imag();
							file_geo << eval[i].real()<< "\t" << eval[i].imag();
							file_geo << "\t";
							count_geo++;
						}
					}
					//Check if we got the correct modes
					if ( count_igpos * count_igneg * count_geo > 0 )
					{
						count_igpos=0;
						count_igneg=0;
						count_geo=0;
					}
					else
					{
						SWEETError("SWE_Plane_Normal_Modes: Could not separate modes!!");
					}

					//std::cout<<"-------------------------" << std::endl;
				}
				file_igpos << std::endl;
				file_igneg << std::endl;
				file_geo << std::endl;
			}

			//}
			//std::cout<<"-------------------------" << std::endl;
			//SWEETError("still needs work...");
#else
			SWEETError("SWE_Plane_Normal_Modes: Cannot test this without Eigen library. Please compile with --eigen=enable");
#endif
		}
		/*
		 * Do a normal mode analysis using perturbation, see
		 * Hillary Weller, John Thuburn, Collin J. Cotter,
		 * "Computational Modes and Grid Imprinting on Five Quasi-Uniform Spherical C Grids"
		 */
		else
		{

			//run_timestep();
			const char* filename;
			char buffer_real[1024];

			if (i_simVars.iodata.output_file_name == "")
				filename = "output_%s_normalmodes.csv";
			else
				filename = i_simVars.iodata.output_file_name.c_str();

			sprintf(buffer_real, filename, "normal_modes_physical", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file(buffer_real, std::ios_base::trunc);
			std::cout << "Writing normal mode analysis to file '" << buffer_real << "'" << std::endl;

			std::cout << "WARNING: OUTPUT IS TRANSPOSED!" << std::endl;

			// use very high precision
			file << std::setprecision(20);

			PlaneData_Spectral* prog[3] = {&io_prog_h_pert, &io_prog_u, &io_prog_v};

			/*
			 * Maximum number of prognostic variables
			 *
			 * Advection e.g. has only one
			 */
			if (number_of_prognostic_variables <= 0)
				SWEETError("simVars.pde.number_of_prognostic_variables must be set!");

			if (number_of_prognostic_variables == 3)
			{
				io_prog_h_pert.spectral_set_zero();
				io_prog_u.spectral_set_zero();
				io_prog_v.spectral_set_zero();
			}
			else if (number_of_prognostic_variables == 1)
			{
				io_prog_h_pert.spectral_set_zero();
			}
			else
			{
				SWEETError("Not yet supported");
			}

#if 0
			if (i_simVars.disc.timestepping_method == SimulationVariables::Discretization::LEAPFROG_EXPLICIT)
			{
				SWEETError("Not yet tested and supported");
				std::cout << "WARNING: Leapfrog time stepping doesn't make real sense since 1st step is based on RK-like method" << std::endl;
				std::cout << "We'll do two Leapfrog time steps here to take the LF errors into account!" << std::endl;
				std::cout << "Therefore, we also halve the time step size here" << std::endl;

				i_simVars.timecontrol.current_timestep_size = 0.5*i_simVars.sim.CFL;
				i_simVars.sim.CFL = -i_simVars.timecontrol.current_timestep_size;
			}
#endif

			int num_timesteps = 1;
			if (i_simVars.misc.normal_mode_analysis_generation >= 10)
			{
				if (i_simVars.timecontrol.max_timesteps_nr > 0)
					num_timesteps = i_simVars.timecontrol.max_timesteps_nr;
			}

			if (i_simVars.timecontrol.max_simulation_time > 0)
				file << "# t " << i_simVars.timecontrol.max_simulation_time << std::endl;
			else
				file << "# t " << (num_timesteps*(-i_simVars.timecontrol.current_timestep_size)) << std::endl;

			file << "# g " << i_simVars.sim.gravitation << std::endl;
			file << "# h " << i_simVars.sim.h0 << std::endl;
//			file << "# r " << i_simVars.sim.sphere_radius << std::endl;
			file << "# f " << i_simVars.sim.plane_rotating_f0 << std::endl;

#if SWEET_USE_PLANE_SPECTRAL_SPACE
			int specmodes = planeDataConfig->get_spectral_iteration_range_area(0)+planeDataConfig->get_spectral_iteration_range_area(1);
			file << "# specnummodes " << specmodes << std::endl;
			file << "# specrealresx " << planeDataConfig->spectral_real_modes[0] << std::endl;
			file << "# specrealresy " << planeDataConfig->spectral_real_modes[1] << std::endl;
#endif

			file << "# physresx " << planeDataConfig->physical_res[0] << std::endl;
			file << "# physresy " << planeDataConfig->physical_res[1] << std::endl;
			file << "# normalmodegeneration " << i_simVars.misc.normal_mode_analysis_generation << std::endl;
			file << "# antialiasing ";

#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			file << 1;
#else
			file << 0;
#endif

			file << std::endl;

			// iterate over all prognostic variables
			for (int outer_prog_id = 0; outer_prog_id < number_of_prognostic_variables; outer_prog_id++)
			{
				if (i_simVars.misc.normal_mode_analysis_generation == 1 || i_simVars.misc.normal_mode_analysis_generation == 11)
				{
					// iterate over physical space
					for (std::size_t outer_i = 0; outer_i < planeDataConfig->physical_array_data_number_of_elements; outer_i++)
					{
						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						std::cout << "." << std::flush;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog[inner_prog_id]->spectral_set_zero();

						// activate mode
						PlaneData_Physical tmp = prog[outer_prog_id]->toPhys();
						tmp.physical_space_data[outer_i] = 1;
						prog[outer_prog_id]->loadPlaneDataPhysical(tmp);

						/*
						 * RUN timestep
						 */

						(i_class->*i_run_timestep_method)();

						if (i_simVars.misc.normal_mode_analysis_generation == 1)
						{
							/*
							 * compute
							 * 1/dt * (U(t+1) - U(t))
							 */
							tmp = prog[outer_prog_id]->toPhys();
							tmp.physical_space_data[outer_i] -= 1.0;
							prog[outer_prog_id]->loadPlaneDataPhysical(tmp);

							for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								(*prog[inner_prog_id]) /= i_simVars.timecontrol.current_timestep_size;
						}

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							tmp = prog[outer_prog_id]->toPhys();
							for (std::size_t k = 0; k < planeDataConfig->physical_array_data_number_of_elements; k++)
							{
								file << tmp.physical_space_data[k];
								if (inner_prog_id != number_of_prognostic_variables-1 || k != planeDataConfig->physical_array_data_number_of_elements-1)
									file << "\t";
								else
									file << std::endl;
							}
						}
					}
				}
#if 1
				else if (i_simVars.misc.normal_mode_analysis_generation == 3 || i_simVars.misc.normal_mode_analysis_generation == 13)
				{
#if !SWEET_USE_PLANE_SPECTRAL_SPACE
					SWEETError("Only available with if plane spectral space is activated during compile time!");
#else

					// iterate over spectral space
					for (int r = 0; r < 2; r++)
					{

						for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
						{
							for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
							{
								// reset time control
								i_simVars.timecontrol.current_timestep_nr = 0;
								i_simVars.timecontrol.current_simulation_time = 0;

								std::cout << "." << std::flush;

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
									prog[inner_prog_id]->spectral_set_zero();

								// activate mode via real coefficient
								prog[outer_prog_id]->spectral_set(j, i, 1.0);

								/*
								 * RUN timestep
								 */
								(i_class->*i_run_timestep_method)();

								if (i_simVars.misc.normal_mode_analysis_generation == 3)
								{
									/*
									 * compute
									 * 1/dt * (U(t+1) - U(t))
									 */

									std::complex<double> val = prog[outer_prog_id]->spectral_get(j, i);
									val = val - 1.0;
									prog[outer_prog_id]->spectral_set(j, i, val);

									for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
										(*prog[inner_prog_id]) /= i_simVars.timecontrol.current_timestep_size;
								}

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								{

									/*
									 * REAL
									 */

									for (int r = 0; r < 2; r++)
									{
										for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
										{
											for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
											{
												file << prog[inner_prog_id]->spectral_get(j, i).real();
												file << "\t";
											}
										}
									}
								}

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								{
									/*
									 * IMAG
									 */
									int c = 0;
									for (int r = 0; r < 2; r++)
									{
										for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
										{
											for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
											{
												file << prog[inner_prog_id]->spectral_get(j, i).imag();

												if (inner_prog_id != number_of_prognostic_variables-1 || c != specmodes-1)
													file << "\t";
												else
													file << std::endl;

												c++;
											}
										}
									}
								}
							}
						}
					}
#endif
				}
#else
				else if (i_simVars.misc.normal_mode_analysis_generation == 3 || i_simVars.misc.normal_mode_analysis_generation == 13)
				{
					PlaneDataComplex t1(planeDataConfig);
					PlaneDataComplex t2(planeDataConfig);
					PlaneDataComplex t3(planeDataConfig);
					PlaneDataComplex* prog_cplx[3] = {&t1, &t2, &t3};

					// iterate over spectral space
					for (std::size_t outer_i = 0; outer_i < planeDataConfig->spectral_complex_array_data_number_of_elements; outer_i++)
					{
						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						std::cout << "." << std::flush;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog_cplx[inner_prog_id]->spectral_set_zero();

						// activate mode via real coefficient
						prog_cplx[outer_prog_id]->request_data_spectral();
						prog_cplx[outer_prog_id]->spectral_space_data[outer_i].real(1);

						// convert PlaneDataComplex to PlaneData
						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							*prog[inner_prog_id] = Convert_PlaneDataComplex_To_PlaneData::physical_convert(*prog_cplx[inner_prog_id]);
							prog[inner_prog_id]->spectral_zeroAliasingModes();
						}

						/*
						 * RUN timestep
						 */
						(i_class->*i_run_timestep_method)();

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							prog[inner_prog_id]->spectral_zeroAliasingModes();
#warning "update this physical_convert maybe to spectral_convert"

							*prog_cplx[inner_prog_id] = Convert_PlaneData_To_PlaneDataComplex::physical_convert(*prog[inner_prog_id]);

							prog_cplx[inner_prog_id]->request_data_spectral();
						}

						if (i_simVars.misc.normal_mode_analysis_generation == 3)
						{
							/*
							 * compute
							 * 1/dt * (U(t+1) - U(t))
							 */
							prog_cplx[outer_prog_id]->request_data_spectral();
							prog_cplx[outer_prog_id]->spectral_space_data[outer_i] -= 1.0;

							for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								prog_cplx[inner_prog_id]->operator*=(1.0/i_simVars.timecontrol.current_timestep_size);
						}

						// convert PlaneDataComplex to PlaneData
						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							prog_cplx[inner_prog_id]->request_data_spectral();

							/*
							 * REAL
							 */
							for (std::size_t k = 0; k < planeDataConfig->spectral_complex_array_data_number_of_elements; k++)
							{
								file << prog_cplx[inner_prog_id]->spectral_space_data[k].real();
								file << "\t";
							}

							/*
							 * IMAG
							 */
							for (std::size_t k = 0; k < planeDataConfig->spectral_complex_array_data_number_of_elements; k++)
							{
								file << prog_cplx[inner_prog_id]->spectral_space_data[k].imag();

								if (inner_prog_id != number_of_prognostic_variables-1 || k != planeDataConfig->spectral_complex_array_data_number_of_elements-1)
									file << "\t";
								else
									file << std::endl;
							}
						}
					}
				}
#endif
			}
		}
	}

	~SWE_Plane_Normal_Modes()
	{

	}
};

#endif /* SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_ */


#include <RcppEigen.h>
#include <Eigen/Core>
#include <Eigen/Dense>

// [[Rcpp::depends(RcppEigen)]]
using namespace Rcpp;
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::DiagonalMatrix;

// [[Rcpp::export]]
float r_score(Eigen::MatrixXd x, Eigen::MatrixXd x0) {
  int nr = x.rows();
  int nc = x.cols();
  int n0r = x0.rows();
  Eigen::MatrixXd A  = x.transpose()*((x*x.transpose()+MatrixXd::Identity(nr, nr)*(1.0/nc)).inverse());
  Eigen::MatrixXd IJ = MatrixXd::Identity(n0r, n0r)-(MatrixXd::Identity(n0r, n0r)*(1.0/n0r));
  float q1 = n0r-1+(IJ*x0).array().square().sum();
  Eigen::MatrixXd IJX0A = IJ*x0*A;
  Eigen::MatrixXd IJX0AX = IJX0A*x;
  float q2 = IJX0A.array().square().sum() + IJX0AX.array().square().sum();
  float q12 = (x0.transpose()*IJX0AX).trace();
  return q12/(sqrt(q1*q2));
}

// [[Rcpp::export]]
float pev_score(Eigen::MatrixXd X, Eigen::MatrixXd X0){
  int p = X.cols();
  int n = X.rows();
  int n0 = X0.rows();
  Eigen::MatrixXd x(n,p+1);
  x << (Eigen::ArrayXd::Zero(n)+1), X;
  Eigen::MatrixXd x0(n0,p+1);
  x0 << (Eigen::ArrayXd::Zero(n0)+1), X0;
  return (x0*((x.transpose()*x+(Eigen::MatrixXd::Identity(p+1,p+1)*(1.0/p))).inverse())*x0.transpose()).trace();
}

// [[Rcpp::export]]
float cd_score(Eigen::MatrixXd x, Eigen::MatrixXd x0){
  int p = x.cols();
  Eigen::MatrixXd cd = x0*(x.transpose()*x+(Eigen::MatrixXd::Identity(p,p)*1.0)).inverse()*x.transpose();
  return (((cd*cd.transpose()).diagonal().array())/((x0*x0.transpose()).diagonal().array())).mean();
}


#include <vector>
#include <boost/math/distributions/students_t.hpp>
#include "students_t_dist.h"

stochastic::StudentstDistribution::StudentstDistribution(double mean,
                                                         double std_dev,
                                                         double dof)
    : Distribution(),
      mean_{mean},
      std_dev_{std_dev},
      dof_{dof},
      distribution_{dof_}
{}

std::vector<double> stochastic::StudentstDistribution::cumulative_dist_func(
    const std::vector<double>& locations) const {
  std::vector<double> evaluations(locations.size());

  for (unsigned int i = 0; i < locations.size(); ++i) {
    evaluations[i] = cdf(distribution_, (locations[i] - mean_) / std_dev_);
  }

  return evaluations;
}

std::vector<double> stochastic::StudentstDistribution::inv_cumulative_dist_func(
    const std::vector<double>& probabilities) const {
  std::vector<double> evaluations(probabilities.size());

  for (unsigned int i = 0; i < probabilities.size(); ++i) {
    evaluations[i] =
        std_dev_ * quantile(distribution_, probabilities[i]) + mean_;
  }

  return evaluations;
}


#define ARMA_ALLOW_FAKE_GCC
#define ARMA_NO_DEBUG

#include "TimeSeries.hpp"

#include <armadillo>
#include <cmath>
#include <iostream>
#include <string>

using namespace arma;

namespace TimeSeries {

/*
 * Discretize Normal for given grid.
 */
vec discretizeNormal(vec grid,
    const double mean,
    const double sigma,
    bool cutTails)
{
  uword sz = grid.n_elem;
  vec prs(sz);

  if (sz == 1) {
    [[unlikely]] prs(0) = 1.0;
    return prs;
  } else if (sz == 2) {
    [[unlikely]] prs(0) = normcdf((grid(0) + grid(1)) / 2.0, mean, sigma);
    prs(1) = 1.0 - prs(0);
    return prs;
  }

#pragma omp parallel for
  for (uword ix = 1; ix < sz - 1; ++ix)
    prs(ix) = normcdf((grid(ix) + grid(ix + 1)) / 2.0, mean, sigma) - normcdf((grid(ix - 1) + grid(ix)) / 2.0, mean, sigma);

  if (cutTails) {
    const double stepOut = (grid(1) - grid(0)) / 2.0;
    prs(0) = normcdf(grid(0) + stepOut, mean, sigma) - normcdf(grid(0) - stepOut, mean, sigma);
    prs(sz - 1) = normcdf(grid(sz - 1) + stepOut, mean, sigma) - normcdf(grid(sz - 1) - stepOut, mean, sigma);

    prs /= sum(prs);
  } else {
    prs(0) = normcdf((grid(0) + grid(1)) / 2.0, mean, sigma);
    prs(sz - 1) = 1.0 - normcdf((grid(sz - 2) + grid(sz - 1)) / 2.0, mean, sigma);
  }

  return prs;
}

/*
 * Discretize Normal over uniformly spaced grid
 * spanning +/- pmSigma standard deviations.
 */
DiscreteRV
discretizeNormal(const double mean,
    const double sigma,
    const uword n_elem,
    const double pmSigma,
    bool cutTails)
{
  DiscreteRV drv;
  if (n_elem == 1) {
    drv.support = { mean };
    drv.probabilities = { 1.0 };
  } else {
    [[likely]] drv.support = linspace<vec>(mean - pmSigma * sigma, mean + pmSigma * sigma, n_elem);
    drv.probabilities = discretizeNormal(drv.support, mean, sigma, cutTails);
  }
  return drv;
}

/*
 *
 * VAR(1)
 *
 */
const MvNormal
VAR::conditional(vec& current) const
{
  MvNormal mvn;
  mvn.mean = m_intercept + m_rho * current;
  mvn.varCovar = m_sigma;
  return mvn;
}

void VAR::findStationary()
{
  const cx_vec theEigs = eig_gen(m_rho);
  if (any(abs(theEigs) >= 1.0)) {
    [[unlikely]] cout << "Eigenvalues exceeding 1 in abs value. Nonstationary."
                      << endl;
    is_stationary = false;
    m_statMean.reset();
    m_statSigma.reset();
    return;
  }

  is_stationary = true;

  const mat eyeRho = eye(m_size, m_size) - m_rho;
  m_statMean = solve(eyeRho, m_intercept);

  const mat kprod = kron(m_rho, m_rho);
  const mat theEye(kprod.n_rows, kprod.n_cols, fill::eye);
  const vec vecSigma = m_sigma.as_col();
  const vec tmp = solve(theEye - kprod, vecSigma);
  m_statSigma = reshape(tmp, m_rho.n_rows, m_rho.n_cols);
}

vec& VAR::stationaryMean()
{
  if (m_statMean.is_empty())
    findStationary();
  return m_statMean;
}

mat& VAR::stationarySigma()
{
  if (m_statSigma.is_empty())
    findStationary();
  return m_statSigma;
}

bool VAR::stationaryQ()
{
  if (m_statMean.is_empty())
    findStationary();
  return is_stationary;
}

void VAR::print()
{
  cout << "Intercept: " << endl;
  intercept().print();
  cout << "Rho: " << endl;
  rho().print();
  cout << "Sigma: " << endl;
  sigma().print();

  cout << "Stationary: " << endl;
  stationaryMean().print();
  cout << "Stationary Sigma: " << endl;
  stationarySigma().print();
}

/*
 *
 * Markov Chain
 *
 */
MarkovChain::MarkovChain(const AR process,
    const uword sz,
    const double pmMCsd,
    bool expSupport)
{
  m_size = sz;
  double pMean = process.stationaryMean();
  double pSd = process.stationarySigma();
  if (sz == 1)
    m_support = { pMean };
  else
    [[likely]] m_support = linspace<rowvec>(pMean - pmMCsd * pSd, pMean + pmMCsd * pSd, sz);

  m_tran.set_size(sz, sz);
#pragma omp parallel for
  for (uword rIx = 0; rIx < sz; ++rIx) {
    double condMean = process.intercept() + process.rho() * m_support(rIx);
    vec condPrs = discretizeNormal(m_support.t(), condMean, process.sigma());
    m_tran.row(rIx) = condPrs.t();
  }

  if (expSupport)
    m_support = exp(m_support);
}

rowvec
stationaryDistribution(const mat& transitionMatrix)
{
  cx_vec eigval;
  cx_mat eigvec;

  const mat tranT = transitionMatrix.t();
  const sp_mat sp_ver(tranT);

  eigs_opts opts;
  // opts.tol = 1.0E-6;
  eigs_gen(eigval, eigvec, sp_ver, 1, 1.0001, opts);

  const uword unitEigIx = (abs(abs(eigval) - 1.0)).index_min();

  cout << "Stationary eigenvalue " << eigval(unitEigIx) << endl;

  const rowvec stat = abs(eigvec.col(unitEigIx)).t();
  return stat / sum(stat);
}

uvec simulateChain(const mat& transitionMatrix,
    const uword initState,
    const uword simSz)
{
  const mat cumMat = cumsum(transitionMatrix, 1);
  const uword sz = transitionMatrix.n_rows;

  uvec state(simSz);

  state(0) = initState;
  const vec draws(simSz, fill::randu);
  for (uword tIx = 1; tIx < simSz; ++tIx) {
    uword newState = 0;
    for (uword stateIx = 1; stateIx < sz; ++stateIx)
      if (cumMat(state(tIx - 1), stateIx) > draws(tIx)) {
        newState = stateIx;
        break;
      }
    state(tIx) = newState;
  }

  return state;
}

const rowvec&
MarkovChain::stationary()
{
  if (m_stationary.is_empty()) {
    m_stationary = stationaryDistribution(m_tran);
  }

  return m_stationary;
}

void MarkovChain::save(const std::string fname) const
{
  m_support.save(hdf5_name(fname, "MC/support", hdf5_opts::replace));
  m_stationary.save(hdf5_name(fname, "MC/stationary", hdf5_opts::replace));
  m_tran.save(hdf5_name(fname, "MC/transitions", hdf5_opts::replace));
}

void MarkovChain::print() const
{ /* TODO */
}

void trimMarkovChain(mat& grids, mat& transition)
{
  /*
   * Based on Gordon 2020 wip.
   */
  rowvec probs = stationaryDistribution(transition);
  uword sz = probs.n_elem;

  uvec removePoints;
  uword removeCount = 0;
  for (uword ix = 0; ix < sz; ++ix)
    if (probs(ix) <= minPr) {
      [[likely]] removePoints.resize(removeCount + 1);
      removePoints(removeCount) = ix;
      removeCount++;
    }

  uword newSz = sz - removeCount;
  cout << "Removing " << removeCount << " points out of " << sz << "." << endl;

  grids.shed_rows(removePoints);
  transition.shed_rows(removePoints);
  transition.shed_cols(removePoints);
#pragma omp parallel for
  for (uword fIx = 0; fIx < newSz; ++fIx)
    transition.row(fIx) = transition.row(fIx) / sum(transition.row(fIx));
}

/*
 *
 * Discretized VAR(1)
 *
 */
void DiscreteVAR::impl(bool trimGrids, OrthoMethod method)
{
  // Sizing
  m_size = m_var.size();
  m_flatSize = prod(m_supportSizes);
  m_grids.set_size(m_flatSize, m_size);
  umat m_map(m_flatSize, m_size);

  OrthogonalizedVAR ortho(m_var, method);
  mat LL = ortho.getSupportRotationMatrix();
  VAR orthog = ortho.getVAR();

  const vec Atilde = orthog.intercept();
  const mat Btilde = orthog.rho();
  const vec DD = orthog.sigma().diag();

  const vec uncondE = orthog.stationaryMean();
  const vec uncondV = orthog.stationarySigma().diag();

  // Prepare logical grids
  field<vec> m_orthogGrids(m_size);
#pragma omp parallel for
  for (uword vIx = 0; vIx < m_size; ++vIx)
    if (m_supportSizes(vIx) > 1) {
      [[likely]] m_orthogGrids(vIx) = linspace<vec>(
          uncondE(vIx) - pmSd * sqrt(uncondV(vIx)),
          uncondE(vIx) + pmSd * sqrt(uncondV(vIx)),
          m_supportSizes(vIx));
    } else {
      m_orthogGrids(vIx) = { uncondE(vIx) };
    }

    // Prepare flat grids
#pragma omp parallel for
  for (uword flatIx = 0; flatIx < m_flatSize; ++flatIx) {
    uword tmp = flatIx;
    for (uword vIx = 0; vIx < m_size; ++vIx) {
      const vec logical = m_orthogGrids(vIx);
      m_map(flatIx, vIx) = tmp % m_supportSizes(vIx);
      m_grids(flatIx, vIx) = logical(m_map(flatIx, vIx));
      tmp = tmp / m_supportSizes(vIx);
    }
  }

  // Transition matrix
  m_tran.set_size(m_flatSize, m_flatSize);
  for (uword flatIx = 0; flatIx < m_flatSize; ++flatIx) {
    const rowvec tmpVal = m_grids.row(flatIx);
    const vec condMeans = Atilde + Btilde * tmpVal.t();
    const vec condSd = sqrt(DD);
    field<vec> condPrs(m_size);
#pragma omp parallel for
    for (uword vIx = 0; vIx < m_size; ++vIx)
      condPrs(vIx) = discretizeNormal(m_orthogGrids(vIx), condMeans(vIx), condSd(vIx));

#pragma omp parallel for
    for (uword flatPrIx = 0; flatPrIx < m_flatSize; ++flatPrIx) {
      double goPr = 1.0;
      for (uword vPrIx = 0; vPrIx < m_size; ++vPrIx) {
        vec tmpDist = condPrs(vPrIx);
        goPr *= tmpDist(m_map(flatPrIx, vPrIx));
      }
      m_tran(flatIx, flatPrIx) = goPr;
    }
  }

  // Adjust grids
  m_grids = (LL * m_grids.t()).t();

  if (trimGrids) {
    trimMarkovChain(m_grids, m_tran);
    m_flatSize = m_tran.n_rows;
  }

  // Finding midIx
  vec distances(m_flatSize);
  const vec target = m_var.stationaryMean();
#pragma omp parallel for
  for (uword flatIx = 0; flatIx < m_flatSize; ++flatIx) {
    const vec tmp = target - m_grids.row(flatIx).t();
    distances(flatIx) = sum(tmp % tmp);
  }
  m_midIx = distances.index_min();
}

void DiscreteVAR::print() const
{
  cout << "Grid:" << endl;
  m_grids.print();
  cout << endl
       << "Transition matrix: " << endl;
  m_tran.print();
}

void DiscreteVAR::save(const std::string fname) const
{
  uvec tmp = { m_midIx };
  tmp.save(hdf5_name(fname, "DVAR/midIx", hdf5_opts::replace));
  m_grids.save(hdf5_name(fname, "DVAR/grids", hdf5_opts::replace));
  m_tran.save(hdf5_name(fname, "DVAR/transitions", hdf5_opts::replace));
  m_var.intercept().save(
      hdf5_name(fname, "DVAR/varIntercept", hdf5_opts::replace));
  m_var.rho().save(hdf5_name(fname, "DVAR/varRho", hdf5_opts::replace));
  m_var.sigma().save(hdf5_name(fname, "DVAR/varSigma", hdf5_opts::replace));
}

/*
 *
 * Stochastic volatility VAR
 *
 */
void DiscreteStochVolVAR::impl(bool trimGrids)
{
  m_size = m_var.size();
  uword justDimsSz = prod(m_supportSizes);
  m_flatSize = justDimsSz * m_volGridSize;
  m_grids.set_size(m_flatSize, m_size + 1);
  umat m_map(m_flatSize, m_size + 1);

  const MarkovChain volMC(m_vol, m_volGridSize, pmSd, true);
  const rowvec& vols = volMC.support();
  const mat& volTran = volMC.transition();

  OrthogonalizedVAR ortho(m_var); // Default to Cholesky
  mat rotationLL = ortho.getSupportRotationMatrix();
  VAR orthog = ortho.getVAR();
  const vec uncondE = orthog.stationaryMean();
  const vec uncondV = orthog.stationarySigma().diag();

  const vec Atilde = orthog.intercept();
  const mat Btilde = orthog.rho();
  const vec DD = orthog.sigma().diag();

  // Prepare logical grids
  field<vec> m_orthogGrids(m_size, m_volGridSize);
#pragma omp parallel for collapse(2)
  for (uword volIx = 0; volIx < m_volGridSize; ++volIx)
    for (uword iIx = 0; iIx < m_size; ++iIx)
      if (m_supportSizes(iIx) > 1) {
        [[likely]] m_orthogGrids(iIx, volIx) = linspace<vec>(
            uncondE(iIx) - pmSd * vols(volIx) * sqrt(uncondV(iIx)),
            uncondE(iIx) + pmSd * vols(volIx) * sqrt(uncondV(iIx)),
            m_supportSizes(iIx));
      } else {
        m_orthogGrids(iIx, volIx) = { uncondE(iIx) };
      }

      // Prepare flat grids
#pragma omp parallel for collapse(2)
  for (uword volIx = 0; volIx < m_volGridSize; ++volIx) {
    for (uword flatIx = 0; flatIx < justDimsSz; ++flatIx) {
      uword withV = justDimsSz * volIx + flatIx;

      m_map(withV, m_size) = volIx;
      m_grids(withV, m_size) = vols(volIx);

      uword tmp = flatIx;
      for (uword iIx = 0; iIx < m_size; ++iIx) {
        const vec logical = m_orthogGrids(iIx, volIx);
        m_map(withV, iIx) = tmp % m_supportSizes(iIx);
        m_grids(withV, iIx) = logical(m_map(withV, iIx));
        tmp /= m_supportSizes(iIx);
      }
    }
  }

  // Transition matrix
  m_tran.set_size(m_flatSize, m_flatSize);
  m_tran.fill(0.0);
#pragma omp parallel for
  for (uword flatIx = 0; flatIx < m_flatSize; ++flatIx) {
    const uword thisVolIx = m_map(flatIx, m_size);
    const rowvec thisValues = m_grids.row(flatIx).head(m_size);
    const vec condMeans = Atilde + Btilde * thisValues.t();
    const vec condSdReference = sqrt(DD);

    field<vec> condPrs(m_size);
    for (uword iIx = 0; iIx < m_size; ++iIx)
      condPrs(iIx) = discretizeNormal(m_orthogGrids(iIx, thisVolIx), condMeans(iIx), vols(thisVolIx) * condSdReference(iIx));

    for (uword flatPrIx = 0; flatPrIx < m_flatSize; ++flatPrIx) {
      double goPr = 1.0;
      for (uword iiIx = 0; iiIx < m_size; ++iiIx) {
        const vec condPrVec = condPrs(iiIx);
        goPr *= condPrVec(m_map(flatPrIx, iiIx));
      }
      const uword vPrIx = m_map(flatPrIx, m_size);
      m_tran(flatIx, flatPrIx) = goPr * volTran(thisVolIx, vPrIx);
    }
  }

  // Adjust grids
  m_grids.cols(0, m_size - 1) = (rotationLL * m_grids.cols(0, m_size - 1).t()).t();

  if (trimGrids) {
    [[likely]] trimMarkovChain(m_grids, m_tran);
    m_flatSize = m_tran.n_rows;
  }

  // Finding midIx
  vec distances(m_flatSize);
  vec uncondEextended(m_size + 1);
  uncondEextended.head(m_size) = m_var.stationaryMean();
  uncondEextended(m_size) = vols(m_volGridSize / 2);
#pragma omp parallel for
  for (uword flatIx = 0; flatIx < m_flatSize; ++flatIx) {
    const vec tmp = uncondEextended - m_grids.row(flatIx).t();
    distances(flatIx) = sum(tmp % tmp);
  }
  m_midIx = distances.index_min();
}

void DiscreteStochVolVAR::save(const std::string fname) const
{
  uvec tmp = { m_midIx };
  tmp.save(hdf5_name(fname, "DsvVAR/midIx", hdf5_opts::replace));
  m_grids.save(hdf5_name(fname, "DsvVAR/grids", hdf5_opts::replace));
  m_tran.save(hdf5_name(fname, "DsvVAR/transitions", hdf5_opts::replace));
  m_var.intercept().save(
      hdf5_name(fname, "DsvVAR/varIntercept", hdf5_opts::replace));
  m_var.rho().save(hdf5_name(fname, "DsvVAR/varRho", hdf5_opts::replace));
  m_var.sigma().save(hdf5_name(fname, "DsvVAR/varSigma", hdf5_opts::replace));

  vec volAR = { m_vol.intercept(), m_vol.rho(), m_vol.sigma() };
  volAR.save(
      hdf5_name(fname, "DsvVAR/volatilityAR1params", hdf5_opts::replace));
}

void DiscreteStochVolVAR::print() const
{ /* TODO */
}

} // namespace TimeSeries


//
//  main_picking.cpp
//  HCI 557 Picking example
//
//  Created by Rafael Radkowski on 5/28/15.
//  Copyright (c) 2015 -. All rights reserved.
//

// stl include
#include <iostream>
#include <string>
#include <math.h>
#include <map>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include <glm/gtx/transform.hpp> // after <glm/glm.hpp>

#include "Bezier.h"

using namespace std;

// stores all binomnal coefficents that have been calculated in this program.
// [  n , [k, coeff] ]
map<unsigned int, map< unsigned int,  unsigned int> > binominal_coefficients;

bool eigenvaluessort(pair<float, Eigen::Vector3f > i, pair<float, Eigen::Vector3f > j)
{
    return (i.first > j.first); // largest first
}

/*!
@brief - calculate the eigenvectors for a given point using the points in its surrounding.
@param centroid - the centroid of the eigenvectors
@param points - all the other points
@param eigenvectors - a 4x4 matrix which column vectors are the eigen vectors
*/
//static
void calcEigenvectors(const glm::vec3& centroid, const vector<glm::vec3>& points, vector<glm::vec3>& eigenvectors)
{

    Eigen::Matrix3f cov;
    cov << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;

    size_t size = points.size();
    for (int i=0; i<size; i++) {

        glm::vec3 p = points[i];
        cov(0,0) += (p[0] - centroid[0] ) * (p[0] - centroid[0]);
        cov(0,1) += (p[0] - centroid[0] ) * (p[1] - centroid[1]);
        cov(0,2) += (p[0] - centroid[0] ) * (p[2] - centroid[2]);

        cov(1,0) += (p[1] - centroid[1] ) * (p[0] - centroid[0]);
        cov(1,1) += (p[1] - centroid[1] ) * (p[1] - centroid[1]);
        cov(1,2) += (p[1] - centroid[1] ) * (p[2] - centroid[2]);

        cov(2,0) += (p[2] - centroid[2] ) * (p[0] - centroid[0]);
        cov(2,1) += (p[2] - centroid[2] ) * (p[1] - centroid[1]);
        cov(2,2) += (p[2] - centroid[2] ) * (p[2] - centroid[2]);
    }

    cov(0,0) /= size;
    cov(0,1) /= size;
    cov(0,2) /= size;

    cov(1,0) /= size;
    cov(1,1) /= size;
    cov(1,2) /= size;

    cov(2,0) /= size;
    cov(2,1) /= size;
    cov(2,2) /= size;

    Eigen::EigenSolver<Eigen::Matrix3f> es(cov);

    // Columns are the eigenvectors
    Eigen::Matrix3f D = es.pseudoEigenvalueMatrix();
    Eigen::Matrix3f V = es.pseudoEigenvectors();

    Eigen::Vector3f e0( V(0), V(1), V(2));
    Eigen::Vector3f e1( V(3), V(4), V(5));
    Eigen::Vector3f e2( V(6), V(7), V(8));

    //Eigen::Vector3f e0( V(0), V(3), V(6));
    //Eigen::Vector3f e1( V(1), V(4), V(7));
    //Eigen::Vector3f e2( V(2), V(5), V(8));

    float ev0 = D(0);
    float ev1 = D(4);
    float ev2 = D(8);

    vector<pair<float, Eigen::Vector3f > > results(3);
    results[0] = make_pair(ev0, e0);
    results[1] = make_pair(ev1, e1);
    results[2] = make_pair(ev2, e2);

    // sort the vectors, largest first.
    std::sort (results.begin(), results.end(), eigenvaluessort);

    eigenvectors = vector<glm::vec3>(3);
    eigenvectors[0] = glm::vec3(results[0].second(0), results[0].second(1), results[0].second(2));
    eigenvectors[1] = glm::vec3(results[1].second(0), results[1].second(1), results[1].second(2));
    eigenvectors[2] = glm::vec3(results[2].second(0), results[2].second(1), results[2].second(2));

   // cout << "Covariance matrix:" << endl << cov << endl;
  //  cout << "The eigenvalues of A are:" << endl << es.eigenvalues() << endl;
  //  cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl;

    return;

}

/*!
@brief Function to calculate all eigenvectors for all points
@param all_matches -  the matches for all points in "points". The index of each matchs complies with the point index.
@param points - all points
@param eigenvectors_per_point - the result eigenvectors per point. The index of each matchs complies with the point index.
*/
void computeReferenceFrames( const int n, const int m, glm::vec3 up, const vector< vector<float> >& points, vector< vector<glm::vec3> >& eigenvector_per_point )
{

    for(int i=0; i<m; i++)
    {
        for(int j=0; j<n; j++)
        {
            int current = i*n + j;
            vector<float> p = points[current];

            // find neighbors.
            vector< glm::vec3 > neighbors;

            const int box[8] = { -n-1, -1 ,n-1,
                                -n ,       n,
                                -n+1, +1, n+1};

            for(int k=0; k<8; k++)
            {
                int idx = i + box[k];
                if(idx > 0 && idx < points.size() ) neighbors.push_back( glm::vec3(points[idx][0],points[idx][1], points[idx][2] ));
            }

            glm::vec3 centroid(p[0], p[1], p[2]);
            vector<glm::vec3> eigenvectors;

            calcEigenvectors(centroid, neighbors, eigenvectors);

             ///////////////////////////////////////////////////////////////////
            //
            // Check for right-hand coordinate system.
            //
            // Calc the cross product between x, and y
            glm::vec3 cp = cross(eigenvectors[0], eigenvectors[1]);
            float angle = acos( dot(cp, eigenvectors[2]) );

            if( angle != angle ) // Check for NaN - tiny value, good.
            {
                // do not do anything. vector is fine.
            }
            else if(angle > 3.00)
            {
            //   cout << "replace " << i << " a: " << angle << endl;
                eigenvectors[2] = cp; //replace with cp
            }

            ///////////////////////////////////////////////////////////////////
            //
            // Verify alignment and flip the coordinate system if it is not aligned with
            // the alignment axis. (Should be the camera axis later. )
            //
            static glm::vec3 flip(-1,-1,-1);

            float angle_alignment = acos( dot(eigenvectors[2], up) );

            if(angle_alignment > 1.57)
            {
                eigenvectors[1] = flip * eigenvectors[1];
                eigenvectors[2] = flip * eigenvectors[2];

            }

            // store the eigenvectors
            eigenvector_per_point.push_back(eigenvectors);
        }
    }

}

/*!
@brief - transfers an eigenvector into a matrix in homogenous coordinates.
@param eigenvectors  - vector with eigenvectors sorted corresponding to the eigenvalues, largest first ev1 > ev2 > ev3
@param matrix - the glm matrix.
Notice that all matrix types are column-major rather than row-major.
*/
//static
void Eigenvec2matrix(const vector< glm::vec3 >& eigenvectors, glm::mat4& matrix)
{
    matrix = glm::mat4();

    matrix[0][0] = eigenvectors[0][0];
    matrix[0][1] = eigenvectors[0][1];
    matrix[0][2] = eigenvectors[0][2];

    matrix[1][0] = eigenvectors[1][0];
    matrix[1][1] = eigenvectors[1][1];
    matrix[1][2] = eigenvectors[1][2];

    matrix[2][0] = eigenvectors[2][0];
    matrix[2][1] = eigenvectors[2][1];
    matrix[2][2] = eigenvectors[2][2];

    matrix[3][0] = 0.0;
    matrix[3][1] = 0.0;
    matrix[3][2] = 0.0;

}

/*!
Calculates the factorial of a number
@param x - the value that should be factorized.
@return - the factor.
*/
int factorial(int x, int result) {
  if (x == 1  || x == 0) return result;
  else return factorial(x - 1, x * result);
}

/*!
Calculate the binominal coefficient for n and k
@param n - the number of line segments where the number of supporting points is n+1
@param k - the current point
*/
int BinomialCoeff(int n, int k)
{
    int c = binominal_coefficients[n][k];

    if(c==0) // binominal coefficient is min = 1, 0! = 1
    {
        c = factorial(n)/ ( factorial(k) * factorial(n-k) );
        binominal_coefficients[n][k] = c;

        return c;

    }
    return c;
    //return factorial(n)/ ( factorial(k) * factorial(n-k) );
}

/*!
Calculate the bernstein polynom for P(x) = BC(n,k)  x^k (1-x)^{n-k}
@param x - the interpolation variable x = [0,1]
@param n - the number of line segments where the number of supporting points is n+1
@param k - the current point
*/
float ComputeBernsteinP(float x, int n, int k)
{
    int c = BinomialCoeff(n,k);

    float p_x = float(c) * pow(x, k) *  pow((1.0-x), n-k);

    return p_x;
}

/*!
Computes the points for a cubic spline
@param control_points - a vector of four points in x,y or x,y,z.
@param result - the output points
@param num - the number of points that should be generated along the spline
*/
bool ComputeCubicSpline(const int num, const vector< vector<float> >& control_points, vector< vector<float> >& result)
{
    result.clear();

    float increment = 1.0/float(num-1);

    vector<float> cp0 = control_points[0];
    vector<float> cp1 = control_points[1];
    vector<float> cp2 = control_points[2];
    vector<float> cp3 = control_points[3];

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

        double t = increment * i;

        vector<float>p(3);
        p[0] = cp0[0] * ComputeBernsteinP(t,3,0) + cp1[0] * ComputeBernsteinP(t,3,1) + cp2[0] * ComputeBernsteinP(t,3,2) + cp3[0] * ComputeBernsteinP(t,3,3);
        p[1] = cp0[1] * ComputeBernsteinP(t,3,0) + cp1[1] * ComputeBernsteinP(t,3,1) + cp2[1] * ComputeBernsteinP(t,3,2) + cp3[1] * ComputeBernsteinP(t,3,3);
        p[2] = cp0[2] * ComputeBernsteinP(t,3,0) + cp1[2] * ComputeBernsteinP(t,3,1) + cp2[2] * ComputeBernsteinP(t,3,2) + cp3[2] * ComputeBernsteinP(t,3,3);

        cout << t << " :\t" << p[0] << "\t" << p[1] << "\t" << p[2] << endl;

        result.push_back(p);
    }

    return true;
}

/*!
Computes the points for a cubic spline
@param control_points - a vector of four points in x,y or x,y,z.
@param result - the output points
@param num - the number of points that should be generated along the spline
*/
bool ComputeCubicSplineC(const int num, const vector< vector<float> >& control_points, vector< vector<float> >& result)
{
    result.clear();

    float increment = 1.0/float(num-1);

    vector<float> cp0 = control_points[0];
    vector<float> cp1 = control_points[1];
    vector<float> cp2 = control_points[2];
    vector<float> cp3 = control_points[3];

    // the bezier matrix
    static vector< vector<float> > bm( 4, vector<float>(4, 0.0));
    bm[0][0] = -1; bm[0][1] = 3;  bm[0][2] = -3; bm[0][3] = 1;
    bm[1][0] = 3;  bm[1][1] = -6; bm[1][2] = 3;  bm[1][3] = 0;
    bm[2][0] = -3; bm[2][1] = 3;  bm[2][2] = 0;  bm[2][3] = 0;
    bm[3][0] = 1; bm[3][1] = 0;  bm[3][2] = 0;  bm[3][3] = 0;

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

        float t = increment * i;

        float C0 = bm[0][0] * pow(t, 3) + bm[1][0] * pow(t, 2) + bm[2][0] * t + bm[3][0];
        float C1 = bm[0][1] * pow(t, 3) + bm[1][1] * pow(t, 2) + bm[2][1] * t + bm[3][1];
        float C2 = bm[0][2] * pow(t, 3) + bm[1][2] * pow(t, 2) + bm[2][2] * t + bm[3][2];
        float C3 = bm[0][3] * pow(t, 3) + bm[1][3] * pow(t, 2) + bm[2][3] * t + bm[3][3];

        vector<float>p(3);
        p[0] = C0 * cp0[0] + C1 * cp1[0] + C2 * cp2[0] +  C3 * cp3[0];
        p[1] = C0 * cp0[1] + C1 * cp1[1] + C2 * cp2[1] +  C3 * cp3[1];
        p[2] = C0 * cp0[2] + C1 * cp1[2] + C2 * cp2[2] +  C3 * cp3[2];

        cout << t << " :\t" << p[0] << "\t" << p[1] << "\t" << p[2] << endl;
        result.push_back(p);
    }

    return true;
}

/*!
Computes a cubic surface patch
@param control_points - a vector of four points in x,y or x,y,z.
@param result - the output points
@param num - the number of points that should be generated along the spline.

NOTE: This code is not fast, it is written that way for clarity.
Usually, one would precomupute all values for BEZ * BEZ and put them into a table.

*/
bool ComputeCubicPatch(const int num, const vector< vector<float> >& control_points, vector< vector<float> >& result)
{
    result.clear();
    float increment = 1.0/float(num-1);

    vector<float> cp00 = control_points[0];
    vector<float> cp01 = control_points[1];
    vector<float> cp02 = control_points[2];
    vector<float> cp03 = control_points[3];

    vector<float> cp10 = control_points[4];
    vector<float> cp11 = control_points[5];
    vector<float> cp12 = control_points[6];
    vector<float> cp13 = control_points[7];

    vector<float> cp20 = control_points[8];
    vector<float> cp21 = control_points[9];
    vector<float> cp22 = control_points[10];
    vector<float> cp23 = control_points[11];

    vector<float> cp30 = control_points[12];
    vector<float> cp31 = control_points[13];
    vector<float> cp32 = control_points[14];
    vector<float> cp33 = control_points[15];

    // runs along the first coordinate
    for (int i=0; i<num; i++) {
        float t = increment * i;

        // runs along the second corrdinate
        for (int j=0; j<num; j++) {
            float v = increment * j;

            vector<float>p(3);

            // x-coordinate
            float px0 =  cp00[0] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,0) + cp01[0] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,1) + cp02[0] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,2) + cp03[0] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,3);
            float px1 =  cp10[0] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,0) + cp11[0] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,1) + cp12[0] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,2) + cp13[0] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,3);
            float px2 =  cp20[0] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,0) + cp21[0] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,1) + cp22[0] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,2) + cp23[0] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,3);
            float px3 =  cp30[0] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,0) + cp31[0] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,1) + cp23[0] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,2) + cp33[0] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,3);
            p[0] = px0 + px1 + px2 + px3;

            // y-coordinate
            float py0 =  cp00[1] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,0) + cp01[1] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,1) + cp02[1] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,2) + cp03[1] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,3);
            float py1 =  cp10[1] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,0) + cp11[1] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,1) + cp12[1] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,2) + cp13[1] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,3);
            float py2 =  cp20[1] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,0) + cp21[1] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,1) + cp22[1] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,2) + cp23[1] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,3);
            float py3 =  cp30[1] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,0) + cp31[1] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,1) + cp23[1] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,2) + cp33[1] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,3);
            p[1] = py0 + py1 + py2 + py3;

            // z-coordinate
            float pz0 =  cp00[2] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,0) + cp01[2] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,1) + cp02[2] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,2) + cp03[2] * ComputeBernsteinP(v,3,0)  * ComputeBernsteinP(t,3,3);
            float pz1 =  cp10[2] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,0) + cp11[2] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,1) + cp12[2] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,2) + cp13[2] * ComputeBernsteinP(v,3,1)  * ComputeBernsteinP(t,3,3);
            float pz2 =  cp20[2] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,0) + cp21[2] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,1) + cp22[2] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,2) + cp23[2] * ComputeBernsteinP(v,3,2)  * ComputeBernsteinP(t,3,3);
            float pz3 =  cp30[2] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,0) + cp31[2] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,1) + cp23[2] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,2) + cp33[2] * ComputeBernsteinP(v,3,3)  * ComputeBernsteinP(t,3,3);
            p[2] = pz0 + pz1 + pz2 + pz3;

            cout << t << ", " << v << " :\t" << p[0] << "\t" << p[1] << "\t" << p[2] << endl;
            result.push_back(p);

        }
    }

    return true;
}

/*!
Computes / reorganizes all points so they can be used as triangles to render a mesh
@param n, m, the size of the mesh in number of points as columns n and rows m
@param control_points, control points as vec3 <float> [x, y, x].

     0------1-------2-------3
     |      |       |       |
     4------5-------6-------7
     |      |       |       |
     8------9-------10------11
     |      |       |       |
     12-----13------14------15

@param vertices, the vertices in an vector. Three vertices in a row are one triangle
@param normals, a normal vector for each point
*/
bool TriangulateCubicPatch(const int n, const int m, const vector< vector<float> >& points, const glm::vec3 up, vector< glm::vec3 >& vertices, vector< glm::vec3 >& normals )
{
    int num = (n * m) - n;

    vertices.clear();
    normals.clear();

    // Here, we compute the normal vectors.
    // Note, this can also be optimized in order to speedup the process.
    // However, I compute everything in single steps so you guys can easily find and distinguish them.
    vector< vector<glm::vec3> > eigenvector_per_point;
    computeReferenceFrames( n, m, up, points, eigenvector_per_point);

    // lets find the vertices and let's associate one normal vector per vertex.
    for(int i=0; i<num; i++)
    {
        // suppose we have four control points per quad. We need two triangles per quad
        /*
            0--n--2n--3n
            | /|
            |/ |
            1--n+1

        */

        // triangle 0
        if( i%(n) < n-1 )
        {
            int idx0 = i;
            int idx1 = i+1;
            int idx2 = i + n;

            //cout << 0 <<": " << idx0 << "\t:" << idx1 << "\t" << idx2 << endl;

            vertices.push_back( glm::vec3( points[idx0][0], points[idx0][1], points[idx0][2]  ));
            vertices.push_back( glm::vec3( points[idx1][0], points[idx1][1], points[idx1][2]  ));
            vertices.push_back( glm::vec3( points[idx2][0], points[idx2][1], points[idx2][2]  ));

            normals.push_back(eigenvector_per_point[idx0][2]);
            normals.push_back(eigenvector_per_point[idx1][2]);
            normals.push_back(eigenvector_per_point[idx2][2]);
        }

        // triangle 1
        if( i%(n) < n-1 )
        {
            int idx0 = i+1;
            int idx1 = idx0+n;
            int idx2 = idx0+n-1;

            //cout << 1 <<": " << idx0 << "\t:" << idx1 << "\t" << idx2 << endl;

            vertices.push_back( glm::vec3( points[idx0][0], points[idx0][1], points[idx0][2]  ));
            vertices.push_back( glm::vec3( points[idx1][0], points[idx1][1], points[idx1][2]  ));
            vertices.push_back( glm::vec3( points[idx2][0], points[idx2][1], points[idx2][2]  ));

            normals.push_back(eigenvector_per_point[idx0][2]);
            normals.push_back(eigenvector_per_point[idx1][2]);
            normals.push_back(eigenvector_per_point[idx2][2]);
        }
    }

    return true;
}


#include <fstream>
#include <map>
#include <set>
#include <vector>

#include <iostream>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

#include <Eigen/Dense>
#include <gmpxx.h>

#include "LinearAlgebra.hxx"

typedef Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> MatrixXq;
typedef Eigen::Matrix<mpq_class, Eigen::Dynamic, 1> VectorXq;

template<class Archive>
void eval_mat::serialize (Archive & ar, unsigned int const) {
  ar & values;
}

void eval_mat::save (std::string const & filename) const {
  std::ofstream file;
  file.open(filename);
  boost::archive::text_oarchive oa {file};
  oa << *this;
}

void eval_mat::load (std::string const & filename) {
  std::ifstream file;
  file.open(filename);
  boost::archive::text_iarchive ia {file};
  ia >> *this;
}

std::set<size_t> eval_mat::row_set () const {
  std::set<size_t> ret;
  std::transform (values.cbegin(), values.cend(), std::inserter (ret, ret.begin()),
    [] (auto const & v) {
      return v.first.second;
    });
  return ret;
}

std::set<size_t> findDependentVariables (std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> const & matrix, size_t rows, size_t cols) {
  MatrixXq mq(cols, rows);

  std::for_each(matrix.cbegin(), matrix.cend(),
    [&mq] (auto const & v) {
      mq (v.first.second, v.first.first) = v.second;
    });

  Eigen::FullPivLU<MatrixXq> lu_decompq(mq);

  std::cout << "rank of the system : " << lu_decompq.rank() << std::endl;

  std::set<size_t> ret;
  VectorXq vector = VectorXq::Zero (cols);
  for (size_t counter = 0; counter < cols; ++counter) {
    if (counter != 0) {
      vector (cols - counter) = 0;
    }
    vector (cols - counter - 1) = 1;
    if (lu_decompq.solve(vector).isZero()) {
      ret.insert (cols - counter - 1);
    }
  }

  return ret;
}

std::map<size_t, std::map<size_t, mpq_class>> solveLinearSystem (std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> const & matrix, size_t rows, size_t cols) {
  MatrixXq mq (rows, cols);

  std::for_each (matrix.cbegin(), matrix.cend(),
      [&mq] (auto const & v) {
        mq (v.first.first, v.first.second) = v.second;
      });

  Eigen::FullPivLU<MatrixXq> lu_decompq(mq);

  std::cout << "rank of the system : " << lu_decompq.rank() << std::endl;

  std::cout << lu_decompq.kernel() << std::endl;

  return {};

}