#include "DenoiseSystem.h"

#include "../Components/DenoiseData.h"

#include <_deps/imgui/imgui.h>

#include <spdlog/spdlog.h>
#include <Eigen/Core>
#include <Eigen/Sparse>

using namespace Ubpa;

void DenoiseSystem::OnUpdate(Ubpa::UECS::Schedule& schedule) {
	schedule.RegisterCommand([](Ubpa::UECS::World* w) {
		auto data = w->entityMngr.GetSingleton<DenoiseData>();
		if (!data)
			return;

		if (ImGui::Begin("Denoise")) {
			if (ImGui::Button("Mesh to HEMesh")) {
				data->heMesh->Clear();
				[&]() {
					if (!data->mesh) {
						spdlog::warn("mesh is nullptr");
						return;
					}

					if (data->mesh->GetSubMeshes().size() != 1) {
						spdlog::warn("number of submeshes isn't 1");
						return;
					}

					data->copy = *data->mesh;

					std::vector<size_t> indices(data->mesh->GetIndices().begin(), data->mesh->GetIndices().end());

					data->heMesh->Init(indices, 3);
					if (!data->heMesh->IsTriMesh())
						spdlog::warn("HEMesh init fail");

					for (size_t i = 0; i < data->mesh->GetPositions().size(); i++) {
						data->heMesh->Vertices().at(i)->position = data->mesh->GetPositions().at(i);
						data->heMesh->Vertices().at(i)->idx = -1;
						data->heMesh->Vertices().at(i)->bidx = -1;
					}

					spdlog::info("Mesh to HEMesh success");
				}();
			}

			if (ImGui::Button("Solve Minimal surface")) {
				[&]() {
					if (!data->heMesh->IsTriMesh()) {
						spdlog::warn("HEMesh isn't triangle mesh");
						return;
					}

					const auto vertices = data->heMesh->Vertices();
					int totalPoints = static_cast<int>(vertices.size()), boundaryPoints = { 0 }, innerPoints = { 0 };
					for (int i = 0; i < totalPoints; ++i) {
						vertices[i]->idx = i;
						if (vertices[i]->IsOnBoundary()) ++boundaryPoints;
					}
					innerPoints = totalPoints - boundaryPoints;

					auto Idx = [&](int idx, int k) {
						return k * totalPoints + idx;
					};
					auto ConsIdx = [&](int idx, int k) {
						return 3 * totalPoints + idx * 3 + k;
					};

					Eigen::SparseMatrix<float> A(3 * (totalPoints + boundaryPoints), 3 * totalPoints);
					Eigen::SparseVector<float> b(3 * (totalPoints + boundaryPoints));
					spdlog::info(totalPoints);

					int b_idx = 0;
					for (auto* v : data->heMesh->Vertices()) {
						const auto P = v->position;

						// A_ij & x_i
						if (v->IsOnBoundary()) {
							for (auto k : { 0, 1, 2 }) {
								b.coeffRef(ConsIdx(b_idx, k)) = P[k];
								A.coeffRef(ConsIdx(b_idx, k), Idx(v->idx, k)) = 1;
							}
							++b_idx;
						}
						const auto& adj_v = v->AdjVertices();
						for (auto k : { 0, 1, 2 })
							A.coeffRef(Idx(v->idx, k), Idx(v->idx, k)) = v->AdjVertices().size();

						for (auto* adj : adj_v) {
							for (auto k : { 0, 1, 2 }) A.coeffRef(Idx(v->idx, k), Idx(adj->idx, k)) = -1;
						}
					}
					spdlog::info("Build Matrix successful");

					Eigen::ConjugateGradient<Eigen::SparseMatrix<float> > solver;
					solver.setTolerance(1e-4);
					solver.compute(A.transpose() * A);
					Eigen::SparseVector<float> x(3 * totalPoints);
					x = solver.solve(A.transpose() * b);

					spdlog::info("Solve Matrix successful");

					for (auto* v : data->heMesh->Vertices()) {
						for (auto k : { 0, 1, 2 }) {
							v->position[k] = x.coeffRef(Idx(v->idx, k));
						}
					}
					spdlog::info("Data transform successful");
				}();
			}

			if (ImGui::Button("Parameterization")) {
				[&]() {
					if (!data->heMesh->IsTriMesh()) {
						spdlog::warn("HEMesh isn't triangle mesh");
						return;
					}

					const auto vertices = data->heMesh->Vertices();
					int totalPoints = static_cast<int>(vertices.size()), boundaryPoints = { 0 }, innerPoints = { 0 };
					for (int i = 0; i < totalPoints; ++i) vertices[i]->idx = i;
					int bpt;
					for (int i = 0; i < totalPoints; ++i)
						if (vertices[i]->IsOnBoundary()) bpt = vertices[i]->idx; // start point
					do {
						vertices[bpt]->bidx = boundaryPoints++;
						const auto& adj_v = vertices[bpt]->AdjVertices();
						for (auto* v : data->heMesh->Vertices()) {
							if (v->IsOnBoundary() && v->bidx < 0) {
								bpt = v->idx;
								break;
							}
						}
					} while (vertices[bpt]->bidx < 0);

					innerPoints = totalPoints - boundaryPoints;

					auto Idx = [&](int idx, int k) {
						return k * totalPoints + idx;
					};
					auto ConsIdx = [&](int idx, int k) {
						return 2 * totalPoints + idx * 2 + k;
					};

					Eigen::SparseMatrix<float> A(2 * (totalPoints + boundaryPoints), 2 * totalPoints);
					Eigen::SparseVector<float> b(2 * (totalPoints + boundaryPoints));
					spdlog::info(totalPoints);

					for (auto* v : data->heMesh->Vertices()) {
						const auto P = v->position;

						// A_ij & x_i
						if (v->IsOnBoundary()) {
							{ // project to UV surface
								float t = 4 * float(v->bidx) / float(boundaryPoints);
								float U = { 0.0f }, V = { 0.0f };
								if (t <= 1.0f) V = t;
								else if (t <= 2.0f) U = t - 1, V = 1.0f;
								else if (t <= 3.0f) U = 1.0f, V = 1.0f - (t - 2.0f);
								else U = 1.0f - (t - 3.0f);

								spdlog::info(std::to_string(U) + " " + std::to_string(V));

								b.coeffRef(ConsIdx(v->bidx, 0)) = U;
								b.coeffRef(ConsIdx(v->bidx, 1)) = V;
							}
							for (auto k : { 0, 1 }) A.coeffRef(ConsIdx(v->bidx, k), Idx(v->idx, k)) = 1;
						}
						const auto& adj_v = v->AdjVertices();
						for (auto k : { 0, 1 })
							A.coeffRef(Idx(v->idx, k), Idx(v->idx, k)) = v->AdjVertices().size();

						for (auto* adj : adj_v) {
							for (auto k : { 0, 1 }) A.coeffRef(Idx(v->idx, k), Idx(adj->idx, k)) = -1;
						}
					}
					spdlog::info("Build Matrix successful");

					Eigen::ConjugateGradient<Eigen::SparseMatrix<float> > solver;
					solver.setTolerance(1e-2);
					solver.compute(A.transpose() * A);
					Eigen::SparseVector<float> x(2 * totalPoints);
					x = solver.solve(A.transpose() * b);

					spdlog::info("Solve Matrix successful");

					for (auto* v : data->heMesh->Vertices()) {
						v->uv = { x.coeffRef(Idx(v->idx, 0)), x.coeffRef(Idx(v->idx, 1)) };
						spdlog::info(std::to_string(v->uv[0]) + " " + std::to_string(v->uv[1]));
					}

					spdlog::info("Data transform successful");
				}();
			}

			if (ImGui::Button("Set Normal to Color")) {
				[&]() {
					if (!data->mesh) {
						spdlog::warn("mesh is nullptr");
						return;
					}

					data->mesh->SetToEditable();
					const auto& normals = data->mesh->GetNormals();
					std::vector<rgbf> colors;
					for (const auto& n : normals)
						colors.push_back((n.as<valf3>() + valf3{ 1.f }) / 2.f);
					data->mesh->SetColors(std::move(colors));

					spdlog::info("Set Normal to Color Success");
				}();
			}

			if (ImGui::Button("HEMesh to Mesh")) {
				[&]() {
					if (!data->mesh) {
						spdlog::warn("mesh is nullptr");
						return;
					}

					if (!data->heMesh->IsTriMesh() || data->heMesh->IsEmpty()) {
						spdlog::warn("HEMesh isn't triangle mesh or is empty");
						return;
					}

					data->mesh->SetToEditable();

					const size_t N = data->heMesh->Vertices().size();
					const size_t M = data->heMesh->Polygons().size();
					std::vector<Ubpa::pointf3> positions(N);
					std::vector<Ubpa::pointf2> uvs(N);
					std::vector<uint32_t> indices(M * 3);
					for (size_t i = 0; i < N; i++) {
						positions[i] = data->heMesh->Vertices().at(i)->position;
						uvs[i] = data->heMesh->Vertices().at(i)->uv;
					}
					for (size_t i = 0; i < M; i++) {
						auto tri = data->heMesh->Indices(data->heMesh->Polygons().at(i));
						indices[3 * i + 0] = static_cast<uint32_t>(tri[0]);
						indices[3 * i + 1] = static_cast<uint32_t>(tri[1]);
						indices[3 * i + 2] = static_cast<uint32_t>(tri[2]);
					}
					data->mesh->SetPositions(std::move(positions));
					data->mesh->SetUV(std::move(uvs));
					data->mesh->SetIndices(std::move(indices));
					data->mesh->SetSubMeshCount(1);
					data->mesh->SetSubMesh(0, { 0, M * 3 });
					data->mesh->GenNormals();
					data->mesh->GenTangents();

					spdlog::info("HEMesh to Mesh success");
				}();
			}

			if (ImGui::Button("Recover Mesh")) {
				[&]() {
					if (!data->mesh) {
						spdlog::warn("mesh is nullptr");
						return;
					}
					if (data->copy.GetPositions().empty()) {
						spdlog::warn("copied mesh is empty");
						return;
					}

					*data->mesh = data->copy;

					spdlog::info("recover success");
				}();
			}
		}
		ImGui::End();
	});
}


#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/mat_ZZ.h>
#include <gmp.h>

#include "Random.h"
#include "params.h"

using namespace std;
using namespace NTL;

vec_ZZ RandomVector()
{
    vec_ZZ w;
    unsigned int i;
    w.SetLength(N0);
    for(i=0; i<N0; i++)
    {
        w[i] = conv<ZZ>(rand())%q1;
    }
    return w;
}

//==============================================================================
//Generates a random polynomial of fixed degree
//==============================================================================
ZZX RandomPoly(const unsigned int degree)
{
    unsigned int i;
    ZZX f;
    f.SetLength(degree+1);
    for(i=0; i<=degree; i++)
    {
        f[i] = rand();
    }
    return f;
}

//==============================================================================
//Generates a random polynomial of fixed degree and "approximately" fixed squared norm
//==============================================================================
ZZX RandomPolyFixedSqNorm(const ZZ& SqNorm, const unsigned int degree)
{
    unsigned int i;
    ZZ SqNorm0, Ratio;
    ZZX f;
    f.SetLength(degree+1);

    RR_t sigma = sqrt( ( (double) conv<double>(SqNorm)/(degree+1) ) );

    for(i=0; i<=degree; i++)
    {
        f[i] = conv<ZZ>(Sample3(sigma));
    }
    f[degree] |= 1;
    return f;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/gamma.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/five.hpp>
#include <boost/simd/function/rsqrt.hpp>

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void limit_test(Env& $)
{
  using p_t = bs::pack<T, N>;

  STF_ULP_EQUAL (bs::gamma(p_t(1))            , p_t(1), 0.5);
  STF_IEEE_EQUAL(bs::gamma(p_t(0))            , p_t(bs::Inf<T>())        );
  STF_IEEE_EQUAL(bs::gamma(p_t(bs::Inf<T>())) , bs::Inf<p_t>());
  STF_IEEE_EQUAL(bs::gamma(p_t(bs::Minf<T>())), p_t(bs::Nan<T>())        );
}

STF_CASE_TPL("Check gamma limit cases" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
  limit_test<T, N>($);
  limit_test<T, N/2>($);
  limit_test<T, N*2>($);
}

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : -T(i);
    b[i] = bs::gamma(a1[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);

  STF_ULP_EQUAL(bs::gamma(aa1), bb, 0.5);
}

STF_CASE_TPL("Check gamma on pack" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
   test<T, N>($);
   test<T, N/2>($);
   test<T, N*2>($);
}

STF_CASE_TPL (" gamma",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::gamma;
  using p_t = bs::pack<T>;

  using r_t = decltype(gamma(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(gamma(bs::Minf<p_t>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(gamma(bs::Inf<p_t>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(gamma(bs::Nan<p_t>()), bs::Nan<r_t>(), 0);
#endif
  STF_ULP_EQUAL(gamma(bs::Zero<p_t>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(gamma(bs::Mzero<p_t>()), bs::Minf<r_t>(), 0);
  STF_ULP_EQUAL(gamma(p_t(1)), p_t(1), 0);
  STF_ULP_EQUAL(gamma(p_t(2)), p_t(1), 0);
  STF_ULP_EQUAL(gamma(p_t(3)), p_t(2), 0);
  STF_ULP_EQUAL(gamma(p_t(5)), p_t(24), 0);
 }


/*! \file
  \brief Boxplot display of two example functions (compared to a 1-D plot).
*/

// demo_functions_boxplot.cpp

// Copyright Paul A. Bristow 2008, 2009, 2021

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

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.

// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

#include <boost/svg_plot/svg_1d_plot.hpp> // For 1d plots.
#include <boost/svg_plot/svg_boxplot.hpp>  // For boxplots.

#include <vector>  // using std::vector as container for two function's data values.
#include <cmath> // using ::sin;
#include <iostream> // using std::cout; std::endl;

//[boxplot_functions_1

/*` Two example functions to show how they display as a 1-D plot and as a boxplot.
One function is effectively 1/x and the other is effectively sin(x),
but both are scaled to avoid too much overlap when displayed as a 1-D plot.
*/

// Effectively 1/x
double f(double x)
{
    return 50 / (x);
}

// Effectively sin(x)
double g(double x)
{
    return 60 + 25 * ::sin(x * 50);
}
//] [boxplot_functions_1]

int main()
{
  using namespace boost::svg;  // For SVG colors.
  try
  {

    std::vector<double> data1, data2;  // Container for two function data-series values.
    for(double i = 0.1; i < 10; i += 0.1)
    { // Fill our vectors with 100 values:
      double fv = f(i);
      double gv = g(i);
      //std::cout << i << ' ' << fv << ' ' << gv << std::endl; // Optionally display values?
      data1.push_back(fv);
      data2.push_back(gv);
    }

    // First display as a 1D plot.
    svg_1d_plot my_1d_plot;  // To hold a SVG 1-D plot.
    my_1d_plot.title(" 1D plots of example sin functions")
      .background_border_color(cyan)
      .x_min(0).x_max(100.) // Range of values shown on plot.
      .x_major_tick(10);

    my_1d_plot.plot(data1, "[50 / x]").stroke_color(blue);  // 1/x function.
    my_1d_plot.plot(data2, "[60 + 25 * sin(50x)]").stroke_color(red); // sin(x) function.

    my_1d_plot.write("./demo_functions_1d_plot.svg");  // Write out the plot.

    // Repeat display as a boxplot.
      svg_boxplot my_boxplot; // To hold a SVG boxplot.

    my_boxplot.title("Boxplots of 1/x and sin(x) Functions")
      .background_border_color(magenta)
      .x_label("Functions")
      .y_label("Population Size");

    my_boxplot.y_range(0, 100).y_major_interval(20); // Axis information.

    my_boxplot.plot(data1, "[50 / x]"); // 1/x function.
    my_boxplot.plot(data2, "[60 + 25 * sin(50x)]"); // sin(x) function.

    my_boxplot.write("./demo_functions_boxplot.svg");  // Write out the plot.
  }
  catch (const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n  " << e.what() << std::endl;
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
} // int main()


#include "mex.h"        // to compile in Matlab
#include <iostream> 	// to check outputs here and there
#include <cstdlib>    	// to use srand (if you uncomment the relevant line)
#include <string>   	// to read in the kernel name
#include <vector>       // to construct the index list
#include <map>	    	// to map kernels to integers for the switch
#include <Eigen/Core>   // to use basic Eigen structures
using namespace Eigen;
using namespace std;

//Commented lines contain various checks on integrity

//Date of last editing: Saturday 8th of September 2012

float LinearInterpolation (VectorXf X , VectorXf Y, float X_PointOfInterest){
//Produce Y_point_of_interest given X,Y and target X_point_of_interest
//X : vector containing the X variables of the interpolant
//Y : vector containing the Y variables of the interpolant
//PointOfInterest : Point of X to estimate the new point of Y

  float   xk, xkp1, yk, ykp1;  //Points adjecent to the point of interpolation
  if ( X.size() != Y.size() ){cout << "Problem with vector sizes" << endl; return(-1);}
//cout <<  " X(0): " <<  X(0) <<" X(Y.size()-1): " <<X(Y.size()-1)   <<   " Point of interest: " << X_PointOfInterest<< endl;
  if ( X_PointOfInterest < X(0) || X_PointOfInterest > X(Y.size()-1) ){cout << "You interpolate out of the curve boundaries" << endl; return(-1);}
//Find the points right before and right after the point of interest
  for (int i=1; i<X.size() ; i++){
    if (X(i)>= X_PointOfInterest){
      xkp1 = X(i);
      xk = X(i-1);
      ykp1 = Y(i);
      yk = Y(i-1);
      break;}
  }
//point-slope form for a line formula
  float t = (X_PointOfInterest -xk)/(xkp1 -xk);
  float yPOI = (1-t) * yk + t * ykp1;  // estimate point of interest
// cout << "(" << xk << ",  " << X_PointOfInterest << " , " << xkp1 << ") & (" << yk << ", " << yPOI  << ", " << ykp1 << ")"<< endl;
  return (yPOI);
}

VectorXf fnvalspapi(VectorXf X, VectorXf Y, VectorXf X_target){
  //evaluate Y_target for X_target given X and Y

	int N =  X_target.size();
	VectorXf rr(N);
	for (int i=0; i<N ; i++){
      rr(i) =  LinearInterpolation(X, Y, X_target(i)) ;  }
	return(rr);
}

int MonotonicityCheck(VectorXf X){
   // evaluate vector monotonicity of vector X
   int N =  X.size();
   for (int i=0; i<(N-1) ; i++){
     if ( X(i) >X (1+i) ) return (-1);   }
   return (1);
}

float rttemp1( VectorXf t_reg,  VectorXf curvei, VectorXf curvek,int nknots, float lambda, VectorXf initial){
  //Calculate the cost of the given warping
  // t_reg  : time grid of y_reg
  // curvei : query curve
  // curvek : reference curve
  // nknots : number of knots
  // lambda : time distortion penalty parameter
  // initial: position of knots on the simplex

  int N = t_reg.size();
  VectorXf struct_ = VectorXf::LinSpaced( nknots+2, t_reg(0)  , t_reg.maxCoeff() );
  VectorXf hik(N);
  VectorXf Q(2+ initial.size()) ;           //Solution with the placement of the knots on the simplex
  Q(0) = t_reg(0) ; Q(1+ initial.size()) = t_reg.maxCoeff()  ; Q.segment(1, initial.size()) = initial;
  hik =  fnvalspapi( struct_, Q , t_reg);   // compute the new internal time-scale

  //cout << "hik: " << hik.transpose() << endl;
  //cout << "Monotonicity Checked on Hik: " << MonotonicityCheck(hik) << endl;
  //if(  MonotonicityCheck(hik) == -1) { cout <<" Q.transpose()  is :"<< Q.transpose()  << endl;}

  return ( (fnvalspapi(t_reg,curvei,hik)-fnvalspapi(t_reg,curvek ,t_reg)).array().pow(2).sum() + lambda * (hik - t_reg).array().pow(2).sum() );
}

VectorXf NewSolution( VectorXf x0 , float Step, int point, VectorXf t_reg){
    //generate new solution on the simplex defined in [t_reg(0)-x0-t_reg(N-1)] using a displacement of size Step
    // x0   : initial solution
    // Step : displacement size
    // point: knot to perturb
    // t_reg: time_grid

    VectorXf InitConf (2+ x0.size());
    InitConf(0) = t_reg(0); InitConf( x0.size()+1) = t_reg.maxCoeff()  ; InitConf.segment(1, x0.size()) = x0;
    float LowBou =  InitConf(point-1);
    float UppBou =  InitConf(point+1);
    float New_State = (UppBou - LowBou) * Step + LowBou;
    InitConf(point) = New_State;
    //if (  MonotonicityCheck( InitConf.segment(1, x0.size()) ) == -1) {
    //    cout << "We generated a unacceptable solutiion" << endl << "Initial seed was : " << x0.transpose()
    //         << endl << " and we produced :"<<InitConf.segment(1, x0.size()).transpose() << endl;}
    return (InitConf.segment(1, x0.size()) ) ;
  }

//====
typedef struct { float Val; VectorXf Mapping;} rthink_Output; //define structure that holds the results.
//====

rthink_Output rthik_SA(VectorXf t_reg,  VectorXf curvei, VectorXf curvek,int nknots, float lambda){
  // Random Search solver for the optimization problem of pairwise warping
  // t_reg : time_grid
  // curvei: query curve
  // curvek: reference curve
  // nknots : number of knots
  // lambda : time distortion penalty parameter

  int k=0; float OldSol, bk;
  VectorXf xk(nknots);
  bk = (t_reg.maxCoeff() - t_reg(0))/(1+ float(nknots ));   //Distance between adjacent knots and edges-knots
  xk  = VectorXf::LinSpaced(nknots , t_reg(0) + bk  ,  - bk + t_reg.maxCoeff()  ); //Initial candidate solution with equispaced knots
  VectorXf xn(nknots);   VectorXf help(2+nknots);

  float NewSol;
  OldSol =  rttemp1(t_reg, curvei, curvek, nknots , lambda, xk);  //Cost of initial solution
  int z= 99*nknots;                                               //Number of random search to do (proportional to the # of knots)
  //srand(1);                                                     //Fix the seed to have reproducable behaviour
  VectorXf Steps(z); Steps = (ArrayXf::Random(z)+1.)/2.;          //Generate possible random pertubations magnitude
  VectorXi Posit(z); for (int u=0; u <z; u++ ) Posit(u) =1+ rand()%(nknots+0); //Generate list of positions to purturb

  k=0;
  while((OldSol > .0001) && (k<z)) {
    xn = NewSolution(xk,Steps(k), Posit(k), t_reg );              //Get a new solution
    NewSol = rttemp1(t_reg, curvei, curvek, nknots, lambda, xn);  //Cost of new solution
    if (  (NewSol < OldSol)  ) {                                  //If it's better than the old one, use it.
      OldSol= NewSol; xk= xn;
    }
  k++;
}

  VectorXf x3 =  VectorXf::LinSpaced(2+nknots, t_reg(0), t_reg( t_reg.size()-1));
  help(0) =  t_reg(0) ; help(nknots+1) = t_reg( t_reg.size()-1) ; help.segment(1,nknots) = xk;

  rthink_Output G;
  G.Val = OldSol;
  G.Mapping = fnvalspapi(  x3 , help  , t_reg);
  return G ;
}

//Use with syntax :  [hik(indd,:),ftemp]=rthik_E( curvei.coefs,curvek.coefs,t_reg,nknots,lambda);
#define IS_REAL_1D_FULL_DOUBLE(P) (!mxIsComplex(P) && mxGetNumberOfDimensions(P) == 2 && !mxIsSparse(P) && mxIsDouble(P))

void mexFunction (int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[])
{
    #define p_curvei	prhs[0]
	#define p_curvek	prhs[1]
	#define p_treg		prhs[2]
	#define p_knots		prhs[3]
	#define p_lambda	prhs[4]

    #define p_hik         plhs[0]
    #define p_f           plhs[1]

    if(nrhs != 5)       // Check the number of input arguments
    mexErrMsgTxt("Wrong number of input arguments.");
    else if(!IS_REAL_1D_FULL_DOUBLE(p_curvei))   //  Check lw
     mexErrMsgTxt("curvei needs be a real 2D full double array.");
    else if(!IS_REAL_1D_FULL_DOUBLE(p_curvek))   //  Check lw
     mexErrMsgTxt("curvek needs be a real 2D full double array.");
    else if(!IS_REAL_1D_FULL_DOUBLE(p_treg))   //  Check lw
     mexErrMsgTxt("t_reg needs be a real 2D full double array.");

   const int LengthOfVector = mxGetN(p_treg);

   //Read in the variables
   Map<VectorXd> CurvI(mxGetPr(p_curvei) , LengthOfVector);
   Map<VectorXd> CurvK(mxGetPr(p_curvek) , LengthOfVector);
   Map<VectorXd> T_Reg(mxGetPr(p_treg)   , LengthOfVector);

   int knots = *mxGetPr(p_knots);
   double lamdba = *mxGetPr(p_lambda);
   srand(12);
   //Past as floats cause really nobody cares about the small digits there.
   rthink_Output Res = rthik_SA(T_Reg.cast<float>(),CurvI.cast<float>(),CurvK.cast<float>(), knots, float(lamdba));

  //Declare the outputs
  p_hik = mxCreateDoubleMatrix(1, LengthOfVector, mxREAL);
  p_f   = mxCreateDoubleMatrix(1, 1, mxREAL);

  double *hik = mxGetPr(p_hik );
  double *f = mxGetPr(p_f );
  for (int u=0; u< LengthOfVector; u++) { hik[u] = Res.Mapping(u);}
  f[0] = Res.Val;

}


#include "RBGL.hpp"
#include "Basic2DMatrix.hpp"
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>

extern "C"
{

#include <Rdefines.h>

    using namespace std;
    using namespace boost;

    typedef std::set<int> SubgraphAsSet;
    typedef std::vector< SubgraphAsSet > CliqueVector;
    typedef std::vector< CliqueVector > ResultCliquesType;

    static void addNewClique(CliqueVector& cliques, int i, int j)
    {
        SubgraphAsSet s;
        s.insert(i);
        s.insert(j);
        cliques.push_back(s);
    }

    static void findAllCliques(ResultCliquesType& rCliques,
                               Basic2DMatrix<double>& D)
    {
        CliqueVector cliques;
        SubgraphAsSet::iterator s;
        CliqueVector::iterator ci, cj;
        int i, j, k, N=0;
        const int nv = D.numrows();

        // N: max distance in given graph
        for ( i = 0; i < nv; i++ )
            for ( j = i+1; j < nv; j++ )
            {
                N = max(N, (int)D[i][j]);
                // each edge is 1-clique
                if ( D[i][j] == 1 ) addNewClique(cliques, i, j);
            }

        for ( k = 1; k <= N; k++ )
        {
            for ( i = 0; i < nv; i++ )
            {
                for ( ci = cliques.begin(); ci != cliques.end(); ci++ )
                {
                    // i is already in this clique
                    if ( (*ci).find(i) != (*ci).end() ) continue;

                    for ( s = (*ci).begin(); s != (*ci).end(); s++ )
                    {
                        if ( D[i][*s] > k || D[*s][i] > k ) break;
                    }

                    // add i to this clique
                    if ( s == (*ci).end() )
                    {
                        (*ci).insert(i);

                        // eliminate its subsequent subsets
                        for ( cj = ci + 1; cj != cliques.end(); )
                        {
                            if (includes((*ci).begin(), (*ci).end(),
                                         (*cj).begin(), (*cj).end()) )
                                cj = cliques.erase(cj);
                            else
                                cj++;
                        }
                    }
                }
            }
            rCliques.push_back(cliques);
        }

#if DEBUG
        cout << " Cliques: " << endl;
        for ( i = 0; i < rCliques.size(); i++ )
        {
            cout << i+1 << " cliques: " << endl;
            for ( ci=rCliques[i].begin(); ci!=rCliques[i].end(); ci++ )
            {
                cout << "    ";
                for ( s = (*ci).begin(); s != (*ci).end(); s++ )
                    cout << (*s)+1 << " ";
                cout << endl;
            }
        }
#endif
    }

    SEXP kCliques(SEXP num_verts_in, SEXP num_edges_in,
                  SEXP R_edges_in, SEXP R_weights_in)
    {
        // R_weights_in has to be INTEGER now
        int nv = INTEGER(num_verts_in)[0];

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        Basic2DMatrix<double> D(nv, nv);

        // find out the shortest distance between any two nodes
        johnson_all_pairs_shortest_paths(g, D);

        // find k-cliques now
        ResultCliquesType rCliques;
        findAllCliques(rCliques, D);

        ResultCliquesType::iterator ci;
        CliqueVector::iterator vi;
        SubgraphAsSet::iterator si;
        int i, j, k;

        SEXP ansList, cList, sList;
        PROTECT(ansList = allocVector(VECSXP, (int)rCliques.size()));

        for ( i = 0, ci = rCliques.begin(); ci != rCliques.end(); i++, ci++)
        {
            PROTECT(cList = allocVector(VECSXP, (*ci).size()));
            for ( j = 0, vi = (*ci).begin(); vi != (*ci).end(); j++, vi++ )
            {
                PROTECT(sList = allocVector(INTSXP, (*vi).size()));
                for ( k = 0, si = (*vi).begin(); si != (*vi).end(); k++, si++ )
                {
                    INTEGER(sList)[k] = *si;
                }
                SET_VECTOR_ELT(cList,j,sList);
                UNPROTECT(1);
            }
            SET_VECTOR_ELT(ansList,i,cList);
            UNPROTECT(1);
        }
        UNPROTECT(1);
        return(ansList);
    }

    SEXP lambdaSets(SEXP num_verts_in, SEXP num_edges_in,
                    SEXP R_edges_in, SEXP R_capacity_in)
    {
        using namespace boost;

        typedef adjacency_list_traits<vecS, vecS, directedS> Tr;
        typedef Tr::edge_descriptor Tr_edge_desc;

        typedef adjacency_list<vecS, vecS, directedS, no_property,
        property<edge_capacity_t, double,
        property<edge_residual_capacity_t, double,
        property<edge_reverse_t, Tr_edge_desc> > > >
        FlowGraph;

        typedef graph_traits<FlowGraph>::vertex_descriptor vertex_descriptor;
	typedef graph_traits<FlowGraph>::edge_descriptor edge_descriptor;

        FlowGraph flow_g;

        property_map < FlowGraph, edge_capacity_t >::type
        cap = get(edge_capacity, flow_g);
        property_map < FlowGraph, edge_residual_capacity_t >::type
        res_cap = get(edge_residual_capacity, flow_g);
        property_map < FlowGraph, edge_reverse_t >::type
        rev_edge = get(edge_reverse, flow_g);

        edge_descriptor e1, e2;
        bool in1, in2;

        if (!isInteger(R_edges_in)) error("R_edges_in should be integer");

        int NV = INTEGER(num_verts_in)[0];
        int NE = asInteger(num_edges_in);
        int* edges_in = INTEGER(R_edges_in);
	int i, j, k, MaxC=0;

        for (i = 0; i < NE ; i++, edges_in += 2)
        {
            tie(e1, in1) = boost::add_edge(*edges_in, *(edges_in+1), flow_g);
            tie(e2, in2) = boost::add_edge(*(edges_in+1), *edges_in, flow_g);
            if ( !in1 || !in2 )
                error("unable to add edge: (%d, %d)", *edges_in, *(edges_in+1));

            // fill in capacity_map
            cap[e1] = 1;
            cap[e2] = 1;

            // fill in reverse_edge_map
            rev_edge[e1] = e2;
            rev_edge[e2] = e1;
        }

        // ASSUMPTION: max_flow(u, v) = max_flow(v, u)
        // compute edge connectivities between u and v, (u < v)
	// we only need lower-left triangle of the matrix w/o diagonal
        Basic2DMatrix<int> CC(NV, NV);

        for ( i = 0; i < NV; i++ )
        {
            vertex_descriptor s = vertex(i, flow_g);

            for ( j = 0; j < i; j++ )
            {
                vertex_descriptor t = vertex(j, flow_g);

                CC[i][j] = (int)edmonds_karp_max_flow(flow_g, s, t);
                MaxC = max(MaxC, CC[i][j]);
            }
        }

        // calc lambda sets by successively partition V
        Basic2DMatrix<int> P(MaxC+1, NV);
        for ( k = 0; k <= MaxC; k++ )
        {
            for ( i = 0; i < NV; i++ ) P[k][i] = i;

            for ( i = 1; i < NV; i++ )
                for ( j = 0; j < i; j++ )
                    if ( CC[i][j] >= k )  P[k][i] = P[k][j];
        }

#if DEBUG
        cout << " edge-connectivity matrix: " << endl;
        for ( i = 0; i < NV; i++ )
        {
            for ( j = 0; j < NV; j++ ) cout << CC[i][j] << " ";
            cout << endl;
        }

        cout << " P matrix: " << endl;
        for ( k = 0; k <= MaxC; k++ )
        {
            cout << " k = " << k << ": ";
            for ( j = 0; j < NV; j++ ) cout << P[k][j] << " ";
            cout << endl;
        }
#endif

        SEXP ansList, conn, eList;
        PROTECT(ansList = allocVector(VECSXP,2));
        PROTECT(conn = NEW_NUMERIC(1));
        PROTECT(eList = allocMatrix(INTSXP, MaxC+1, NV));

        REAL(conn)[0] = MaxC;

        for ( i = 0, j = 0; j < NV; j++ )
            for ( k = 0; k <= MaxC; k++ )
                INTEGER(eList)[i++] = P[k][j];

        SET_VECTOR_ELT(ansList,0,conn);
        SET_VECTOR_ELT(ansList,1,eList);
        UNPROTECT(3);

        return(ansList);
    }
}


#include <stan/math/prim.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <limits>
#include <vector>

using Eigen::Dynamic;
using Eigen::Matrix;

TEST(ProbDistributionsMultinomialLogit, RNGSize) {
  boost::random::mt19937 rng;
  Matrix<double, Dynamic, 1> beta(5);
  beta << log(0.3), log(0.1), log(0.2), log(0.2), log(0.2);
  std::vector<int> sample = stan::math::multinomial_logit_rng(beta, 10, rng);
  EXPECT_EQ(5U, sample.size());
}

TEST(ProbDistributionsMultinomialLogit, MultinomialLogit) {
  std::vector<int> ns;
  ns.push_back(1);
  ns.push_back(2);
  ns.push_back(3);
  Matrix<double, Dynamic, 1> beta(3, 1);
  beta << log(0.2), log(0.3), log(0.5);
  EXPECT_FLOAT_EQ(-2.002481, stan::math::multinomial_logit_log(ns, beta));
}
TEST(ProbDistributionsMultinomialLogit, Propto) {
  std::vector<int> ns;
  ns.push_back(1);
  ns.push_back(2);
  ns.push_back(3);
  Matrix<double, Dynamic, 1> beta(3, 1);
  beta << log(0.2), log(0.3), log(0.5);
  EXPECT_FLOAT_EQ(0.0, stan::math::multinomial_logit_log<true>(ns, beta));
}

using stan::math::multinomial_logit_log;

TEST(ProbDistributionsMultinomialLogit, error) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  double inf = std::numeric_limits<double>::infinity();

  std::vector<int> ns;
  ns.push_back(1);
  ns.push_back(2);
  ns.push_back(3);
  Matrix<double, Dynamic, 1> beta(3, 1);
  beta << log(0.2), log(0.3), log(0.5);

  EXPECT_NO_THROW(multinomial_logit_log(ns, beta));

  ns[1] = 0;
  EXPECT_NO_THROW(multinomial_logit_log(ns, beta));
  ns[1] = -1;
  EXPECT_THROW(multinomial_logit_log(ns, beta), std::domain_error);
  ns[1] = 1;

  beta(0) = nan;
  EXPECT_THROW(multinomial_logit_log(ns, beta), std::domain_error);
  beta(0) = inf;
  EXPECT_THROW(multinomial_logit_log(ns, beta), std::domain_error);
  beta(0) = -inf;
  EXPECT_THROW(multinomial_logit_log(ns, beta), std::domain_error);

  beta(0) = 0.2;
  beta(1) = 0.3;
  beta(2) = 0.5;

  ns.resize(2);
  EXPECT_THROW(multinomial_logit_log(ns, beta), std::invalid_argument);
}

TEST(ProbDistributionsMultinomialLogit, zeros) {
  double result;
  std::vector<int> ns;
  ns.push_back(0);
  ns.push_back(1);
  ns.push_back(2);
  Matrix<double, Dynamic, 1> beta(3, 1);
  beta << log(0.2), log(0.3), log(0.5);

  result = multinomial_logit_log(ns, beta);
  EXPECT_FALSE(std::isnan(result));

  std::vector<int> ns2;
  ns2.push_back(0);
  ns2.push_back(0);
  ns2.push_back(0);

  double result2 = multinomial_logit_log(ns2, beta);
  EXPECT_FLOAT_EQ(0.0, result2);
}

TEST(ProbDistributionsMultinomialLogit, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int M = 10;
  int trials = 1000;
  int N = M * trials;

  int K = 3;
  Matrix<double, Dynamic, 1> beta(K);
  beta << -1, 1, -10;
  Eigen::VectorXd theta = stan::math::softmax(beta);
  boost::math::chi_squared mydist(K - 1);

  double expect[K];
  for (int i = 0; i < K; ++i)
    expect[i] = N * theta(i);

  int bin[K];
  for (int i = 0; i < K; ++i)
    bin[i] = 0;

  for (int count = 0; count < M; ++count) {
    std::vector<int> a = stan::math::multinomial_logit_rng(beta, trials, rng);
    for (int i = 0; i < K; ++i)
      bin[i] += a[i];
  }

  double chi = 0;
  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j])) / expect[j];

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}


/*
 * Copyright Nick Thompson, 2019
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */
#define BOOST_TEST_MODULE lanczos_smoothing_test

#include <random>
#include <array>
#include <boost/range.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/differentiation/lanczos_smoothing.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/special_functions/next.hpp> // for float_distance
#include <boost/math/tools/condition_numbers.hpp>

using std::abs;
using std::pow;
using std::sqrt;
using std::sin;
using boost::math::constants::two_pi;
using boost::multiprecision::cpp_bin_float_50;
using boost::multiprecision::cpp_bin_float_100;
using boost::math::differentiation::discrete_lanczos_derivative;
using boost::math::differentiation::detail::discrete_legendre;
using boost::math::differentiation::detail::interior_velocity_filter;
using boost::math::differentiation::detail::boundary_velocity_filter;
using boost::math::tools::summation_condition_number;

template<class Real>
void test_dlp_norms()
{
    std::cout << "Testing Discrete Legendre Polynomial norms on type " << typeid(Real).name() << "\n";
    Real tol = std::numeric_limits<Real>::epsilon();
    auto dlp = discrete_legendre<Real>(1, Real(0));
    BOOST_CHECK_CLOSE_FRACTION(dlp.norm_sq(0), 3, tol);
    BOOST_CHECK_CLOSE_FRACTION(dlp.norm_sq(1), 2, tol);
    dlp = discrete_legendre<Real>(2, Real(0));
    BOOST_CHECK_CLOSE_FRACTION(dlp.norm_sq(0), Real(5)/Real(2), tol);
    BOOST_CHECK_CLOSE_FRACTION(dlp.norm_sq(1), Real(5)/Real(4), tol);
    BOOST_CHECK_CLOSE_FRACTION(dlp.norm_sq(2), Real(3*3*7)/Real(pow(2,6)), 2*tol);
    dlp = discrete_legendre<Real>(200, Real(0));
    for(size_t r = 0; r < 10; ++r)
    {
        Real calc = dlp.norm_sq(r);
        Real expected = Real(2)/Real(2*r+1);
        // As long as r << n, ||q_r||^2 -> 2/(2r+1) as n->infty
        BOOST_CHECK_CLOSE_FRACTION(calc, expected, 0.05);
    }

}

template<class Real>
void test_dlp_evaluation()
{
    std::cout << "Testing evaluation of Discrete Legendre polynomials on type " << typeid(Real).name() << "\n";
    Real tol = std::numeric_limits<Real>::epsilon();
    size_t n = 25;
    Real x = 0.72;
    auto dlp = discrete_legendre<Real>(n, x);
    Real q0 = dlp(x, 0);
    BOOST_TEST(q0 == 1);
    Real q1 = dlp(x, 1);
    BOOST_TEST(q1 == x);
    Real q2 = dlp(x, 2);
    int N = 2*n+1;
    Real expected = 0.5*(3*x*x - Real(N*N - 1)/Real(4*n*n));
    BOOST_CHECK_CLOSE_FRACTION(q2, expected, tol);
    Real q3 = dlp(x, 3);
    expected = (x/3)*(5*expected - (Real(N*N - 4))/(2*n*n));
    BOOST_CHECK_CLOSE_FRACTION(q3, expected, 2*tol);

    // q_r(x) is even for even r, and odd for odd r:
    for (size_t n = 8; n < 22; ++n)
    {
        dlp = discrete_legendre<Real>(n, x);
        for(size_t r = 2; r <= n; ++r)
        {
            if (r & 1)
            {
                Real q1 = dlp(x, r);
                Real q2 = -dlp(-x, r);
                BOOST_CHECK_CLOSE_FRACTION(q1, q2, tol);
            }
            else
            {
                Real q1 = dlp(x, r);
                Real q2 = dlp(-x, r);
                BOOST_CHECK_CLOSE_FRACTION(q1, q2, tol);
            }

            Real l2_sq = 0;
            for (int j = -(int)n; j <= (int) n; ++j)
            {
                Real y = Real(j)/Real(n);
                Real term = dlp(y, r);
                l2_sq += term*term;
            }
            l2_sq /= n;
            Real l2_sq_expected = dlp.norm_sq(r);
            BOOST_CHECK_CLOSE_FRACTION(l2_sq, l2_sq_expected, 20*tol);
        }
    }
}

template<class Real>
void test_dlp_next()
{
    std::cout << "Testing Discrete Legendre polynomial 'next' function on type " << typeid(Real).name() << "\n";
    Real tol = std::numeric_limits<Real>::epsilon();

    for(size_t n = 2; n < 20; ++n)
    {
        for(Real x = -1; x <= 1; x += 0.1)
        {
            auto dlp = discrete_legendre<Real>(n, x);
            for (size_t k = 2; k < n; ++k)
            {
                BOOST_CHECK_CLOSE(dlp.next(), dlp(x, k), tol);
            }

            dlp = discrete_legendre<Real>(n, x);
            for (size_t k = 2; k < n; ++k)
            {
                BOOST_CHECK_CLOSE(dlp.next_prime(), dlp.prime(x, k), tol);
            }
        }
    }
}

template<class Real>
void test_dlp_derivatives()
{
    std::cout << "Testing Discrete Legendre polynomial derivatives on type " << typeid(Real).name() << "\n";
    Real tol = 10*std::numeric_limits<Real>::epsilon();
    int n = 25;
    Real x = 0.72;
    auto dlp = discrete_legendre<Real>(n, x);
    Real q0p = dlp.prime(x, 0);
    BOOST_TEST(q0p == 0);
    Real q1p = dlp.prime(x, 1);
    BOOST_TEST(q1p == 1);
    Real q2p = dlp.prime(x, 2);
    Real expected = 3*x;
    BOOST_CHECK_CLOSE_FRACTION(q2p, expected, tol);
}

template<class Real>
void test_dlp_second_derivative()
{
    std::cout << "Testing Discrete Legendre polynomial derivatives on type " << typeid(Real).name() << "\n";
    int n = 25;
    Real x = Real(1)/Real(3);
    auto dlp = discrete_legendre<Real>(n, x);
    Real q2pp = dlp.next_dbl_prime();
    BOOST_TEST(q2pp == 3);
}

template<class Real>
void test_interior_velocity_filter()
{
    using boost::math::constants::half;
    std::cout << "Testing interior filter on type " << typeid(Real).name() << "\n";
    Real tol = std::numeric_limits<Real>::epsilon();
    for(int n = 1; n < 10; ++n)
    {
        for (int p = 1; p < n; p += 2)
        {
            auto f = interior_velocity_filter<Real>(n,p);
            // Since we only store half the filter coefficients,
            // we need to reindex the moment sums:
            auto cond = summation_condition_number<Real>(0);
            for (size_t j = 0; j < f.size(); ++j)
            {
                cond += j*f[j];
            }
            BOOST_CHECK_CLOSE_FRACTION(cond.sum(), half<Real>(), 2*cond()*tol);

            for (int l = 3; l <= p; l += 2)
            {
                cond = summation_condition_number<Real>(0);
                for (size_t j = 0; j < f.size() - 1; ++j)
                {
                    cond += pow(Real(j), l)*f[j];
                }
                Real expected = -pow(Real(f.size() - 1), l)*f[f.size()-1];
                BOOST_CHECK_CLOSE_FRACTION(expected, cond.sum(), 7*cond()*tol);
            }
            //std::cout << "(n,p) = (" << n  << "," << p << ") = {";
            //for (auto & x : f)
            //{
            //    std::cout << x << ", ";
            //}
            //std::cout << "}\n";
        }
    }
}

template<class Real>
void test_interior_lanczos()
{
    std::cout << "Testing interior Lanczos on type " << typeid(Real).name() << "\n";
    Real tol = std::numeric_limits<Real>::epsilon();
    std::vector<Real> v(500);
    std::fill(v.begin(), v.end(), 7);

    for (size_t n = 1; n < 10; ++n)
    {
        for (size_t p = 2; p < 2*n; p += 2)
        {
            auto dld = discrete_lanczos_derivative(Real(0.1), n, p);
            for (size_t m = n; m < v.size() - n; ++m)
            {
                Real dvdt = dld(v, m);
                BOOST_CHECK_SMALL(dvdt, tol);
            }
            auto dvdt = dld(v);
            for (size_t m = n; m < v.size() - n; ++m)
            {
                BOOST_CHECK_SMALL(dvdt[m], tol);
            }
        }
    }

    for(size_t i = 0; i < v.size(); ++i)
    {
        v[i] = 7*i+8;
    }

    for (size_t n = 1; n < 10; ++n)
    {
        for (size_t p = 2; p < 2*n; p += 2)
        {
            auto dld = discrete_lanczos_derivative(Real(1), n, p);
            for (size_t m = n; m < v.size() - n; ++m)
            {
                Real dvdt = dld(v, m);
                BOOST_CHECK_CLOSE_FRACTION(dvdt, 7, 2000*tol);
            }
            auto dvdt = dld(v);
            for (size_t m = n; m < v.size() - n; ++m)
            {
                BOOST_CHECK_CLOSE_FRACTION(dvdt[m], 7, 2000*tol);
            }
        }
    }

    //std::random_device rd{};
    //auto seed = rd();
    //std::cout << "Seed = " << seed << "\n";
    std::mt19937 gen(4172378669);
    std::normal_distribution<> dis{0, 0.01};
    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = 7*i+8 + dis(gen);
    }

    for (size_t n = 1; n < 10; ++n)
    {
        for (size_t p = 2; p < 2*n; p += 2)
        {
            auto dld = discrete_lanczos_derivative(Real(1), n, p);
            for (size_t m = n; m < v.size() - n; ++m)
            {
                BOOST_CHECK_CLOSE_FRACTION(dld(v, m), Real(7), Real(0.0042));
            }
        }
    }

    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = 15*i*i + 7*i+8 + dis(gen);
    }

    for (size_t n = 1; n < 10; ++n)
    {
        for (size_t p = 2; p < 2*n; p += 2)
        {
            auto dld = discrete_lanczos_derivative(Real(1), n, p);
            for (size_t m = n; m < v.size() - n; ++m)
            {
                BOOST_CHECK_CLOSE_FRACTION(dld(v,m), Real(30*m + 7), Real(0.00008));
            }
        }
    }

    std::normal_distribution<> dis1{0, 0.0001};
    Real omega = Real(1)/Real(16);
    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = sin(i*omega) + dis1(gen);
    }

    for (size_t n = 10; n < 20; ++n)
    {
        for (size_t p = 3; p < 100 && p < n/2; p += 2)
        {
            auto dld = discrete_lanczos_derivative(Real(1), n, p);

            for (size_t m = n; m < v.size() - n && m < n + 10; ++m)
            {
                BOOST_CHECK_CLOSE_FRACTION(dld(v,m), omega*cos(omega*m), Real(0.03));
            }
        }
    }
}

template<class Real>
void test_boundary_velocity_filters()
{
    std::cout << "Testing boundary filters on type " << typeid(Real).name() << "\n";
    Real tol = std::numeric_limits<Real>::epsilon();
    for(int n = 1; n < 5; ++n)
    {
        for (int p = 1; p < 2*n+1; ++p)
        {
            for (int s = -n; s <= n; ++s)
            {
                auto f = boundary_velocity_filter<Real>(n, p, s);
                // Sum is zero:
                auto cond = summation_condition_number<Real>(0);
                for (size_t i = 0; i < f.size() - 1; ++i)
                {
                    cond += f[i];
                }

                BOOST_CHECK_CLOSE_FRACTION(cond.sum(), -f[f.size()-1], 6*cond()*tol);

                cond = summation_condition_number<Real>(0);
                for (size_t k = 0; k < f.size(); ++k)
                {
                    Real j = Real(k) - Real(n);
                    // note the shifted index here:
                    cond += (j-s)*f[k];
                }
                BOOST_CHECK_CLOSE_FRACTION(cond.sum(), 1, 6*cond()*tol);

                for (int l = 2; l <= p; ++l)
                {
                    cond = summation_condition_number<Real>(0);
                    for (size_t k = 0; k < f.size() - 1; ++k)
                    {
                        Real j = Real(k) - Real(n);
                        // The condition number of this sum is infinite!
                        // No need to get to worked up about the tolerance.
                        cond += pow(j-s, l)*f[k];
                    }

                    Real expected = -pow(Real(f.size()-1) - Real(n) - Real(s), l)*f[f.size()-1];
                    if (expected == 0)
                    {
                        BOOST_CHECK_SMALL(cond.sum(), cond()*tol);
                    }
                    else
                    {
                        BOOST_CHECK_CLOSE_FRACTION(expected, cond.sum(), 200*cond()*tol);
                    }
                }

                //std::cout << "(n,p,s) = ("<< n << ", " << p << "," << s << ") = {";
                //for (auto & x : f)
                //{
                //    std::cout << x << ", ";
                //}
                //std::cout << "}\n";*/
            }
        }
    }
}

template<class Real>
void test_boundary_lanczos()
{
    std::cout << "Testing Lanczos boundary on type " << typeid(Real).name() << "\n";
    Real tol = std::numeric_limits<Real>::epsilon();
    std::vector<Real> v(500, 7);

    for (size_t n = 1; n < 10; ++n)
    {
        for (size_t p = 2; p < 2*n; ++p)
        {
            auto lsd = discrete_lanczos_derivative(Real(0.0125), n, p);
            for (size_t m = 0; m < n; ++m)
            {
                Real dvdt = lsd(v,m);
                BOOST_CHECK_SMALL(dvdt, 4*sqrt(tol));
            }
            for (size_t m = v.size() - n; m < v.size(); ++m)
            {
                Real dvdt = lsd(v,m);
                BOOST_CHECK_SMALL(dvdt, 4*sqrt(tol));
            }
        }
    }

    for(size_t i = 0; i < v.size(); ++i)
    {
        v[i] = 7*i+8;
    }

    for (size_t n = 3; n < 10; ++n)
    {
        for (size_t p = 2; p < 2*n; ++p)
        {
            auto lsd = discrete_lanczos_derivative(Real(1), n, p);
            for (size_t m = 0; m < n; ++m)
            {
                Real dvdt = lsd(v,m);
                BOOST_CHECK_CLOSE_FRACTION(dvdt, 7, sqrt(tol));
            }

            for (size_t m = v.size() - n; m < v.size(); ++m)
            {
                Real dvdt = lsd(v,m);
                BOOST_CHECK_CLOSE_FRACTION(dvdt, 7, 4*sqrt(tol));
            }
        }
    }

    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = 15*i*i + 7*i+8;
    }

    for (size_t n = 1; n < 10; ++n)
    {
        for (size_t p = 2; p < 2*n; ++p)
        {
            auto lsd = discrete_lanczos_derivative(Real(1), n, p);
            for (size_t m = 0; m < v.size(); ++m)
            {
                BOOST_CHECK_CLOSE_FRACTION(lsd(v,m), 30*m+7, 30*sqrt(tol));
            }
        }
    }

    // Demonstrate that the boundary filters are also denoising:
    //std::random_device rd{};
    //auto seed = rd();
    //std::cout << "seed = " << seed << "\n";
    std::mt19937 gen(311354333);
    std::normal_distribution<> dis{0, 0.01};
    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] += dis(gen);
    }

    for (size_t n = 1; n < 10; ++n)
    {
        for (size_t p = 2; p < n; ++p)
        {
            auto lsd = discrete_lanczos_derivative(Real(1), n, p);
            for (size_t m = 0; m < v.size(); ++m)
            {
                BOOST_CHECK_CLOSE_FRACTION(lsd(v,m), 30*m+7, 0.005);
            }
            auto dvdt = lsd(v);
            for (size_t m = 0; m < v.size(); ++m)
            {
                BOOST_CHECK_CLOSE_FRACTION(dvdt[m], 30*m+7, 0.005);
            }
        }
    }
}

template<class Real>
void test_acceleration_filters()
{
    Real eps = std::numeric_limits<Real>::epsilon();
    for (size_t n = 1; n < 5; ++n)
    {
        for(size_t p = 3; p <= 2*n; ++p)
        {
            for(int64_t s = -int64_t(n); s <= 0; ++s)
            {
                auto g = boost::math::differentiation::detail::acceleration_filter<long double>(n,p,s);

                std::vector<Real> f(g.size());
                for (size_t i = 0; i < g.size(); ++i)
                {
                    f[i] = static_cast<Real>(g[i]);
                }

                auto cond = summation_condition_number<Real>(0);

                for (size_t i = 0; i < f.size() - 1; ++i)
                {
                    cond += f[i];
                }
                BOOST_CHECK_CLOSE_FRACTION(cond.sum(), -f[f.size()-1], 10*cond()*eps);

                cond = summation_condition_number<Real>(0);
                for (size_t k = 0; k < f.size() -1; ++k)
                {
                    Real j = Real(k) - Real(n);
                    cond += (j-s)*f[k];
                }
                Real expected = -(Real(f.size()-1)- Real(n) - s)*f[f.size()-1];
                BOOST_CHECK_CLOSE_FRACTION(cond.sum(), expected, 10*cond()*eps);

                cond = summation_condition_number<Real>(0);
                for (size_t k = 0; k < f.size(); ++k)
                {
                    Real j = Real(k) - Real(n);
                    cond += (j-s)*(j-s)*f[k];
                }
                BOOST_CHECK_CLOSE_FRACTION(cond.sum(), 2, 100*cond()*eps);
                // See unlabelled equation in McDevitt, 2012, just after equation 26:
                // It appears that there is an off-by-one error in that equation, since p + 1 moments don't vanish, only p.
                // This test is itself suspect; the condition number of the moment sum is infinite.
                // So the *slightest* error in the filter gets amplified by the test; in terms of the
                // behavior of the actual filter, it's not a big deal.
                for (size_t l = 3; l <= p; ++l)
                {
                    cond = summation_condition_number<Real>(0);
                    for (size_t k = 0; k < f.size() - 1; ++k)
                    {
                        Real j = Real(k) - Real(n);
                        cond += pow((j-s), l)*f[k];
                    }
                    Real expected = -pow(Real(f.size()- 1 - n -s), l)*f[f.size()-1];
                    BOOST_CHECK_CLOSE_FRACTION(cond.sum(), expected, 1000*cond()*eps);
                }
            }
        }
    }
}

template<class Real>
void test_lanczos_acceleration()
{
    Real eps = std::numeric_limits<Real>::epsilon();
    std::vector<Real> v(100, 7);
    auto lanczos = discrete_lanczos_derivative<Real, 2>(Real(1), 4, 3);
    for (size_t i = 0; i < v.size(); ++i)
    {
        BOOST_CHECK_SMALL(lanczos(v, i), eps);
    }

    for(size_t i = 0; i < v.size(); ++i)
    {
        v[i] = 7*i + 6;
    }
    for (size_t i = 0; i < v.size(); ++i)
    {
        BOOST_CHECK_SMALL(lanczos(v,i), 200*eps);
    }

    for(size_t i = 0; i < v.size(); ++i)
    {
        v[i] = 7*i*i + 9*i + 6;
    }
    for (size_t i = 0; i < v.size(); ++i)
    {
        BOOST_CHECK_CLOSE_FRACTION(lanczos(v, i), 14, 1500*eps);
    }

    // Now add noise, and kick up the smoothing of the Lanzcos derivative (increase n):
    //std::random_device rd{};
    //auto seed = rd();
    //std::cout << "seed = " << seed << "\n";
    size_t seed = 2507134629;
    std::mt19937 gen(seed);
    Real std_dev = 0.1;
    std::normal_distribution<Real> dis{0, std_dev};
    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] += dis(gen);
    }
    lanczos = discrete_lanczos_derivative<Real, 2>(Real(1), 18, 3);
    auto w = lanczos(v);
    for (size_t i = 0; i < v.size(); ++i)
    {
        BOOST_CHECK_CLOSE_FRACTION(w[i], 14, std_dev/200);
    }
}

template<class Real>
void test_rescaling()
{
    std::cout << "Test rescaling on type " << typeid(Real).name() << "\n";
    Real tol = std::numeric_limits<Real>::epsilon();
    std::vector<Real> v(500);
    for(size_t i = 0; i < v.size(); ++i)
    {
        v[i] = 7*i*i + 9*i + 6;
    }
    std::vector<Real> dvdt1(500);
    std::vector<Real> dvdt2(500);
    auto lanczos1 = discrete_lanczos_derivative(Real(1));
    auto lanczos2 = discrete_lanczos_derivative(Real(2));

    lanczos1(v, dvdt1);
    lanczos2(v, dvdt2);

    for(size_t i = 0; i < v.size(); ++i)
    {
        BOOST_CHECK_CLOSE_FRACTION(dvdt1[i], 2*dvdt2[i], tol);
    }

    auto lanczos3 = discrete_lanczos_derivative<Real, 2>(Real(1));
    auto lanczos4 = discrete_lanczos_derivative<Real, 2>(Real(2));

    std::vector<Real> dv2dt21(500);
    std::vector<Real> dv2dt22(500);

    for(size_t i = 0; i < v.size(); ++i)
    {
        BOOST_CHECK_CLOSE_FRACTION(dv2dt21[i], 4*dv2dt22[i], tol);
    }
}

template<class Real>
void test_data_representations()
{
    std::cout << "Test rescaling on type " << typeid(Real).name() << "\n";
    Real tol = 150*std::numeric_limits<Real>::epsilon();
    std::array<Real, 500> v;
    for(size_t i = 0; i < v.size(); ++i)
    {
        v[i] = 9*i + 6;
    }
    std::array<Real, 500> dvdt;
    auto lanczos = discrete_lanczos_derivative(Real(1));

    lanczos(v, dvdt);

    for(size_t i = 0; i < v.size(); ++i)
    {
        BOOST_CHECK_CLOSE_FRACTION(dvdt[i], 9, tol);
    }

    boost::numeric::ublas::vector<Real> w(500);
    boost::numeric::ublas::vector<Real> dwdt(500);
    for(size_t i = 0; i < w.size(); ++i)
    {
        w[i] = 9*i + 6;
    }

    lanczos(w, dwdt);

    for(size_t i = 0; i < v.size(); ++i)
    {
        BOOST_CHECK_CLOSE_FRACTION(dwdt[i], 9, tol);
    }

    auto v1 = boost::make_iterator_range(v.begin(), v.end());
    auto v2 = boost::make_iterator_range(dvdt.begin(), dvdt.end());
    lanczos(v1, v2);

    for(size_t i = 0; i < v2.size(); ++i)
    {
        BOOST_CHECK_CLOSE_FRACTION(v2[i], 9, tol);
    }

    auto lanczos2 = discrete_lanczos_derivative<Real, 2>(Real(1));

    lanczos2(v1, v2);

    for(size_t i = 0; i < v2.size(); ++i)
    {
        BOOST_CHECK_SMALL(v2[i], 10*tol);
    }

}

BOOST_AUTO_TEST_CASE(lanczos_smoothing_test)
{
    test_dlp_second_derivative<double>();
    test_dlp_norms<double>();
    test_dlp_evaluation<double>();
    test_dlp_derivatives<double>();
    test_dlp_next<double>();

    // Takes too long!
    //test_dlp_norms<cpp_bin_float_50>();
    test_boundary_velocity_filters<double>();
    test_boundary_velocity_filters<long double>();
    // Takes too long!
    //test_boundary_velocity_filters<cpp_bin_float_50>();
    test_boundary_lanczos<double>();
    test_boundary_lanczos<long double>();
    // Takes too long!
    //test_boundary_lanczos<cpp_bin_float_50>();

    test_interior_velocity_filter<double>();
    test_interior_velocity_filter<long double>();
    test_interior_lanczos<double>();

    test_acceleration_filters<double>();

    test_lanczos_acceleration<float>();
    test_lanczos_acceleration<double>();

    test_rescaling<double>();
    test_data_representations<double>();
}


#include <iostream>
#include <iomanip>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <random>
#include <set>

constexpr int seed = 1;

using Mat33_t = Eigen::Matrix3d;
using Vec3_t = Eigen::Vector3d;
using Vec2_t = Eigen::Vector2d;

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

eigen_vector<Vec3_t> gen_3Dpoints(const int n) {

    std::mt19937 mt(seed);
    const double z_dist = 3;
    const double r = 1;
    std::uniform_real_distribution<> rand(-r, r);

    eigen_vector<Vec3_t> pts;
    pts.reserve(n);
    for (unsigned int i = 0; i < n; i++) {
        const double x = rand(mt);
        const double y = rand(mt);
        const double z = rand(mt) + z_dist;
        pts.emplace_back(x, y, z);
    }
    return pts;
}

template<typename T>
void generate_keypts(const int num_pts, const double outlier_ratio,
                     T &kps_1, T &kps_2, std::vector<bool> &is_inliers) {

    double data[9];
    data[0] = 0.99875035875531126;
    data[1] = 0.043232155011259037;
    data[2] = 0.025073923889563768;
    data[3] = -0.043243275321131029;
    data[4] = 0.9990645671709939;
    data[5] = -9.8807568748168046e-05;
    data[6] = -0.025054740582133875;
    data[7] = -0.00098559449940248617;
    data[8] = 0.99968559486362751;

    Mat33_t R(data);
    Vec3_t t{-0.49, 0.12, -0.05};

    eigen_vector<Vec3_t> pts = gen_3Dpoints(num_pts);

    Mat33_t K;
    K << 500, 0, 500,
            0, 500, 250,
            0, 0, 1;

    kps_1.clear();
    kps_1.reserve(num_pts);
    kps_2.clear();
    kps_2.reserve(num_pts);

    std::mt19937 mt(seed);
    const double r = 1;
    std::uniform_real_distribution<> rand(-r, r);
    const auto try_bernoulli = [&mt](const double p) {
        std::uniform_real_distribution<> rand(0, 1);
        return rand(mt) < p;
    };

    for (const auto &pos_w : pts) {

        const double x = rand(mt);
        const double y = rand(mt);
        const double z = rand(mt);
        const Vec3_t noise{x, y, z};

        const bool is_outlier = try_bernoulli(outlier_ratio);

        const Vec3_t &pos_1 = pos_w;
        const Vec3_t pos_2 = R * pos_w + t
                             + (is_outlier ? noise : Vec3_t::Zero());

        kps_1.emplace_back((K * pos_1).hnormalized());
        kps_2.emplace_back((K * pos_2).hnormalized());
        is_inliers.push_back(!is_outlier);
    }
}

// Fundamental solver; Modified version of eightpt solver from opengv
// Reference:
// https://github.com/laurentkneip/opengv/blob/91f4b19c73450833a40e463ad3648aae80b3a7f3/src/relative_pose/methods.cpp#L424
template<typename T>
Mat33_t solve_F(const T &kps_1, const T &kps_2) {
    const int num_pts = kps_1.size();
    Eigen::MatrixXd A(num_pts, 9);

    for (unsigned int i = 0; i < num_pts; i++) {
        A.block<1, 3>(i, 0) = kps_2.at(i)(0) * kps_1.at(i).homogeneous();
        A.block<1, 3>(i, 3) = kps_2.at(i)(1) * kps_1.at(i).homogeneous();
        A.block<1, 3>(i, 6) = kps_1.at(i).homogeneous();
    }

    const Eigen::JacobiSVD<Eigen::MatrixXd> SVD(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
    const Eigen::Matrix<Mat33_t::Scalar, 9, 1> f = SVD.matrixV().col(8);
    Mat33_t F_temp(3, 3);
    F_temp.row(0) = f.block<3, 1>(0, 0).transpose();
    F_temp.row(1) = f.block<3, 1>(3, 0).transpose();
    F_temp.row(2) = f.block<3, 1>(6, 0).transpose();

    const Eigen::JacobiSVD<Mat33_t> SVD2(F_temp, Eigen::ComputeFullU | Eigen::ComputeFullV);

    Vec3_t s = SVD2.singularValues();
    s(2) = 0.0;

    const Mat33_t &U = SVD2.matrixU();
    const Mat33_t S = s.asDiagonal();
    const Mat33_t &V_T = SVD2.matrixV().transpose();

    return U * S * V_T;
}

template<typename T>
std::vector<bool> check_inlier(const Mat33_t &F, const T &kps_1, const T &kps_2) {
    std::vector<bool> inl_flag(kps_1.size(), false);
    for (int i = 0; i < kps_1.size(); i++) {
        const double err = kps_2.at(i).homogeneous().dot(F * kps_1.at(i).homogeneous());
        inl_flag.at(i) = std::abs(err) < 1e-8;
    }
    return inl_flag;
}

// Find F matrix by ransac
template<typename T>
Mat33_t estimate_F(const T &kps_1, const T &kps_2, const int num_trial) {
    const auto num_pts = kps_1.size();
    const auto num_sample = std::min<int>(8, num_pts);

    std::uniform_int_distribution<> dist(0, num_pts - 1);
    std::mt19937 mt(seed);

    Mat33_t F;
    int max_inl = 0;

    for (unsigned int i = 0; i < num_trial; i++) {
        std::set<int> sample_id;
        while (sample_id.size() < num_sample) {
            sample_id.insert(dist(mt));
        }

        T smpl1, smpl2;
        for (const auto id: sample_id) {
            smpl1.push_back(kps_1.at(id));
            smpl2.push_back(kps_2.at(id));
        }
        const Mat33_t F_cand = solve_F(smpl1, smpl2);
        const auto inl_flag = check_inlier(F_cand, kps_1, kps_2);
        const int num_inl = std::count(inl_flag.begin(), inl_flag.end(), true);
        if (max_inl < num_inl) {
            max_inl = num_inl;
            F = F_cand;
        }
    }

    return F;
}

template<typename T>
void print_test_result(const T &is_inliers, const T &inl_flag) {
    int true_pos = 0;
    int true_neg = 0;
    int false_pos = 0;
    int false_neg = 0;

    const auto num_pts = is_inliers.size();
    for (int i = 0; i < num_pts; i++) {
        const bool gt = is_inliers.at(i);
        const bool est = inl_flag.at(i);
        if (gt == est) {
            if (gt) true_pos++;
            else true_neg++;
        } else {
            if (est) false_pos++;
            else false_neg++;
        }
    }

    std::cout << std::endl;
    std::cout << "<< Confusion Matrix >>" << std::endl;
    std::cout << " GT \\ est | inlier | outlier |" << std::endl;

    std::cout << "  inlier  |  ";
    std::cout << std::setw(4) << true_pos << "  |  ";
    std::cout << std::setw(4) << false_neg << "   |" << std::endl;

    std::cout << " outlier  |  ";
    std::cout << std::setw(4) << false_pos << "  |  ";
    std::cout << std::setw(4) << true_neg << "   |" << std::endl;
}

int main() {
    const int num_pts = 1000;
    const double outlier_ratio = 0.3;

    // テスト用の特徴点と外れ値かどうかの正解
    eigen_vector<Vec2_t> kps_1, kps_2;
    std::vector<bool> is_inliers;
    generate_keypts(num_pts, outlier_ratio, kps_1, kps_2, is_inliers);

    // F行列を推定
    // num_trial は (1/(1-outlier_ratio)) ^ 8 より大きく
    const Mat33_t F = estimate_F(kps_1, kps_2, 100);

    // 推定したF行列を用いて特徴点マッチのインライア判定（インデックスでマッチ）
    const auto inl_flag = check_inlier(F, kps_1, kps_2);

    // F行列を用いたインライア判定の結果の表示
    const auto num_inls = std::count(inl_flag.begin(), inl_flag.end(), true);
    std::cout << "#inlier: " << num_inls << std::endl;

    // F行列推定による外れ値検出の正当性の表示
    print_test_result(is_inliers, inl_flag);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[envelope
//` Shows how to calculate the bounding box of a polygon

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>

/*<-*/ #include "create_svg_two.hpp" /*->*/

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point;

    boost::geometry::model::polygon<point> polygon;

    boost::geometry::read_wkt(
        "POLYGON((2 1.3,2.4 1.7,2.8 1.8,3.4 1.2,3.7 1.6,3.4 2,4.1 3,5.3 2.6,5.4 1.2,4.9 0.8,2.9 0.7,2 1.3)"
            "(4.0 2.0, 4.2 1.4, 4.8 1.9, 4.4 2.2, 4.0 2.0))", polygon);

    boost::geometry::model::box<point> box;
    boost::geometry::envelope(polygon, box);

    std::cout << "envelope:" << boost::geometry::dsv(box) << std::endl;

    /*<-*/ create_svg("envelope.svg", polygon, box); /*->*/
    return 0;
}

//]

//[envelope_output
/*`
Output:
[pre
envelope:((2, 0.7), (5.4, 3))

[$img/algorithms/envelope.png]
]
*/
//]


#include <boost/test/unit_test.hpp>
#include "types/ds/hash_functions.hpp"
#include <limits>
#include <iostream>

BOOST_AUTO_TEST_SUITE(TestHashFunctions)

    BOOST_AUTO_TEST_CASE(hash_division_method_m_is_power_of_2)
    {
        const size_t num_of_buckets = 16;
        Types::DS::HashModulo hash_func(num_of_buckets);
        std::vector<size_t> stat(num_of_buckets, 0);
        const auto max = static_cast<size_t>(std::numeric_limits<uint8_t>::max());
        for (size_t i = 0; i < max; ++i) {
            ++stat[hash_func(i)];
        }

//        std::cout << "Test of hash function that uses division method. Num of buckets is " <<
//            "power of 2: " << num_of_buckets << std::endl;
//        std::cout << "Here is statistics that shows how many numbers [0, max(uchar)] would be " <<
//            "assigned to the same bucket" << std::endl;
//        for (size_t i = 0; i < stat.size(); ++i) {
//            std::cout << i << ": " << stat[i] << std::endl;
//        }
    }

    BOOST_AUTO_TEST_CASE(hash_multiplication_method)
    {
        const size_t num_of_buckets = 16;
        Types::DS::HashMultiplication hash_func(num_of_buckets);
        std::vector<size_t> stat(num_of_buckets, 0);
        const auto max = static_cast<size_t>(std::numeric_limits<uint8_t>::max());
        for (size_t i = 0; i < max; ++i) {
            ++stat[hash_func(i)];
        }

//        std::cout << "Test of hash function that uses multiplication method. Num of buckets is " <<
//                  "power of 2: " << num_of_buckets << std::endl;
//        std::cout << "Here is statistics that shows how many numbers [0, max(uchar)] would be " <<
//                  "assigned to the same bucket" << std::endl;
//        for (size_t i = 0; i < stat.size(); ++i) {
//            std::cout << i << ": " << stat[i] << std::endl;
//        }
    }

    BOOST_AUTO_TEST_CASE(hash_multiply_shift_method)
    {
        const size_t num_of_buckets = 16;
        Types::DS::HashMultiplyShift hash_func(num_of_buckets);
        std::vector<size_t> stat(num_of_buckets, 0);
        const auto max = static_cast<int32_t>(std::numeric_limits<uint8_t>::max());
        for (int32_t i = 1; i < max; ++i) {
            ++stat[hash_func(i)];
        }

//        std::cout << "Test of hash function that uses multiply-shift method. Num of buckets is " <<
//                  "power of 2: " << num_of_buckets << std::endl;
//        std::cout << "Here is statistics that shows how many numbers [0, max(uchar)] would be " <<
//                  "assigned to the same bucket" << std::endl;
//        for (size_t i = 0; i < stat.size(); ++i) {
//            std::cout << i << ": " << stat[i] << std::endl;
//        }
    }

    BOOST_AUTO_TEST_CASE(hash_multiply_add_shift_method)
    {
        const size_t num_of_buckets = 16;
        Types::DS::HashMultiplyAddShift hash_func(num_of_buckets);
        std::vector<size_t> stat(num_of_buckets, 0);
        const auto max = static_cast<int32_t>(std::numeric_limits<uint8_t>::max());
        for (int32_t i = 1; i < max; ++i) {
            ++stat[hash_func(i)];
        }

//        std::cout << "Test of hash function that uses multiply-add-shift method. Num of buckets is " <<
//                  "power of 2: " << num_of_buckets << std::endl;
//        std::cout << "Here is statistics that shows how many numbers [0, max(uchar)] would be " <<
//                  "assigned to the same bucket" << std::endl;
//        for (size_t i = 0; i < stat.size(); ++i) {
//            std::cout << i << ": " << stat[i] << std::endl;
//        }
    }

    BOOST_AUTO_TEST_CASE(hash_vector)
    {
        const size_t num_of_buckets = 16;
        Types::DS::HashVector hash_func(num_of_buckets);
        std::vector<size_t> stat(num_of_buckets, 0);

        Types::DS::helpers::RandomGenerator size_generator(10);
        Types::DS::helpers::RandomGenerator num_generator(8);
        for (size_t i = 0; i < 100; ++i) {
            std::vector<int32_t> v(size_generator.generate(), 0);
            std::generate(
                v.begin(), v.end(), [&num_generator](){ return num_generator.generate(); });

            ++stat[hash_func(v)];
        }

//        std::cout << "Test of hash function for vectors. Num of buckets is " <<
//                  "power of 2: " << num_of_buckets << std::endl;
//        std::cout << "Here is statistics that shows how many vectors would be " <<
//                  "assigned to the same bucket" << std::endl;
//        for (size_t i = 0; i < stat.size(); ++i) {
//            std::cout << i << ": " << stat[i] << std::endl;
//        }
    }

    BOOST_AUTO_TEST_CASE(hash_string)
    {
        const size_t num_of_buckets = 16;
        Types::DS::HashString<char> hash_func(num_of_buckets);
        std::vector<size_t> stat(num_of_buckets, 0);

        Types::DS::helpers::RandomGenerator size_generator(10);
        Types::DS::helpers::RandomGenerator char_generator(1, 127);
        for (size_t i = 0; i < 100; ++i) {
            std::string s(size_generator.generate(), '0');
            std::generate(
                s.begin(), s.end(), [&char_generator](){ return char_generator.generate(); });

            ++stat[hash_func(s)];
        }

//        std::cout << "Test of hash function for vectors. Num of buckets is " <<
//                  "power of 2: " << num_of_buckets << std::endl;
//        std::cout << "Here is statistics that shows how many vectors would be " <<
//                  "assigned to the same bucket" << std::endl;
//        for (size_t i = 0; i < stat.size(); ++i) {
//            std::cout << i << ": " << stat[i] << std::endl;
//        }
    }

BOOST_AUTO_TEST_SUITE_END()


/*
 * utilities_cumtrapz_test.cpp Test fixtures for the cumtrapz function
 *
 * Author:                   Tom Clark (thclark @ github)
 *
 * Copyright (c) 2016-9 Octue Ltd. All Rights Reserved.
 *
 */

#include "gtest/gtest.h"
#include <Eigen/Dense>

#include "utilities/interp.h"

using namespace utilities;

class InterpTest : public ::testing::Test {};

TEST_F(InterpTest, test_linear_interp_double) {

    Eigen::VectorXd x(5);
    Eigen::VectorXd y(5);

    x << 0, 1, 2, 3, 4;
    y << 0, 2, 4, 6, 8;
    double xi = 1.5;

    utilities::LinearInterpolant s(x, y);
    double yi = s(xi);

    ASSERT_NEAR(yi, 3.0, 0.00001);

}

TEST_F(InterpTest, test_linear_interp_at_nodes) {

    Eigen::VectorXd x(5);
    Eigen::VectorXd y(5);
    x << 0, 1, 2, 3, 4;
    y << 0, 2, 4, 6, 8;
    utilities::LinearInterpolant s(x, y);

    // First node (endpoint)
    double xi = 0.0;
    double yi = s(xi);
    ASSERT_NEAR(yi, 0.0, 0.00001);

    // Any inter node
    xi = 3.0;
    yi = s(xi);
    ASSERT_NEAR(yi, 6.0, 0.00001);

    // Last node (endpoint)
    xi = 4.0;
    yi = s(xi);
    ASSERT_NEAR(yi, 8.0, 0.00001);

}

TEST_F(InterpTest, test_linear_interp_beyond_endpoints) {

    Eigen::VectorXd x(5);
    Eigen::VectorXd y(5);
    x << 0, 1, 2, 3, 4;
    y << 0, 2, 4, 6, 8;
    utilities::LinearInterpolant s(x, y);

    double xi = -1.0;
    double yi = s(xi);
    ASSERT_NEAR(yi, 0.0, 0.00001);

    xi = 5.0;
    yi = s(xi);
    ASSERT_NEAR(yi, 8.0, 0.00001);

}

TEST_F(InterpTest, test_linear_interp_vector) {

    Eigen::ArrayXd x(5);
    Eigen::ArrayXd xi(2);
    Eigen::ArrayXd y(5);
    Eigen::ArrayXd yi(2);
    Eigen::ArrayXd yi_correct(2);

    x << 0, 1, 2, 3, 4;
    y << 0, 2, 4, 6, 8;
    xi << 0.5, 1.5;
    yi_correct << 1.0, 3.0;

    utilities::LinearInterpolant s(x, y);
    yi = s(xi);

    ASSERT_TRUE(yi.isApprox(yi_correct));

}

TEST_F(InterpTest, test_cubic_interp_double) {

    Eigen::VectorXd x(5);
    Eigen::VectorXd y(5);

    x << 0, 1, 2, 3, 4;
    y << 0, 1, 4, 9, 16;
    double xi = 1.5;

    utilities::CubicSplineInterpolant s(x, y);
    double yi = s(xi);

    ASSERT_NEAR(yi, 2.25, 0.00001);

}

TEST_F(InterpTest, test_cubic_interp_vector) {

    Eigen::VectorXd x(5);
    Eigen::VectorXd xi(2);
    Eigen::VectorXd y(5);
    Eigen::VectorXd yi(2);
    Eigen::VectorXd yi_correct(2);

    x << 0, 1, 2, 3, 4;
    y << 0, 1, 4, 9, 16;
    xi << 0.5, 1.5;
    yi_correct << 0.25, 2.25;

    utilities::CubicSplineInterpolant s(x, y);
    yi = s(xi);

    ASSERT_TRUE(yi.isApprox(yi_correct));

}


#include "eigen-helpers.hpp"

#include "perceive/geometry/rotation.hpp"

#include <Eigen/SVD>

using std::stringstream;

namespace perceive
{
// ------------------------------------------------------------------- Is finite
// Checks that an Eigen Matrix is finite
template<typename T> bool is_finiteT(const T& M)
{
   int n_rows = int(M.rows());
   int n_cols = int(M.cols());
   for(int i = 0; i < n_rows; ++i)
      for(int j = 0; j < n_cols; ++j)
         if(!std::isfinite(M(i, j))) return false;
   return true;
}

bool is_finite(const Vector2r& M) { return is_finiteT(M); }
bool is_finite(const Vector3r& M) { return is_finiteT(M); }
bool is_finite(const Vector4r& M) { return is_finiteT(M); }
bool is_finite(const Matrix3r& M) { return is_finiteT(M); }
bool is_finite(const Matrix34r& M) { return is_finiteT(M); }
bool is_finite(const MatrixXr& M) { return is_finiteT(M); }

// -------------------------------------------------------------------- str(...)

std::string str(const Vector2r& M) { return format("[{}, {}]", M(0), M(1)); }
std::string str(const Vector3r& M)
{
   return format("[{}, {}, {}]", M(0), M(1), M(2));
}
std::string str(const Vector4r& M)
{
   return format("[{}, {}, {}, {}]", M(0), M(1), M(2), M(3));
}
std::string str(const Matrix3r& M)
{
   stringstream ss("");
   ss << M;
   return ss.str();
}
std::string str(const Matrix34r& M)
{
   stringstream ss("");
   ss << M;
   return ss.str();
}
std::string str(const MatrixXr& M)
{
   stringstream ss("");
   ss << M;
   return ss.str();
}

std::string str(std::string name, const Matrix3r& M)
{
   stringstream ss("");
   ss << name << " = \n" << M << "\n";
   return ss.str();
}
std::string str(std::string name, const Matrix34r& M)
{
   stringstream ss("");
   ss << name << " = \n" << M << "\n";
   return ss.str();
}
std::string str(std::string name, const MatrixXr& M)
{
   stringstream ss("");
   ss << name << " = \n" << M << "\n";
   return ss.str();
}

// ------------------------------------------------------------------------- SVD

// -- Thin

real svd_thin(const MatrixXr& M, VectorXr& out)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinV);
   out = svd.matrixV().col(M.cols() - 1);
   return svd.singularValues()(M.cols() - 1);
}

real svd_thin(const Matrix3r& M, Vector3r& out)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinV);
   out = svd.matrixV().col(M.cols() - 1);
   return svd.singularValues()(M.cols() - 1);
}

// real svd_thin(const Matrix3d& M, Vector3d& out)
// {
//     Eigen::JacobiSVD<MatrixXd> svd(M, Eigen::ComputeThinV);
//     out = svd.matrixV().col(M.cols() - 1);
//     return svd.singularValues()(M.cols() - 1);
// }

// real svd_thin(const MatrixXd& M, VectorXd& out)
// {
//     Eigen::JacobiSVD<MatrixXd> svd(M, Eigen::ComputeThinV);
//     out = svd.matrixV().col(M.cols() - 1);
//     return svd.singularValues()(M.cols() - 1);
// }

real svd_thin(const MatrixXr& M, Vector6r& out)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinV);
   out = svd.matrixV().col(M.cols() - 1);
   return svd.singularValues()(M.cols() - 1);
}

// -- UD

void svd_UV(const MatrixXr& M, MatrixXr& U, MatrixXr& V)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U = svd.matrixU();
   V = svd.matrixV();
}

void svd_UV(const Matrix3r& M, Matrix3r& U, Matrix3r& V)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U = svd.matrixU();
   V = svd.matrixV();
}

// void svd_UV(const MatrixXd& M, MatrixXd& U, MatrixXd& V)
// {
//     Eigen::JacobiSVD<MatrixXd> svd(M,
//     Eigen::ComputeThinU|Eigen::ComputeThinV); U = svd.matrixU(); V =
//     svd.matrixV();
// }

// -- UDV

void svd_UDV(const MatrixXr& M, MatrixXr& U, VectorXr& D, MatrixXr& V)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U      = svd.matrixU();
   V      = svd.matrixV();
   auto s = svd.singularValues();
   auto n = s.rows();
   D.resize(n);
   for(uint i = 0; i < n; ++i) D(i) = s(i);
}

void svd_UDV(const Matrix3r& M, Matrix3r& U, Vector3r& D, Matrix3r& V)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U      = svd.matrixU();
   V      = svd.matrixV();
   auto s = svd.singularValues();
   for(uint i = 0; i < 3; ++i) D(i) = s(i);
}

void svd_UDV(const Matrix3r& M, Matrix3r& U, Matrix3r& D, Matrix3r& V)
{
   Vector3r d;
   svd_UDV(M, U, d, V);
   D = Matrix3r::Zero();
   for(auto i = 0; i < 3; ++i) D(i, i) = d(i);
}

// void svd_UDV(const MatrixXd& M, MatrixXd& U, VectorXd& D, MatrixXd& V)
// {
//     Eigen::JacobiSVD<MatrixXd> svd(M,
//     Eigen::ComputeThinU|Eigen::ComputeThinV); U = svd.matrixU(); V =
//     svd.matrixV(); auto s = svd.singularValues(); auto n = s.rows();
//     D.resize(n);
//     for(uint i = 0; i < n; ++i)
//         D(i) = s(i);
// }

Matrix3r SVD3DRet::Dm() const noexcept
{
   Matrix3r X = Matrix3r::Identity();
   for(auto i = 0; i < 3; ++i) X(i, i) = D(i);
   return X;
}

string SVD3DRet::to_string() const noexcept
{
   std::stringstream ss{""};
   ss << format("U = \n") << U << "\n\n"
      << format("D = \n") << Dm() << "\n\n"
      << format("V = \n") << V << "\n\n";
   return ss.str();
}

Vector3r SVD3DRet::eigen_vector(int ind) const noexcept
{
   Expects(ind >= 0 && ind < 3);
   return Vector3r(V(ind, 0), V(ind, 1), V(ind, 2));
}

Quaternion SVD3DRet::rot_vec() const noexcept
{
   return rot3x3_to_quaternion(V.transpose());
}

template<typename J>
using MatrixXc_J
    = Eigen::Matrix<std::complex<J>, Eigen::Dynamic, Eigen::Dynamic>;
template<typename J>
using VectorXc_J = Eigen::Matrix<std::complex<J>, Eigen::Dynamic, 1>;

template<typename T>
void svd_UDV_T(const MatrixXc_J<T>& M,
               MatrixXc_J<T>& U,
               VectorXc_J<T>& D,
               MatrixXc_J<T>& V)
{
   using MatrixXc = MatrixXc_J<T>;
   // using VectorXc = VectorXc_J<T>;
   Eigen::JacobiSVD<MatrixXc> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U      = svd.matrixU();
   V      = svd.matrixV();
   auto s = svd.singularValues();
   auto n = s.rows();
   D.resize(n, 1);
   for(uint i = 0; i < n; ++i) D(i) = s(i);
}

void svd_UDV(const MatrixXc_J<long double>& M,
             MatrixXc_J<long double>& U,
             VectorXc_J<long double>& D,
             MatrixXc_J<long double>& V)
{
   svd_UDV_T<long double>(M, U, D, V);
}

void svd_UDV(const MatrixXc_J<double>& M,
             MatrixXc_J<double>& U,
             VectorXc_J<double>& D,
             MatrixXc_J<double>& V)
{
   svd_UDV_T<double>(M, U, D, V);
}

void svd_UDV(const MatrixXc_J<float>& M,
             MatrixXc_J<float>& U,
             VectorXc_J<float>& D,
             MatrixXc_J<float>& V)
{
   svd_UDV_T<float>(M, U, D, V);
}

template<typename T>
void svd_UDV_T(const Eigen::Matrix<std::complex<T>, 3, 3>& M,
               Eigen::Matrix<std::complex<T>, 3, 3>& U,
               Eigen::Matrix<std::complex<T>, 3, 1>& D,
               Eigen::Matrix<std::complex<T>, 3, 3>& V)
{
   using MatrixXc = MatrixXc_J<T>;
   // using VectorXc = VectorXc_J<T>;

   Eigen::JacobiSVD<MatrixXc> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U      = svd.matrixU();
   V      = svd.matrixV();
   auto s = svd.singularValues();
   auto n = s.rows();
   D.resize(n, 1);
   for(uint i = 0; i < n; ++i) D(i) = s(i);
}

void svd_UDV(const Eigen::Matrix<std::complex<long double>, 3, 3>& M,
             Eigen::Matrix<std::complex<long double>, 3, 3>& U,
             Eigen::Matrix<std::complex<long double>, 3, 1>& D,
             Eigen::Matrix<std::complex<long double>, 3, 3>& V)
{
   svd_UDV_T(M, U, D, V);
}

void svd_UDV(const Eigen::Matrix<std::complex<double>, 3, 3>& M,
             Eigen::Matrix<std::complex<double>, 3, 3>& U,
             Eigen::Matrix<std::complex<double>, 3, 1>& D,
             Eigen::Matrix<std::complex<double>, 3, 3>& V)
{
   svd_UDV_T(M, U, D, V);
}

void svd_UDV(const Eigen::Matrix<std::complex<float>, 3, 3>& M,
             Eigen::Matrix<std::complex<float>, 3, 3>& U,
             Eigen::Matrix<std::complex<float>, 3, 1>& D,
             Eigen::Matrix<std::complex<float>, 3, 3>& V)
{
   svd_UDV_T(M, U, D, V);
}

template<typename T, typename S> double bdcsvd_thin_TS(const T& M, S& out)
{
   Eigen::BDCSVD<T> svd(M, Eigen::ComputeThinV);
   out = svd.matrixV().col(M.cols() - 1);
   return svd.singularValues()(M.cols() - 1);
}

// double bdcsvd_thin(const MatrixXd& M, VectorXd& out)
// { return bdcsvd_thin_TS(M, out); }
real bdcsvd_thin(const MatrixXr& M, VectorXr& out)
{
   return bdcsvd_thin_TS(M, out);
}

// ------------------------------------------------------------ condition-number

template<typename T> real condition_number_T(const T& M)
{
   Eigen::JacobiSVD<T> svd(M);
   auto vals = svd.singularValues();
   return vals(0) / vals(vals.rows() - 1);
}

real condition_number(const MatrixXr& M) { return condition_number_T(M); }

real condition_number(const Matrix3r& M) { return condition_number_T(M); }

// -------------------------------------------------------- Cross-product Matrix

template<typename T> void to_cross_product_matrix_T(const T& X, Matrix3r& M)
{
   M(0, 0) = M(1, 1) = M(2, 2) = 0.0;
   M(0, 1)                     = -X(2);
   M(1, 0)                     = X(2);
   M(0, 2)                     = X(1);
   M(2, 0)                     = -X(1);
   M(1, 2)                     = -X(0);
   M(2, 1)                     = X(0);
}

void to_cross_product_matrix(const Vector3r& X, Matrix3r& M)
{
   to_cross_product_matrix_T(X, M);
}

void to_cross_product_matrix(const Vector3& X, Matrix3r& M)
{
   to_cross_product_matrix_T(X, M);
}

Matrix3r make_cross_product_matrix(const Vector3& X)
{
   Matrix3r M;
   to_cross_product_matrix_T(X, M);
   return M;
}

Matrix3r make_cross_product_matrix(const Vector3r& X)
{
   Matrix3r M;
   to_cross_product_matrix_T(X, M);
   return M;
}

// --------------------------------------------------- Quadratic with Constraint

// Minimize xGtGx such that Hx = h
//  * G must have more rows than columns.
//  * G.cols() == H.cols()
MatrixXr minimize_with_constraint(const MatrixXr& G, const MatrixXr& H, real h)
{
   const unsigned n_vars      = unsigned(G.cols());
   const unsigned n_equations = unsigned(G.rows());
   MatrixXr sol               = MatrixXr::Zero(n_vars, 1);

   if(G.rows() < G.cols()) {
      LOG_ERR(format("Must have more equations than unknowns!"));
      return sol;
   }

   if(H.rows() != n_vars) {
      LOG_ERR(format("G and H must have the same number of columns!"));
      return sol;
   }

   MatrixXr A = MatrixXr::Zero(n_vars + 1, n_vars + 1);
   MatrixXr b = MatrixXr::Zero(n_vars + 1, 1); // Ax - b = 0

   b(n_vars) = h;
   for(unsigned row = 0; row < n_vars; ++row) {
      A(row, n_vars) = H(row);
      A(n_vars, row) = H(row);
   }
   A.block(0, 0, n_vars, n_vars) = G.transpose() * G;

   if(false) {
      MatrixXr U, V;
      VectorXr D;
      svd_UDV(A, U, D, V);

      cout << str("A\n") << A << endl << endl;
      cout << str("U\n") << U << endl << endl;
      cout << str("D\n") << D << endl << endl;
      cout << str("V\n") << V << endl << endl;
   }

   MatrixXr Ainv       = A.inverse();
   MatrixXr sol_lambda = Ainv * b;
   sol                 = sol_lambda.block(0, 0, n_vars, 1);

   return sol;
}

} // namespace perceive


/**
* Add Guassian Noise or Speckle Noise for 3-chanel-images
*
*/
#include "NoiseFilter.h"
#include <opencv2/opencv.hpp>
#include <iostream>
#include <Eigen/Eigen>

using namespace cv;
using namespace std;

/**
* Add Gaussian Noise for RGB CV_8U or CV_16U 3-channel-images
*
* @param img Input RGB CV_8U or CV_16U 3-channel-images.
* @param mean Mean of Gaussian noise
* @param sigma Standard deviation of Gaussian noise. range from 0 to 1.
* @return noise  Image with guassian noise
*/
cv::Mat NoiseFilter::AddGaussianNoise(cv::Mat img, float mean, float sigma) {
	Mat noise;
	noise = img.clone();
	RNG rng;
	sigma = sigma * 255;

	// generate noise
	rng.fill(noise, RNG::NORMAL, mean, sigma);

	// create a mask to make sure to only affect the area with color and not the empty background.
	cv::Mat grayscaleMat, mask;
	cvtColor(img, grayscaleMat, CV_RGB2GRAY);
    cv::threshold(grayscaleMat, mask, 0.0, 255, CV_THRESH_BINARY); // 0.0 -> black background

	// add the noise
	cv::Mat out;
	add( noise, img, out, mask);

	return out;
}

/**
* Add Speckle Noise for RGB CV_8U or CV_16U 3-channel-images
*
* @param img Input RGB CV_8U or CV_16U 3-channel-images.
* @param dev Standard deviation of speckle noise. range from 0 to 1.
* @return noise  Image with speckle noise
*/
cv::Mat NoiseFilter::AddSpeckleNoiseRGB(cv::Mat img, float dev) {
	Mat res;
	res = img.clone();
	CvMat* pNoise = cvCreateMat(img.rows, img.cols, CV_32F);
	CvRNG rng(6);

	cout << img.type() << endl;

	cvRandArr(&rng, pNoise, CV_RAND_NORMAL, 1, dev);
	for (int i = 0; i < img.rows; i++) {
		for (int j = 0; j < img.cols; j++) {
			float tmp = pNoise->data.fl[img.cols*i + j];
			res.at<Vec3b>(i,j) = img.at<Vec3b>(i,j) * tmp;
		}
	}
	return res;
}


#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <armadillo>
#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <time.h>
using namespace std;

arma::vec init_func(arma::vec Y, double alpha, double t);
double dx1dt(double x2);
double dx2dt(double x1, double p2, double alpha, double t);
double dp1dt(double x1, double p2, double alpha, double t);
double dp2dt(double p1);

arma::vec init_func(arma::vec Function, double alpha, double t) {
    arma::vec ODE(static_cast<arma::uword>(4), arma::fill::zeros);

    ODE(0) = dx1dt(Function(1));
    ODE(1) = dx2dt(Function(0), Function(3), alpha, t);
    ODE(2) = dp1dt(Function(0), Function(3), alpha, t);
    ODE(3) = dp2dt(Function(2));

    return ODE;
}

double dx1dt(double x2) {
    return x2;
}

double dx2dt(double x1, double p2, double alpha, double t) {
    return p2 / 2 - sqrt(2) * x1 * exp(-alpha*t);
}

double dp1dt(double x1, double p2, double alpha, double t) {
    return 2*x1 + sqrt(2) * p2 * exp(-alpha*t);
}

double dp2dt(double p1) {
    return -p1;
}


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2014 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* Convex Non-Negative Matrix Factorization                                 *
***************************************************************************/

#ifndef CNNMF_HPP
#define CNNMF_HPP

#include <Eigen/Dense>
#include <cmath>
#include "NNMF.hpp"

// [1] C. Ding and T. Li and M. Jordan, "Convex and Semi-Nonnegative Matrix
//     Factorizations", IEEE Transactions on Pattern Analysis and Machine
//     Intelligence, Vol. 99(1), 2008.

using namespace Eigen;

class CNNMF
{
public:
	CNNMF(const int maxIterations = 200, const double threshold = 1e-4, const double tolerance = 1e-4, const bool verbose = false);

	template<class Derived>
	void compute(const MatrixBase<Derived> &dataset, const int K);
	template<class Derived>
	void compute(const MatrixBase<Derived> &dataset, const MatrixXd &G0, const MatrixXd &H0);

	MatrixXd G() const { return m_G; }
	MatrixXd H() const { return m_H; }

private:
	void cnnmf_mur(const MatrixXd &dataset);

	MatrixXd m_G;
	MatrixXd m_H;
	int m_components;
	int m_maxIterations;
	double m_threshold;
	double m_tolerance;
	bool m_verbose;
};

/***************************** Implementation *****************************/

CNNMF::CNNMF(const int maxIterations, const double threshold, const double tolerance, const bool verbose)
: m_G(MatrixXd()), m_H(MatrixXd()), m_components(0), m_maxIterations(maxIterations), m_threshold(threshold), m_tolerance(tolerance), m_verbose(verbose)
{
}

template<class Derived>
void CNNMF::compute(const MatrixBase<Derived> &dataset, const int K)
{
	if (K >= dataset.cols() || K >= dataset.rows())
	{
		m_G = MatrixXd::Identity(dataset.cols(), K);
		m_H = MatrixXd::Identity(K, dataset.cols());
		return;
	}

	// Initialize G and H matrices
	NNMF nnmf(m_maxIterations, m_threshold, m_tolerance, true);
	nnmf.compute(dataset, K);

	m_H = nnmf.H();
	m_G = m_H.transpose().fullPivHouseholderQr().solve(MatrixXd::Identity(dataset.cols(), dataset.cols())).transpose().cwiseMax(0);

	// Set components
	m_components = K;

	// Compute Convex Non-Negative Matrix Factorization decomposition
	cnnmf_mur(dataset.cast<double>());
}

template<typename T>
void CNNMF::compute(const MatrixBase<T> &dataset, const MatrixXd &G0, const MatrixXd &H0)
{
	// Initialize W and H matrices
	m_components = G0.cols();
	m_G = G0;
	m_H = H0;

	// Compute Convex Non-Negative Matrix Factorization decomposition
	cnnmf_mur(dataset.cast<double>());
}

// Multiplicative Updated Rules
void CNNMF::cnnmf_mur(const MatrixXd &dataset)
{
	// Set dimension variables
	const double NM = (double) (dataset.rows() * dataset.cols());

	// Set usefull matrices
	const MatrixXd VtV = dataset.transpose() * dataset;
	const MatrixXd Yp = (VtV.cwiseAbs() + VtV) / 2.0;
	const MatrixXd Yn = (VtV.cwiseAbs() - VtV) / 2.0;
	MatrixXd num, den;

	if (m_verbose)
	{
		mexPrintf("Iteration\t\tMax iterations\t\tRMSE\t\t\tRatio\t\tThreshold\n");
		mexPrintf("-----------------------------------------------------------------------------\n");
	}

	// Declare previous iteration matrices and error
	double dnorm0 = 0;

	for (int i = 0; i < m_maxIterations; ++i)
	{
		// Update G
		num = (Yp + (Yn * m_G * m_H)) * m_H.transpose();
		den = (Yn + (Yp * m_G * m_H)) * m_H.transpose();
		m_G = m_G.cwiseProduct(num.cwiseQuotient(den).cwiseSqrt()).cwiseMax(0).eval();

		// Update H
		num = m_G.transpose() * (Yp + (Yn * m_G * m_H));
		den = m_G.transpose() * (Yn + (Yp * m_G * m_H));
		m_H = m_H.cwiseProduct(num.cwiseQuotient(den).cwiseSqrt()).cwiseMax(0).eval();

		// Compute squared error
		const ArrayXXd d = dataset - (dataset * m_G * m_H.transpose());
		const double dnorm = std::sqrt((d * d).sum() / NM);

		if (m_verbose)
		{
			mexPrintf("%d\t\t\t\t%d\t\t\t\t\t%.2e\t\t%.2e\t\t%.2e\n", i+1, m_maxIterations, dnorm, (dnorm0 - dnorm), (m_threshold * std::max(1.0, dnorm0)));
			mexEvalString("drawnow");
		}

		if (i > 0)
		{
			if (dnorm0 - dnorm <= (m_threshold * std::max(1.0, dnorm0)))
				return;
		}

		dnorm0 = dnorm;
	}
}

#endif


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

#include "rj_geometry/transform_matrix.hpp"

using namespace rj_geometry;

TEST(TransformMatrix, Convert) {
    // Test conversion to/from Eigen
    TransformMatrix transform(Point(0, 1), 1.0);
    Eigen::Matrix<double, 3, 3> transform_eigen = transform;
    EXPECT_EQ(transform_eigen * Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 1, 1));
    TransformMatrix transformed_back = transform_eigen;
    EXPECT_EQ(transform * Point(0, 1), transformed_back * Point(0, 1));
    EXPECT_EQ(transform * Point(1, 0), transformed_back * Point(1, 0));
}

TEST(TransformMatrix, Compose) {
    // Test composition: self-compose a 90-degree rotation with an offset from
    // the origin
    TransformMatrix transform1(Point(0, 1), M_PI / 2);
    TransformMatrix transform2(Point(1, 0), M_PI / 2);

    EXPECT_LT(
        ((transform1 * transform2) * Point(1, 0) - transform1 * (transform2 * Point(1, 0))).mag(),
        1e-6)
        << "Composition should be associative!";

    EXPECT_LT(
        ((transform2 * transform1) * Point(1, 0) - transform2 * (transform1 * Point(1, 0))).mag(),
        1e-6)
        << "Composition should be associative!";
}

TEST(TransformMatrix, Reconstruct) {
    Eigen::Matrix<double, 3, 3> transform_eigen;
    transform_eigen << 0, 1, 0, -1, 0, 1, 0, 0, 1;
    TransformMatrix transform = transform_eigen;
    EXPECT_EQ(transform.origin(), Point(0, 1));
    EXPECT_NEAR(transform.rotation(), -M_PI / 2 + 2 * M_PI, 1e-6);
}