#include <iostream>
#include <list>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include <kv/lp.hpp>

#ifdef TEST_DD
#include <kv/dd.hpp>
#include <kv/rdd.hpp>
typedef kv::dd fl;
#else
typedef double fl;
#endif

namespace ub = boost::numeric::ublas;

int main()
{
	fl r;
	kv::interval<fl> ri;

	ub::vector<fl> objfunc;
	std::list< ub::vector<fl> > constraints;
	ub::vector<fl> tmp;

#ifdef TEST_DD
	std::cout.precision(33);
#else
	std::cout.precision(17);
#endif

	// simple problem

	objfunc.resize(3);
	tmp.resize(3);

	objfunc(0) = 0.;
	objfunc(1) = -2.;
	objfunc(2) = -1.;

	tmp(0) = -5.;
	tmp(1) = 1.;
	tmp(2) = -1.;
	constraints.push_back(tmp);

	tmp(0) = -10.;
	tmp(1) = 1.;
	tmp(2) = 2.;
	constraints.push_back(tmp);

	r = kv::lp_minimize(objfunc, constraints);
	std::cout << r << "\n";

	ri.lower() = kv::lp_minimize_verified(objfunc, constraints, -1);
	ri.upper() = kv::lp_minimize_verified(objfunc, constraints, 1);
	std::cout << ri << "\n";

	// simple problem by akky

	constraints.clear();
	objfunc.resize(4);
	tmp.resize(4);

	objfunc(0) = 0.;
	objfunc(1) = 1.;
	objfunc(2) = 0.;
	objfunc(3) = -1.;

	tmp(0) = -2.;
	tmp(1) = 1.;
	tmp(2) = 0.;
	tmp(3) = 0.;
	constraints.push_back(tmp);

	tmp(0) = 0.;
	tmp(1) = -2.;
	tmp(2) = 1.;
	tmp(3) = 0.;
	constraints.push_back(tmp);

	tmp(0) = -4.;
	tmp(1) = 4.;
	tmp(2) = -1.;
	tmp(3) = 0.;
	constraints.push_back(tmp);

	tmp(0) = 0.;
	tmp(1) = -2.;
	tmp(2) = 0.;
	tmp(3) = 1.;
	constraints.push_back(tmp);

	tmp(0) = 0.;
	tmp(1) = -4.;
	tmp(2) = 2.;
	tmp(3) = 1.;
	constraints.push_back(tmp);

	tmp(0) = -4.;
	tmp(1) = 6.;
	tmp(2) = -2.;
	tmp(3) = -1.;
	constraints.push_back(tmp);

	r = kv::lp_minimize(objfunc, constraints);
	std::cout << r << "\n";

	ri.lower() = kv::lp_minimize_verified(objfunc, constraints, -1);
	ri.upper() = kv::lp_minimize_verified(objfunc, constraints, 1);
	std::cout << ri << "\n";
}


#include "BSpline.h"

__pragma(warning(push, 0))
#include <Eigen/Eigen>
__pragma(warning(pop))

#include <iostream>

#include <omp.h>

namespace Chaf
{
	BSpline::BSpline()
	{
		m_T = { 0, 0, 0 };
	}

	double BSpline::genBasis(const std::vector<double>& T, double t, size_t i, size_t k)
	{
		if (k == 1)
		{
			if ((t >= T[i] && t < T[i + 1]) || (t >= T[i] && t <= T[i + 1] && T[i + 1] == T.back()))
			{
				return 1.0;
			}
			else
			{
				return 0.0;
			}
		}
		double left = 0.0;
		left = T[i + k - 1] - T[i] == 0.0 ? 0.0 : (t - T[i]) / (T[i + k - 1] - T[i]) * genBasis(T, t, i, k - 1);

		double right = 0.0;
		right = T[i + k] - T[i + 1] == 0.0 ? 0.0 : (T[i + k] - t) / (T[i + k] - T[i + 1]) * genBasis(T, t, i + 1, k - 1);

		return  left + right;
	}

	void BSpline::addPoint(const ImPlotPoint& p, PointType type)
	{
		Types.push_back(type);
		Control_Points_X.push_back(p.x);
		Control_Points_Y.push_back(p.y);
	}

	void BSpline::genDeBoorPoints(std::vector<double>& x, std::vector<double>& y)
	{
		std::vector<size_t> indices = { 1 };
		std::vector<double> s;
		for (size_t i = 0; i < Types.size(); i++)
		{
			indices.push_back(Types[i] + indices.back());
			s.push_back(static_cast<double>(i));
		}

		m_T.clear();
		m_T.push_back(0);
		m_T.push_back(0);
		m_T.push_back(0);
		for (size_t i = 0; i < Types.size(); i++)
		{
			for (size_t k = 0; k < Types[i]; k++)
			{
				m_T.push_back(static_cast<double>(i));
			}
		}
		m_T.push_back(static_cast<double>(Types.size() - 1));
		m_T.push_back(static_cast<double>(Types.size() - 1));
		m_T.push_back(static_cast<double>(Types.size() - 1));

		size_t n = m_T.size() - 4;

		x.resize(n);
		y.resize(n);
		Eigen::MatrixXd A(n, n);
		Eigen::MatrixXd b(n, 2);

		A.setZero();
		b.setZero();

		// Begin
		switch (Types[0])
		{
		case PointType::C2:
			A(0, 0) = 1;

			A(1, 0) = 2;
			A(1, 1) = -3;
			A(1, 2) = 1;

			b(0, 0) = Control_Points_X[0];
			b(0, 1) = Control_Points_Y[0];
			break;
		case PointType::Line:
			A(0, 0) = 1;

			A(1, 0) = 2;
			A(1, 1) = -3;
			A(1, 2) = 1;

			A(2, 2) = 1;

			b(0, 0) = Control_Points_X[0];
			b(0, 1) = Control_Points_Y[0];

			b(2, 0) = Control_Points_X[0];
			b(2, 1) = Control_Points_Y[0];
			break;
		case PointType::Sharp:
			A(0, 0) = 1;

			A(1, 0) = 2;
			A(1, 1) = -3;
			A(1, 2) = 1;

			A(2, 2) = 1;

			A(3, 3) = 1;

			b(0, 0) = Control_Points_X[0];
			b(0, 1) = Control_Points_Y[0];

			b(2, 0) = Control_Points_X[0];
			b(2, 1) = Control_Points_Y[0];

			b(3, 0) = Control_Points_X[0];
			b(3, 1) = Control_Points_Y[0];
			break;
		default:
			break;
		}

		// Inner
#pragma omp parallel for
		for (int i = 1; i < Types.size() - 1; i++)
		{
			size_t index = indices[i];

			if (Types[i] == PointType::C2)
			{
				A(index, index - 1) = genBasis(m_T, s[i], index - 1, 4);
				A(index, index) = genBasis(m_T, s[i], index, 4);
				A(index, index + 1) = genBasis(m_T, s[i], index + 1, 4);
				b(index, 0) = Control_Points_X[i];
				b(index, 1) = Control_Points_Y[i];
			}
			else if (Types[i] == PointType::Line)
			{
				if (Types[i - 1] != PointType::Line)
				{
					A(index, index) = 2;
					A(index, index + 1) = -3;
					A(index, index + 2) = 1;

					A(index + 1, index + 1) = 1;

					b(index + 1, 0) = Control_Points_X[i];
					b(index + 1, 1) = Control_Points_Y[i];
				}
				else if (Types[i + 1] != PointType::Line)
				{
					A(index, index) = 1;

					A(index + 1, index - 1) = 1;
					A(index + 1, index) = -3;
					A(index + 1, index + 1) = 2;

					b(index, 0) = Control_Points_X[i];
					b(index, 1) = Control_Points_Y[i];
				}
				else
				{
					A(index, index) = 1;

					A(index + 1, index + 1) = 1;

					b(index, 0) = Control_Points_X[i];
					b(index, 1) = Control_Points_Y[i];

					b(index + 1, 0) = Control_Points_X[i];
					b(index + 1, 1) = Control_Points_Y[i];
				}
			}
			else if (Types[i] == PointType::Sharp)
			{
				A(index, index - 1) = 2;
				A(index, index) = -3;
				A(index, index + 1) = 1;

				A(index + 1, index + 1) = 1;

				A(index + 2, index + 1) = 1;
				A(index + 2, index + 2) = -3;
				A(index + 2, index + 3) = 2;

				b(index + 1, 0) = Control_Points_X[i];
				b(index + 1, 1) = Control_Points_Y[i];
			}
		}

		// End
		switch (Types.back())
		{
		case PointType::C2:
			A(n - 2, n - 3) = 1;
			A(n - 2, n - 2) = -3;
			A(n - 2, n - 1) = 2;

			A(n - 1, n - 1) = 1;

			b(n - 1, 0) = Control_Points_X.back();
			b(n - 1, 1) = Control_Points_Y.back();
			break;
		case PointType::Line:
			A(n - 3, n - 3) = 1;

			A(n - 2, n - 3) = 1;
			A(n - 2, n - 2) = -3;
			A(n - 2, n - 1) = 2;

			A(n - 1, n - 1) = 1;

			b(n - 3, 0) = Control_Points_X.back();
			b(n - 3, 1) = Control_Points_Y.back();

			b(n - 1, 0) = Control_Points_X.back();
			b(n - 1, 1) = Control_Points_Y.back();
			break;
		case PointType::Sharp:
			A(n - 4, n - 4) = 1;

			A(n - 3, n - 3) = 1;

			A(n - 2, n - 1) = 2;
			A(n - 2, n - 2) = -3;
			A(n - 2, n - 3) = 1;

			A(n - 1, n - 1) = 1;

			b(n - 4, 0) = Control_Points_X.back();
			b(n - 4, 1) = Control_Points_Y.back();

			b(n - 3, 0) = Control_Points_X.back();
			b(n - 3, 1) = Control_Points_Y.back();

			b(n - 1, 0) = Control_Points_X.back();
			b(n - 1, 1) = Control_Points_Y.back();
		default:
			break;
		}

		std::cout << A << std::endl
			<< "===========" << std::endl;

		Eigen::MatrixXd res = A.colPivHouseholderQr().solve(b);

		for (size_t i = 0; i < x.size(); i++)
		{
			x[i] = res(i, 0);
			y[i] = res(i, 1);
		}
	}

	void BSpline::genBSpline(const std::vector<double>& px, const std::vector<double>& py, std::vector<double>& x, std::vector<double>& y, size_t sample)
	{
		x.resize(sample+1);
		y.resize(sample+1);

#pragma omp parallel for
		for (int i = 0; i <= sample; i++)
		{
			double t = m_T.back() / static_cast<double>(sample) * static_cast<double>(i);
			auto p = genBSpline(px, py, t);
			x[i] = p.x;
			y[i] = p.y;
		}
	}

	void BSpline::clear()
	{
		m_T.clear();
		Control_Points_X.clear();
		Control_Points_Y.clear();
		Types.clear();
	}

	ImPlotPoint BSpline::genBSpline(const std::vector<double>& px, const std::vector<double>& py, double t)
	{
		ImPlotPoint result = { 0.0, 0.0 };
		for (size_t i = 0; i < px.size(); i++)
		{
			auto basis = genBasis(m_T, t, i, 4);
			result.x += basis * px[i];
			result.y += basis * py[i];
		}

		return result;
	}

}


#ifndef HYPSYS1D_POLYNOMIAL_BASIS_HPP
#define HYPSYS1D_POLYNOMIAL_BASIS_HPP

#include <Eigen/Dense>

/// Legendre polynomial basis
class PolynomialBasis {
  public:

    PolynomialBasis(int q) : p(q) {
        set_scaling_factor(1.0);
    }

    PolynomialBasis(int q, double scaling_factor_) : p(q) {
        set_scaling_factor(scaling_factor_);
    }

    void set_scaling_factor(double scaling_factor_) const {
        scaling_factor = scaling_factor_;
    }

    /// Computes the Legendre polynomial basis
    /// at a given reference point xi \in [0,1]
    Eigen::VectorXd operator() (double xi) const;

    /// Computes the derivative of Legendre polynomial basis
    /// at a given reference point xi \in [0,1]
    Eigen::VectorXd deriv (double xi) const;

    int get_degree() const {
        return p;
    }

  private:

    int p;
    mutable double scaling_factor;
};

#endif // HYPSYS1D_POLYNOMIAL_BASIS_HPP


/* KMeansRexCore.cpp
A fast, easy-to-read implementation of the K-Means clustering algorithm.
  allowing customized initialization (random samples or plus plus)
  and vectorized execution via the Eigen matrix template library.

Intended to be compiled as a shared library libkmeansrex.so
 which can then be utilized from high-level interactive environments,
  such as Matlab or Python.

Contains:
  Utility Fcns:
    discrete_rand : sampling discrete r.v.
    select_without_replacement : sample discrete w/out replacement

  Cluster Location Mu Initialization:
    sampleRowsRandom : sample at random (w/out replacement)
    sampleRowsPlusPlus : sample via K-Means++ (Arthur et al)
              see http://en.wikipedia.org/wiki/K-means%2B%2B

  K-Means Algorithm (aka Lloyd's Algorithm)
    run_lloyd : executes lloyd for spec'd number of iterations

  External "C" function interfaces (for calling from Python)
    NOTE: These take only pointers to float arrays, not Eigen array types

    RunKMeans          : compute cluster centers and assignments via lloyd
    SampleRowsPlusPlus : get just a plusplus initialization

Dependencies:
  mersenneTwister2002.c : random number generator

Author: Mike Hughes (www.michaelchughes.com)
Date:   2 April 2015
*/
/*
extern "C" {
  void RunKMeans(double *X_IN,  int N,  int D, int K, int Niter, int seed, char* initname, double *Mu_OUT, double *Z_OUT);
  void SampleRowsPlusPlus(double *X_IN,  int N,  int D, int K, int seed, double *Mu_OUT);
}
*/
#include <iostream>
#include "mersenneTwister2002.c"
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

/*  DEFINE Custom Type Names to make code more readable
    ExtMat :  2-dim matrix/array externally defined (e.g. in Matlab or Python)
*/
typedef Map<ArrayXXd> ExtMat;
typedef ArrayXXd Mat;
typedef ArrayXd Vec;

// ====================================================== Utility Functions
void set_seed( int seed ) {
  init_genrand( seed );
}

int discrete_rand( Vec &p ) {
    double total = p.sum();
    int K = (int) p.size();

    double r = total*genrand_double();
    double cursum = p(0);
    int newk = 0;
    while ( r >= cursum && newk < K-1) {
        newk++;
        cursum += p[newk];
    }
    if ( newk < 0 || newk >= K ) {
        cerr << "Badness. Chose illegal discrete value." << endl;
        return -1;
    }
    return newk;
}

void select_without_replacement( int N, int K, Vec &chosenIDs) {
    Vec p = Vec::Ones(N);
    for (int kk =0; kk<K; kk++) {
      int choice;
      int doKeep = false;
      while ( doKeep==false) {

        doKeep=true;
        choice = discrete_rand( p );

        for (int previd=0; previd<kk; previd++) {
          if (chosenIDs[previd] == choice ) {
            doKeep = false;
            break;
          }
        }
      }
      chosenIDs[kk] = choice;
    }
}

// ======================================================= Init Cluster Locs Mu

void sampleRowsRandom( ExtMat &X, ExtMat &Mu ) {
    int N = X.rows();
    int K = Mu.rows();
    Vec ChosenIDs = Vec::Zero(K);
    select_without_replacement( N, K, ChosenIDs );
		for (int kk=0; kk<K; kk++) {
		  Mu.row( kk ) = X.row( ChosenIDs[kk] );
		}
}

void sampleRowsPlusPlus( ExtMat &X, ExtMat &Mu ) {
    int N = X.rows();
    int K = Mu.rows();
    Vec ChosenIDs = Vec::Ones(K);
    int choice = discrete_rand( ChosenIDs );
    Mu.row(0) = X.row( choice );
    ChosenIDs[0] = choice;
    Vec minDist(N);
    Vec curDist(N);
    for (int kk=1; kk<K; kk++) {
      curDist = ( X.rowwise() - Mu.row(kk-1) ).square().rowwise().sum().sqrt();
      if (kk==1) {
        minDist = curDist;
      } else {
        minDist = curDist.min( minDist );
      }
      choice = discrete_rand( minDist );
      ChosenIDs[kk] = choice;
      Mu.row(kk) = X.row( choice );
    }
}

void init_Mu( ExtMat &X, ExtMat &Mu, const char* initname ) {
	  if ( string( initname ) == "random" ) {
    		sampleRowsRandom( X, Mu );
	  } else if ( string( initname ) == "plusplus" ) {
  			sampleRowsPlusPlus( X, Mu );
	  }
}

// ======================================================= Update Cluster Assignments Z
void pairwise_distance( ExtMat &X, ExtMat &Mu, Mat &Dist ) {

  int N = X.rows();
  int D = X.cols();
  int K = Mu.rows();

  // For small dims D, for loop is noticeably faster than fully vectorized.
  // Odd but true.  So we do fastest thing
  if ( D <= 16 )
  {
    for (int kk=0; kk<K; kk++) {
      Dist.col(kk) = ( X.rowwise() - Mu.row(kk) ).square().rowwise().sum();
    }
  }
  else
  {
	Dist.matrix() = -2*( X.matrix() * Mu.transpose().matrix() );
    Dist.rowwise() += Mu.square().rowwise().sum().transpose().row(0);
  }
}

double assignClosest( ExtMat &X, ExtMat &Mu, ExtMat &Z, Mat &Dist) {
  double totalDist = 0;
  int minRowID;

  pairwise_distance( X, Mu, Dist );

  for (int nn=0; nn<X.rows(); nn++) {
    totalDist += Dist.row(nn).minCoeff( &minRowID );
    Z(nn,0) = minRowID;
  }
  return totalDist;
}

// ======================================================= Update Cluster Locations Mu
void calc_Mu( ExtMat &X, ExtMat &Mu, ExtMat &Z) {
  Mu = Mat::Zero( Mu.rows(), Mu.cols() );
  Vec NperCluster = Vec::Zero( Mu.rows() );

  for (int nn=0; nn<X.rows(); nn++) {
    Mu.row( (int) Z(nn,0) ) += X.row( nn );
    NperCluster[ (int) Z(nn,0)] += 1;
  }
  Mu.colwise() /= NperCluster;
}

// ======================================================= Overall Lloyd Algorithm
void run_lloyd( ExtMat &X, ExtMat &Mu, ExtMat &Z, int Niter )  {
  double prevDist=INT_MAX,totalDist = 0;

  Mat Dist = Mat::Zero( X.rows(), Mu.rows() );

  for (int iter=0; iter<Niter; iter++) {

    totalDist = assignClosest( X, Mu, Z, Dist );
    calc_Mu( X, Mu, Z );
    if ( prevDist == totalDist ) {
      break;
    }
    prevDist = totalDist;
  }
}

// =================================================================================
// =================================================================================
// ===========================  EXTERNALLY CALLABLE FUNCTIONS ======================
// =================================================================================
// =================================================================================

void RunKMeans(double *X_IN,  int N,  int D, int K, int Niter,
               int seed, const char* initname, double *Mu_OUT, double *Z_OUT) {
  set_seed( seed );

  ExtMat X  ( X_IN, N, D);
  ExtMat Mu ( Mu_OUT, K, D);
  ExtMat Z  ( Z_OUT, N, 1);

  init_Mu( X, Mu, initname);
  run_lloyd( X, Mu, Z, Niter );
}

void SampleRowsPlusPlus(double *X_IN,  int N,  int D, int K, int seed, double *Mu_OUT) {
  set_seed( seed );

  ExtMat X  ( X_IN, N, D);
  ExtMat Mu ( Mu_OUT, K, D);

  sampleRowsPlusPlus( X, Mu);
}


// 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)

// Copyright Paul A. Bristow 2015 - 2016.
// Copyright Christopher Kormanyos 2015 - 2016.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

//! \brief Example program showing use of math constants.

// Below are snippets of code that are included into Quickbook file fixed_point.qbk.

#include <cmath>
#include <limits>
#include <iomanip>
#include <iostream>

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
// http://www.boost.org/doc/libs/release/libs/multiprecision/doc/html/boost_multiprecision/tut/floats/cpp_bin_float.html

int main()
{
//[bin_float_50_pi
  // Construct a 50 decimal digit multiprecision floating-point version of pi for reference.
  using boost::multiprecision::cpp_bin_float_50;  // 50 decimal digits precision.

  std::cout << std::setprecision(std::numeric_limits<cpp_bin_float_50>::digits10)
    << std::fixed
    << boost::math::constants::pi<cpp_bin_float_50>()
    // 3.14159265358979323846264338327950288419716939937510
    << std::endl;
//] //[/bin_float_50_pi]

  // Use a rather precice fixed_point type that uses a 64-bit integer as its underlying representation.
//[fixed_point__constant
  typedef boost::fixed_point::negatable<3, -60> fixed_point_type;

  std::cout << std::setprecision(std::numeric_limits<fixed_point_type>::digits10)
    << std::fixed
    << boost::math::constants::pi<fixed_point_type>()
    // 3.141592653589793238
    << std::endl;
//] [/fixed_point__constant]

  // Use a small (and so very imprecise) fixed_point type that will fit into a single byte.
//[fixed_point_imprecise_constant
  typedef boost::fixed_point::negatable<2, -5> tiny_fixed_point_type;

  std::cout << std::setprecision(std::numeric_limits<tiny_fixed_point_type>::digits10)
    << std::fixed
    << boost::math::constants::pi<tiny_fixed_point_type>()
    // 3.1
    << std::endl;
//] [/fixed_point_imprecise_constant]

  // Use a precise fixed_point type that will fit into a 128-bit 16 byte item.
//[fixed_point_precise_constant

  typedef boost::fixed_point::negatable<2, -125> precise_fixed_point_type;

  std::cout << std::setprecision(std::numeric_limits<precise_fixed_point_type>::digits10)
    << std::fixed
    << boost::math::constants::pi<precise_fixed_point_type>()
    // 3.1415926535897932384626433832795028842
    << std::endl;
//] [/fixed_point_precise_constant]

} // int main()


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <istream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <complex>

#include "codegen_helper.hpp"

#define PI 3.14159265

// Read a CSV file and dump it to an Eigen Matrix
Eigen::Matrix<double, Eigen::Dynamic, 1> readCSV(std::istream &input, int matSize)
{
    int a = 0;
    int b = 0;

    std::string csvLine;
    Eigen::Matrix<double, Eigen::Dynamic, 1> out;
    out.resize(matSize*matSize);
    // read every line from the stream
    while( std::getline(input, csvLine) )
    {

        std::istringstream csvStream(csvLine);
        std::vector<std::string> csvColumn;
        std::string csvElement;
        // read every element from the line that is seperated by commas
        // and put it into the vector or strings
        while( getline(csvStream, csvElement, ',') )
        {
            csvColumn.push_back(csvElement);
            out[b] = std::stod(csvElement);
            b++;
        }
        a++;
    }
    std::cout << "a : " << a << " b : " << b << std::endl;
    return out;
}

// Build a complex Eigen::Matrix representing the 2D FFT coeffs from 2 files representing the real and imaginary 2D FFT matrices
template <typename Scalar>
Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, 1> CSVtoFTcoeff(std::istream &inputReal, std::istream &inputImag, int matSize)
{
    std::complex<Scalar> consti;
    consti = std::complex<double>{0,1};

    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> outReal;
    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> outImag;
    Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, 1> out;

    outReal = readCSV(inputReal, matSize);
    outImag = readCSV(inputImag, matSize);

    out = outReal + consti*outImag;

    return out;
}

// Evaluate a 2D function from its FT coefficients
template <typename Scalar>
Scalar evaluateFromFT(Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, Eigen::Dynamic> FTcoeffs, Scalar x, Scalar y)
{
    Scalar zero;
    zero = 0;

    std::complex<Scalar> eval;
    eval = std::complex<double>{0,0};

    std::complex<Scalar> consti;
    consti = std::complex<double>{0,1};

    int m = (int)FTcoeffs.rows();
    int n = (int)FTcoeffs.cols();

    Scalar coeffThreshold;
    coeffThreshold = 1e-6;

    for(int i=0; i<n; i++)
    {
        for(int j=0; j<m; j++)
        {
            Scalar coeffReal;
            Scalar coeffImag;

            coeffReal = CppAD::CondExpLt(std::abs(FTcoeffs(i,j)), coeffThreshold, zero, FTcoeffs(i,j).real());
            coeffImag = CppAD::CondExpLt(std::abs(FTcoeffs(i,j)), coeffThreshold, zero, FTcoeffs(i,j).imag());
            std::complex<Scalar> coeff(coeffReal, coeffImag);

            eval += coeff*(cos(2*PI*(y*(i-n/2)/n + x*(j-m/2)/m)) + consti*sin(2*PI*(y*(i-n/2)/n + x*(j-m/2)/m)));
        }
    }
    std::complex<Scalar> scale;
    scale = std::complex<double>{(double)n*m,0};
    eval/=scale;
    return CppAD::sqrt(eval.real()*eval.real() - eval.imag()*eval.imag());
}

// Generates the model for the function f(q, pair) = dist. between frames of given pair
ADFun tapeADShoulderDistanceCheck(Eigen::Matrix<std::complex<ADScalar>, Eigen::Dynamic, Eigen::Dynamic> FTcoeffs)
{
    // Initnialize AD input and output
    Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ad_X;
    Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ad_Y;
    ad_X.resize(2);
    ad_Y.resize(1);
    CppAD::Independent(ad_X);
    // Initialize AD function
    ADFun ad_fun;

    // Tape the function
    ADScalar d = evaluateFromFT<ADScalar>(FTcoeffs, ad_X[0], ad_X[1]);
    ad_Y[0] = d;
    ad_fun.Dependent(ad_X, ad_Y);

    return ad_fun;
}


//
// Copyright 2019 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// 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_GIL_IMAGE_PROCESSING_NUMERIC_HPP
#define BOOST_GIL_IMAGE_PROCESSING_NUMERIC_HPP

#include <boost/gil/detail/math.hpp>
#include <cmath>
#include <boost/gil/image_view.hpp>
#include <boost/gil/typedefs.hpp>

namespace boost { namespace gil {

/// \defgroup ImageProcessingMath
/// \brief Math operations for IP algorithms
///
/// This is mostly handful of mathemtical operations that are required by other
/// image processing algorithms
///
/// \brief Normalized cardinal sine
/// \ingroup ImageProcessingMath
///
/// normalized_sinc(x) = sin(pi * x) / (pi * x)
///
inline double normalized_sinc(double x)
{
    return std::sin(x * boost::gil::pi) / (x * boost::gil::pi);
}

/// \brief Lanczos response at point x
/// \ingroup ImageProcessingMath
///
/// Lanczos response is defined as:
/// x == 0: 1
/// -a < x && x < a: 0
/// otherwise: normalized_sinc(x) / normalized_sinc(x / a)
inline double lanczos(double x, std::ptrdiff_t a)
{
    // means == but <= avoids compiler warning
    if (0 <= x && x <= 0)
        return 1;

    if (-a < x && x < a)
        return normalized_sinc(x) / normalized_sinc(x / static_cast<double>(a));

    return 0;
}

inline void compute_tensor_entries(
    boost::gil::gray16_view_t dx,
    boost::gil::gray16_view_t dy,
    boost::gil::gray32f_view_t m11,
    boost::gil::gray32f_view_t m12_21,
    boost::gil::gray32f_view_t m22)
{
    for (std::ptrdiff_t y = 0; y < dx.height(); ++y) {
        for (std::ptrdiff_t x = 0; x < dx.width(); ++x) {
            auto dx_value = dx(x, y);
            auto dy_value = dy(x, y);
            m11(x, y) = dx_value * dx_value;
            m12_21(x, y) = dx_value * dy_value;
            m22(x, y) = dy_value * dy_value;
        }
    }
}

/// \brief Compute xy gradient, and second order x and y gradients
/// \ingroup ImageProcessingMath
///
/// Hessian matrix is defined as a matrix of partial derivates
/// for 2d case, it is [[ddxx, dxdy], [dxdy, ddyy].
/// d stands for derivative, and x or y stand for direction.
/// For example, dx stands for derivative (gradient) in horizontal
/// direction, and ddxx means second order derivative in horizon direction
/// https://en.wikipedia.org/wiki/Hessian_matrix
template <typename GradientView, typename OutputView>
inline void compute_hessian_entries(
    GradientView dx,
    GradientView dy,
    OutputView ddxx,
    OutputView dxdy,
    OutputView ddyy)
{
    using x_coord_t = typename OutputView::x_coord_t;
    using y_coord_t = typename OutputView::y_coord_t;
    using pixel_t = typename std::remove_reference<decltype(ddxx(0, 0))>::type;
    using channel_t = typename std::remove_reference<
                        decltype(
                            std::declval<pixel_t>().at(
                                std::integral_constant<int, 0>{}
                            )
                        )
                       >::type;

    constexpr double x_kernel[3][3] =
    {
        {1, 0, -1},
        {2, 0, -2},
        {1, 0, -1}
    };
    constexpr double y_kernel[3][3] =
    {
        {1, 2, 1},
        {0, 0, 0},
        {-1, -2, -1}
    };
    constexpr auto chosen_channel = std::integral_constant<int, 0>{};
    for (y_coord_t y = 1; y < ddxx.height() - 1; ++y)
    {
        for (x_coord_t x = 1; x < ddxx.width() - 1; ++x)
        {
            pixel_t ddxx_i;
            static_transform(ddxx_i, ddxx_i,
                [](channel_t) { return static_cast<channel_t>(0); });
            pixel_t dxdy_i;
            static_transform(dxdy_i, dxdy_i,
                [](channel_t) { return static_cast<channel_t>(0); });
            pixel_t ddyy_i;
            static_transform(ddyy_i, ddyy_i,
                [](channel_t) { return static_cast<channel_t>(0); });
            for (y_coord_t y_filter = 0; y_filter < 2; ++y_filter)
            {
                for (x_coord_t x_filter = 0; x_filter < 2; ++x_filter)
                {
                    auto adjusted_y = y + y_filter - 1;
                    auto adjusted_x = x + x_filter - 1;
                    ddxx_i.at(std::integral_constant<int, 0>{}) +=
                        dx(adjusted_x, adjusted_y).at(chosen_channel)
                        * x_kernel[y_filter][x_filter];
                    dxdy_i.at(std::integral_constant<int, 0>{}) +=
                        dx(adjusted_x, adjusted_y).at(chosen_channel)
                        * y_kernel[y_filter][x_filter];
                    ddyy_i.at(std::integral_constant<int, 0>{}) +=
                        dy(adjusted_x, adjusted_y).at(chosen_channel)
                        * y_kernel[y_filter][x_filter];
                }
            }
            ddxx(x, y) = ddxx_i;
            dxdy(x, y) = dxdy_i;
            ddyy(x, y) = ddyy_i;
        }
    }
}

/// \brief Generate mean kernel
/// \ingroup ImageProcessingMath
///
/// Fills supplied view with normalized mean
/// in which all entries will be equal to
/// \code 1 / (dst.size()) \endcode
inline void generate_normalized_mean(boost::gil::gray32f_view_t dst)
{
    if (dst.width() != dst.height() || dst.width() % 2 != 1)
        throw std::invalid_argument("kernel dimensions should be odd and equal");
    const float entry = 1.0f / static_cast<float>(dst.size());

    for (auto& pixel: dst) {
        pixel.at(std::integral_constant<int, 0>{}) = entry;
    }
}

/// \brief Generate kernel with all 1s
/// \ingroup ImageProcessingMath
///
/// Fills supplied view with 1s (ones)
inline void generate_unnormalized_mean(boost::gil::gray32f_view_t dst)
{
    if (dst.width() != dst.height() || dst.width() % 2 != 1)
        throw std::invalid_argument("kernel dimensions should be odd and equal");

    for (auto& pixel: dst) {
        pixel.at(std::integral_constant<int, 0>{}) = 1.0f;
    }
}

/// \brief Generate Gaussian kernel
/// \ingroup ImageProcessingMath
///
/// Fills supplied view with values taken from Gaussian distribution. See
/// https://en.wikipedia.org/wiki/Gaussian_blur
inline void generate_gaussian_kernel(boost::gil::gray32f_view_t dst, double sigma)
{
    if (dst.width() != dst.height() || dst.width() % 2 != 1)
        throw std::invalid_argument("kernel dimensions should be odd and equal");

    const double denominator = 2 * boost::gil::pi * sigma * sigma;
    const auto middle = boost::gil::point_t(dst.width() / 2, dst.height() / 2);
    for (boost::gil::gray32f_view_t::coord_t y = 0; y < dst.height(); ++y)
    {
        for (boost::gil::gray32f_view_t::coord_t x = 0; x < dst.width(); ++x)
        {
            const auto delta_x = std::abs(middle.x - x);
            const auto delta_y = std::abs(middle.y - y);
            const double power = (delta_x * delta_x +  delta_y * delta_y) / (2 * sigma * sigma);
            const double nominator = std::exp(-power);
            const float value = nominator / denominator;
            dst(x, y).at(std::integral_constant<int, 0>{}) = value;
        }
    }
}

}} // namespace boost::gil

#endif


#include <iostream>
#include <string>
#include <utility>
#include <stdexcept>
#include <cmath>
#include <cstdlib>
#include <boost/program_options.hpp>
#include "logical_expr.hpp"
#include "quine_mccluskey.hpp"

using namespace std;

template<typename Property>
void print_term_expr(const logical_expr::logical_term<Property> &term,
            char first_char = 'A', char inverter = '~')
{
    for( int i = 0; i < term.size(); ++i ) {
        if( term[i] == false )  cout << inverter;
        if( term[i] != logical_expr::dont_care )
            cout << static_cast<char>(first_char + i);
    }
}

template<typename TermType>
void print_func_expr(
        const logical_expr::logical_function<TermType> &func,
        char first_char = 'A', const string &funcname = "f", char inverter = '~')
{
    cout << funcname << " = ";
    for( auto it = func.begin(); it != func.end(); ++it ) {
        print_term_expr(*it, first_char, inverter);
        if( it + 1 != func.end() )
            cout << " + ";
    }
    cout << endl;
}

template<typename TermType>
void print_truth_table(
        const logical_expr::logical_function<TermType> &f,
        char first_char = 'A', const string &funcname ="f"
    )
{
    cout << "Truth Table: ";
    print_func_expr(f, first_char, funcname);
    for( char c = first_char; c != first_char + f.term_size(); ++c )
        cout << c;
    cout << " | " << funcname << "()" << endl;
    for( int i = 0; i < f.term_size() + 6; ++i )
        cout << ((i == f.term_size() + 1) ? '|' : '-');
    cout << endl;
    logical_expr::arg_generator<> generator(0, std::pow(2, f.term_size()), f.term_size());
    for( auto arg : generator )
        cout << arg << " |  " << f(arg) << endl;
}

int main(int argc, char **argv)
{
    int exit_code = EXIT_SUCCESS;
    try {
        bool print_process = true;
        char first_char = 'A';
        constexpr char inverter = '~';

        //
        // Parse command line options
        //
        using namespace boost::program_options;
        options_description opt("Options");
        opt.add_options()
            ("quiet,q", "never print the information of the process of simplifying")
            ("first-char,c", value<char>(), "specify a character of the first variable used for input expression")
            ("help,h", "display this help and exit");
        variables_map argmap;
        store(parse_command_line(argc, argv, opt), argmap);
        notify(argmap);
        if( argmap.count("help") ) {
            std::cout << opt << endl;
            return EXIT_SUCCESS;
        }
        if( argmap.count("quiet") )
            print_process = false;
        if( argmap.count("first-char") )
            first_char = argmap["first-char"].as<char>();

        // Input a target logical function to be simplfied from stdin
        if( print_process )
            cout << "Logical Function Simplifier (Quine-McCluskey)"   << endl
                 << "[*] Enter a logical function to be simplified"   << endl
                 << "    (ex. \"f(A, B, C) = A + BC + ~A~B + ABC\" )" << endl
                 << "[*] Input: " << flush;
        string line;
        getline(cin, line);

        // Parse input logical expression and return tokenized
        logical_expr::function_parser<inverter, true> parser(line, first_char);
        auto token = parser.parse();
        // Create a logical function with logical_term<term_mark>
        typedef quine_mccluskey::simplifier::property_type PropertyType;
        typedef quine_mccluskey::simplifier::term_type TermType;
        logical_expr::logical_function<TermType> function;
        for( string term : token.second )
            function += logical_expr::parse_logical_term<PropertyType, inverter>(term, token.first.size(), first_char);

        // Create a simplifier using Quine-McCluskey algorithm
        quine_mccluskey::simplifier qm(function);
        if( print_process ) {
            cout << endl << "Sum of products form:" << endl;
            print_truth_table(qm.get_std_spf(), first_char);    // Print the function in sum of products form
            cout << endl << "Compressing ..." << endl;
            qm.compress_table(true);                            // Compress the compression table
            cout << endl << "Prime implicants: " << endl;
            for( const auto &term : qm.get_prime_implicants() ) {      // Print the prime implicants
                print_term_expr(term, first_char);
                cout << "  ";
            }
            cout << endl << endl << "Result of simplifying:" << endl;
        }
        else
            qm.compress_table(false);

        for( const auto &func : qm.simplify() )        // Simplify and print its results
            print_func_expr(func, first_char, parser.function_name() + "\'");
    }
    catch( std::exception &e ) {
        cerr << endl << "[-] Exception: " << e.what() << endl;
        exit_code = EXIT_FAILURE;
    }
    return exit_code;
}


/*
A class to represent the extension field GF(2^M)
*/

#ifndef EXTENSION_FIELD2
#define EXTENSION_FIELD2

#include <boost/operators.hpp>
#include <iostream>
#include <ostream>
#include <vector>
#include <utility>
#include "./helpers.hpp"

using namespace std;

// divide a by b; computer a/b and remainder(a/b)
// return pair with fist: quotient, second: remainder
template<class uint>
pair<uint,uint> divide(const uint a, const uint b){

	uint quot = 0; // quotient
	uint rem = a; // remainder

	if(b > a){ // then a is not divisible by b, and quot = 0
		return pair<uint,uint>(quot,rem);
	}

	while((rem >= b) && (rem != 0)){
		unsigned exp = degree(rem) - degree(b);
		quot |= (1 << exp);
		uint btmp = (b << exp);
		rem ^= btmp;
	}
	return pair<uint,uint>(quot,rem);
};

/*
GF(2^m)
Parameters are:
- m: the degree of the extension
- prim_poly: primitive polynomial defining the extension field
*/
template<class uint_, uint_ m_, uint_ prim_poly_>
class GF2M:
	boost::field_operators< GF2M<uint_,m_,prim_poly_>,
	boost::equality_comparable< GF2M<uint_,m_,prim_poly_>
	> >
{
public:
	//! an element of the extension field is a polynomial, represented by an unsigned integer
	typedef uint_ uint;
	static const unsigned m = m_;
	static const uint prim_poly = prim_poly_;
	uint el;
	bool empty;
	//! constructors
	GF2M(uint x){el = x % 2;empty = false;};
	//GF2M(unsigned x){el = x ;empty = false;};
	GF2M(uint x,unsigned v){el = x ;empty = false;}; // generates instance with el = x

	GF2M(){ el = 0; empty = true;}; // -1 marks empty

	GF2M& operator += (const GF2M& x){
		el = el ^ x.el;
		return *this;
	}

	GF2M& operator -= (const GF2M& x){
		el = el ^ x.el;
		return *this;
	}

	GF2M& operator *= (const GF2M& x){
		uint a = el;
		uint b = x.el;
		el = 0; // product of this and x
		while (a && b) {
				if (b & 1) // if b is odd, add the corresponding a to p
					el ^= a;

				if (a &  ( ((uint)1) << ( (unsigned) m - 1 )) ) // GF modulo: if a >= 2^m, it overflows when shifted left, so reduce
					a = (a << 1) ^ prim_poly; // XOR with the primitive polynomial
				else
					a <<= 1; // a*2 /* equivalent to a*2
				b >>= 1; // b / 2
		}
		return *this;
	}

	GF2M& operator /= (const GF2M& x){
		*this = (x.inverse() * (*this));
		return *this;
	}

	//! compute the multiplicative inverse via the extended Euclidean algorithm
	GF2M inverse() const {

		uint rem1 = prim_poly;
		uint rem2 = el;
		uint aux1 = 0;
		uint aux2 = 1;

		while( rem2 != 0 ){
			// res.first = quotient(rem1/rem2)
			// res.second = remainder(rem1/rem2)

			pair< uint, uint > res = divide(rem1,rem2);

			GF2M aux_new = GF2M(res.first,0)*GF2M(aux2,0) + GF2M(aux1,0);

			// prepare for the next step
			rem1 = rem2;
			rem2 = res.second;
			aux1 = aux2;
			aux2 = aux_new.el;

		}
		//aux1.multiply_monomial(pow(rem1.poly[0],-1), 0);
		return GF2M(aux1,0);
	}

	unsigned order() const {
		// multiplicative neutral element = 1
		//PFE one = PFE(1);
		GF2M one = GF2M(1);
		GF2M tmp = *this;  //EFE(el);
		unsigned ord = 1;
		while(!(tmp == one)){
			ord++;
			tmp *= *this;
		}
		return ord;
	}

	bool iszero() const {
		return (el == 0);
	}

	bool isempty() const {
		return empty;
	}

	bool operator ==(const GF2M& x) const {
		return (el == x.el);
	}

	bool operator < (const GF2M& x) const {
		return (el < x.el);
	}

};

template<class uint, uint m, uint prim_poly>
ostream &operator<<(ostream& stream, const GF2M<uint,m,prim_poly>& x)
    {
	  	stream << bitset<32>(x.el);
      	return stream; // must return stream
	};

template<class uint, uint m, uint prim_poly>
GF2M<uint,m,prim_poly> pow(const GF2M<uint,m,prim_poly>& a, int exp){

	if(a.iszero()) return a;
	if(exp == 0) {return GF2M<uint,m,prim_poly>(1);}

	GF2M<uint,m,prim_poly> tmp;

	if(exp < 0){
		GF2M<uint,m,prim_poly> am = a.inverse();
		tmp = am;
		for(unsigned i =1; i < abs(exp); ++i) tmp *= am;
	} else { // exp > 0
		tmp = a;
		for(unsigned i =1; i < abs(exp); ++i) tmp *= a;
	}

	return tmp;
};

#endif


// Boost.Geometry

// Copyright (c) 2016-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP
#define BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/formulas/differential_quantities.hpp>
#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/result_direct.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The solution of the direct problem of geodesics on latlong coordinates,
       Forsyth-Andoyer-Lambert type approximation with second order terms.
\author See
    - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
      http://www.dtic.mil/docs/citations/AD0627893
    - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
      http://www.dtic.mil/docs/citations/AD0703541

*/
template <
    typename CT,
    bool EnableCoordinates = true,
    bool EnableReverseAzimuth = false,
    bool EnableReducedLength = false,
    bool EnableGeodesicScale = false
>
class thomas_direct
{
    static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
    static const bool CalcCoordinates = EnableCoordinates || CalcQuantities;
    static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcCoordinates || CalcQuantities;

public:
    typedef result_direct<CT> result_type;

    template <typename T, typename Dist, typename Azi, typename Spheroid>
    static inline result_type apply(T const& lo1,
                                    T const& la1,
                                    Dist const& distance,
                                    Azi const& azimuth12,
                                    Spheroid const& spheroid)
    {
        result_type result;

        CT const lon1 = lo1;
        CT const lat1 = la1;

        if ( math::equals(distance, Dist(0)) || distance < Dist(0) )
        {
            result.lon2 = lon1;
            result.lat2 = lat1;
            return result;
        }

        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;
        CT const c4 = 4;

        CT const a = CT(get_radius<0>(spheroid));
        CT const b = CT(get_radius<2>(spheroid));
        CT const f = formula::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;

        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;

        // keep azimuth small - experiments show low accuracy
        // if the azimuth is closer to (+-)180 deg.
        CT azi12_alt = azimuth12;
        CT lat1_alt = lat1;
        bool alter_result = vflip_if_south(lat1, azimuth12, lat1_alt, azi12_alt);

        CT const theta1 = math::equals(lat1_alt, pi_half) ? lat1_alt :
                          math::equals(lat1_alt, -pi_half) ? lat1_alt :
                          atan(one_minus_f * tan(lat1_alt));
        CT const sin_theta1 = sin(theta1);
        CT const cos_theta1 = cos(theta1);

        CT const sin_a12 = sin(azi12_alt);
        CT const cos_a12 = cos(azi12_alt);

        CT const M = cos_theta1 * sin_a12; // cos_theta0
        CT const theta0 = acos(M);
        CT const sin_theta0 = sin(theta0);

        CT const N = cos_theta1 * cos_a12;
        CT const C1 = f * M; // lower-case c1 in the technical report
        CT const C2 = f * (c1 - math::sqr(M)) / c4; // lower-case c2 in the technical report
        CT const D = (c1 - C2) * (c1 - C2 - C1 * M);
        CT const P = C2 * (c1 + C1 * M / c2) / D;

        // special case for equator:
        // sin_theta0 = 0 <=> lat1 = 0 ^ |azimuth12| = pi/2
        // NOTE: in this case it doesn't matter what's the value of cos_sigma1 because
        //       theta1=0, theta0=0, M=1|-1, C2=0 so X=0 and Y=0 so d_sigma=d
        //       cos_a12=0 so N=0, therefore
        //       lat2=0, azi21=pi/2|-pi/2
        //       d_eta = atan2(sin_d_sigma, cos_d_sigma)
        //       H = C1 * d_sigma
        CT const cos_sigma1 = math::equals(sin_theta0, c0)
                                ? c1
                                : normalized1_1(sin_theta1 / sin_theta0);
        CT const sigma1 = acos(cos_sigma1);
        CT const d = distance / (a * D);
        CT const u = 2 * (sigma1 - d);
        CT const cos_d = cos(d);
        CT const sin_d = sin(d);
        CT const cos_u = cos(u);
        CT const sin_u = sin(u);

        CT const W = c1 - c2 * P * cos_u;
        CT const V = cos_u * cos_d - sin_u * sin_d;
        CT const X = math::sqr(C2) * sin_d * cos_d * (2 * math::sqr(V) - c1);
        CT const Y = c2 * P * V * W * sin_d;
        CT const d_sigma = d + X - Y;
        CT const sin_d_sigma = sin(d_sigma);
        CT const cos_d_sigma = cos(d_sigma);

        if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
        {
            result.reverse_azimuth = atan2(M, N * cos_d_sigma - sin_theta1 * sin_d_sigma);

            if (alter_result)
            {
                vflip_rev_azi(result.reverse_azimuth, azimuth12);
            }
        }

        if (BOOST_GEOMETRY_CONDITION(CalcCoordinates))
        {
            CT const S_sigma = c2 * sigma1 - d_sigma;
            CT const cos_S_sigma = cos(S_sigma);
            CT const d_eta = atan2(sin_d_sigma * sin_a12, cos_theta1 * cos_d_sigma - sin_theta1 * sin_d_sigma * cos_a12);
            CT const H = C1 * (c1 - C2) * d_sigma - C1 * C2 * sin_d_sigma * cos_S_sigma;
            CT const d_lambda = d_eta - H;

            result.lon2 = lon1 + d_lambda;

            if (! math::equals(M, c0))
            {
                CT const sin_a21 = sin(result.reverse_azimuth);
                CT const tan_theta2 = (sin_theta1 * cos_d_sigma + N * sin_d_sigma) * sin_a21 / M;
                result.lat2 = atan(tan_theta2 / one_minus_f);
            }
            else
            {
                CT const sigma2 = S_sigma - sigma1;
                //theta2 = asin(cos(sigma2)) <=> sin_theta0 = 1
                // NOTE: cos(sigma2) defines the sign of tan_theta2
                CT const tan_theta2 = cos(sigma2) / math::abs(sin(sigma2));
                result.lat2 = atan(tan_theta2 / one_minus_f);
            }

            if (alter_result)
            {
                result.lat2 = -result.lat2;
            }
        }

        if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
        {
            typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
            quantities::apply(lon1, lat1, result.lon2, result.lat2,
                              azimuth12, result.reverse_azimuth,
                              b, f,
                              result.reduced_length, result.geodesic_scale);
        }

        return result;
    }

private:
    static inline bool vflip_if_south(CT const& lat1, CT const& azi12, CT & lat1_alt, CT & azi12_alt)
    {
        CT const c2 = 2;
        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;

        if (azi12 > pi_half)
        {
            azi12_alt = pi - azi12;
            lat1_alt = -lat1;
            return true;
        }
        else if (azi12 < -pi_half)
        {
            azi12_alt = -pi - azi12;
            lat1_alt = -lat1;
            return true;
        }

        return false;
    }

    static inline void vflip_rev_azi(CT & rev_azi, CT const& azimuth12)
    {
        CT const c0 = 0;
        CT const pi = math::pi<CT>();

        if (rev_azi == c0)
        {
            rev_azi = azimuth12 >= 0 ? pi : -pi;
        }
        else if (rev_azi > c0)
        {
            rev_azi = pi - rev_azi;
        }
        else
        {
            rev_azi = -pi - rev_azi;
        }
    }

    static inline CT normalized1_1(CT const& value)
    {
        CT const c1 = 1;
        return value > c1 ? c1 :
               value < -c1 ? -c1 :
               value;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP


#ifndef _FM_HPP
#define _FM_HPP
#include <map>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>

using namespace std;
using namespace boost::math;

static const int SAMPLE_RATE {44100};

class Param
{
private:
    const float ratio {0.99};
    float target_value;
    float intermediate_value;
public:
    Param(float v) : target_value{v}, intermediate_value{v}
    {
    }

    void set(float v)
    {
        target_value = v;
    }

    float get()
    {
        if ( intermediate_value != target_value )
            intermediate_value = ratio*intermediate_value + (1.0-ratio)*target_value;
        return intermediate_value;
    }

    float abs()
    {
        return target_value;
    }
};

class FMSynth
{
private:
  float carrier_phase {0.0};
  float modulating_phase {0.0};
public:
  float carrier_frequency {200.0};
  float modulating_frequency {100.0};
  Param modulation_index {0.0};

  struct Band
  {
    float amplitude {0.0};
    enum {NEGATIVE, CENTER, POSITIVE, MIXED} type;
  };

  float process()
  {
    const float value {sin(carrier_phase + modulation_index.get()*sin(modulating_phase))};

    carrier_phase += carrier_frequency * 2.0 * M_PI / SAMPLE_RATE;
    if ( carrier_phase > 2*M_PI )
        carrier_phase -= 2*M_PI;
    modulating_phase += modulating_frequency * 2.0 * M_PI / SAMPLE_RATE;
    if ( modulating_phase > 2*M_PI )
        modulating_phase -= 2*M_PI;

    return value;
  }

  float peak_deviation()
  {
      return modulation_index.abs() * modulating_frequency;
  }

  map<float, Band> spectra()
  {
    const float bandwidth {2.0f * (peak_deviation() + modulating_frequency)};
    const int n {static_cast<int>(ceil(bandwidth / modulating_frequency / 2.0)) + 1};
    map<float, Band> bands;

    // initial band
    bands[carrier_frequency].amplitude += cyl_bessel_j(0, modulation_index.abs());
    bands[carrier_frequency].type = Band::CENTER;

    // side bands
    for (int i = 1; i <= n; i ++)
    {
      const double amplitude_pos {cyl_bessel_j(i, modulation_index.abs())};
      double amplitude_neg {amplitude_pos * pow(-1.0, i)};
      const float frequency_pos {carrier_frequency + modulating_frequency*i};
      float frequency_neg {carrier_frequency - modulating_frequency*i};

      // reflect
      if (frequency_neg < 0)
      {
        frequency_neg *= -1;
        amplitude_neg *= -1;
      }

      if ( bands.count(frequency_pos) )
          bands[frequency_pos].type = Band::MIXED;
      else
          bands[frequency_pos].type = Band::POSITIVE;
      bands[frequency_pos].amplitude += amplitude_pos;

      if ( bands.count(frequency_neg) )
          bands[frequency_neg].type = Band::MIXED;
      else
          bands[frequency_neg].type = Band::NEGATIVE;
      bands[frequency_neg].amplitude += amplitude_neg;
    }

    return bands;
  }
};

#endif


#include <iostream>
#include <sstream>
#include <fstream>
#include <unordered_map>
#include <Eigen/Core>
#include <Eigen/Geometry>

const float fx = 2892.843;
const float fy = 2882.249;
const int w = 1600;
const int h = 1200;
const float cx = 824.4251;
const float cy = 605.18715;

void dtu_to_cameras_txt(const std:: string & out_folder) {

  std::cout<<"Creating DTU intrinsics file: "<<out_folder<<"/cameras.txt "<<std::flush;
  std::ofstream cameras_txt (out_folder+"/cameras.txt");
  //intrinsics calibration fixed

  cameras_txt << "0 PINHOLE " << w << " " << h << " " << fx << " " << fy << " " << cx << " " << cy<<std::endl;
  cameras_txt.close();
  std::cout<<"   --->DONE"<<std::endl;
}

void dtu_to_images_txt(const std::string & pathImages,const std:: string & pathCalib, const std:: string & out_folder) {

  std::cout<<"Creating DTU images file: "<<out_folder<<"/images.txt "<<std::endl;
  std::cout<<"pathImages : "<<pathImages<<" "<<std::endl;
  std::cout<<"pathCalib : "<<pathCalib<<" "<<std::endl;

  std::ofstream images_txt (out_folder+"/images.txt");

  //Path Prefixes/postfixes
  const std::string image_prefix = "rect_";
  const std::string image_postfix = "_3_r5000.png";
  const std::string pose_prefix = "pos_";
  const std::string pose_postfix = ".txt";
  const int n_zeros = 3;

  Eigen::Matrix3f K = Eigen::Matrix3f::Zero();
  K(0,0) = fx;
  K(1,1) = fy;
  K(0,2) = cx;
  K(1,2) = cy;
  K(2,2) = 1.0;

  Eigen::Matrix3f K_inv = K.inverse(); //TODO check

  for (int i = 1; i <= 64; ++i){
    std::string number = std::string(n_zeros - std::to_string(i).length(), '0') + std::to_string(i);

    std::ifstream camFile(pathCalib  + "/" + pose_prefix  + number + pose_postfix);
    std::ifstream imgFile(pathImages + "/" + image_prefix + number + image_postfix);

    Eigen::Matrix<float,3,4> cameraMatrix;
    if(camFile.is_open() && imgFile.is_open()){
    //Reading the transpose rotation  since the file stores the inverse of the Hartley Zisserman convention
      camFile >> cameraMatrix(0,0) >> cameraMatrix(0,1) >> cameraMatrix(0,2) >> cameraMatrix(0,3);
      camFile >> cameraMatrix(1,0) >> cameraMatrix(1,1) >> cameraMatrix(1,2) >> cameraMatrix(1,3);
      camFile >> cameraMatrix(2,0) >> cameraMatrix(2,1) >> cameraMatrix(2,2) >> cameraMatrix(2,3);

      Eigen::Matrix<float,3,4> extrinsics;
      extrinsics = K_inv * cameraMatrix;

      Eigen::Matrix3f rotation;
      Eigen::Vector3f traslation;
      for (int curR = 0; curR < 3; ++curR) {
        for (int curC = 0; curC < 3; ++curC) {
          rotation(curR,curC) = extrinsics(curR,curC);
        }
      }
      for (int curR = 0; curR < 3; ++curR) {
        traslation(curR) = extrinsics(curR,3);
      }

      Eigen::Quaternionf q(rotation);

      images_txt << i << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z ()
          << " " << traslation(0) << " " << traslation(1) << " " << traslation(2)
          << " 0 " << image_prefix + number + image_postfix << std::endl;
      images_txt << std::endl;
    }
  }

  images_txt.close();
  std::cout<<"   --->DONE"<<std::endl;
}

void dtu_to_points_txt(const std:: string & out_folder){

  std::cout<<"Creating DTU 3D Points file: "<<out_folder<<"/points3D.txt "<<std::flush;

  std::ofstream points_txt (out_folder+"/points3D.txt");
  points_txt.close();
  std::cout<<"   --->DONE"<<std::endl;
}

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

  if(argc==4){
    dtu_to_cameras_txt(std::string(argv[3]));
    dtu_to_images_txt(std::string(argv[1]), std::string(argv[2]), std::string(argv[3]));
    dtu_to_points_txt(std::string(argv[3]));

  }else{
    std::cout<< "Wrong Arguments" << std::endl;
    std::cout<< "Usage:" << std::endl;
    std::cout<< "dtu_to_colmap pathImages pathcalib outputfolder" << std::endl;
  }

  return 0;
}


/*
Simulate outbreak of infectious disease.

Infected individuals infect others from an infinite pool. The model keeps track of
who infected whom and when. Infected individuals are initially in a latent phase i.e.
they show no symptoms nor can infect others. The illness then progresses according to
stochastic processes.

Follows the model description in:

Tom Britton and Gianpaolo Scalia Tomba (2018)
Estimation in emerging epidemics: biases and remedies, arXiv:1803.01688v1.
*/

#include <iostream>
#include <iomanip>
#include <math.h>
#include <random>
#include <vector>
#include <cstdlib>
#include <cmath>
#include <chrono>
#include <Eigen/Core>

#include "infectee.hpp"

class Outbreak
{
  public:
    std::vector<Infectee *> infected; // infected individuals (present and past)
    Eigen::MatrixXi counters;         // counts of each infection state per output interval
    std::mt19937_64 prng;             // pseudo random-number generator
    params_struct params;             // user-given parameters (defaults in infectee.hpp)

    Outbreak(std::mt19937_64 &prng, const params_struct &params = params_struct()) : prng(prng), params(params)

    {
        uint n_output = lrint(1. * params.max_time / params.output_interval);
        this->counters = Eigen::MatrixXi::Zero(n_output, N_STATES);

        std::vector<Infectee *> new_infected, new_infected1;
        this->infected.push_back(new Infectee(NULL, 0, prng, params));
        uint output_counter = 0;

        double time = params.timestep;
        while (time <= params.max_time)
        {
            bool is_output_step = std::fmod(time + 1e-9, params.output_interval) < params.timestep;

            // iterate over all infected individuals
            for (std::vector<Infectee *>::iterator it = this->infected.begin(); it != this->infected.end(); ++it)
            {
                new_infected1 = (*it)->update(time, prng, params);

                if (!new_infected1.empty()) // append new infectees by single infector
                {
                    new_infected.reserve(new_infected.size() + new_infected1.size());
                    new_infected.insert(new_infected.end(), new_infected1.begin(), new_infected1.end());
                }

                if (is_output_step)
                    this->counters(output_counter, (*it)->istatus())++;
            }

            if (!new_infected.empty()) // append all new infectees from time step
            {
                this->infected.reserve(this->infected.size() + new_infected.size());
                this->infected.insert(this->infected.end(), new_infected.begin(), new_infected.end());
                // std::cout << "t=" << time << ": New infected " << new_infected.size() << ", total " << infected.size() << std::endl;
                new_infected.clear();
            }

            if (is_output_step)
            {
                if (params.verbose)
                    std::cout << "t=" << time << ": " << this->counters.row(output_counter) << std::endl;
                output_counter++;
            }

            if (this->infected.size() > params.max_infected)
            {
                if (params.verbose)
                    std::cout << "Max number of infected individuals reached. Stopping." << std::endl;
                break;
            }
            time += params.timestep;
        }
    }

    ~Outbreak()
    {
        for (std::vector<Infectee *>::iterator it = this->infected.begin(); it != this->infected.end(); ++it)
            delete *it;  // need to release these manually as allocated dynamically
    }

    Eigen::MatrixXi getCounters()
    {
        return this->counters;
    }

    std::vector<Infectee*> getInfected()
    {
        return this->infected;
    }

    float getR0()
    {
        // Estimate the basic reproduction number (R0) by considering
        // reported cases due to infectors now past the infectious period.
        int n_infected = 0;
        int n_infectors = 0;

        for (std::vector<Infectee *>::iterator it = this->infected.begin(); it != this->infected.end(); ++it)
        {
            if ((*it)->istatus() > 3)
            {
                n_infectors++;
                for (std::vector<Infectee *>::iterator it2 = (*it)->infected.begin(); it2 != (*it)->infected.end(); ++it2)
                {
                    if ((*it2)->is_reported())
                        n_infected++;
                }
            }
        }
        // std::cout << "N_infected: " << n_infected << " n_infectors: " << n_infectors << std::endl;

        return (float) n_infected / n_infectors;
    }

    // Print various statistics for debugging.
    void printStats()
    {
        const uint N_GROUPS = 4;
        Eigen::ArrayXd end_time_sums = Eigen::ArrayXd::Zero(N_GROUPS);
        Eigen::ArrayXi status_sums = Eigen::ArrayXi::Zero(N_GROUPS);
        double offset;

        for (std::vector<Infectee *>::iterator it = this->infected.begin(); it != this->infected.end(); ++it)
        {
            // handle latent period
            if ((*it)->status_trajectory[1] == 1)
                offset = (*it)->end_times[0];
            else
                offset = (*it)->end_times[2];
            end_time_sums[0] += offset - (*it)->infection_time;
            status_sums[0]++;

            // infectious period
            end_time_sums[1] += (*it)->end_times[3] - offset;
            offset = (*it)->end_times[3];
            status_sums[1]++;

            // recovering period
            if ((*it)->status_trajectory[3] == 4)
            {
                end_time_sums[2] += (*it)->end_times[4] - offset;
                status_sums[2]++;
            }
            else // dying period
            {
                end_time_sums[3] += (*it)->end_times[5] - offset;
                status_sums[3]++;
            }
        }

        std::cout.precision(5);
        std::cout << std::setw(20) << "Means:" << std::setw(20) << "Latent period"
                  << std::setw(20) << "Infectious period" << std::setw(20) << "Recovering period"
                  << std::setw(20) << "Dying period" << std::endl;
        std::cout << std::setw(20) << (end_time_sums / status_sums.cast<double>()).transpose() << std::endl;
        std::cout << std::setw(20) << "Expected:" << std::setw(20) << params.latent_period_scale * params.latent_period_shape
                  << std::setw(20) << params.infect_period_scale * params.infect_period_shape
                  << std::setw(20) << params.recover_period_scale * params.recover_period_shape
                  << std::setw(20) << params.dying_period_scale * params.dying_period_shape << std::endl;
        std::cout << "Pr(recovery): " << (1. * status_sums[2]) / (status_sums[2] + status_sums[3])
                  << " Expected " << params.p_recovery << std::endl;
    }
};

int main(int argc, char *argv[])
{
    params_struct params;
    params.verbose = true;
    uint seed;
    double R0;

    if (argc > 1)
    {
        R0 = std::atof(argv[1]);
    }
    else
    {
        R0 = 1.7;
    }
    if (argc > 2)
    {
        seed = std::atoi(argv[2]);
    }
    else
    {
        seed = static_cast<uint>(std::chrono::system_clock::now().time_since_epoch().count());
        std::cout << "Using seed = " << seed << std::endl;
    }
    std::mt19937_64 prng(seed);
    params.infect_delta = params.infect_period_shape * params.infect_period_scale / R0;

    Outbreak ob(prng, params);

    std::cout << "Estimated R0: " << ob.getR0() << std::endl;

    // Eigen::MatrixXi c = ob.getCounters();
    // std::cout << c << std::endl;

    std::vector<Infectee*> inf = ob.getInfected();
    if (inf.size() > 3)
    {
        std::cout << *(inf[0]) << std::endl;
        std::cout << *(inf[1]) << std::endl;
        std::cout << *(inf[2]) << std::endl;
        std::cout << *(inf[(int) (inf.size()/4)]) << std::endl;
    }

    ob.printStats();
    return 0;
}


#include <iostream>
#include <string>
#include <acado_toolkit.hpp>
#include <acado_gnuplot.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#define pi 3.14159

using namespace std;

static boost::random::mt19937 gen;

int gen_data(float bounds[8],int index, char cost_func, int thread);
float rand_gen(float lim[2]);

int gen_data(float bounds[8], int index, char cost_func, int thread)
{
  std::string states_nm, parameters_nm, control_nm;
  bool flag = false;

  USING_NAMESPACE_ACADO

  DifferentialState        q1,q2,qd1,qd2;   // the differential states
  Control                  tau1,tau2;       // the control input u
  Parameter                T;
  DifferentialEquation     f(0.0,0.5);

  //  -------------------------------------
  OCP ocp( 0.0, 0.5 );

  switch(cost_func){
    case 'a': ocp.minimizeMayerTerm(T*T + tau1*qd1*tau1*qd1 + tau2*qd2*tau2*qd2);
              cout << "cost function = sq_power" << endl;
              break;

    default:  //ocp.minimizeMayerTerm(T*T + tau1*tau1 + tau2*tau2);
              ocp.minimizeLagrangeTerm(tau1*tau1);
              ocp.minimizeLagrangeTerm(tau2*tau2);
              cout << "cost function = sq_torque" << endl;
  }

  f << dot(q1) == qd1;
  f << dot(q2) == qd2;
  f << dot(qd1) == -(48*tau1 - 48*tau2 + 24*qd1*qd1*sin(q2) + 24*qd2*qd2*sin(q2) + 18*qd1*qd1*sin(2*q2) - 72*tau2*cos(q2) + 48*qd1*qd2*sin(q2))/(36*cos(q2)*cos(q2) - 64);
  f << dot(qd2) == (48*tau1 - 240*tau2 + 120*qd1*qd1*sin(q2) + 24*qd2*qd2*sin(q2) + 36*qd1*qd1*sin(2*q2) + 18*qd2*qd2*sin(2*q2) + 72*tau1*cos(q2) - 144*tau2*cos(q2) + 48*qd1*qd2*sin(q2) + 36*qd1*qd2*sin(2*q2))/(18*cos(2*q2) - 46);

  ocp.subjectTo(f);
  ocp.subjectTo(AT_START, q1 == bounds[0] );
  ocp.subjectTo(AT_START, q2 == bounds[1]);
  ocp.subjectTo(AT_START, qd1 == bounds[2]);
  ocp.subjectTo(AT_START, qd2 == bounds[3]);

  ocp.subjectTo(AT_END, q1 == bounds[4]);
  ocp.subjectTo(AT_END, q2 == bounds[5]);
  ocp.subjectTo(AT_END, qd1 == bounds[6]);
  ocp.subjectTo(AT_END, qd2 == bounds[7]);

  // ocp.subjectTo(0.0 <= q1 <= 2*pi);
  // ocp.subjectTo(0.0 <= q2 <= 2*pi);
  ocp.subjectTo(-400 <= tau1 <= 400);  // bounds on the control input u,
  ocp.subjectTo(-400 <= tau2 <= 400);

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

  OptimizationAlgorithm algorithm(ocp);     // the optimization algorithm
  algorithm.set( DISCRETIZATION_TYPE , MULTIPLE_SHOOTING);
  algorithm.set( INTEGRATOR_TYPE , INT_BDF);
  algorithm.set( HESSIAN_APPROXIMATION   , BLOCK_BFGS_UPDATE);
  algorithm.set( KKT_TOLERANCE   , 1e-4);
  algorithm.set( ABSOLUTE_TOLERANCE, 1e-4);
  algorithm.set( INTEGRATOR_TOLERANCE, 1e-4);
  algorithm.set( MAX_NUM_ITERATIONS, 1000);
  algorithm.set( MAX_NUM_INTEGRATOR_STEPS, 10000);

  algorithm.set(PRINT_COPYRIGHT,BT_FALSE);
  algorithm.set(PRINTLEVEL,LOW);
  algorithm.set(PRINT_INTEGRATOR_PROFILE,BT_FALSE);
  algorithm.set(PRINT_SCP_METHOD_PROFILE,BT_FALSE);

  algorithm.solve();                        // solves the problem.

  VariablesGrid grid;
  algorithm.getDifferentialStates(grid);
  DVector final_state(4), req_state(4), diff(4);
  final_state = grid.getLastVector();
  req_state(0)=bounds[4];
  req_state(1)=bounds[5];
  req_state(2)=bounds[6];
  req_state(3)=bounds[7];
  diff = final_state - req_state;

  if (diff.getNorm(VN_L2) < 0.001){
    flag = true;
    states_nm = "states_"+to_string(index)+"_"+to_string(thread)+".txt";
    parameters_nm = "parameters_"+to_string(index)+"_"+to_string(thread)+".txt";
    control_nm = "control_"+to_string(index)+"_"+to_string(thread)+".txt";
    algorithm.getDifferentialStates(states_nm.c_str());
    algorithm.getObjectiveValue(parameters_nm.c_str());
    algorithm.getControls(control_nm.c_str());
  }
  clearAllStaticCounters();
  return flag;
}

float rand_gen(float lim[2]) {
    static boost::random::uniform_real_distribution<> dist(0, 1);
    return lim[0] + dist(gen)*(lim[1] - lim[0]);
}

int main(int argc, const char * argv[]){
  // srand (time(NULL));
  int i;
  float q_lims[2] = {0.0,2*pi};
  float qd_lims[2] = {-30.0,30.0};

  if(argc!=4){
    cout << "Incorrect number of arguments." << endl;
  }
  else{
    int num_iter = atoi(argv[2]);
    gen.seed(time(0) + atoi(argv[3]));
    i = 0;
    while(i<num_iter){
      float bounds[8] = {rand_gen(q_lims),rand_gen(q_lims),rand_gen(qd_lims),rand_gen(qd_lims),rand_gen(q_lims),rand_gen(q_lims),rand_gen(qd_lims),rand_gen(qd_lims)};
      cout << i << endl;
      cout << bounds[0] << " " << bounds[1] << " " << bounds[2] << " " << bounds[3] << " " << bounds[4] << " " << bounds[5] << " " << bounds[6] << " " << bounds[7] << endl;
      bool success = gen_data(bounds,i,*argv[1],atoi(argv[3]));
      if(success){
        i++;
      }
    }
  }
  return 0;
}


// ===========================================================================
// Imagine++ Libraries
// Copyright (C) Imagine
// For detailed information: http://imagine.enpc.fr/software
// ===========================================================================

#include "Imagine/LinAlg/MyEigen.h"
#include <Eigen/Dense>

using namespace Eigen;

//N'existe pas dans Eigen Blas
//corriger
//info=0 si succes, -i si ieme parametre illegal, i si bdsdc ne converge pas
//info important car utilise dans Matrix.h
// Singular values decomposition
/* jobz est non usite... et est dans la liste des arguments de Imagine::svd */
template <typename T>
void subSingularValuesDecomposition(const char *jobz,int *m,int *n, T *a, T *s, T *u, T *vt, int *info)
{
    assert((*jobz == 'A') || (*jobz == 'S')); // ?
    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (a, *m, *n, OuterStride<>(*m));
    Eigen::JacobiSVD<Matrix<T, Dynamic, Dynamic> > svd (AMap, ComputeFullU | ComputeFullV);
    Matrix<T, Dynamic, Dynamic> u1 = svd.matrixU();
    Matrix<T, Dynamic, Dynamic> v1 = svd.matrixV();
    Matrix<T, Dynamic, Dynamic> sv = svd.singularValues();
    Map<Matrix<T, Dynamic, 1> > sMap (s, sv.rows());
    sMap = sv;
    Map<Matrix<T, Dynamic, Dynamic> > uMap (u, u1.rows(), u1.cols());
    uMap = u1;
    Map<Matrix<T, Dynamic, Dynamic> > vtMap (vt, v1.rows(), v1.cols());
    vtMap = v1.transpose();
    Matrix<T, Dynamic, Dynamic> sigma = Matrix<T, Dynamic, Dynamic>::Zero(*m,*n);
    for (int i = 0 ; i < sMap.rows() ; i++){
        sigma(i, i) = sMap(i);
    };
    *info = 1 - AMap.isApprox(uMap * sigma * vtMap);
}

void singularValuesDecomposition(const char *jobz,int *m,int *n, double *a, double *s, double *u, double *vt, int *info) { subSingularValuesDecomposition<double>(jobz, m, n, a, s, u, vt, info); }
void singularValuesDecomposition(const char *jobz,int *m,int *n, float *a, float *s, float *u, float *vt, int *info) { subSingularValuesDecomposition<float>(jobz, m, n, a, s, u, vt, info); }

// Returns Eigen values (replaces xgeev_)
template <typename T>
void subEigenValues(int *n, T *a, T *wr, T *wi, T *vr, int *info)
{
    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (a, *n, *n, OuterStride<>(*n));

    EigenSolver<Matrix<T, Dynamic, Dynamic> > ces(AMap);
    *info = 1 - int(ces.info());
    Matrix<std::complex<T>, Dynamic, 1> w1 = ces.eigenvalues();
    Map<Matrix<T, Dynamic, 1> > wrMap (wr, w1.rows());
    Map<Matrix<T, Dynamic, 1> > wiMap (wi, w1.rows());
    Matrix<std::complex<T>, Dynamic, Dynamic> Vr = ces.eigenvectors();
    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > VrMap (vr, *n, Vr.cols(), OuterStride<>(1));
    wrMap = w1.real();
    wiMap = w1.imag();
    int j = 0;
    while (j < Vr.cols())
    {
        Matrix<T, Dynamic, 1> Xr = Vr.col(j).real();
        Matrix<T, Dynamic, 1> Xc = Vr.col(j).imag();
        if (Xc.isZero())
        {
            VrMap.col(j) = Xr;
            j++;
        }
        else
        {
            VrMap.col(j) = Xr;
            VrMap.col(j + 1) = Xc;
            j += 2;
        };
    }
}

void eigenValues(int *n, double *a, double *wr, double *wi, double *vr, int *info) { subEigenValues<double>(n, a, wr, wi, vr, info); }
void eigenValues(int *n, float *a, float *wr, float *wi, float *vr, int *info) { subEigenValues<float>(n, a, wr, wi, vr, info); }


#include <vector>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <tf/transform_datatypes.h>

tf::Quaternion getAverageQuaternion(
  const std::vector<tf::Quaternion>& quaternions,
  const std::vector<double>& weights)
{
  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, quaternions.size());
  Eigen::Vector3d vec;
  for (size_t i = 0; i < quaternions.size(); ++i)
  {
    // Weigh the quaternions according to their associated weight
    tf::Quaternion quat = quaternions[i] * weights[i];
    // Append the weighted Quaternion to a matrix Q.
    Q(0,i) = quat.x();
    Q(1,i) = quat.y();
    Q(2,i) = quat.z();
    Q(3,i) = quat.w();
  }

  // Creat a solver for finding the eigenvectors and eigenvalues
  Eigen::EigenSolver<Eigen::MatrixXd> es(Q * Q.transpose());

  // Find index of maximum (real) Eigenvalue.
  auto eigenvalues = es.eigenvalues();
  size_t max_idx = 0;
  double max_value = eigenvalues[max_idx].real();
  for (size_t i = 1; i < 4; ++i)
  {
    double real = eigenvalues[i].real();
    if (real > max_value)
    {
      max_value = real;
      max_idx = i;
    }
  }

  // Get corresponding Eigenvector, normalize it and return it as the average quat
  auto eigenvector = es.eigenvectors().col(max_idx).normalized();

  tf::Quaternion mean_orientation(
    eigenvector[0].real(),
    eigenvector[1].real(),
    eigenvector[2].real(),
    eigenvector[3].real()
  );

  return mean_orientation;
}


//
//  cross_product.cpp
//  BGV-Adder
//
//  Created by Hindrik Stegenga on 26/10/2020.
//  Copyright © 2020 RUG. All rights reserved.
//

#include "cross_product.hpp"
#include <helib/FHE.h>
#include <NTL/ZZX.h>
#include <NTL/tools.h>

using helib::Ctxt;

array<long, CV_SIZE> compute_cross_product(array<i16, CV_SIZE> lhs, array<i16, CV_SIZE> rhs) {
    long k = 128; // Security parameter
    long L = 16; // Number of levels in the modulus default is 16
    long c = 3; // Nr of columns in key switch matrix.
    long w = 64; // secret key hamming weight

    // Change to 65537 for noise warning. (Always fun times)
    // Compensate for that by setting L = 128. (Increase modchain basically)
    // Set P to 65537 and L to 32 to showcase decryption failures.

    long p = 1021; // plaintext base default = 1021
    long d = 0; // Degree of field extension
    long r = 1; // hensel lifting

    // Determine a value for m
    auto m = helib::FindM(k, L, c, p, d, 0, 0);
    // Setup context
    auto context = helib::Context(m, p, r);
    // Build mod chain
    helib::buildModChain(context, L, c);

    // Generating secret key and public key
    NTL::ZZX encryption_polynomial = context.alMod.getFactorsOverZZ()[0];
    auto secretKey = helib::SecKey(context);
    secretKey.GenSecKey();
    const helib::PubKey& publicKey = secretKey;

    // Initialize ciphertexts
    array<Ctxt, CV_SIZE> lhs_ciphertext {
        Ctxt(publicKey),
        Ctxt(publicKey),
        Ctxt(publicKey)
    }, rhs_ciphertext {
        Ctxt(publicKey),
        Ctxt(publicKey),
        Ctxt(publicKey)
    };

    // Plaintext must be encrypted as a polynomial using zzx api.
    for (size_t i = 0; i < CV_SIZE; ++i) {
        publicKey.Encrypt(lhs_ciphertext[i], NTL::ZZX(lhs[i]));
        publicKey.Encrypt(rhs_ciphertext[i], NTL::ZZX(rhs[i]));
    }

    // Apply operations on the ciphertexts.
    // Cross product is vec a x b.
    // 0 => ay * bz - az * by
    // 1 => az * bx - ax * bz
    // 2 => ax * by - ay * bx

    // Left hand side of equations
    array<Ctxt, CV_SIZE> lhs_a = {
        lhs_ciphertext[1], //ay
        lhs_ciphertext[2], //az
        lhs_ciphertext[0], //ax
    };
    array<Ctxt, CV_SIZE> lhs_b = {
        rhs_ciphertext[2], //bz
        rhs_ciphertext[0], //bx
        rhs_ciphertext[1], //by
    };

    // Compute first set of products
    for (size_t i = 0; i < CV_SIZE; ++i) {
        lhs_a[i] *= lhs_b[i];
    }

    // Right hand side of equations
    array<Ctxt, CV_SIZE> rhs_a = {
        lhs_ciphertext[2], //az
        lhs_ciphertext[0], //ax
        lhs_ciphertext[1], //ay
    };
    array<Ctxt, CV_SIZE> rhs_b = {
        rhs_ciphertext[1], //by
        rhs_ciphertext[2], //bz
        rhs_ciphertext[0], //bx
    };

    // Compute second set of products
    for (size_t i = 0; i < CV_SIZE; ++i) {
        rhs_a[i] *= rhs_b[i];
    }

    // Compute negation of products
    for (size_t i = 0; i < CV_SIZE; ++i) {
        lhs_a[i] -= rhs_a[i];
    }

    array<Ctxt, CV_SIZE> cipher_results = lhs_a;

    // Decrypt the results using secret key and convert back from
    // polynomial representation to numeric.
    array<long, CV_SIZE> return_values = {0,0,0};
    array<NTL::ZZX, CV_SIZE> plaintext_results;
    for (size_t i = 0; i < 3; ++i) {
        NTL::ZZX zzx;
        secretKey.Decrypt(zzx, cipher_results[i]);
        conv(return_values[i], zzx[0]);
        // Compensate for negative numbers by checking if it's larger than p/2,
        // In such case it wrapped around due to negative numbers
        if (return_values[i] > p / 2) {
            return_values[i] += (-1 * p);
        }
    }

    return return_values;
}


#include "ShapeEvaluation.h"
#include "EvaluationUtil.h"

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

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMajorMatrix;

namespace shapeworks {

  //---------------------------------------------------------------------------
double ShapeEvaluation::ComputeCompactness(const ParticleSystem &particleSystem, const int nModes,
                                                       const std::string &saveTo)
{
  const int N = particleSystem.N();
  if (nModes > N-1){
    throw std::invalid_argument("Invalid mode of variation specified");
  }
  Eigen::VectorXd cumsum = ShapeEvaluation::ComputeFullCompactness(particleSystem);

  if (!saveTo.empty()) {
    std::ofstream of(saveTo);
    of << cumsum;
    of.close();
  }

  return cumsum(nModes - 1);
}

//---------------------------------------------------------------------------
Eigen::VectorXd ShapeEvaluation::ComputeFullCompactness(const ParticleSystem &particleSystem, std::function<void(float)> progress_callback)
{
  const int N = particleSystem.N();
  const int D = particleSystem.D();
  const int num_modes = N-1; // the number of modes is one less than the number of samples

  if (num_modes < 1) {
    return Eigen::VectorXd();
  }
  Eigen::MatrixXd Y = particleSystem.Particles();
  const Eigen::VectorXd mu = Y.rowwise().mean();
  Y.colwise() -= mu;

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(Y);
  const auto S = svd.singularValues().array().pow(2) / (N * D);

  // Compute cumulative sum
  Eigen::VectorXd cumsum(num_modes);
  cumsum(0) = S(0);
  for (int i = 1; i < num_modes; i++) {
    if (progress_callback) {
      progress_callback(static_cast<float>(i) / static_cast<float>(N));
    }
    cumsum(i) = cumsum(i-1) + S(i);
  }
  cumsum /= S.sum();
  return cumsum;
}

//---------------------------------------------------------------------------
double ShapeEvaluation::ComputeGeneralization(const ParticleSystem &particleSystem, const int nModes,
                                                          const std::string &saveTo)
{
  const int N = particleSystem.N();
  const int D = particleSystem.D();
  const Eigen::MatrixXd &P = particleSystem.Particles();

  if (nModes > N-1){
    throw std::invalid_argument("Invalid mode of variation specified");
  }
  // Keep track of the reconstructions so we can visualize them later
  std::vector<Reconstruction> reconstructions;

  double totalDist = 0.0;
  for (int leave = 0; leave < N; leave++) {

    Eigen::MatrixXd Y(D, N - 1);
    Y.leftCols(leave) = P.leftCols(leave);
    Y.rightCols(N - leave - 1) = P.rightCols(N - leave - 1);

    const Eigen::VectorXd mu = Y.rowwise().mean();
    Y.colwise() -= mu;
    const Eigen::VectorXd Ytest = P.col(leave);

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Y, Eigen::ComputeFullU);
    const auto epsi = svd.matrixU().block(0, 0, D, nModes);
    const auto betas = epsi.transpose() * (Ytest - mu);
    const Eigen::VectorXd rec = epsi * betas + mu;

    const int numParticles = D / VDimension;
    const Eigen::Map<const RowMajorMatrix> Ytest_reshaped(Ytest.data(), numParticles, VDimension);
    const Eigen::Map<const RowMajorMatrix> rec_reshaped(rec.data(), numParticles, VDimension);
    const double dist = (rec_reshaped - Ytest_reshaped).rowwise().norm().sum() / numParticles;
    totalDist += dist;

    reconstructions.push_back({dist, leave, rec_reshaped});
  }
  const double generalization = totalDist / N;

  // Save the reconstructions if needed. Generates XML files that can be opened in
  // ShapeWorksView2
  if (!saveTo.empty()) {
    SaveReconstructions(reconstructions, particleSystem.Paths(), saveTo);
  }

  return generalization;
}

Eigen::VectorXd ShapeEvaluation::ComputeFullGeneralization(const ParticleSystem &particleSystem, std::function<void(float)> progress_callback)
{
  const int N = particleSystem.N();
  const int D = particleSystem.D();
  const Eigen::MatrixXd &P = particleSystem.Particles();

  if (N <= 1) {
    return Eigen::VectorXd();
  }

  Eigen::VectorXd generalizations(N-1);

  Eigen::VectorXd totalDists = Eigen::VectorXd::Zero(N-1);

  for (int leave = 0; leave < N; leave++) {
    if (progress_callback) {
      progress_callback(static_cast<float>(leave) / static_cast<float>(N));
    }
    Eigen::MatrixXd Y(D, N - 1);
    Y.leftCols(leave) = P.leftCols(leave);
    Y.rightCols(N - leave - 1) = P.rightCols(N - leave - 1);

    const Eigen::VectorXd mu = Y.rowwise().mean();
    Y.colwise() -= mu;
    const Eigen::VectorXd Ytest = P.col(leave);

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Y, Eigen::ComputeFullU);

    for (int mode = 1; mode < N; mode++) {

      const auto epsi = svd.matrixU().block(0, 0, D, mode);
      const auto betas = epsi.transpose() * (Ytest - mu);
      const Eigen::VectorXd rec = epsi * betas + mu;

      const int numParticles = D / VDimension;
      const Eigen::Map<const RowMajorMatrix> Ytest_reshaped(Ytest.data(), numParticles, VDimension);
      const Eigen::Map<const RowMajorMatrix> rec_reshaped(rec.data(), numParticles, VDimension);
      const double dist = (rec_reshaped - Ytest_reshaped).rowwise().norm().sum() / numParticles;
      totalDists(mode-1) += dist;
    }
  }

  generalizations = totalDists / N;

  return generalizations;
}

//---------------------------------------------------------------------------
double ShapeEvaluation::ComputeSpecificity(const ParticleSystem &particleSystem, const int nModes,
                                                       const std::string &saveTo)
{

  const int N = particleSystem.N();
  const int D = particleSystem.D();

  if (nModes > N-1){
    throw std::invalid_argument("Invalid mode of variation specified");
  }
  const int nSamples = 1000;

  // Keep track of the reconstructions so we can visualize them later
  std::vector<Reconstruction> reconstructions;

  Eigen::VectorXd meanSpecificity(nModes);
  Eigen::VectorXd stdSpecificity(nModes);
  Eigen::MatrixXd spec_store(nModes, 4);

  // PCA calculations
  const Eigen::MatrixXd &ptsModels = particleSystem.Particles();
  const Eigen::VectorXd mu = ptsModels.rowwise().mean();
  Eigen::MatrixXd Y = ptsModels;

  Y.colwise() -= mu;

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(Y, Eigen::ComputeFullU);
  const auto epsi = svd.matrixU().block(0, 0, D, nModes);
  const auto allEigenValues = svd.singularValues();
  const auto eigenValues = allEigenValues.head(nModes);

  Eigen::MatrixXd samplingBetas(nModes, nSamples);
  MultiVariateNormalRandom sampling{eigenValues.asDiagonal()};
  for (int modeNumber = 0; modeNumber < nModes; modeNumber++) {
    for (int i = 0; i < nSamples; i++) {
      samplingBetas.col(i) = sampling();
    }

    Eigen::MatrixXd samplingPoints = (epsi * samplingBetas).colwise() + mu;

    const int numParticles = D / VDimension;
    const int nTrain = ptsModels.cols();

    Eigen::VectorXd distanceToClosestTrainingSample(nSamples);

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

      Eigen::VectorXd pts_m = samplingPoints.col(i);
      Eigen::MatrixXd ptsDistance_vec = ptsModels.colwise() - pts_m;
      Eigen::MatrixXd ptsDistance(Eigen::MatrixXd::Constant(1, nTrain, 0.0));

      for (int j = 0; j < nTrain; j++) {
        Eigen::Map<const RowMajorMatrix> ptsDistance_vec_reshaped(ptsDistance_vec.col(j).data(), numParticles,
                                                                  VDimension);
        ptsDistance(j) = (ptsDistance_vec_reshaped).rowwise().norm().sum();
      }

      int closestIdx, _r;
      distanceToClosestTrainingSample(i) = ptsDistance.minCoeff(&_r, &closestIdx);

      Eigen::Map<const RowMajorMatrix> pts_m_reshaped(pts_m.data(), numParticles, VDimension);
      reconstructions.push_back(Reconstruction{
              distanceToClosestTrainingSample(i),
              (int) closestIdx,
              pts_m_reshaped,
      });
    }

    meanSpecificity(modeNumber) = distanceToClosestTrainingSample.mean();
  }

  if (!saveTo.empty()) {
    SaveReconstructions(reconstructions, particleSystem.Paths(), saveTo);
  }

  const int numParticles = D / VDimension;
  const double specificity = meanSpecificity(nModes - 1) / numParticles;

  return specificity;
}

//---------------------------------------------------------------------------
Eigen::VectorXd ShapeEvaluation::ComputeFullSpecificity(const ParticleSystem &particleSystem, std::function<void(float)> progress_callback)
{
  const int N = particleSystem.N();
  const int D = particleSystem.D();
  const int numParticles = D / VDimension;

  Eigen::VectorXd specificities(N-1);

  // PCA calculations
  const Eigen::MatrixXd &ptsModels = particleSystem.Particles();
  const int nTrain = ptsModels.cols();

  const Eigen::VectorXd mu = ptsModels.rowwise().mean();
  Eigen::MatrixXd Y = ptsModels;
  Y.colwise() -= mu;
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(Y, Eigen::ComputeFullU);
  const auto allEigenValues = svd.singularValues();

  for (int nModes=1;nModes<N;nModes++) {
    if (progress_callback) {
      progress_callback(static_cast<float>(nModes) / static_cast<float>(N));
    }

    const int nSamples = 1000;

    Eigen::VectorXd stdSpecificity(nModes);
    Eigen::MatrixXd spec_store(nModes, 4);
    const auto eigenValues = allEigenValues.head(nModes);
    const auto epsi = svd.matrixU().block(0, 0, D, nModes);

    Eigen::MatrixXd samplingBetas(nModes, nSamples);
    MultiVariateNormalRandom sampling{eigenValues.asDiagonal()};
    for (int i = 0; i < nSamples; i++) {
      samplingBetas.col(i) = sampling();
    }

    Eigen::MatrixXd samplingPoints = (epsi * samplingBetas).colwise() + mu;
    Eigen::VectorXd distanceToClosestTrainingSample(nSamples);

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

      Eigen::VectorXd pts_m = samplingPoints.col(i);
      Eigen::MatrixXd ptsDistance_vec = ptsModels.colwise() - pts_m;
      Eigen::MatrixXd ptsDistance(Eigen::MatrixXd::Constant(1, nTrain, 0.0));

      for (int j = 0; j < nTrain; j++) {
        Eigen::Map<const RowMajorMatrix> ptsDistance_vec_reshaped(ptsDistance_vec.col(j).data(), numParticles,
                                                                  VDimension);
        ptsDistance(j) = (ptsDistance_vec_reshaped).rowwise().norm().sum();
      }

      int closestIdx, _r;
      distanceToClosestTrainingSample(i) = ptsDistance.minCoeff(&_r, &closestIdx);
    }

    double meanSpecificity = distanceToClosestTrainingSample.mean();
    const double specificity = meanSpecificity / numParticles;
    specificities(nModes-1) = specificity;
  }
  return specificities;
}

} // shapeworks


/*! \file mesh_generator.hpp
  \brief Set of functions to generate points.
  \author Elad Steinberg
*/
#ifndef MESHGENERATOR_HPP
#define MESHGENERATOR_HPP 1

#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#endif // _MSC_VER
#include <vector>
#include <cmath>
#include "../tessellation/geometry.hpp"
#include <algorithm>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

/*! \brief Generates a cartesian mesh
  \param nx Number of points along the x axis
  \param ny Number of points along the y axis
  \param lower_left Lower left point
  \param upper_right Upper right point
  \return Set of two dimensional points
*/
std::vector<Vector2D> cartesian_mesh(int nx, int ny,
				Vector2D const& lower_left,
				Vector2D const& upper_right);

/*!
  \brief Generates a round grid with constant point density
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param xc X of circle center
  \param yc Y of circle center
  \param xmin Left edge of confining rectangle
  \param xmax Right edge of confining rectangle
  \param ymax Upper edge of confining rectangle
  \param ymin Lower edge of confining rectangle
  \return List of two dimensional points
*/
std::vector<Vector2D> CirclePointsRmax(int PointNum,double Rmin,double Rmax,
	double xmax, double ymax, double xmin, double ymin, double xc = 0, double yc = 0);

/*!
  \brief Generates a round grid with 1/r^2 point density confined to a rectangle given by xmin,xmax,ymin and ymax.
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param xc X of circle center
  \param yc Y of circle center
  \param xmin Left edge of confining rectangle
  \param xmax Right edge of confining rectangle
  \param ymax Upper edge of confining rectangle
  \param ymin Lower edge of confining rectangle
  \return List of two dimensional points
*/
std::vector<Vector2D> CirclePointsRmax_2(int PointNum,double Rmin,double Rmax,
				    double xc=0,double yc=0,double xmax=1,double ymax=1,double xmin=-1,
				    double ymin=1);
/*!
  \brief Generates a round grid with 1/r point density confined to a rectangle given by xmin,xmax,ymin and ymax.
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param xc X of circle center
  \param yc Y of circle center
  \param xmin Left edge of confining rectangle
  \param xmax Right edge of confining rectangle
  \param ymax Upper edge of confining rectangle
  \param ymin Lower edge of confining rectangle
  \return List of two dimensional points
*/
std::vector<Vector2D> CirclePointsRmax_1(int PointNum,double Rmin,double Rmax,
				    double xc=0,double yc=0,double xmax=1,double ymax=1,double xmin=-1,double ymin=-1);

/*!
  \brief Creates a circle of evenly spaced points
  \param point_number Number of points along the circumference
  \param radius Radius of the circle
  \param center Position of the center of the circle
  \return List of two dimensional points
*/
std::vector<Vector2D> circle_circumference(size_t point_number,
					   double radius,
					   Vector2D const& center);

/*!
  \brief Creates a line of evenly spaced points y=slope*x+b
  \param PointNum The number of points
  \param xmin The minimum x of the line
  \param xmax The maximum x of the line
  \param ymin The minimum y of the line
  \param ymax The maximum y of the line
  \return List of two dimensional points
*/
std::vector<Vector2D> Line(int PointNum,double xmin,double xmax,double ymin,double ymax);

/*!
  \brief Generates a round grid with r^alpha point density confined to a rectangle given by xmin,xmax,ymin and ymax.
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param xc X of circle center
  \param yc Y of circle center
  \param xmin Left edge of confining rectangle
  \param xmax Right edge of confining rectangle
  \param ymax Upper edge of confining rectangle
  \param ymin Lower edge of confining rectangle
  \param alpha The point density, should not be -1 or -2
  \return List of two dimensional points
*/
std::vector<Vector2D> CirclePointsRmax_a(int PointNum,double Rmin,double Rmax,
				    double xc,double yc,double xmax,double ymax,double xmin,double ymin,
				    double alpha);

/*!
  \brief Generates a rectangular grid with random 1/r point density
  \param PointNum The number of points.
  \param xl The left boundary
  \param xr The right boundary
  \param yd The lower boundary
  \param yu The upper boundary
  \param minR The inner radius in which there are no points
  \param xc The X of center of the circle
  \param yc The Y of center of the circle
  \return List of two dimensional points
*/
std::vector<Vector2D> RandPointsR(int PointNum,double xl=-0.5,double xr=0.5,
			     double yd=-0.5,double yu=0.5,double minR=0,double xc=0,
				 double yc=0);

/*!
  \brief Generates a random rectangular grid with uniform point density and a constant seed
  \param PointNum The number of points.
  \param lowerleft The lower left point of the domain
  \param upperright The upper right point of the domain
  \return List of two dimensional points
*/
std::vector<Vector2D> RandSquare(int PointNum,Vector2D const& lowerleft, Vector2D const& upperright);

/*!
\brief Generates a random rectangular grid with uniform point density and a constant seed
\param PointNum The number of points.
\param xl The left boundary
\param xr The right boundary
\param yd The lower boundary
\param yu The upper boundary
\return List of two dimensional points
*/
std::vector<Vector2D> RandSquare(int PointNum, double xl = -0.5, double xr = 0.5,
	double yd = -0.5, double yu = 0.5);

/*!
  \brief Generates a random rectangular grid with uniform point density. This is when reseting the seed between calls isn't wanted
  \param PointNum The number of points.
  \param eng The random number generator
  \param xl The left boundary
  \param xr The right boundary
  \param yd The lower boundary
  \param yu The upper boundary
  \return List of two dimensional points
*/

std::vector<Vector2D> RandSquare(int PointNum,boost::random::mt19937 &eng,
	double xl=-0.5,double xr=0.5,double yd=-0.5,double yu=0.5);

/*!
  \brief Generates a random round grid with r^(1-a) point density
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param alpha The radial density of the points, shouldn't be 1
  \param lowerleft The lowerleft corner of the domain
  \param upperright The upperright corner of the domain
  \param center The center of the coordinates system
  \return List of two dimensional points
*/

std::vector<Vector2D> RandPointsRa(int PointNum,double Rmin,double Rmax,double alpha,
	Vector2D const& lowerleft,Vector2D const& upperright,Vector2D const& center=Vector2D(0,0));

/*!
  \brief Generates a random round grid with 1/r point density
  \param PointNum The number of points.
  \param Rmin The min radius
  \param Rmax The max radius
  \param xc X of circle center
  \param yc Y of circle center
  \return List of two dimensional points
*/
std::vector<Vector2D> RandPointsRmax(int PointNum,double Rmin,double Rmax,
				double xc=0,double yc=0);

/*!
\brief Generates a random round grid with 1/y point density where y is the cylindrical r coordinate
\param PointNum The number of points.
\param ll The lower left point of the domain. Should be ll.y>0
\param ur The upper right point of the domain
\return List of two dimensional points
*/
std::vector<Vector2D> RandPointsCylinder(int PointNum, Vector2D const& ll,Vector2D const& ur);

#endif //MESHGENERATOR_HPP


/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example iterative-mtl4.cpp
*
*   The following tutorial shows how to use the iterative solvers in ViennaCL with objects from the MTL4 Library directly.
*
*   \note MTL4 provides iterative solvers through the ITK library. You might also want to check these.
*
*   We begin with including the necessary headers:
**/

// include necessary system headers
#include <iostream>

// MTL4 headers
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

// Must be set prior to any ViennaCL includes if you want to use ViennaCL algorithms on Eigen objects
#define VIENNACL_WITH_MTL4 1

// ViennaCL includes
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include "vector-io.hpp"

/**
*  In the following we run the CG method, the BiCGStab method, and the GMRES method with MTL4 types directly.
*  First, the matrices are set up, then the respective solvers are called.
**/
int main(int, char *[])
{
  typedef double    ScalarType;

  mtl::compressed2D<ScalarType> mtl4_matrix;
  mtl4_matrix.change_dim(65025, 65025);
  set_to_zero(mtl4_matrix);

  mtl::dense_vector<ScalarType> mtl4_rhs(65025, 1.0);
  mtl::dense_vector<ScalarType> mtl4_result(65025, 0.0);
  mtl::dense_vector<ScalarType> mtl4_residual(65025, 0.0);

  /**
  * Read system from file
  **/

  mtl::io::matrix_market_istream("../examples/testdata/mat65k.mtx") >> mtl4_matrix;

  /**
  *  Conjugate Gradient (CG) solver:
  **/
  std::cout << "----- Running CG -----" << std::endl;
  mtl4_result = viennacl::linalg::solve(mtl4_matrix, mtl4_rhs, viennacl::linalg::cg_tag());

  mtl4_residual = mtl4_matrix * mtl4_result - mtl4_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(mtl4_residual) / viennacl::linalg::norm_2(mtl4_rhs) << std::endl;

  /**
  *  Stabilized Bi-Conjugate Gradient (BiCGStab) solver:
  **/
  std::cout << "----- Running BiCGStab -----" << std::endl;
  mtl4_result = viennacl::linalg::solve(mtl4_matrix, mtl4_rhs, viennacl::linalg::bicgstab_tag());

  mtl4_residual = mtl4_matrix * mtl4_result - mtl4_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(mtl4_residual) / viennacl::linalg::norm_2(mtl4_rhs) << std::endl;

  /**
  *  Generalized Minimum Residual (GMRES) solver:
  **/
  std::cout << "----- Running GMRES -----" << std::endl;
  mtl4_result = viennacl::linalg::solve(mtl4_matrix, mtl4_rhs, viennacl::linalg::gmres_tag());

  mtl4_residual = mtl4_matrix * mtl4_result - mtl4_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(mtl4_residual) / viennacl::linalg::norm_2(mtl4_rhs) << std::endl;

  /**
  *   That's it. Print a success message and exit.
  **/
  std::cout << std::endl;
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  std::cout << std::endl;
}