/**
 * @file main.cpp
 * @brief Entry-point for dynamic notch filter example
 * @author Parker Lusk <parkerclusk@gmail.com>
 * @date 8 Dec 2020
 */

#include <chrono>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>

#include <Eigen/Dense>
#include <unsupported/Eigen/FFT>

#include <plot.hpp>

#include <adaptnotch/adaptnotch.h>

#include "csv.h"

static constexpr int DATUMS = 4; // number of columns to be extracted from CSV
using Data = Eigen::Matrix<double, Eigen::Dynamic, DATUMS>;

void usage(int argc, char const *argv[])
{
  std::cout << argv[0] << " <input csv data> [axis] [plot]" << std::endl << std::endl;
  std::cout << "\tRun adaptive notching algorithm on gyro data stored in CSV.";
  std::cout << std::endl << "\t";
  std::cout << "CSV file expected to have been generated from sfpro or to be ";
  std::cout << std::endl << "\t";
  std::cout << "in same format. A gyro axis to analyze may be specified as";
  std::cout << std::endl << "\t";
  std::cout << "(1, 2, 3) which corresponds to axes (x, y, z).";
  std::cout << std::endl << std::endl;
  std::cout << "\tIf a 3rd argument is specified (e.g., '1'), an ASCII plot";
  std::cout << std::endl << "\t";
  std::cout << "of the pre- and post-spectrum is shown in the terminal.";
  std::cout << std::endl << std::endl;
}

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

Data parseCSV(const std::string& file)
{

  // count number of entries (estimate)
  std::ifstream ifile(file);
  const int N = std::count(std::istreambuf_iterator<char>(ifile),
                            std::istreambuf_iterator<char>(), '\n');
  ifile.close();

  io::CSVReader<DATUMS> in(file);
  in.next_line(); // ignore "dsp clock" message

  // we only care about these four (DATUMS) columns
  in.read_header(io::ignore_extra_column,
                  "timestamp(us)", "ang_x", "ang_y", "ang_z");

  Data D = Data::Zero(N, DATUMS);
  int i = 0;
  int time_us;
  double wx, wy, wz;
  while (in.read_row(time_us, wx, wy, wz)) {
    D.row(i++) << time_us*1e-6, wx, wy, wz;
  }

  // resize to however many valid entries there were
  D.conservativeResize(i, DATUMS);

  return D;
}

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

void plotResults(const adaptnotch::AdaptiveNotch& filter,
            const Eigen::VectorXd& gyro, const Eigen::VectorXd& gyrof, int n)
{
  static const int N = filter.params().NFFT;
  static Eigen::FFT<double> fft;
  static plot::TerminalInfo term;
  static bool init = false;
  if (!init) {
    term.detect();
    fft.SetFlag(Eigen::FFT<double>::Unscaled);
    fft.SetFlag(Eigen::FFT<double>::HalfSpectrum);
    init = true;
  }
  constexpr float ymax = 0.2f; // arbitrary FFT mag scaling
  static plot::RealCanvas<plot::BrailleCanvas> prefilter({ { 0.0f, ymax }, { N/2.0f, 0.0f } }, plot::Size(60, 10), term);
  static plot::RealCanvas<plot::BrailleCanvas> postfilter({ { 0.0f, ymax }, { N/2.0f, 0.0f } }, plot::Size(60, 10), term);

  // Build block layout
  auto layout =
      plot::alignment(
          { term.size().x, 0 },
          plot::margin(
                  plot::vbox(
                      plot::frame(u8"Original Spectrum", plot::Align::Center, &prefilter),
                      plot::frame(u8"Filtered Spectrum", plot::Align::Center, &postfilter))));

  // pre-filter spectrum
  const Eigen::VectorXd Y = filter.spectrum() / N;

  // select N most recent filtered measurements
  const size_t s = (n-N<0) ? 0 : n-N;
  const Eigen::VectorXd yf = gyrof.segment(s,N);

  // post-filter spectrum
  Eigen::VectorXcd Yfc;
  fft.fwd(Yfc, yf);
  const Eigen::VectorXd Yf = Yfc.array().abs() / N;

  // Plot spectrum pre-filtering
  prefilter.clear();
  for (size_t i=1; i<N/2; i++) {
    prefilter.path(plot::palette::royalblue,{{static_cast<float>(i-1), static_cast<float>(Y(i-1))},
                                          {static_cast<float>(i), static_cast<float>(Y(i))}});
  }

  // Plot spectrum post-filtering
  postfilter.clear();
  for (size_t i=1; i<N/2; i++) {
    postfilter.path(plot::palette::royalblue,{{static_cast<float>(i-1), static_cast<float>(Yf(i-1))},
                                          {static_cast<float>(i), static_cast<float>(Yf(i))}});
  }

  for (auto const& line: layout)
    std::cout << term.clear_line() << line << std::endl;
  std::cout << term.move_up(layout.size().y) << std::flush;
}

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

int main(int argc, char const *argv[])
{

  int axis = 1;     ///< x, y, or z axis of gyro to analyze
  std::string file; ///< input data from IMU
  bool shouldPlot = false; ///< show FFT plots in terminal

  if (argc < 2) {
    std::cerr << "Not enough input arguments." << std::endl << std::endl;
    usage(argc, argv);
    return -1;
  } else if (argc >= 2) {
    file = std::string(argv[1]);
  }

  if (argc >= 3) {
    axis = std::stoi(argv[2]);
    if (axis < 1 || axis > 3) axis = 1;
  }

  if (argc >= 4) shouldPlot = true;

  //
  // Process raw IMU data
  //

  Data D = parseCSV(file);

  const Eigen::VectorXd diff = D.col(0).bottomRows(D.rows()-1) - D.col(0).topRows(D.rows()-1);
  const double Ts = diff.mean();
  const double Fs = 1./Ts;
  const int N = D.rows();

  //
  // Adaptive notch filter setup
  //

  adaptnotch::AdaptiveNotch::Params params;
  adaptnotch::AdaptiveNotch filter(params);

  //
  // Main loop - simulated gyro sampling
  //

  Eigen::VectorXd gyrof = Eigen::VectorXd::Zero(N);
  const auto start = std::chrono::steady_clock::now();

  for (size_t n=0; n<N; n++) {
    const double gyro = D(n, axis);
    gyrof(n) = filter.apply(gyro);
    if (shouldPlot) plotResults(filter, D.col(axis), gyrof, n);
  }

  //
  // Timing stats
  //

  const auto end = std::chrono::steady_clock::now();
  const double duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() * 1e-6;

  const double timu = D(N-1,0) - D(0,0);

  std::cout << "Processed " << timu << " seconds (" << N << " samples) of IMU";
  std::cout << " data in " << duration << " seconds" << std::endl;
  std::cout << "Real-time factor: " << timu / duration << std::endl;
  std::cout << "Estimated peak freq: " << filter.peakFreq() << std::endl;

  //
  // Write data to file
  //

  Eigen::MatrixXd out(N, 3);
  out << Eigen::VectorXd::LinSpaced(N, 0, N*Ts), D.col(axis), gyrof;

  std::ofstream of("data_processed.txt");
  of << out << std::endl;
  of.close();

  return 0;
}


//  Copyright (c) 2019 AUTHORS
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
#include "octotiger/grid.hpp"

#include <hpx/runtime/threads/run_as_os_thread.hpp>

#include "octotiger/test_problems/blast.hpp"

#include <algorithm>
#include <functional>
#include <memory>
#include <mutex>
#include <unordered_map>
#include <vector>

#if !defined(OCTOTIGER_HAVE_BOOST_MULTIPRECISION)
#include <quadmath.h>
using sed_real = __float128;
#else
#include <boost/multiprecision/cpp_bin_float.hpp>
using sed_real = boost::multiprecision::cpp_bin_float_quad;
#endif

/*extern "C" {*/
/* Subroutine */int sed_1d__(sed_real *time, int *nstep, sed_real *xpos, sed_real *eblast, sed_real *omega_in__, sed_real *xgeom_in__, sed_real *rho0,
		sed_real *vel0, sed_real *ener0, sed_real *pres0, sed_real *cs0, sed_real *gam0, sed_real *den, sed_real *ener, sed_real *pres, sed_real *vel,
		sed_real *cs);
//}

constexpr real blast_wave_t0 = 7e-4;

std::vector<real> blast_wave_analytic(real x, real y, real z, real t) {
	static const auto dxmin = 2.0 * opts().xscale / INX / double(1 << opts().max_level);
	real r = std::sqrt(x * x + y * y + z * z);
	r = std::max(r, dxmin * 1.0e-3);
	t += blast_wave_t0;
	real rmax = 3.0 * opts().xscale;
	real d, v, p;
	sedov::solution(t, r, rmax, d, v, p);
	std::vector<real> u(opts().n_fields, 0.0);
	u[rho_i] = u[spc_i] = std::max(d, 1.0e-20);
	real s = d * v;
	u[sx_i] = s * x / r;
	u[sy_i] = s * y / r;
	u[sz_i] = s * z / r;
	real e = std::max(p / (grid::get_fgamma() - 1), 1.0e-20);
	u[egas_i] = e + s * v * 0.5;
	u[tau_i] = std::pow(e, 1 / grid::get_fgamma());
	return u;
}

std::vector<real> blast_wave(real x, real y, real z, real dx) {
	std::vector<real> u(opts().n_fields, 0.0);
	u[rho_i] = u[spc_i] = 1.0;
	const auto r2 = x * x + y * y + z * z;
	const auto rmax = dx * 3.5;
	if (r2 < rmax * rmax) {
		u[egas_i] = opts().eblast0 / (dx * dx * dx)  / 160.0;
	} else {
		u[egas_i] = 1.0e-20;
	}
	u[tau_i] = std::pow(u[egas_i], 1.0 / grid::get_fgamma());
	return u;

}


// c10e14ExprWithSimbolTable.cpp: определяет точку входа для консольного приложения.
//

#include "stdafx.h"
#include <boost/regex.hpp>
#include <iostream>
#include <sstream>
typedef std::string str;
class Expr
{
public:
	Expr(const char* a) { s0 = a;reed(); }
	int Eval()
	{
		std::vector<str> ter = terms;
		std::vector<str> sig = signs;
		boost::smatch sm;
		for (auto &t : ter)
		{
			while (boost::regex_search(t, sm, boost::regex("([0-9]+)(" + ops[0] + ")([0-9]+)")))
			{
				t.replace(sm.position(),sm.length(),f(sm[1], sm[2], sm[3]) );
				std::cout << t << std::endl;
			}
		}
		for (auto t : ter)
			std::cout << t << std::endl;
		if (ter.size() == sig.size()) ter.insert(ter.begin(), "0");
		while (ter.size() > 1)
		{
			ter[0] = f(ter[0], sig[0], ter[1]);
			ter.erase(ter.begin());
			sig.erase(sig.begin());
		}
/*
		str s = s0;

		boost::smatch f00;
		for (auto o : ops)
		{
			while (boost::regex_search(s, f00, boost::regex(str("((?:^-)?([0-9]+))([") + o + "])((?2))")))
			{
				s.replace(f00.position(), f00.length(),f(f00[1], f00[3], f00[4]));
			}
		}
		i = s_to_i(s);*/
		i = s_to_i(ter[0]);
		return i;
	}
	//void print() { std::cout << i << std::endl; }
	void printBrackets()
	{

		std::vector<str> ter = terms;
		int ts = ter.size();
		int ss = signs.size();
		if (ts > ss) { std::cout << '(' << ter[0] << ')'; ter.erase(ter.begin()); }
		for (int i = 0; i < ts; i++)
		{
			std::cout << signs[i] << '(' << ter[i] << ')';
		}
		std::cout << std::endl;
	}

private:
	str s0;
	int s_to_i(str z)
	{
		std::stringstream io;
		io << z;
		int i;
		io >> i;
		return i;
	}
	int i;
	std::vector<str> ops = { "\\*|/","+-" };
	std::vector<str> terms;
	std::vector<str> signs;
	void reed()
	{
		boost::regex e("([+-])?([^+-]+)");
		boost::sregex_iterator it(s0.begin(), s0.end(), e);
		boost::sregex_iterator itend;
		while (it != itend)
		{
			str z = (*it)[1];
			if(z.size()>0) signs.push_back(z);
			terms.push_back((*it)[2]);
			it++;
		}
	}
	str f(str x0, str op0, str y0)
	{
		int x = s_to_i(x0);
		int y = s_to_i(y0);
		char op = op0[0];
		int r;
		switch (op)
		{
		case '+':
			r= x + y;
			break;
		case '-':
			r = x - y;
			break;
		case '*': r = x*y;
			break;
		case '/':r = x / y;
			break;
		}
		str st = (std::to_string(int(r)));
		std::cout <<x0<<op0<<y0<<'='<< r << std::endl;
		return st;
	}
};
int main()
{
	Expr ex("-3-2+4*3/2");
	//ex.Eval();
	ex.printBrackets();
	std::cout << ex.Eval() << std::endl;

	/*str s = "3-2+4*3/2";
	boost::smatch sm;
	boost::regex e("^([\\+\\-])?([^\\+\\-]+)(?:([\\+\\-])([^\\+\\-]+))*");
	boost::regex_match(s, sm, e, boost::match_extra);
	for(auto i:sm)
		std::cout << i << std::endl;
	boost::regex e1("([+-])?([^+-]+)");
	boost::sregex_iterator it(s.begin(),s.end(), e1);
	boost::sregex_iterator itend;
	while (it != itend)
	{
		std::cout << (*it)[1]<<'\n'<< (*it)[2] << std::endl;
		it++;
	}*/
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <filesystem>
#include <string>

using namespace std::string_literals;

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

#include "miMaS/field.h"
#include "miMaS/complex_field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/rk.h"
#include "miMaS/config.h"
#include "miMaS/signal_handler.h"
#include "miMaS/iteration.h"

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*fh.step.dx+fh.range.x_min)
#define Vk(k) (k*fh.step.dv+fh.range.v_min)

auto
maxwellian ( double rho , double u , double T ) {
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

double
error_H ( std::size_t Nx , std::size_t Nv , double dt )
{
  const double Tf = 5.;

  field<double,1> fh(boost::extents[Nv][Nx]);
  complex_field<double,1> hfh(boost::extents[Nv][Nx]);

  const double Kx = 0.5;
  fh.range.v_min = -8.; fh.range.v_max = 8.;
  fh.range.x_min =  0.; fh.range.x_max = 2./Kx*math::pi<double>();
  fh.compute_steps();

  ublas::vector<double> v(Nv,0.);
  std::generate( v.begin() , v.end() , [&,k=0]() mutable {return (k++)*fh.step.dv+fh.range.v_min;} );

  ublas::vector<double> kx(Nx); // beware, Nx need to be odd
  {
    double l = fh.range.len_x();
    for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
    for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  const double alpha = 0.2 , ui = 2.;
  auto tb_M1 = maxwellian( 0.5*alpha , ui , 1. ) , tb_M2 = maxwellian( 0.5*alpha , -ui , 1. );
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
    fft::fft(fh[k].begin(),fh[k].end(),hfh[k].begin());
  }

  std::size_t iter = 0;
  double current_time = 0.;

  ublas::vector<double> uc(Nx,0.);
  ublas::vector<double> E (Nx,0.);

  std::vector<double> H; H.reserve(int(std::ceil(Tf/dt))+1);

  const double rho_c = 1.-alpha;
  const double sqrt_rho_c = std::sqrt(rho_c);

  // init E with Poisson solver, init also ee, Emax, H and times
  {
    poisson<double> poisson_solver(Nx,fh.range.len_x());
    ublas::vector<double> rho(Nx,0.); rho = fh.density(); // compute density from init hot data
    for ( auto i=0 ; i<Nx ; ++i ) { rho[i] += (1.-alpha); } // add (1-alpha) for cold particules
    E = poisson_solver(rho);

    double total_energy = energy(fh,E);
    total_energy += 0.; // sum(rho_c*u_c*u_c) = 0 because u_c = 0 at time 0
    H.push_back( total_energy );
  }

  // initialize memory for all temporary variables
  ublas::vector<double> J(Nx,0.);
  fft::spectrum_ d(Nx);
  field<double,1> Edvf(tools::array_view<const std::size_t>(fh.shape(),2));
  ublas::vector<double> uc1(Nx) , uc2(Nx) , uc3(Nx) , uc4(Nx) , uc5(Nx) , uc6(Nx) , uc7(Nx),
                        E1 (Nx) , E2 (Nx) , E3 (Nx) , E4 (Nx) , E5 (Nx) , E6 (Nx) , E7 (Nx);
  complex_field<double,1> hfh1(boost::extents[Nv][Nx]) , hfh2(boost::extents[Nv][Nx]) ,
                          hfh3(boost::extents[Nv][Nx]) , hfh4(boost::extents[Nv][Nx]) ,
                          hfh5(boost::extents[Nv][Nx]) , hfh6(boost::extents[Nv][Nx]) ,
                          hfh7(boost::extents[Nv][Nx]) ;
  fh.write("init.dat");

  while (  current_time < Tf ) {

///////////////////////////////////////////////////////////////////////////////
// DP4(3) /////////////////////////////////////////////////////////////////////

    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c05 = std::cos(0.5*dt*sqrt_rho_c), s05 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<Nx ; ++i ) {
        uc1[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c - 0.5*dt*J[i]*s05/sqrt_rho_c;
        E1[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) - 0.5*dt*d[i]*std::exp(-0.5*I*kx[i]*v[k]*dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c05 = std::cos(0.5*dt*sqrt_rho_c), s05 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<Nx ; ++i ) {
        uc2[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c;
        E2[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<Nx ; ++i ) {
          hfh2[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) - 0.5*dt*d[i];
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c1  = std::cos(dt*sqrt_rho_c)     , s1  = std::sin(dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*dt*sqrt_rho_c) , s05 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<Nx ; ++i ) {
        uc3[i] =  uc[i]*c1 + E[i]*s1/sqrt_rho_c - dt*J[i]*s05/sqrt_rho_c;
        E3[i]  = -uc[i]*s1*sqrt_rho_c + E[i]*c1 - dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<Nx ; ++i ) {
          hfh3[k][i] = hfh[k][i]*std::exp(-I*kx[i]*v[k]*dt) - dt*d[i]*std::exp(-0.5*I*kx[i]*v[k]*dt);
        }
      }
    } // end stage 3

    // STAGE 4
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh3[k].begin(),hfh3[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E3);

      double c1  = std::cos(dt*sqrt_rho_c)     , s1  = std::sin(dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*dt*sqrt_rho_c) , s05 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<Nx ; ++i ) {
        uc4[i] = -(1./3.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./3.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./3.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/3.;
        E4[i]  = -(1./3.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./3.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./3.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/3. - (1./6.)*dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<Nx ; ++i ) {
          hfh4[k][i] = -(1./3.)*hfh[k][i]*std::exp(-I*kx[i]*v[k]*dt) + (1./3.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) + (2./3.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) + hfh3[k][i]/3. - (1./6.)*dt*d[i];
        }
      }
    } // end stage 4

    // STAGE 5
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh4[k].begin(),hfh4[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E4);

      double c1  = std::cos(dt*sqrt_rho_c)     , s1  = std::sin(dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*dt*sqrt_rho_c) , s05 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<Nx ; ++i ) {
        uc5[i] = -(1./5.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./5.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./5.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/5. + (2./5.)*uc4[i];
        E5[i]  = -(1./5.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./5.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./5.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/5. + (2./5.)*E4[i] - (1./10.)*dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<Nx ; ++i ) {
          hfh5[k][i] = -(1./5.)*hfh[k][i]*std::exp(-I*kx[i]*v[k]*dt) + (1./5.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) + (2./5.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*v[k]*dt) + hfh3[k][i]/5. + (2./5.)*hfh4[k][i] - 0.1*dt*d[i];
        }
      }
    } // end stage 5

///////////////////////////////////////////////////////////////////////////////
// MONITORING /////////////////////////////////////////////////////////////////

    // SAVE TIME STEP
    std::copy(  uc4.begin() ,  uc4.end() ,  uc.begin() );
    std::copy(   E4.begin() ,   E4.end() ,   E.begin() );
    std::copy( hfh4.begin() , hfh4.end() , hfh.begin() );

    for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
    double total_energy = energy(fh,E);
    {
      auto rhoh = fh.density();
      fft::spectrum_ hrhoh(Nx); hrhoh.fft(&rhoh[0]);
      fft::spectrum_ hE(Nx); hE.fft(&E[0]);
      fft::spectrum_ hrhoc(Nx);
      hrhoc[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
      for ( auto i=1 ; i<Nx ; ++i )
        { hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i]; }
      ublas::vector<double> rhoc (Nx,0.); hrhoc.ifft(rhoc.begin());

      for ( auto i=0 ; i<Nx ; ++i )
        { total_energy += rhoc[i]*uc[i]*uc[i]; }
    }
    H.push_back( total_energy );

    // increment time
    current_time += dt;

    ++iter;
    if ( current_time+dt > Tf ) { dt = Tf - current_time; }
  } // while (  current_time < Tf ) // end of time loop

  for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
  fh.write("vp.dat");

  double h = std::abs(*std::max_element( H.begin() , H.end() , [&](double a,double b){return ( std::abs((a-H[0])/std::abs(H[0])) < std::abs((b-H[0])/std::abs(H[0])) );} ));
  return std::abs((h-H[0])/std::abs(H[0]));
}

int
main ( int argc , char const * argv[] )
{
  const std::size_t Nx = 75 , Nv = 1024;
  const double dt_max = 3.*16./Nv;

  for ( auto i=1 ; i<6 ; ++i ) {
    double dt = dt_max/double(i);
    std::cout << dt << " " << std::flush;
    double h = error_H(Nx,Nv,dt);
    std::cout << h << std::endl;
  }

  return 0;
}


///////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::normal::normalizing_constant.hpp         //
//                                                                               //
//  (C) Copyright 2009 Erwann Rogard                                             //
//  Use, modification and distribution are subject to the                        //
//  Boost Software License, Version 1.0. (See accompanying file                  //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)             //
///////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_NORMAL_NORMALIZING_CONSTANT_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_NORMAL_NORMALIZING_CONSTANT_HPP_ER_2009
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/math/distributions/normal.hpp>

namespace boost{
namespace math{

template<typename T,typename P>
T normalizing_constant(const boost::math::normal_distribution<T,P>& d){
    static T pi = boost::math::constants::pi<T>;
    static T two = static_cast<T>(2);
    return sqrt(two * pi) * d.scale();
}

}// math
}// boost

#endif


#define __STDCPP_WANT_MATH_SPEC_FUNCS__ 1

#include <iostream>
#include <vector>
#include <map>
#include <cmath>
#include <limits>
#include <SFML/Graphics.hpp>
#include <SFML/Window.hpp>
#include <boost/math/special_functions/beta.hpp>
#include "readdata.h"
#include "classes.h"

#define STOP 1.0e-8
#define TINY 1.0e-30

//stolen incomplete beta function cuz im too lazy to implement
double incbeta(double a, double b, double x) {
    if (x < 0.0 || x > 1.0) return 1.0/0.0;

    /*The continued fraction converges nicely for x < (a+1)/(a+b+2)*/
    if (x > (a+1.0)/(a+b+2.0)) {
        return (1.0-incbeta(b,a,1.0-x)); /*Use the fact that beta is symmetrical.*/
    }

    /*Find the first part before the continued fraction.*/
    const double lbeta_ab = lgamma(a)+lgamma(b)-lgamma(a+b);
    const double front = exp(log(x)*a+log(1.0-x)*b-lbeta_ab) / a;

    /*Use Lentz's algorithm to evaluate the continued fraction.*/
    double f = 1.0, c = 1.0, d = 0.0;

    int i, m;
    for (i = 0; i <= 200; ++i) {
        m = i/2;

        double numerator;
        if (i == 0) {
            numerator = 1.0; /*First numerator is 1.0.*/
        } else if (i % 2 == 0) {
            numerator = (m*(b-m)*x)/((a+2.0*m-1.0)*(a+2.0*m)); /*Even term.*/
        } else {
            numerator = -((a+m)*(a+b+m)*x)/((a+2.0*m)*(a+2.0*m+1)); /*Odd term.*/
        }

        /*Do an iteration of Lentz's algorithm.*/
        d = 1.0 + numerator * d;
        if (fabs(d) < TINY) d = TINY;
        d = 1.0 / d;

        c = 1.0 + numerator / c;
        if (fabs(c) < TINY) c = TINY;

        const double cd = c*d;
        f *= cd;

        /*Check for stop.*/
        if (fabs(1.0-cd) < STOP) {
            return front * (f-1.0);
        }
    }

    return 1.0/0.0; /*Needed more loops, did not converge.*/
}

double commulDistribution(double v, double t)
{
	double incbetaVal = (v/(pow(t,2)+v));
	double incbetaAns = incbeta((v/2),0.5,incbetaVal);
	/*
	std::cout << "func value small thingy yeah: "  << (v/(pow(t,2)+v)) << std::endl;
	std::cout << "inc beta func: " << incbetaAns << std::endl;
	std::cout << "real beta func: "  << std::beta((v/2),0.5) << std::endl;
	double regularizedIncBeta = incbetaAns/std::beta((v/2),0.5);*/

	double commulDistr = 1-incbetaAns/2;

	return commulDistr;
}

double invCommulDistribution(double v, double t)
{
	double signVal = (t-0.5) < 0 ? -1: 1;
	double pVal = t<0.5 ? (2*t) : (2*(1-t));
	double val2Divis = boost::math::ibeta_inv((v/2),0.5,pVal);
	double val2 = v*(1/val2Divis-1);
	double val2Sqrt = sqrt(val2);
	/*std::cout << "(signVal: "  << signVal << ")" << std::endl;
	std::cout << "(pVal: "  << pVal << ")" << std::endl;
	std::cout << "(val2Divis: "  << val2Divis << ")" << std::endl;
	std::cout << "(val2: "  << val2 << ")" << std::endl;*/
	double invCommulDistr = signVal * val2Sqrt;

	return invCommulDistr;
}

void windowFunc(int width, int height, std::vector<DataValues> data)
{
	sf::RenderWindow window(sf::VideoMode(width,height),"Graph view");

	Date firstDate = data[0].date;
	Date lastDate = data[data.size()-1].date;
	int dateSpanDays = lastDate.ToDays()-firstDate.ToDays();
	float lowestVar = std::numeric_limits<float>::infinity();
	float highestVar = -std::numeric_limits<float>::infinity();
	for(DataValues datai : data)
	{
		if(lowestVar>datai.value)
		{
			lowestVar=datai.value;
		}
		if(highestVar<datai.value)
		{
			highestVar=datai.value;
		}
	}

	float highlowDiff = highestVar-lowestVar;

	float lineWidth = 2.5;
	float textScale = 0.5;
	float statsPointRadius = 4;
	int floatPrecision = 2;

	//drawing bla bla bla
	sf::Font font;
	sf::FloatRect bounds;
	#ifdef _WIN32
	font.loadFromFile("c:/windows/fonts/arial.ttf");
	#endif
	#ifdef linux
	font.loadFromFile("/usr/share/fonts/truetype/freefont/FreeSans.ttf");
	#endif
	std::vector<sf::RectangleShape> shapes;
	std::vector<sf::Text> texts;

	sf::RectangleShape lineHor(sf::Vector2f(width-(width/8), lineWidth));
	lineHor.setOrigin((width-(width/8))/2,lineWidth/2);
	lineHor.setPosition(width/1.85,height-(height/6));

	shapes.push_back(lineHor);

	sf::RectangleShape lineVer(sf::Vector2f(height-(height/5.5), lineWidth));
	lineVer.setOrigin((height-(height/5.5))/2,lineWidth/2);
	lineVer.setPosition((width/1.85)-(width/2)+(width/8)/2,(height-(height/6))-((height-(height/5.5))/2)+lineWidth/2);
	lineVer.setRotation(90);

	shapes.push_back(lineVer);

	std::ostringstream outstr;
	outstr.precision(floatPrecision);
	outstr << std::fixed << (lowestVar-highlowDiff/2);
	sf::Text lowNumText(outstr.str(),font);
	bounds = lowNumText.getLocalBounds();
	lowNumText.setOrigin(bounds.width,bounds.height);
	lowNumText.setScale(textScale, textScale);
	lowNumText.setPosition((width/1.85)-(width/2)+(width/8)/2-textScale*30,height-(height/6)-textScale*10);
	outstr.str("");

	texts.push_back(lowNumText);

	outstr << std::fixed << (highestVar+highlowDiff/2);
	sf::Text highNumText(outstr.str(),font);
	bounds = highNumText.getLocalBounds();
	highNumText.setOrigin(bounds.width,0);
	highNumText.setScale(textScale, textScale);
	highNumText.setPosition((width/1.85)-(width/2)+(width/8)/2-bounds.height/2,height-(height-(height/5.5)+(height/6))+textScale*10);

	texts.push_back(highNumText);

	std::string dateStr = std::to_string(firstDate.year)+"/"+std::to_string(firstDate.month)+"/"+std::to_string(firstDate.day);
	sf::Text lowDateText(dateStr,font);
	bounds = lowDateText.getLocalBounds();
	lowDateText.setOrigin(0,0);
	lowDateText.setScale(textScale, textScale);
	lowDateText.setPosition((width/1.85)-(width/2)+(width/8)/2,height-(height/6)+textScale*10);

	texts.push_back(lowDateText);

	dateStr = std::to_string(lastDate.year)+"/"+std::to_string(lastDate.month)+"/"+std::to_string(lastDate.day);
	sf::Text highDateText(dateStr,font);
	bounds = highDateText.getLocalBounds();
	highDateText.setOrigin(bounds.width,0);
	highDateText.setScale(textScale, textScale);
	highDateText.setPosition(width/1.85+(width-(width/8))/2,height-(height/6)+textScale*10);

	texts.push_back(highDateText);

	for(int i = 0; i < texts.size(); i++)
	{
		texts[i].setFillColor(sf::Color(100, 150, 255));
	}
	//drawing over

	//drawing the graph now

	std::vector<sf::CircleShape> circles;

	float topStart = height-(height-(height/5.5)+(height/6));
	float verticalHeight = height-(height/5.5);
	float pixelPerOne = verticalHeight/((highestVar+highlowDiff/2)-(lowestVar-highlowDiff/2));
	float leftStart = (width/1.85)-(width/2)+(width/8)/2;
	float horizontalHeight = width-(width/8);
	float pixelPerOneDate = horizontalHeight/dateSpanDays;

	float lastX = 0;
	float lastY = 0;

	for(DataValues datapoint : data)
	{
		sf::CircleShape circle;
		circle.setRadius(statsPointRadius);
		circle.setOrigin(statsPointRadius,statsPointRadius);
		float x = leftStart+(dateSpanDays-(lastDate.ToDays()-datapoint.date.ToDays()))*pixelPerOneDate;
		float y = topStart+((highestVar+highlowDiff/2)-datapoint.value)*pixelPerOne;
		circle.setPosition(x,y);
		circles.push_back(circle);
		if(lastX!=0 || lastY!=0)
		{
			float triangleWidth = x-lastX;
			float triangleHeight = y-lastY;
			float triangleBigSide = std::sqrt(std::pow(triangleWidth,2)+std::pow(triangleHeight,2));
			float angle = std::atan2(triangleWidth,-triangleHeight)-M_PI/2;

			sf::RectangleShape graphLine(sf::Vector2f(triangleBigSide, lineWidth));
			graphLine.setOrigin(lineWidth/2,lineWidth/2);
			graphLine.setPosition(lastX,lastY);
			graphLine.setRotation(angle*(180/M_PI));

			shapes.push_back(graphLine);
		}
		lastX = x;
		lastY = y;
	}

	for(int i = 0; i < circles.size(); i++)
	{
		circles[i].setFillColor(sf::Color(100, 150, 255));
	}

	for(int i = 0; i < shapes.size(); i++)
	{
		shapes[i].setFillColor(sf::Color(50, 100, 250));
	}

	//drawing the graph ends here

    while(window.isOpen())
    {
        sf::Event event;
        while(window.pollEvent(event))
        {
            if(event.type == sf::Event::Closed)
            {
                window.close();
            }
        }

        window.clear();
		for(sf::CircleShape circlei : circles)
		{
			window.draw(circlei);
		}
        for(sf::RectangleShape rectanglei : shapes)
		{
			window.draw(rectanglei);
		}
		for(sf::Text texti : texts)
		{
			window.draw(texti);
		}
        window.display();
    }
}

void calculateStats(std::string datapath, std::string searchstr)
{
	std::vector<DataValues> data;

	readfile(datapath, data);

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

	std::vector<DataValues> varVec;
	double averageVariable = 0;

	for(int i = 0; i < data.size(); i++)
	{
		std::cout << data[i].name << " (" << data[i].date.day << "): " << data[i].value << std::endl;
		if(data[i].name.find(searchstr)!=std::string::npos)
		{
			averageVariable+=data[i].value;
			varVec.push_back(data[i]);
		}
	}

	if(varVec.size()==0)
	{
		return;
	}

	averageVariable/=varVec.size();

	double sStandardDev = 0;
	for(int i = 0; i < varVec.size(); i++)
	{
		sStandardDev+=pow(averageVariable-varVec[i].value,2);
	}
	sStandardDev = sqrt(sStandardDev/(varVec.size()-1));

	double sdStandardDev = sStandardDev/(sqrt(varVec.size()));

	double tvalue = invCommulDistribution(varVec.size()-1,0.975);
	double marginOfError = tvalue*sdStandardDev;

	std::cout << std::endl;
	std::cout << "Average variable: " << averageVariable << std::endl;
	std::cout << "Sample standard deviation: " << sStandardDev << std::endl;
	std::cout << "Sampling distribution standard deviation: " << sdStandardDev << std::endl;
	std::cout << "T Value for 95%, " << varVec.size()-1 << " df: " << tvalue << std::endl;
	std::cout << "Margin of error: " << marginOfError << std::endl;

	std::cout << "\nReal value between " << averageVariable-marginOfError << " and " << averageVariable+marginOfError << std::endl;

	windowFunc(640,480,varVec);
}

int main()
{
	std::string datapath;
	std::string searchstr;

	std::cout << "path to data: " << std::endl;
	getline(std::cin, datapath);

	std::cout << "variable name: " << std::endl;
	getline(std::cin, searchstr);
	while(true)
	{
		calculateStats(datapath, searchstr);

		std::string userAction;

		std::cout << "\n\nPress enter to quit" << "\nType path to change path" << "\nType var to change the variable" << "\nType update to run with same settings\n";
		getline(std::cin, userAction);
		if(userAction=="path")
		{
			std::cout << "\nnew path to data: " << std::endl;
			getline(std::cin, datapath);
		}
		else if(userAction=="var")
		{
			std::cout << "\nnew variable name: " << std::endl;
			getline(std::cin, searchstr);
		}
		else if(userAction=="update")
		{
		} else
		{
			break;
		}
	}
	return 0;
}


#include <iostream>
#include <NTL/ZZ.h>
#include <fstream>

uint64_t msb(uint64_t n) {
    if (n == 0)
        return 0;

    uint64_t tmp;
    tmp = n >> (uint) 1;

    uint64_t mask = 1;
    while (tmp != 0) {
        tmp >>= (uint) 1;
        mask <<= (uint) 1;
    }

    return mask;
}

NTL::ZZ fibonacci(uint64_t n) {
    NTL::ZZ V_l;
    V_l = 2;
    NTL::ZZ V_h;
    V_h = 1;

    NTL::ZZ Q_l;
    Q_l = 1;
    NTL::ZZ Q_h;
    Q_h = 1;

    uint64_t mask = msb(n);
    while (mask != 0) {
        Q_l = Q_l * Q_h;
        if (n & mask) {
            Q_h = -Q_l;
            V_l = V_h * V_l - Q_l;
            V_h = V_h * V_h - 2 * Q_h;
        }
        else {
            Q_h = Q_l;
            V_h = V_h * V_l - Q_l;
            V_l = V_l * V_l - 2 * Q_h;
        }

        mask >>= (uint) 1;
    }

    return (2 * V_h - V_l) / 5;
}

int main() {
    std::ofstream outfile("case2_output.txt", std::ios::app);
    std::ifstream infile("case2_input.txt");

    long k;
    while (infile >> k) {
        std::cout << k << std::endl;

        NTL::ZZ x;
        x = NTL::power2_ZZ(k-1)-1;
        NTL::ZZ y;
        y = fibonacci(k+1);
        NTL::ZZ res;
        res = NTL::GCD(x, y);

        outfile << k << " " << res << std::endl;

        std::cout << res << std::endl;
        std::cout << std::endl;
    }

    infile.close();
    outfile.close();

    return 0;
}


// This file is part of PolyMPC, a lightweight C++ template library
// for real-time nonlinear optimization and optimal control.
//
// Copyright (C) 2020 Listov Petr <petr.listov@epfl.ch>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef BFGS_HPP
#define BFGS_HPP

#include <Eigen/Dense>
#include <limits>

/** Damped BFGS update
 * Implements "Procedure 18.2 Damped BFGS updating for SQP" form Numerical Optimization by Nocedal.
 *
 * @param[in,out]   B hessian matrix, is updated by this function
 * @param[in]       s step vector (x - x_prev)
 * @param[in]       y gradient change (grad - grad_prev)
 */
template <typename Mat, typename Vec>
void BFGS_update(Eigen::MatrixBase<Mat>& B, const Eigen::MatrixBase<Vec>& s, const Eigen::MatrixBase<Vec>& y)
{
    using Scalar = typename Mat::Scalar;
    Scalar sy, sr, sBs;
    typename Vec::PlainObject Bs, r;

    Bs.noalias() = B * s;
    sBs = s.dot(Bs);
    sy = s.dot(y);

    if (sy < 0.2 * sBs) {
        // damped update to enforce positive definite B
        Scalar theta;
        theta = 0.8 * sBs / (sBs - sy);
        r.noalias() = theta * y + (1 - theta) * Bs;
        sr = theta * sy + (1 - theta) * sBs;
    } else {
        // unmodified BFGS
        r = y;
        sr = sy;
    }

    if (sr < std::numeric_limits<Scalar>::epsilon()) {
        return;
    }

    B.noalias() += -Bs * Bs.transpose() / sBs;
    B.noalias() += r * r.transpose() / sr;
}

#endif /* BFGS_HPP */


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  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 NON INFRINGEMENT. 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.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/matrix.h>
#include <Eigen/Eigenvalues>

namespace cinolib
{

// http://www.math.harvard.edu/archive/21b_fall_04/exhibits/2dmatrices/index.html
//
CINO_INLINE
void eigen_decomposition_2x2(const double   a00,
                             const double   a01,
                             const double   a10,
                             const double   a11,
                                   vec2d  & v_min, // eigenvectors
                                   vec2d  & v_max,
                                   double & min,   // eigenvalues
                                   double & max)
{
    eigenvalues_2x2(a00,a01,a10,a11,min,max);

    if(std::fabs(a10)>1e-5)
    {
        v_max = vec2d(max-a11,a10);
        v_min = vec2d(min-a11,a10);
    }
    else if(std::fabs(a01)>1e-5)
    {
        v_max = vec2d(a01,max-a00);
        v_min = vec2d(a01,min-a00);
    }
    else
    {
        v_max = (a00>=a11) ? vec2d(1,0) : vec2d(0,1);
        v_min = (a00>=a11) ? vec2d(0,1) : vec2d(1,0);
    }

    v_max.normalize();
    v_min.normalize();
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// http://www.math.harvard.edu/archive/21b_fall_04/exhibits/2dmatrices/index.html
//
CINO_INLINE
void eigenvalues_2x2(const double   a00,
                     const double   a01,
                     const double   a10,
                     const double   a11,
                           double & min,
                           double & max)
{
    double T = a00 + a11; // trace
    double D = determinant_2x2(a00,a01,a10,a11);

    min = T/2.0 - sqrt(T*T/4.0-D);
    max = T/2.0 + sqrt(T*T/4.0-D);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
void eigenvectors_2x2(const double   a00,
                      const double   a01,
                      const double   a10,
                      const double   a11,
                            vec2d  & v_min,
                            vec2d  & v_max)
{
    double min, max;
    eigen_decomposition_2x2(a00, a01, a10, a11, v_min, v_max, min, max);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
double determinant_2x2(const double a00, const double a01, const double a10, const double a11)
{
    return ((a00*a11) - (a10*a01));
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
double determinant_2x2(const vec2d a0, const vec2d a1)
{
    return determinant_2x2(a0[0], a0[1], a1[0], a1[1]);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
void eigen_decomposition_3x3(const double   a[3][3],
                                   vec3d  & v_min, // eigenvectors
                                   vec3d  & v_mid,
                                   vec3d  & v_max,
                                   double & min,   // eigenvalues
                                   double & mid,
                                   double & max)
{
    eigen_decomposition_3x3(a[0][0], a[0][1], a[0][2],
                            a[1][0], a[1][1], a[1][2],
                            a[2][0], a[2][1], a[2][2],
                            v_min, v_mid, v_max,
                            min, mid, max);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
void eigen_decomposition_3x3(const double   a00,
                             const double   a01,
                             const double   a02,
                             const double   a10,
                             const double   a11,
                             const double   a12,
                             const double   a20,
                             const double   a21,
                             const double   a22,
                                   vec3d  & v_min, // eigenvectors
                                   vec3d  & v_mid,
                                   vec3d  & v_max,
                                   double & min,   // eigenvalues
                                   double & mid,
                                   double & max)
{
    Eigen::Matrix3d m;
    m << a00, a01, a02,
         a10, a11, a12,
         a20, a21, a22;

    bool symmetric = (a10==a01) && (a20==a02) && (a21==a12);

    if(symmetric)
    {
        // eigen decomposition for self-adjoint matrices
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(m);
        assert(eig.info() == Eigen::Success);

        v_min = vec3d(eig.eigenvectors()(0,0), eig.eigenvectors()(1,0), eig.eigenvectors()(2,0));
        v_mid = vec3d(eig.eigenvectors()(0,1), eig.eigenvectors()(1,1), eig.eigenvectors()(2,1));
        v_max = vec3d(eig.eigenvectors()(0,2), eig.eigenvectors()(1,2), eig.eigenvectors()(2,2));

        min = eig.eigenvalues()[0];
        mid = eig.eigenvalues()[1];
        max = eig.eigenvalues()[2];
    }
    else
    {
        // eigen decomposition for general matrices
        Eigen::EigenSolver<Eigen::Matrix3d> eig(m);
        assert(eig.info() == Eigen::Success);

        // WARNING: I am taking only the real part!
        v_min = vec3d(eig.eigenvectors()(0,0).real(), eig.eigenvectors()(1,0).real(), eig.eigenvectors()(2,0).real());
        v_mid = vec3d(eig.eigenvectors()(0,1).real(), eig.eigenvectors()(1,1).real(), eig.eigenvectors()(2,1).real());
        v_max = vec3d(eig.eigenvectors()(0,2).real(), eig.eigenvectors()(1,2).real(), eig.eigenvectors()(2,2).real());

        // WARNING: I am taking only the real part!
        min = eig.eigenvalues()[0].real();
        mid = eig.eigenvalues()[1].real();
        max = eig.eigenvalues()[2].real();
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
void eigenvalues_3x3(const double   a00,
                     const double   a01,
                     const double   a02,
                     const double   a10,
                     const double   a11,
                     const double   a12,
                     const double   a20,
                     const double   a21,
                     const double   a22,
                           double & min,
                           double & mid,
                           double & max)
{
    vec3d v_min, v_mid, v_max;
    eigen_decomposition_3x3(a00, a01, a02, a10, a11, a12, a20, a21, a22, v_min, v_mid, v_max, min, mid, max);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
void eigenvectors_3x3(const double   a00,
                      const double   a01,
                      const double   a02,
                      const double   a10,
                      const double   a11,
                      const double   a12,
                      const double   a20,
                      const double   a21,
                      const double   a22,
                            vec3d  & v_min,
                            vec3d  & v_mid,
                            vec3d  & v_max)
{
    double min, mid, max;
    eigen_decomposition_3x3(a00, a01, a02, a10, a11, a12, a20, a21, a22, v_min, v_mid, v_max, min, mid, max);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
double determinant_3x3(const double a00, const double a01, const double a02,
                       const double a10, const double a11, const double a12,
                       const double a20, const double a21, const double a22)
{
    return a00 * determinant_2x2(a11, a12, a21, a22) -
           a01 * determinant_2x2(a10, a12, a20, a22) +
           a02 * determinant_2x2(a10, a11, a20, a21);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
void from_std_3x3_to_Eigen_3x3(const double stdM[3][3], Eigen::Matrix3d & eigenM)
{
    eigenM.coeffRef(0,0) = stdM[0][0];  eigenM.coeffRef(0,1) = stdM[0][1];  eigenM.coeffRef(0,2) = stdM[0][2];
    eigenM.coeffRef(1,0) = stdM[1][0];  eigenM.coeffRef(1,1) = stdM[1][1];  eigenM.coeffRef(1,2) = stdM[1][2];
    eigenM.coeffRef(2,0) = stdM[2][0];  eigenM.coeffRef(2,1) = stdM[2][1];  eigenM.coeffRef(2,2) = stdM[2][2];
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
void from_eigen_3x3_to_std_3x3(const Eigen::Matrix3d & eigenM, double stdM[3][3])
{
    stdM[0][0] = eigenM.coeffRef(0,0);  stdM[0][1] = eigenM.coeffRef(0,1);  stdM[0][2] = eigenM.coeffRef(0,2);
    stdM[1][0] = eigenM.coeffRef(1,0);  stdM[1][1] = eigenM.coeffRef(1,1);  stdM[1][2] = eigenM.coeffRef(1,2);
    stdM[2][0] = eigenM.coeffRef(2,0);  stdM[2][1] = eigenM.coeffRef(2,1);  stdM[2][2] = eigenM.coeffRef(2,2);
}

}


/*!
 * @file
 * @brief
 * @copyright alphya 2019-2021
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef NYARUGA_UTIL_PARTIAL_DIFF_HPP
#define NYARUGA_UTIL_PARTIAL_DIFF_HPP

#pragma once

#include <concepts>
#include <type_traits>
#include <nyaruga_util/diff.hpp>
#include <nyaruga_util/bind_select_arg_replace.hpp>
#include <boost/hana/functional/arg.hpp>

namespace nyaruga {

namespace util {

namespace hana = boost::hana;

// Partial differentiation
template <std::size_t count, typename NumType = num_t, typename F>
constexpr auto partial_diff(F && f) noexcept
{
   return [f](auto&& ... args) noexcept -> NumType
   {
      return diff<NumType>(bind_select_arg_replace<count>(std::forward<decltype(f)>(f), args...))
         (hana::arg<count>(std::forward<decltype(args)>(args)...));
   };
}

} // namespace nyaruga::util

/* usage
#include <iostream>
#include <nyaruga_util/partial_diff.hpp>

int main()
{
   auto lambda = [](auto ... a) { return static_cast<nyaruga::util::num_t>((a * ... )); };

   std::cout << std::setprecision(18) << nyaruga::util::partial_diff<1>(lambda)(nyaruga::util::num_t(2), 5., 6.);
}
*/

#endif // #ifndef NYARUGA_UTIL_PARTIAL_DIFF_HPP


/*
 * Copyright (c) 2019 Nobuyuki Umetani
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <cmath>
#include <iostream>
#include <vector>
#include <chrono>
#include <Eigen/Core>
#if defined(_WIN32) // windows
#  define NOMINMAX   // to remove min,max macro
#  include <windows.h>  // should put before glfw3.h
#endif
#define GL_SILENCE_DEPRECATION
#include <GLFW/glfw3.h>

#include "delfem2/mshuni.h"
#include "delfem2/dtri2_v2dtri.h"
#include "delfem2/dtri.h"
#include "delfem2/eigen/ls_dense.h"
#include "delfem2/eigen/ls_sparse.h"
#include "delfem2/eigen/ls_ilu_sparse.h"
#include "delfem2/lsitrsol.h"
#include "delfem2/femsolidlinear.h"
#include "delfem2/glfw/viewer3.h"
#include "delfem2/glfw/util.h"
#include "delfem2/opengl/old/mshuni.h"

namespace dfm2 = delfem2;

void MakeMesh(
    std::vector<double>& aXY1,
    std::vector<unsigned int>& aTri1,
    std::vector<int>& aBCFlag,
    unsigned int ndim)
{
  std::vector< std::vector<double> > aaXY;
  const double len = 1.0;
  {
    aaXY.resize(1);
    aaXY[0].push_back(-len); aaXY[0].push_back(-len);
    aaXY[0].push_back(-len); aaXY[0].push_back(+len);
    aaXY[0].push_back(+len); aaXY[0].push_back(+len);
    aaXY[0].push_back(+len); aaXY[0].push_back(-len);
  }
  std::vector<delfem2::CDynPntSur> aPo2D;
  std::vector<delfem2::CDynTri> aETri;
  std::vector<delfem2::CVec2d> aVec2;
  delfem2::GenMesh(aPo2D,aETri,aVec2,
                   aaXY,0.05,0.05);
  MeshTri2D_Export(
      aXY1,aTri1,
      aVec2,aETri);
  const unsigned int np = aXY1.size()/2;
  aBCFlag.assign(np*ndim, 0);
  for(unsigned int ip=0;ip<np;++ip){
//    const double px = aXY1[ip*2+0];
    const double py = aXY1[ip*2+1];
    if( fabs(py-len) > 0.0001 ){ continue; }
    for(unsigned int idim=0;idim<ndim;++idim) {
      aBCFlag[ip * 2 + idim] = 1;
    }
  }
  std::cout<<"  ntri;"<<aTri1.size()/3<<"  nXY:"<<aXY1.size()/2<<std::endl;
}

void Solve1(
    std::vector<double>& aVal,
    const std::vector<double>& aXY1,
    const std::vector<unsigned int>& aTri1,
    const std::vector<int>& aBCFlag)
{
  const unsigned int np = aXY1.size()/2;
  const unsigned int nDoF = np*2;
  // -----------
  std::vector<unsigned int> psup_ind0, psup0;
  dfm2::JArray_PSuP_MeshElem(
      psup_ind0, psup0,
      aTri1.data(), aTri1.size()/3, 3,
      aXY1.size()/2);
  // -------------
  delfem2::CMatrixSparseBlock<Eigen::Matrix2d,Eigen::aligned_allocator<Eigen::Matrix2d>> mA;
  mA.Initialize(np);
  mA.SetPattern(psup_ind0.data(), psup_ind0.size(), psup0.data(), psup0.size());
  // ----------------------
  double myu = 10.0;
  double lambda = 10.0;
  double rho = 1.0;
  double g_x = 0.0;
  double g_y = -3.0;
  mA.setZero();
  Eigen::VectorXd vec_b(nDoF);
  vec_b.setZero();
  dfm2::MergeLinSys_SolidLinear_Static_MeshTri2D(
      mA,vec_b.data(),
      myu,lambda,rho,g_x,g_y,
      aXY1.data(), aXY1.size()/2,
      aTri1.data(), aTri1.size()/3,
      aVal.data());
  SetFixedBC_Dia(mA, aBCFlag.data(), 1.f);
  SetFixedBC_Col(mA, aBCFlag.data());
  SetFixedBC_Row(mA, aBCFlag.data());
  delfem2::setZero_Flag(vec_b, aBCFlag,0);
  // ---------------
  Eigen::VectorXd vec_x(vec_b.size());
  {
    double conv_ratio = 1.0e-6;
    int iteration = 1000;
    const std::size_t n = vec_b.size();
    Eigen::VectorXd tmp0(n), tmp1(n);
    std::vector<double> aConv = delfem2::Solve_CG(
        vec_b, vec_x, tmp0, tmp1,
        conv_ratio, iteration, mA);
    std::cout << aConv.size() << std::endl;
  }
//  SolveLinSys_PCG(mat_A,vec_b,vec_x,ilu_A, conv_ratio,iteration);
  // --------------
  {
    delfem2::CILU_SparseBlock<Eigen::Matrix2d,Eigen::aligned_allocator<Eigen::Matrix2d>> ilu;
    delfem2::ILU_SetPattern0(ilu,mA);
    delfem2::ILU_CopyValue(ilu,mA);
    delfem2::ILU_Decompose(ilu);
    Eigen::VectorXd vecX1(vec_b.size());
  }
  // --------------
  delfem2::XPlusAY(aVal,
      aBCFlag,
      1.0,vec_x);
}

int main()
{
  std::vector<unsigned int> aTri1;
  std::vector<double> aXY1;
  std::vector<int> aBCFlag; // master slave flag
  MakeMesh(
      aXY1, aTri1, aBCFlag,
      2);
  // ---
  std::vector<double> aVal;
  {
    const unsigned int np = aXY1.size()/2;
    aVal.assign(np * 2, 0.0);
    Solve1(aVal,aXY1,aTri1,aBCFlag);
  }
  // --------
  dfm2::glfw::CViewer3 viewer(1.5);
  dfm2::glfw::InitGLOld();
  viewer.OpenWindow();
  // ---------
  while(!::glfwWindowShouldClose(viewer.window)){
    viewer.DrawBegin_oldGL();
    delfem2::opengl::DrawMeshTri2D_FaceDisp2D(
        aXY1.data(), aXY1.size()/2,
        aTri1.data(), aTri1.size()/3,
        aVal.data(), 2);
    viewer.SwapBuffers();
    glfwPollEvents();
    viewer.ExitIfClosed();
  }
}


#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>

// 李群李代数 库
#include "sophus/so3.hpp"
#include "sophus/se3.hpp"

#include<stdio.h>
#include"mex.h"

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
    // nlhs represent the number of parameters of the output
    // plhs is a array of the mxarray pointers, each pointing to the output
    // nrhs represents the number of parameters of the input
    // prhs is a array of the mxarray pointers, each pointing to the input

    // prhs[0], 6x1 matrix
    // prhs[1], Mx1 cell, each cell with NX3 points
    // prhs[2], Mx1 cell, each cell with PXQ single matrix
    // prhs[3], 1x2, or 1x3, or 1x4, or 1x5 matrix
    // prhs[4], 3x3 matrix
    // prhs[5], 1x2 matrix

    if(nrhs < 1){
        mexErrMsgIdAndTxt( "euler2se3Mex:invalidNumInputs", "at least 1 input arguments required");
        return;
    }

    // get the euler transformation
    const size_t *dimArrayOfSe3 = mxGetDimensions(prhs[0]);
    size_t sizeRowsSe3 = *(dimArrayOfSe3 + 0);
    size_t sizeColsSe3 = *(dimArrayOfSe3 + 1);
    if(sizeRowsSe3 != 6 || sizeColsSe3 != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 1st param should be 6x1");
        return;
    }
    double *ptrSe3 = (double *)(mxGetPr(prhs[0]));
    Eigen::Matrix<double, 6, 1> se3;
    for(int i = 0; i < 6; i++){
        se3(i, 0) = *(ptrSe3 + i);
    }

    Sophus::SE3<double> SE3 = Sophus::SE3<double>::exp(se3);
    Eigen::Matrix<double, 4, 4> SE3Matrix = SE3.matrix();

    // cout<<"SE3 updated = "<<endl<<SE3Matrix<<endl;

    // the eulerTransform will be 4x4
    size_t dimArrayOfEulerTransform[2] = { 4, 4 };
    plhs[0] = mxCreateNumericArray(2, dimArrayOfEulerTransform, mxDOUBLE_CLASS, mxREAL);
    double *ptrEulerTransform = (double *)mxGetData(plhs[0]);
    for(int i = 0; i < 4; i++){
        for(int j = 0; j < 4; j++){
            ptrEulerTransform[i * 4 + j] = SE3Matrix(j, i);
        }
    }
}


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

#include <Eigen/Core>
#include <smooth/feedback/ocp.hpp>

template<typename T>
using X = Eigen::Vector<T, 2>;

template<typename T>
using U = Eigen::Vector<T, 1>;

template<typename T, std::size_t N>
using Vec = Eigen::Vector<T, N>;

/// @brief Objective function
struct DITheta
{
  template<typename T>
  T operator()(T, const X<T> &, const X<T> &, const Vec<T, 1> & q) const
  {
    return q.x();
  }

  Eigen::SparseMatrix<double>
  jacobian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(1, 6);
    ret.coeffRef(0, 5) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double>
  hessian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(6, 6);
    return ret;
  }
};

struct DIDyn
{
  template<typename T>
  smooth::Tangent<X<T>> operator()(T, const X<T> & x, const U<T> & u) const
  {
    return {x.y(), u.x()};
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(2, 4);
    ret.coeffRef(0, 2) = 1;
    ret.coeffRef(1, 3) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(4, 8);
    return ret;
  }
};

struct DIIntegral
{
  template<typename T>
  Vec<T, 1> operator()(T, const X<T> & x, const U<T> & u) const
  {
    return Vec<T, 1>{x.squaredNorm() + u.squaredNorm()};
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> & x, const U<double> & u) const
  {
    Eigen::SparseMatrix<double> ret(1, 4);
    ret.coeffRef(0, 1) = 2 * x.x();
    ret.coeffRef(0, 2) = 2 * x.y();
    ret.coeffRef(0, 3) = 2 * u.x();
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(4, 4);
    ret.coeffRef(1, 1) = 2;
    ret.coeffRef(2, 2) = 2;
    ret.coeffRef(3, 3) = 2;
    return ret;
  }
};

struct DICr
{
  template<typename T>
  Vec<T, 2> operator()(T, const X<T> & x, const U<T> & u) const
  {
    return Vec<T, 2>{x.y(), u.x()};
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(2, 4);
    ret.coeffRef(0, 2) = 1;
    ret.coeffRef(1, 3) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(4, 8);
    return ret;
  }
};

struct DICe
{
  template<typename T>
  Vec<T, 5> operator()(T tf, const X<T> & x0, const X<T> & xf, const Vec<T, 1> &) const
  {
    Vec<T, 5> ret;
    ret << tf, x0, xf;
    return ret;
  }

  Eigen::SparseMatrix<double>
  jacobian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(5, 6);
    ret.coeffRef(0, 0) = 1;
    ret.coeffRef(1, 1) = 1;
    ret.coeffRef(2, 2) = 1;
    ret.coeffRef(3, 3) = 1;
    ret.coeffRef(4, 4) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double>
  hessian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(6, 30);
    return ret;
  }
};

using OcpDI = smooth::feedback::OCP<X<double>, U<double>, DITheta, DIDyn, DIIntegral, DICr, DICe>;

inline const OcpDI ocp_di{
  .theta = DITheta{},
  .f     = DIDyn{},
  .g     = DIIntegral{},
  .cr    = DICr{},
  .crl   = Vec<double, 2>{{-0.5, -1}},
  .cru   = Vec<double, 2>{{1.5, 1}},
  .ce    = DICe{},
  .cel   = Vec<double, 5>{{5, 1, 1, 0.1, 0}},
  .ceu   = Vec<double, 5>{{5, 1, 1, 0.1, 0}},
};


#ifndef MATHEVAL_IMPLEMENTATION
#error "Do not include math.hpp directly!"
#endif

#pragma once
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include "matheval.hpp"
#if defined(__linux__)
#include <fenv.h>
#endif

namespace matheval {

namespace math {

/// @brief Sign function
template <typename T>
T sgn(T x) {
    return (T{0} < x) - (x < T{0});
}

/// @brief isnan function with adjusted return type
template <typename T>
T isnan(T x) {
    return std::isnan(x);
}

/// @brief isinf function with adjusted return type
template <typename T>
T isinf(T x) {
    return std::isinf(x);
}

/// @brief Convert radians to degrees
template <typename T>
T deg(T x) {
    return x * boost::math::constants::radian<T>();
}

/// @brief Convert degrees to radians
template <typename T>
T rad(T x) {
    return x * boost::math::constants::degree<T>();
}

/// @brief acosinus
template <typename T>
T acos(T x) {
  if (std::fabs(x) > 1) {
    throw matheval::acosInvalid{x};
  }
  return std::acos(x);
}

/// @brief cosinus
template <typename T>
T cos(T x) {
  if (std::isinf(x)) {
    throw matheval::cosInvalid{};
  }
  return std::cos(x);
}

/// @brief inverse hyperbolic cosine
template <typename T>
T acosh(T x) {
  if (x < 1.0) {
    throw matheval::acoshInvalid{x};
  }
  return std::acosh(x);
}

/// @brief asinus
template <typename T>
T asin(T x) {
  if (std::fabs(x) > 1) {
    throw matheval::asinInvalid{x};
  }
  return std::asin(x);
}

/// @brief inverse hyperbolic tangent
template <typename T>
T atanh(T x) {
  const T abs_x = std::fabs(x);
  if (abs_x > 1) {
    throw matheval::atanhInvalid{x};
  } else if (abs_x == 1.0) {
    throw matheval::atanhDivideByZero{};
  }
  return std::atanh(x);
}

/// @brief unary plus
template <typename T>
T plus(T x) {
    return x;
}

/// @brief natural logarithm
template <typename T>
T log(T x) {
  if (x == 0.0) {
    throw matheval::logDivideByZero{};
  } else if (x < 0.0) {
    throw matheval::logInvalid{x};
  }
  return std::log(x);
}

/// @brief log2
template <typename T>
T log2(T x) {
  if (x == 0.0) {
    throw matheval::logDivideByZero{};
  } else if (x < 0.0) {
    throw matheval::logInvalid{x};
  }
  return std::log2(x);
}

/// @brief log10
template <typename T>
T log10(T x) {
  if (x == 0.0) {
    throw matheval::logDivideByZero{};
  } else if (x < 0.0) {
    throw matheval::logInvalid{x};
  }
  return std::log10(x);
}

/// @brief sinus
template <typename T>
T sin(T x) {
  if (isinf(x)) {
    throw matheval::sinInvalid{};
  }
  return std::sin(x);
}

/// @brief square root
template <typename T>
T sqrt(T x) {
  if (x < 0.0) {
    throw matheval::sqrtInvalid{x};
  }
  return std::sqrt(x);
}

/// @brief tangens
template <typename T>
T tan(T x) {
  if (isinf(x)) {
    throw matheval::tanInvalid{};
  }
  return std::tan(x);
}

/// @brief gamma
template <typename T>
T tgamma(T x) {
  if (x == 0) {
    throw matheval::tgammaDivideByZero{};
  } else if (x == -INFINITY) {
    throw matheval::tgammaInvalid{x};
  } else if (x < 0 && x == ceil(x)) {
    throw matheval::tgammaInvalid{x};
  }
#if 0
  int psigngam;
  return lgamma_r(x, &psigngam);
#else
  return std::tgamma(x);
#endif
}

/// @brief if/else function
template <typename T>
T ifelse(T expr, T res_true, T res_false) {
  return expr ? res_true : res_false;
}

/// @brief binary plus
template <typename T>
T plus(T x, T y) {
    return x + y;
}

/// @brief unary minus
template <typename T>
T minus(T x) {
    return -x;
}

/// @brief binary minus
template <typename T>
T minus(T x, T y) {
    return x - y;
}

/// @brief multiply
template <typename T>
T multiplies(T x, T y) {
    return x * y;
}

/// @brief divide
template <typename T>
T divides(T x, T y) {
  if (y == 0) {
    throw matheval::divideByZero{};
  }
    return x / y;
}

/// @brief modulo
template <typename T>
T fmod(T x, T y) {
  if (y == 0) {
    throw matheval::moduloByZero{};
  }
  if (isinf(x)) {
    throw matheval::moduloWithInfinity{};
  }
  return std::fmod(x,y);
}

/// @brief power
template <typename T>
T pow(T x, T y) {
#if defined(__linux__)
  errno = 0;
  feclearexcept(FE_ALL_EXCEPT);
  T res = std::pow(x,y);
  if (fetestexcept(FE_INVALID)) {
    throw matheval::powInvalid{};
  } else if (fetestexcept(FE_DIVBYZERO)) {
    throw matheval::powDivideByZero{};
  } else if (fetestexcept(FE_OVERFLOW)) {
    throw matheval::powOverflow{};
  } else if (fetestexcept(FE_UNDERFLOW)) {
    throw matheval::powUnderflow{};
  }
  return res;
#elif defined(__APPLE__) && defined(__clang__)
  if (y < 0) {
    throw matheval::powDivideByZero{};
  } else if (x < 0 &&
	     isfinite(y) &&
	     y != floor(y)) {
    throw matheval::powInvalid{};
  } else if (x == 0 && y < 0) {
    throw matheval::powInvalid{};
  }
  return std::pow(x,y);
#else
#error unknown platform
#endif
}

/// @brief unary not
template <typename T>
T unary_not(T x) {
    return !x;
}

/// @brief logical and
template <typename T>
T logical_and(T x, T y) {
    return x && y;
}

/// @brief logical or
template <typename T>
T logical_or(T x, T y) {
    return x || y;
}

/// @brief less
template <typename T>
T less(T x, T y) {
    return x < y;
}

/// @brief less equals
template <typename T>
T less_equals(T x, T y) {
    return x <= y;
}

/// @brief greater
template <typename T>
T greater(T x, T y) {
    return x > y;
}

/// @brief greater equals
template <typename T>
T greater_equals(T x, T y) {
    return x >= y;
}

/// @brief equals
template <typename T>
T equals(T x, T y) {
    return x == y;
}

/// @brief not equals
template <typename T>
T not_equals(T x, T y) {
    return x != y;
}

} // namespace math

} // namespace matheval


#include "bigssMathEigen.h"

#include <iostream>
#include <algorithm>

#include <Eigen/Dense>

void BIGSS::ax_xb(const Eigen::MatrixX4d &A, const Eigen::MatrixX4d &B, Eigen::Matrix4d &X)
{
  Eigen::Matrix3d mList, xtmp, mat;
  Eigen::Vector3d rotMat;

  Eigen::MatrixXd C;
  Eigen::VectorXd d;

  size_t nX = A.rows() / 4;

  mat.setZero();
  mList.setZero();
  xtmp.setZero();

  X.setIdentity();

  C = Eigen::MatrixXd::Zero(3 * nX, 3);
  d = Eigen::VectorXd::Zero(3 * nX);

  for (size_t i = 0; i<nX; i++) {
    Eigen::Matrix3d ablk = A.block(4 * i, 0, 3, 3);
    Eigen::Matrix3d bblk = B.block(4 * i, 0, 3, 3);
    std::cout << ablk << std::endl << std::endl;
    Eigen::AngleAxisd arot, brot;
    arot.fromRotationMatrix(ablk);
    brot.fromRotationMatrix(bblk);

    Eigen::Vector3d aax = arot.axis();
    Eigen::Vector3d bax = brot.axis();

    xtmp = bax * aax.transpose();

    mList += brot.angle() * arot.angle() * xtmp;
  }

  Eigen::JacobiSVD<Eigen::Matrix3d> svd(mList, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Vector3d sv = svd.singularValues();
  Eigen::Matrix3d v = svd.matrixV();

  mat(0, 0) = 1 / sv[0];
  mat(1, 1) = 1 / sv[1];
  mat(2, 2) = 1 / sv[2];

  xtmp = v * mat * v.transpose() * mList.transpose();
  std::cout << xtmp << std::endl;

  Eigen::AngleAxisd xax;
  xax.fromRotationMatrix(xtmp);

  Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
  for (size_t i = 0; i<nX; i++) {
    Eigen::MatrixXd dref = d.segment(3 * i, 3);

    Eigen::MatrixXd ablk = A.block(4 * i, 0, 3, 3);
    C.block(3 * i, 0, 3, 3) = I - ablk;
    Eigen::VectorXd aref = A.block(4 * i, 3, 3, 1);
    Eigen::VectorXd bblk = B.block(4 * i, 3, 3, 1);
    Eigen::VectorXd cc = aref - xtmp * bblk;
    d.segment(3 * i, 3) = cc;
  }

  Eigen::MatrixXd P = C.transpose() * C;
  Eigen::Vector3d trans = P.inverse() * C.transpose() * d;

  X.block<3, 3>(0, 0) = xtmp;
  X.block<3, 1>(0, 3) = trans;
}

Eigen::MatrixXd BIGSS::princomp(const Eigen::MatrixXd &X)
{
  Eigen::MatrixXd centered = X.rowwise() - X.colwise().mean();
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(centered, Eigen::ComputeThinV);
  Eigen::MatrixXd Vt = svd.matrixV();

  return Vt;
}

void BIGSS::fit3DLine(const Eigen::MatrixXd &points, Eigen::Vector3d &point, Eigen::Vector3d &vec)
{
  Eigen::MatrixXd W = princomp(points);
  vec = W.col(0);
  point = points.colwise().mean();
}

bool BIGSS::computeTransform(const Eigen::Matrix3Xd &ptsMoving, const Eigen::Matrix3Xd &ptsFixed, Eigen::Affine3d &T)
{
  T.setIdentity();

  if (ptsMoving.cols() != ptsFixed.cols())
    return false;

  if (ptsMoving.cols() < 3)
    return false;

  Eigen::Matrix3Xd aBar = ptsMoving.colwise() - ptsMoving.rowwise().mean();
  Eigen::Matrix3Xd bBar = ptsFixed.colwise() - ptsFixed.rowwise().mean();

  Eigen::Matrix3d H = aBar * bBar.transpose();

  double traceH = H.trace();

  Eigen::Vector3d delta;
  delta(0) = H(1, 2) - H(2, 1);
  delta(1) = H(2, 0) - H(0, 2);
  delta(2) = H(0, 1) - H(1, 0);

  Eigen::Matrix4d G;
  G(0, 0) = traceH;
  G.block<1, 3>(0, 1) = delta.transpose();
  G.block<3, 1>(1, 0) = delta;
  G.block<3, 3>(1, 1) = H + H.transpose() - (traceH * Eigen::Matrix3d::Identity());

  Eigen::EigenSolver<Eigen::Matrix4d> eig(G, true);
  Eigen::Matrix4cd evecs = eig.eigenvectors();
  Eigen::Vector4cd evals = eig.eigenvalues();
  Eigen::Vector4d::Index idx;
  //evals.maxCoeff(&idx);
  evals.real().maxCoeff(&idx);
  Eigen::Vector4cd ee = evecs.col(idx);
  //Eigen::Vector4d evec = ee.cast<Eigen::Vector4d>();
  //Eigen::Vector4d evec = evecs.col(idx).cast<Eigen::Vector4d>();

  Eigen::Quaterniond quat(ee(0).real(), ee(1).real(), ee(2).real(), ee(3).real());
  T = T.rotate(quat);
  Eigen::Vector3d p = ptsFixed.rowwise().mean() - T * ptsMoving.rowwise().mean();
  T = T.pretranslate(p);

  return true;
}

bool BIGSS::computeCorrespondencelessTransform(Eigen::Matrix3Xd &ptsMoving, const Eigen::Matrix3Xd &ptsFixed, Eigen::Affine3d &T, Eigen::VectorXi &ordering)
{
  T.setIdentity();
  Eigen::Affine3d guessT;
  int nPts = ptsMoving.cols();

  if (nPts != ptsFixed.cols())
    return false;

  if (nPts > 5)
    return false;

  // compute number of permutations
  int nPerms = 1;
  for (int i = 1; i < nPts; i++)
    nPerms *= (i + 1);

  // generate the permutations
  Eigen::VectorXi indices;
  indices.setLinSpaced(nPts, 0, nPts - 1);

  Eigen::Matrix3Xd bestPts;

  double maxError = std::numeric_limits<double>::max();
  Eigen::VectorXd errors;
  errors.setZero(nPerms);
  int i = 0;
  do// (int i = 0; i < nPerms; i++)
  {
    Eigen::Matrix3Xd newPts = ptsMoving;

    // find the next permutation
    //std::next_permutation(indices.data(), indices.data() + nPts);
    for (int j = 0; j < nPts; j++)
    {
      newPts.col(indices(j)) = ptsMoving.col(j);
    }

    // compute the transform
    computeTransform(newPts, ptsFixed, guessT);

    // compute the error
    Eigen::MatrixXd dt = guessT * newPts - ptsFixed;
    Eigen::VectorXd res = dt.colwise().norm();
    errors(i) = res.sum();
    if (errors(i) < maxError)
    {
      ordering = indices;
      maxError = errors(i);
      bestPts = newPts;
      T = guessT;
    }
    i++;
  } while (std::next_permutation(indices.data(), indices.data() + nPts));

  ptsMoving = bestPts;

  return true;
}


//============================================================================
// Name        : generatetable.cpp
// Author      :
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include <iostream>
#include <fstream>
#include <stdio.h>
#include <algorithm>
#include <math.h>
//#include <boost/lambda/bind.hpp>
//#include <boost/lambda/lambda.hpp>
#include <boost/spirit/home/phoenix.hpp>
#include <boost/function.hpp>
#include <boost/foreach.hpp>
#include <eigen3/Eigen/Dense>
#include <time.h>
#include <vector>
#include <map>
#include <cstring>
#include <signal.h>
#include <limits>
#include <boost/program_options.hpp>

//using namespace boost::lambda;
using namespace boost::phoenix;
using namespace Eigen;
namespace po = boost::program_options;
using std::vector;
using std::cout;
using std::endl;
using std::cerr;
using std::ostream;
using std::ofstream;
using std::fstream;
using std::map;
using std::max;
using std::numeric_limits;
using boost::result_of;

namespace bp = boost::phoenix;

//boost::lambda::placeholder1_type X;
actor<argument<0> > X;

bool quitting = false;

const double pi = 3.141592653589793238462643383279502884197169399;
void quit(int);

int numberOfThreads=3;

class Uniform {
public:
	double width;
	Uniform(double width): width(width){}
	inline double operator()(double x) const{
		return 1/width;
	}
	double getIntBegin() const {
		return -width/2;
	}
	double getIntEnd() const {
		return width/2;
	}
};

class Gaussian {
public:
	double mean;
	double var;
	double weight;
	Gaussian(double var): mean(0), var(var), weight(1){}
	Gaussian(double mean, double var): mean(mean),var(var), weight(1){}
	Gaussian(double mean, double var, double weight): mean(mean),var(var), weight(weight){}
	inline double operator()(double x) const{
		return 1/sqrt(2*pi*var)*exp(-(x-mean)*(x-mean)/(2*var));
	}
	void setMean(double mean) {this->mean=mean;}

	inline bool operator<(const Gaussian & other) const {
		return mean<other.mean;
	}
	double getIntBegin() const {
		return mean - 4 * sqrt(var); //integralBegin
	}
	double getIntEnd() const {
		return mean + 4 * sqrt(var); //integralEnd
	}
};

typedef map<double, vector<Gaussian> >::iterator tableIterator;
typedef vector<Gaussian>::iterator vectorIterator;

inline double normal(double x,double mean, double var) {
	return 1/sqrt(2*pi*var)*exp(-(x-mean)*(x-mean)/(2*var));
}

template <class F>
inline typename boost::result_of<F(double)>::type integrate(double from, double to, double increment, F const &f, typename boost::result_of<F(double)>::type accumulator = 0) {
	int N = (to-from)/increment;
	double x;
	int i;
	typename boost::result_of<F(double)>::type perThreadAccumulator;
	#pragma omp parallel private(x,perThreadAccumulator,i) num_threads(numberOfThreads)
	{
		perThreadAccumulator = accumulator;
		#pragma omp for schedule(dynamic, N/40)
		for(i=0;i<N; i++) {
			x=from+i*increment;
			perThreadAccumulator += f(x);
		}
		#pragma omp critical
		accumulator += perThreadAccumulator;
	}
	accumulator*=increment;
	return accumulator;
}

//template <class F>
//inline double integrate_(double from, double to, double increment, F const &f) {
//	double result=0;
//	int N = (to-from)/increment;
//	double x;
//	#pragma omp parallel for private(x) reduction(+:result) schedule(dynamic, N/40) num_threads(numberOfThreads)
//	for(int i=0;i<N; i++) {
//		x=from+i*increment;
//		result += f(x);
//	}
//	result*=increment;
//	return result;
//}

template <class Distribution>
VectorXd EMintegrand (double x, const Distribution& original, const vector<Gaussian>& splitted) {
	double denominator = 0;
	for (vector<Gaussian>::const_iterator it = splitted.begin(); it!=splitted.end(); ++it) {
		denominator+=(*it)(x)*it->weight;
	}
	if(denominator==0) return VectorXd::Zero(splitted.size());
	VectorXd result(splitted.size());
	const double constTerm = original(x)/denominator;
	for(uint i=0; i<splitted.size(); i++) {
		result(i) = splitted[i].weight*splitted[i](x)*constTerm;
	}
	return result;
}

//template <class Distribution>
//double EMintegrand (double x, int index, const Distribution& original, const vector<Gaussian>& splitted) {
//	double denominator = 0;
//	for (vector<Gaussian>::const_iterator it = splitted.begin(); it!=splitted.end(); ++it) {
//		denominator+=(*it)(x)*it->weight;
//	}
//	if(denominator==0) return 0;
//	return splitted[index].weight*splitted[index](x)*original(x)/denominator;
//}

template <class Distribution>
double DistIntegrand (double x, const Distribution& original, const vector<Gaussian>& splitted) {
	double originalValue = original(x);
	double approximatingValue = 0;
	BOOST_FOREACH(const Gaussian & hyp, splitted) {
		approximatingValue += hyp(x)*hyp.weight;
	}
	return originalValue*log(originalValue/approximatingValue);
}

double calculateChange(vector<Gaussian> & oldHyps, vector<Gaussian> & newHyps) {
	double change = 0;
	for(vectorIterator oldIt = oldHyps.begin(), newIt = newHyps.begin(); oldIt!= oldHyps.end(); oldIt++, newIt++) {
		change = max(change,fabs(oldIt->mean-newIt->mean));
		change = max(change,fabs(oldIt->var-newIt->var));
		change = max(change,fabs(oldIt->weight-newIt->weight));
	}
	return change;
}

void printHypotheses(vector<Gaussian> & hypotheses) {
	BOOST_FOREACH(Gaussian & g, hypotheses) {
		cout<< g.mean << ", "<<g.var<<": "<<g.weight<< " ||\n";
	}
}

template<class Distribution>
double EM(vector<Gaussian> & splitted, const Distribution original, double maxVar = 1, bool print = true) {
	cout<<"---- Starting a new EM ---- \n";
	double Dist = numeric_limits<double>::quiet_NaN(); // Bhattacharyya coefficient
	double ib = original.getIntBegin(); //integralBegin
	double ie = original.getIntEnd(); //integralEnd
	double iinc = (ie - ib) / 2000; //integralIncrement
	vector<Gaussian> oldHypotheses = splitted;

	for(int j=0; j<100000; j++) {
		if(print) cout<<"it "<<j<< ": ";

		VectorXd newWeights_ = integrate(ib,ie,iinc,bp::bind(EMintegrand<Distribution>,X,original,splitted),VectorXd::Zero(splitted.size()));
		VectorXd newMeans_ =   integrate(ib,ie,iinc,bp::bind(EMintegrand<Distribution>,X,original,splitted)*X,VectorXd::Zero(splitted.size())).cwiseQuotient(newWeights_);
		VectorXd secondMoments = integrate(ib,ie,iinc,bp::bind(EMintegrand<Distribution>,X,original,splitted)*X*X,VectorXd::Zero(splitted.size())).cwiseQuotient(newWeights_);
		VectorXd newVars_ = secondMoments - newMeans_.cwiseProduct(newMeans_);

		for (uint i=0; i<splitted.size(); ++i) {
			splitted[i].weight = newWeights_(i);
			splitted[i].mean = newMeans_(i);
			if(newVars_(i)<maxVar) splitted[i].var = newVars_(i);
			else {splitted[i].var=maxVar;}
		}
		double newDist = integrate(ib,ie,iinc,bp::bind(DistIntegrand<Distribution>,X,original,splitted));
		cout << newDist;
		double change = calculateChange(splitted,oldHypotheses);
		//if((Dist - newDist) < 1e-6) break;
		Dist = newDist;
		if(change<1e-5) break; else oldHypotheses = splitted;
		if(print) cout << "\r";
		fflush(stdout);
		if(quitting) {
			cout<<"\ninterrupted";
			break;
		}
	}
	cout<<"\n";
	printHypotheses(splitted);
	return Dist;
}

void expandHypotheses(vector<Gaussian> &hypotheses) {
	if(hypotheses.size()%2) { //odd case
		int middleIndex = hypotheses.size()/2;
		Gaussian &g = hypotheses.at(middleIndex);
		Gaussian g1(g.mean-sqrt(g.var), g.var/sqrt(2),g.weight/2);
		Gaussian g2(g.mean+sqrt(g.var), g.var/sqrt(2),g.weight/2);
		hypotheses.erase(hypotheses.begin()+middleIndex);
		hypotheses.push_back(g1);
		hypotheses.push_back(g2);
	} else { // even case
		int rightIndex = hypotheses.size()/2;
		int leftIndex = rightIndex-1;
		Gaussian &oldLeft = hypotheses.at(leftIndex);
		Gaussian &oldRight = hypotheses.at(rightIndex);
		Gaussian gm((oldLeft.mean+oldRight.mean)/2, (oldLeft.var+oldRight.var)/2, (oldLeft.weight+oldRight.weight)/4);
		Gaussian gl((oldLeft.mean-sqrt(oldLeft.var)),oldLeft.var/sqrt(2),oldLeft.weight/2);
		Gaussian gr((oldRight.mean+sqrt(oldRight.var)),oldRight.var/sqrt(2),oldRight.weight/2);
		hypotheses.erase(hypotheses.begin()+leftIndex);
		hypotheses.erase(hypotheses.begin()+rightIndex);
		hypotheses.push_back(gl);
		hypotheses.push_back(gm);
		hypotheses.push_back(gr);
	}
	std::sort(hypotheses.begin(), hypotheses.end());
}

void stretchHypotheses(vector<Gaussian>&hypotheses, double ratio) {
	BOOST_FOREACH(Gaussian & g, hypotheses) {
		g.mean*=sqrt(ratio);
	}
}

double linearIncrement(int step, double maxvariance, int tableSize) {return 1+(maxvariance-1)*(step+1)/(tableSize);}
double geometricIncrement(int step, double maxvariance, int tableSize) {
	double logmax = log(maxvariance);
	double logstep = logmax*(step+1)/tableSize;
	return exp(logstep);
}

bool nonSaturatedCriterion(vector<Gaussian> hypotheses) {
	BOOST_FOREACH(Gaussian & g, hypotheses) {
		if(g.var<1) return true;
	}
	return false;
}

bool KLdivUpperBoundCriterion(double KLdiv, double KLdivUpperBound) {
	return KLdiv < KLdivUpperBound;
}

enum Criterion { KLDIVUPPERBOUND, SATURATION};

template <class Distribution>
void fillTable(map<double,vector<Gaussian> > &table,int tableSize,bool geometricTableSteps, double maxvariance, double maxVar, Criterion criterion, double KLdivUpperBound) {
	vector <Gaussian> splitted;
	splitted.push_back(Gaussian(0,1,1));
	double oldVariance = 1;
	for(int i=0; i<tableSize && !quitting; i++) {
		double variance;
		if(geometricTableSteps) variance = geometricIncrement(i,maxvariance,tableSize);
		else variance = linearIncrement(i,maxvariance,tableSize);
		cout<<"\n\nGenerating Table Entry #"<< i+1 << " With Variance: "<<variance<< "\n";
		Distribution original(variance);
		stretchHypotheses(splitted,variance/oldVariance);

		bool isApproximatedWell = false;
		while(!isApproximatedWell && !quitting) {
			double KLdiv = EM(splitted, original, maxVar);
			switch (criterion) {
			case SATURATION: isApproximatedWell = nonSaturatedCriterion(splitted); break;
			case KLDIVUPPERBOUND:	isApproximatedWell = KLdivUpperBoundCriterion(KLdiv,KLdivUpperBound); break;
			default: std::cerr<<"Undefined criterion!!!"; exit(1);
			}
			if(!isApproximatedWell) expandHypotheses(splitted);
		}

		table[variance] = splitted;
		oldVariance=variance;
	}
}

int main(int argc, char *argv[]) {
	signal(SIGINT,quit);
	double maxVar=1;
	//	int iterations = 1000;
	//	int hypotheses = 5;
	double maxvariance = 2;
	int tableSize = 2;
	bool geometricTableSteps = true;
	bool uniformTable = false;
	double KLdivUpperBound = 1e-3;
	Criterion criterion = KLDIVUPPERBOUND;
	std::string filename="";

	// Declare the supported options.
	po::options_description desc("Allowed options");
	desc.add_options()
	    ("help", "produce help message")
	    ("output-file,o", po::value<std::string>(&filename), "The output file name, dumps the table to the standard output if none provided")
	    ("maxvariance,m", po::value<double>(&maxvariance)->default_value(2), "The maximum source Gaussian variance to be included in the table")
	    ("tablesize,s",   po::value<int>(&tableSize)->default_value(2), "The number of entries in the table")
	    ("geometric,g", "Indicates that the variance values for consequtive entries should increase geometrically (Default)")
	    ("linear,l", "Indicates that the variance values for consequtive entries should increase linearly")
	    ("numofthreads,n", po::value<int>(&numberOfThreads)->default_value(3), "The number of threads to use for the computation of the table")
	    ("usekldiv", "Use an upper-bound for the Kulbeck-Leibler distance of the mixture to stop refining a table entry(Default)")
	    ("usesaturation", "Use a special saturation condition to stop refining a table entry, the condition is that the resulting mixture starts to contain hypoteses that are narrower than the maximum allowed")
	    ("klupperbound,k", po::value<double>(&KLdivUpperBound)->default_value(1e-3), "The upper bound for the Kulbeck-Leibler distance of the splitted mixture to the original")
	    ("uniform,u", "Indicates that the original distribution to be splitted is a uniform distribution")
	    ("width,w", po::value<double>(&maxvariance)->default_value(2), "The width of the uniform distribution (The distribution is 1/width from/to (-/+) 1/(2*width)")
	;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
	    cout << desc << "\n";
	    return 1;
	}

	if (vm.count("linear")) {
		geometricTableSteps = false;
	}

	if (vm.count("usesaturation")) {
		criterion = SATURATION;
	}

	map<double,vector<Gaussian> > table;

	if(uniformTable) {
		cout<<"creating a uniform table\n";
		fillTable<Uniform>(table,tableSize,geometricTableSteps,maxvariance,maxVar,criterion,KLdivUpperBound);
	} else	{
		fillTable<Gaussian>(table,tableSize,geometricTableSteps,maxvariance,maxVar,criterion,KLdivUpperBound);
	}

	//	vector <Gaussian> splitted;
	//	splitted.push_back(Gaussian(0,1,1));
	//	double oldVariance = 1;
	//	for(int i=0; i<tableSize && !quitting; i++) {
	//		double variance;
	//		if(geometricTableSteps) variance = geometricIncrement(i,maxvariance,tableSize);
	//		else variance = linearIncrement(i,maxvariance,tableSize);
	//		cout<<"\n\nGenerating Table Entry #"<< i+1 << " With Variance: "<<variance<< "\n";
	//		Gaussian original(0,variance,1);
	//		stretchHypotheses(splitted,variance/oldVariance);
	//
	//		bool isApproximatedWell = false;
	//		while(!isApproximatedWell) {
	//			double KLdiv = EM(splitted, original, maxVar);
	//			switch (criterion) {
	//			case SATURATION: isApproximatedWell = nonSaturatedCriterion(splitted); break;
	//			case KLDIVUPPERBOUND:	isApproximatedWell = KLdivUpperBoundCriterion(KLdiv,KLdivUpperBound); break;
	//			default: std::cerr<<"Undefined criterion!!!"; exit(1);
	//			}
	//			if(!isApproximatedWell) expandHypotheses(splitted);
	//		}
	//
	//		table[variance] = splitted;
	//		oldVariance=variance;
	//	}

	cout<< endl;
	ofstream the_file;
	ostream* hypothesisout;
	if(filename=="")	hypothesisout=&cout;
	else {
		the_file.open(filename.c_str());
		hypothesisout=&the_file;
	}

	for(tableIterator it=table.begin(); it!=table.end(); it++) {
		*hypothesisout<<"\n"<<it->first<<" "<<it->second.size()<<"\n";
		BOOST_FOREACH(Gaussian & g, it->second) {
			*hypothesisout<<"\t"<<g.mean<<"  "<<g.var<< "  "<<g.weight<<"\n";
		}
	}

	//	//*hypothesisout << original.mean<< " " << original.var << " " <<original.weight << endl;
	//	for (uint i=0; i<splitted.size(); ++i) {
	//		*hypothesisout<< splitted[i].mean<< " " << splitted[i].var << " " << splitted[i].weight << endl;
	//	}
	if(the_file.is_open()) the_file.close();

	return 0;
}

void quit(int in) {
	quitting=true;
}