/*
 *
 * Copyright (c) Kresimir Fresl 2002
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Author assumes no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_CLAPACK_OVERLOADS_HPP
#define BOOST_NUMERIC_BINDINGS_CLAPACK_OVERLOADS_HPP

#include <boost/numeric/bindings/atlas/clapack_inc.hpp>
#include <boost/numeric/bindings/traits/type.hpp>

namespace boost { namespace numeric { namespace bindings {

  namespace atlas { namespace detail {

    //
    // general system of linear equations A * X = B
    //

    // 'driver' function -- factor and solve
    inline
    int gesv (CBLAS_ORDER const Order,
              int const N, int const NRHS,
              float* A, int const lda, int* ipiv,
              float* B, int const ldb)
    {
      return clapack_sgesv (Order, N, NRHS, A, lda, ipiv, B, ldb);
    }

    inline
    int gesv (CBLAS_ORDER const Order,
              int const N, int const NRHS,
              double* A, int const lda, int* ipiv,
              double* B, int const ldb)
    {
      return clapack_dgesv (Order, N, NRHS, A, lda, ipiv, B, ldb);
    }

    inline
    int gesv (CBLAS_ORDER const Order,
              int const N, int const NRHS,
              traits::complex_f* A, int const lda, int* ipiv,
              traits::complex_f* B, int const ldb)
    {
      return clapack_cgesv (Order, N, NRHS,
                            static_cast<void*> (A), lda, ipiv,
                            static_cast<void*> (B), ldb);
    }

    inline
    int gesv (CBLAS_ORDER const Order,
              int const N, int const NRHS,
              traits::complex_d* A, int const lda, int* ipiv,
              traits::complex_d* B, int const ldb)
    {
      return clapack_zgesv (Order, N, NRHS,
                            static_cast<void*> (A), lda, ipiv,
                            static_cast<void*> (B), ldb);
    }

    // LU factorization
    inline
    int getrf (CBLAS_ORDER const Order,
               int const M, int const N,
               float* A, int const lda, int* ipiv)
    {
      return clapack_sgetrf (Order, M, N, A, lda, ipiv);
    }

    inline
    int getrf (CBLAS_ORDER const Order,
               int const M, int const N,
               double* A, int const lda, int* ipiv)
    {
      return clapack_dgetrf (Order, M, N, A, lda, ipiv);
    }

    inline
    int getrf (CBLAS_ORDER const Order,
               int const M, int const N,
               traits::complex_f* A, int const lda, int* ipiv)
    {
      return clapack_cgetrf (Order, M, N, static_cast<void*> (A), lda, ipiv);
    }

    inline
    int getrf (CBLAS_ORDER const Order,
               int const M, int const N,
               traits::complex_d* A, int const lda, int* ipiv)
    {
      return clapack_zgetrf (Order, M, N, static_cast<void*> (A), lda, ipiv);
    }

    // solve (using factorization computed by getrf())
    inline
    int getrs (CBLAS_ORDER const Order, CBLAS_TRANSPOSE const Trans,
               int const N, int const NRHS,
               float const* A, int const lda, int const* ipiv,
               float* B, int const ldb)
    {
      return clapack_sgetrs (Order, Trans, N, NRHS, A, lda, ipiv, B, ldb);
    }

    inline
    int getrs (CBLAS_ORDER const Order, CBLAS_TRANSPOSE const Trans,
               int const N, int const NRHS,
               double const* A, int const lda, int const* ipiv,
               double* B, int const ldb)
    {
      return clapack_dgetrs (Order, Trans, N, NRHS, A, lda, ipiv, B, ldb);
    }

    inline
    int getrs (CBLAS_ORDER const Order, CBLAS_TRANSPOSE const Trans,
               int const N, int const NRHS,
               traits::complex_f const* A, int const lda,
               int const* ipiv,
               traits::complex_f* B, int const ldb)
    {
      return clapack_cgetrs (Order, Trans, N, NRHS,
                             static_cast<void const*> (A), lda, ipiv,
                             static_cast<void*> (B), ldb);
    }

    inline
    int getrs (CBLAS_ORDER const Order, CBLAS_TRANSPOSE const Trans,
               int const N, int const NRHS,
               traits::complex_d const* A, int const lda,
               int const* ipiv,
               traits::complex_d* B, int const ldb)
    {
      return clapack_zgetrs (Order, Trans, N, NRHS,
                             static_cast<void const*> (A), lda, ipiv,
                             static_cast<void*> (B), ldb);
    }

    // invert (using factorization computed by getrf())
    inline
    int getri (CBLAS_ORDER const Order,
               int const N, float* A, int const lda,
               int const* ipiv)
    {
      return clapack_sgetri (Order, N, A, lda, ipiv);
    }

    inline
    int getri (CBLAS_ORDER const Order,
               int const N, double* A, int const lda,
               int const* ipiv)
    {
      return clapack_dgetri (Order, N, A, lda, ipiv);
    }

    inline
    int getri (CBLAS_ORDER const Order,
               int const N, traits::complex_f* A, int const lda,
               int const* ipiv)
    {
      return clapack_cgetri (Order, N, static_cast<void*> (A), lda, ipiv);
    }

    inline
    int getri (CBLAS_ORDER const Order,
               int const N, traits::complex_d* A, int const lda,
               int const* ipiv)
    {
      return clapack_zgetri (Order, N, static_cast<void*> (A), lda, ipiv);
    }

    //
    // system of linear equations A * X = B
    // with A symmetric positive definite matrix
    //

    // 'driver' function -- factor and solve
    inline
    int posv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, int const NRHS,
              float* A, int const lda, float* B, int const ldb)
    {
      return clapack_sposv (Order, Uplo, N, NRHS, A, lda, B, ldb);
    }

    inline
    int posv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, int const NRHS,
              double* A, int const lda, double* B, int const ldb)
    {
      return clapack_dposv (Order, Uplo, N, NRHS, A, lda, B, ldb);
    }

    inline
    int posv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, int const NRHS,
              traits::complex_f* A, int const lda,
              traits::complex_f* B, int const ldb)
    {
      return clapack_cposv (Order, Uplo, N, NRHS,
                            static_cast<void*> (A), lda,
                            static_cast<void*> (B), ldb);
    }

    inline
    int posv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, int const NRHS,
              traits::complex_d* A, int const lda,
              traits::complex_d* B, int const ldb)
    {
      return clapack_zposv (Order, Uplo, N, NRHS,
                            static_cast<void*> (A), lda,
                            static_cast<void*> (B), ldb);
    }

    // Cholesky factorization
    inline
    int potrf (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, float* A, int const lda)
    {
      return clapack_spotrf (Order, Uplo, N, A, lda);
    }

    inline
    int potrf (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, double* A, int const lda)
    {
      return clapack_dpotrf (Order, Uplo, N, A, lda);
    }

    inline
    int potrf (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_f* A, int const lda)
    {
      return clapack_cpotrf (Order, Uplo, N, static_cast<void*> (A), lda);
    }

    inline
    int potrf (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_d* A, int const lda)
    {
      return clapack_zpotrf (Order, Uplo, N, static_cast<void*> (A), lda);
    }

    // solve (using factorization computed by potrf())
    inline
    int potrs (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, int const NRHS,
               float const* A, int const lda, float* B, int const ldb)
    {
      return clapack_spotrs (Order, Uplo, N, NRHS, A, lda, B, ldb);
    }

    inline
    int potrs (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, int const NRHS,
               double const* A, int const lda, double* B, int const ldb)
    {
      return clapack_dpotrs (Order, Uplo, N, NRHS, A, lda, B, ldb);
    }

    inline
    int potrs (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, int const NRHS,
               traits::complex_f const* A, int const lda,
               traits::complex_f* B, int const ldb)
    {
      return clapack_cpotrs (Order, Uplo, N, NRHS,
                             static_cast<void const*> (A), lda,
                             static_cast<void*> (B), ldb);
    }

    inline
    int potrs (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, int const NRHS,
               traits::complex_d const* A, int const lda,
               traits::complex_d* B, int const ldb)
    {
      return clapack_zpotrs (Order, Uplo, N, NRHS,
                             static_cast<void const*> (A), lda,
                             static_cast<void*> (B), ldb);
    }

#ifdef BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG
    // .. ATLAS bug with row major hermitian matrices
    // .... symmetric matrices are OK, but to simplify generic potrs() ...
    inline
    int potrs_bug (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
                   int const N, int const NRHS,
                   float const* A, int const lda, float* B, int const ldb)
    {
      return clapack_spotrs (Order, Uplo, N, NRHS, A, lda, B, ldb);
    }

    inline
    int potrs_bug (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
                   int const N, int const NRHS,
                   double const* A, int const lda, double* B, int const ldb)
    {
      return clapack_dpotrs (Order, Uplo, N, NRHS, A, lda, B, ldb);
    }

    inline
    int potrs_bug (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
                   int const N, int const NRHS,
                   traits::complex_f const* A, int const lda,
                   traits::complex_f* B, int const ldb)
    {
      int sz = N * lda;
      traits::complex_f* A1 = new traits::complex_f[sz];
      for (int i = 0; i < sz; ++i)
        A1[i] = std::conj (A[i]);
      int r = clapack_cpotrs (Order, Uplo, N, NRHS,
                              static_cast<void const*> (A1), lda,
                              static_cast<void*> (B), ldb);
      delete[] A1;
      return r;
    }

    inline
    int potrs_bug (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
                   int const N, int const NRHS,
                   traits::complex_d const* A, int const lda,
                   traits::complex_d* B, int const ldb)
    {
      int sz = N * lda;
      traits::complex_d* A1 = new traits::complex_d[sz];
      for (int i = 0; i < sz; ++i)
        A1[i] = std::conj (A[i]);
      int r = clapack_zpotrs (Order, Uplo, N, NRHS,
                              static_cast<void const*> (A1), lda,
                              static_cast<void*> (B), ldb);
      delete[] A1;
      return r;
    }
#endif // BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG

    // invert (using factorization computed by potrf())
    inline
    int potri (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, float* A, int const lda)
    {
      return clapack_spotri (Order, Uplo, N, A, lda);
    }

    inline
    int potri (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, double* A, int const lda)
    {
      return clapack_dpotri (Order, Uplo, N, A, lda);
    }

    inline
    int potri (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_f* A, int const lda)
    {
      return clapack_cpotri (Order, Uplo, N, static_cast<void*> (A), lda);
    }

    inline
    int potri (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_d* A, int const lda)
    {
      return clapack_zpotri (Order, Uplo, N, static_cast<void*> (A), lda);
    }

  }} // namepaces detail & atlas

}}}

#endif // BOOST_NUMERIC_BINDINGS_CLAPACK_OVERLOADS_HPP


#include "EvaluatorUtils.h"

#include <NTL/ZZ.h>
#include <cmath>

ZZ EvaluatorUtils::evaluateVal(const double& x, const long& bits) {
	return evaluateVal(to_RR(x), bits);
}

ZZ EvaluatorUtils::evaluateVal(const RR& x, const long& bits) {
	RR xp = MakeRR(x.x, x.e + bits);
	return RoundToZZ(xp);
}

CZZ EvaluatorUtils::evaluateVal(const double& xr, const double& xi, const long& bits) {
	return evaluateVal(to_RR(xr), to_RR(xi), bits);
}

CZZ EvaluatorUtils::evaluateVal(const RR& xr, const RR& xi, const long& bits) {
	RR xrp = MakeRR(xr.x, xr.e + bits);
	RR xip = MakeRR(xi.x, xi.e + bits);
	return CZZ(RoundToZZ(xrp), RoundToZZ(xip));
}

void EvaluatorUtils::evaluateRRVal(RR& xr, RR& xi, CZZ& x, const long& bits) {
	xr = to_RR(x.r);
	xr.e -= bits;
	xi = to_RR(x.i);
	xr.e -= bits;
}

CZZ EvaluatorUtils::evaluateRandomVal(const long& bits) {
	return CZZ(RandomBits_ZZ(bits), RandomBits_ZZ(bits));
}

CZZ EvaluatorUtils::evaluateRandomCircleVal(const long& bits) {
	RR angle = random_RR();
	RR mr = cos(angle * 2 * Pi);
	RR mi = sin(angle * 2 * Pi);
	return evaluateVal(mr, mi, bits);
}

CZZ* EvaluatorUtils::evaluateRandomVals(const long& size, const long& bits) {
	CZZ* res = new CZZ[size];
	for (long i = 0; i < size; i++) {
		res[i] = CZZ(RandomBits_ZZ(bits), RandomBits_ZZ(bits));
	}
	return res;
}

CZZ* EvaluatorUtils::evaluateRandomZZVals(const long& size, const long& bits) {
	CZZ* res = new CZZ[size];
	for (long i = 0; i < size; i++) {
		res[i].r = RandomBits_ZZ(bits);
	}
	return res;
}

CZZ EvaluatorUtils::evaluatePow(const double& xr, const double& xi, const long& degree, const long& bits) {
	long logDegree = log2(degree);
	long po2Degree = 1 << logDegree;
	CZZ res = evaluatePow2(xr, xi, logDegree, bits);
	long remDegree = degree - po2Degree;
	if(remDegree > 0) {
		CZZ tmp = evaluatePow(xr, xi, remDegree, bits);
		res *= tmp;
		res >>= bits;
	}
	return res;
}

CZZ EvaluatorUtils::evaluatePow(const RR& xr, const RR& xi, const long& degree, const long& bits) {
	long logDegree = log2(degree);
	long po2Degree = 1 << logDegree;
	CZZ res = evaluatePow2(xr, xi, logDegree, bits);
	long remDegree = degree - po2Degree;
	if(remDegree > 0) {
		CZZ tmp = evaluatePow(xr, xi, remDegree, bits);
		res *= tmp;
		res >>= bits;
	}
	return res;
}

CZZ EvaluatorUtils::evaluatePow2(const double& xr, const double& xi, const long& logDegree, const long& bits) {
	return evaluatePow2(to_RR(xr), to_RR(xi), logDegree, bits);
}

CZZ EvaluatorUtils::evaluatePow2(const RR& xr, const RR& xi, const long& logDegree, const long& bits) {
	CZZ res = evaluateVal(xr, xi, bits);
	for (int i = 0; i < logDegree; ++i) {
		res *= res;
		res >>= bits;
	}
	return res;
}

CZZ* EvaluatorUtils::evaluatePowvec(const double& xr, const double& xi, const long& degree, const long& bits) {
	return  evaluatePowvec(to_RR(xr), to_RR(xi), degree, bits);
}

CZZ* EvaluatorUtils::evaluatePowvec(const RR& xr, const RR& xi, const long& degree, const long& bits) {
	CZZ* res = new CZZ[degree];
	CZZ m = evaluateVal(xr, xi, bits);
	res[0] = m;
	for (long i = 0; i < degree - 1; ++i) {
		res[i + 1] = (res[i] * m) >> bits;
	}
	return res;
}

CZZ* EvaluatorUtils::evaluatePow2vec(const double& xr, const double& xi, const long& logDegree, const long& bits) {
	return evaluatePow2vec(to_RR(xr), to_RR(xi), logDegree, bits);
}

CZZ* EvaluatorUtils::evaluatePow2vec(const RR& xr, const RR& xi, const long& logDegree, const long& bits) {
	CZZ* res = new CZZ[logDegree + 1];
	CZZ m = evaluateVal(xr, xi, bits);
	res[0] = m;
	for (long i = 0; i < logDegree; ++i) {
		res[i + 1] = (res[i] * res[i]) >> bits;
	}
	return res;
}

CZZ EvaluatorUtils::evaluateInverse(const double& xr, const double& xi, const long& bits) {
	return evaluateInverse(to_RR(xr), to_RR(xi), bits);
}

CZZ EvaluatorUtils::evaluateInverse(const RR& xr, const RR& xi, const long& bits) {
	RR xinvr = xr / (xr * xr + xi * xi);
	RR xinvi = -xi / (xr * xr + xi * xi);

	return evaluateVal(xinvr, xinvi, bits);
}

CZZ EvaluatorUtils::evaluateLogarithm(const double& xr, const double& xi, const long& bits) {
	double xlogr = log(xr * xr + xi * xi) / 2;
	double xlogi = atan(xi / xr);

	return evaluateVal(xlogr, xlogi, bits);
}

CZZ EvaluatorUtils::evaluateExponent(const double& xr, const double& xi, const long& bits) {
	double xrexp = exp(xr);
	double xexpr = xrexp * cos(xi);
	double xexpi = xrexp * sin(xi);

	return evaluateVal(xexpr, xexpi, bits);
}

CZZ EvaluatorUtils::evaluateExponent(const RR& xr, const RR& xi, const long& bits) {
	RR xrexp = exp(xr);
	RR xexpr = xrexp * cos(xi);
	RR xexpi = xrexp * sin(xi);

	return evaluateVal(xexpr, xexpi, bits);
}

CZZ EvaluatorUtils::evaluateSigmoid(const double& xr, const double& xi, const long& bits) {
	double xrexp = exp(xr);
	double xexpr = xrexp * cos(xi);
	double xexpi = xrexp * sin(xi);

	double xsigmoidr = (xexpr * (xexpr + 1) + (xexpi * xexpi)) / ((xexpr + 1) * (xexpr + 1) + (xexpi * xexpi));
	double xsigmoidi = xexpi / ((xexpr + 1) * (xexpr + 1) + (xexpi * xexpi));

	return evaluateVal(xsigmoidr, xsigmoidi, bits);
}

CZZ EvaluatorUtils::evaluateSigmoid(const RR& xr, const RR& xi, const long& bits) {
	RR xrexp = exp(xr);
	RR xexpr = xrexp * cos(xi);
	RR xexpi = xrexp * sin(xi);

	RR xsigmoidr = (xexpr * (xexpr + 1) + (xexpi * xexpi)) / ((xexpr + 1) * (xexpr + 1) + (xexpi * xexpi));
	RR xsigmoidi = xexpi / ((xexpr + 1) * (xexpr + 1) + (xexpi * xexpi));

	return evaluateVal(xsigmoidr, xsigmoidi, bits);
}

void EvaluatorUtils::leftShiftAndEqual(CZZ*& vals, const long& size, const long& bits) {
	for (long i = 0; i < size; ++i) {
		vals[i] <<= bits;
	}
}

void EvaluatorUtils::leftRotateAndEqual(CZZ*& vals, const long& size, const long& rotSize) {
	long remrotSize = rotSize % size;
	if(remrotSize != 0) {
		long divisor = GCD(remrotSize, size);
		long steps = size / divisor;
		for (long i = 0; i < divisor; ++i) {
			CZZ tmp = vals[i];
			long idx = i;
			for (long j = 0; j < steps - 1; ++j) {
				vals[idx] = vals[(idx + remrotSize) % size];
				idx = (idx + remrotSize) % size;
			}
			vals[idx] = tmp;
		}
	}
}

void EvaluatorUtils::rightRotateAndEqual(CZZ*& vals, const long& size, const long& rotSize) {
	long remrotSize = rotSize % size;
	long leftremrotSize = (size - remrotSize) % size;
	leftRotateAndEqual(vals, size, leftremrotSize);
}


#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <iostream>
#include <iomanip>
#include <string>

#include "post_processing/macroscopic_quantities.hpp"
#include "post_processing/mass.hpp"
#include "post_processing/momentum.hpp"
#include "post_processing/energy.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "aux/eigen2hdf.hpp"

using namespace boltzmann;
using namespace std;

TEST(spectral, momentsfile)
{

  std::string filename = "mqeval.h5";
  MQEval mq_coeffs_;
  int K = 40;
  SpectralBasisFactoryKS::basis_type basis;
  SpectralBasisFactoryKS::create(basis, K);
  mq_coeffs_.init(basis);

  Eigen::VectorXd vec(basis.size());

  hid_t fh5 = H5Fopen(filename.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
  eigen2hdf::load(fh5, "coeffs", vec);
  H5Fclose(fh5);

  auto evaluator = mq_coeffs_.evaluator();
  evaluator(vec);

  cout << setw(10) << "rho "
       << setw(20) << setprecision(8) << scientific << evaluator.m << "\n";

  cout << setw(10) << "e "
       << setw(20) << setprecision(8) << scientific << evaluator.e << "\n";

  cout << setw(10) << "v "
       << setw(20) << setprecision(8) << scientific << evaluator.v << "\n";

  cout << setw(10) << "v "
       << setw(20) << setprecision(8) << scientific << evaluator.v << "\n";
}

TEST(spectral, moments)
{

  std::string filename = "mqeval.h5";
  MQEval mq_coeffs;
  int K = 40;
  SpectralBasisFactoryKS::basis_type basis;
  SpectralBasisFactoryKS::create(basis, K);
  mq_coeffs.init(basis);

  auto cmass = mq_coeffs.cmass();
  auto cenergy = mq_coeffs.cenergy();

  Energy energy(basis);
  Mass mass(basis);

  Eigen::ArrayXd cenergy_ref(basis.size());
  cenergy_ref.setZero();
  Eigen::ArrayXd cmass_ref(basis.size());
  cmass_ref.setZero();

  for (auto entry : energy.entries()) {
    cenergy_ref(entry.first) = entry.second;
  }

  for (auto entry : mass.entries()) {
    cmass_ref(entry.first) = entry.second;
  }

  double diff_mass_max = (cmass_ref.segment(0, cmass.size()) - cmass).cwiseAbs().maxCoeff();
  double diff_energy_max = (cenergy_ref.segment(0, cenergy.size()) - cenergy).cwiseAbs().maxCoeff();

  EXPECT_NEAR(diff_mass_max, 0, 1e-11) << "max diff coeffs mass";
  EXPECT_NEAR(diff_energy_max, 0, 1e-11) << "max diff coeffs energy";
}


/*
 * TravellingSalesman.cpp
 *
 *  Created on: 2013/05/13
 *      Author: kryozahiro
 */

#include "TravellingSalesman.h"

#include <random>
#include <boost/lexical_cast.hpp>
using namespace std;

TravellingSalesman::TravellingSalesman(int size, int seed) :
		Problem(ProgramType(DataType(0, 0, 1, 0), DataType(0, size - 1, 1, size))) {
	//都市のランダム生成
	mt19937_64 engine(seed);
	uniform_real_distribution<> dist(-1.0, 1.0);
	for (int i = 0; i < size; ++i) {
		cities.push_back(pair<double, double>(dist(engine), dist(engine)));
	}
}

double TravellingSalesman::evaluate(Program& program) {
	vector<double> path = program(vector<double>());
	assert(getProgramType().getOutputType().accepts(path));
	vector<pair<double, double>> cityList = cities;

	pair<double, double> prev = cityList[path[0]];
	cityList.erase(cityList.begin() + path[0]);

	//経路の長さを計算する
	double length = 0;
	for (vector<int>::size_type i = 1; i < path.size(); ++i) {
		int cityIndex = ((int)path[i]) % cityList.size();
		pair<double, double> next = cityList[cityIndex];
		cityList.erase(cityList.begin() + cityIndex);

		length += (next.first - prev.first) * (next.first - prev.first) + (next.second - prev.second) * (next.second - prev.second);
		prev = next;
	}
	return length;
}

string TravellingSalesman::toString() const {
	string ret;
	for (vector<pair<int, int>>::size_type i = 0; i < cities.size(); ++i) {
		ret += boost::lexical_cast<string>(cities[i].first) + " " + boost::lexical_cast<string>(cities[i].second) + "\n";
	}
	return ret;
}

/*string TravellingSalesman::showProgram(Program& program) const {
	string ret;

	vector<double> path = program(vector<double>());
	assert(getProgramType().acceptsOutput(path));
	vector<pair<double, double>> cityList = cities;

	pair<double, double> prev = cityList[path[0]];
	cityList.erase(cityList.begin() + path[0]);
	ret += boost::lexical_cast<string>(prev.first) + " " + boost::lexical_cast<string>(prev.second) + "\n";

	//経路の長さを計算する
	for (vector<int>::size_type i = 1; i < path.size(); ++i) {
		int cityIndex = ((int)path[i]) % cityList.size();
		pair<double, double> next = cityList[cityIndex];
		cityList.erase(cityList.begin() + cityIndex);

		ret += boost::lexical_cast<string>(next.first) + " " + boost::lexical_cast<string>(next.second) + "\n";
	}
	ret += boost::lexical_cast<string>(prev.first) + " " + boost::lexical_cast<string>(prev.second) + "\n";
	return ret;
}*/


/**
 * \file SecondOrderFilter.cpp
 * @see http://abvolt.com/research/publications2.htm
 * @see http://www.music.mcgill.ca/~ich/classes/FiltersChap2.pdf for the allpass filter
 */

#include "SecondOrderFilter.h"
#include "IIRFilter.h"

#include <cassert>
#include <cmath>

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

namespace ATK
{
  template <typename DataType>
  SecondOrderBaseCoefficients<DataType>::SecondOrderBaseCoefficients(int nb_channels)
    :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType>
  void SecondOrderBaseCoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);
  }

  template <typename DataType_>
  void SecondOrderBaseCoefficients<DataType_>::set_cut_frequency(DataType_ cut_frequency)
  {
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  DataType_ SecondOrderBaseCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template<typename DataType>
  SecondOrderBandPassCoefficients<DataType>::SecondOrderBandPassCoefficients(int nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void SecondOrderBandPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);
    DataType Q_inv = 1 / Q;

    coefficients_in[2] = Q_inv * c / d;
    coefficients_in[1] = 0;
    coefficients_in[0] = -Q_inv * c / d;
    coefficients_out[1] = - 2 * (c * c - 1) / d;
    coefficients_out[0] = - (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d;
  }

  template <typename DataType_>
  void SecondOrderBandPassCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ SecondOrderBandPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  SecondOrderLowPassCoefficients<DataType>::SecondOrderLowPassCoefficients(int nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void SecondOrderLowPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

    coefficients_in[2] = c * c / d;
    coefficients_in[1] = 2 * c * c / d;
    coefficients_in[0] = c * c / d;
    coefficients_out[1] = - 2 * (c * c - 1) / d;
    coefficients_out[0] = - (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d;
  }

  template<typename DataType>
  SecondOrderHighPassCoefficients<DataType>::SecondOrderHighPassCoefficients(int nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void SecondOrderHighPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

    coefficients_in[2] = 1;
    coefficients_in[1] = -2;
    coefficients_in[0] = 1;
    coefficients_out[1] = - 2 * (c * c - 1) / d;
    coefficients_out[0] = - (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d;
  }

  template<typename DataType>
  SecondOrderBandPassPeakCoefficients<DataType>::SecondOrderBandPassPeakCoefficients(int nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void SecondOrderBandPassPeakCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    DataType Q_inv = 1 / Q;
    if(gain <= 1)
    {
      DataType V0 = 1 / gain;
      DataType d = 1 + V0 * Q_inv * c + c * c;

      coefficients_in[2] = (1 + Q_inv * c + c * c) / d;
      coefficients_in[1] = 2 * (c * c - 1) / d;
      coefficients_in[0] = (1 - Q_inv * c + c * c) / d;
      coefficients_out[1] = -2 * (c * c - 1) / d;
      coefficients_out[0] = -(1 - V0 * Q_inv * c + c * c) / d;
    }
    else
    {
      DataType V0 = gain;
      DataType d = 1 + Q_inv * c + c * c;

      coefficients_in[2] = (1 + V0 * Q_inv * c + c * c) / d;
      coefficients_in[1] = 2 * (c * c - 1) / d;
      coefficients_in[0] = (1 - V0 * Q_inv * c + c * c) / d;
      coefficients_out[1] = -2 * (c * c - 1) / d;
      coefficients_out[0] = -(1 - Q_inv * c + c * c) / d;
    }
  }

  template <typename DataType_>
  void SecondOrderBandPassPeakCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ SecondOrderBandPassPeakCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void SecondOrderBandPassPeakCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ SecondOrderBandPassPeakCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  SecondOrderAllPassCoefficients<DataType>::SecondOrderAllPassCoefficients(int nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void SecondOrderAllPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType c = std::tan(boost::math::constants::pi<DataType>() * Q);
    DataType d = -std::cos(2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);

    coefficients_in[2] = -c;
    coefficients_in[1] = d * (1 - c);
    coefficients_in[0] = 1;
    coefficients_out[1] = -d * (1 - c);
    coefficients_out[0] = c;
  }

  template <typename DataType_>
  void SecondOrderAllPassCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ SecondOrderAllPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  SecondOrderLowShelvingCoefficients<DataType>::SecondOrderLowShelvingCoefficients(int nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void SecondOrderLowShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    if(gain <= 1)
    {
      DataType V0 = 1 / gain;
      DataType d = (1 + std::sqrt(static_cast<DataType>(2.) * V0) * c + V0 * c * c);

      coefficients_in[2] = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d;
      coefficients_in[1] = 2 * (c * c - 1) / d;
      coefficients_in[0] = (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d;
      coefficients_out[1] = - 2 * (V0 * c * c - 1) / d;
      coefficients_out[0] = - (1 - std::sqrt(static_cast<DataType>(2.) * V0) * c + V0 * c * c) / d;
    }
    else
    {
      DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

      coefficients_in[2] = (1 + std::sqrt(static_cast<DataType>(2.) * gain) * c + gain * c * c) / d;
      coefficients_in[1] = 2 * (gain * c * c - 1) / d;
      coefficients_in[0] = (1 - std::sqrt(static_cast<DataType>(2.) * gain) * c + gain * c * c) / d;
      coefficients_out[1] = - 2 * (c * c - 1) / d;
      coefficients_out[0] = - (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d;
    }
  }

  template <typename DataType_>
  void SecondOrderLowShelvingCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ SecondOrderLowShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  SecondOrderHighShelvingCoefficients<DataType>::SecondOrderHighShelvingCoefficients(int nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void SecondOrderHighShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    if(gain <= 1)
    {
      DataType V0 = 1 / gain;
      DataType d = (V0 + std::sqrt(static_cast<DataType>(2.) * V0) * c + c * c);

      coefficients_in[2] = -(1 + std::sqrt(static_cast<DataType>(2.0)) * c + c * c) / d;
      coefficients_in[1] = -2 * (c * c - 1) / d;
      coefficients_in[0] = -(1 - std::sqrt(static_cast<DataType>(2.0)) * c + c * c) / d;
      coefficients_out[1] = - 2 * (c * c - V0) / d;
      coefficients_out[0] = - (V0 - std::sqrt(static_cast<DataType>(2.0) * V0) * c + c * c) / d;
    }
    else
    {
      DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

      coefficients_in[2] = -(gain + std::sqrt(static_cast<DataType>(2.0) * gain) * c + c * c) / d;
      coefficients_in[1] = -2 * (c * c - gain) / d;
      coefficients_in[0] = -(gain - std::sqrt(static_cast<DataType>(2.0) * gain) * c + c * c) / d;
      coefficients_out[1] = - 2 * (c * c - 1) / d;
      coefficients_out[0] = - (1 - std::sqrt(static_cast<DataType>(2.0)) * c + c * c) / d;
    }
  }

  template<typename DataType_>
  void SecondOrderHighShelvingCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ SecondOrderHighShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template class SecondOrderBaseCoefficients<float>;
  template class SecondOrderBaseCoefficients<double>;

  template class SecondOrderBandPassCoefficients<float>;
  template class SecondOrderBandPassCoefficients<double>;
  template class SecondOrderLowPassCoefficients<float>;
  template class SecondOrderLowPassCoefficients<double>;
  template class SecondOrderHighPassCoefficients<float>;
  template class SecondOrderHighPassCoefficients<double>;
  template class SecondOrderBandPassPeakCoefficients<float>;
  template class SecondOrderBandPassPeakCoefficients<double>;
  template class SecondOrderAllPassCoefficients<float>;
  template class SecondOrderAllPassCoefficients<double>;
  template class SecondOrderLowShelvingCoefficients<float>;
  template class SecondOrderLowShelvingCoefficients<double>;
  template class SecondOrderHighShelvingCoefficients<float>;
  template class SecondOrderHighShelvingCoefficients<double>;

  template class IIRFilter<SecondOrderBandPassCoefficients<float> >;
  template class IIRFilter<SecondOrderBandPassCoefficients<double> >;
  template class IIRFilter<SecondOrderLowPassCoefficients<float> >;
  template class IIRFilter<SecondOrderLowPassCoefficients<double> >;
  template class IIRFilter<SecondOrderHighPassCoefficients<float> >;
  template class IIRFilter<SecondOrderHighPassCoefficients<double> >;
  template class IIRFilter<SecondOrderBandPassPeakCoefficients<float> >;
  template class IIRFilter<SecondOrderBandPassPeakCoefficients<double> >;
  template class IIRFilter<SecondOrderAllPassCoefficients<float> >;
  template class IIRFilter<SecondOrderAllPassCoefficients<double> >;
  template class IIRFilter<SecondOrderLowShelvingCoefficients<float> >;
  template class IIRFilter<SecondOrderLowShelvingCoefficients<double> >;
  template class IIRFilter<SecondOrderHighShelvingCoefficients<float> >;
  template class IIRFilter<SecondOrderHighShelvingCoefficients<double> >;
}


#include "basis/Functions.h"
#include "form/RefElement.h"
#include "quadrules/AutoRule.h"
#include "quadrules/SimplexQuadratureRule.h"
#include "tensor/EigenMap.h"
#include "tensor/Managed.h"
#include "util/Combinatorics.h"

#include <Eigen/Core>

#include <array>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <memory>
#include <vector>

using Eigen::DiagonalMatrix;
using Eigen::MatrixXd;
using tndm::AllIntegerSums;
using tndm::binom;
using tndm::gradTetraDubinerP;
using tndm::ModalRefElement;
using tndm::TetraDubinerP;

int main(int argc, char** argv) {

    if (argc < 2) {
        std::cerr << "Usage: `tensors <degree>`" << std::endl;
        return -1;
    }

    unsigned N = atoi(argv[1]);
    unsigned numBF = binom(N + 3, 3);

    std::cout << "Number of basis functions: " << numBF << std::endl;

    auto rule = tndm::simplexQuadratureRule<3u>(2u * N + 1u);

    auto truncate = [](double x) { return (std::fabs(x) < 1e-15) ? 0.0 : x; };

    DiagonalMatrix<double, Eigen::Dynamic> W(rule.size());

    auto phi = ModalRefElement<3u>(N).evaluateBasisAt(rule.points());
    auto phiMap = EigenMap(phi);

    for (std::size_t i = 0; i < rule.size(); ++i) {
        W.diagonal()(i) = rule.weights()[i];
    }

    auto m = phiMap * W * phiMap.transpose();
    std::cout << "Mass matrix:" << std::endl;
    std::cout << m.unaryExpr(truncate) << std::endl;

    std::array<MatrixXd, 3> dphi;
    for (auto& matrix : dphi) {
        matrix.resize(rule.size(), numBF);
    }
    std::size_t bf = 0;
    for (auto j : AllIntegerSums<3>(N)) {
        for (std::size_t i = 0; i < rule.size(); ++i) {
            auto grad = gradTetraDubinerP(j, rule.points()[i]);
            for (std::size_t d = 0; d < grad.size(); ++d) {
                dphi[d](i, bf) = grad[d];
            }
        }
        ++bf;
    }

    auto kXi = dphi[0].transpose() * W * phiMap.transpose();
    std::cout << "dphidxi * phi:" << std::endl;
    std::cout << kXi.unaryExpr(truncate) << std::endl;

    for (std::size_t x = 0; x < 3; ++x) {
        for (std::size_t y = 0; y < 3; ++y) {
            auto kxy = dphi[x].transpose() * W * dphi[y];
            std::cout << "dphi[" << x << "] * dphi[" << y << "]:" << std::endl;
            std::cout << kxy.unaryExpr(truncate) << std::endl;
        }
    }

    return 0;
}


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <cmath>
#include <iostream>

#include <Eigen/LU>

#include "Tudat/Basics/utilities.h"
#include "Tudat/Mathematics/BasicMathematics/leastSquaresEstimation.h"

namespace tudat
{

namespace linear_algebra
{

//! Function to get condition number of matrix (using SVD decomposition)
double getConditionNumberOfInformationMatrix( const Eigen::MatrixXd informationMatrix )
{
    return getConditionNumberOfDecomposedMatrix(
                ( informationMatrix.jacobiSvd( Eigen::ComputeThinU | Eigen::ComputeFullV ) ) );
}

//! Function to get condition number of matrix from SVD decomposition
double getConditionNumberOfDecomposedMatrix( const Eigen::JacobiSVD< Eigen::MatrixXd >& singularValueDecomposition )
{
    Eigen::VectorXd singularValues = singularValueDecomposition.singularValues( );
    return singularValues( 0 ) / singularValues( singularValues.rows( ) - 1 );
}

//! Solve system of equations with SVD decomposition, checking condition number in the process
Eigen::VectorXd solveSystemOfEquationsWithSvd( const Eigen::MatrixXd matrixToInvert,
                                               const Eigen::VectorXd rightHandSideVector,
                                               const bool checkConditionNumber,
                                               const double maximumAllowedConditionNumber )
{
    Eigen::JacobiSVD< Eigen::MatrixXd > svdDecomposition = matrixToInvert.jacobiSvd(
                Eigen::ComputeThinU | Eigen::ComputeThinV );
    if( checkConditionNumber )
    {
        double conditionNumber = getConditionNumberOfDecomposedMatrix( svdDecomposition );

        if( conditionNumber > maximumAllowedConditionNumber )
        {
            std::cerr << "Warning when performing least squares, condition number is " << conditionNumber << std::endl;
        }
    }
    return svdDecomposition.solve( rightHandSideVector );
}

//! Function to multiply information matrix by diagonal weights matrix
Eigen::MatrixXd multiplyInformationMatrixByDiagonalWeightMatrix(
        const Eigen::MatrixXd& informationMatrix,
        const Eigen::VectorXd& diagonalOfWeightMatrix )
{
    Eigen::MatrixXd weightedInformationMatrix = Eigen::MatrixXd::Zero( informationMatrix.rows( ), informationMatrix.cols( ) );

    for( int i = 0; i < informationMatrix.cols( ); i++ )
    {
        weightedInformationMatrix.block( 0, i, informationMatrix.rows( ), 1 ) =
                informationMatrix.block( 0, i, informationMatrix.rows( ), 1 ).cwiseProduct( diagonalOfWeightMatrix );
    }

    return weightedInformationMatrix;
}

//! Function to compute inverse of covariance matrix at current iteration, including influence of a priori information
Eigen::MatrixXd calculateInverseOfUpdatedCovarianceMatrix(
        const Eigen::MatrixXd& informationMatrix,
        const Eigen::VectorXd& diagonalOfWeightMatrix,
        const Eigen::MatrixXd& inverseOfAPrioriCovarianceMatrix )
{
    return inverseOfAPrioriCovarianceMatrix + informationMatrix.transpose( ) * multiplyInformationMatrixByDiagonalWeightMatrix(
                informationMatrix, diagonalOfWeightMatrix );
}

//! Function to compute inverse of covariance matrix at current iteration
Eigen::MatrixXd calculateInverseOfUpdatedCovarianceMatrix(
        const Eigen::MatrixXd& informationMatrix,
        const Eigen::VectorXd& diagonalOfWeightMatrix )
{
    return calculateInverseOfUpdatedCovarianceMatrix(
                informationMatrix, diagonalOfWeightMatrix,
                Eigen::MatrixXd::Zero( informationMatrix.cols( ), informationMatrix.cols( ) ) );
}

//! Function to perform an iteration least squares estimation from information matrix, weights and residuals and a priori
//! information
std::pair< Eigen::VectorXd, Eigen::MatrixXd > performLeastSquaresAdjustmentFromInformationMatrix(
        const Eigen::MatrixXd& informationMatrix,
        const Eigen::VectorXd& observationResiduals,
        const Eigen::VectorXd& diagonalOfWeightMatrix,
        const Eigen::MatrixXd& inverseOfAPrioriCovarianceMatrix,
        const bool checkConditionNumber,
        const double maximumAllowedConditionNumber,
        const Eigen::MatrixXd& constraintMultiplier,
        const Eigen::VectorXd& constraintRightHandside )
{
//    std::cout<<"Residuals "<<observationResiduals.transpose( )<<std::endl;
//    std::cout<<"Weight diag. "<<diagonalOfWeightMatrix.transpose( )<<std::endl;
//    std::cout<<"Partials "<<informationMatrix.transpose( )<<std::endl;

    Eigen::VectorXd rightHandSide = informationMatrix.transpose( ) *
            ( diagonalOfWeightMatrix.cwiseProduct( observationResiduals ) );
    Eigen::MatrixXd inverseOfCovarianceMatrix = calculateInverseOfUpdatedCovarianceMatrix(
                informationMatrix, diagonalOfWeightMatrix, inverseOfAPrioriCovarianceMatrix );

    // Add constraints to inverse covariance matrix if required
    if( constraintMultiplier.rows( ) != 0 )
    {
        if( constraintMultiplier.rows( ) != constraintRightHandside.rows( ) )
        {
            throw std::runtime_error( "Error when performing constrained least-squares, constraints are incompatible" );
        }

        if( constraintMultiplier.cols( ) != informationMatrix.cols( ) )
        {
            throw std::runtime_error( "Error when performing constrained least-squares, constraints are incompatible with partials" );
        }

        int numberOfConstraints = constraintMultiplier.rows( );
        int numberOfParameters = constraintMultiplier.cols( );

        inverseOfCovarianceMatrix.conservativeResize(
                    numberOfParameters + numberOfConstraints, numberOfParameters + numberOfConstraints );
        inverseOfCovarianceMatrix.block( numberOfParameters, 0, numberOfConstraints, numberOfParameters ) =
               constraintMultiplier;
        inverseOfCovarianceMatrix.block( 0, numberOfParameters, numberOfParameters, numberOfConstraints ) =
               constraintMultiplier.transpose( );
        inverseOfCovarianceMatrix.block(
                    numberOfParameters, numberOfParameters, numberOfConstraints, numberOfConstraints ).setZero( );

        rightHandSide.conservativeResize( numberOfParameters + numberOfConstraints );
        rightHandSide.segment( numberOfParameters, numberOfConstraints ) = constraintRightHandside;
    }

//    std::cout<<"RHS "<<rightHandSide.transpose( )<<std::endl;
//    std::cout<<"Inv cov "<<inverseOfCovarianceMatrix<<std::endl;

    return std::make_pair( solveSystemOfEquationsWithSvd(
                               inverseOfCovarianceMatrix, rightHandSide, checkConditionNumber, maximumAllowedConditionNumber ),
                           inverseOfCovarianceMatrix );

}

//! Function to perform an iteration least squares estimation from information matrix, weights and residuals
std::pair< Eigen::VectorXd, Eigen::MatrixXd > performLeastSquaresAdjustmentFromInformationMatrix(
        const Eigen::MatrixXd& informationMatrix,
        const Eigen::VectorXd& observationResiduals,
        const Eigen::VectorXd& diagonalOfWeightMatrix,
        const bool checkConditionNumber,
        const double maximumAllowedConditionNumber )
{
    return performLeastSquaresAdjustmentFromInformationMatrix(
                informationMatrix, observationResiduals, diagonalOfWeightMatrix,
                Eigen::MatrixXd::Zero( informationMatrix.cols( ), informationMatrix.cols( ) ),
                checkConditionNumber, maximumAllowedConditionNumber );
}

//! Function to perform an iteration of least squares estimation from information matrix and residuals
std::pair< Eigen::VectorXd, Eigen::MatrixXd > performLeastSquaresAdjustmentFromInformationMatrix(
        const Eigen::MatrixXd& informationMatrix,
        const Eigen::VectorXd& observationResiduals,
        const bool checkConditionNumber,
        const double maximumAllowedConditionNumber )
{
    return performLeastSquaresAdjustmentFromInformationMatrix(
                informationMatrix, observationResiduals, Eigen::VectorXd::Constant( observationResiduals.size( ), 1, 1.0 ),
                checkConditionNumber, maximumAllowedConditionNumber );
}

//! Function to fit a univariate polynomial through a set of data
Eigen::VectorXd getLeastSquaresPolynomialFit(
        const Eigen::VectorXd& independentValues,
        const Eigen::VectorXd& dependentValues,
        const std::vector< double >& polynomialPowers )
{
    if( independentValues.rows( ) != dependentValues.rows( ) )
    {
        throw std::runtime_error( "Error when doing least squares polynomial fit, size of dependent and independent "
                                  "variable vectors is not equal." );
    }

    Eigen::MatrixXd informationMatrix = Eigen::MatrixXd::Zero( dependentValues.rows( ), polynomialPowers.size( ) );

    // Compute information matrix
    for( int i = 0; i < independentValues.rows( ); i++ )
    {
        for( unsigned int j = 0; j < polynomialPowers.size( ); j++ )
        {
            informationMatrix( i, j ) = std::pow( independentValues( i ), polynomialPowers.at( j ) );
        }
    }

    return performLeastSquaresAdjustmentFromInformationMatrix( informationMatrix, dependentValues ).first;
}

//! Function to fit a univariate polynomial through a set of data
std::vector< double > getLeastSquaresPolynomialFit(
        const std::map< double, double >& independentDependentValueMap,
        const std::vector< double >& polynomialPowers )
{
    return utilities::convertEigenVectorToStlVector(
                getLeastSquaresPolynomialFit(
                    utilities::convertStlVectorToEigenVector(
                        utilities::createVectorFromMapKeys( independentDependentValueMap ) ),
                    utilities::convertStlVectorToEigenVector(
                        utilities::createVectorFromMapValues( independentDependentValueMap ) ), polynomialPowers ) );

}

//! Function to perform a non-linear least squares estimation with the Levenberg-Marquardt method.
Eigen::VectorXd nonLinearLeastSquaresFit(
        const std::function< std::pair< Eigen::VectorXd, Eigen::MatrixXd >( const Eigen::VectorXd& ) >& observationAndJacobianFunctions,
        const Eigen::VectorXd& initialEstimate, const Eigen::VectorXd& actualObservations, const double initialScaling,
        const double convergenceTolerance, const unsigned int maximumNumberOfIterations )
{
    // Set current estimate to initial value
    Eigen::VectorXd currentEstimate = initialEstimate;

    // Initialize variables
    std::pair< Eigen::VectorXd, Eigen::MatrixXd > pairOfEstimatedObservationsAndDesignMatrix;
    Eigen::MatrixXd designMatrix;
    Eigen::VectorXd offsetInObservations;
    Eigen::VectorXd updateInEstimate;

    // Initial parameters for Levenberg–Marquardt method
    double levenbergMarquardtDampingParameter = 0.0;
    double scalingParameterUpdate = 2.0;
    double levenbergMarquardtGainRatio = 0.0;

    // Start iterative loop
    unsigned int iteration = 0;
    do
    {
        // Compute current system and jacobian functions
        pairOfEstimatedObservationsAndDesignMatrix = observationAndJacobianFunctions( currentEstimate );
        designMatrix = pairOfEstimatedObservationsAndDesignMatrix.second;

        // Offset in observation
        offsetInObservations = actualObservations - pairOfEstimatedObservationsAndDesignMatrix.first;

        // Compute damping parameter for first iteration
        if ( iteration == 0 )
        {
            levenbergMarquardtDampingParameter = initialScaling * ( designMatrix.transpose( ) * designMatrix ).diagonal( ).maxCoeff( );
        }

        // Compute update in estimate
        Eigen::VectorXd diagonalOfWeightMatrix = Eigen::VectorXd::Ones( offsetInObservations.rows( ) );
        Eigen::MatrixXd inverseOfAPrioriCovarianceMatrix = levenbergMarquardtDampingParameter *
//                Eigen::MatrixXd( ( designMatrix.transpose( ) * designMatrix ).diagonal( ).asDiagonal( ) ); // Marquardt’s update
                Eigen::MatrixXd::Identity( currentEstimate.rows( ), currentEstimate.rows( ) );
        updateInEstimate = linear_algebra::performLeastSquaresAdjustmentFromInformationMatrix(
                    designMatrix, offsetInObservations, diagonalOfWeightMatrix, inverseOfAPrioriCovarianceMatrix, false ).first;

        // Check that update is real
        if ( ( !updateInEstimate.allFinite( ) ) || ( updateInEstimate.hasNaN( ) ) )
        {
            throw std::runtime_error( "Error in non-linear least squares estimation. Iterative process diverges." );
        }

        // Compute gain ratio
        levenbergMarquardtGainRatio =
                ( offsetInObservations.squaredNorm( ) -
                  ( actualObservations - observationAndJacobianFunctions( currentEstimate + updateInEstimate ).first ).squaredNorm( ) ) /
                ( updateInEstimate.transpose( ) * ( levenbergMarquardtDampingParameter * updateInEstimate +
                                                    designMatrix.transpose( ) * offsetInObservations ) );

        // Update damping parameter
        if ( levenbergMarquardtGainRatio > 0 )
        {
            // Reduce damping parameter, since good approximation
            levenbergMarquardtDampingParameter *= std::max( 1.0 / 3.0, 1.0 - std::pow( 2.0 * levenbergMarquardtGainRatio - 1.0, 3 ) );
            scalingParameterUpdate = 2; // reset

            // Correct estimate
            currentEstimate += updateInEstimate;
        }
        else
        {
            // Increase damping parameter, since bad approximation and reject step
            levenbergMarquardtDampingParameter *= scalingParameterUpdate;
            scalingParameterUpdate *= 2.0;
        }

        // Increase iteration counter
        iteration++;
    }
    while ( ( updateInEstimate.norm( ) > convergenceTolerance ) && ( iteration <= maximumNumberOfIterations ) );

    // Warn user of exceeded maximum number of iterations
    if ( iteration > maximumNumberOfIterations )
    {
        std::cerr << "Warning in non-linear least squares estimation. Maximum number of iterations exceeded." << std::endl;
    }

    // Give out new estimate in parameters
    return currentEstimate;
}

} // namespace linear_algebra

} // namespace tudat


///////////////////////////////////////////////////////////////////////////////
// rolling_variance.hpp
// Copyright (C) 2005 Eric Niebler
// Copyright (C) 2014 Pieter Bastiaan Ober (Integricom).
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_ROLLING_VARIANCE_HPP_EAN_15_11_2011
#define BOOST_ACCUMULATORS_STATISTICS_ROLLING_VARIANCE_HPP_EAN_15_11_2011

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

#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <boost/accumulators/statistics/rolling_moment.hpp>

#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/utility/enable_if.hpp>

namespace boost { namespace accumulators
{
namespace impl
{
    //! Immediate (lazy) calculation of the rolling variance.
    /*!
    Calculation of sample variance \f$\sigma_n^2\f$ is done as follows, see also
    http://en.wikipedia.org/wiki/Algorithms_for_calculating_variance.
    For a rolling window of size \f$N\f$, when \f$n <= N\f$, the variance is computed according to the formula
    \f[
    \sigma_n^2 = \frac{1}{n-1} \sum_{i = 1}^n (x_i - \mu_n)^2.
    \f]
    When \f$n > N\f$, the sample variance over the window becomes:
    \f[
    \sigma_n^2 = \frac{1}{N-1} \sum_{i = n-N+1}^n (x_i - \mu_n)^2.
    \f]
    */
    ///////////////////////////////////////////////////////////////////////////////
    // lazy_rolling_variance_impl
    //
    template<typename Sample>
    struct lazy_rolling_variance_impl
        : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::fdiv<Sample, std::size_t,void,void>::result_type result_type;

        lazy_rolling_variance_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            result_type mean = rolling_mean(args);
            size_t nr_samples = rolling_count(args);
            if (nr_samples < 2) return result_type();
            return nr_samples*(rolling_moment<2>(args) - mean*mean)/(nr_samples-1);
        }
    };

    //! Iterative calculation of the rolling variance.
    /*!
    Iterative calculation of sample variance \f$\sigma_n^2\f$ is done as follows, see also
    http://en.wikipedia.org/wiki/Algorithms_for_calculating_variance.
    For a rolling window of size \f$N\f$, for the first \f$N\f$ samples, the variance is computed according to the formula
    \f[
    \sigma_n^2 = \frac{1}{n-1} \sum_{i = 1}^n (x_i - \mu_n)^2 = \frac{1}{n-1}M_{2,n},
    \f]
    where the sum of squares \f$M_{2,n}\f$ can be recursively computed as:
    \f[
    M_{2,n} = \sum_{i = 1}^n (x_i - \mu_n)^2 = M_{2,n-1} + (x_n - \mu_n)(x_n - \mu_{n-1}),
    \f]
    and the estimate of the sample mean as:
    \f[
    \mu_n = \frac{1}{n} \sum_{i = 1}^n x_i = \mu_{n-1} + \frac{1}{n}(x_n - \mu_{n-1}).
    \f]
    For further samples, when the rolling window is fully filled with data, one has to take into account that the oldest
    sample \f$x_{n-N}\f$ is dropped from the window. The sample variance over the window now becomes:
    \f[
    \sigma_n^2 = \frac{1}{N-1} \sum_{i = n-N+1}^n (x_i - \mu_n)^2 = \frac{1}{n-1}M_{2,n},
    \f]
    where the sum of squares \f$M_{2,n}\f$ now equals:
    \f[
    M_{2,n} = \sum_{i = n-N+1}^n (x_i - \mu_n)^2 = M_{2,n-1} + (x_n - \mu_n)(x_n - \mu_{n-1}) - (x_{n-N} - \mu_n)(x_{n-N} - \mu_{n-1}),
    \f]
    and the estimated mean is:
    \f[
    \mu_n = \frac{1}{N} \sum_{i = n-N+1}^n x_i = \mu_{n-1} + \frac{1}{n}(x_n - x_{n-N}).
    \f]

    Note that the sample variance is not defined for \f$n <= 1\f$.

    */
    ///////////////////////////////////////////////////////////////////////////////
    // immediate_rolling_variance_impl
    //
    template<typename Sample>
    struct immediate_rolling_variance_impl
        : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type result_type;

        template<typename Args>
        immediate_rolling_variance_impl(Args const &args)
            : previous_mean_(numeric::fdiv(args[sample | Sample()], numeric::one<std::size_t>::value))
            , sum_of_squares_(numeric::fdiv(args[sample | Sample()], numeric::one<std::size_t>::value))
        {
        }

        template<typename Args>
        void operator()(Args const &args)
        {
            Sample added_sample = args[sample];

            result_type mean = immediate_rolling_mean(args);
            sum_of_squares_ += (added_sample-mean)*(added_sample-previous_mean_);

            if(is_rolling_window_plus1_full(args))
            {
                Sample removed_sample = rolling_window_plus1(args).front();
                sum_of_squares_ -= (removed_sample-mean)*(removed_sample-previous_mean_);
                prevent_underflow(sum_of_squares_);
            }
            previous_mean_ = mean;
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            size_t nr_samples = rolling_count(args);
            if (nr_samples < 2) return result_type();
            return numeric::fdiv(sum_of_squares_,(nr_samples-1));
        }

    private:

        result_type previous_mean_;
        result_type sum_of_squares_;

        template<typename T>
        void prevent_underflow(T &non_negative_number,typename boost::enable_if<boost::is_arithmetic<T>,T>::type* = 0)
        {
            if (non_negative_number < T(0)) non_negative_number = T(0);
        }
        template<typename T>
        void prevent_underflow(T &non_arithmetic_quantity,typename boost::disable_if<boost::is_arithmetic<T>,T>::type* = 0)
        {
        }
    };
} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag:: lazy_rolling_variance
// tag:: immediate_rolling_variance
// tag:: rolling_variance
//
namespace tag
{
    struct lazy_rolling_variance
        : depends_on< rolling_count, rolling_mean, rolling_moment<2> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::lazy_rolling_variance_impl< mpl::_1 > impl;

        #ifdef BOOST_ACCUMULATORS_DOXYGEN_INVOKED
        /// tag::rolling_window::window_size named parameter
        static boost::parameter::keyword<tag::rolling_window_size> const window_size;
        #endif
    };

    struct immediate_rolling_variance
        : depends_on< rolling_window_plus1, rolling_count, immediate_rolling_mean>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::immediate_rolling_variance_impl< mpl::_1> impl;

        #ifdef BOOST_ACCUMULATORS_DOXYGEN_INVOKED
        /// tag::rolling_window::window_size named parameter
        static boost::parameter::keyword<tag::rolling_window_size> const window_size;
        #endif
    };

    // make immediate_rolling_variance the default implementation
    struct rolling_variance : immediate_rolling_variance {};
} // namespace tag

///////////////////////////////////////////////////////////////////////////////
// extract::lazy_rolling_variance
// extract::immediate_rolling_variance
// extract::rolling_variance
//
namespace extract
{
    extractor<tag::lazy_rolling_variance> const lazy_rolling_variance = {};
    extractor<tag::immediate_rolling_variance> const immediate_rolling_variance = {};
    extractor<tag::rolling_variance> const rolling_variance = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(lazy_rolling_variance)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(immediate_rolling_variance)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(rolling_variance)
}

using extract::lazy_rolling_variance;
using extract::immediate_rolling_variance;
using extract::rolling_variance;

// rolling_variance(lazy) -> lazy_rolling_variance
template<>
struct as_feature<tag::rolling_variance(lazy)>
{
    typedef tag::lazy_rolling_variance type;
};

// rolling_variance(immediate) -> immediate_rolling_variance
template<>
struct as_feature<tag::rolling_variance(immediate)>
{
    typedef tag::immediate_rolling_variance type;
};

// for the purposes of feature-based dependency resolution,
// lazy_rolling_variance provides the same feature as rolling_variance
template<>
struct feature_of<tag::lazy_rolling_variance>
    : feature_of<tag::rolling_variance>
{
};

// for the purposes of feature-based dependency resolution,
// immediate_rolling_variance provides the same feature as rolling_variance
template<>
struct feature_of<tag::immediate_rolling_variance>
  : feature_of<tag::rolling_variance>
{
};
}} // namespace boost::accumulators

#endif


/**
 * dpmeans.hpp
 * @author koide
 * 16/06/07
 **/
#ifndef KKL_DPMEANS_HPP
#define KKL_DPMEANS_HPP

#include <vector>
#include <Eigen/Dense>
#include <Eigen/StdVector>

namespace kkl {
  namespace alg {

/**
 * @brief DP-means
 * @ref http://people.eecs.berkeley.edu/~jordan/papers/kulis-jordan-icml12.pdf
 */
template<typename T, int dim>
class DPmeans {
  typedef Eigen::Matrix<T, dim, 1> VectorTd;

public:
  // constructor, destructor
  DPmeans() {}
  ~DPmeans() {}

  /**
   * @brief train
   * @param x           input data
   * @param lambda      cluster penalty prameter
   * @param criteria    convergence criteria
   * @param loop_limit  maximum number of iterations
   * @return
   */
  bool train(const std::vector<VectorTd>& x, T lambda, T criteria = 0.01, int loop_limit = 128) {
    labels.assign(x.size(), 0);
    centroids.clear();
    centroids.push_back(VectorTd::Zero());

    for (int i = 0; i < loop_limit; i++) {
      updateCentroids(x, criteria);
      if (updateLabels(x, lambda) ) {
        return true;
      }
    }

    return false;
  }

private:
  // update centroids acoording to labels
  bool updateCentroids(const std::vector<VectorTd>& x, T criteria) {
    int k = centroids.size();
    std::vector<int> accums(k, 0);
    std::vector<VectorTd> new_centroids(k);
    std::for_each(new_centroids.begin(), new_centroids.end(), [=](VectorTd& centroid) { centroid.setZero(); });

    // update centrods
    for (int i = 0; i < x.size(); i++) {
      accums[labels[i]]++;
      new_centroids[labels[i]] += x[i];
    }
    for (int i = 0; i < k; i++) {
      new_centroids[i] /= std::max( 1, accums[i] );
    }

    centroids.swap(new_centroids);

    // check if converged
    for (int i = 0; i < centroids.size(); i++) {
      if ((centroids[i] - new_centroids[i]).squaredNorm() > criteria * criteria) {
        return false;
      }
    }
    return true;
  }

  // update labels acoording to estimated centroids
  // if the distance between a point and the closest centroid is lager than lambda, the point is added as a new centroid
  bool updateLabels(const std::vector<VectorTd>& x, T lambda) {
    bool is_converged = true;
    bool k_incremented = false;
    // for each point
    for (int i = 0; i < x.size(); i++) {
      T min_d = (centroids[0] - x[i]).squaredNorm();
      int min_label = 0;

      // find the closest centroid
      for (int j = 1; j < centroids.size(); j++) {
        T d = (centroids[j] - x[i]).squaredNorm();
        if (d < min_d) {
          min_d = d;
          min_label = j;
        }
      }

      // check if the distance between the point and the closest centroid is larger than lambda
      if ( !k_incremented && min_d > lambda * lambda) {
        k_incremented = true;
        min_label = centroids.size();
        centroids.push_back(x[i]);

//				i = 0;	// re-calculate all labels since a new centroid is added
      }

      // if a label is changed, estimation is not converged
      if (labels[i] != min_label) {
        is_converged = false;
      }
      labels[i] = min_label;
    }

    return is_converged;
  }

public:
  std::vector<int> labels;			    // labels assigned to input data
  std::vector<VectorTd> centroids;	// estimated centroids
};

  }
}

#endif


#include <Eigen/Dense>
#include <SFML/Graphics.hpp>

#include <iomanip>
#include <sstream>

const int GRIDSIZE = 256;
const double WINDOW_SCALING_FACTOR = 3.;
const double GAUSS_SEIDEL_TOLERANCE = 1e-4;
const int GAUSS_SEIDEL_ITER = 20;

const int FPS = 60;
const double VISC = 0.;
const double DIFF = 0.00002;
const double SOURCE = 20000.;
const double FORCE = 150.;
const double DISSOLVE = 0.005;

using Eigen::MatrixXd;
using Eigen::MatrixXi;
using Eigen::VectorXd;
using Eigen::VectorXi;

void set_bnd(int b, MatrixXd &x)
{
    if (b == 1)
    {
        x.row(0) = -x.row(1);
        x.row(x.rows() - 1) = -x.row(x.rows() - 2);
    }
    else
    {
        x.row(0) = x.row(1);
        x.row(x.rows() - 1) = x.row(x.rows() - 2);
    }

    if (b == 2)
    {
        x.col(0) = -x.col(1);
        x.col(x.cols() - 1) = -x.col(x.cols() - 2);
    }
    else
    {
        x.col(0) = x.col(1);
        x.col(x.cols() - 1) = x.col(x.cols() - 2);
    }

    x(0, 0) = 0.5 * (x(1, 0) + x(0, 1));
    x(0, x.cols() - 1) = 0.5 * (x(1, x.cols() - 1) + x(0, x.cols() - 2));
    x(x.rows() - 1, 0) = 0.5 * (x(x.rows() - 2, 0) + x(x.rows() - 1, 1));
    x(x.rows() - 1, x.cols() - 1) = 0.5 * (x(x.rows() - 2, x.cols() - 1) + x(x.rows() - 1, x.cols() - 2));
}

void lin_solve(int b, MatrixXd &x, const MatrixXd &x0, double a, double c)
{
    MatrixXd x_last = x;

    const int N = x.rows() - 2;

    for (int i = 0; i < GAUSS_SEIDEL_ITER; i++)
    {
        x.block(1, 1, N, N) = (x0.block(1, 1, N, N) +
                               a * (x.block(0, 1, N, N) +
                                    x.block(2, 1, N, N) +
                                    x.block(1, 0, N, N) +
                                    x.block(1, 2, N, N))) /
                              c;
        set_bnd(b, x);

        if ((x_last - x).lpNorm<Eigen::Infinity>() < GAUSS_SEIDEL_TOLERANCE)
        {
            break;
        }

        x_last = x;
    }
}

void add_source(MatrixXd &d, const MatrixXd &s, double dt)
{
    d += dt * s;
}

void diffuse(int b, MatrixXd &x, const MatrixXd &x0, double diff, double dt)
{
    const int N = x.rows() - 2;
    const double a = dt * diff * N * N;
    lin_solve(b, x, x0, a, 1. + 4. * a);
}

void advect(int b, MatrixXd &d, const MatrixXd &d0,
            const MatrixXd &u, const MatrixXd &v, double dt)
{
    const int N = d.rows() - 2;

    const double dt0 = dt * N;

    for (int i = 1; i <= N; i++)
    {
        for (int j = 1; j <= N; j++)
        {
            double x = i - dt0 * u(i, j);
            double y = j - dt0 * v(i, j);

            x = std::clamp(x, 0.5, N + 0.5);
            y = std::clamp(y, 0.5, N + 0.5);

            const int i0 = int(x);
            const int j0 = int(y);

            const int i1 = i0 + 1;
            const int j1 = j0 + 1;

            const double s1 = x - i0;
            const double t1 = y - j0;

            const double s0 = 1. - s1;
            const double t0 = 1. - t1;

            d(i, j) = s0 * (t0 * d0(i0, j0) + t1 * d0(i0, j1)) +
                      s1 * (t0 * d0(i1, j0) + t1 * d0(i1, j1));
        }
    }

    set_bnd(b, d);
}

void project(MatrixXd &u, MatrixXd &v, MatrixXd &p, MatrixXd &div)
{
    const int N = div.rows() - 2;
    const double h = 1. / N;

    div.block(1, 1, N, N) = -0.5 * h *
                            (u.block(2, 1, N, N) -
                             u.block(0, 1, N, N) +
                             v.block(1, 2, N, N) -
                             v.block(1, 0, N, N));

    set_bnd(0, div);

    p.block(1, 1, N, N).setZero();
    set_bnd(0, p);

    lin_solve(0, p, div, 1., 4.);

    u.block(1, 1, N, N) -= 0.5 / h * (p.block(2, 1, N, N) - p.block(0, 1, N, N));
    v.block(1, 1, N, N) -= 0.5 / h * (p.block(1, 2, N, N) - p.block(1, 0, N, N));

    set_bnd(1, u);
    set_bnd(2, v);
}

void dens_step(MatrixXd &x, MatrixXd &x0,
               const MatrixXd &u, const MatrixXd &v,
               double diff, double dt)
{
    x *= (1. - DISSOLVE);
    add_source(x, x0, dt);
    diffuse(0, x0, x, diff, dt);
    advect(0, x, x0, u, v, dt);
}

void vel_step(MatrixXd &u, MatrixXd &v,
              MatrixXd &u0, MatrixXd &v0,
              double visc, double dt)
{
    add_source(u, u0, dt);
    add_source(v, v0, dt);

    diffuse(1, u0, u, visc, dt);
    diffuse(2, v0, v, visc, dt);

    project(u0, v0, u, v);

    advect(1, u, u0, u0, v0, dt);
    advect(2, v, v0, u0, v0, dt);

    project(u, v, u0, v0);
}

sf::Vector2i screen_coord_to_grid(sf::Vector2i coords)
{
    coords.x /= WINDOW_SCALING_FACTOR;
    coords.y /= WINDOW_SCALING_FACTOR;

    coords.x += 1;
    coords.y += 1;

    coords.x = std::clamp(coords.x, 1, GRIDSIZE - 2);
    coords.y = std::clamp(coords.y, 1, GRIDSIZE - 2);

    return {coords.x, GRIDSIZE - coords.y};
}

int main()
{
    // Velocity
    MatrixXd u(GRIDSIZE, GRIDSIZE);
    u.setZero();
    MatrixXd v(GRIDSIZE, GRIDSIZE);
    v.setZero();
    MatrixXd u_prev(GRIDSIZE, GRIDSIZE);
    u_prev.setZero();
    MatrixXd v_prev(GRIDSIZE, GRIDSIZE);
    v_prev.setZero();
    // Density
    MatrixXd d(GRIDSIZE, GRIDSIZE);
    d.setZero();
    MatrixXd d_prev(GRIDSIZE, GRIDSIZE);
    d_prev.setZero();

    // Drawing
    const uint WINDOW_SIZE = (GRIDSIZE - 2) * WINDOW_SCALING_FACTOR;
    sf::VideoMode mode(WINDOW_SIZE, WINDOW_SIZE);
    sf::RenderWindow window(mode, "Fluid", sf::Style::Close);
    window.setFramerateLimit(FPS);

    using MatrixXu32 = Eigen::Matrix<uint32_t, Eigen::Dynamic, Eigen::Dynamic>;
    using MatrixXu8 = Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic>;
    using color_stride_map_t = Eigen::Map<MatrixXu8, 0, Eigen::InnerStride<4>>;
    MatrixXu32 buffer(GRIDSIZE - 2, GRIDSIZE - 2);
    color_stride_map_t mapped_red((uint8_t *)buffer.data() + 0, buffer.rows(), buffer.cols());
    color_stride_map_t mapped_green((uint8_t *)buffer.data() + 1, buffer.rows(), buffer.cols());
    color_stride_map_t mapped_blue((uint8_t *)buffer.data() + 2, buffer.rows(), buffer.cols());
    color_stride_map_t mapped_alpha((uint8_t *)buffer.data() + 3, buffer.rows(), buffer.cols());
    mapped_red.setConstant(0);
    mapped_green.setConstant(255);
    mapped_blue.setConstant(255);
    mapped_alpha.setConstant(255);

    sf::Texture texture;
    texture.create(buffer.rows(), buffer.cols());
    texture.setSmooth(true);

    sf::Vector2i last_mouse_position(-1, -1);
    bool left_pressed = false;
    bool right_pressed = false;

    uint frames = 0;
    double current_framerate;
    sf::Clock clock;
    sf::Font font;
    if (not font.loadFromFile("../DejaVuSans-Bold.ttf"))
    {
        throw std::runtime_error("Font file not found!");
    }
    sf::Text text;
    text.setFont(font);
    const uint text_height = WINDOW_SIZE / 20;
    text.setCharacterSize(text_height);
    text.setFillColor(sf::Color(100, 100, 100));
    text.setPosition(text_height * 0.4, WINDOW_SIZE - text_height * 1.4);

    while (window.isOpen())
    {
        // Update
        {
            const double dt = 1. / FPS;
            vel_step(u, v, u_prev, v_prev, VISC, dt);
            dens_step(d, d_prev, u, v, DIFF, dt);

            d_prev.setZero();
            u_prev.setZero();
            v_prev.setZero();

            const int center = GRIDSIZE / 2;
            const int lower = GRIDSIZE / 5;
            const double source = 10000.;
            const double force = 50.;

            d_prev.block<2, 2>(lower, center).setConstant(source);
            u_prev.block<2, 2>(lower, center).setConstant(force);
            d_prev.block<2, 2>(d_prev.rows() - lower, center).setConstant(source);
            u_prev.block<2, 2>(u_prev.rows() - lower, center).setConstant(-force);
        }

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

            if (event.type == sf::Event::MouseButtonPressed)
            {
                if (event.mouseButton.button == sf::Mouse::Right)
                {
                    right_pressed = true;
                }
                if (event.mouseButton.button == sf::Mouse::Left)
                {
                    left_pressed = true;
                }
            }

            if (event.type == sf::Event::MouseButtonReleased)
            {
                if (event.mouseButton.button == sf::Mouse::Right)
                {
                    right_pressed = false;
                }
                if (event.mouseButton.button == sf::Mouse::Left)
                {
                    left_pressed = false;
                }
            }
        }

        if (right_pressed)
        {
            const sf::Vector2i mouse_position = sf::Mouse::getPosition(window);
            const sf::Vector2i idx = screen_coord_to_grid(mouse_position);

            d_prev(idx.x, idx.y) = SOURCE;
        }
        if (left_pressed)
        {
            const sf::Vector2i mouse_position = sf::Mouse::getPosition(window);
            const sf::Vector2i idx = screen_coord_to_grid(mouse_position);

            if (last_mouse_position != sf::Vector2i(-1, -1))
            {
                const double dx = last_mouse_position.x - mouse_position.x;
                const double dy = last_mouse_position.y - mouse_position.y;
                u_prev(idx.x, idx.y) = -FORCE * dx / WINDOW_SCALING_FACTOR;
                v_prev(idx.x, idx.y) = FORCE * dy / WINDOW_SCALING_FACTOR;
            }
            last_mouse_position = mouse_position;
        }
        else
        {
            last_mouse_position = {-1, -1};
        }

        window.clear(sf::Color::Black);

        mapped_alpha << d.block(1, 1, GRIDSIZE - 2, GRIDSIZE - 2).cwiseMin(255.).cast<uint8_t>();

        texture.update((uint8_t *)buffer.data());
        sf::Sprite sprite(texture);
        sprite.setOrigin(sprite.getLocalBounds().width / 2.,
                         sprite.getLocalBounds().height / 2.);
        sprite.setPosition(float(window.getSize().x) / 2, float(window.getSize().y) / 2);
        sprite.setScale(-WINDOW_SCALING_FACTOR, WINDOW_SCALING_FACTOR);
        sprite.setRotation(-180.);
        window.draw(sprite);

        sf::Time elapsed = clock.getElapsedTime();
        if (elapsed.asSeconds() > 0.25)
        {
            current_framerate = frames / elapsed.asSeconds();
            std::stringstream stream;
            stream << std::fixed << std::setprecision(2) << current_framerate;
            text.setString(stream.str());

            std::string s = stream.str();
            frames = 0;
            clock.restart();
        }

        window.draw(text);

        window.display();
        frames += 1;
    }

    return 0;
}


#ifndef GEOMETRY_HPP
#define GEOMETRY_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

// types
template<class U, int M=Eigen::Dynamic, int N=Eigen::Dynamic>
using matrix = Eigen::Matrix<U, M, N>;

template<class U, int N=Eigen::Dynamic>
using vector = matrix<U, N, 1>;

template<class U, int N=Eigen::Dynamic>
using covector = matrix<U, 1, N>;

template<class U>
using quaternion = Eigen::Quaternion<U>;

template<class G>
struct group;

// rigid bodies
template<class U>
struct rigid {
  quaternion<U> rotation;
  vector<U, 3> translation;

  rigid(): rotation(quaternion<U>::Identity()), translation(0, 0, 0) { }
};

// quaternion group
template<class U>
struct group<quaternion<U>> {
  using type = quaternion<U>;
  using algebra = vector<U, 3>;

  static type id() { return type::Identity(); }
  static type inv(const type& self) { return self.conjugate(); }
  static type prod(const type& lhs, const type& rhs) { return lhs * rhs; }

  static type exp(const algebra& self);
  static algebra log(const type& self);

  static algebra dexp(const algebra& self, const algebra& dself);
  static algebra dlog(const type& self, const algebra& dself);

  static algebra Ad(const type& self, const algebra& omega) { return self * omega; }
  static algebra ad(const algebra& self, const algebra& omega) { return self.cross(omega); }
};

// product group, same types
template<class G, int N>
struct group<vector<G, N>> {
  static_assert(N != Eigen::Dynamic, "size must be known at compile-time");
  using type = vector<G, N>;
  using algebra = vector<typename group<G>::algebra, N>;

  template<class F, class ... Self>
  static auto map(F f, const Self& ... self) {
    using result_type = typename std::result_of<F(Self...)>::type;
    vector<result_type, N> result;
    for(int i = 0; i < N; ++i) {
      result(i) = f(self(i)...);
    }

    return result;
  }

  static type id() {
    return type::Constant(group<G>::id());
  }

  static type inv(const type& self) {
    return map(group<G>::inv, self);
  }

  static type prod(const type& lhs, const type& rhs) {
    return map(group<G>::prod, lhs, rhs);
  }

  static type exp(const algebra& self) {
    return map(group<G>::exp, self);
  }

  static algebra log(const type& self) {
    return map(group<G>::log, self);
  }

  static algebra dexp(const algebra& self, const algebra& dself) {
    return map(group<G>::dexp, self, dself);
  }

  static algebra dlog(const type& self, const algebra& dself) {
    return map(group<G>::dlog, self, dself);
  }

  static algebra Ad(const type& self, const algebra& omega) {
    return map(group<G>::Ad, self, omega);
  }

  static algebra ad(const algebra& self, const algebra& omega) {
    return map(group<G>::ad, self, omega);
  }
};

// product group, different types
template<class G, class A, class ... Gs>
struct product {
  using type = G;
  using algebra = A;

  static type id() {
    return group<G>::pack(group<Gs>::id()...);
  }

  static type inv(const type& self) {
    return group<G>::unpack(self, [](const Gs&...gs) {
      return group<G>::pack(group<Gs>::inv(gs)...);
    });
  }

  static type prod(const type& lhs, const type& rhs) {
    return group<G>::unpack(lhs, [&](const Gs& ... lhs) {
      return group<G>::unpack(rhs, [&](const Gs& ... rhs) {
        return group<G>::pack(group<Gs>::prod(lhs, rhs)...);
      });
    });
  }

  static type exp(const algebra& self) {
    return group<G>::unpack(self, [](const typename group<Gs>::algebra& ... ws) {
      return group<G>::pack(group<Gs>::exp(ws)...);
    });
  }

  static algebra log(const type& self) {
    return group<G>::unpack(self, [](const typename group<Gs>::algebra& ... ws) {
      return group<G>::pack(group<Gs>::log(ws)...);
    });
  }

  static algebra dexp(const algebra& self, const algebra& dself) {
    return group<G>::unpack(self, [&](const typename group<Gs>::algebra& ... ws) {
      return group<G>::unpack(dself, [&](const typename group<Gs>::algebra& ... dws) {
        return group<G>::pack(group<Gs>::dexp(ws, dws)...);
      });
    });
  }

  static algebra dlog(const type& self, const algebra& dself) {
    return group<G>::unpack(self, [&](const Gs& ... gs) {
      return group<G>::unpack(dself, [&](const typename group<Gs>::algebra& ... dgs) {
        group<G>::pack(group<Gs>::dlog(gs, dgs)...);
      });
    });
  }

  static algebra Ad(const type& self, const algebra& dself) {
    return group<G>::unpack(self, [&](const Gs& ... gs) {
      return group<G>::unpack(dself, [&](const typename group<Gs>::algebra& ... dgs) {
        return group<G>::pack(group<Gs>::Ad(gs, dgs)...);
      });
    });
  }

  static algebra ad(const type& self, const algebra& dself) {
    return group<G>::unpack(self, [&](const Gs& ... gs) {
      return group<G>::unpack(dself, [&](const typename group<Gs>::algebra& ... dgs) {
        group<G>::pack(group<Gs>::ad(gs, dgs)...);
      });
    });
  }

};

// eucliean spaces
template<class E>
struct euclidean {
  using type = E;
  using algebra = E;

  static type inv(const type& self) {
    return -self;
  }

  static type prod(const type& lhs, const type& rhs) {
    return lhs + rhs;
  }

  static type exp(const algebra& self) {
    return self;
  }

  static algebra log(const type& self) {
    return self;
  }

  static algebra dexp(const algebra& self, const algebra& dself) {
    return dself;
  }

  static algebra dlog(const type& self, const algebra& dself) {
    return dself;
  }

  static algebra Ad(const type& self, const algebra& omega) {
    return omega;
  }

  static algebra ad(const algebra& self, const algebra& omega) {
    return group<E>::id();
  }

};

// scalars
template<> struct group<double> : euclidean<double> {
  static double id() { return 0; }
};

// shorthands for real types
using real = double;

using vec = vector<real>;

using vec2 = vector<real, 2>;
using vec3 = vector<real, 3>;
using vec6 = vector<real, 6>;

using mat22 = matrix<real, 2, 2>;
using mat33 = matrix<real, 3, 3>;
using mat66 = matrix<real, 6, 6>;

using quat = quaternion<real>;

#endif


#include <gtest/gtest.h>
#include <random>
#include <gmp.h>
#include <boost/multiprecision/gmp.hpp>
#include <ojlibs/power.hpp>
using namespace std;
using namespace ojlibs;
namespace bm = boost::multiprecision;

std::mt19937 gen;

void test_power(const bm::mpz_int &a, int b) {
    bm::mpz_int expect = pow(a, b);
    bm::mpz_int answer = power(a, b);

    EXPECT_EQ(expect, answer);
}

TEST(BASIC, SMALL) {
    test_power(12, 0);
    test_power(12, 1);
    test_power(12, 2);
}

TEST(BASIC, RANDOM) {
    gmp_randstate_t ran;
    gmp_randinit_default(ran);

    static const int TEST_GROUP = 1000;
    uniform_int_distribution<> dist(0, 10000);

    for (int i = 0; i < TEST_GROUP; ++i) {
        bm::mpz_int a;
        mpz_urandomb(a.backend().data(), ran, 15);

        int b = dist(gen);

        test_power(a, b);
    }
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/eps.hpp
 *
 * \brief Floating-point relative accuracy.
 *
 * Given a scalar \f$x\f$, compute the positive distance from \f$|x|\f$ to
 * the next larger in magnitude floating point number of the same precision as
 * \f$x\f$.
 * Except for numbers whose absolute value is smaller than the smallest positive
 * normalied floating-point number representable by its type , if
 * \f$2^y \le |x| < 2^{y+1}\f$, then the \c eps(x) function return
 * \f$2^{y-d}\f$, where \f$d\f$ is the number of radix digits in the mantissa.
 *
 * For all X of class double such that abs(X) <= realmin, eps(X) = 2^(-1074). Similarly, for all X of class single such that abs(X) <= realmin('single'), eps(X) = 2^(-149).
 *
 * <hr/>
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_EPS_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_EPS_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <cmath>
#include <limits>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

/**
 * \brief Compute the distance from \c 1.0 to the next largest floating-point
 *  precision number.
 * \tparam RealT The floaint-point type.
 * \return The distance from \c 1.0 to the next largest floating-point
 *  precision number.
 */
template <typename RealT>
BOOST_UBLAS_INLINE
typename type_traits<RealT>::real_type eps()
{
    // NOTE: type_traits<>::real_type is used in case of RealT is a non real
    //       type (e.g., std::complex).

    return ::std::numeric_limits<typename type_traits<RealT>::real_type>::epsilon();
}

/**
 * \brief  Given a scalar \f$x\f$, compute the positive distance from
 *  \f$|x|\f$ to the next larger in magnitude floating point number of the
 *  same precision as \f$x\f$.
 * \tparam RealT The floaint-point type.
 * \param x A floating-point scalar value.
 * \return The positive distance from \f$|x|\f$ to the next larger in
 *  magnitude floating point number of the same precision as \f$a\f$.
 */
template <typename RealT>
BOOST_UBLAS_INLINE
typename type_traits<RealT>::real_type eps(RealT x)
{
    // NOTE: type_traits<>::real_type is used in case of RealT is a non real
    //       type (e.g., std::complex).

    typedef typename type_traits<RealT>::real_type real_type;
    real_type y = ::std::abs(x);

    if (y == ::std::numeric_limits<real_type>::infinity()
        ||
        ::std::isnan(y))
    {
        return ::std::numeric_limits<real_type>::quiet_NaN();
    }
    else if (y <= ::std::numeric_limits<real_type>::min())
    {
        return ::std::numeric_limits<real_type>::denorm_min();
    }
    else
    {
        int e;
        ::std::frexp(y, &e);

        return ::std::ldexp(1, e - ::std::numeric_limits<real_type>::digits);
    }
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_EPS_HPP


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <gtest/gtest_VW.h>
#include <boost/random.hpp>
#include <vw/Math/Statistics.h>

using namespace vw;

static const double DELTA = 1e-5;

TEST(Statistics, vector_stats) {

  std::vector<double> vec(5);
  for (int i=0; i<5; ++i) {
    vec[i] = static_cast<double>(i);
  }

  EXPECT_EQ( 2, vw::math::mean(vec) );
  EXPECT_EQ( 2, vw::math::median(vec) );
  EXPECT_NEAR( 1.58114, vw::math::standard_deviation(vec, 2), DELTA );

}

TEST(Statistics, histogram) {

  int    num_bins =  9;
  double min_val  =  2;
  double max_val  = 10;
  math::Histogram hist(num_bins, min_val, max_val);

  hist(4);
  hist(6);
  hist(7);
  hist(9);
  hist(2);
  hist(1);
  hist(8);
  hist(12);
  hist(5);
  hist(6);
  hist(6);

  EXPECT_EQ(11, hist.get_total_num_values());
  EXPECT_EQ( 2, hist.get_bin_value (0));
  EXPECT_EQ( 3, hist.get_bin_value (4));
  EXPECT_EQ( 6, hist.get_bin_center(4));
}

using namespace vw::math;

TEST(Statistics, CDF_cauchy) {
  boost::mt19937 random_gen(42);
  boost::cauchy_distribution<double> cauchy(35,80);
  boost::variate_generator<boost::mt19937&,
    boost::cauchy_distribution<double> > generator(random_gen, cauchy);

  { // Default settings
    CDFAccumulator<double> cdf;
    for ( uint16 i = 0; i < 50000; i++ )
      cdf( generator() );

    EXPECT_NEAR( cdf.median(), 35.0, 2.0 );
    EXPECT_NEAR( cdf.first_quartile(), -45, 4.0 );
    EXPECT_NEAR( cdf.third_quartile(), 115, 4.0 );
  }
  { // More quantiles == more precision
    CDFAccumulator<double> cdf(2000,500);
    for ( uint16 i = 0; i < 50000; i++ )
      cdf( generator() );

    EXPECT_NEAR( cdf.median(), 35.0, 1.0 );
    EXPECT_NEAR( cdf.first_quartile(), -45, 2.0 );
    EXPECT_NEAR( cdf.third_quartile(), 115, 2.0 );
  }
}

TEST(Statistics, CDF_triangular) {
  boost::mt19937 random_gen(42);
  boost::triangle_distribution<double> triangular(10,60,80);
  boost::variate_generator<boost::mt19937&,
    boost::triangle_distribution<double> > generator(random_gen, triangular);

  { // Default settings
    CDFAccumulator<double> cdf;
    for ( uint16 i = 0; i < 50000; i++ )
      cdf( generator() );

    EXPECT_NEAR( cdf.median(), 51.833, 1.0 );
    EXPECT_NEAR( cdf.first_quartile(), 39.6, 2.0 );
    EXPECT_NEAR( cdf.third_quartile(), 61.3, 2.0 );
    EXPECT_NEAR( cdf.approximate_mean(), 50, 1.0 );
    EXPECT_NEAR( cdf.approximate_mean(0.05), 50, 0.5 );
    EXPECT_NEAR( cdf.approximate_stddev(), 14.7196, 2.0 );
    EXPECT_NEAR( cdf.approximate_stddev(0.05), 14.7196, 1.0 );
  }
  { // More quantiles == more precision
    CDFAccumulator<double> cdf(2000,500);
    for ( uint16 i = 0; i < 50000; i++ )
      cdf( generator() );

    EXPECT_NEAR( cdf.median(), 51.833, 0.5 );
    EXPECT_NEAR( cdf.first_quartile(), 39.6, 1.0 );
    EXPECT_NEAR( cdf.third_quartile(), 61.3, 1.0 );
    EXPECT_NEAR( cdf.approximate_mean(), 50, 0.5 );
    EXPECT_NEAR( cdf.approximate_mean(0.05), 50, 0.25 );
    EXPECT_NEAR( cdf.approximate_stddev(), 14.7196, 3.0 );
    EXPECT_NEAR( cdf.approximate_stddev(0.05), 14.7196, 0.5 );
  }
}

TEST(Statistics, CDF_Merge ) {
  boost::mt19937 random_gen(42);
  boost::normal_distribution<double> norm1(0, 3);
  boost::normal_distribution<double> norm2(5, 3);
  boost::variate_generator<boost::mt19937&, boost::normal_distribution<double> > generator1( random_gen, norm1 );
  boost::variate_generator<boost::mt19937&, boost::normal_distribution<double> > generator2( random_gen, norm2 );

  CDFAccumulator<double> cdf0, cdf1, cdf2, cdf3;
  // 0 is not updated.
  // 1 will only see gen1 .. but will later be merged with 2.
  // 2 will only see gen2
  // 3 is our control and sees both gen1 and gen2;
  for ( size_t i = 0; i < 50000; i++ ) {
    double sample1 = generator1(), sample2 = generator2();
    cdf1( sample1 );
    cdf2( sample2 );
    cdf3( sample1 );
    cdf3( sample2 );
  }

  cdf1.update();
  cdf2.update();
  cdf3.update();
  cdf1(cdf2);

  EXPECT_NEAR( cdf1.median(), cdf3.median(), 0.01 );
  EXPECT_NEAR( cdf1.first_quartile(), cdf3.first_quartile(), 0.01 );
  EXPECT_NEAR( cdf1.third_quartile(), cdf3.third_quartile(), 0.01 );
  EXPECT_NEAR( cdf1.approximate_mean(),
               cdf3.approximate_mean(), 0.01 );

  // Check that this works for an unused CDF object.
  cdf0(cdf1);
  EXPECT_NEAR( cdf1.median(), cdf0.median(), 0.01 );

  // Test the duplication function
  cdf0.duplicate(cdf2);
  EXPECT_NEAR( cdf2.median(), cdf0.median(), 0.01 );
}


// 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 Christopher Kormanyos 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 a bare metal real-time performance measurement of a derivative(negatable), 32-bit.

#include <mcal_benchmark.h>
#include <mcal_cpu.h>
#include <mcal_irq.h>
#include <mcal_port.h>

#define BOOST_FIXED_POINT_DISABLE_MULTIPRECISION // Do not use Boost.Multiprecision.
#define BOOST_FIXED_POINT_DISABLE_IOSTREAM       // Do not use I/O streaming.

#include <boost/fixed_point/fixed_point.hpp>

typedef boost::fixed_point::negatable<6, -9> numeric_type;
//typedef float numeric_type;

namespace app
{
  namespace benchmark
  {
    void task_init();
    void task_func();
  }

  typedef mcal::benchmark::benchmark_port_type port_type;
}

namespace local
{
  template<typename RealValueType,
           typename RealFunctionType>
  RealValueType first_derivative(const RealValueType& x,
                                 const RealValueType& dx,
                                 RealFunctionType real_function)
  {
    const RealValueType dx2(dx  + dx);
    const RealValueType dx3(dx2 + dx);

    const RealValueType m1((  real_function(x + dx)
                            - real_function(x - dx))  / 2U);
    const RealValueType m2((  real_function(x + dx2)
                            - real_function(x - dx2)) / 4U);
    const RealValueType m3((  real_function(x + dx3)
                            - real_function(x - dx3)) / 6U);

    const RealValueType fifteen_m1(m1 * 15U);
    const RealValueType six_m2    (m2 *  6U);
    const RealValueType ten_dx    (dx * 10U);

    return ((fifteen_m1 - six_m2) + m3) / ten_dx;
  }
}

numeric_type a;
numeric_type b;
numeric_type c;
numeric_type d;

void app::benchmark::task_init()
{
  port_type::set_direction_output();

  a = numeric_type(12U) / 10U;
  b = numeric_type(34U) / 10U;
  c = numeric_type(56U) / 10U;
}

void app::benchmark::task_func()
{
  // Compute the approximate derivative of (a * x^2) + (b * x) + c
  // evaluated at 1/2, where the approximate values of the coefficients
  // are: a = 1.2, b = 3.4, and c = 5.6. The step-size for evaluating
  // the derivative is set to a value of approximately 1/4.

  mcal::irq::disable_all();
  port_type::set_pin_high();

  d = local::first_derivative(numeric_type(1U) / 2U, // x-value
                              numeric_type(1U) / 4U, // step size dx
                              [](const numeric_type& x) -> numeric_type
                              {
                                return (((a * x) + b) * x) + c;
                              });

  port_type::set_pin_low();
  mcal::irq::enable_all();

  // The expected result is ((2 * a) + b) = (2.4 + 3.4) = 4.6 (exact).
  // We obtain a fixed-point result of approximately 4.5938.

  // Verify that the result lies within (4.5 < result < 4.7).
  // The expected result is 4.6, so this is a wide tolerance.

  const bool value_is_ok =    (d > numeric_type(4))
                           && (d < numeric_type(5));

  if(value_is_ok)
  {
    // The benchmark is OK.
    // Perform one nop and leave.

    mcal::cpu::nop();
  }
  else
  {
    // The benchmark result is not OK!
    // Remain in a blocking loop and crash the system.

    for(;;) { mcal::cpu::nop(); }
  }
}