/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Eigen>
#include <cmath>

namespace fluid {
namespace algorithm {

class SpectralShape
{

  using ArrayXd = Eigen::ArrayXd;

public:
  SpectralShape() {}

  void processFrame(Eigen::Ref<ArrayXd> in, double sampleRate, double minFreq,
                    double maxFreq, double rolloffTarget, bool logFreq,
                    bool usePower)
  {
    using namespace std;
    maxFreq = (maxFreq == -1) ? (sampleRate / 2) : min(maxFreq, sampleRate / 2);
    ArrayXd mag = in.max(epsilon);
    index   nBins = mag.size();
    double  binHz = sampleRate / ((nBins - 1) * 2.);
    index   minBin = ceil(minFreq / binHz);
    index   maxBin =
        min(static_cast<index>(floorl(maxFreq / binHz)), (nBins - 1));
    if (maxBin <= minBin)
    {
      mOutputBuffer.setZero();
      return;
    }

    if (logFreq && minBin == 0)
    {
      minBin = 1;
      mag(1) += mag(0);
    }
    ArrayXd amp = usePower ? mag.square() : mag;
    amp = amp.segment(minBin, maxBin - minBin);
    double  ampSum = amp.sum();
    ArrayXd freqs =
        ArrayXd::LinSpaced(maxBin - minBin, minBin * binHz, maxBin * binHz);
    if (logFreq)
    { freqs = 69 + (12 * (freqs / 440).log() * log2E); } // MIDI cents

    double centroid = (amp * freqs).sum() / ampSum;
    double spread = (amp * (freqs - centroid).square()).sum() / ampSum;
    double skewness = (amp * (freqs - centroid).pow(3)).sum() /
                      (spread * sqrt(spread) * ampSum);
    double kurtosis =
        (amp * (freqs - centroid).pow(4)).sum() / (spread * spread * ampSum);

    double flatness = exp(amp.log().mean()) / amp.mean();
    double rolloff = maxBin - 1;
    double cumSum = 0;
    double target = ampSum * rolloffTarget / 100.0;
    for (index i = 0; cumSum <= target && i < amp.size(); i++)
    {
      cumSum += amp(i);
      if (cumSum >= target)
      {
        rolloff = (i == 0) ? freqs(i)
                           : freqs(i) - (freqs(i) - freqs(i - 1)) *
                                            (cumSum - target) / amp(i);
        break;
      }
    }
    double crest = amp.maxCoeff() / amp.mean();

    mOutputBuffer(0) = centroid;
    mOutputBuffer(1) = sqrt(spread);
    mOutputBuffer(2) = skewness;
    mOutputBuffer(3) = kurtosis;
    mOutputBuffer(4) = rolloff;
    mOutputBuffer(5) = 20 * log10(max(flatness, epsilon));
    mOutputBuffer(6) = 20 * log10(max(crest, epsilon));
  }

  void processFrame(const RealVector& input, RealVectorView output,
                    double sampleRate, double minFreq = 0, double maxFreq = -1,
                    double rolloffTarget = 0.95, bool logFreq = false,
                    bool usePower = false)
  {
    assert(output.size() == 7);
    ArrayXd in = _impl::asEigen<Eigen::Array>(input);
    processFrame(in, sampleRate, minFreq, maxFreq, rolloffTarget, logFreq,
                 usePower);
    output = _impl::asFluid(mOutputBuffer);
  }

private:
  ArrayXd mOutputBuffer{7};
};

} // namespace algorithm
} // namespace fluid


#include "PolyMesh/IOManager.h"
#include <Eigen/Sparse>
#include <Eigen/SVD>

using namespace acamcad;
using namespace polymesh;
using namespace Eigen;

void MeshInterpolation(PolyMesh* source, PolyMesh* target, double delta_t)
{
	std::string command = "mkdir result ";
	system(command.c_str());

	int nv = source->numVertices();
	int nf = source->numPolygons();

	std::vector<Matrix2d> S(nf);
	std::vector<Vector3d> xx(nf);
	std::vector<Vector3d> yy(nf);
	std::vector<double> area(nf);
	std::vector<double> angle(nf);

	Matrix2d I = MatrixXd::Identity(2, 2);

	std::vector<std::vector<int>> v_id(nf);

	// Initial
	for (FaceIter f_it = source->polyfaces_begin(); f_it != source->polyfaces_end(); ++f_it)
	{
		int id = (*f_it)->index();
		MHalfedge* heh = (*f_it)->halfEdge();
		MVert* v0 = heh->fromVertex();
		MVert* v1 = heh->toVertex();
		MHalfedge* next_heh = heh->next();
		MVert* v2 = next_heh->toVertex();

		v_id[id].push_back(v0->index());
		v_id[id].push_back(v1->index());
		v_id[id].push_back(v2->index());

		area[id] = cross(v1->position() - v0->position(), v2->position()- v0->position()).norm() / 2.0;

		xx[id] = Vector3d(v2->position()[0] - v1->position()[0], v0->position()[0] - v2->position()[0], v1->position()[0] - v0->position()[0]);
		yy[id] = Vector3d(v1->position()[1] - v2->position()[1], v2->position()[1] - v0->position()[1], v0->position()[1] - v1->position()[1]);

		Vector3d u(target->vert(v0->index())->position()[0], target->vert(v1->index())->position()[0], target->vert(v2->index())->position()[0]);
		Vector3d v(target->vert(v0->index())->position()[1], target->vert(v1->index())->position()[1], target->vert(v2->index())->position()[1]);

		Matrix2d J;
		J << yy[id].dot(u) / (2 * area[id]), xx[id].dot(u) / (2 * area[id]), yy[id].dot(v) / (2 * area[id]), xx[id].dot(v) / (2 * area[id]);
		JacobiSVD<Matrix2d> svd(J, ComputeFullV | ComputeFullU);

		Matrix2d V = svd.matrixV();
		Matrix2d U = svd.matrixU();
		Matrix2d R = U * V.transpose();

		Matrix2d sigma;
		sigma(0, 1) = sigma(1, 0) = 0;
		sigma(0, 0) = svd.singularValues()[0];
		sigma(1, 1) = svd.singularValues()[1];
		S[id] = V * sigma * V.transpose();
		angle[id] = atan2(R(1, 0), R(1, 1));
	}

	// fix nv-1
	double u0 = source->vert(nv - 1)->position()[0];
	double v0 = source->vert(nv - 1)->position()[1];

	SparseMatrix<double> A(2 * nv - 2, 2 * nv - 2);
	std::vector<Triplet<double>>triplet;

	for (FaceIter f_it = source->polyfaces_begin(); f_it != source->polyfaces_end(); ++f_it)
	{
		int id = (*f_it)->index();
		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				if ((v_id[id][i] == nv - 1) || (v_id[id][j] == nv - 1))
				{
					continue;
				}
				else
				{
					triplet.push_back(Triplet<double>(2 * v_id[id][i], 2 * v_id[id][j], yy[id][i] * yy[id][j] / (area[id] * area[id] * 2)));
					triplet.push_back(Triplet<double>(2 * v_id[id][i], 2 * v_id[id][j], xx[id][i] * xx[id][j] / (area[id] * area[id] * 2)));
					triplet.push_back(Triplet<double>(2 * v_id[id][i] + 1, 2 * v_id[id][j] + 1, yy[id][i] * yy[id][j] / (area[id] * area[id] * 2)));
					triplet.push_back(Triplet<double>(2 * v_id[id][i] + 1, 2 * v_id[id][j] + 1, xx[id][i] * xx[id][j] / (area[id] * area[id] * 2)));
				}
			}
		}
	}
	A.setFromTriplets(triplet.begin(), triplet.end());
	SparseLU<SparseMatrix<double>> solver;
	solver.analyzePattern(A);
	solver.factorize(A);

	double t = 0;
	while (t <= 1)
	{
		std::cout << "iter:" << t << std::endl;

		VectorXd b(2 * nv - 2);
		b.setZero();

		for (FaceIter f_it = source->polyfaces_begin(); f_it != source->polyfaces_end(); ++f_it)
		{
			int id = (*f_it)->index();
			Matrix2d A, R;
			double theta_t = angle[id] * t;

			R << cos(theta_t), -sin(theta_t), sin(theta_t), cos(theta_t);
			A = R * ((1.0 - t)*I + t * S[id]);
			for (int i = 0; i < 3; i++)
			{
				for (int j = 0; j < 3; j++)
				{
					if ((v_id[id][i] == nv - 1) || (v_id[id][j] == nv - 1))
					{
						if ((v_id[id][i] != nv - 1) && (v_id[id][j] == nv - 1))
						{
							b[2 * v_id[id][i]] += -yy[id][i] * yy[id][j] * u0 / (area[id] * area[id] * 2);
							b[2 * v_id[id][i]] += -xx[id][i] * xx[id][j] * u0 / (area[id] * area[id] * 2);
							b[2 * v_id[id][i] + 1] += -yy[id][i] * yy[id][j] * v0 / (area[id] * area[id] * 2);
							b[2 * v_id[id][i] + 1] += -xx[id][i] * xx[id][j] * v0 / (area[id] * area[id] * 2);
						}
					}
				}
				if (v_id[id][i] != nv - 1)
				{
					b[2 * v_id[id][i]] += yy[id][i] * A(0, 0) / area[id];
					b[2 * v_id[id][i]] += xx[id][i] * A(0, 1) / area[id];
					b[2 * v_id[id][i] + 1] += yy[id][i] * A(1, 0) / area[id];
					b[2 * v_id[id][i] + 1] += xx[id][i] * A(1, 1) / area[id];
				}
			}
		}
		VectorXd result = solver.solve(b);
		for (VertexIter v_it = source->vertices_begin(); v_it != source->vertices_end(); ++v_it)
		{
			int id = (*v_it)->index();
			if (id != nv - 1)
			{
				(*v_it)->setPosition(result[2 * id], result[2 * id + 1], 0);
			}
		}

		std::string string_t = std::to_string(t);
		writeMesh("result\\" + string_t + ".obj", source);
		t += delta_t;
	}
}

int main(int argc, const char **argv)
{
	if (argc != 4)
	{
		std::cout << "========== Hw8 Usage  ==========\n";
		std::cout << std::endl;
		std::cout << "blablabla.exe	source.obj target.obj delta_t\n";
		std::cout << std::endl;
		std::cout << "=================================================\n";
        return 0;
	}
	std::string source_path = argv[1];
	std::string target_path = argv[2];
	std::string t_string = argv[3];

	PolyMesh* source_mesh = new PolyMesh();
	PolyMesh* target_mesh = new PolyMesh();
	loadMesh(source_path, source_mesh);
	loadMesh(target_path, target_mesh);
	double delta_t = atof(t_string.c_str());
	MeshInterpolation(source_mesh, target_mesh, delta_t);
    return 0;
}


//  (C) Copyright Thomas Becker 2015. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

#ifndef TMB_NORMAL_DISTRIBUTION_PIVOT_09_27_2015_HPP
#define TMB_NORMAL_DISTRIBUTION_PIVOT_09_27_2015_HPP

#include <boost/math/special_functions/erf.hpp>
#include "distribution_specific_pivot_base.hpp"

/*
 * Pivot calculator for normally distributed data.
 */

namespace median_project
{
namespace numerical_quick_median_detail
{

/**
 * Pivot calculator for normal distributions.
 */
class normal_distribution_pivot : public distribution_specific_pivot_base
{
  private:
    virtual double cdf(double x) const override
    {
        return .5 * (1.0 + boost::math::erf((x - m_total_sequence_mean) / (sqrt(2.0) * m_total_sequence_std_dev)));
    }

    virtual double quantile(double x) const override
    {
        return m_total_sequence_mean + m_total_sequence_std_dev * sqrt(2.0) * boost::math::erf_inv(2.0 * x - 1.0);
    }
};
} // end namespace numerical_quick_median_detail
} // end namespace median_project

#endif // TMB_NORMAL_DISTRIBUTION_PIVOT_09_27_2015_HPP


#include<iostream>
#include<thread>
#include<random>
#include<type_traits>
#include<vector>
#include <boost/multiprecision/cpp_int.hpp>
#include"printtable.h"
#include "calcSignature.hpp"

namespace ExactSignature {
  using std::vector;

  typedef boost::multiprecision::cpp_rational Number;

  class Signature {
  public:
    vector<vector<Number>> m_data;

    template<typename Numeric>
    void sigOfSegment(int d, int m, const Numeric* segment) {
      m_data.resize(m);
      auto& first = m_data[0];
      first.resize(d);
      for (int i = 0; i<d; ++i)
        first[i] = (Number)segment[i];
      for (int level = 2; level <= m; ++level) {
        const auto& last = m_data[level - 2];
        auto& s = m_data[level - 1];
        s.assign(calcSigLevelLength(d, level), 0);
        int i = 0;
        for (auto l : last)
          for (auto p = segment; p<segment + d; ++p)
            s[i++] = (Number)(*p * l * Number(1, level));
      }
    }

    void sigOfNothing(int d, int m) {
      m_data.resize(m);
      m_data[0].assign(d, 0);
      for (int level = 2; level <= m; ++level) {
        auto& s = m_data[level - 1];
        s.assign(calcSigLevelLength(d, level), 0);
      }
    }

    //if a is the signature of path A, b of B, then
    //a.concatenateWith(d,m,b) makes a be the signature of the concatenated path AB
    //This is also the (concatenation) product of the elements a and b in the tensor algebra.
    void concatenateWith(int d, int m, const Signature& other) {
      for (int level = m; level>0; --level) {
        for (int mylevel = level - 1; mylevel>0; --mylevel) {
          int otherlevel = level - mylevel;
          auto& oth = other.m_data[otherlevel - 1];
          for (auto dest = m_data[level - 1].begin(),
            my = m_data[mylevel - 1].begin(),
            myE = m_data[mylevel - 1].end(); my != myE; ++my) {
            for (const Number& d : oth) {
              *(dest++) += d * *my;
            }
          }
        }
        auto source = other.m_data[level - 1].begin();
        for (auto dest = m_data[level - 1].begin(),
          e = m_data[level - 1].end();
          dest != e;)
          *(dest++) += *(source++);

      }
    }
    void swap(Signature& other) {
      m_data.swap(other.m_data);
    }
    void multiplyByConstant(Number c) {
      for (auto& a : m_data)
        for (auto& b : a)
          b *= c;
    }
    template<typename Numeric>
    void writeOut(Numeric* dest) const {
      for (auto& a : m_data)
        for (auto& b : a)
          *(dest++) = (Numeric)b;
          //*(dest++) = (Numeric)denominator(b);
    }
  };

  //This also calculates the concatenation product in the tensor algebra,
  //but in the case where we assume 0 instead of 1 in the zeroth level.
  //It is not in-place
  Signature concatenateWith_zeroFirstLevel(int d, int m,
    const Signature& a,
    const Signature& b) {
    Signature out;
    out.sigOfNothing(d, m);
    for (int level = m; level>0; --level) {
      for (int alevel = level - 1; alevel>0; --alevel) {
        int blevel = level - alevel;
        auto& aa = a.m_data[alevel - 1];
        auto& bb = b.m_data[blevel - 1];
        auto dest = out.m_data[level - 1].begin();
        for (const Number& c : aa) {
          for (const Number& d : bb) {
            *(dest++) += d * c;
          }
        }
      }
    }
    return out;
  }

  void logTensorHorner(Signature& x) {
    const int m = (int)x.m_data.size();
    const int d = (int)x.m_data[0].size();
    if (m <= 1)
      return;
    Signature s, t;
    s.sigOfNothing(d, m - 1);
    t.sigOfNothing(d, m);
    for (int depth = m; depth > 0; --depth) {
      Number constant = (Number) 1.0 / depth;
      //make t be x*s up to level (1+m-depth). [this does nothing the first time round]
      for (int lev = 2; lev <= 1 + m - depth; ++lev) {
        //t.m_data[lev - 1] = x.m_data[lev - 1];
        auto& tt = t.m_data[lev - 1];
        std::fill(tt.begin(), tt.end(), (Number) 0.0);
        for (int leftLev = 1; leftLev < lev; ++leftLev) {
          int rightLev = lev - leftLev;
          auto& aa = x.m_data[leftLev - 1];
          auto& bb = s.m_data[rightLev - 1];
          auto dest = t.m_data[lev - 1].begin();
          for (const Number& c : aa)
            for (const Number& dd : bb)
              *(dest++) += dd * c;
        }
      }
      //make s be x*constant-t up to level (1+m-depth)
      if (depth>1)
        for (int lev = 1; lev <= 1 + m - depth; ++lev) {
          auto is = s.m_data[lev - 1].begin();
          auto ix = x.m_data[lev - 1].begin();
          auto es = s.m_data[lev - 1].end();
          auto it = t.m_data[lev - 1].begin();
          for (; is != es; ++is, ++it, ++ix)
            *is = constant * *ix - *it;
        }
    }
    //x isn't modified until this next bit.
    //make x be x-t
    for (int lev = 2; lev <= m; ++lev) {
      auto it = t.m_data[lev - 1].begin();
      auto ix = x.m_data[lev - 1].begin();
      auto ex = x.m_data[lev - 1].end();
      for (; ix != ex; ++ix, ++it)
        *ix -= *it;
    }
  }

}

std::mt19937 g_rnd;
using PathReal = double;
std::vector<PathReal> randomPath(int pathLength, int d){
  std::vector<PathReal> out(pathLength*d);
  std::uniform_real_distribution<PathReal> urd(0, 1);
  for (auto& d : out)
    d = urd(g_rnd);
  return out;
}

template<bool arbprec>
using SignatureType = std::conditional_t<arbprec,
  ExactSignature::Signature,
  CalcSignature::Signature>;

//the stroke ending at from ... the stroke ending before to
template<bool arbprec>
SignatureType<arbprec> sigPiece(const std::vector<PathReal>& path, int d, int m, size_t from, size_t to) {
  SignatureType<arbprec> s1, s2;
  std::vector<PathReal> displacement(d);
  for (size_t i = from; i < to; ++i) {
    for (int j = 0; j < d; ++j)
      displacement[j] = path[i*d + j] - path[(i - 1)*d + j];
    s1.sigOfSegment(d, m, displacement.data());
    if (i == from) {
      s2.swap(s1);
    }
    else {
      s2.concatenateWith(d, m, s1);
    }
  }
  return s2;
}

//Calculate the signature or log signature of path,
//using float or arbitrary precision
template<bool arbprec>
std::vector<double> sig(const std::vector<PathReal>& path, int d, int m, bool log) {
  size_t pathLength = path.size() / d;
  auto s2 = sigPiece<arbprec>(path, d, m, 1, pathLength);
  if (log)
    logTensorHorner(s2);
  std::vector<double> out(calcSigTotalLength(d, m));
  s2.writeOut(out.data());
  return out;
}

//like sig but split the work between processors
template<bool arbprec>
std::vector<double> sigParallel(const std::vector<PathReal>& path, int d, int m, bool log) {
  size_t pathLength = path.size() / d;
  size_t nThreads = 4;
  size_t each = (pathLength - 1) / nThreads;
  if (each < 1)
    return sig<arbprec>(path, d, m, log);
  size_t start = 1;
  std::vector<SignatureType<arbprec>> pieces(nThreads);
  std::vector<std::thread> threads;
  for (size_t thread = 0; thread < nThreads; ++thread) {
    size_t start = thread*each + 1;
    size_t end = (thread + 1 == nThreads) ? pathLength : (thread*each + each + 1);
    threads.emplace_back([&pieces,path,d,m,start, end,thread]() {
      pieces[thread] = sigPiece<arbprec>(path, d, m, start, end);
    });
  }
  for (auto& t : threads)
    t.join();
  auto s2 = pieces[0];
  for (size_t i = 1; i < nThreads; ++i)
    s2.concatenateWith(d, m, pieces[i]);
  if (log)
    logTensorHorner(s2);
  std::vector<double> out(calcSigTotalLength(d, m));
  s2.writeOut(out.data());
  return out;
}

void test() {
  using N = boost::multiprecision::cpp_rational;
  N a = 0.0625;
  N b = a - N(4, 5);
  std::cout << b << " "<<(double) b << "\n";
}

void go() {
  auto d = 2, m = 4, pathLength=4;
  auto path = randomPath(pathLength, d);
  bool log = true;
  auto s1 = sig<false>(path, d, m, log);
  //auto s2 = sig<true>(path, d, m, log);
  auto s2 = sigParallel<true>(path, d, m, log);

  //for (size_t i = 0; i < s1.size(); ++i) {
  //  std::cout << s1[i] << "  " << s2[i] << "\n";
  //}
  auto diffs = diffVectors(s1, s2);
  PrintTable::printTable(s1, s2, diffs);
  std::cout << "l2 norm " << l2norm(s1) << " "<< l2norm(s2) << "\n";
  std::cout << "l2 diff " << l2norm(diffs) << "\n";
  std::cout << "total diff "<<l1norm(diffs) << "\n";
  printMaxDiff(s1, s2);
  printMaxRelDiff(s1, s2);
  printGreatestWith0b(s1, s2);
}

int main() {
  //test();
  go();
}


#include <Eigen/Dense>
#include <chrono>
#include <thread>
#include <Eigen/LU>
#include <cmath>

class simpleObserver
{
public:
    simpleObserver(float scalingFactor = 1);
    Eigen::MatrixXf calculateClosestPoints(Eigen::MatrixXf bodyNodePositions, Eigen::MatrixXf pointCloud);
    /* First Calculate a Distance Matrix, then find at each column of the Distance matrix the lowest number,
    // calculate the average Points on the number and return the set of average points
    // bodyNodePositions(n,3) n = number of Body Nodes
    \param pointCloud(l,3) l = number of Points
    \param distMatrix(n,l), n rows and l columns
    // ======== rows ==========
    // =
    // s
    // l
    // o
    // c
    \return Eigen::MatrixXf avgPoints(n,3)
    */
    Eigen::MatrixXf calculateClosestPointsExpFun(Eigen::MatrixXf bodyNodePositions, Eigen::MatrixXf pointCloud);
    Eigen::MatrixXf calculateClosestPointsWeighted(Eigen::MatrixXf bodyNodePositions, Eigen::MatrixXf pointCloud);

protected:
    /*
    \int scaling factor is used to define how much the calculated target point should be trusted
    scaling factor = 1 equals full trust in the target points
    scaling factor = 0 equals no trust in the target points
    target points to send = initial points + scaling * numberOfPoints/avgNumberOfPoints * (target-initial)
    */
    float mScalingFactor;
    void distanceMatrix(Eigen::MatrixXf *distMatrix, Eigen::MatrixXf *avgPoints, Eigen::MatrixXf *bodyNodePositions, Eigen::MatrixXf *pointCloud, int lowerRow, int upperRow,int *numberOfPoints);
    float fun(float x);
};


#include "uniform_bspline.hpp"

#include <iostream>

#include <Eigen/Core>
#include <gtest/gtest.h>

template <typename T>
using EigenAlignedVec = std::vector<T, Eigen::aligned_allocator<T>>;

TEST(UniformBSplineExample, Spline1d1d) {
    // clang-format off
    {
    //! [Spline1d1d_Definition]
    ubs::UniformBSpline<double, 3, double, double, std::vector<double>> spline;
    //! [Spline1d1d_Definition]
    }
    // clang-format on

    //! [Spline1d1d_DefinitionShort]
    ubs::UniformBSpline11d<3> spline;
    //! [Spline1d1d_DefinitionShort]

    //! [Spline1d1d_ControlPoints]
    std::vector<double> controlPoints{0.0, 1.0, 2.0, 3.0, 4.0};
    spline.setControlPoints(controlPoints);
    //! [Spline1d1d_ControlPoints]

    //! [Spline1d1d_Eval]
    double val0 = spline.evaluate(0.0);
    double val1 = spline.evaluate(1.0);
    //! [Spline1d1d_Eval]
    EXPECT_DOUBLE_EQ(1.0, val0);
    EXPECT_DOUBLE_EQ(3.0, val1);

    //! [Spline1d1d_Derivative]
    double deriv1 = spline.derivative(0.0, 1);
    double deriv2 = spline.derivative(1.0, 2);
    //! [Spline1d1d_Derivative]
    EXPECT_DOUBLE_EQ(2.0, deriv1);
    EXPECT_DOUBLE_EQ(0.0, deriv2);

    //! [Spline1d1d_Smoothness]
    double smoothness = spline.smoothness<1>();
    //! [Spline1d1d_Smoothness]
    EXPECT_DOUBLE_EQ(4.0, smoothness);

    //! [Spline1d1d_Bounds]
    spline.setBounds(-2.0, 5.0);
    //! [Spline1d1d_Bounds]
    EXPECT_EQ(spline.getLowerBound(), -2.0);
    EXPECT_EQ(spline.getUpperBound(), 5.0);
}

TEST(UniformBSplineExample, Spline1d3d) {
    //! [Spline1d3d_Definition]
    ubs::UniformBSpline<double, 3, double, Eigen::Vector3d, EigenAlignedVec<Eigen::Vector3d>> spline;
    //! [Spline1d3d_Definition]

    //! [Spline1d3d_Eval]
    Eigen::Vector3d val0 = spline.evaluate(0.0);
    Eigen::Vector3d val1 = spline.evaluate(1.0);
    //! [Spline1d3d_Eval]
    EXPECT_DOUBLE_EQ(0.0, val0.norm());
    EXPECT_DOUBLE_EQ(0.0, val1.norm());

    //! [Spline1d3d_Derivative]
    Eigen::Vector3d deriv1 = spline.derivative(0.0, 1);
    Eigen::Vector3d deriv2 = spline.derivative(1.0, 2);
    //! [Spline1d3d_Derivative]
    EXPECT_DOUBLE_EQ(0.0, deriv1.norm());
    EXPECT_DOUBLE_EQ(0.0, deriv2.norm());

    //! [Spline1d3d_Smoothness]
    Eigen::Vector3d smoothness = spline.smoothness<1>();
    //! [Spline1d3d_Smoothness]
    EXPECT_DOUBLE_EQ(0.0, smoothness.sum());
}

TEST(UniformBSplineExample, Spline2d1d) {
    //! [Spline2d1d_Definition]
    ubs::UniformBSpline<double, 3, Eigen::Vector2d, double, Eigen::MatrixXd> spline;
    //! [Spline2d1d_Definition]

    //! [Spline2d1d_Evaluate_Long]
    double val = spline.evaluate(Eigen::Vector2d{0.0, 0.0});
    //! [Spline2d1d_Evaluate_Long]
    EXPECT_DOUBLE_EQ(0.0, val);

    //! [Spline2d1d_Evaluate_Short]
    val = spline.evaluate({0.0, 0.0});
    //! [Spline2d1d_Evaluate_Short]
    EXPECT_DOUBLE_EQ(0.0, val);

    //! [Spline2d1d_Derivative_10]
    double deriv10 = spline.derivative({0.0, 0.0}, {1, 0});
    //! [Spline2d1d_Derivative_10]
    EXPECT_DOUBLE_EQ(0.0, deriv10);

    //! [Spline2d1d_Derivative_11]
    double deriv11 = spline.derivative({0.0, 0.0}, {1, 1});
    //! [Spline2d1d_Derivative_11]
    EXPECT_DOUBLE_EQ(0.0, deriv11);
}

TEST(UniformBSplineExample, Spline2d3d) {
    //! [Spline2d3d_Definition]
    ubs::UniformBSpline<double, 3, Eigen::Vector2d, Eigen::Vector3d, ubs::EigenAlignedMultiArray<Eigen::Vector3d, 2>>
        spline;
    //! [Spline2d3d_Definition]

    //! [Spline2d3d_Definition_Short]
    ubs::EigenUniformBSpline<double, 3, 2, 3> splineS;
    //! [Spline2d3d_Definition_Short]

    //! [Spline2d3d_Evaluate]
    Eigen::Vector3d val = spline.evaluate({0.0, 0.2});
    //! [Spline2d3d_Evaluate]
    EXPECT_DOUBLE_EQ(0.0, val.norm());
}


#include <mesh_array.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <cmath>   // needed for std::sqrt and std::fabs

BOOST_AUTO_TEST_SUITE(mesh_array);

BOOST_AUTO_TEST_CASE(mesh_array_compute_distance_map_sphere)
{
  using std::sqrt;
  using std::fabs;

  typedef tiny::MathTypes<float> MT;
  typedef MT::real_type          T;

  mesh_array::T3Mesh surf;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sX;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sY;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sZ;

  size_t const slices   = 12u;
  size_t const segments = 24u;
  T      const radius   = 12.0f;

  mesh_array::make_sphere<MT>(radius, slices, segments, surf, sX, sY, sZ);

  mesh_array::T4Mesh mesh;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> X;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Y;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Z;

  mesh_array::tetgen(surf, sX, sY, sZ, mesh, X, Y, Z, mesh_array::tetgen_quality_settings() );

  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> phi;

  mesh_array::compute_distance_map( mesh, X, Y, Z, phi );

  for (size_t i = 0u; i < phi.size(); ++i)
  {
    T const real_distance = radius - std::sqrt(X(i)*X(i) + Y(i)*Y(i) + Z(i)*Z(i));

    BOOST_CHECK_CLOSE(  phi(i), real_distance, 1.0 );
  }
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once

#include "out.hh"
#include "types.hh"

#include "eigen.hh"
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <sophus/se2.hpp>
#include <sophus/so2.hpp>

namespace raytrace {
using Vec1 = Vec<1>;
using Vec2 = Eigen::Vector2d;
using Vec3 = Eigen::Vector3d;
using Mat2 = Eigen::Matrix2d;

double cost(const Vec3 &eta, const std::vector<Vec2> &x, const std::vector<Vec2> &z) {
  const se2 T = se2::exp(eta);

  double error_sq = 0.0;
  for (std::size_t k = 0; k < x.size(); ++k) {
    const Vec2 error = (T * x[k]) - z[k];
    error_sq += error.squaredNorm();
  }

  return error_sq;
}

Vec2 error(const Vec3 &eta, const std::vector<Vec2> &x, const std::vector<Vec2> &z) {
  const se2 T = se2::exp(eta);

  // double error_sq = 0.0;
  Vec2 error_sq = Vec2::Zero();
  for (std::size_t k = 0; k < x.size(); ++k) {
    const Vec2 error = (T * x[k]) - z[k];
    error_sq += error.array().square().matrix();
  }

  return error_sq;
}

struct dExpSe2 {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Eigen::Matrix2d Rprime;
  Eigen::Matrix2d V;
  Eigen::Matrix2d Vprime;
  Vec3            eta;

  Mat<2, 3> jacobian_from_y(const Vec2 &y) const {
    Mat<2, 3> m;
    m.setZero();
    m.block<2, 2>(0, 0) = V;
    m.block<2, 1>(0, 2) = (Rprime * y) + (Vprime * eta.head<2>());
    return m;
  }
};

void dexp_se2(const Vec3 &eta, Out<dExpSe2> jac) {
  // todo: Use taylor exp instead of explicit sincos for stability near zero
  const double c          = std::cos(eta(2));
  const double s          = std::sin(eta(2));
  const double inv_theta  = 1 / eta(2);
  const double inv_theta2 = inv_theta * inv_theta;

  const double cos_inv_theta  = c * inv_theta;
  const double sin_inv_theta  = s * inv_theta;
  const double sin_inv_theta2 = sin_inv_theta * inv_theta;

  const double cos_m1_invtheta2 = (c - 1) * inv_theta2;

  Eigen::Matrix2d v;
  v.row(0) << s, (c - 1);
  v.row(1) << 1 - c, s;
  v *= inv_theta;

  Eigen::Matrix2d dv_dtheta;
  dv_dtheta.row(0) << cos_inv_theta - (sin_inv_theta2), -sin_inv_theta - (cos_m1_invtheta2);
  dv_dtheta.row(1) << sin_inv_theta + cos_m1_invtheta2, cos_inv_theta - sin_inv_theta2;

  Eigen::Matrix2d dR_dtheta;
  dR_dtheta.row(0) << -s, -c;
  dR_dtheta.row(1) << c, -s;

  jac->Rprime = dR_dtheta;
  jac->V      = v;
  jac->Vprime = dv_dtheta;
  jac->eta    = eta;
}

Vec3 dcost(const Vec3 &eta, const std::vector<Vec2> &x, const std::vector<Vec2> &z) {
  const se2 T = se2::exp(eta);
  dExpSe2   preJ;
  dexp_se2(eta, out(preJ));

  Vec3 derror_sq_total = Vec3::Zero();
  for (std::size_t k = 0; k < x.size(); ++k) {
    const Vec2 error = (T * x[k]) - z[k];

    const Mat<2, 3> derror_deta = preJ.jacobian_from_y(x[k]);
    const Vec3 derror_sq = (2 * error.transpose()) * derror_deta;
    derror_sq_total += derror_sq;
  }

  return derror_sq_total;
}

template <int cols, typename Callable1, typename Callable2>
Eigen::Matrix<double, cols, 1> gauss_newton(const Eigen::Matrix<double, cols, 1> &x0,
                                            const Callable1 &fcn,
                                            const Callable2 &jac,
                                            const double     feps      = 1e-6,
                                            const int        max_iters = 15) {
  using VecX     = Eigen::Matrix<double, 1, cols>;
  using Gradient = Eigen::Matrix<double, cols, 1>;
  VecX x         = x0;

  double last_cost = fcn(x);
  for (int k = 0; k < max_iters; ++k) {
    // todo: llt
    const Gradient J = jac(x);
    const VecX     c = (J.transpose() * J).inverse() * J.transpose() * fcn(x);
    x                = x - c;
    double new_cost  = fcn(x);

    if (std::abs(new_cost - last_cost) < feps) {
      break;
    }

    last_cost = new_cost;
  }
  return x;
}
}


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#ifndef NDHIST_STATS_VAR_HPP_INCLUDED
#define NDHIST_STATS_VAR_HPP_INCLUDED 1

#include <boost/python.hpp>

#include <ndhist/ndhist.hpp>
#include <ndhist/stats/expectation.hpp>

namespace ndhist {
namespace stats {

namespace detail {

template <typename AxisValueType, typename WeightValueType>
double
calc_axis_var_impl(
    ndhist const & h
  , intptr_t const axis
)
{
    // Do the projection here, so it won't be done twice.
    ndhist const proj = (h.get_nd() == 1 ? h : h.project(bp::object(axis)));

    double expectation1 = calc_axis_expectation_impl<AxisValueType, WeightValueType>(proj, 1, axis);
    double expectation2 = calc_axis_expectation_impl<AxisValueType, WeightValueType>(proj, 2, axis);

    // V[x] = E[x^2] - E[x]^2
    return expectation2 - expectation1*expectation1;
}

}// namespace detail

namespace py {

/**
 * @brief Calculates the variance along the given axis of the given
 *     ndhist object. As in statistics, the variance is defined as
 *     :math:`V[x] = E[x^2] - E[x]^2`.
 *     This function generates a projection along the given axis and then
 *     calculates the variance.
 *     If None is given as axis, the variance for all individual axes of the
 *     ndhist object will be calculated and returned as a tuple. But if the
 *     dimensionality of the ndhist object is 1, a scalar value is returned.
 *
 * @note This function is only defined for ndhist objects with POD axis values
 *     AND POD weight values.
 */
boost::python::object
var(
    ndhist const & h
  , boost::python::object const & axis = boost::python::object()
);

}// namespace py

}// namespace stats
}// namespace ndhist

#endif // !NDHIST_STATS_VAR_HPP_INCLUDED


// neural_network.cpp example program using posit arithmetic for a neural network simulation
//
// Copyright (C) 2017-2020 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.

#define MTL_WITH_INITLIST 1
#include <boost/numeric/mtl/mtl.hpp>
// select number system
#include <universal/posit/posit>

// Turn it off for now
#undef USE_POSIT

template<size_t nbits, size_t es>
sw::unum::posit<nbits, es> Sigmoid(sw::unum::posit<nbits, es>& x, bool derivative = false) {
	if (derivative) return x*(1-x);
	return (1);
}

int main(int argc, char** argv)
try {
	using namespace std;
	using namespace mtl;
	using namespace sw::unum;

	const size_t nbits = 16;
	const size_t es = 1;
	const size_t vecSize = 32;

#ifdef USE_POSIT
	using Ty     = sw::unum::posit<8, 0>;
	using Matrix = mtl::dense2D< Ty >;
	using Vector = mtl::dense_vector< Ty >;
#else
	using Ty     = float;
	using Matrix = mtl::dense2D<float>;
	using Vector = mtl::dense_vector<float>;
#endif

#if defined(MTL_WITH_INITLIST) && defined(MTL_WITH_AUTO) && defined(MTL_WITH_RANGEDFOR)
	Matrix X = {
		{0,0,1},
		{0,1,1},
		{1,0,1},
		{1,1,1}
	};
#endif

	// output data
	Vector y = { 0.0f, 1.0f, 1.0f, 0.0f };

	cout << "Weights are    :\n" << X << endl;
	cout << "Training values: " << y << endl;
#if 0
	// synapse layers: syn0 = 2*np.random.random((3,4)) - 1
	Matrix synapse_L0(3,4);
	Matrix synapse_L1(4,1);

    constexpr int TRAINING_STEPS     = 50000;
    constexpr int REPORTING_INTERVAL = 10000;

	// training
	for ( int j = 0; j < TRAINING_STEPS; j++ ) {
		Matrix l0 = X;
		Matrix l1 = Sigmoid<nbits, es>(dot(l0, synapse_L0));
		Matrix l2 = Sigmoid<nbits, es>(dot(l1, synapse_L1));

		Vector l2_error = y - level_2;

		if (j % REPORTING_INTERVAL == 0) {
			cout << "Error: " << Mean(Abs(l2_error)) << '\n';
		}

		l2_delta = l2_error * Sigmoid<nbits, es>(l2, true);
		l1_error = l2_delta.dot(Transpose(synapse_L1));
		l1_delta = l1_error * Sigmoid<nbits, es>(l1, true);

		// update weights
		synapse_L1 += dot(Transpose(l1), l2_delta);
		synapse_L0 += dot(Transpose(l0), l1_delta);
	}

	cout << "Output after training\n";
	cout << l2 << endl;
#endif

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


#ifndef CGWR_HPP
#define CGWR_HPP

#include <iostream>
#include <cmath>
#include <algorithm>

//#include <boost/rational.hpp>
//#include <boost/math/special_functions/factorials.hpp>
//#include <boost/math/special_functions/powm1.hpp>

/*********************************************************************
- CG:
< j1 j2 m1 m2 | J M >

- Wigner 3j or 6j:
(j1,j2,j3) or {j1,j2,j3}
(l1,l2,l3)    {m1,m2,m3}

- Wigner 9j:
{j1 j2 j3}
{j4 j5 j6}
{j7 j8 j9}

-Racah:
W(j1 j2 l2 l1 | j3 l3)

CG translated as array=[j1,j2,m1,m2,J,M] with _CGWR::C_CG token
or
Wigner 3j translated as array=[j1,j2,j3,m1,m2,m3] with _CGWR::C_W3j token
or
Wigner 6j translated as array=[j1,j2,j3,l1,l2,l3] with _CGWR::C_W6j token
or
and so on with usual notations

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

class _CGWR { // Clebsch-Gordan Wigner Racah
public:

    // Token
    /***********/
    static const unsigned int C_CG;
    static const unsigned int C_W3j;
    static const unsigned int C_W6j;
    static const unsigned int C_W9j;
    static const unsigned int C_RACAH;
    /***********/

    // Init the class with the coeff and the token
    /***********/
    _CGWR(double long [], unsigned int);
    /***********/

    _CGWR& operator= ( const _CGWR& other ); //TODO

    // Public methods
    /***********/
    double long CG(void);
    double long W3j(void);
    double long W6j(void);
    double long W9j(void); //TODO
    double long Racah(void);
    /***********/

private:
    double long j1, j2, j3, j;
    double long l1, l2, l3, l;
    double long m1, m2 , m3, m;

    unsigned int selected_Symb;

    // mid computations
    /***********/
    long double fact ( long double n ); // factorial
    double long Delta(double long , double long , double long ); // triangle relation
    double long delta_k(double long , double long ); // kronecker

    // variadic
    template<typename _T>
    bool is_integer(_T n); // return true if integer
    template<typename _T, typename ..._args>
    bool is_integer(_T, _args...);

    template<typename _T>
    bool is_halfinteger(_T); // return true if half integer
    template<typename _T, typename ..._args>
    bool is_halfinteger(_T, _args...);
    /***********/

};

#endif // CGWR_HPP


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <boost/bind.hpp>
#include <boost/noncopyable.hpp>
#include "test_case.hpp"
#include <Eigen/Dense>
#include <Eigen/QR>
#include "metro/ModifiedNewtonRaphson.hpp"
#include "metro/ModifiedCholesky.hpp"
#include "metro/CholeskyStepper.hpp"

BOOST_AUTO_TEST_SUITE( test_modified_newton_raphson ) ;

namespace impl {
	typedef metro::SmoothFunction FunctionBase ;

	struct Multimodal1d_1: public FunctionBase {
		// This function f has two local maxima and one local minimum
		// f(x) = -x^4 + 2x^2
		// f'(x) = -4x^3 + 4x
		// f''(x) = -12 x^2 + 4
		// maxima are at \pm 1
		// minima is at 0
		void evaluate_at( Vector const& point, int const number_of_derivatives = 0 ) {
			assert( point.size() == 1 ) ;
			m_point = point ;
		}

		void evaluate( int const number_of_derivatives = 0 ) {}

		double get_value_of_function() const {
			assert( m_point.size() == 1 ) ;
			return -std::pow( m_point(0), 4 ) + 2 * std::pow( m_point(0), 2 ) ;
		}
		Eigen::VectorXd get_value_of_first_derivative() const {
			assert( m_point.size() == 1 ) ;
			return Eigen::VectorXd::Constant( 1, -4.0 * std::pow( m_point(0), 3 ) + 4 * m_point(0) ) ;
		}

		Eigen::MatrixXd get_value_of_second_derivative() const {
			return Eigen::MatrixXd::Constant(
				1, 1,
				-12.0 * std::pow( m_point(0), 2 ) + 4.0
			) ;
		}
		int number_of_parameters() const { return 1 ; }
		Vector const& parameters() const { return m_point ; }

		std::string get_summary() const { return "Multimodal1d_1" ; }
	private:
		Vector m_point ;
	} ;

	struct Multimodal2d_1: public FunctionBase {
		// This function f has four local maxima and one local minimum.
		// We should walk to one of the maxima depending on where we start.
		// For appropriately modified NR, we should never walk to the minimum.
		// f' = -0.1 x^4 - 0.1 y^4 + x^2 + y^2
		// This has a minimum at (0,0)
		// And maxima at nonzero solutions of
		// -0.4 x^3 - 2x = 0 and -0.4 y^3 + 2y = 0
		// (x,y) = (\pm sqrt(5), \pm sqrt(5))
		//
		// First derivative is
		//
		// f' = ( -0.4 x^3 + 2x, -0.4 y^3 + 2y )
		//
		// Diagonals of second derivative are:
		// f'' = -1.2 x^2 + 2, -1.2 y^2 + 2
		//
		// With zeroes on off-diagonals
		//
		void evaluate_at( Vector const& point, int const number_of_derivatives = 0 ) {
			assert( point.size() == 2 ) ;
			m_point = point ;
		}

		void evaluate( int const number_of_derivatives = 0 ) {}

		double get_value_of_function() const {
			return -0.1 * ( std::pow( m_point(0), 4 ) + std::pow( m_point(1), 4 ) )
				+ m_point(0) * m_point(0)
				+ m_point(1) * m_point(1)
			;
		}
		Eigen::VectorXd get_value_of_first_derivative() const {
			Eigen::VectorXd result ;
			result.setZero(2) ;
			result(0) = -0.4 * std::pow( m_point(0), 3 ) + 2.0 * m_point(0) ;
			result(1) = -0.4 * std::pow( m_point(1), 3 ) + 2.0 * m_point(1) ;
			return result ;
		}

		Eigen::MatrixXd get_value_of_second_derivative() const {
			Eigen::MatrixXd result ;
			result.setZero(2,2) ;
			result(0,0) = -1.2 * std::pow( m_point(0), 2 ) + 2.0 ;
			result(1,1) = -1.2 * std::pow( m_point(1), 2 ) + 2.0 ;
			return result ;
		}

		int number_of_parameters() const { return 2 ; }
		Vector const& parameters() const { return m_point ; }

		std::string get_summary() const { return "Multimodal2d_1" ; }

	private:
		Vector m_point ;
	} ;

	struct Rosenbrock: public FunctionBase {
		// Rosenbrock's function
		// has global minimum at (1,1)
		// f(x1,x2) = 100*(x2−x1^2)^2+(1−x1)^2
		// f'(x1,x2) = ( -4 * x1 * (x2−x1^2) -2 * x1,  )
		// f''(x1,x2) = ( -400*x1-2
		void evaluate_at( Vector const& point, int const number_of_derivatives = 0 ) {
			assert( point.size() == 2 ) ;
			m_point = point ;
		}
		double get_value_of_function() const {
			Vector const& x = m_point ;
			Vector x2 = m_point.array().square() ;
			return 100.0 * (x(1) - x2(0)) * (x(1) - x2(0)) + (1 - x(0)) * (1 - x(0) )  ;
		}

		Eigen::VectorXd get_value_of_first_derivative() const {
			assert(0) ;
		}

		Eigen::MatrixXd get_value_of_second_derivative() const {
			assert(0) ;
		}

		int number_of_parameters() const { return 2 ; }
		Vector const& parameters() const { return m_point ; }

		std::string get_summary() const { return "Rosenbrock" ; }

	private:
		Vector m_point ;
	} ;

	// The line search property of modified newton-raphson
	// should enable it to find a maximum even if we tell it to
	// jump way past it on each iteration.
	// This class heads for the maximum but goes 10 times too far.
	template< typename Function >
	struct OvershootTarget: public boost::noncopyable {
		OvershootTarget( FunctionBase::Vector target ):
			m_target( target )
		{}

		bool step( Function& function, FunctionBase::Vector* search_direction ) {
			// walk toward the target but go ten times too far.
			FunctionBase::Vector const& v = function.parameters() ;
			assert( v.size() == m_target.size() ) ;
			FunctionBase::Vector direction = m_target - v ;

			if( direction.array().abs().maxCoeff() < 1E-3 ) {
				return false ;
			} else {
				(*search_direction) = 10.0 * direction ;
				return true ;
			}
		}

	private:
		FunctionBase::Vector m_target ;
	} ;

	template< typename Function >
	struct StoppingCondition {
		StoppingCondition( double tolerance  = 1E-12 ):
			m_tolerance( tolerance ),
			m_iterations(0)
			{}
		bool operator()(
			Function const& function,
			Eigen::VectorXd const& step
		) {
			++m_iterations ;
			// Stop when all elements of the derivative are close to 0
			// and the step size is also close to zero.
			return
				function.get_value_of_first_derivative().array().abs().maxCoeff() < m_tolerance
				&& step.array().abs().maxCoeff() < m_tolerance ;
		}
		std::size_t iterations() const { return m_iterations ; }
	private:
		double m_tolerance ;
		std::size_t m_iterations ;
	} ;
}

AUTO_TEST_CASE( test_newton_direction_1d ) {
	typedef impl::FunctionBase::Vector Vector ;
	typedef impl::FunctionBase::Matrix Matrix ;
	impl::Multimodal1d_1 function ;
	Eigen::ColPivHouseholderQR< Matrix > solver ;
	double const one_third = 0.3333333333333333333333333 ;
	Vector point( 1 ) ;
	for( point(0) = -10; point(0) < 10; point(0) += 0.01 ) {
		// Check that the solver always finds an ascent direction
		function.evaluate_at( point ) ;
		solver.compute( function.get_value_of_second_derivative() ) ;
		Vector h = solver.solve( -function.get_value_of_first_derivative() ) ;
		double directional_derivative = function.get_value_of_first_derivative().transpose() * h ;
		// f''(x) = -12 x^2 + 4
		// so 2nd derivative is negative for x < sqtr
		BOOST_CHECK(
			( std::abs( point(0) ) > std::sqrt( one_third ) && directional_derivative > 0 )
				||
			( std::abs( point(0) ) < std::sqrt( one_third ) && directional_derivative < 0 )
		) ;
	}
}

namespace {
	template< typename Function >
	struct ModifiedCholeskySolver {
		typedef impl::FunctionBase::Vector Vector ;
		typedef impl::FunctionBase::Matrix Matrix ;

		Vector compute( Function& function, Vector const& point ) {
			function.evaluate_at( point ) ;
			m_solver.compute( -function.get_value_of_second_derivative() ) ;
			return m_solver.solve( function.get_value_of_first_derivative() ) ;
		} ;

	private:
		metro::ModifiedCholesky< Matrix > m_solver ;
	} ;
}

AUTO_TEST_CASE( test_cholesky_stepper_direction_1d ) {
	typedef impl::FunctionBase::Vector Vector ;
	typedef impl::FunctionBase::Matrix Matrix ;
	impl::Multimodal1d_1 function ;
	metro::CholeskyStepper stepper( 1E-12, 100 ) ;
#if 0
		[]( int iteration,
			double ll,
			double target_ll,
			metro::CholeskyStepper::Vector const& point,
			metro::CholeskyStepper::Vector const& first_derivative,
			metro::CholeskyStepper::Vector const& step,
			bool converged
		) {
			std::cerr
				<< " ITERATION:" << iteration << "\n"
				<< "        LL:" << ll << "\n"
				<< "    TARGET:" << target_ll << "\n"
				<< "     POINT:" << point.transpose() << "\n"
				<< "DERIVATIVE:" << first_derivative.transpose() << "\n"
				<< "      STEP:" << step.transpose() << "\n"
				<< " CONVERGED:" << ( converged ? "yes" : "no" ) << "\n" ;
		}
	 ) ;
#endif
	Vector point( 1 ) ;
	// start at -10.005 to avoid meeting maxima or minima
	for( point(0) = -10.005; point(0) < 10; point(0) += 0.01 ) {
		stepper.reset() ;
		// Check that the solver always finds an ascent direction
		function.evaluate_at( point ) ;
		Vector h ;
		BOOST_CHECK( stepper.step( function, &h ) ) ;
		double directional_derivative = function.get_value_of_first_derivative().transpose() * h ;
		//std::cerr << "x = " << point(0) << ", function = " << function.get_value_of_function()
		//	<< ", derivative = " << function.get_value_of_first_derivative().transpose() << ", "
		//	<< "DD = " << directional_derivative << ".\n" ;
		BOOST_CHECK_GT( directional_derivative, 0 ) ;
	}
}

AUTO_TEST_CASE( test_cholesky_stepper_direction_2d ) {
	typedef impl::FunctionBase::Vector Vector ;
	typedef impl::FunctionBase::Matrix Matrix ;
	impl::Multimodal2d_1 function ;
	metro::CholeskyStepper stepper( 1E-12, 100 ) ;

	Vector point( 2 ) ;
	// start at -10.01 so we do not reach 0 (where there is a maximum)
	for( point(0) = -10.01; point(0) < 10; point(0) += 0.025 ) {
		for( point(1) = -10.01; point(1) < 10; point(1) += 0.025 ) {
			stepper.reset() ;
			function.evaluate_at( point ) ;
			Vector h ;
			stepper.step( function, &h ) ;
			double const directional_derivative = function.get_value_of_first_derivative().transpose() * h ;
			BOOST_CHECK_GT( directional_derivative, 0 ) ;
		}
	}
}

AUTO_TEST_CASE( test_modified_ewton_raphson_overshoot_1d ) {
	typedef impl::FunctionBase::Vector Vector ;
	impl::Multimodal1d_1 function ;

	// For 1d function 1, maxima are at -1 and 1.
	Vector target ;
	target.setZero(1) ;
	target(0) = -1 ;

	{
		impl::OvershootTarget< impl::Multimodal1d_1 > overshoot( target ) ;
		Vector start = Vector::Constant( 1, -0.001 ) ;
		Vector result = metro::find_maximum_by_modified_newton_raphson_with_line_search(
			function,
			start,
			overshoot
		) ;

		BOOST_CHECK_CLOSE( result(0), -1, 0.1 ) ;
	}
}

AUTO_TEST_CASE( test_newton_raphson_overshoot_2d ) {
	typedef impl::FunctionBase::Vector Vector ;
	impl::Multimodal2d_1 function ;

	Vector maximum ;
	maximum.setZero(2) ;
	maximum(0) = -sqrt(5) ;
	maximum(1) = -sqrt(5) ;

	{
		impl::OvershootTarget< impl::Multimodal2d_1 > overshoot( maximum ) ;
		Vector start = Vector::Constant(2,-1) ;
		Vector result = metro::find_maximum_by_modified_newton_raphson_with_line_search(
			function,
			start,
			overshoot
		) ;

		BOOST_CHECK_CLOSE( result(0), -std::sqrt(5), 0.1 ) ;
		BOOST_CHECK_CLOSE( result(1), -std::sqrt(5), 0.1 ) ;
	}
}

AUTO_TEST_CASE( test_modified_cholesky_newton_raphson_2d ) {
	typedef impl::FunctionBase::Vector Vector ;
	impl::Multimodal2d_1 function ;

	{
		metro::CholeskyStepper solver( 1E-12, 100 ) ;
		Vector start = Vector::Constant(2,-1) ;
		Vector result = metro::find_maximum_by_modified_newton_raphson_with_line_search(
			function,
			start,
			solver
		) ;

		BOOST_CHECK_CLOSE( result(0), -std::sqrt(5), 0.1 ) ;
		BOOST_CHECK_CLOSE( result(1), -std::sqrt(5), 0.1 ) ;
		std::cerr << "test_newton_raphson_overshoot_2d: took " << solver.number_of_iterations() << " iterations.\n" ;
	}
	{
		metro::CholeskyStepper solver( 1E-12, 100 ) ;
		Vector start = Vector::Constant(2,-1) ;
		start(1) = -0.1 ;
		Vector result = metro::find_maximum_by_modified_newton_raphson_with_line_search(
			function,
			start,
			solver
		) ;

		BOOST_CHECK_CLOSE( result(0), -std::sqrt(5), 0.1 ) ;
		BOOST_CHECK_CLOSE( result(1), -std::sqrt(5), 0.1 ) ;
		std::cerr << "test_newton_raphson_overshoot_2d: took " << solver.number_of_iterations() << " iterations.\n" ;
	}
	{
		metro::CholeskyStepper solver( 1E-12, 100 ) ;
		Vector start = Vector::Constant(2,-1) ;
		start(0) = -100 ;
		start(1) = -0.1 ;
		Vector result = metro::find_maximum_by_modified_newton_raphson_with_line_search(
			function,
			start,
			solver
		) ;

		BOOST_CHECK_CLOSE( result(0), -std::sqrt(5), 0.1 ) ;
		BOOST_CHECK_CLOSE( result(1), -std::sqrt(5), 0.1 ) ;
		std::cerr << "test_newton_raphson_overshoot_2d: took " << solver.number_of_iterations() << " iterations.\n" ;
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Core>
#include <Eigen/LU>

#include "args.hxx"

#include <fstream>
#include <iostream>

using ChartType = Eigen::Matrix<double, 24, 3>;
using ChartHmType = Eigen::Matrix<double, 24, 4>;
using CCMType = Eigen::Matrix<double, 4, 3>;

std::vector<std::string> StringSplit(const std::string &str, char sep) {
    std::vector<std::string> v;
    std::stringstream ss(str);
    std::string buffer;
    while( std::getline(ss, buffer, sep) ) {
        v.push_back(buffer);
    }
    return v;
}

bool LoadColorchartCsv(const std::string& filename, ChartType& dst_chart) {
    std::ifstream ifs(filename);
    if (!ifs) {
        std::cout << "Can't open " << filename << std::endl;
        return false;
    }

    std::string str;
    std::getline(ifs, str); // skip first line

    int i = 0;
    while (std::getline(ifs, str)) {
        std::vector<std::string> data_str = StringSplit(str, ',');
        dst_chart(i, 0) = std::stod(data_str[1]);
        dst_chart(i, 1) = std::stod(data_str[2]);
        dst_chart(i, 2) = std::stod(data_str[3]);
        i++;
        if (i == 24) break;
    }
    return true;
}

void conv_sRGB2XYZ(const ChartType &rgb, ChartType &xyz) {
    Eigen::Matrix<double, 3, 3> M;
    M << 0.412391, 0.357584, 0.180481,
         0.212639, 0.715169, 0.072192,
         0.019331, 0.119195, 0.950532;
    xyz = (M * rgb.transpose()).transpose();
}

void SolveCCM(const ChartType& source_xyz, const ChartType& reference_xyz,
              CCMType &ccm) {
    // source_xyz * ccm == reference_xyz
    // (24, 3 + 1) * (4, 3) = (24 * 3)
    ChartHmType source_xyz_hm;
    source_xyz_hm.col(0) = source_xyz.col(0);
    source_xyz_hm.col(1) = source_xyz.col(1);
    source_xyz_hm.col(2) = source_xyz.col(2);
    source_xyz_hm.col(3) = Eigen::VectorXd::Ones(24);
    auto source_xyz_hm_t = source_xyz_hm.transpose();
    auto pinv = (source_xyz_hm_t * source_xyz_hm).inverse() * source_xyz_hm_t;
    ccm = pinv * reference_xyz;
}

void WriteCCM(const std::string &filename, const CCMType& ccm) {
    std::ofstream csv(filename);
    for (int i = 0; i < 4; i++) {
        csv << ccm(i, 0) << ","
            << ccm(i, 1) << ","
            << ccm(i, 2) << std::endl;
    }
}

bool ParseArgs(const int argc, char** argv, std::string& ref_csv,
               std::string& src_csv, std::string& out_csv, double& gamma) {
    args::ArgumentParser parser("Compute CCM.");
    args::HelpFlag help(parser, "help", "Display this help menu",
                        {'h', "help"});
    args::Positional<std::string> ref_csv_arg(parser, "reference",
                                              "reference csv file");
    args::Positional<std::string> src_csv_arg(parser, "source",
                                              "source csv file");
    args::Positional<std::string> out_csv_arg(parser, "output",
                                              "output csv file", "ccm.csv");
    args::ValueFlag<double> gamma_arg(parser, "gamma",
                                     "Gamma value of reference and source data",
                                     {'g', "gamma"}, 1.0);
    try {
        parser.ParseCLI(argc, argv);
    } catch (args::Help) {
        std::cout << parser;
        return false;
    } catch (args::ParseError e) {
        std::cerr << e.what() << std::endl;
        std::cerr << parser;
        return false;
    } catch (args::ValidationError e) {
        std::cerr << e.what() << std::endl;
        std::cerr << parser;
        return false;
    }
    if (!ref_csv_arg) {
        std::cout << "Error: reference is not set" << std::endl;
        std::cout << parser;
        return false;
    }
    if (!src_csv_arg) {
        std::cout << "Error: src is not set" << std::endl;
        std::cout << parser;
        return false;
    }

    ref_csv = args::get(ref_csv_arg);
    src_csv = args::get(src_csv_arg);
    out_csv = args::get(out_csv_arg);
    gamma = args::get(gamma_arg);

    return true;
}

int main(int argc, char** argv) {
    // Parse arguments
    std::string ref_csv, src_csv, out_csv;
    double gamma;
    if (!ParseArgs(argc, argv, ref_csv, src_csv, out_csv, gamma)) {
        return 1;
    }

    // Load color charts
    ChartType reference_raw, source_raw;
    if (!LoadColorchartCsv(ref_csv, reference_raw)) {
        return 1;
    }
    if (!LoadColorchartCsv(src_csv, source_raw)) {
        return 1;
    }

    // Degamma
    ChartType reference_linear = reference_raw.array().pow(gamma);
    ChartType source_linear = source_raw.array().pow(gamma);

    // XYZ
    ChartType reference_xyz, source_xyz;
    conv_sRGB2XYZ(reference_linear, reference_xyz);
    conv_sRGB2XYZ(source_linear, source_xyz);

    // Solve
    CCMType ccm;
    SolveCCM(source_xyz, reference_xyz, ccm);
    std::cout << "CCM:" << std::endl;
    std::cout << ccm << std::endl;

    // Save
    WriteCCM(out_csv, ccm);

    return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

// Based on Ellis, Daniel P.W. "Chroma feature analysis and synthesis"
// https://www.ee.columbia.edu/~dpwe/resources/matlab/chroma-ansyn/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class ChromaFilterBank
{
public:
  ChromaFilterBank(index maxBins, index maxFFT)
      : mFiltersStorage(maxBins, maxFFT / 2 + 1)
  {}

  void init(index nChroma, index nBins, double ref, double sampleRate)
  {
    using namespace Eigen;
    const double log2E = 1.44269504088896340736;
    index fftSize = 2 * (nBins - 1);
    ArrayXd freqs = ArrayXd::LinSpaced(fftSize, 0, sampleRate);
    freqs = nChroma * (freqs / (ref / 16)).log() * log2E;
    freqs[0] = freqs[1] - 1.5 * nChroma;

    ArrayXd widths =  ArrayXd::Ones(fftSize);
    widths.segment(0, fftSize - 1) =
        freqs.segment(1, fftSize - 1) -
        freqs.segment(0, fftSize - 1);
    widths = widths.max(1.0);
    ArrayXXd diffs =
      freqs.replicate(1, nChroma).transpose() -
      ArrayXd::LinSpaced(nChroma, 0, nChroma - 1).transpose().replicate(fftSize, 1).transpose();
    index halfChroma = std::lrint(nChroma / 2);
    ArrayXXd remainder =  diffs.unaryExpr([&](const double x){
        return std::fmod(x + 10* nChroma + halfChroma, nChroma) - halfChroma;
    });
    MatrixXd filters = (-0.5 * (2 * remainder / widths.replicate(1, nChroma).transpose()).square()).exp();
    filters = filters.block(0, 0, nChroma, nBins);
    filters.colwise().normalize();
    mFiltersStorage.setZero();
    mFiltersStorage.block(0, 0, nChroma, nBins) = filters;
    mNChroma = nChroma;
    mNBins = nBins;
    mScale = 2.0 / (fftSize * mNChroma);
    mSampleRate = sampleRate;
  }

  void processFrame(const RealVectorView in, RealVectorView out,
    double minFreq = 0, double maxFreq = -1, index normalize = 0)
  {
    using namespace Eigen;
    using namespace std;
    ArrayXd frame = _impl::asEigen<Eigen::Array>(in);
    Eigen::Ref<Eigen::MatrixXd> filters = mFiltersStorage.block(0, 0, mNChroma, mNBins);

    if(minFreq != 0 || maxFreq != -1){
        maxFreq = (maxFreq == -1) ? (mSampleRate / 2) : min(maxFreq, mSampleRate / 2);
        double  binHz = mSampleRate / ((mNBins - 1) * 2.);
        index   minBin = minFreq == 0? 0 : ceil(minFreq / binHz);
        index   maxBin =
            min(static_cast<index>(floorl(maxFreq / binHz)), (mNBins - 1));
        frame.segment(0, minBin).setZero();
        frame.segment(maxBin, frame.size() - maxBin).setZero();
    }

    ArrayXd result = mScale * (filters * frame.square().matrix()).array();

    if (normalize > 0) {
      double norm = normalize == 1? result.sum() : result.maxCoeff();
      result = result / std::max(norm, epsilon);
    }
    out = _impl::asFluid(result);
  }

  index mNChroma;
  index mNBins;
  double mScale;
  double mSampleRate;
  Eigen::MatrixXd mFiltersStorage;
};
} // namespace algorithm
} // namespace fluid


/**
 * Contains various primitive datatypes for SISL, most of which derive from
 * Eigen datatypes.
 *
 * @author Joshua Horacsek
 */

#ifndef _SISL_PRIMITIVE_H_
#define _SISL_PRIMITIVE_H_

#include <Eigen/Dense>

// By default, SISL will do all floating point arithmetic in double precision
#ifndef sisl_float
    #define sisl_float double
#endif

namespace sisl {

    // Basic primitive datatypes
    //! dynamically sized (mathematical) vector datatype
    using vector       = Eigen::Matrix<sisl_float, Eigen::Dynamic, 1>;
    //! dynamically size point datatype
    using point        = Eigen::Matrix<sisl_float, Eigen::Dynamic, 1>;
    //! Dynamically sized transform (i.e. matrix)
    using transform    = Eigen::Matrix<sisl_float, Eigen::Dynamic, Eigen::Dynamic>;

    //! A fixed size 3-vector for holding colours
    using rgb_color    = Eigen::Matrix<sisl_float, 3, 1>;
    //! A fixed size 4-vector for holding colours and an alpha channel
    using rgba_color   = Eigen::Matrix<sisl_float, 4, 1>;
    //! Lattice_site are integer tuples (vectors)
    using lattice_site = Eigen::Matrix<int, Eigen::Dynamic, 1>;
    //! integer tuples (vectors)
    using int_tuple    = Eigen::Matrix<int, Eigen::Dynamic, 1>;

    /*! The various datatypes SISL can read, mainly used in the raw filetype reader */
    enum e_datatype {
        SDT_UNKNOWN = -1,
        SDT_UINT8 = 0,
        SDT_UINT16 = 1,
        SDT_UINT32 = 2,
        SDT_FLOAT = 3,
        SDT_DOUBLE = 4
    };

    /*! \brief 3D plane class.
     *
     * A plain plane class.
     */
    struct plane {
        vector     n; //!< normal direction
        sisl_float d; //!< normal offset

        /*! Plane constructor
         * \param _n Input normal direction (note, this will get normalized on assignment).
         * \param _d Input normal offset.
         */
        plane(const vector &_n, const sisl_float &_d) : n(_n), d(_d) { }
        /*! Plane copy constructor
         * \param _p A plane to copy
         */
        plane(const plane &_p) : n(_p.n), d(_p.d) { }
    };

    /*! \brief 3D vertex.
     *
     * A vertex in the graphics sense, i.e. a spatial point, and a normal to help shading
     * heuristics.
     */
    struct vertex3 {
        vector p; //!< vertex spatial location
        vector n; //!< vertex normal direction

        /*! Vertex constructor
         * \param _p Spatial location of a point.
         * \param _n Point normal.
         */
        vertex3(const vector &_p, const vector &_n) : p(_p), n(_n) { }

        /*! Vertex constructor
         * \param x,y,z Spatial location of a point.
         * \param i,j,k Point normal.
         */
        vertex3(
            const double &x, const double &y, const double &z,
            const double &i, const double &j, const double &k) : p(vector(3)), n(vector(3)) {
            p << x,y,z;
            n << i,j,k;
        }
        /*! Vertex copy constructor
         * \param _v A vertex to copy
         */
        vertex3(const vertex3 &_v) : p(_v.p), n(_v.n) { }
    };

    /** \brief Triangle in 3D space.
     *
     * A collection of three points that creates a ploygon in space
     */
    struct triangle {
        vertex3 v1; //!< triangle vertex 1
        vertex3 v2; //!< triangle vertex 2
        vertex3 v3; //!< triangle vertex 3

        /*! Ray constructor
         * \param _v1,_v2,_v3 The vertices that make up a triangle.
         */
        triangle(const vertex3 &_v1,
                 const vertex3 &_v2,
                 const vertex3 &_v3) : v1(_v1), v2(_v2), v3(_v3) { }
        /*! Plane copy constructor
         * \param _p A plane to copy
         */
        triangle(const triangle &_t) : v1(_t.v1), v2(_t.v2), v3(_t.v3) { }
    };

    /*! \brief A ray in space.
     *
     * A ray class, i.e. a half line.
     */
    struct ray {
        point  o; //!< ray origin
        vector d; //!< ray direction

        /*! Ray constructor
         * \param _o Input ray origin.
         * \param _d Input ray direction.
         */
        ray(const point &_o, const vector &_d) : o(_o), d(_d.normalized()) { }

        /*! Ray copy constructor
         * \param _r Ray to copy
         */
        ray(const ray &_r) : o(_r.o), d(_r.d.normalized()) { }

        /*! Returns the point at a distinance t away from the ray,
         *  travelling in the direction of the ray.
         * \param t Distance along ray from origin
         */
        point at(const sisl_float &t) {return o+t*d;}
    };
}
#endif // _SISL_PRIMITIVE_H_


#define BOOST_TEST_MODULE Exponentiation test
#include <boost/test/unit_test.hpp>

#include "Variable.h"
#include "Sum.h"
using namespace omnn::math;
using namespace boost::unit_test;

BOOST_AUTO_TEST_CASE(Exponentiation_tests)
{
    auto a = 2_v ^ 3;
    BOOST_TEST(a==8);
    a = 2_v ^ 11;
    BOOST_TEST(a==(1<<11));
    Variable x;
    auto _1 = (x-2)^3;
    auto _2 = (x-2)*(x-2)*(x-2);
    BOOST_TEST(_1==_2);
    _1 = -32;
    _1 ^= .5;
    _2 = 4;
    _2 *= -2_v^(1_v/2);
    BOOST_TEST(_1==_2);
    _1 = -1_v*((-4_v)^(1_v/2))*((-16_v)^((1_v/2)));
    _2 = 8;
    BOOST_TEST(_1==_2);

    _1 = (((x-2)^2)+((x+2)^2));
    _1 = _1(x);
    _2 = ((-1)^(1_v/2))*2;
    BOOST_TEST(_1==_2);
}

BOOST_AUTO_TEST_CASE(Compare_test)
{
    auto _1 = 25 ^ Fraction{1_v,-2};
    auto _2 = (1_v / 5)*(1_v^(1_v/2));
    if (_1.IsProduct())
        for(auto&a:_1.as<Product>())
            std::cout << a.Hash() << std::endl;
    if (_2.IsProduct())
        for(auto&a:_2.as<Product>())
            std::cout << a.Hash() << std::endl;
    auto c = _1 == _2;
    BOOST_TEST(c);

    DECL_VA(x);
    _1 =(-1_v)^x;
    _2 = (-1_v)^((1_v/2)*x + _1/4 + ((-1_v)/4));
    c = _1 != _2;
    BOOST_TEST(c);
}

BOOST_AUTO_TEST_CASE(Sqrt_test)
{
    auto a = 25_v;
    auto e = a ^ (1_v/2);
    auto c = e == (1_v^(1_v/2))*5;
    BOOST_TEST(c);
    BOOST_TEST(e != 5);
    // https://math.stackexchange.com/questions/41784/convert-any-number-to-positive-how/41787#comment5776496_41787
    e = a.Sqrt();
    BOOST_TEST(e == 5);
}

BOOST_AUTO_TEST_CASE(Polynomial_Sqrt_test
                     ,*disabled()
                     )
{
    Variable x,y;
    auto a = 4*(x^2)-12*x*y+9*(y^2);
    auto e = a.Sqrt();
    BOOST_TEST(e == 2*x-3*y);
}

BOOST_AUTO_TEST_CASE(Polynomial_Exp_test
                     ,*disabled()
                     )
{
    auto v = "v"_va;
    auto a = "a"_va;
    auto b = "b"_va;

    auto _ = (4_v*(v^2) + 2048)^(1_v/2);
    _.SetView(Valuable::View::Solving);
    _.optimize();

    auto t=4_v;
    auto c=2048_v;
    auto _2ab=2_v*a*b;
    auto d = (-_2ab).sq() + 4_v*(t-a.Sq())*(b.Sq()-c);
    auto x = (_2ab+(d^(1_v/2)))/(2_v*(t-a.Sq()));
    _.eval({{v,x}});
    auto ab = 0_v;
    _.eval({{b,ab}});

    auto& ebs = _.as<Exponentiation>().getBase().as<Sum>();
    for(auto& m: ebs){
//        if (m.) {
//            <#statements#>
//        }
    }
    auto it = ebs.begin();
    auto m1 = *it;
    for(auto& v: it->getCommonVars())
        std::cout << v.first.str() << '^' << v.second.str() << std::endl;
    auto m2 =*++it;
    auto ms = m1+m2;
//    Valuable f = Fraction{

//    auto av = _(a);
    _.optimize();

    {
    DECL_VA(x);
    auto _ = ((-18_v*(x^2) + 108*x -180)^((1/3)));
    _.SetView(Valuable::View::Equation);
    _.optimize();
    }
}

// TODO : check with mathematicians which way is correct, fix te test and enable it
BOOST_AUTO_TEST_CASE(Multival_test
                    ,*disabled()
                    )
{
    auto a = 1_v ^ (1_v/2);
    BOOST_TEST(!!a.IsMultival() == true);

    DECL_VA(x);
    BOOST_TEST(!!x.IsMultival() == true);
    a = x / x;

    // Waht is the value of a?
    // first impression is that x/x is allways 1
    // what about multivalue expressions like this?
    // it equals both values by disjunction: either -1 or 1 at the same time
    // this would be a circle function on image
    // the division of multiples we could apply in an alternative way:
    // permutations of the operation on possible values:
    // [-1/1, 1/1, 1/-1, -1/-1]  => [1, -1] != 1
    // so this way the answer should remain multival 1^(1/2)
    // 1^(1/2) / 1^(1/2) == 1^(1/2)
    // TODO : check with mathematicians which way is correct, fix te test and enable it
    BOOST_TEST(a != 1);

    BOOST_TEST(!!a.IsMultival() == true);
    BOOST_TEST(a == x/x);
}


#include <boost/test/unit_test.hpp>

#include "arithmetique_modulaire.h"

BOOST_AUTO_TEST_SUITE(test_arithmetique_modulaire)

    struct fixure_arithmetiques {
        std::vector<size_t> premiers;

        explicit fixure_arithmetiques() {
            premiers = std::vector<size_t>{2, 3, 5, 7, 11, 13, 17, 19, 23, 29,
                                           31, 37, 41, 43, 47, 53, 59, 61, 67,
                                           71, 73, 79, 83, 89, 97};
        }

        ~fixure_arithmetiques() = default;
    };

    BOOST_FIXTURE_TEST_CASE(inverse1, fixure_arithmetiques) {
        BOOST_CHECK_EQUAL(arithmetique_modulaire::inverse_modulaire<unsigned long>(3, 11, premiers), 4);
        BOOST_CHECK_EQUAL(arithmetique_modulaire::inverse_modulaire<unsigned long long>(97643, 456753, premiers),
                          368123);
        BOOST_CHECK_EQUAL(
                arithmetique_modulaire::inverse_modulaire<unsigned long long>(107113, 3246999210ULL, premiers),
                180730717ULL);
    }

    BOOST_FIXTURE_TEST_CASE(inverse2, fixure_arithmetiques) {
        BOOST_CHECK_EQUAL(arithmetique_modulaire::inverse_modulaire<long>(3, 11), 4);
        BOOST_CHECK_EQUAL(arithmetique_modulaire::inverse_modulaire<long long>(97643, 456753), 368123);
        BOOST_CHECK_EQUAL(arithmetique_modulaire::inverse_modulaire<long long>(107113, 3246999210LL),
                          180730717LL);
    }

    BOOST_FIXTURE_TEST_CASE(chinois1, fixure_arithmetiques) {
        std::vector<unsigned long> modulos{3, 5, 7};
        std::vector<unsigned long> restes{2, 3, 2};

        auto reste = arithmetique_modulaire::restes_chinois<unsigned long>(modulos, restes, premiers);
        BOOST_CHECK_EQUAL(reste, 23);
    }

    BOOST_FIXTURE_TEST_CASE(chinois2, fixure_arithmetiques) {
        std::vector<unsigned long> modulos{3, 4, 5};
        std::vector<unsigned long> restes{2, 3, 1};

        auto reste = arithmetique_modulaire::restes_chinois<unsigned long>(modulos, restes, premiers);
        BOOST_CHECK_EQUAL(reste, 11);
    }

    BOOST_FIXTURE_TEST_CASE(chinois3, fixure_arithmetiques) {
        std::vector<long> modulos{3, 5, 7};
        std::vector<long> restes{2, 3, 2};

        auto reste = arithmetique_modulaire::restes_chinois<long>(modulos, restes);
        BOOST_CHECK_EQUAL(reste, 23);
    }

    BOOST_FIXTURE_TEST_CASE(chinois4, fixure_arithmetiques) {
        std::vector<long> modulos{3, 4, 5};
        std::vector<long> restes{2, 3, 1};

        auto reste = arithmetique_modulaire::restes_chinois<long>(modulos, restes);
        BOOST_CHECK_EQUAL(reste, 11);
    }

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

double* diff_ts(double *y, int len, double dt)
{
  double *dy;
  dy = new double[len];

  dy[0] = y[0];
  for (int i = 1; i<len; i++)
  {
    dy[i] = (y[i] - y[i-1])/dt;
  }
  return dy;
}

mat diffMat( int len, int DIM, double dt, mat y)
{
  mat dy(DIM, len) ;
  for (int d = 0; d<DIM; d++)
  {
    for (int i = 1; i<len; i++)
    {
      dy(d,i) = (y(d,i) - y(d,i-1))/dt;
    }
    dy(d,0) = dy(d,1);

  }
  return dy;

}


#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include "../include/layer.h"

using namespace Eigen;

int main()
{
    using std::cout;
    using std::endl;
    using std::vector;
    using namespace MyDL;

    int n = 1;
    int c = 1;
    int h = 2;
    int w = 2;
    int Fn = 2;
    int Fh = 2;
    int Fw = 2;
    int stride = 1;
    int pad = 1;

    Conv2D conv(c, h, w, Fn, Fh, Fw, stride, pad, 0.1);

    vector<MatrixXd> inputs, outs;
    MatrixXd X = MatrixXd::Zero(n, c*h*w);
    X << 1,2,3,4;
    // X << 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16;

    inputs.push_back(X);
    outs = conv.forward(inputs);

    cout << outs[0] << endl;

    // ----------------------
    // col2im
    // ----------------------

    int Oh = (2*pad+h - Fh) / stride + 1;
    int Ow = (2*pad+w - Fw) / stride + 1;

    MatrixXd col = MatrixXd::Ones(n*Oh*Ow,c*Fh*Fw);
    MatrixXd img;

    conv.col2im(col, img);

    cout << img << endl;

    // MatrixXd pad_img, img;

    // conv.col2im(col, pad_img);

    // cout << pad_img << endl;

    // conv.suppress(pad_img, img);

    // cout << img << endl;

    vector<MatrixXd> grads, douts;

    douts.push_back(MatrixXd::Ones(n, Fn*Oh*Ow));

    grads = conv.backward(douts);

    cout << grads[0] << endl;

    return 0;
}


/* Copyright (C) 2018-2019 Thomas Jespersen, TKJ Electronics. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the MIT License
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the MIT License for further details.
 *
 * Contact information
 * ------------------------------------------
 * Thomas Jespersen, TKJ Electronics
 * Web      :  http://www.tkjelectronics.dk
 * e-mail   :  thomasj@tkjelectronics.dk
 * ------------------------------------------
 */

#include <signal.h>
#include <condition_variable>
#include <stdlib.h>
#include <stdio.h>

#include <boost/thread.hpp>
#include <boost/chrono.hpp>
#include <boost/format.hpp>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "omp.h"

#include <iostream>
#include <cstring>
#include <string>

#include <boost/math/quaternion.hpp> // see https://www.boost.org/doc/libs/1_66_0/libs/math/doc/html/quaternions.html

void QuaternionIntegration_ForwardEuler(
        double dt,
        double omeg_x, double omeg_y, double omeg_z,
        double& q0, double& q1, double& q2, double& q3);
void QuaternionIntegration_Exponential(
        double dt,
        double omeg_x, double omeg_y, double omeg_z,
        double& q0, double& q1, double& q2, double& q3);
void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3);

#define deg2rad(x) (M_PI*x/180.f)
#define rad2deg(x) (180.f*x/M_PI)

int main(int argc, char** argv ) {
    std::string argv_str(realpath(argv[0], 0));
    std::string base = argv_str.substr(0, argv_str.find_last_of("/"));

    double angularVelocity[3] = {0.3, 0.2, 0.1};
    double dt = 1.0/200;

    // Create initial quaternion for testing - using Boost Quaternions: https://www.boost.org/doc/libs/1_66_0/libs/math/doc/html/quaternions.html
    boost::math::quaternion<double> q0(0.9848,0.1736,0,0); // w,x,y,z
    q0 = q0 / sqrt(boost::math::norm(q0)); // normalize

    // Convert angular velocity vector into a quaternion
    boost::math::quaternion<double> q_omeg(0, angularVelocity[0], angularVelocity[1], angularVelocity[2]);   // use as body angular velocity

    std::cout << "Quaternion are printed in order: \t(w, x, y, z)" << std::endl << std::endl;

    /* Inertial vs Body angular velocity test */
    std::cout << ">>>>> Inertial vs Body angular velocity test <<<<<" << std::endl;
    boost::math::quaternion<double> q_body = q0 * boost::math::exp(0.5*dt*q_omeg); // compute the new quaternion using the quaternion exponential
    boost::math::quaternion<double> q_inertial = boost::math::exp(0.5*dt*q_omeg) * q0; // compute the new quaternion using the quaternion exponential

    // Print results for comparison
    std::cout << "q0 \t\t=\t" << q0 << std::endl;
    std::cout << "q_omeg \t\t=\t" << q_omeg << std::endl;
    std::cout << "q_body \t\t=\t" << q_body << std::endl;
    std::cout << "q_inertial \t=\t" << q_inertial << std::endl;

    std::cout << std::endl;

    /* Integration method test - methods are inspired from https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/  */
    std::cout << ">>>>> Integration method test <<<<<" << std::endl;
    // Method 1: Quaternion Exponential
    boost::math::quaternion<double> q1_exp = q0 * boost::math::exp(0.5*dt*q_omeg);

    // Method 2: Manual Quaternion Exponential
    boost::math::quaternion<double> q_exp_delta;
    if (norm(q_omeg) > 0)
        q_exp_delta = cos(0.5 * dt * sqrt(norm(q_omeg))) + q_omeg / sqrt(norm(q_omeg)) * sin(0.5 * dt * sqrt(norm(q_omeg)));
    else
        q_exp_delta = boost::math::quaternion<double>(1, 0, 0, 0); // unit quaternion since the angular velocity is zero (no movement)

    boost::math::quaternion<double> q2_exp_manual = q0 * q_exp_delta;

    // Method 3: Manual Quaternion Exponential
    double q_1[4] = {q0.R_component_1(), q0.R_component_2(), q0.R_component_3(), q0.R_component_4()};
    QuaternionIntegration_Exponential(dt, angularVelocity[0],angularVelocity[1],angularVelocity[2], q_1[0],q_1[1],q_1[2],q_1[3]);
    boost::math::quaternion<double> q3_exp_manual(q_1[0],q_1[1],q_1[2],q_1[3]);

    // Method 4: Delta Quaternion
    boost::math::quaternion<double> q_delta(1, 0.5*dt*angularVelocity[0], 0.5*dt*angularVelocity[1], 0.5*dt*angularVelocity[2]);
    boost::math::quaternion<double> q4_delta = q0 * q_delta; // compute the new quaternion using body angular velocity to construct the delta quaternion (hence right multiplication)

    // Method 5: Forward Euler
    boost::math::quaternion<double> dq = 0.5 * q0 * q_omeg; // quaternion derivative using body angular velocity
    //boost::math::quaternion<double> dq = 0.5 * q_omeg * q0; // quaternion derivative using inertial angular velocity
    boost::math::quaternion<double> q5_euler = q0 + dt*dq;
    q5_euler = q5_euler / sqrt(boost::math::norm(q5_euler)); // normalize

    // Method 6: Forward Euler 2
    double q_2[4] = {q0.R_component_1(), q0.R_component_2(), q0.R_component_3(), q0.R_component_4()};
    QuaternionIntegration_ForwardEuler(dt, angularVelocity[0],angularVelocity[1],angularVelocity[2], q_2[0],q_2[1],q_2[2],q_2[3]);
    boost::math::quaternion<double> q6_euler(q_2[0],q_2[1],q_2[2],q_2[3]);

    // Print results for comparison
    std::cout << "Angular Velocity \t=\t(" << angularVelocity[0] << ", " << angularVelocity[1] << ", " << angularVelocity[2] << ")" << std::endl;
    std::cout << "Boost Exponential \t=\t" << q1_exp << std::endl;
    std::cout << "Manual Exponential \t=\t" << q2_exp_manual << std::endl;
    std::cout << "Delta \t\t\t=\t" << q4_delta << std::endl;
    std::cout << "Forward Euler \t\t=\t" << q5_euler << std::endl;
    std::cout << "Manual Exponential my \t=\t" << q3_exp_manual << std::endl;
    std::cout << "Forward Euler my \t=\t" << q6_euler << std::endl;

    return 0;
}

void QuaternionIntegration_ForwardEuler(
        double dt,
        double omeg_x, double omeg_y, double omeg_z,
        double& q0, double& q1, double& q2, double& q3)
{
    // The angular velocity is given in body frame
    /* Forward Euler method
     * q_new = q + dt * dq
     * dq = 0.5 * q0 * q_omeg
     * q_omeg = [0,omeg_x,omeg_y,omeg_z]
     */

    // dq = 0.5*Phi(q0)*[0,omeg_x,omeg_y,omeg_z]
    double dq0 = 0.5 * (-q1*omeg_x -q2*omeg_y -q3*omeg_z);
    double dq1 = 0.5 * (+q0*omeg_x -q3*omeg_y +q2*omeg_z);
    double dq2 = 0.5 * (+q3*omeg_x +q0*omeg_y -q1*omeg_z);
    double dq3 = 0.5 * (-q2*omeg_x +q1*omeg_y +q0*omeg_z);

    q0 += dt*dq0;
    q1 += dt*dq1;
    q2 += dt*dq2;
    q3 += dt*dq3;

    normalizeQuaternion(q0, q1, q2, q3);
}

void QuaternionIntegration_Exponential(
        double dt,
        double omeg_x, double omeg_y, double omeg_z,
        double& q0, double& q1, double& q2, double& q3)
{
    // The angular velocity is given in body frame
    /* Quaternion Exponential method
     * q_new = q0 * exp(1/2*dt*q_omeg)
     * q_omeg = [0,omeg_x,omeg_y,omeg_z]
     */

    double omeg_norm = sqrt(omeg_x*omeg_x + omeg_y*omeg_y + omeg_z*omeg_z);

    double q_exp0, q_exp1, q_exp2, q_exp3;

    if (omeg_norm > 0) {
        double sinOmeg = sin(0.5 * dt * omeg_norm);
        q_exp0 = cos(0.5 * dt * omeg_norm);
        q_exp1 = sinOmeg * omeg_x / omeg_norm;
        q_exp2 = sinOmeg * omeg_y / omeg_norm;
        q_exp3 = sinOmeg * omeg_z / omeg_norm;
    } else {
        // unit quaternion since the angular velocity is zero (no movement)
        q_exp0 = 1.0;
        q_exp1 = 0.0;
        q_exp2 = 0.0;
        q_exp3 = 0.0;
    }

    // q_new = Phi(q0) * q_exp
    double q0_new = +q0*q_exp0 -q1*q_exp1 -q2*q_exp2 -q3*q_exp3;
    double q1_new = +q1*q_exp0 +q0*q_exp1 -q3*q_exp2 +q2*q_exp3;
    double q2_new = +q2*q_exp0 +q3*q_exp1 +q0*q_exp2 -q1*q_exp3;
    double q3_new = +q3*q_exp0 -q2*q_exp1 +q1*q_exp2 +q0*q_exp3;

    // Update quaternion
    q0 = q0_new;
    q1 = q1_new;
    q2 = q2_new;
    q3 = q3_new;
}

void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3)
{
    double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 /= norm;
    q1 /= norm;
    q2 /= norm;
    q3 /= norm;
}


#include <gtest/gtest.h>
#include <boost/lexical_cast.hpp>
#include "quadrature/qhermite.hpp"
#include "spectral/polar_to_nodal.hpp"
#include "spectral/p2n_factory.hpp"

using namespace std;
using namespace boltzmann;

// Polar-Laguerre coefficients with exponential decay
void test2(int K, const std::function<double(double)>& cfct, double tol)
{
  typedef Eigen::VectorXd vec_t;

  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;
  SpectralBasisFactoryKS::create(polar_basis, K, K, 2, true);
  SpectralBasisFactoryKS::write_basis_descriptor(polar_basis, "spectral_basis.desc");

  Polar2Nodal<polar_basis_t> p2n;
  p2n.init(polar_basis, 1.0);

  unsigned int N = polar_basis.n_dofs();
  Eigen::VectorXd cp(N);
  for (unsigned int i = 0; i < N; ++i) {
    // cp(i) = cfct(float(i)/N);
    cp(i) = 1;
  }
  Eigen::MatrixXd cn(K, K);
  p2n.to_nodal(cn, cp);
  Eigen::VectorXd cp2(N);
  p2n.to_polar(cp2, cn);

  Eigen::VectorXd diff(N);
  diff.setZero();

  for (unsigned int i = 0; i < N; ++i) {
    diff(i) = std::abs(cp2(i) - cp(i));
  }

  double max_diff = diff.cwiseAbs().maxCoeff();
  cout << "max_diff: " << max_diff << "\n";
  EXPECT_TRUE(max_diff < tol) << max_diff;
}

// Polar-Laguerre coefficients with exponential decay
void test3(int K, const std::function<double(double)>& cfct, double tol)
{
  typedef Eigen::VectorXd vec_t;

  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;
  SpectralBasisFactoryKS::create(polar_basis, K, K, 2, true);
  SpectralBasisFactoryKS::write_basis_descriptor(polar_basis, "spectral_basis.desc");

  const auto& p2n = P2NFactory<polar_basis_t>::GetInstance(polar_basis, 1.0);

  // Polar2Nodal<polar_basis_t> p2n;
  // p2n.init(polar_basis, 1.0);

  unsigned int N = polar_basis.n_dofs();
  Eigen::VectorXd cp(N);
  for (unsigned int i = 0; i < N; ++i) {
    // cp(i) = cfct(float(i)/N);
    cp(i) = 1;
  }
  Eigen::MatrixXd cn(K, K);
  p2n.to_nodal(cn, cp);
  Eigen::VectorXd cp2(N);
  p2n.to_polar(cp2, cn);

  Eigen::VectorXd diff(N);
  diff.setZero();

  for (unsigned int i = 0; i < N; ++i) {
    diff(i) = std::abs(cp2(i) - cp(i));
  }

  double max_diff = diff.cwiseAbs().maxCoeff();
  cout << "max_diff: " << max_diff << "\n";
  EXPECT_TRUE(max_diff < tol) << max_diff;
}

TEST(spectral, polar2nodal)
{
  const double tol = 1e-13;
  cout << "Tolerance: " << tol << "\n";
  std::vector<int> Ks = {6, 10, 20, 30, 40, 50, 70};
  for (auto K : Ks) {
    cout << "running test for K=" << boost::lexical_cast<string>(K) << "\n";
    // test2(K, [](double i){return std::exp(-1*i);}, tol);
    test2(K, [](double i) { return 1; }, tol);
  }
}


/**
 * @file quasiinterpolation_main.cc
 * @brief NPDE exam TEMPLATE MAIN FILE
 * @author Oliver Rietmann
 * @date 15.07.2020
 * @copyright Developed at SAM, ETH Zurich
 */

#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>

#include "iohelper.h"
#include "quasiinterpolation.h"

constexpr double PI = 3.14159265358979323846;
// A bump function
double u1(Eigen::Vector2d x) {
  double abs_x = x.norm();
  return abs_x < 0.5 ? std::cos(PI * abs_x) : 0.0;
};
// The gradient of the bump function
Eigen::Vector2d grad_u1(Eigen::Vector2d x) {
  double abs_x = x.norm();
  if (abs_x == 0.0 || abs_x >= 0.5)
    return Eigen::Vector2d::Zero();
  else
    return -PI * std::sin(PI * abs_x) / abs_x * x;
};

// A simplex auxiliary function
inline double square(double y) { return y * y; }

// A smoother bump function
double u2(Eigen::Vector2d x) { return square(u1(x)); };
// ... and its gradient
Eigen::Vector2d grad_u2(Eigen::Vector2d x) { return 2.0 * u1(x) * grad_u1(x); };

int main(int /* argc */, char** /*argv*/) {
  // Build mesh hierarchy
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory),
                            CURRENT_SOURCE_DIR "/../meshes/mesh.msh");
  std::shared_ptr<lf::mesh::Mesh> base_mesh_p{reader.mesh()};
  std::shared_ptr<lf::refinement::MeshHierarchy> mesh_hierarchy_p =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(base_mesh_p, 6);

  // Compute meshwidth for each refinement
  int L = mesh_hierarchy_p->NumLevels();
  Eigen::VectorXd meshwidth(L);
  for (int k = 0; k < L; ++k) {
    std::shared_ptr<const lf::mesh::Mesh> mesh_p = mesh_hierarchy_p->getMesh(k);
    auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
    meshwidth(k) = QuasiInterpolation::maxLength(mesh_p->Entities(1));
  }
  {
    // Errors for u1
    lf::mesh::utils::MeshFunctionGlobal u1_mf(u1);
    lf::mesh::utils::MeshFunctionGlobal grad_u1_mf(grad_u1);
    // Retrieve error norms
    auto [l2_error, h1_error] = QuasiInterpolation::interpolationError(
        mesh_hierarchy_p, u1_mf, grad_u1_mf);
    QuasiInterpolation::printError(meshwidth, l2_error, h1_error,
                                   "errors for u1");
    QuasiInterpolation::writeCSV(meshwidth, l2_error, h1_error,
                                 CURRENT_BINARY_DIR "/convergence_u1.csv");
    // Call a Python script to generate plots
    std::system("python3 " CURRENT_SOURCE_DIR
                "/plot_convergence.py " CURRENT_BINARY_DIR
                "/convergence_u1.csv " CURRENT_BINARY_DIR
                "/convergence_u1.eps 'Interpolation error for $u(x)=u_1(x)$'");
  }
  {
    // Errors for u2
    lf::mesh::utils::MeshFunctionGlobal u2_mf(u2);
    lf::mesh::utils::MeshFunctionGlobal grad_u2_mf(grad_u2);
    auto [l2_error, h1_error] = QuasiInterpolation::interpolationError(
        mesh_hierarchy_p, u2_mf, grad_u2_mf);
    QuasiInterpolation::printError(meshwidth, l2_error, h1_error,
                                   "errors for u2");
    QuasiInterpolation::writeCSV(meshwidth, l2_error, h1_error,
                                 CURRENT_BINARY_DIR "/convergence_u2.csv");
    std::system("python3 " CURRENT_SOURCE_DIR
                "/plot_convergence.py " CURRENT_BINARY_DIR
                "/convergence_u2.csv " CURRENT_BINARY_DIR
                "/convergence_u2.eps 'Interpolation error for $u(x)=u_2(x)$'");
  }
  return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/FluidEigenMappings.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class RunningStats
{
public:
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXXd = Eigen::ArrayXXd;

  void init(index historySize, index inputSize)
  {
    mInputBuffer = ArrayXXd::Zero(inputSize, historySize + 1);
    mInputSquaredBuffer = ArrayXXd::Zero(inputSize, historySize + 1);
    mXSum = ArrayXd::Zero(inputSize);
    mXSqSum = ArrayXd::Zero(inputSize);
    mX1 = ArrayXd::Zero(inputSize);
    mY1 = ArrayXd::Zero(inputSize);
    mInputSquared = ArrayXd::Zero(inputSize);
    mCleanedInput = ArrayXd::Zero(inputSize);
    mTmpOut = ArrayXd::Zero(inputSize);
    mHead = 0;
    mN = 0;
    mSize = historySize;
  }

  template<typename T>
  void process(FluidTensorView<T, 1> in, FluidTensorView<T, 1> meanOut,
               FluidTensorView<T, 1> stdDevOut)
  {
    // Moving average and _sample_ standard deviation
    // https://www.dsprelated.com/showthread/comp.dsp/97276-1.php

    // oldest inputs and squared inputs from buffer
    mX1 = mInputBuffer.col(mHead);
    mY1 = mInputSquaredBuffer.col(mHead);

    using MapXd = decltype(_impl::asEigen<Eigen::Array>(in));

    MapXd inMap = _impl::asEigen<Eigen::Array>(in);
    mCleanedInput = inMap.isNaN().select(0,inMap).template cast<double>();
    mInputSquared = mCleanedInput.square();

    // running sums
    mXSum += (mCleanedInput - mX1);
    mXSqSum += (mInputSquared - mY1);

    // calculate stats
    mN = std::min(mN + 1, mSize);
    _impl::asEigen<Eigen::Array>(meanOut) = (mXSum / mN).template cast<T>();
    if(mN > 1)
    {
     mTmpOut = ((mN * mXSqSum - mXSum.square()) / (mN * (mN - 1))).sqrt();
     _impl::asEigen<Eigen::Array>(stdDevOut) = mTmpOut.isNaN().select(0,mTmpOut).template cast<T>();
    }
    else
    {
      std::fill(stdDevOut.begin(),stdDevOut.end(),0);
    }

    // write new data
    mInputBuffer.col(mHead) = mCleanedInput;
    mInputSquaredBuffer.col(mHead) = mInputSquared;

    // move on
    mHead++;
    if (mHead >= mInputBuffer.cols() - 1) mHead = 0;
  }

private:
  ArrayXXd mInputBuffer;
  ArrayXXd mInputSquaredBuffer;
  ArrayXd  mXSum;
  ArrayXd  mXSqSum;
  ArrayXd  mX1;
  ArrayXd  mY1;
  ArrayXd  mCleanedInput;
  ArrayXd  mInputSquared;
  ArrayXd  mTmpOut;
  index    mHead;
  index    mN;
  index    mSize;
};
} // namespace algorithm
} // namespace fluid


///////////////////////////////////////////////////////////////
//  Copyright 2011 John Maddock. 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_

#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/random.hpp>
#include <iostream>
#include <iomanip>

void t1()
{
//[random_eg1
//=#include <boost/multiprecision/gmp.hpp>
//=#include <boost/multiprecision/random.hpp>
//=
//=int main()
//={
   using namespace boost::multiprecision;
   using namespace boost::random;

   //
   // Declare our random number generator type, the underlying generator
   // is the Mersenne twister mt19937 engine, and 256 bits are generated:
   //
   typedef independent_bits_engine<mt19937, 256, mpz_int> generator_type;
   generator_type gen;
   //
   // Generate some values:
   //
   std::cout << std::hex << std::showbase;
   for(unsigned i = 0; i < 10; ++i)
      std::cout << gen() << std::endl;
//=   return 0;
//=}
//]
}

void t2()
{
//[random_eg2
//=#include <boost/multiprecision/gmp.hpp>
//=#include <boost/multiprecision/random.hpp>
//=
//=int main()
//={
   using namespace boost::multiprecision;
   using namespace boost::random;

   //
   // Generate integers in a given range using uniform_int,
   // the underlying generator is invoked multiple times
   // to generate enough bits:
   //
   mt19937 mt;
   uniform_int_distribution<mpz_int> ui(0, mpz_int(1) << 256);
   //
   // Generate the numbers:
   //
   std::cout << std::hex << std::showbase;
   for(unsigned i = 0; i < 10; ++i)
      std::cout << ui(mt) << std::endl;

//=   return 0;
//=}
//]
}

void t3()
{
//[random_eg3
//=#include <boost/multiprecision/gmp.hpp>
//=#include <boost/multiprecision/random.hpp>
//=
//=int main()
//={
   using namespace boost::multiprecision;
   using namespace boost::random;
   //
   // We need an underlying generator with at least as many bits as the
   // floating point type to generate numbers in [0, 1) with all the bits
   // in the floating point type randomly filled:
   //
   uniform_01<mpf_float_50> uf;
   independent_bits_engine<mt19937, 50L*1000L/301L, mpz_int> gen;
   //
   // Generate the values:
   //
   std::cout << std::setprecision(50);
   for(unsigned i = 0; i < 20; ++i)
      std::cout << uf(gen) << std::endl;
//=   return 0;
//=}
//]
}

void t4()
{
//[random_eg4
//=#include <boost/multiprecision/gmp.hpp>
//=#include <boost/multiprecision/random.hpp>
//=
//=int main()
//={
   using namespace boost::multiprecision;
   using namespace boost::random;
   //
   // We can repeat the above example, with other distributions:
   //
   uniform_real_distribution<mpf_float_50> ur(-20, 20);
   gamma_distribution<mpf_float_50> gd(20);
   independent_bits_engine<mt19937, 50L*1000L/301L, mpz_int> gen;
   //
   // Generate some values:
   //
   std::cout << std::setprecision(50);
   for(unsigned i = 0; i < 20; ++i)
      std::cout << ur(gen) << std::endl;
   for(unsigned i = 0; i < 20; ++i)
      std::cout << gd(gen) << std::endl;
//=   return 0;
//=}
//]
}

int main()
{
   t1();
   t2();
   t3();
   t4();
   return 0;
}


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//               2014 Piotr Smulewicz
//
// 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)
//=======================================================================
/**
 * @file metric_test.cpp
 * @brief
 * @author Piotr Wygocki ,Piotr Smulewicz
 * @version 1.0
 * @date 2013-02-04
 */

#include "test_utils/sample_graph.hpp"

#include "paal/utils/irange.hpp"
#include "paal/utils/floating.hpp"
#include "paal/data_structures/metric/euclidean_metric.hpp"
#include "paal/data_structures/metric/graph_metrics.hpp"
#include "paal/data_structures/metric/basic_metrics.hpp"

#include <boost/test/unit_test.hpp>

#include <cmath>

BOOST_AUTO_TEST_CASE(euclidean_metric_test) {
    auto metric=paal::data_structures::euclidean_metric<int>();
    std::pair<int,int> a={0,0},b={1,1};

    BOOST_CHECK(paal::utils::compare<double>().e(metric(a,b),std::sqrt(2)));
}

BOOST_AUTO_TEST_CASE(metric_test) {
    typedef sample_graphs_metrics SGM;
    auto gm = SGM::get_graph_metric_small();

    BOOST_CHECK_EQUAL(gm(SGM::A, SGM::B) , 2);
    BOOST_CHECK_EQUAL(gm(SGM::C, SGM::B) , 3);
}

BOOST_AUTO_TEST_CASE(copyrectangle_array_metric) {
    paal::data_structures::rectangle_array_metric<int> m(1, 2);
    m(0, 0) = 1;
    m(0, 1) = 2;

    auto r1 = paal::irange(1);
    auto r2 = paal::irange(2);
    paal::data_structures::rectangle_array_metric<int> copy(
        m, r1, r2);

    BOOST_CHECK_EQUAL(copy(0, 0) , 1);
    BOOST_CHECK_EQUAL(copy(0, 1) , 2);
}

BOOST_AUTO_TEST_CASE(copyarray_metric) {
    paal::data_structures::array_metric<int> m(1);
    m(0, 0) = 13;

    auto r1 = paal::irange(1);
    paal::data_structures::array_metric<int> copy(m, r1);

    BOOST_CHECK_EQUAL(copy(0, 0) , 13);
}


#pragma once

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

/* Creates a matrix that Rotate vector about the z axis
 * Currently needed for our robot relative coordinate system
 */
template <typename T>
boost::numeric::ublas::matrix<T> rotateZMatrix(T theta);

/* Creates a translation matrix
 */
template <typename T>
boost::numeric::ublas::matrix<T> translateMatrix(T x, T y, T z);

/* Creates a projection matrix
 */
template <typename T>
boost::numeric::ublas::matrix<T> projectionMatrix(T ex, T ey,
                                                  T ez);

/* Creates a DH matrix used in Kinematics
 */
template <typename T>
boost::numeric::ublas::matrix<T> createDHMatrix(T a, T alpha,
                                                T d, T theta);

/* Function to invert matrix
 */
template <typename T>
bool invertMatrix(const boost::numeric::ublas::matrix<T>& input,
                  boost::numeric::ublas::matrix<T>& inverse);

/* Creates a 4,1 vector */
template <typename T>
inline boost::numeric::ublas::matrix<T>
vec4(T a, T b, T c, T d) {
   boost::numeric::ublas::matrix<T> m(4, 1);
   m(0, 0) = a;
   m(1, 0) = b;
   m(2, 0) = c;
   m(3, 0) = d;
   return m;
}

/* Creates a 4,1 vector from a 4,1 array */
template <typename T>
inline boost::numeric::ublas::matrix<T>
vec4(const T a[]) {
   boost::numeric::ublas::matrix<T> m(4, 1);
   m(0, 0) = a[0];
   m(1, 0) = a[1];
   m(2, 0) = a[2];
   m(3, 0) = a[3];
   return m;
}

#include "utils/matrix_helpers.tcc"


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_functions.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_math_clamp);

  BOOST_AUTO_TEST_CASE(double_testing)
  {
    double const min_val = 0.1;
    double const max_val = 2.9;
    double clamped_val;

    // normal clamp tests
    clamped_val = math::clamp(1.5, min_val, max_val);
    BOOST_CHECK( clamped_val == 1.5 );

    clamped_val = math::clamp(-1.5, min_val, max_val);
    BOOST_CHECK( clamped_val == min_val );

    clamped_val = math::clamp(3.0, min_val, max_val);
    BOOST_CHECK( clamped_val == max_val );

    // clamp minimum tests
    clamped_val = math::clamp_min(1.5, min_val);
    BOOST_CHECK( clamped_val == 1.5 );

    clamped_val = math::clamp_min(-1.5, min_val);
    BOOST_CHECK( clamped_val == min_val );

    clamped_val = math::clamp_min(3.0, min_val);
    BOOST_CHECK( clamped_val == 3.0 );

    // clamp maximum tests
    clamped_val = math::clamp_max(1.5, max_val);
    BOOST_CHECK( clamped_val == 1.5 );

    clamped_val = math::clamp_max(-1.5, max_val);
    BOOST_CHECK( clamped_val == -1.5 );

    clamped_val = math::clamp_max(3.0, max_val);
    BOOST_CHECK( clamped_val == max_val );

    // clamp zero..one tests
    clamped_val = math::clamp_zero_one(0.5);
    BOOST_CHECK( clamped_val == 0.5 );

    clamped_val = math::clamp_zero_one(-1.5);
    BOOST_CHECK( clamped_val == 0.0 );

    clamped_val = math::clamp_zero_one(3.0);
    BOOST_CHECK( clamped_val == 1.0 );
  }

  BOOST_AUTO_TEST_CASE(float_testing)
  {
    float const min_val = 0.1f;
    float const max_val = 2.9f;
    float clamped_val;

    // normal clamp tests
    clamped_val = math::clamp(1.5f, min_val, max_val);
    BOOST_CHECK( clamped_val == 1.5f );

    clamped_val = math::clamp(-1.5f, min_val, max_val);
    BOOST_CHECK( clamped_val == min_val );

    clamped_val = math::clamp(3.0f, min_val, max_val);
    BOOST_CHECK( clamped_val == max_val );

    // clamp minimum tests
    clamped_val = math::clamp_min(1.5f, min_val);
    BOOST_CHECK( clamped_val == 1.5f );

    clamped_val = math::clamp_min(-1.5f, min_val);
    BOOST_CHECK( clamped_val == min_val );

    clamped_val = math::clamp_min(3.0f, min_val);
    BOOST_CHECK( clamped_val == 3.0f );

    // clamp maximum tests
    clamped_val = math::clamp_max(1.5f, max_val);
    BOOST_CHECK( clamped_val == 1.5f );

    clamped_val = math::clamp_max(-1.5f, max_val);
    BOOST_CHECK( clamped_val == -1.5f );

    clamped_val = math::clamp_max(3.0f, max_val);
    BOOST_CHECK( clamped_val == max_val );

    // clamp zero..one tests
    clamped_val = math::clamp_zero_one(0.5f);
    BOOST_CHECK( clamped_val == 0.5f );

    clamped_val = math::clamp_zero_one(-1.5f);
    BOOST_CHECK( clamped_val == 0.0f );

    clamped_val = math::clamp_zero_one(3.0f);
    BOOST_CHECK( clamped_val == 1.0f );
  }

BOOST_AUTO_TEST_SUITE_END();