/*
@copyright Louis Dionne 2014
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/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>

#include <type_traits>
using namespace boost::hana;

//! [integral_create]
auto one = integral<int, 1>;
auto yes = integral<bool, true>;
//! [integral_create]

//! [integral_api]
using One = decltype(one);

static_assert(std::is_same<One::type, One>{}, "");

static_assert(std::is_same<One::value_type, int>{}, "");

static_assert(One::value == 1               &&
              static_cast<int>(one) == 1    &&
              one() == 1                    &&
              value(one) == 1
, "these are all constant expressions");
//! [integral_api]

namespace anon1 {
//! [integral_shorthands]
auto one = int_<1>;
auto yes = bool_<true>;
//! [integral_shorthands]
}

namespace anon2 {
//! [literals]
using namespace literals; // <-- mandatory to use the _c suffix!

BOOST_HANA_CONSTANT_ASSERT(1234_c == llong<1234>);
BOOST_HANA_CONSTANT_ASSERT(-1234_c == llong<-1234>);
//! [literals]
}

//! [integral_operators]
BOOST_HANA_CONSTANT_ASSERT(int_<1> == integral<int, 1>);
BOOST_HANA_CONSTANT_ASSERT(int_<1> + long_<2> == long_<3>);
BOOST_HANA_CONSTANT_ASSERT(!(bool_<true> && bool_<false>));
//! [integral_operators]

int main() { }


#include "GeometryCalibration.h"

#include <LibRayCastBackproject/VoxelData.h>
#include <LibRayCastBackproject/VolumeRendering.h>

#include <Eigen/Dense>

/// f(x)=1-x^2+x^4 is zero at +/-1, has zero derivative at +/-1 and a maxiumum at f(0)=1; Values outside [-1,1] are clamped to zero.
inline double weighting(double x)
{
	if (x<-1.0||x>1.0) return 0;
	double xx=x*x;
	return 1.0-2*xx+xx*xx;
}

namespace Calibration {
	template <class VectorOfVectors>
	double computeRadiusThreshold(const VectorOfVectors& beads)
	{
		if (beads.empty()) return 0;
		double sum=0;
		for (auto it=beads.begin();it!=beads.end();++it)
			sum+=it->tail(1)[0];
		double mean=sum/(double)beads.size();
		int n_large=0,n_small=0;
		double sum_large=0, sum_small=0;
		for (auto it=beads.begin();it!=beads.end();++it)
			if (it->tail(1)[0]>mean)  { n_large++; sum_large+=it->tail(1)[0]; }
			else                      { n_small++; sum_small+=it->tail(1)[0]; }
		double mean_large=sum_large/(double)n_large;
		double mean_small=sum_small/(double)n_small;
		return 0.5*(mean_large+mean_small);
	}

	double BeadPhantomDetection::computeRadiusThresholdMM(const std::vector<Eigen::Vector4d>& phantom_beads)
	{
		return computeRadiusThreshold(phantom_beads);
	}

	double BeadPhantomDetection::computeRadiusThresholdPx(const std::vector<Eigen::Vector3d>& detected_beads)
	{
		return computeRadiusThreshold(detected_beads);
	}

	void BeadPhantomDetection::gui_declare_section (const GetSetGui::Section& section) {
		GetSet<std::vector<Eigen::Vector4d>>("Location and Radius", section, beads).setDescription("3D Location of beads (X,Y,Z) and their radius (R) in mm. Format: \"X0 Y0 Z0 R0; X1 Y1 Z1 R1; ...\"");
	}

	void BeadPhantomDetection::gui_retreive_section(const GetSetGui::Section& section) {
		beads=GetSet<std::vector<Eigen::Vector4d>>("Location and Radius", section);
	}

	BeadPhantomSimulator::BeadPhantomSimulator() {}

	BeadPhantomSimulator::BeadPhantomSimulator(const BeadPhantomSimulator& other)
	{
		voxelization=other.voxelization;
		drr.voxel_data=0x0;
		drr.raycaster=0x0;
	}

	BeadPhantomSimulator::~BeadPhantomSimulator() {
		if (drr.raycaster ) delete drr.raycaster ;
		if (drr.voxel_data) delete drr.voxel_data;
		drr.raycaster =0x0;
		drr.voxel_data=0x0;
	}

	Geometry::RP3Homography	BeadPhantomSimulator::getModel() const {
		return Geometry::Scale(voxelization.voxel_spacing)*Geometry::Translation(-0.5*voxelization.voxel_number.cast<double>());
	}

	void BeadPhantomSimulator::voxelize(NRRD::Image<float>& phantom, const std::vector<Eigen::Vector4d>& beads, GetSetGui::ProgressInterface &app) const
	{
		// Initilalize 3D voxel data
		const auto& dim = voxelization.voxel_number;
		const auto& spacing = voxelization.voxel_spacing ;
		phantom.set(dim[0], dim[1], dim[2]);
		phantom.spacing(0) = spacing[0];
		phantom.spacing(1) = spacing[1];
		phantom.spacing(2) = spacing[2];
		const auto& model = getModel();
		// Set phantom to zero
		int l = phantom.length();
		#pragma omp parallel for
		for (int i = 0; i<l; i++) phantom[i] = 0;

		// Start progress
		bool cancel_clicked = false;
		app.progressStart(__FUNCTION__, "Computing Voxelized version of the bead phantom...", (int)beads.size(), &cancel_clicked);

		// Then loop over blocks with beads and draw them
		for (int i = 0; i<(int)beads.size(); i++)
		{
			if (cancel_clicked) break;
			app.progressUpdate(i);
			// Get Bead radius and transfrm bead to voxel coordinates.
			auto B(beads[i]);
			double r = B[3];
			double rc = std::ceil(r);
			B[3] = 1;
			// Figure out voxel range affected by bead (assumes model is affine)
			Eigen::Vector3i min = (model.inverse()*(B - Geometry::RP3Point(rc+1, rc+1, rc+1, 0))).head(3).cast<int>();
			Eigen::Vector3i max = (model.inverse()*(B + Geometry::RP3Point(rc+1, rc+1, rc+1, 0))).head(3).cast<int>();
			min = min.cwiseMax(Eigen::Vector3i::Zero());
			// max = max.cwiseMin(dim);
			if (max[0]>=dim[0]) max[0]=dim[0]-1;
			if (max[1]>=dim[1]) max[1]=dim[1]-1;
			if (max[2]>=dim[2]) max[2]=dim[2]-1;

			// And draw bead
			for (int z = min[2]; z<max[2]; z++)
				for (int y = min[1]; y<max[1]; y++)
					for (int x = min[0]; x<max[0]; x++)
					{
						Geometry::RP3Point X = model*Geometry::RP3Point(x+0.5, y+0.5, z+0.5, 1);
						Geometry::dehomogenize(X);
						double d = (X.head(3) - B.head(3)).norm();
						float opacity=0.0f;
						if      (d<r-0.25) opacity=1.0f;
						else if (d<r+0.5) opacity=(float)weighting((r-0.25-d)/0.75);
						phantom[x + y*phantom.size(0) + z*phantom.size(1)*phantom.size(0)]+=opacity;
					}
		}
		app.progressEnd();
		if (cancel_clicked)
			phantom.set(0x0,0);
		if (drr.raycaster) delete drr.raycaster;
		drr.raycaster=0x0;
		if (drr.voxel_data) delete drr.voxel_data;
		drr.voxel_data=0x0;
		if (!!phantom) drr.voxel_data=new VolumeRendering::VoxelData(phantom,false);
	}

	void BeadPhantomSimulator::load_volume(const NRRD::ImageView<float>& phantom)
	{
		if (drr.raycaster) delete drr.raycaster;
		drr.raycaster=0x0;
		if (drr.voxel_data) delete drr.voxel_data;
		drr.voxel_data=0x0;
		if (!!phantom) drr.voxel_data=new VolumeRendering::VoxelData(phantom,false);
	}

	void BeadPhantomSimulator::project(NRRD::ImageView<float>& out, const Geometry::ProjectionMatrix& P) const
	{
		// Out must be allocated
		if (out.dimension()!=2) out.set(0,0x0);
		// Must call voxelize before use.
		if (!drr.voxel_data) {
			out.set(0,0x0);
			return;
		}

		// Set up raycaster
		if (!drr.raycaster)
			drr.raycaster=new VolumeRendering::Raycaster(*drr.voxel_data);
		drr.raycaster->raycastPass<VolumeRendering::DigitallyReconstructedRadiograph>();//.setTransferFunction();
		drr.raycaster->setSamplesPerVoxel(1.5);

		// Allocate output image and raycast
		NRRD::Image<float> tmp(4,out.size(0), out.size(1));
		drr.raycaster->render(tmp,P);

		// Copy temporary image
		int l=out.length();
		for (int i=0;i<l;i++) out[i]=tmp[i*4];
		out.meta_info["Projection Matrix"]=toString(P);
	}

	void BeadPhantomSimulator::gui_declare_section (const GetSetGui::Section& section)
	{
		GetSet<Eigen::Vector3i              >("Voxelization/Voxel Number"  , section, voxelization.voxel_number  ).setDescription("Volume dimension in voxels.");
		GetSet<Eigen::Vector3d              >("Voxelization/Voxel Spacing" , section, voxelization.voxel_spacing ).setDescription("Spacing between voxels in mm.");
		GetSetGui::Section("Voxelization",section).setGrouped();
	}

	void BeadPhantomSimulator::gui_retreive_section(const GetSetGui::Section& section)
	{
		voxelization.voxel_number    =GetSet<Eigen::Vector3i              >("Voxelization/Voxel Number"  ,section);
		voxelization.voxel_spacing   =GetSet<Eigen::Vector3d              >("Voxelization/Voxel Spacing" ,section);
	}

} // namespace Calibration


/**
 * \file boost/numeric/ublasx/operation/pow.hpp
 *
 * \brief Apply the \c std::pow function to a vector or matrix expression.
 *
 * Copyright (c) 2015, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_POW_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_POW_HPP

#include <boost/numeric/ublas/functional.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/inv.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <cmath>

namespace boost { namespace numeric { namespace ublasx {

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

namespace detail {

template <typename MatrixExprT>
struct matrix_pow_traits
{
	typedef typename MatrixExprT::matrix_temporary_type result_type;
};

} // Namespace detail

/**
 * \brief Computes \a me to the power of \a p (me^p).
 *
 * If \a me is a square matrix and \a p is a positive integer, me^p effectively
 * multiplies \a me by itself p-1 times.
 * If \a me is square and nonsingular, me^(-p) effectively multiplies the
 * inverse of \a me by itself p-1 times.
 *
 * \note Fractional exponents are not currently supported.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \param p The exponent.
 * \return The result of \a me to the power of \a p.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT, typename T>
BOOST_UBLAS_INLINE
typename detail::matrix_pow_traits<MatrixExprT>::result_type pow(matrix_expression<MatrixExprT> const& me, T p)
{
	typedef typename detail::matrix_pow_traits<MatrixExprT>::result_type result_type;

	result_type res;

	if (p > 0)
	{
		res = me;

		--p;
		while (p >= 1)
		{
			res = prod(res, me);
			--p;
		}
	}
	else if (p < 0)
	{
		result_type inv_me = inv(me);
		res = inv_me;
		p = -p;

		--p;
		while (p >= 1)
		{
			res = prod(res, inv_me);
			--p;
		}
	}
	else // p == 0
	{
		res = identity_matrix<typename matrix_traits<MatrixExprT>::value_type>(num_rows(me));
	}

	return res;
}

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

#endif // BOOST_NUMERIC_UBLASX_OPERATION_POW_HPP


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

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

#include "miMaS/field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"

namespace math = boost::math::constants;

int main(int,char**)
{
	std::size_t Nx = 125, Nv = 64;
	field<double,1> f(boost::extents[Nv][Nx]);

	f.range.v_min = 0.; f.range.v_max = 2.*math::pi<double>();
	f.step.dv = (f.range.v_max-f.range.v_min)/Nv;
	f.range.x_min = 0.; f.range.x_max = 1.;
	f.step.dx = (f.range.x_max-f.range.x_min)/Nx;
	double dt = 0.5*f.step.dv;

  field<double,1> f_sol = f;

	ublas::vector<double> E (Nx);
  for ( std::size_t i=0 ; i<Nx ; ++i ) { E[i] = 1.; }

#define X(i) (i*f.step.dx+f.range.x_min)
#define V(k) (k*f.step.dv+f.range.v_min)
  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      f[k][i] = cos(V(k));
      f_sol[k][i] = cos(V(k)-dt*20);
    }
  }
#undef X
  f.write("init.dat");

  for ( auto t=0 ; t<20 ; ++t ) {
  	if (t%32==0) { std::cout<<"\r"<<t<<" "<<std::flush ; }
	  field<double,1> Edvf = weno::trp_v(f,E);

    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      for ( auto i=0 ; i<f.size(1) ; ++i ) {
        f[k][i] = f[k][i] - dt*Edvf[k][i];
      }
    }
  }

  f_sol.write("sol.dat");
  f.write("vp.dat");

  return 0;
}


#include <iostream>
#include <blitz/array.h>
#include <blitz/timer.h>

int main() {
    using namespace blitz;

    typedef Array<double,3> Image;
    Image A(512,512,512);
    Timer timer;

    A = 0.0;
    timer.start();
    for (unsigned j=0;j<10;++j) {
        for (Image::iterator i=A.begin(),end=A.end();i!=end;++i) {
            const TinyVector<int,3> pos = i.position();
            *i += pos(0)+pos(1)+pos(2);
        }
    }
    timer.stop();
    double flops = 10.0*512*512*512*2;
    double seconds = timer.elapsedSeconds();

    double timePerOp = seconds / flops;

    cout << "ops = " << flops << endl
         << "seconds = " << seconds << endl;

    double Mflops = flops / seconds / 1.0e+6;
    cout << "Mflops = " << Mflops << endl;

    return 0;
}


#include "polyscope/polyscope.h"

#include <iostream>

#include "geometrycentral/geometry.h"
#include "geometrycentral/halfedge_mesh.h"
#include "geometrycentral/linear_solvers.h"
#include "geometrycentral/polygon_soup_mesh.h"

#include <Eigen/SparseLU>

#include "args/args.hxx"
#include "json/json.hpp"

#define GLM_ENABLE_EXPERIMENTAL
#include "glm/gtx/string_cast.hpp"

#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"

#include <nlopt.hpp>
#include <math.h>

using namespace geometrycentral;
using std::cerr;
using std::cout;
using std::endl;
using std::string;

double objWrapper(unsigned n, const double* x, double* grad, void* f_data);
void angleDefectWrapper(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data);
void validAngleWrapper(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data);
class CatData {
  // Initialized stuff
  Geometry<Euclidean>* geom;
  HalfedgeMesh* mesh;
  std::string niceName;

  // Original mesh information
  VertexData<size_t> vInd;
  size_t nVerts;
  size_t nHalfedges;
  size_t nCorners;
  size_t dim;
  VertexData<double> angleDefects;
  HalfedgeData<size_t> hInd;
  CornerData<size_t> cInd;
  //EdgeData<double> lengths;

public:
  // Derived Information
  HalfedgeData<double> theta;
  HalfedgeData<double> alpha;
  HalfedgeData<double> beta;

  double obj(unsigned n, const double* x, double* grad, void* f_data)
  {
    double accum = 0;
    for (size_t i = 0; i < n; i++)
    {
      accum += pow(x[i],2);
      if (grad)
      {
        grad[i] = 2*x[i];
      }
    }
    return accum;
  }

  void angleDefectCalc(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data)
  {
    // initialize angle defect
    for (size_t i = 0; i < m; i++)
    {
      result[i] = - angleDefects[i];
    }
    if (gradient)
    {
      for (size_t i = 0; i < n*m ; i++)
      {
        gradient[i] = 0.;
      }
    }
    //go through halfedges
    for (EdgePtr e : mesh->edges())
    {
      HalfedgePtr he1 = e.halfedge();
      HalfedgePtr he2 = he1.twin();
      size_t h1 = hInd[he1];
      size_t h2 = hInd[he2];
      size_t v1 = vInd[he1.vertex()];
      size_t v2 = vInd[he2.vertex()];
      result[v1] += x[h1] + x[h2];
      result[v2] += x[h1] + x[h2];

      if (gradient)
      {
        gradient[v1 * n + h1] = 1;
        gradient[v2 * n + h1] = 1;
        gradient[v1 * n + h2] = 1;
        gradient[v2 * n + h2] = 1;
      }
    }
    return;
  }
  void validAngleCalc(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data)
  {
    // first coord specifies angle + alphas - 2pi < 0, second specifies -angle -alphas < 0
    for (CornerPtr c: mesh->corners())
    {
      double curAngle =  geom->angle(c);
      result[2 * cInd[c]] = curAngle - 2 * M_PI;
      result[2 * cInd[c] + 1] = - curAngle;
    }
    for (CornerPtr c: mesh->corners())
    {
      size_t h = hInd[c.halfedge()];
      result[2 * cInd[c]] += x[h];
      result[2 * cInd[c] + 1] -= x[h];
      result[2 * cInd[c.next()]] += x[h];
      result[2 * cInd[c.next()] + 1] -= x[h];
      if (gradient)
      {
        gradient[2 * cInd[c] * n + h] = 1;
        gradient[(2 * cInd[c] + 1) * n + h] = -1;
        gradient[2 * cInd[c.next()] * n + h] = 1;
        gradient[(2 * cInd[c.next()] + 1) * n + h] = -1;
      }
    }
    return;
  }

  void optimize() {
    nlopt::opt opt(nlopt::LN_COBYLA, nHalfedges);
    opt.set_min_objective(&objWrapper, this);
    opt.set_lower_bounds(-2 * M_PI);
    opt.set_upper_bounds(2 * M_PI);

    std::vector<double> tol1(nVerts, 1e-8);
    std::vector<double> tol2(2 * nCorners, 1e-8);

    opt.add_equality_mconstraint(&angleDefectWrapper, this, tol1);
    opt.add_inequality_mconstraint(&validAngleWrapper, this, tol2);

    //opt.add_inequality_constraint(myconstraint, &data[0], 1e-8);
    //opt.add_inequality_constraint(myconstraint, &data[1], 1e-8);
    opt.set_xtol_rel(1e-4);
	std::vector<double> x(nHalfedges, 0);
    double minf;
    try {
      nlopt::result result = opt.optimize(x, minf);
      std::cout << "found minimum" << std::setprecision(10) << minf << std::endl;
    } catch (std::exception& e) {
      std::cout << "nlopt failed: " << e.what() << std::endl;
    }
  }

  CatData(std::string filename) {
    niceName = polyscope::utilities::guessNiceNameFromPath(filename);
    mesh = new HalfedgeMesh(PolygonSoupMesh(filename), geom);
    polyscope::registerSurfaceMesh(niceName, geom);

    vInd = mesh->getVertexIndices();
    nVerts = mesh->nVertices();
    hInd = mesh->getHalfedgeIndices();
    nHalfedges = mesh->nHalfedges();
    cInd = mesh->getCornerIndices();
    nCorners = mesh->nCorners();
    dim = nVerts + nHalfedges;

    theta = HalfedgeData<double>(mesh);
    alpha = HalfedgeData<double>(mesh);
    beta = HalfedgeData<double>(mesh);

    geom->getVertexAngleDefects(angleDefects);
    optimize();
    delete geom;
    delete mesh;
  }
};

double objWrapper(unsigned n, const double* x, double* grad, void* f_data)
{
  return static_cast<CatData*>(f_data)->obj(n, x, grad, NULL);
}

void angleDefectWrapper(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data)
{
  static_cast<CatData*>(func_data)->angleDefectCalc(m, result, n, x, gradient, NULL);
}

void validAngleWrapper(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data)
{
  static_cast<CatData*>(func_data)->validAngleCalc(m, result, n, x, gradient, NULL);
}

int main(int argc, char** argv) {
  // Configure the argument parser
  /*args::ArgumentParser parser("A simple demo of Polyscope.\nBy "
                              "Nick Sharp (nsharp@cs.cmu.edu)",
                              "");
  args::PositionalList<string> files(parser, "files", "One or more files to visualize");
  */
  // Options
  polyscope::options::autocenterStructures = true;
  // Initialize polyscope
  polyscope::init();
  CatData* c = new CatData("C:/spot1.obj");
  delete c;
  // Show the gui
  polyscope::show();

  return 0;
}

/* class CatDataOld {
  // Initialized stuff
  Geometry<Euclidean>* geom;
  HalfedgeMesh* mesh;
  std::string niceName;

  // Original mesh information
  VertexData<size_t> vInd;
  size_t nVerts;
  size_t nHalfedges;
  size_t dim;
  VertexData<double> angleDefects;
  HalfedgeData<size_t> hInd;
  EdgeData<double> lengths;

  public:
    // Derived Information
    HalfedgeData<double> finalCurvature;
    VertexData<double> multiplier;

    HalfedgeData<double> theta;
    HalfedgeData<double> d;
    HalfedgeData<double> alpha;
    HalfedgeData<double> beta;

    EdgeData<char> badEdges;
    EdgeData<double> netEdgeCurvature;
    EdgeData<char> negEdges;

    // Solves the optimization problem
    void solveOptMatrix() {
      Eigen::SparseMatrix<double> d0 = Eigen::SparseMatrix<double>(dim, dim);
      std::vector<Eigen::Triplet<double>> tripletList;
      Vector<double> rhs = Vector<double>(dim);
      // cout << dim << endl;
      for (size_t i = 0; i < nHalfedges; i++) {
        tripletList.emplace_back(i, i, 1.);
        rhs[i] = 0.;
      }

      for (size_t i = nHalfedges; i < dim; i++) {
        rhs[i] = 2 * angleDefects[mesh->vertex(i - nHalfedges)];
      }
      for (EdgePtr e : mesh->edges()) {
        HalfedgePtr h1 = e.halfedge();
        HalfedgePtr h2 = h1.twin();
        size_t v1 = vInd[h1.vertex()];
        size_t v2 = vInd[h2.vertex()];
        tripletList.emplace_back(nHalfedges + v1, hInd[h1], lengths[e]);
        tripletList.emplace_back(nHalfedges + v1, hInd[h2], lengths[e]);
        tripletList.emplace_back(nHalfedges + v2, hInd[h1], lengths[e]);
        tripletList.emplace_back(nHalfedges + v2, hInd[h2], lengths[e]);

        tripletList.emplace_back(hInd[h1], nHalfedges + v1, lengths[e]);
        tripletList.emplace_back(hInd[h2], nHalfedges + v1, lengths[e]);
        tripletList.emplace_back(hInd[h1], nHalfedges + v2, lengths[e]);
        tripletList.emplace_back(hInd[h2], nHalfedges + v2, lengths[e]);
      }
      d0.setFromTriplets(tripletList.begin(), tripletList.end());
      // cout << "Matrix built" << endl;

      Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
      solver.compute(d0);
      if (solver.info() != Eigen::Success) {
        cout << "solving failed" << endl;
      }
      Vector<double> solution = solver.solve(rhs);
      if (solver.info() != Eigen::Success) {
        cout << "solving failed";
      }
      // cout << "Matrix solved";
      for (size_t i = 0; i < nHalfedges; i++) {
        finalCurvature[i] = solution[i];
      }
      for (size_t i = nHalfedges; i < dim; i++) {
        multiplier[i - nHalfedges] = solution[i];
      }
      polyscope::getSurfaceMesh(niceName)->addQuantity("Original Curvature", angleDefects);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Curvature change", finalCurvature);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Lagrange Multiplier", multiplier);
      return;
    }
    // Updates the straight distances between edges, then checks for bad distances
    void updateDistances() {
      size_t bad_halfedges = 0;
      // Initialize all distances first
      for (HalfedgePtr h : mesh->allHalfedges()) {
        theta[h] = lengths[h.edge()] * finalCurvature[h];
        // Basic constraint
        if (theta[h] < 2. * M_PI && theta[h] > -2. * M_PI) {
          d[h] = (theta[h] == 0 ? lengths[h.edge()] : 2 * sin(theta[h] / 2) / finalCurvature[h]);
        } else {
          bad_halfedges++;
          badEdges[h.edge()] = true;
        }
      }
      cout << "Bad angles:" << bad_halfedges << endl;
    }
    // Updates angles based on distances, checks for self intersection
    void updateAngles() {
      size_t bad_halfedges = 0;
      for (HalfedgePtr h : mesh->allHalfedges()) {
        double ij = d[h];
        double jk = d[h.next()];
        double ki = d[h.next().next()];
        double cosAngle = (pow(ij, 2) + pow(ki, 2) - pow(jk, 2)) / (2 * ij * ki);

        if (cosAngle > 1.) {
          alpha[h] = 0;
          bad_halfedges++;
          badEdges[h.edge()] = true;
        } else if (cosAngle < -1.) {
          alpha[h] = M_PI;
          // alpha[h] = 10000;
          bad_halfedges++;
          badEdges[h.edge()] = true;
        } else {
          alpha[h] = acos(cosAngle);
        }
      }
      cout << "Bad distances:" << bad_halfedges << endl;
      for (VertexPtr v : mesh->vertices()) {
        angleDefects[v] = 2 * M_PI;
      }

      bad_halfedges = 0;
      for (HalfedgePtr h : mesh->allHalfedges()) {
        double betaA = alpha[h] + (theta[h] + theta[h.next().next()]) / 2.;
        angleDefects[h.vertex()] -= betaA;
        beta[h] = betaA;
        if (betaA > 2 * M_PI || betaA < 0.) {
          // cout << betaA << endl;
          bad_halfedges++;
          badEdges[h.edge()] = true;
        }
      }
      cout << "Self intersection:" << bad_halfedges << endl;
    }

    double averageAngleDefect() {
      double accum = 0;
      for (size_t i = 0; i < nVerts; i++) {
        accum += abs(angleDefects[i]);
      }
      return accum / nVerts;
    }

    void checkNegedges() {
      size_t neg_edges = 0;
      for (EdgePtr E : mesh->edges()) {
        netEdgeCurvature[E] = finalCurvature[E.halfedge()] + finalCurvature[E.halfedge().twin()];
        if (netEdgeCurvature[E] < 0) {
          negEdges[E] = true;
          neg_edges++;
        }
      }
      cout << "Neg edges:" << neg_edges << endl;
    }
    CatDataOld(std::string filename) {
      niceName = polyscope::utilities::guessNiceNameFromPath(filename);
      mesh = new HalfedgeMesh(PolygonSoupMesh(filename), geom);
      polyscope::registerSurfaceMesh(niceName, geom);

      vInd = mesh->getVertexIndices();
      nVerts = mesh->nVertices();
      hInd = mesh->getHalfedgeIndices();
      nHalfedges = mesh->nHalfedges();
      dim = nVerts + nHalfedges;

      finalCurvature = HalfedgeData<double>(mesh);
      multiplier = VertexData<double>(mesh);
      theta = HalfedgeData<double>(mesh);
      d = HalfedgeData<double>(mesh);
      alpha = HalfedgeData<double>(mesh);
      beta = HalfedgeData<double>(mesh);
      badEdges = EdgeData<char>(mesh, false);
      netEdgeCurvature = EdgeData<double>(mesh);
      negEdges = EdgeData<char>(mesh, false);

      geom->getVertexAngleDefects(angleDefects);
      geom->getEdgeLengths(lengths);
      for (size_t i = 0; i < 1000; i++) {
        cout << "Starting Iteration " << i << endl;
        solveOptMatrix();
        updateDistances();
        updateAngles();
        cout << "Average angle defect: " << averageAngleDefect() << endl;
        cout << "Done" << endl;
      }
      checkNegedges();
      polyscope::getSurfaceMesh(niceName)->addQuantity("Central angles", theta);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Interior angles", alpha);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Straight distances", d);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Exterior angles", beta);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Final Angle Defect", angleDefects);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Net Edge Curvature", netEdgeCurvature);
      polyscope::getSurfaceMesh(niceName)->addSubsetQuantity("Bad edges", badEdges);
      polyscope::getSurfaceMesh(niceName)->addSubsetQuantity("Neg edges", negEdges);
      delete geom;
      delete mesh;
    }
}; */


#include <jaco2_kin_dyn_lib/jaco2_residual_kalman.h>
#include <jaco2_kin_dyn_lib/kdl_conversion.h>
#include <Eigen/Dense>
Jaco2ResidualKalman::Jaco2ResidualKalman():
    n_(0)
{

}

void Jaco2ResidualKalman::setDimensions(std::size_t n)
{
    n_ = n;
    eye_ = Eigen::MatrixXd::Identity(n,n);
}
void Jaco2ResidualKalman::initialize(std::size_t n, const Eigen::VectorXd& state)
{
    state_ = state;
    n_ = n;
    eye_ = Eigen::MatrixXd::Identity(n_,n_);
    cov_ = Eigen::MatrixXd::Identity(n_,n_);
}

void Jaco2ResidualKalman::initialize(const std::vector<double> &state)
{
    Jaco2KinDynLib::convert(state, state_);
    n_ = state.size();
    eye_ = Eigen::MatrixXd::Identity(n_,n_);
    cov_ = Eigen::MatrixXd::Identity(n_,n_);
}

void Jaco2ResidualKalman::setCovarianceProcess(const Eigen::MatrixXd& Q)
{
    cov_state_ = Q;
}

void Jaco2ResidualKalman::setCovarianceMeasurment(const Eigen::MatrixXd &R)
{
    cov_measurment_ = R;
}

Eigen::VectorXd Jaco2ResidualKalman::update(Eigen::VectorXd& measurement)
{

    Eigen::MatrixXd cov_pri = cov_ + cov_measurment_;

    Eigen::MatrixXd K = cov_pri * (cov_pri + cov_state_).inverse();
    state_ += K *(measurement - state_);
    cov_ = (eye_ - K)*cov_pri;

    return state_;

}

void Jaco2ResidualKalman::update(const std::vector<double>& measurement, std::vector<double>& result)
{
    Eigen::VectorXd meas;
    Jaco2KinDynLib::convert(measurement, meas);
    Eigen::VectorXd res = update(meas);
    Jaco2KinDynLib::convert(res, result);
}

Eigen::MatrixXd Jaco2ResidualKalman::getCovariance() const
{
    return cov_;
}


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;
    typedef std::complex<double>  cdouble;

    const unsigned n= 8;
    dense2D<cdouble>              A(n, n);

    A= 3.0;

    dense_vector<cdouble>         v(n), w(n);
    for (unsigned i= 0; i < size(v); i++)
	v[i]= cdouble(i+1, n-i), w[i]= cdouble(i+n);

    rank_one_update(A, v, w);
    std::cout << "A after rank-one update is \n"
	      << with_format(A, 9, 3) << "\n";

    A= 3.0;
    rank_two_update(A, v, w);
    std::cout << "A after rank-two update is \n"
	      << with_format(A, 9, 3) << "\n";

    return 0;
}


/*
 * eig.hpp:
 * Computing all matrix eigenvalues and all eigenvectors A*V=V*D
 *
 * written	Jun. 4, 2015	A. Takayasu
 * modified	by Masahide Kashiwagi
 * modified Oct. 11, 2015 A. Takayasu
 */

#ifndef EIG_HPP
#define EIG_HPP

#include <iostream>
#include <cmath>
#include <limits>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/complex.hpp>
#include <kv/vleq.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

// Eigenvalue computation using QR method for non-symmetric matrix

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T> bool house(const ub::matrix<T>& x, ub::matrix<T>& v, T& beta)
{
	// function [v,beta] = house(x)
	// n = length(x);
	int n = x.size1();
	int i;
	T sigma, mu;
	v.resize(n,1);
	v(0,0) = 1.;
	sigma = 0.;
	for (i=1; i<n; i++){
		sigma += x(i,0)*x(i,0); // sigma = x(2:n)'*x(2:n);
		v(i,0) = x(i,0); // v = [1;x(2:n)];
	}
	//
	// if sigma == 0
	//   beta = 0;
	// else
	//   mu = sqrt(x(1)^2+sigma);
	//   if x(1)<=0
	//     v(1) = x(1)-mu;
	//   else
	//     v(1) = -sigma/(x(1)+mu);
	//   end
	//   beta = 2*v(1)^2/(sigma+v(1)^2);
	//   v = v/v(1);
	// end
	//
	if (sigma == 0.){
		beta = 0.;
	} else {
		using std::sqrt;
		mu = sqrt(x(0,0) * x(0,0) + sigma);
		if (x(0,0) <= 0.){
			v(0,0) = x(0,0) - mu;
		} else {
			v(0,0) = -sigma / (x(0,0) + mu);
		}
		beta = 2 * v(0,0)*v(0,0) / (sigma + v(0,0)*v(0,0));
		v = v / v(0,0);
	}
	return true;
}

template <class T> bool hess(const ub::matrix<T>& A, ub::matrix<T>& Q, ub::matrix<T>& H)
{
	int i,k=0;
	T beta;
	ub::vector<T> vec_tmp;

	// n = size(A,2);
	int n = A.size1();
	if (n != A.size2()) return false;// Square matrix only

	// Q = eye(n);
	ub::matrix<T> x, v, mat_H;
	Q = ub::identity_matrix<T>(n);
	H = A;

	for (k = 0; k < n-2; k++) {
		// [v,beta] = house(H(k+1:n,k));
		x=ub::project(H,ub::range(k+1,n),ub::range(k,k+1));
		house(x,v,beta);
		//   mat_H = eye(n-k) - beta*v*(v');
		mat_H = ub::identity_matrix<T>(n-k-1) - beta*prod(v,trans(v));
		//   H(k+1:n,k) = [mat_H(1,:)*A(k+1:n,k);zeros(n-k-1,1)];
		vec_tmp  = prod(row(mat_H,0),ub::project(H,ub::range(k+1,n),ub::range(k,k+1)));
		H(k+1,k) = vec_tmp(0);
		for (i=k+2;i<n;i++){
			H(i,k)= (T) 0;
		}
		//   H(k+1:n,k+1:n) = mat_H*H(k+1:n,k+1:n);
		ub::project(H,ub::range(k+1,n),ub::range(k+1,n)) = prod(mat_H,ub::project(H,ub::range(k+1,n),ub::range(k+1,n)));
		//   H(1:n,k+1:n) = H(1:n,k+1:n)*mat_H;
		ub::project(H,ub::range(0,n),ub::range(k+1,n)) = prod(ub::project(H,ub::range(0,n),ub::range(k+1,n)),mat_H);
		//   Q(:,k+1:n)=Q(:,k+1:n)*mat_H;
		ub::project(Q,ub::range(0,n),ub::range(k+1,n)) = prod(ub::project(Q,ub::range(0,n),ub::range(k+1,n)),mat_H);
	}
	return true;
}

template <class T> bool francisQR(ub::matrix<T>& Q, ub::matrix<T>& H)
{
	// double tol=1e-15; // Little bit strong!!!
	T tol = std::numeric_limits<T>::epsilon();
	int n = H.size1();
	// % p indicates the 'active' matrix size
	int p = n, q, r;
	T s,t,x,y,z,beta;
	ub::matrix<T> v, mat_tmp, mat_tmp2, mat_T;
	mat_tmp.resize(3,1);
	mat_tmp2.resize(2,1);

	while (p > 2) {
		q = p-1;
		s = H(q-1,q-1) + H(p-1,p-1);
		t = H(q-1,q-1)*H(p-1,p-1) - H(q-1,p-1)*H(p-1,q-1);
		// % compute first 3 elements of first column of M
		x = H(0,0)*H(0,0)+H(0,1)*H(1,0)-s*H(0,0)+t;
		y = H(1,0)*(H(0,0)+H(1,1)-s);
		z = H(1,0)*H(2,1);
		for (int k = 0; k < p-2; k++) {
			mat_tmp(0,0) = x;
			mat_tmp(1,0) = y;
			mat_tmp(2,0) = z;
			// [v,beta] = house([x,y,z].');
			house(mat_tmp,v,beta);
			r = fmax(1,k); // r = max(1,k); Need math.h???
			// T = eye(3) - beta*v*(v');
			mat_T = ub::identity_matrix<T>(3) - beta*prod(v,trans(v));
			// H(k+1:k+3,r:n) = T*H(k+1:k+3,r:n);
			ub::project(H,ub::range(k,k+3),ub::range(r-1,n)) = prod(mat_T,ub::project(H,ub::range(k,k+3),ub::range(r-1,n)));
			r = fmin(k+4,p);
			// H(1:r,k+1:k+3) = H(1:r,k+1:k+3)*T;
			ub::project(H,ub::range(0,r),ub::range(k,k+3)) = prod(ub::project(H,ub::range(0,r),ub::range(k,k+3)),mat_T);
			// Q(:,k+1:k+3) = Q(:,k+1:k+3)*T;
			ub::project(Q,ub::range(0,n),ub::range(k,k+3)) = prod(ub::project(Q,ub::range(0,n),ub::range(k,k+3)),mat_T);
			// x = H(k+2,k+1);
			x = H(k+1,k);
			// y = H(k+3,k+1);
			y = H(k+2,k);
			// if k<p-3, z=H(k+4,k+1);
			if (k<p-3) {
				z = H(k+3,k);
			}
		}
		mat_tmp2(0,0) = x;
		mat_tmp2(1,0) = y;
		// [v,beta] = house([x,y]');
		house(mat_tmp2,v,beta);
		// T = eye(2) - beta*v*(v');
		mat_T = ub::identity_matrix<T>(2) - beta*prod(v,trans(v));
		// H(q:p,p-2:n) = T'*H(q:p,p-2:n);
		ub::project(H,ub::range(q-1,p),ub::range(p-3,n)) = prod(mat_T,ub::project(H,ub::range(q-1,p),ub::range(p-3,n)));
		// H(1:p,p-1:p) = H(1:p,p-1:p)*T;
		ub::project(H,ub::range(0,p),ub::range(p-2,p)) = prod(ub::project(H,ub::range(0,p),ub::range(p-2,p)),mat_T);
		// Q(:,q:p) = Q(:,q:p)*T;
		ub::project(Q,ub::range(0,n),ub::range(q-1,p)) = prod(ub::project(Q,ub::range(0,n),ub::range(q-1,p)),mat_T);
		// check for convergence
		// if abs(H(p,q)) < tol*(abs(H(q,q))+abs(H(p,p)))
		// if (fabs(H(p-1,q-1)) < tol*(fabs(H(q-1,q-1) + fabs(H(p-1,p-1)))))
		using std::abs;
		if (abs(H(p-1,q-1)) < tol*(abs(H(q-1,q-1) + abs(H(p-1,p-1))))) {
			H(p-1,q-1) = (T) 0;
			p--;
		} else if (abs(H(p-2,q-2)) < tol*(abs(H(q-2,q-2)) + fabs(H(q-1,q-1)))){
			H(p-2,q-2) = (T) 0;
			p-=2;
		}
	}
	return true;
}

template <class T> bool lu_factorize_comp(ub::matrix<kv::complex<T> >& A, ub::vector<int>& p)// Numerical recipe in C (ludcmp)
{
  int i,j,k,imax;
  int n = A.size1();

  ub::vector<T> vv;
  vv.resize(n);

  T big, temp;
  kv::complex<T> sum, dum;

  using std::abs;

  for (i = 0; i < n; i++) {
    big = 0.0;
    for (j = 0; j < n; j++) {
      if ((temp = abs(A(i,j))) > big) {
        big = temp;
      }
    }
    if (big == 0.0) {
      std::cout << "Singular matrix in lu_factorize_comp" << std::endl;
      return false;
    }
    vv(i) = 1.0/big;
  }

  for (j = 0; j < n; j++) {
    for (i = 0; i < j; i++) {
      sum = A(i,j);
      for (k = 0; k < i; k++) {
        sum -= A(i,k)*A(k,j);
      }
      A(i,j) = sum;
    }
    big = 0.0;
    for (i = j; i < n; i++) {
      sum = A(i,j);
      for (k = 0; k < j; k++) {
        sum -= A(i,k)*A(k,j);
      }
      A(i,j) = sum;
      if ((temp=vv(i)*abs(sum)) >= big) {
        big = temp;
        imax = i;
      }
    }
    if (j != imax) {
      for (k = 0; k < n; k++) {
        dum = A(imax,k);
        A(imax,k) = A(j,k);
        A(j,k) = dum;
      }
      vv(imax) = vv(j);
    }
    p(j) = imax;
    if (abs(A(j,j)) == 0) {
      std::cout << "Singular matrix in lu_factorize_comp" << std::endl;
      return false;
    }
    if (j != n) {
      dum = 1./(A(j,j));
      for (i = j+1; i < n; i++) {
        A(i,j) *= dum;
      }
    }
  }
  return true;
}

template <class T> bool lu_substitute_comp(const ub::matrix<kv::complex<T> >& A, const ub::vector<int>& p, ub::vector<kv::complex<T> >& b)// Numerical recipe in C (lubksb)
{
  int i, ii=0, ip, j;
  int n = A.size1();
  kv::complex<T> sum;

  using std::abs;

  for (i = 0; i < n; i++) {
    ip = p(i);
    sum = b(ip);
    b(ip) = b(i);
    if (ii==0) {
      for (j = ii; j <= i-1; j++) {
        sum -= A(i,j)*b(j);
      }
    } else if (abs(sum) != 0) {
      ii = i;
    }
    b(i) = sum;
  }
  for (i = n-1; i >= 0; i--) {
      sum = b(i);
      for (j = i+1; j < n; j++) {
        sum -= A(i,j)*b(j);
      }
      b(i) = sum/A(i,i);
  }
  return true;
}

template <class T> bool eig2by2(const ub::matrix<T>& P, const ub::matrix<T>& A, ub::matrix< kv::complex<T> >& V, ub::matrix< kv::complex<T> >& D)
{
	int i, j, k, l;
	T tra, det, x_norm;
	kv::complex<T> tmp, tmp1, am, ap;
	kv::complex<T> b1, b2;
	kv::complex<T> a11, a12, a21, a22, ck;
	int n = A.size1();// n = size(A,2);
	// X = eye(n,n); Orthogonal matrix
	ub::matrix< kv::complex<T> > X;
	ub::vector< kv::complex<T> > x;
	X.resize(n,n);
	for (i = 0; i < n; i++) {
		X(i,i) = 1.;
	}
	D.resize(n,n);// Eigen value matrix (diagonal)

	i=1;
	// Compute eigenvalue
	while (i<=n) {
		if (i!=n) {
			j = i+1;
		}
		if (A(j-1,i-1) == 0 || i==n) {
			// Eigen value is diagonal element
			D(i-1,i-1) = A(i-1,i-1);
			i++;
		} else {
			// If A contains 2 by 2 block on the diagonal,
			// compute a real pair or a complex conjugate pair.
			tra = A(i-1,i-1) + A(j-1,j-1);
			det = A(i-1,i-1)*A(j-1,j-1)-A(i-1,j-1)*A(j-1,i-1);
			tmp = tra*tra-4*det;// Complex!
			using std::sqrt;
			tmp1 = sqrt(tmp);
			am = 0.5*(tra + tmp1);
			ap = 0.5*(tra - tmp1);
			using std::abs;
			if (abs(A(i-1,i-1)-am)/abs(am) < 1) {
				D(i-1,i-1) = am;
				D(j-1,j-1) = ap;
			} else {
				D(i-1,i-1) = ap;
				D(j-1,j-1) = am;
			}
			i += 2;
		}
	}
	// std::cout << "D" << D << std::endl;
	// D(0,0).imag() = -D(0,0).imag();
	// std::cout << "conj(D)" << D << std::endl;

	// Compute eigenvector by backward substitution
	bool flag = true;
	for (i = 1; i <= n; i++) {
		j = i;
		if (flag) {
			// Compute jth element of ith eigenvector X(j,i)
			while (j>0) {
				k = fmin(n,j+1);
				l = fmax(1,j-1);
				if (i==j) {
					if (A(k-1,j-1) != 0 && k!=j) {
						// Block diagonal [A(j,j), A(j,j+1); A(j+1,j), A(j+1,j+1)] appears
						using std::abs;
						if (abs(A(j,j)-D(i-1,i-1)) > abs(A(j-1,j))) {
							X(j,i-1) = -A(j,j-1) / (A(j,j)-D(i-1,i-1));
						} else {
							X(j,i-1) = -(A(j-1,j-1)-D(i-1,i-1))/A(j-1,j);
						}
						if (D(j-1,j-1).imag() != -D(j-1,j-1).imag()) {
							// Conjugate pair X(j,i) = 1+0*1i;
							flag = false; //Conjugate pair flag (flag=false means the next eigenvector is conjugate of ith vector)
						}
						j--;
					} else if (A(j-1,l-1) != 0 && l!=j) {
						// Block diagonal [A(j-1,j-1), A(j-1,j); A(j,j-1), A(j,j)] appears X(j,i)=1
						using std::abs;
						if (abs(A(j-2,j-2)-D(i-1,i-1)) > abs(A(j-1,j-2))) {
							X(j-2,i-1) = -A(j-2,j-1)/(A(j-2,j-2)-D(i-1,i-1));
						} else {
							X(j-2,i-1) = -(A(j-1,j-1)-D(i-1,i-1))/A(j-1,j-2);
						}
						if (D(j-1,j-1).imag() != -D(j-1,j-1).imag()) {
							// Conjugate pair X(j,j) = 1+0*1i;
							flag = false;// Conjugate pair flag (flag=false means the next eigenvector is conjugate of ith vector)
						}
						j -= 2;
					} else {
						// A(i,i) is eigen value
						j--;
					}
				} else {
					if (A(j-1,l-1)!=0 && l!=j) {
						// Block diagonal [A(j-1,j-1), A(j-1,j); A(j,j-1), A(j,j)] appears
						// Real pair
						b1=0; b2=0;
						for (k = j+1; k <= fmin(i+1,n); k++) {
							b1 += A(j-2,k-1)*X(k-1,i-1);
							b2 += A(j-1,k-1)*X(k-1,i-1);
						}
						a11 = A(j-2,j-2)-D(i-1,i-1);
						a12 = A(j-2,j-1);
						a21 = A(j-1,j-2);
						a22 = A(j-1,j-1)-D(i-1,i-1);
						ub::matrix<kv::complex<T> > LU(2,2);
						LU(0,0) = a11; LU(0,1) = a12; LU(1,0) = a21; LU(1,1) = a22;
						ub::vector<kv::complex<T> > b(2);
						b(0) = b1; b(1) = b2;
						ub::vector<int> pm(n);
						lu_factorize_comp(LU,pm);
						lu_substitute_comp(LU,pm,b);
						// ck  = -1/(a11*a22-a12*a21);
	          // X(j-2,i-1) = ck*(a22*b1-a12*b2);
	          // X(j-1,i-1) = ck*(-a21*b1+a11*b2);
						X(j-2,i-1) = b(0);
						X(j-1,i-1) = b(1);
						j -= 2;
					} else {
						// A(i,i) is eigen value
						for (k = j+1; k <= fmin(i+1,n); k++) {
							X(j-1,i-1) = X(j-1,i-1) + A(j-1,k-1)*X(k-1,i-1);
						}
						// X(j-1,i-1) = -X(j-1,i-1)/(D(j-1,j-1)-D(i-1,i-1));
						if ((D(j-1,j-1)-D(i-1,i-1)).real()==0 && (D(j-1,j-1)-D(i-1,i-1)).imag()==0 && X(j-1,i-1).real()==0 && X(j-1,i-1).imag()==0) {
							X(j-1,i-1) = 0; // d(j) = d(i)
						} else {
							X(j-1,i-1) = -X(j-1,i-1)/(D(j-1,j-1)-D(i-1,i-1));
						}
						j--;
					}
				}
			}
			x = column(X,i-1);
			x_norm = 0;
			for (k = 0; k < n; k++) {
				using std::abs;
				x_norm += abs(x(k))*abs(x(k));
			}
			using std::sqrt;
			x_norm = sqrt(x_norm);
			for (k = 0; k < n; k++) {
				X(k,i-1) /= x_norm;
			}
		} else {
			for (k = 0; k < n; k++) {
				X(k,i-1).real() =  X(k,i-2).real();
				X(k,i-1).imag() =  -X(k,i-2).imag();
			}
			flag = true;
		}
	}
	// V = P*X;
	V = prod(P,X);

	return true;
}

template <class T> bool eig(const ub::matrix<T>& A, ub::matrix< kv::complex<T> >& V, ub::matrix< kv::complex<T> >& D)
{
	int n = A.size1();
	if (n != A.size2()) return false;// Square matrix only

	ub::matrix<T> Q, H;
	Q.resize(n,n);
	H.resize(n,n);

	// std::cout << Q << "\n";
	hess(A,Q,H);
	francisQR(Q,H);
	eig2by2(Q,H,V,D);
	// std::cout << "Residual " << prod(A,Q)-prod(Q,H) << "\n";
	// std::cout << "Residual " << prod(A,V)-prod(V,D) << "\n";
	return true;
}

template <class T> bool veig(const ub::matrix<T>& A, ub::vector< kv::complex< kv::interval<T> > >& v)
{
	int i, j, n=A.size1();
	ub::matrix< kv::complex<T> > V, D;
	ub::matrix< kv::complex< kv::interval<T> > > X, C;
	ub::matrix< kv::interval<T> > BA, BC, G;
	// ub::vector< kv::complex< kv::interval<T> > > b, x;
	ub::vector< kv::interval<T> > d, err;

	eig(A,V,D);
	// std::cout << "Residual " << prod(a,V)-prod(V,D) << "\n";
	// C = A*intval(X);
	X.resize(n,n);
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			X(i,j) = V(i,j);
		}
	}
	C = prod(A,X);

	// G = verifylss(X,C);
	G.resize(2*n,n);
	BA.resize(2*n,2*n);
	BC.resize(2*n,n);

	// X  = A + Bi;
	// BA = [A, -B; B, A]
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			BA(i,j) = X(i,j).real();
			BA(i+n,j) = X(i,j).imag();
			BA(i,j+n) = -X(i,j).imag();
			BA(i+n,j+n) = X(i,j).real();
		}
	}

	// C = P + Qi
	// BC = [P;Q]
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			BC(i,j) = C(i,j).real();
			BC(i+n,j) = C(i,j).imag();
		}
	}
	kv::vleq(BA,BC,G);

	// mid_G = mid(G);

	d.resize(2*n);
	err.resize(2*n);

	for (i = 0; i < n; i++) {
		d(i) = mid(G(i,i));
		d(i+n) = mid(G(i+n,i));
		G(i,i) -= d(i);
		G(i+n,i) -= d(i+n);
	}

	// G = G-mid_G;

	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			err(i) += G(i,j);
			err(i+n) += G(i+n,j);
		}
	}

	v.resize(n);
	d += err;
	for (i = 0; i < n; i++) {
		v(i) = kv::complex< kv::interval<T> >(d(i),d(i+n));
	}
	return true;
}

template <class T> bool veig(const ub::matrix< kv::interval<T> >& A, ub::vector< kv::complex< kv::interval<T> > >& v)
{
	int i, j, n=A.size1();
	ub::matrix< kv::complex<T> > V, D;
	ub::matrix< kv::complex< kv::interval<T> > > X, C;
	ub::matrix< kv::interval<T> > BA, BC, G;
	// ub::vector< kv::complex< kv::interval<T> > > b, x;
	ub::vector< kv::interval<T> > d, err;

	eig(mid(A),V,D);
	// std::cout << "Residual " << prod(a,V)-prod(V,D) << "\n";
	// C = A*intval(X);
	X.resize(n,n);
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			X(i,j) = V(i,j);
		}
	}
	C = prod(A,X);

	// G = verifylss(X,C);
	G.resize(2*n,n);
	BA.resize(2*n,2*n);
	BC.resize(2*n,n);

	// X  = A + Bi;
	// BA = [A, -B; B, A]
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			BA(i,j) = X(i,j).real();
			BA(i+n,j) = X(i,j).imag();
			BA(i,j+n) = -X(i,j).imag();
			BA(i+n,j+n) = X(i,j).real();
		}
	}

	// C = P + Qi
	// BC = [P;Q]
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			BC(i,j) = C(i,j).real();
			BC(i+n,j) = C(i,j).imag();
		}
	}
	kv::vleq(BA,BC,G);

	// mid_G = mid(G);

	d.resize(2*n);
	err.resize(2*n);

	for (i = 0; i < n; i++) {
		d(i) = mid(G(i,i));
		d(i+n) = mid(G(i+n,i));
		G(i,i) -= d(i);
		G(i+n,i) -= d(i+n);
	}

	// G = G-mid_G;

	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			err(i) += G(i,j);
			err(i+n) += G(i+n,j);
		}
	}

	v.resize(n);
	d += err;
	for (i = 0; i < n; i++) {
		v(i) = kv::complex< kv::interval<T> >(d(i),d(i+n));
	}
	return true;
}

template <class T> bool invert_comp(const ub::matrix<kv::complex<T> >& A, ub::matrix<kv::complex<T> >& R)
{
  int n = A.size1();
  ub::matrix< kv::complex<T> > LU=A;

  ub::vector< kv::complex<T> > b(n);
  ub::vector<int> pm(n);

  R.resize(n,n);

  lu_factorize_comp(LU,pm);
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      if (j==i) b(j) = 1.0;
      else b(j) = 0.0;
    }
    // std::cout << "b" << b << std::endl;
    lu_substitute_comp(LU,pm,b);
    ub::column(R,i) = b;
  }
  return true;
}

} // namespace kv

#endif // EIG_HPP


/*=================================================================================
 *	                    Copyleft! 2018 William Yu
 *          Some rights reserved：CC(creativecommons.org)BY-NC-SA
 *                      Copyleft! 2018 William Yu
 *      版权部分所有，遵循CC(creativecommons.org)BY-NC-SA协议授权方式使用
 *
 * Filename                :
 * Description             : 视觉SLAM十四讲/ch6/g2o 学习记录
 * Reference               :
 * Programmer(s)           : William Yu, windmillyucong@163.com
 * Company                 : HUST, DMET国家重点实验室FOCUS团队
 * Modification History	   : ver1.0, 2018.04.05, William Yu

=================================================================================*/

/// Include Files
#include <iostream>
#include <g2o/core/base_vertex.h> //定点类型
#include <g2o/core/base_unary_edge.h> //一元边类型
#include <g2o/core/block_solver.h> //求解器
#include <g2o/core/optimization_algorithm_levenberg.h> //莱文贝格-马夸特方法 Levenberg-Marquardt算法
#include <g2o/core/optimization_algorithm_gauss_newton.h> //高斯牛顿法
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono> //计时库
using namespace std;

/*-----------------------------[Note]---------------------------
# G2O图优化
深入理解图优化与g2o：图优化篇 http://www.cnblogs.com/gaoxiang12/p/5244828.html
深入理解图优化与g2o：g2o篇 https://www.cnblogs.com/gaoxiang12/p/5304272.html
--------------------------------------------------------------*/

/// Global Variables

/**
 * @class
 * @brief 待优化变量
 */
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;
    }

    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

/**
 * @class
 * @brief 误差模型
 */
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

/// Function Definitions

/**
 * @function main
 * @author William Yu
 * @brief
 * @param  None
 * @retval None
 */
int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma ) //人为叠加高斯噪声
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

    //-- 图优化过程
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;   //误差项优化变量维度为3，误差值维度为1
    //线性求解器
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    //稀疏矩阵求解
    Block* solver_ptr = new Block( linearSolver );
    //迭代算法 从下面这三行 梯度下降方法，从高斯牛顿GN,  莱文贝格－马夸特方法LM, DogLeg中选择一个
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    //优化模型
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );
    optimizer.setVerbose( true );

    // //--[ERROR]see: https://www.cnblogs.com/xueyuanaichiyu/p/7921382.html
    // // 构建图优化，先设定g2o
    // typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    // Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    // Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) );      // 矩阵块求解器
    // // 梯度下降方法，从GN, LM, DogLeg 中选
    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::unique_ptr<Block>(solver_ptr) );
    // // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::unique_ptr<Block>(solver_ptr) );
    // // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( std::unique_ptr<Block>(solver_ptr) );
    // g2o::SparseOptimizer optimizer;     // 图模型
    // optimizer.setAlgorithm( solver );   // 设置求解器
    // optimizer.setVerbose( true );       // 打开调试输出

    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );

    // 往图中增加边
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // 设置连接的顶点
        edge->setMeasurement( y_data[i] );      // 观测数值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
        optimizer.addEdge( edge );
    }

    // 执行优化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;

    return 0;
}


#include <tiny.h>
#include <convex.h>

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

BOOST_AUTO_TEST_SUITE(convex_simplex);

BOOST_AUTO_TEST_CASE(simplex_testing)
{
    typedef tiny::MathTypes<double>      math_types;
    typedef math_types::vector3_type     V;

    typedef convex::Simplex<V>           S;

    V const not_in_simplex = V::make(1.5, 5.0, 1.0);

    S simplex;

    // Vertify that simplex have been initialized correctly as being empty and ``zeroed''
    BOOST_CHECK( simplex.m_bitmask == 0u );
    for(size_t i=0u; i<4; ++i)
    {
        BOOST_CHECK( simplex.m_w[0] == 0.0 );
        for(size_t j=0u; j<3; ++j)
        {
            BOOST_CHECK( simplex.m_v[i](j) == 0.0 );
            BOOST_CHECK( simplex.m_a[i](j) == 0.0 );
            BOOST_CHECK( simplex.m_b[i](j) == 0.0 );
        }
    }

    // Vertify how different query methods behave on an empty Simplex

    BOOST_CHECK( !convex::is_point_in_simplex( not_in_simplex, simplex ) );

    BOOST_CHECK( !convex::is_full_simplex(simplex) );

    BOOST_CHECK( convex::dimension(simplex) == 0u );

    int bit_A    = 0;
    size_t idx_A = 0;
    BOOST_CHECK_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A ), std::invalid_argument );

    int bit_B    = 0;
    size_t idx_B = 0;
    BOOST_CHECK_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B ), std::invalid_argument );

    int bit_C    = 0;
    size_t idx_C = 0;
    BOOST_CHECK_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ), std::invalid_argument );

    // Next try to insert one simplex vertex into the simplex

    V const p1 = V::make(1.0, 0.0, 0.0);
    V const a1 = V::make(1.0, 1.0, 0.0);
    V const b1 = V::make(1.0, 0.0, 1.0);

    BOOST_CHECK_NO_THROW( convex::add_point_to_simplex( p1, a1, b1, simplex ) );

    // Verify how differnt query method works on a 1-simplex
    BOOST_CHECK( !convex::is_full_simplex(simplex) );
    BOOST_CHECK( convex::dimension(simplex) == 1u );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A ) );
    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    BOOST_CHECK_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B ), std::logic_error );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    bit_C  = 0xFFFF;
    idx_C  = 0xFFFF;
    BOOST_CHECK_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ), std::logic_error );

    BOOST_CHECK( !convex::is_point_in_simplex( not_in_simplex, simplex ) );
    BOOST_CHECK(  convex::is_point_in_simplex( p1, simplex )             );

    // Next try to insert one more simplex vertex into the simplex

    V const p2 = V::make(2.0, 0.5, 1.0);
    V const a2 = V::make(2.0, 1.0, 7.0);
    V const b2 = V::make(2.0, 0.5, 1.0);

    BOOST_CHECK_NO_THROW( convex::add_point_to_simplex( p2, a2, b2, simplex ) );

    BOOST_CHECK( !convex::is_full_simplex(simplex) );
    BOOST_CHECK( convex::dimension(simplex) == 2u );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A ) );
    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    BOOST_CHECK( bit_B == 2 );
    BOOST_CHECK( idx_B == 1 );
    BOOST_CHECK( simplex.m_v[idx_B] == p2 );
    BOOST_CHECK( simplex.m_a[idx_B] == a2 );
    BOOST_CHECK( simplex.m_b[idx_B] == b2 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    bit_C  = 0xFFFF;
    idx_C  = 0xFFFF;
    BOOST_CHECK_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ), std::logic_error );

    BOOST_CHECK( !convex::is_point_in_simplex( not_in_simplex, simplex ) );
    BOOST_CHECK(  convex::is_point_in_simplex( p1, simplex )             );
    BOOST_CHECK(  convex::is_point_in_simplex( p2, simplex )             );

    // Insert one more simplex vertex

    V const p3 = V::make(2.3, 7.5, 1.2);
    V const a3 = V::make(2.1, 1.1, 2.3);
    V const b3 = V::make(2.2, 2.5, 0.1);

    BOOST_CHECK_NO_THROW( convex::add_point_to_simplex( p3, a3, b3, simplex ) );

    BOOST_CHECK( !convex::is_full_simplex(simplex) );
    BOOST_CHECK( convex::dimension(simplex) == 3u );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A ) );
    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    BOOST_CHECK( bit_B == 2 );
    BOOST_CHECK( idx_B == 1 );
    BOOST_CHECK( simplex.m_v[idx_B] == p2 );
    BOOST_CHECK( simplex.m_a[idx_B] == a2 );
    BOOST_CHECK( simplex.m_b[idx_B] == b2 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    bit_C  = 0xFFFF;
    idx_C  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ) );

    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    BOOST_CHECK( bit_B == 2 );
    BOOST_CHECK( idx_B == 1 );
    BOOST_CHECK( simplex.m_v[idx_B] == p2 );
    BOOST_CHECK( simplex.m_a[idx_B] == a2 );
    BOOST_CHECK( simplex.m_b[idx_B] == b2 );

    BOOST_CHECK( bit_C == 4 );
    BOOST_CHECK( idx_C == 2 );
    BOOST_CHECK( simplex.m_v[idx_C] == p3 );
    BOOST_CHECK( simplex.m_a[idx_C] == a3 );
    BOOST_CHECK( simplex.m_b[idx_C] == b3 );

    BOOST_CHECK( !convex::is_point_in_simplex( not_in_simplex, simplex ) );
    BOOST_CHECK(  convex::is_point_in_simplex( p1, simplex )             );
    BOOST_CHECK(  convex::is_point_in_simplex( p2, simplex )             );
    BOOST_CHECK(  convex::is_point_in_simplex( p3, simplex )             );

    // Insert one more vertex then we have a full simplex

    V const p4 = V::make(1.3, 1.5, 1.2);
    V const a4 = V::make(1.1, 1.1, 1.3);
    V const b4 = V::make(1.2, 1.5, 1.1);

    BOOST_CHECK_NO_THROW( convex::add_point_to_simplex( p4, a4, b4, simplex ) );

    BOOST_CHECK( convex::is_full_simplex(simplex) );
    BOOST_CHECK( convex::dimension(simplex) == 4u );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A ) );
    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    BOOST_CHECK( bit_B == 2 );
    BOOST_CHECK( idx_B == 1 );
    BOOST_CHECK( simplex.m_v[idx_B] == p2 );
    BOOST_CHECK( simplex.m_a[idx_B] == a2 );
    BOOST_CHECK( simplex.m_b[idx_B] == b2 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    bit_C  = 0xFFFF;
    idx_C  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ) );

    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    BOOST_CHECK( bit_B == 2 );
    BOOST_CHECK( idx_B == 1 );
    BOOST_CHECK( simplex.m_v[idx_B] == p2 );
    BOOST_CHECK( simplex.m_a[idx_B] == a2 );
    BOOST_CHECK( simplex.m_b[idx_B] == b2 );

    BOOST_CHECK( bit_C == 4 );
    BOOST_CHECK( idx_C == 2 );
    BOOST_CHECK( simplex.m_v[idx_C] == p3 );
    BOOST_CHECK( simplex.m_a[idx_C] == a3 );
    BOOST_CHECK( simplex.m_b[idx_C] == b3 );

    BOOST_CHECK( simplex.m_v[3] == p4 );
    BOOST_CHECK( simplex.m_a[3] == a4 );
    BOOST_CHECK( simplex.m_b[3] == b4 );

    BOOST_CHECK( !convex::is_point_in_simplex( not_in_simplex, simplex ) );
    BOOST_CHECK(  convex::is_point_in_simplex( p1, simplex )             );
    BOOST_CHECK(  convex::is_point_in_simplex( p2, simplex )             );
    BOOST_CHECK(  convex::is_point_in_simplex( p3, simplex )             );
    BOOST_CHECK(  convex::is_point_in_simplex( p4, simplex )             );

    // Test what happens if we try to insert five vertices into the simplex

    V const p5 = V::make(2.3, 2.5, 2.2);
    V const a5 = V::make(2.1, 2.1, 2.3);
    V const b5 = V::make(2.2, 2.5, 2.1);

    BOOST_CHECK_THROW( convex::add_point_to_simplex( p5, a5, b5, simplex ), std::logic_error );

    // Now let us erase one of the simplex vertices

    simplex.m_bitmask = simplex.m_bitmask & ~bit_B;

    BOOST_CHECK( !convex::is_full_simplex(simplex) );
    BOOST_CHECK( convex::dimension(simplex) == 3u );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A ) );
    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    BOOST_CHECK( bit_B == 4 );
    BOOST_CHECK( idx_B == 2 );
    BOOST_CHECK( simplex.m_v[idx_B] == p3 );
    BOOST_CHECK( simplex.m_a[idx_B] == a3 );
    BOOST_CHECK( simplex.m_b[idx_B] == b3 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    bit_C  = 0xFFFF;
    idx_C  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ) );

    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    BOOST_CHECK( bit_B == 4 );
    BOOST_CHECK( idx_B == 2 );
    BOOST_CHECK( simplex.m_v[idx_B] == p3 );
    BOOST_CHECK( simplex.m_a[idx_B] == a3 );
    BOOST_CHECK( simplex.m_b[idx_B] == b3 );

    BOOST_CHECK( bit_C == 8 );
    BOOST_CHECK( idx_C == 3 );
    BOOST_CHECK( simplex.m_v[idx_C] == p4 );
    BOOST_CHECK( simplex.m_a[idx_C] == a4 );
    BOOST_CHECK( simplex.m_b[idx_C] == b4 );

    BOOST_CHECK( !convex::is_point_in_simplex( not_in_simplex, simplex ) );
    BOOST_CHECK(  convex::is_point_in_simplex( p1, simplex )             );
    BOOST_CHECK( !convex::is_point_in_simplex( p2, simplex )             );
    BOOST_CHECK(  convex::is_point_in_simplex( p3, simplex )             );
    BOOST_CHECK(  convex::is_point_in_simplex( p4, simplex )             );

    // Let us erase one more simplex

    simplex.m_bitmask = simplex.m_bitmask & ~bit_B;

    BOOST_CHECK( !convex::is_full_simplex(simplex) );
    BOOST_CHECK( convex::dimension(simplex) == 2u );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A ) );
    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    BOOST_CHECK( bit_B == 8 );
    BOOST_CHECK( idx_B == 3 );
    BOOST_CHECK( simplex.m_v[idx_B] == p4 );
    BOOST_CHECK( simplex.m_a[idx_B] == a4 );
    BOOST_CHECK( simplex.m_b[idx_B] == b4 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    bit_C  = 0xFFFF;
    idx_C  = 0xFFFF;
    BOOST_CHECK_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ), std::logic_error );

    BOOST_CHECK( !convex::is_point_in_simplex( not_in_simplex, simplex ) );
    BOOST_CHECK(  convex::is_point_in_simplex( p1, simplex )             );
    BOOST_CHECK( !convex::is_point_in_simplex( p2, simplex )             );
    BOOST_CHECK( !convex::is_point_in_simplex( p3, simplex )             );
    BOOST_CHECK(  convex::is_point_in_simplex( p4, simplex )             );

    // Insert a new simplex vertex

    BOOST_CHECK_NO_THROW( convex::add_point_to_simplex( p5, a5, b5, simplex ) );

    BOOST_CHECK( !convex::is_full_simplex(simplex) );
    BOOST_CHECK( convex::dimension(simplex) == 3u );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A ) );
    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    BOOST_CHECK( bit_B == 2 );
    BOOST_CHECK( idx_B == 1 );
    BOOST_CHECK( simplex.m_v[idx_B] == p5 );
    BOOST_CHECK( simplex.m_a[idx_B] == a5 );
    BOOST_CHECK( simplex.m_b[idx_B] == b5 );

    bit_A  = 0xFFFF;
    idx_A  = 0xFFFF;
    bit_B  = 0xFFFF;
    idx_B  = 0xFFFF;
    bit_C  = 0xFFFF;
    idx_C  = 0xFFFF;
    BOOST_CHECK_NO_THROW( convex::get_used_indices( simplex.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ) );

    BOOST_CHECK( bit_A == 1 );
    BOOST_CHECK( idx_A == 0 );
    BOOST_CHECK( simplex.m_v[idx_A] == p1 );
    BOOST_CHECK( simplex.m_a[idx_A] == a1 );
    BOOST_CHECK( simplex.m_b[idx_A] == b1 );

    BOOST_CHECK( bit_B == 2 );
    BOOST_CHECK( idx_B == 1 );
    BOOST_CHECK( simplex.m_v[idx_B] == p5 );
    BOOST_CHECK( simplex.m_a[idx_B] == a5 );
    BOOST_CHECK( simplex.m_b[idx_B] == b5 );

    BOOST_CHECK( bit_C == 8 );
    BOOST_CHECK( idx_C == 3 );
    BOOST_CHECK( simplex.m_v[idx_C] == p4 );
    BOOST_CHECK( simplex.m_a[idx_C] == a4 );
    BOOST_CHECK( simplex.m_b[idx_C] == b4 );

    BOOST_CHECK( !convex::is_point_in_simplex( not_in_simplex, simplex ) );
    BOOST_CHECK(  convex::is_point_in_simplex( p1, simplex )             );
    BOOST_CHECK( !convex::is_point_in_simplex( p2, simplex )             );
    BOOST_CHECK( !convex::is_point_in_simplex( p3, simplex )             );
    BOOST_CHECK(  convex::is_point_in_simplex( p4, simplex )             );
    BOOST_CHECK(  convex::is_point_in_simplex( p5, simplex )             );

}

// 2011-11-12: Kenny: The degenerate tests always test if a tetrahedron will be degenerate, what about triangle and line cases?
// 2011-11-12: Kenny: It seems to me that you assume some order on the simplex vertices. In principle one could have bitmask = 11 = 13 = 14 = 14, corresponding to an used mask 0111, 1011, 1101, 1110. Does this order have any influence on your test? The same goes for triangle and line cases.
// 2011-11-12: Kenny: There is no tests that confirm non-degenerate point is correctly classified.

BOOST_AUTO_TEST_CASE(simplex_degenerate_point)
{
    typedef tiny::MathTypes<double>        math_types;
    typedef math_types::vector3_type       V;
    typedef math_types::real_type          T;
    typedef convex::Simplex<V>             S;

    S simplex;

    // Vertify that simplex have been initialized correctly as being empty and ``zeroed''
    BOOST_CHECK( simplex.m_bitmask == 0u );

    for(size_t i=0u; i<100u; ++i)
    {
        V const rand_in0 = V::random();
        V const rand_in1 = V::random();
        V const rand_in2 = V::random();

        simplex.m_v[0] = rand_in0;
        simplex.m_v[1] = rand_in1;
        simplex.m_v[2] = rand_in2;
        simplex.m_bitmask = 7;

        bool const is_degenerate = convex::is_degenerate_point(rand_in1, simplex);
        BOOST_CHECK( is_degenerate == true);
    }

    for(size_t i=0u; i<100u; ++i)
    {
        V const rand_in0 = V::random();
        V const rand_in1 = V::random();
        V const rand_in2 = V::random();
        simplex.m_v[0] = rand_in0;
        simplex.m_v[1] = rand_in1;
        simplex.m_v[2] = rand_in2;
        simplex.m_bitmask = 7;

        // 2011-11-12: Kenny Why not use tiny::Random value; w0 = value(); ????
        T w0 = std::rand()/RAND_MAX;
        T w1 = std::rand()/RAND_MAX;

        // 2011-11-12: Kenny Why use a while loop? You could just do tiny::Random value = tiny::Random( 0, 0.5) or someting like that? Or just divide by some positive number larger than or equal 2?
        // 2011-11-12: Kenny Why not use value_traits or hardwire 1 to the floating point precision used in the test?
        while (w0+w1 >= 1)
        {
            w0 = std::rand()/RAND_MAX;
            w1 = std::rand()/RAND_MAX;
        }

        // 2011-11-12: Kenny Why not use value_traits or hardwire 1 to the floating point precision used in the test?
        // 2011-11-12: Kenny: If by design you initialize w0 and w1 to sum to one instead of using the while loop then all w's could be const declared.
        T w2 = 1 - w0 - w1;

        V const new_in_plane = w0*rand_in0 + w1*rand_in1 + w2*rand_in2;

        bool const is_degenerate = convex::is_degenerate_point(new_in_plane, simplex);
        BOOST_CHECK( is_degenerate == true);
    }

}
BOOST_AUTO_TEST_SUITE_END();


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <boost/math/special_functions/laguerre.hpp>
#include <eve/module/polynomial.hpp>

//==================================================================================================
//== Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of laguerre on wide"
        , eve::test::simd::ieee_reals

        )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  using wi_t = eve::as_integer_t<T>;
  using i_t  = eve::as_integer_t<v_t>;
  TTS_EXPR_IS( eve::laguerre(i_t(), T())  , T);
  TTS_EXPR_IS( eve::laguerre(wi_t(), T())  , T);
  TTS_EXPR_IS( eve::laguerre(i_t(), v_t())  , v_t);
  TTS_EXPR_IS( eve::laguerre(wi_t(), v_t())  , T);

};

//==================================================================================================
//== laguerre tests
//==================================================================================================
EVE_TEST( "Check behavior of laguerre on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::between(-1, 1), eve::test::as_integer(eve::test::ramp(0)))
        )
  <typename T, typename I>(T const& a0,I const & i0)
{
  using v_t = eve::element_type_t<T>;
  auto eve__laguerrev  =  [](auto n, auto x) { return eve::laguerre(n, x); };
  for(unsigned int n=0; n < 5; ++n)
  {
    auto boost_laguerre =  [&](auto i, auto) { return boost::math::laguerre(n, a0.get(i)); };
    TTS_ULP_EQUAL(eve__laguerrev(n, a0), T(boost_laguerre), 1024);
  }
  auto boost_laguerrev =  [&](auto i, auto) { return boost::math::laguerre(i0.get(i), a0.get(i)); };
  TTS_RELATIVE_EQUAL(eve__laguerrev(i0    , a0), T(boost_laguerrev), 0.01);
  for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
  {
    auto boost_laguerre2 =  [&](auto i, auto) { return boost::math::laguerre(i0.get(i), a0.get(j)); };
    TTS_RELATIVE_EQUAL(eve__laguerrev(i0 , a0.get(j)), T(boost_laguerre2), 0.01);
  }
  for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
  {
    for(unsigned int n=0; n < eve::cardinal_v<T>; ++n)
    {
      TTS_RELATIVE_EQUAL(eve__laguerrev(i0.get(j) , a0.get(n)), v_t(boost::math::laguerre(i0.get(j), a0.get(n))), 0.01);
    }
  }
};


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_POLYNOMIALS_FUNCTIONS_SCALAR_TCHEBEVAL_HPP_INCLUDED
#define NT2_TOOLBOX_POLYNOMIALS_FUNCTIONS_SCALAR_TCHEBEVAL_HPP_INCLUDED
#include <nt2/toolbox/polynomials/functions/tchebeval.hpp>
#include <nt2/include/constants/digits.hpp>
#include <nt2/include/functions/scalar/average.hpp>
#include <nt2/toolbox/polynomials/category.hpp>
#include <nt2/sdk/meta/fusion.hpp>
#include <boost/fusion/adapted/array.hpp>
#include <nt2/include/functions/scalar/fma.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::tchebeval_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< arithmetic_<A0> >)(fusion_sequence_<A1>)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(2)
    {
      return tchebeval(result_type(a0), a1);
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is floating_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::tchebeval_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< floating_<A0> >)(fusion_sequence_<A1>)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      typename A1::const_iterator p = a1.begin();
      A0 b0 = *p++;
      A0 b1 = Zero<A0>();
      A0 b2 = Zero<A0>();;
      while (p != a1.end())
      {
        b2 = -b1;
        b1 = b0;
        b0 = nt2::fma(a0, b1, b2+*p++);
      }
      return average(b0, b2);
    }
  };
} }

#endif


/**
 * @file QuadraticCost.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the MIT License.
 * @date 2021
 */

#include <string>

#include <Eigen/Dense>

#include <ScsEigen/Logger.h>
#include <ScsEigen/QuadraticCost.h>

using namespace ScsEigen;

QuadraticCost::QuadraticCost(const Eigen::Ref<const Eigen::MatrixXd>& Q,
                             const Eigen::Ref<const Eigen::MatrixXd>& b,
                             double c)
    : Cost((Q.rows() == Q.cols() && Q.rows() == b.rows()) ? Q.rows() : 0, "Quadratic cost")
{
    if (Q.rows() != Q.cols() || Q.rows() != b.rows())
    {

        log()->error("[QuadraticCost::QuadraticCost] Q matrix must be square and the size of b "
                     "should be coherent with Q");
        assert(false);
    } else
    {
        m_Q = (Q + Q.transpose()) / 2;
        m_b = b;
        m_c = c;
    }
}

bool QuadraticCost::setQ(const Eigen::Ref<const Eigen::MatrixXd>& Q)
{
    if (m_Q.size() != 0)
    {
        if (Q.size() != m_Q.size())
        {
            log()->error("[QuadraticCost::setQ] The size of the matrix 'Q' cannot change.");
            return false;
        }
    } else if (Q.rows() != Q.cols())
    {
        log()->error("[QuadraticCost::QuadraticCost] Q matrix must be square.");
        return false;
    } else if (!this->setNumberOfVariables(Q.rows()))
    {
        log()->error("[QuadraticCost::setQ] Unable to set the number of variables.");
        return false;
    }

    m_Q = (Q + Q.transpose()) / 2;
    return true;
}

bool QuadraticCost::setB(const Eigen::Ref<const Eigen::VectorXd>& b)
{
    if (m_b.size() != 0)
    {
        if (b.size() != m_b.size())
        {
            log()->error("[QuadraticCost::setB] The size of the vector 'b' cannot change.");
            return false;
        }
    } else if (!this->setNumberOfVariables(b.size()))
    {
        log()->error("[QuadraticCost::setB] Unable to set the number of variables.");
        return false;
    }

    m_b = b;
    return true;
}

void QuadraticCost::setC(double c)
{
    m_c = c;
}

Eigen::Ref<const Eigen::VectorXd> QuadraticCost::getB() const
{
    return m_b;
}

Eigen::Ref<const Eigen::MatrixXd> QuadraticCost::getQ() const
{
    return m_Q;
}

double QuadraticCost::getC() const
{
    return m_c;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_STATISTICS_FUNCTIONS_GENERIC_LOGNPDF_HPP_INCLUDED
#define NT2_STATISTICS_FUNCTIONS_GENERIC_LOGNPDF_HPP_INCLUDED
#include <nt2/statistics/functions/lognpdf.hpp>
#include <boost/assert.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/functions/globalall.hpp>
#include <nt2/include/functions/if_else.hpp>
#include <nt2/include/functions/is_gez.hpp>
#include <nt2/include/functions/is_lez.hpp>
#include <nt2/include/functions/log.hpp>
#include <nt2/include/functions/normpdf.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( lognpdf_, tag::cpu_
                              , (A0)
                              , (generic_< floating_<A0> >)
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      A0 x = if_else(is_lez(a0), Inf<A0>(), a0);
      return normpdf(log(x))/x;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( lognpdf_, tag::cpu_
                            , (A0)(A1)
                            , (generic_<floating_<A0> > )
                              (generic_<floating_<A1> >)
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
      {
        A0 x = if_else(is_lez(a0), Inf<A0>(), a0);
        return normpdf(log(x), a1)/x;
      }
  };

  BOOST_DISPATCH_IMPLEMENT  ( lognpdf_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (generic_< floating_<A0> >)
                              (generic_< floating_<A1> >)
                              (generic_< floating_<A2> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(3)
    {
      BOOST_ASSERT_MSG(nt2::globalall(nt2::is_gez(a2)), "sigma(s) must be positive");
      A0 x = if_else(is_lez(a0), Inf<A0>(), a0);
      return  normpdf(log(x), a1, a2)/x;
    }
  };

} }

#endif


/**
 * @file  maximumprinciple_main.cc
 * @brief NPDE homework "MaximumPrinciple" code
 * @author Oliver Rietmann
 * @date 25.03.2019
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Dense>
#include <Eigen/SparseCholesky>
#include <functional>
#include <iostream>

#include "maximumprinciple.h"

using namespace MaximumPrinciple;

int main() {
  /* SAM_LISTING_BEGIN_3 */
  unsigned int M = 4;
  double c = 0.99;
  std::function<double(double, double)> f = [](double x, double y) {
    double ret = 0.0;
    //====================
    // Your code goes here
    //====================
    return ret;
  };

  Eigen::SimplicialLLT<Eigen::SparseMatrix<double>> solver;
  Eigen::VectorXd phi = computeLoadVector(M, f);

  Eigen::SparseMatrix<double> A = computeGalerkinMatrix(M, c);
  Eigen::VectorXd mu = solver.compute(A).solve(phi);
  std::cout << "mu = " << std::endl << mu << std::endl;
  /* SAM_LISTING_END_3 */
  // Output of inverse Galerkin matrix
  Eigen::MatrixXd A_dense = A;
  std::cout << "Inverse of Galerkin matrix for M = 4, c = 0.99" << std::endl
            << A_dense.partialPivLu().inverse() << std::endl;

  Eigen::SparseMatrix<double> A_TR = computeGalerkinMatrixTR(M, c);
  Eigen::VectorXd mu_TR = solver.compute(A_TR).solve(phi);
  std::cout << "mu_TR = " << std::endl << mu_TR << std::endl;

  return 0;
}


// C++ standard libs
#include <algorithm>
#include <exception>
#include <queue>
#include <stack>
#include <tuple>
#include <unordered_set>

// Outside libs
#include <boost/functional/hash.hpp>
#include <personal.hpp>

// Local libs
#include "include/knight.hpp"

namespace kc{
//==================================================================================
// Tools
//==================================================================================
size_t knight_steps(size_t board_size, int8_t opt_level/*=0*/) {
    size_t solution;
    switch(opt_level) {
        case 0: solution = knight_steps_bfs(board_size); break;
        case 1: solution = knight_steps_astar(board_size); break;
        case 2: solution = knight_steps_parallel(board_size); break;
    }
    return solution;
}

void print_pos(const Pos& pos) {
    std::cout << "Pos: (" << pos.first << ", " << pos.second << ")\n";
}

void print_all(const Pos& pos, size_t steps) {
    std::cout << "Pos: (" << pos.first << ", " << pos.second
        << "); Steps: " << steps << std::endl;
}

//==================================================================================
// Breadth First
//==================================================================================
size_t knight_steps_bfs(size_t board_size) {
    // Initialize
    int n = board_size;
    std::queue<std::pair<Pos, size_t>> options;
    Pos goal = std::make_pair(n-1, n-1);
    Pos start = std::make_pair(0,0);
    options.push(std::make_pair(start,0));
    std::unordered_set<Pos, boost::hash<Pos>> visited;

    std::vector<Pos> movement = {std::make_pair(1,2), std::make_pair(2,1),
        std::make_pair(2,-1), std::make_pair(1,-2)};

    auto bounds_check = [&](int num) -> bool {
        return ((num >= 0) && (num < n));
    };

    while (!options.empty()) {
        // Remove from queue
        auto current = options.front();
        options.pop();
        Pos current_pos = current.first;
        size_t current_steps = current.second;
        //print_all(current_pos, current_steps);

        // Check if visited before
        if (visited.count(current_pos) > 0) {
            continue;
        } else {
            visited.insert(current_pos);
        }

        // Check if done
        if (current_pos == goal) {
            return current_steps;
        }

        // Otherwise add new options to queue
        for (auto& delta: movement) {
            auto forward_x = current_pos.first + delta.first;
            auto forward_y = current_pos.second + delta.second;
            auto back_x = current_pos.first - delta.first;
            auto back_y = current_pos.second - delta.second;
            if ((bounds_check(forward_x)) && (bounds_check(forward_y))) {
                Pos forward_move = std::make_pair(forward_x, forward_y);
                options.push(std::make_pair(forward_move, current_steps+1));
            }
            if ((bounds_check(back_x)) && (bounds_check(back_y))) {
                Pos back_move = std::make_pair(back_x, back_y);
                options.push(std::make_pair(back_move, current_steps+1));
            }
        }
    }
    return 0;
}

//==================================================================================
// A* Star
//==================================================================================
size_t knight_steps_astar(size_t board_size) {
    // Initialize
    // We want to prioritize a lower weight
    // (Pos, steps, weight)
    using Option = std::tuple<Pos, size_t, int>;
    int n = board_size;
    auto queue_sort = [] (const Option& a, const Option& b) {
        return std::get<2>(a) > std::get<2>(b);
    };

    std::priority_queue<
        Option, std::vector<Option>, decltype(queue_sort)>
        options(queue_sort);
    Pos goal = std::make_pair(n-1, n-1);
    Pos start = std::make_pair(0,0);
    auto calc_weight = [&](const Pos& current_pos, size_t steps) -> size_t {
        auto future = std::min(std::abs((current_pos.first - goal.first)),std::abs((current_pos.second - goal.second)));
        auto past = steps;
        return future + past;
    };
    options.push(std::make_tuple(start, 0, calc_weight(start,0)));
    std::unordered_set<Pos, boost::hash<Pos>> visited;

    std::vector<Pos> movement = {std::make_pair(1,2), std::make_pair(2,1),
        std::make_pair(2,-1), std::make_pair(1,-2)};

    auto bounds_check = [&](int num) -> bool {
        return ((num >= 0) && (num < n));
    };

    while (!options.empty()) {
        // Remove from queue
        auto current = options.top();
        options.pop();
        Pos current_pos = std::get<0>(current);
        int current_steps = std::get<1>(current);
        //print_all(current_pos, current_steps);

        // Check if visited before
        if (visited.count(current_pos) > 0) {
            continue;
        } else {
            visited.insert(current_pos);
        }

        // Check if done
        if (current_pos == goal) {
            return current_steps;
        }

        // Otherwise add new options to queue
        for (auto& delta: movement) {
            auto forward_x = current_pos.first + delta.first;
            auto forward_y = current_pos.second + delta.second;
            auto back_x = current_pos.first - delta.first;
            auto back_y = current_pos.second - delta.second;
            if ((bounds_check(forward_x)) && (bounds_check(forward_y))) {
                Pos forward_move = std::make_pair(forward_x, forward_y);
                options.push(std::make_tuple(
                    forward_move, current_steps+1,calc_weight(forward_move, current_steps+1)
                ));
            }
            if ((bounds_check(back_x)) && (bounds_check(back_y))) {
                Pos back_move = std::make_pair(back_x, back_y);
                options.push(std::make_tuple(
                    back_move, current_steps+1, calc_weight(back_move, current_steps+1)
                ));
            }
        }
    }
    return 0;
}

//==================================================================================
// Async
//==================================================================================
size_t knight_steps_async(size_t board_size) {
    // Initialize
    // We want to prioritize a lower weight
    // (Pos, steps, weight)
    using Option = std::tuple<Pos, size_t, int>;
    int n = board_size;
    auto queue_sort = [] (const Option& a, const Option& b) {
        return std::get<2>(a) > std::get<2>(b);
    };

    std::priority_queue<
        Option, std::vector<Option>, decltype(queue_sort)>
        options(queue_sort);
    Pos goal = std::make_pair(n-1, n-1);
    Pos start = std::make_pair(0,0);
    auto calc_weight = [&](const Pos& current_pos, size_t steps) -> size_t {
        auto future = std::min(std::abs((current_pos.first - goal.first)),std::abs((current_pos.second - goal.second)));
        auto past = steps;
        return future + past;
    };
    options.push(std::make_tuple(start, 0, calc_weight(start,0)));
    std::unordered_set<Pos, boost::hash<Pos>> visited;

    std::vector<Pos> movement = {std::make_pair(1,2), std::make_pair(2,1),
        std::make_pair(2,-1), std::make_pair(1,-2)};

    auto bounds_check = [&](int num) -> bool {
        return ((num >= 0) && (num < n));
    };

    while (!options.empty()) {
        // Remove from queue
        auto current = options.top();
        options.pop();
        Pos current_pos = std::get<0>(current);
        int current_steps = std::get<1>(current);
        //print_all(current_pos, current_steps);

        // Check if visited before
        if (visited.count(current_pos) > 0) {
            continue;
        } else {
            visited.insert(current_pos);
        }

        // Check if done
        if (current_pos == goal) {
            return current_steps;
        }

        // Otherwise add new options to queue
        for (auto& delta: movement) {
            auto forward_x = current_pos.first + delta.first;
            auto forward_y = current_pos.second + delta.second;
            auto back_x = current_pos.first - delta.first;
            auto back_y = current_pos.second - delta.second;
            if ((bounds_check(forward_x)) && (bounds_check(forward_y))) {
                Pos forward_move = std::make_pair(forward_x, forward_y);
                options.push(std::make_tuple(
                    forward_move, current_steps+1,calc_weight(forward_move, current_steps+1)
                ));
            }
            if ((bounds_check(back_x)) && (bounds_check(back_y))) {
                Pos back_move = std::make_pair(back_x, back_y);
                options.push(std::make_tuple(
                    back_move, current_steps+1, calc_weight(back_move, current_steps+1)
                ));
            }
        }
    }
    return 0;
}

//==================================================================================
// Parallel
//==================================================================================
size_t knight_steps_parallel(size_t board_size) {
    return 0;
}

}


#include <fstream>
#include <iostream>
#include <vector>

#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
#include <boost/geometry/algorithms/simplify.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
#include <boost/geometry/strategies/agnostic/simplify_douglas_peucker.hpp>
#include <boost/geometry.hpp>

namespace bg = boost::geometry;
using tokenizer = boost::tokenizer<boost::char_separator<char>>;
using point_t = bg::model::point<double, 2, bg::cs::geographic<bg::degree>>;
using polyline_t = bg::model::linestring<point_t>;

auto readFile() {
  polyline_t coordinates{};

  std::fstream coordinateFile;
  boost::char_separator<char> sep{","};
  coordinateFile.open("coordinates.csv", std::ios::in);
  if(coordinateFile.is_open()) {
    std::string line;
    while(getline(coordinateFile, line)) {
      tokenizer tok{line, sep};
      std::vector<std::string> vars {tok.begin(), tok.end()};
      point_t coord{boost::lexical_cast<double>(vars[0]), boost::lexical_cast<double>(vars[1])};
      bg::append(coordinates, coord);
    }
  }
  coordinateFile.close();
  return coordinates;
}

double ConvertToEarthRadiusProportion(const double distance) {
  constexpr double earth_radius = 6378140.0;
  return distance/earth_radius;
}

int main(int /*argc*/, char **/*argv[]*/) {
  std::cout << "Douglas Peucker simplification algorithm application using Boost::Geometry" << std::endl;
  polyline_t polyline = readFile();
  polyline_t decimated_polyline{};
  bg::strategy::simplify::douglas_peucker<point_t, bg::strategy::distance::cross_track<double>> douglas_peucker;
  bg::simplify(polyline, decimated_polyline, ConvertToEarthRadiusProportion(100.0), douglas_peucker);
  std::cout << "Original Polyline Data Points: " << polyline.size() <<std::endl;
  std::cout << "After bg::simplify applied: " << decimated_polyline.size() <<std::endl;
  return 0;
}


/**
 * \file dcs/math/stats/distribution/students_t.hpp
 *
 * \brief The Student's t distribution.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_STUDENTS_T_HPP
#define DCS_MATH_STATS_DISTRIBUTION_STUDENTS_T_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(103500) // 1.35
# 	error "Required Boost library version >= 1.35"
#endif

#include <boost/math/distributions/students_t.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/stats/distribution/chi_squared.hpp>
#include <dcs/math/stats/distribution/normal.hpp>
#include <dcs/math/stats/function/rand.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

using ::std::size_t;

/**
 * \brief The Student's t distribution with parameter \f$\nu\f$ (the degrees of
 *  freedom).
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class students_t_distribution
{
	public: typedef RealT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

	public: explicit students_t_distribution(support_type df)
		: dist_(df)
	{
		// empty
	}

	// compiler-generated copy ctor and assignment operator are fine

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * Student's t distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A random number distributed according to this Student's t
	 * distribution.
	 *
	 * A \c Student's t random number distribution produces random numbers * \f$x\f$
	 * distributed according to the probability density function:
	 * \f[
	 *   \frac{\Gamma(\frac{\nu+1}{2})} {\sqrt{\nu\pi}\,\Gamma(\frac{\nu}{2})} \left(1+\frac{x^2}{\nu} \right)^{-(\frac{\nu+1}{2})}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
		normal_distribution<value_type> norm;
		chi_squared_distribution<value_type> chi(dist_.degrees_of_freedom());
		return	::dcs::math::stats::rand(norm, rng)
				/ ::std::sqrt(
						::dcs::math::stats::rand(chi, rng)
						/ dist_.degrees_of_freedom()
					);
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * normal distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A vector of random numbers distributed according to this
	 * normal distribution.
	 *
	 * A \c normal random number distribution produces random numbers * \f$x\f$
	 * distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\mu,\sigma) = \frac{1}{\sigma\sqrt{2\pi}}\exp\left(-\frac{(x-\mu)^2}{2\sigma^2}\right)
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, size_t n)
	{
		::std::vector<support_type> rnds(n);

		for ( ; n > 0; --n)
		{
			rnds.push_back(rand(rng));
		}

		return rnds;
	}
//@}TODO

	public: support_type degrees_of_freedom() const
	{
		return dist_.degrees_of_freedom();
	}

	public: support_type location() const
	{
		return support_type(0);
	}

	public: support_type scale() const
	{
		return support_type(1);
	}

	public: support_type quantile(value_type p) const
	{
		return ::boost::math::quantile(dist_, p);
	}

	private: ::boost::math::students_t_distribution<value_type,policy_type> dist_;
};

template <
    typename CharT,
    typename CharTraitsT,
    typename RealT,
    typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, students_t_distribution<RealT,PolicyT> const& dist)
{
    return os << "StudentT("
              << "df=" <<  dist.degrees_of_freedom()
              << ")";
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_STUDENTS_T_HPP


#include <stdexcept>

#include <catch2/catch.hpp>
#define eigen_assert(x)                                                                       \
	if (!(x)) {                                                                               \
		throw std::runtime_error(#x);                                                         \
	}

#include "../../src/filters/MultiSensorEKF.h"

#include <Eigen/Core>

using namespace filters;
using namespace filters::statespace;

TEST_CASE("Test Output Jacobian", "[filters]") {
	auto outputFunc = [](const Eigen::VectorXd& x, const Eigen::VectorXd& b) {
		return (2 * x + b).topRows(x.size()-1);
	};

	Eigen::VectorXd std(Eigen::VectorXd::Ones(3));
	filters::statespace::NoiseCovMatX cov(std, 3, 2);
	filters::Output o(3, 2, 3, outputFunc, cov);

	Eigen::VectorXd state(Eigen::VectorXd::Ones(3));

	Eigen::MatrixXd jacobX = o.outputFuncJacobianX(state);
	Eigen::MatrixXd jacobV = o.outputFuncJacobianV(state);

	Eigen::MatrixXd jacobXTrue(2,3);
	jacobXTrue << 2, 0, 0,
				  0, 2, 0;

	REQUIRE((jacobX - jacobXTrue).array().abs().maxCoeff() <= 1e-4);
	REQUIRE((jacobV - jacobXTrue/2).array().abs().maxCoeff() <= 1e-4);
}

TEST_CASE("MultiSensorEKF Test Runtime Errors", "[filters]") {
	constexpr int stateDim = 3;
	constexpr int inputDim = 2;

	auto outputFunc1 = [](const Eigen::VectorXd& x, const Eigen::VectorXd& b) {
		return (2 * x + b.topRows(stateDim)).topRows(x.size() - 1);
	};
	auto outputFunc2 = [](const Eigen::VectorXd& x, const Eigen::VectorXd& b) {
		return Eigen::Vector3d::Ones() * ((2 * x + b).norm());
	};
	auto stateFunc = [](const Vectord<stateDim>& x, const Vectord<inputDim>& u,
						const Vectord<stateDim>& noise) { return x + noise; };

	NoiseCovMat<-1,-1,-1> covMat1(Eigen::MatrixXd::Identity(4, 4).eval(), stateDim, 2);
	Output out1(stateDim, 2, 4, outputFunc1, covMat1);
	NoiseCovMat<-1,-1,-1> covMat2(Eigen::MatrixXd::Identity(3, 3).eval(), stateDim, 3);
	Output out2(stateDim, 3, 3, outputFunc2, covMat2);
	NoiseCovMat<stateDim, stateDim, inputDim> processNoise(
		Eigen::Matrix<double, stateDim, stateDim>::Identity().eval());
	std::array<Output, 2> outputs = {out1, out2};
	filters::MultiSensorEKF<stateDim, inputDim, stateDim, 2> ekf(stateFunc, processNoise, 0.1,
																 outputs);
	REQUIRE_NOTHROW(ekf.predict(Eigen::Vector2d::Zero().eval()));
	REQUIRE_NOTHROW(ekf.correct<0>(Eigen::Vector2d::Ones().eval()));
	REQUIRE_NOTHROW(ekf.correct<1>(Eigen::Vector3d::Ones().eval() * 3));
}