#include <iostream>
#include <memory>
#include <random>
#include <string>

#include <Eigen/Sparse>

#include "GeometricMultigridOperators.h"
#include "InitialMultigridTestDomains.h"
#include "Renderer.h"
#include "ScalarGrid.h"
#include "Transform.h"
#include "UniformGrid.h"
#include "Utilities.h"

using namespace FluidSim2D::RenderTools;
using namespace FluidSim2D::SimTools;

std::unique_ptr<Renderer> renderer;

static constexpr int gridSize = 256;
static constexpr bool useComplexDomain = true;
static constexpr bool useSolidSphere = true;

int main(int argc, char** argv)
{
	using namespace GeometricMultigridOperators;

	using StoreReal = double;
	using SolveReal = double;

	using Vector = std::conditional<std::is_same<SolveReal, float>::value, Eigen::VectorXf, Eigen::VectorXd>::type;

	UniformGrid<CellLabels> domainCellLabels;
	VectorGrid<StoreReal> boundaryWeights;

	int mgLevels;
	{
		UniformGrid<CellLabels> baseDomainCellLabels;
		VectorGrid<StoreReal> baseBoundaryWeights;

		// Complex domain set up
		if (useComplexDomain)
			buildComplexDomain(baseDomainCellLabels,
								baseBoundaryWeights,
								gridSize,
								useSolidSphere);

		// Simple domain set up
		else
			buildSimpleDomain(baseDomainCellLabels,
								baseBoundaryWeights,
								gridSize,
								1 /*dirichlet band*/);

		// Build expanded domain
		std::pair<Vec2i, int> mgSettings = buildExpandedDomain(domainCellLabels, boundaryWeights, baseDomainCellLabels, baseBoundaryWeights);

		mgLevels = mgSettings.second;
	}

	SolveReal dx = boundaryWeights.dx();

	UniformGrid<StoreReal> solutionGrid(domainCellLabels.size(), 0);

	tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(),tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = domainCellLabels.unflatten(cellIndex);

			if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
				domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
			{
				Vec2f point(dx * Vec2f(cell));
				solutionGrid(cell) = 4. * (std::sin(2 * PI * point[0]) * std::sin(2 * PI * point[1]) +
											std::sin(4 * PI * point[0]) * std::sin(4 * PI * point[1]));
			}
		}
	});

	// Print initial guess
	solutionGrid.printAsOBJ("initialGuess");

	UniformGrid<CellLabels> coarseCellLabels = buildCoarseCellLabels(domainCellLabels);

	assert(unitTestBoundaryCells<StoreReal>(domainCellLabels, &boundaryWeights) && unitTestBoundaryCells<StoreReal>(coarseCellLabels));
	assert(unitTestExteriorCells(domainCellLabels) && unitTestExteriorCells(coarseCellLabels));
	assert(unitTestCoarsening(coarseCellLabels, domainCellLabels));

	SolveReal coarseDx = 2 * dx;

	{
		//
		// Debug test for simple transfers
		//

		// Test a simple tansfer to the coarse grid and a transfer back
		UniformGrid<StoreReal> coarseInitialGuess(coarseCellLabels.size(), 0);

		downsample<SolveReal>(coarseInitialGuess, solutionGrid, coarseCellLabels, domainCellLabels);

		coarseInitialGuess.printAsOBJ("downsampledGrid");

		// Transfer back
		UniformGrid<StoreReal> transferGrid(domainCellLabels.size(), 0);
		upsampleAndAdd<SolveReal>(transferGrid, coarseInitialGuess, domainCellLabels, coarseCellLabels);

		transferGrid.printAsOBJ("upsampledGrid");
	}

	//
	// Debug test by downsampling residual, solving for correction error and upsampling correction back
	//
	{
		//
		// Compute residual
		//

		UniformGrid<StoreReal> residualGrid(domainCellLabels.size(), 0);
		UniformGrid<StoreReal> rhsGrid(domainCellLabels.size(), 0);

		computePoissonResidual<SolveReal>(residualGrid, solutionGrid, rhsGrid, domainCellLabels, dx, &boundaryWeights);

		residualGrid.printAsOBJ("residualGrid");

		//
		// Restrict residual to coarse RHS
		//

		UniformGrid<StoreReal> coarseRHSGrid(coarseCellLabels.size(), 0);
		downsample<SolveReal>(coarseRHSGrid, residualGrid, coarseCellLabels, domainCellLabels);

		coarseRHSGrid.printAsOBJ("downsampledResidual");

		//
		// Apply direct solver
		//

		UniformGrid<StoreReal> coarseSolution(coarseCellLabels.size(), 0);

		{

			UniformGrid<StoreReal> coarseResidualGrid(coarseCellLabels.size(), 0);

			//
			// Solver with direct solver
			//

			// Build indices
			int interiorCellCount = 0;

			UniformGrid<int> interiorCellIndices(coarseCellLabels.size(), -1);

			forEachVoxelRange(Vec2i(0), coarseCellLabels.size(), [&](const Vec2i &cell)
			{
				if (coarseCellLabels(cell) == CellLabels::INTERIOR_CELL ||
					coarseCellLabels(cell) == CellLabels::BOUNDARY_CELL)
					interiorCellIndices(cell) = interiorCellCount++;
			});

			Vector rhsVector = Vector::Zero(interiorCellCount);

			tbb::parallel_for(tbb::blocked_range<int>(0, coarseCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int> &range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = interiorCellIndices.unflatten(cellIndex);

					if (coarseCellLabels(cell) == CellLabels::INTERIOR_CELL ||
						coarseCellLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = interiorCellIndices(cell);
						assert(index >= 0);

						rhsVector(index) = coarseRHSGrid(cell);
					}
					else
					{
						assert(interiorCellIndices(cell) == -1);
					}
				}
			});

			// Build rows
			std::vector<Eigen::Triplet<SolveReal>> sparseElements;

			SolveReal gridScalar = 1. / sqr(coarseDx);
			forEachVoxelRange(Vec2i(0), coarseCellLabels.size(), [&](const Vec2i &cell)
			{
				if (coarseCellLabels(cell) == CellLabels::INTERIOR_CELL)
				{
					int diagonal = 0;
					int index = interiorCellIndices(cell);
					assert(index >= 0);

					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i adjacentCell = cellToCell(cell, axis, direction);
							assert(coarseCellLabels(adjacentCell) == CellLabels::INTERIOR_CELL ||
									coarseCellLabels(adjacentCell) == CellLabels::BOUNDARY_CELL);

							int adjacentIndex = interiorCellIndices(adjacentCell);
							assert(adjacentIndex >= 0);

							sparseElements.emplace_back(index, adjacentIndex, -gridScalar);
						}

					sparseElements.emplace_back(index, index, 4. * gridScalar);
				}
				else if (coarseCellLabels(cell) == CellLabels::BOUNDARY_CELL)
				{
					int diagonal = 0;
					int index = interiorCellIndices(cell);
					assert(index >= 0);

					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i adjacentCell = cellToCell(cell, axis, direction);

							if (coarseCellLabels(adjacentCell) == CellLabels::INTERIOR_CELL ||
								coarseCellLabels(adjacentCell) == CellLabels::BOUNDARY_CELL)
							{
								int adjacentIndex = interiorCellIndices(adjacentCell);
								assert(adjacentIndex >= 0);

								sparseElements.emplace_back(index, adjacentIndex, -gridScalar);
								++diagonal;
							}
							else
							{
								assert(interiorCellIndices(adjacentCell) == -1);
								if (coarseCellLabels(adjacentCell) == CellLabels::DIRICHLET_CELL)
									++diagonal;
							}
						}

					sparseElements.emplace_back(index, index, diagonal * gridScalar);
				}
			});

			// Solve system
			Eigen::SparseMatrix<SolveReal> sparseMatrix(interiorCellCount, interiorCellCount);
			sparseMatrix.setFromTriplets(sparseElements.begin(), sparseElements.end());
			sparseMatrix.makeCompressed();

			Eigen::SimplicialCholesky<Eigen::SparseMatrix<SolveReal>> solver;
			solver.compute(sparseMatrix);

			if (solver.info() != Eigen::Success)
			{
			    std::cout << "Solver failed to pre-compute system" << std::endl;
			    return 0;
			}

			Vector solutionVector = solver.solve(rhsVector);
			if (solver.info() != Eigen::Success)
			{
			    std::cout << "Solver failed" << std::endl;
			    return 0;
			}

			tbb::parallel_for(tbb::blocked_range<int>(0, coarseCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = interiorCellIndices.unflatten(cellIndex);

					if (coarseCellLabels(cell) == CellLabels::INTERIOR_CELL ||
						coarseCellLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = interiorCellIndices(cell);
						assert(index >= 0);

						coarseSolution(cell) = solutionVector(index);
					}
					else
					{
						assert(interiorCellIndices(cell) == -1);
					}
				}
			});

			coarseSolution.printAsOBJ("coarseSolutionGrid");
		}

		//
		// Prolongate solution
		//

		UniformGrid<StoreReal> correctionGrid(domainCellLabels.size(), 0);

		upsampleAndAdd<SolveReal>(correctionGrid, coarseSolution, domainCellLabels, coarseCellLabels);

		correctionGrid.printAsOBJ("prolongatedCorrectio");

		//
		// Apply correction
		//

		upsampleAndAdd<SolveReal>(solutionGrid, coarseSolution, domainCellLabels, coarseCellLabels);
		solutionGrid.printAsOBJ("solutionGrid");

		//
		// Print out grids
		//

		// Print domain labels to make sure they are set up correctly
		int pixelHeight = 1080;
		int pixelWidth = pixelHeight;
		renderer = std::make_unique<Renderer>("MG Error Correction and Transfer Test", Vec2i(pixelWidth, pixelHeight), Vec2f(0), 1, &argc, argv);

		ScalarGrid<float> tempGrid(Transform(dx, Vec2f(0)), domainCellLabels.size());

		tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i cell = domainCellLabels.unflatten(cellIndex);

				tempGrid(cell) = float(domainCellLabels(cell));
			}
		});

		tempGrid.drawVolumetric(*renderer, Vec3f(0), Vec3f(1), float(CellLabels::INTERIOR_CELL), float(CellLabels::BOUNDARY_CELL));

		renderer->run();
	}
}


//
// Created by Yohsuke Murase on 2020/06/04.
//

#include <iostream>
#include <ostream>
#include <vector>
#include <array>
#include <random>
#include <cassert>
#include <mpi.h>
#include <Eigen/Dense>
#include <fstream>
#include "StrategyN2M3.hpp"

// calculate the distribution of fixation probability rho
// against randomly selected N2M3 deterministic strategies

std::array<double,2> CalcPayoffs(const std::array<double,64>& stationary_state, double benefit) {
  const double cost = 1.0;
  std::array<double,2> ans = {0.0, 0.0};
  for (size_t i = 0; i < 64; i++) {
    StateN2M3 s(i);
    double pa = 0.0, pb = 0.0;
    if (s.a_1 == C) { pa -= cost; pb += benefit; }
    if (s.b_1 == C) { pb -= cost; pa += benefit; }
    ans[0] += stationary_state[i] * pa;
    ans[1] += stationary_state[i] * pb;
  }
  return std::move(ans);
}

double FixationProb(size_t N, double sigma, double e, double benefit, const StrategyN2M3 &res, const StrategyN2M3 &mut, double s_yy) {
  auto a_xx = mut.StationaryState(e);
  auto a_xy = mut.StationaryState(e, &res);

  double s_xx = CalcPayoffs(a_xx, benefit)[0];
  auto _xy = CalcPayoffs(a_xy, benefit);
  double s_xy = _xy[0];
  double s_yx = _xy[1];

  // \frac{1}{\rho} = \sum_{i=0}^{N-1} \exp\left( \sigma \sum_{j=1}^{i} \left[(N-j-1)s_{yy} + js_{yx} - (N-j)s_{xy} - (j-1)s_{xx} \right] \right) \\
  //                = \sum_{i=0}^{N-1} \exp\left( \frac{\sigma i}{2} \left[(-i+2N-3)s_{yy} + (i+1)s_{yx} - (-i+2N-1)s_{xy} - (i-1)s_{xx} \right] \right)

  double num_games = (N-1);
  s_xx /= num_games;
  s_yy /= num_games;
  s_xy /= num_games;
  s_yx /= num_games;
  double rho_inv = 0.0;
  for (int i=0; i < N; i++) {
    double x = sigma * i * 0.5 * (
        (2*N-3-i) * s_yy
            + (i+1) * s_yx
            - (2*N-1-i) * s_xy
            - (i-1) * s_xx
    );
    rho_inv += std::exp(x);
  }
  return 1.0 / rho_inv;
}

int main(int argc, char *argv[]) {
  MPI_Init(&argc, &argv);
  Eigen::initParallel();
  if( argc != 8 ) {
    std::cerr << "Error : invalid argument" << std::endl;
    std::cerr << "  Usage : " << argv[0] << " <N> <sigma> <e> <benefit> <resident 0:caprin, +:num_resident_samples> <num_mutants> <seed>" << std::endl;
    MPI_Finalize();
    return 1;
  }

  size_t N = std::strtoul(argv[1], nullptr,0);
  double sigma = std::strtod(argv[2], nullptr);
  double e = std::strtod(argv[3], nullptr);
  double benefit = std::strtod(argv[4], nullptr);

  long n_resident = std::strtol(argv[5], nullptr, 0);
  long n_mutants = std::strtol(argv[6], nullptr, 0);
  int seed = std::strtol(argv[7], nullptr, 0);

  std::uniform_int_distribution<uint64_t > dist(0, std::numeric_limits<uint64_t>::max() );

  const size_t NUM_BINS = 1000;
  std::vector<size_t> counts(NUM_BINS, 0ul);
  size_t robust_count = 0ul;

  int my_rank = 0;
  MPI_Comm_rank(MPI_COMM_WORLD, &my_rank);
  int num_procs = 0;
  MPI_Comm_size(MPI_COMM_WORLD, &num_procs);

  if (n_resident > 0) {
    {
      std::seed_seq seq = {seed, my_rank};
      std::mt19937_64 rnd(seq);
      size_t my_n_resident = n_resident / num_procs;
      if (my_rank < n_resident % num_procs) { my_n_resident++; }
      // std::cerr << "my_n_resident: " << my_n_resident << " " << my_rank << ' ' << th << std::endl;

      for (size_t i = 0; i < my_n_resident; i++) {
        uint64_t r = dist(rnd);
        StrategyN2M3 res(r);
        auto a_yy = res.StationaryState(e);
        double s_yy = CalcPayoffs(a_yy, benefit)[0];
        for (size_t j = 0; j < n_mutants; j++) {
          uint64_t r2 = dist(rnd);
          StrategyN2M3 mut(r2);
          double rho = FixationProb(N, sigma, e, benefit, res, mut, s_yy);
          size_t b = static_cast<size_t>(rho * NUM_BINS);
          counts[b]++;
          if (rho <= 1.0 / N) { robust_count++; }
        }
      }
    }
  }
  else {
    {
      std::seed_seq seq = {seed, my_rank};
      std::mt19937_64 rnd_tl(seq);

      StrategyN2M3 res = StrategyN2M3::CAPRI2();
      auto a_yy = res.StationaryState(e);
      double s_yy = CalcPayoffs(a_yy, benefit)[0];

      size_t my_n_mutants = n_mutants / num_procs;
      if (my_rank < n_mutants % num_procs) { my_n_mutants++; }
      //std::cerr << "my_n_mutants: " << my_n_mutants << ' ' << my_rank << ' ' << th << std::endl;

      for (size_t j = 0; j < my_n_mutants; j++) {
        uint64_t r2 = dist(rnd_tl);
        StrategyN2M3 mut(r2);
        double rho = FixationProb(N, sigma, e, benefit, res, mut, s_yy);
        size_t b = static_cast<size_t>(rho * NUM_BINS);
        counts[b] += 1;
        if (rho <= 1.0 / N) { robust_count++; }
      }
    }
  }

  // reduce counts
  std::vector<size_t> all_counts(NUM_BINS, 0ul);
  size_t all_robust_count = 0ul;
  MPI_Reduce(counts.data(), all_counts.data(), all_counts.size(), MPI_UNSIGNED_LONG, MPI_SUM, 0, MPI_COMM_WORLD);
  MPI_Reduce(&robust_count, &all_robust_count, 1, MPI_UNSIGNED_LONG, MPI_SUM, 0, MPI_COMM_WORLD);

  // print counts
  if (my_rank == 0) {
    std::ofstream fout("dist.dat");
    double dx = 1.0 / NUM_BINS;
    double total = (n_resident == 0) ? n_mutants : n_resident * n_mutants;
    for (size_t i = 0; i < NUM_BINS; i++) {
      fout << i * dx << ' ' << (double)all_counts[i]/total << std::endl;
    }

    std::cerr << "robust_count/total: " << all_robust_count << " / " << total << " : " << (double)all_robust_count/total << std::endl;
  }

  MPI_Finalize();
  return 0;
}


#include "decomp.h"

// #include <Eigen/Eigenvalues>
#include <Eigen/SVD>

Cx5 LowRankKernels(Cx5 const &mIn, float const thresh)
{
  Index const kSz = mIn.dimension(0) * mIn.dimension(1) * mIn.dimension(2) * mIn.dimension(3);
  Index const nK = mIn.dimension(4);
  Eigen::Map<Eigen::MatrixXcf const> m(mIn.data(), kSz, nK);
  Log::Print(FMT_STRING("SVD Kernel Size {} Kernels {}"), kSz, nK);
  auto const svd = m.transpose().bdcSvd(Eigen::ComputeThinV);
  Eigen::ArrayXf const vals = svd.singularValues();
  Index const nRetain = (vals > (vals[0] * thresh)).cast<int>().sum();
  Log::Print(FMT_STRING("Retaining {} kernels"), nRetain);
  Cx5 out(mIn.dimension(0), mIn.dimension(1), mIn.dimension(2), mIn.dimension(3), nRetain);
  Eigen::Map<Eigen::MatrixXcf> lr(out.data(), kSz, nRetain);
  lr = svd.matrixV().leftCols(nRetain).conjugate();
  return out;
}

void PCA(Cx2 const &dataIn, Cx2 &vecIn, R1 &valIn)
{
  Eigen::Map<Eigen::MatrixXcf const> data(dataIn.data(), dataIn.dimension(0), dataIn.dimension(1));
  Eigen::Map<Eigen::MatrixXcf> vecs(vecIn.data(), vecIn.dimension(0), vecIn.dimension(1));
  Eigen::Map<Eigen::VectorXf> vals(valIn.data(), valIn.dimension(0));
  assert(vecs.rows() == data.rows());
  assert(vecs.cols() == data.rows());
  assert(vals.rows() == data.rows());
  auto const svd = data.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::MatrixXcf const V = svd.matrixV();
  vecs = V;
  vals = svd.singularValues().array().sqrt();
}


#include <chrono>
#include <iomanip>
#include <iostream>
#include <limits>
#include <vector>

#include <Eigen/Dense>

using Evec = Eigen::VectorXd;
using Emat = Eigen::MatrixXd;
using EVec3 = Eigen::Vector3d;

// Assume A=(m,n), m>n
// U = (m,n), S = (n,n), VT = (n,n)
void testSVD(const Emat &U, const Evec &Sdiag, const Emat &VT, const Emat &A, const Evec &x, const Evec &b) {
    Emat S(U.cols(), VT.rows());
    S = Sdiag.asDiagonal();

    // step 1, test if USVT==A
    Emat Arecon = U * (S * VT);
    Emat Aerror = Arecon - A;
    printf("Aerror max min %g, %g\n", Aerror.maxCoeff(), Aerror.minCoeff());

    const double eps = std::numeric_limits<double>::epsilon();
    // step 2, test backward error
    Evec Sdiaginv = Sdiag;
    for (int i = 0; i < Sdiaginv.size(); i++) {
        Sdiaginv[i] = Sdiaginv[i] < Sdiag[0] * eps ? 0 : 1.0 / Sdiaginv[i];
    }

    Emat V = VT.transpose();
    for (int i = 0; i < Sdiaginv.size(); i++) {
        V.col(i) *= Sdiaginv[i];
    }

    Evec x2 = V * (U.transpose() * b);
    Evec b2 = A * x2;
    Evec xerror = x2 - x;
    Evec berror = b2 - b;
    printf("xerror max min %g, %g\n", xerror.maxCoeff(), xerror.minCoeff());
    printf("berror max min %g, %g\n", berror.maxCoeff(), berror.minCoeff());
}

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */

template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

inline double pot(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    return rnorm < 1e-12 ? 0 : 1 / rnorm;
}

int main(int argc, char **argv) {
    Eigen::initParallel();

    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = 2 * atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {-(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {-(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};

    auto pointMEquiv = surface(pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(pCheck, (double *)&(pCenterCheck[0]), scaleCheck, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    // Aup for solving MEquiv
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointMCheck.size() / 3;
    Eigen::MatrixXd Aup(checkN, equivN);
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1], pointMEquiv[3 * l + 2]);
            Aup(k, l) = pot(Cpoint, Lpoint);
        }
    }

    Evec x(Aup.cols());
    x.setRandom();
    Evec b = Aup * x;

    // jacobi svd
    using std::cout;
    using std::endl;

    {
        cout << "JacobiSVD" << endl;
        Eigen::JacobiSVD<Emat> svd(Aup, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Emat U = svd.matrixU();
        Emat VT = svd.matrixV().transpose();
        Evec Svec = svd.singularValues();
        testSVD(U, Svec, VT, Aup, x, b);
    }
    {
        cout << "BDCSVD" << endl;
        Eigen::BDCSVD<Emat> svd(Aup, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Emat U = svd.matrixU();
        Emat VT = svd.matrixV().transpose();
        Evec Svec = svd.singularValues();
        testSVD(U, Svec, VT, Aup, x, b);
    }
    {
        cout << "HouseholderQR" << endl;
        Evec x2 = Aup.colPivHouseholderQr().solve(b);
        Evec b2 = Aup * x2;
        Evec xerror = x2 - x;
        Evec berror = b2 - b;
        printf("xerror max min %g, %g\n", xerror.maxCoeff(), xerror.minCoeff());
        printf("berror max min %g, %g\n", berror.maxCoeff(), berror.minCoeff());
    }

    return 0;
}


#include <fstream>
#include <boost/config.hpp>
#include <boost/version.hpp>
// CGAL headers
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Alpha_shape_2.h>
#include <CGAL/Alpha_shape_face_base_2.h>
#include <CGAL/Alpha_shape_vertex_base_2.h>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif
#include <CGAL/point_generators_2.h>

// Qt headers
#include <QtGui>
#include <QString>
#include <QActionGroup>
#include <QFileDialog>
#include <QInputDialog>

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/AlphaShapeGraphicsItem.h>
#include <CGAL/Qt/GraphicsViewPolylineInput.h>

// for viewportsBbox
#include <CGAL/Qt/utility.h>

// the two base classes
#include "ui_Alpha_shapes_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef K::Iso_rectangle_2 Iso_rectangle_2;

typedef CGAL::Alpha_shape_vertex_base_2<K> Vb;
typedef CGAL::Alpha_shape_face_base_2<K>  Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb> Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds> Delaunay;

typedef CGAL::Alpha_shape_2<Delaunay> Alpha_shape_2;

typedef Alpha_shape_2::Alpha_iterator Alpha_iterator;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Alpha_shapes_2
{
  Q_OBJECT

private:
  double alpha;
  std::vector<Point_2> points;
  Alpha_shape_2 as;
  QGraphicsScene scene;

  CGAL::Qt::AlphaShapeGraphicsItem<Alpha_shape_2> * agi;
  CGAL::Qt::GraphicsViewPolylineInput<K> * pi;

public:
  MainWindow();

public Q_SLOTS:

  void processInput(CGAL::Object o);

  void alphaChanged(int i);

  void on_actionInsertRandomPoints_triggered();

  void on_actionLoadPoints_triggered();

  void on_actionClear_triggered();

  void on_actionRecenter_triggered();

  void open(QString fileName);

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow()
{
  setupUi(this);

  this->graphicsView->setAcceptDrops(false);

  // Add a GraphicItem for the alpha shape
  agi = new CGAL::Qt::AlphaShapeGraphicsItem<Alpha_shape_2>(&as);

  QObject::connect(this, SIGNAL(changed()),
                   agi, SLOT(modelChanged()));

  agi->setVerticesPen(QPen(Qt::red, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  agi->setEdgesPen(QPen(Qt::lightGray, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  agi->setRegularEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  agi->setSingularEdgesPen(QPen(Qt::cyan, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  agi->setRegularFacesBrush(QBrush(Qt::cyan));
  scene.addItem(agi);

  //
  // Manual handling of actions
  //

  QObject::connect(this->alphaSlider, SIGNAL(valueChanged(int)),
                   this, SLOT(alphaChanged(int)));

  QObject::connect(this->alphaBox, SIGNAL(valueChanged(int)),
                   this, SLOT(alphaChanged(int)));

  QObject::connect(this->alphaSlider, SIGNAL(valueChanged(int)),
                   this->alphaBox, SLOT(setValue(int)));

  QObject::connect(this->alphaBox, SIGNAL(valueChanged(int)),
                   this->alphaSlider, SLOT(setValue(int)));

  QObject::connect(this->actionQuit, SIGNAL(triggered()),
                   this, SLOT(close()));

  pi = new CGAL::Qt::GraphicsViewPolylineInput<K>(this, &scene, 1, false); // inputs a list with one point
  QObject::connect(pi, SIGNAL(generate(CGAL::Object)),
                   this, SLOT(processInput(CGAL::Object)));

  scene.installEventFilter(pi);
  //this->actionShowAlphaShape->setChecked(true);

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&scene);
  this->graphicsView->setMouseTracking(true);

  // Turn the vertical axis upside down
  this->graphicsView->transform().scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Alpha_shapes_2.html");
  this->addAboutCGAL();

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
          this, SLOT(open(QString)));
}

void
MainWindow::processInput(CGAL::Object o)
{
  std::list<Point_2> input;
  if(CGAL::assign(input, o)){
    if(input.size() == 1) {
      points.push_back(input.front());
      as.make_alpha_shape(points.begin(), points.end());
      as.set_alpha(alpha);
    }
    Q_EMIT( changed());
  }
}

void MainWindow::alphaChanged(int i)
{
  if (as.number_of_alphas() > 0){
    if(i < 100){
      int n = static_cast<int>((i * as.number_of_alphas())/ 100);
      if(n == 0) n++;
      alpha = as.get_nth_alpha(n);
      as.set_alpha(alpha);
    } else {
      Alpha_iterator alpha_end_it = as.alpha_end();
      alpha = (*(--alpha_end_it))+1;
      as.set_alpha(alpha);
    }
  } else {
    alpha = 0;
    as.set_alpha(0);
  }
  Q_EMIT( changed());
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */

void
MainWindow::on_actionClear_triggered()
{
  as.clear();
  points.clear();
  Q_EMIT( changed());
}

void
MainWindow::on_actionInsertRandomPoints_triggered()
{
  QRectF rect = CGAL::Qt::viewportsBbox(&scene);
  CGAL::Qt::Converter<K> convert;
  Iso_rectangle_2 isor = convert(rect);
  CGAL::Random_points_in_iso_rectangle_2<Point_2> pg((isor.min)(), (isor.max)());
  bool ok = false;

  const int number_of_points =
    QInputDialog::getInt(this,
                             tr("Number of random points"),
                             tr("Enter number of random points"),
                             100,
                             0,
                             (std::numeric_limits<int>::max)(),
                             1,
                             &ok);

  if(!ok) {
    return;
  }

  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);

  points.reserve(points.size() + number_of_points);
  for(int i = 0; i < number_of_points; ++i){
    points.push_back(*pg++);
  }
  as.make_alpha_shape(points.begin(), points.end());
  as.set_alpha(alpha);
  // default cursor
  QApplication::restoreOverrideCursor();
  Q_EMIT( changed());
}

void
MainWindow::on_actionLoadPoints_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
                                                  tr("Open Points file"),
                                                  ".",
                                                  tr("CGAL files (*.pts.cgal);;"
                                                   #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                     "WKT files (*.wktk *.WKT);;"
                                                   #endif
                                                     "All files (*)"));
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
  std::cerr << "open " << std::endl;
  std::cerr << qPrintable(fileName) << std::endl;
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::ifstream ifs(qPrintable(fileName));
  if(fileName.endsWith(".wkt",Qt::CaseInsensitive))
  {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
    CGAL::IO::read_multi_point_WKT(ifs, points);
#endif
  }
  else
  {
    K::Point_2 p;
    while(ifs >> p) {
      points.push_back(p);
    }
  }
  as.make_alpha_shape(points.begin(), points.end());
  as.set_alpha(alpha);
  // default cursor
  QApplication::restoreOverrideCursor();
  this->addToRecentFiles(fileName);
  actionRecenter->trigger();
  Q_EMIT( changed());

}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(agi->boundingRect());
  this->graphicsView->fitInView(agi->boundingRect(), Qt::KeepAspectRatio);
}

#include "Alpha_shapes_2.moc"
#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Alpha_shape_2 demo");

  // Import resources from libCGAL (Qt5).
  CGAL_QT_INIT_RESOURCES;

  MainWindow mainWindow;
  mainWindow.show();
  return app.exec();
}


#include <iostream>
#include <cstdlib>
#include <cmath>
#include <string>
#include "spida/shape/shapeT.h"
#include "spida/helper/constants.h"
#include <boost/math/special_functions/airy.hpp>
#include <boost/math/special_functions/bessel.hpp>

namespace spida{

ShapeT::ShapeT(const GridT& grid,double A,double tp) :
            Shape(grid),
            m_t(grid.getT()),
            m_A(A),m_tp(tp),
            m_offset(0.0),
            m_chirp(0.0),
            m_slow_phase(0.0),
            m_omega0(0.0) {}

std::vector<dcmplx> ShapeT::shapeCV() const
{
    std::vector<dcmplx> v;
    shapeCV(v);
    return v;
}

std::vector<double> ShapeT::shapeRV() const
{
    std::vector<double> v;
    shapeRV(v);
    return v;
}

std::vector<dcmplx> ShapeT::envelope() const
{
    std::vector<dcmplx> v;
    envelope(v);
    return v;
}

void ShapeT::shapeCV(std::vector<dcmplx>& v) const
{
    v.clear();
    v.resize(m_t.size());
    for(auto i = 0; i < m_t.size(); i++)
        v[i] = shapeCV(m_t[i]);
}

void ShapeT::shapeRV(std::vector<double>& v) const
{
    v.clear();
    v.resize(m_t.size());
    for(auto i = 0; i < m_t.size(); i++)
        v[i] = shapeRV(m_t[i]);
}

void ShapeT::envelope(std::vector<dcmplx>& v) const
{
    v.clear();
    v.resize(m_t.size());
    for(auto i = 0; i < m_t.size(); i++)
        v[i] = computeEnvelope(m_t[i]);
}

dcmplx ShapeT::slowPhaseFactor(double t) const {
    return exp(-ii*m_chirp*pow((t-m_offset),2)+ii*m_slow_phase);
}

dcmplx ShapeT::fastPhaseFactor(double t) const {
    return exp(-ii*m_omega0*(t-m_offset));
}

double GaussT::compute(double t) const
{
    return exp(-pow((t-ShapeT::offset())/ShapeT::width(),2));
}

double SechT::compute(double t) const
{
    return (1.0/cosh((t-ShapeT::offset()\
                        )/ShapeT::width()));
}

double SuperGaussT::compute(double t) const {
    return exp(-pow((t-ShapeT::offset())/ShapeT::width(),2*m_M));
}

double AiryT::compute(double t) const
{
	double airy = boost::math::airy_ai<double>((t-ShapeT::offset())/ShapeT::width());
    double apodization = exp(-pow(m_apod*(t-ShapeT::offset())/ShapeT::width(),2));
    return airy*apodization;
}

BesselT::BesselT(const GridT& grid,double A,double tp,double apod) :
    ShapeT(grid,A,tp),
    m_apod(apod),
    m_j1(boost::math::cyl_bessel_j_zero<double>(0,1)) {}

double  BesselT::compute(double t) const
{
    double bessel = boost::math::cyl_bessel_j<double>(0,m_j1*fabs(t-ShapeT::offset())/ShapeT::width());
    double apodization = exp(-pow(m_apod*(t-ShapeT::offset())/ShapeT::width(),2));
    return bessel*apodization;
}

}


#ifndef ROMPC_UTILS_HPP
#define ROMPC_UTILS_HPP

/**
	@file rompc_utils.hpp
	Header file defining useful utilities for ROMPC controller.
*/

#include <memory>
#include <iostream>
#include <Eigen/Dense>
#include <qpOASES.hpp>

using Vec3 = Eigen::Vector3d;
using Vec4 = Eigen::Vector4d;
using Mat3 = Eigen::Matrix3d;
using VecX = Eigen::VectorXd;
using MatX = Eigen::MatrixXd;
using RowMajMat = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
                      Eigen::RowMajor>;
using ArrPtr = std::unique_ptr<qpOASES::real_t[]>;

namespace ROMPC_UTILS {

class Target {
public:
    Target();
    virtual void initialize(Vec3 p, double psi) {};
    virtual Vec3 get_pos(double t);
    virtual Vec3 get_vel(double t);
    virtual Vec4 get_att_quat(double t);
    virtual Vec3 get_att_euler(double t);
    virtual Vec3 get_att_aa(double t);
    virtual Vec3 get_om(double t);
    virtual ~Target() = default;
protected:
    Vec3 _p_r_i_I; // position relative to inertial in inertial coord
    Vec3 _v_r_I_R; // inertial velocity target frame coord.
    Vec4 _q_I_to_R; // quat rotation from inertial NED to target FRD frame
    Vec3 _om_R_I_R; // ang vel (p, q, r) of R w.r.t I frame, in R coordinates
    Vec3 _euler; // euler angles (roll, pitch, yaw)
    Vec3 _aa_I_to_R; // axis angle rotation from inertial NED to target FRD frame
};

/**
    @class SGF

    @brief Steady glideslope flight target. Velocity (u,v,w) is constant,
    body rates (p,q,r) are constant and zero, no side motion (v=0), roll angle
    is zero (phi=0), and constant descent rate defined by the glideslope
    angle gamma, which is negative when velocity is below local horizon.
    When gamma = 0 this is equivalent to steady level flight.
*/
class SGF : public Target {
public:
    SGF(double S, double gamma, double th);

    // Initialize position and yaw angle
    void initialize(Vec3 p, double psi) override;

    Vec3 get_pos(double t) override;

protected:
    Vec3 _v_r_I_I; // velocity as seen from I in inertial coord.
    double _S_xy; // speed in the inertial x-y plane
    double _th; // constant pitch angle
};

/**
    @class STF

    @brief Steady turning flight target. Velocity (u,v,w) is constant,
    body rates (p,q,r) are constant (but not zero), constant yaw rate
    \dot{\psi} (implicitly defined by constant speed S and turning radius
    R), constant altitude.
*/
class STF: public Target {
public:
    STF(Vec3 v, Vec3 om, double phi,  double th, double R);
    void compute_euler(double t);

    // Initialize position and yaw angle
    void initialize(Vec3 p, double psi) override;

    Vec3 get_pos(double t) override;
    Vec4 get_att_quat(double t) override;
    Vec3 get_att_euler(double t) override;
    Vec3 get_att_aa(double t) override;

protected:
    double _phi; // roll angle constant
    double _th; // pitch angle constant
    double _psi; // yaw angle/heading
    double _psi_dot; // yaw rate
    double _R; // turning radius
    Vec3 _p_c_i_I; // center of circle w.r.t inertial frame in I coord
};

/**
    @class OCP

    @brief ROMPC Optimal control problem with only control
    constraints.
*/
class OCP {
public:
    OCP(const std::string filepath, const double tmax);
    void solve(const VecX x0, Vec4& uopt);
    bool success();
    double solve_time();
    double get_dt();
    int get_N();

private:
    void eigen_to_qpoases(const MatX& M, ArrPtr& m);
    void set_x0(const VecX x0);

    int _nV; // number of vars in OCP
    int _nC; // number of constraints
    int _n; // state dimension
    MatX _G; // J = 1/2 U^T F U + x0^T G U
    MatX _E2; // s.t. E1 U <= ub = e + E2 x0
    VecX _e;

    ArrPtr _F;
    ArrPtr _E1;
    ArrPtr _ub;
    ArrPtr _g; // g = x0^T * G
    ArrPtr _U; // solution vector U = [u0, ..., u_N-1]

    double _dt; // discretization time of system
    int _N; // horizon

    qpOASES::QProblem _ocp; // ocp object
    double _tmax; // max amount of time to solve QP
    bool _success;
    double _solve_time;
};

void tangential_transf(const Vec3& aa, Mat3& T);

void om_to_aadot(const Vec3& aa, const Vec3& om, Vec3& aadot);

void aadot_to_om(const Vec3& aa, const Vec3& aadot, Vec3& om);

}

#endif // ROMPC_UTILS_HPP


#include "yavque/Utilities/pauli_operators.hpp"

#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace yavque
{
Eigen::SparseMatrix<double> pauli_x()
{
	std::vector<Eigen::Triplet<double>> t{{1, 0, 1.0}, {0, 1, 1.0}};
	Eigen::SparseMatrix<double> res(2, 2);
	res.setFromTriplets(t.begin(), t.end());
	return res;
}
Eigen::SparseMatrix<cx_double> pauli_y()
{
	constexpr cx_double I(0., 1.);
	std::vector<Eigen::Triplet<cx_double>> t{{1, 0, I}, {0, 1, -I}};
	Eigen::SparseMatrix<cx_double> res(2, 2);
	res.setFromTriplets(t.begin(), t.end());
	return res;
}

Eigen::SparseMatrix<double> pauli_z()
{
	std::vector<Eigen::Triplet<double>> t{{0, 0, 1.0}, {1, 1, -1.0}};
	Eigen::SparseMatrix<double> res(2, 2);
	res.setFromTriplets(t.begin(), t.end());
	return res;
}

Eigen::SparseMatrix<double> pauli_xx()
{
	Eigen::SparseMatrix<double> res(4, 4);
	res.coeffRef(0, 3) = 1.0;
	res.coeffRef(1, 2) = 1.0;
	res.coeffRef(2, 1) = 1.0;
	res.coeffRef(3, 0) = 1.0;

	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> pauli_yy()
{
	Eigen::SparseMatrix<double> res(4, 4);
	res.coeffRef(0, 3) = -1.0;
	res.coeffRef(1, 2) = 1.0;
	res.coeffRef(2, 1) = 1.0;
	res.coeffRef(3, 0) = -1.0;

	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> pauli_xx_yy()
{
	std::vector<Eigen::Triplet<double>> t{{2, 1, 2.0}, {1, 2, 2.0}};
	Eigen::SparseMatrix<double> res(4, 4);
	res.setFromTriplets(t.begin(), t.end());
	return res;
}

Eigen::SparseMatrix<double> pauli_zz()
{
	Eigen::SparseMatrix<double> res(4, 4);
	res.coeffRef(0, 0) = 1.0;
	res.coeffRef(1, 1) = -1.0;
	res.coeffRef(2, 2) = -1.0;
	res.coeffRef(3, 3) = 1.0;

	res.makeCompressed();
	return res;
}
} // namespace yavque


//
//  glasso.cpp
//
//  Python bindings of graphical lasso
//
//  Created by Kohei Miyaguchi on 2017/06/10.
//  Copyright © 2017年 Kohei Miyaguchi. All rights reserved.
//

#include <iostream>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/functional.h>

namespace py = pybind11;
using namespace pybind11::literals;

struct GraphicalLassoResult {
    Eigen::MatrixXd Theta, Sigma;
    bool converged;
    GraphicalLassoResult(long m) : Theta{m, m}, Sigma{Eigen::MatrixXd::Identity(m, m)}, converged{false} {}
};

using GraphicalLasso = std::function<GraphicalLassoResult&(Eigen::MatrixXd const &, Eigen::MatrixXd const &)>;

double linesearch(const Eigen::MatrixXd& S, const Eigen::MatrixXd& W, const Eigen::MatrixXd& G, const Eigen::MatrixXd& Lambda, int iter_max=INT_MAX, bool verbose=false, double eps=1e-3);

GraphicalLasso graphicalLasso_stateful(long m, double tol, long iter_max, bool verbose, double eps) {
    using namespace Eigen;
    static const auto Zmm = MatrixXd::Zero(m, m);
    static const auto Ones = MatrixXd::Ones(m, m);
    MatrixXd W{m, m}, G{m, m};
    GraphicalLassoResult result(m);

    return [=](MatrixXd const &S, MatrixXd const &Lambda) mutable -> GraphicalLassoResult& {
        MatrixXd &Theta = result.Theta, &Sigma = result.Sigma;
        // Find feasible point of W assuming S and Sigma is SPD
        W = Sigma - S;
        MatrixXd shrinks = Lambda.array() / (W.cwiseAbs().array() + eps);
        double shrink = (Lambda.array() >= W.array()).select(shrinks, Ones).minCoeff();
        if (shrink < 1) {
            if (verbose) {
                printf("shrink rate: %f\n", shrink);
            }
            W *= shrink;
        }
        W.diagonal() = Lambda.diagonal();

        // Perform projected subgradient method over (S, Lambda, W, Theta) (not Sigma!)
        Theta = (S + W).inverse();
        double t = 0.0, gap = 2 * tol;
        for (int i = 0; i < iter_max; i++) {
            G = Theta;
            G.diagonal().setZero();
            G = (
                 ((W.array() >= Lambda.array()) * (G.array() > Zmm.array())) +
                 ((W.array() <= -Lambda.array()) * (G.array() < Zmm.array()))
                 ).select(Zmm, G);
            t = linesearch(S, W, G, Lambda, verbose);
            W = (W + t * G).cwiseMin(Lambda).cwiseMax(-Lambda);
            Theta = (S + W).inverse();
            S.cwiseProduct(Theta).sum();
            gap = S.cwiseProduct(Theta).sum() + Theta.cwiseAbs().cwiseProduct(Lambda).sum() - m;
            if (verbose) {
                printf("(glasso) step: %d, gap: %f\n", i, gap);
            }
            if (std::abs(gap / m) < tol) {
                result.converged = true;
                goto CONVERGED;
            }
        }
        result.converged = false;

    CONVERGED: // finish
        Theta = W.cwiseAbs().cwiseEqual(Lambda).select(Theta, Zmm);
        Sigma = S + W;
        return result;
    };
}

GraphicalLassoResult graphicalLasso(
        Eigen::MatrixXd const &S, Eigen::MatrixXd const &Lambda, Eigen::MatrixXd const &Sigma_init, Eigen::MatrixXd const &Theta_init,
        double tol, long iter_max, bool verbose, double eps) {
    using namespace Eigen;
    long m = S.rows();
    MatrixXd Zmm = MatrixXd::Zero(m, m);
    MatrixXd Ones = MatrixXd::Ones(m, m);
    MatrixXd W{m, m}, G{m, m};
    GraphicalLassoResult result(m);
    result.Theta = Theta_init;
    result.Sigma = Sigma_init;
    MatrixXd &Theta = result.Theta, &Sigma = result.Sigma;

    // Find feasible point of W assuming S and Sigma is SPD
    W = Sigma - S;
    MatrixXd shrinks = Lambda.array() / (W.cwiseAbs().array() + eps);
    double shrink = (Lambda.array() >= W.array()).select(shrinks, Ones).minCoeff();
    if (shrink < 1) {
        if (verbose) {
            printf("shrink rate: %f\n", shrink);
        }
        W *= shrink;
    }
    W.diagonal() = Lambda.diagonal();

    // Perform projected subgradient method over (S, Lambda, W, Theta) (not Sigma!)
    Theta = (S + W).inverse();
    double t = 0.0, gap = 2 * tol;
    for (int i = 0; i < iter_max; i++) {
        G = Theta;
        G.diagonal().setZero();
        G = (
             ((W.array() >= Lambda.array()) * (G.array() > Zmm.array())) +
             ((W.array() <= -Lambda.array()) * (G.array() < Zmm.array()))
             ).select(Zmm, G);
        t = linesearch(S, W, G, Lambda, verbose);
        W = (W + t * G).cwiseMin(Lambda).cwiseMax(-Lambda);
        Theta = (S + W).inverse();
        S.cwiseProduct(Theta).sum();
        gap = S.cwiseProduct(Theta).sum() + Theta.cwiseAbs().cwiseProduct(Lambda).sum() - m;
        if (verbose) {
            printf("(glasso) step: %d, gap: %f\n", i, gap);
        }
        if (std::abs(gap / m) < tol) {
            result.converged = true;
            goto CONVERGED;
        }
    }
    result.converged = false;

CONVERGED: // finish
    Theta = W.cwiseAbs().cwiseEqual(Lambda).select(Theta, Zmm);
    Sigma = S + W;
    return result;
}

void test_glasso() {
    using namespace Eigen;

    MatrixXd S(3, 3), K(3, 3);

    K << 3.0, 1.0, 0.0,
    1.0, 2.0, 0.5,
    0.0, 0.5, 1.0;
    S = K.inverse();

    MatrixXd Lambda = MatrixXd::Ones(3, 3) * 0.1;
    MatrixXd I = MatrixXd::Identity(3, 3);

    printf("start testing glasso:\n");
    auto result = graphicalLasso(S, Lambda, I, I, 1e-10, 100, true, 1e-5);

    MatrixXd Thetatrue(3, 3), Sigmatrue(3, 3);
    Thetatrue <<   2.04478, 0.343284,        0,
    0.343284,   1.3808, 0.262195,
    0, 0.262195, 0.835366;
    Sigmatrue <<   0.511765, -0.135294, 0.0424646,
    -0.135294,  0.805882, -0.252941,
    0.0424646, -0.252941,   1.27647;

    std::cout << "Thetaguess\n" << result.Theta << std::endl;
    std::cout << "Thetatrue\n" << Thetatrue << std::endl;
    std::cout << "Sigmaguess\n" << result.Sigma << std::endl;
    std::cout << "Sigmatrue\n" << Sigmatrue << std::endl;

    double abserror = (result.Theta - Thetatrue).cwiseAbs().sum() + (result.Sigma - Sigmatrue).cwiseAbs().sum();
    printf("absolute error: %f\n", abserror);
    assert(0.001 > abserror);
}

// subroutine of glasso
double linesearch(const Eigen::MatrixXd& S, const Eigen::MatrixXd& W, const Eigen::MatrixXd& G, const Eigen::MatrixXd& Lambda, int iter_max, bool verbose, double eps) {
    using namespace Eigen;
    double f0 = log((S + W).determinant());
    MatrixXd SWG = (S + W).inverse() * G;

    double nom = SWG.diagonal().sum();
    double denom = (SWG.array() * SWG.transpose().array()).sum();
    if (verbose) {
        printf("(linesearch) step:-, nom:%f denom:%f, f0:%f\n", nom, denom, f0);
    }
    if (std::abs(denom) <= 0.0) return 0.0;
    double t = nom / denom;
    if (t <= 0) return 0.0;

    for (int i = 0; 0.0 < t && i < iter_max; i++) {
        double f = log((S + (W + t * G).cwiseMin(Lambda).cwiseMax(-Lambda)).determinant());
        if (verbose) {
            printf("(linesearch) step:%d, t:%f f:%f, f0:%f\n", i, t, f, f0);
        }
        if (f >= f0) break;
        t *= 0.5;
    }
    return t;
}

PYBIND11_PLUGIN(glassobind) {
    py::module m("glassobind", "graphical lasso plugin");
    py::class_<GraphicalLassoResult>(m, "GraphicalLassoResult")
        .def(py::init<long>())
        .def_readonly("theta", &GraphicalLassoResult::Theta)
        .def_readonly("sigma", &GraphicalLassoResult::Sigma)
        .def_readonly("converged", &GraphicalLassoResult::converged);
    m.def("glasso", &graphicalLasso, "performs graphical lasso",
          "emp_cov"_a, "lambda"_a, "sigma_init"_a, "theta_init"_a, "tol"_a, "iter_max"_a, "verbose"_a, "eps"_a);
    m.def("test_glasso", &test_glasso, "test function");
    return m.ptr();
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file mcransac.hpp
///
/// \author Keenan Burnett
/// \brief Rigid and motion-compensated RANSAC implementations along with some auxilliary
///     SE(3) math functions.
//////////////////////////////////////////////////////////////////////////////////////////////
#pragma once
#include <math.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <chrono>
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <steam/steam.hpp>

namespace p = boost::python;
namespace np = boost::python::numpy;

/*!
   \brief Enforce orthogonality conditions on the given rotation matrix such that det(R) == 1 and R.tranpose() * R = I
   \param R The input rotation matrix either 2x2 or 3x3, will be overwritten with a slightly modified matrix to
   satisfy orthogonality conditions.
*/
void enforce_orthogonality(Eigen::MatrixXd &R);

/*!
   \brief Retrieve the rigid transformation that transforms points in p1 into points in p2.
   The output transform type (float or double) and size SE(2) vs. SE(3) depends on the size of the input points p1, p2.
   \param p1 A dim x N vector of points in either 2D (dim = 2) or 3D (dim = 3)
   \param p2 A dim x N vector of points in either 2D (dim = 2) or 3D (dim = 3)
   \param Tf [out] This matrix will be overwritten as the output transform
   \pre p1 and p2 are the same size. p1 and p2 are the matched feature point locations between two point clouds
   \post orthogonality is enforced on the rotation matrix.
*/
void get_rigid_transform(Eigen::MatrixXd p1, Eigen::MatrixXd p2, Eigen::MatrixXd &Tf);

/*!
   \brief Returns a random subset of indices, where 0 <= indices[i] <= max_index. indices are non-repeating.
*/
std::vector<int> random_subset(int max_index, int subset_size);

/*!
   \brief Returns the output of the carrot operator.
   For 3 x 1 input, carrot(x) * y is equivalent to cross_product(x, y)
   For 6 x 1 input, x = [rho, phi]^T. out = [carrot(phi), rho; 0 0 0 1]
   \param x Input vector which can be 3 x 1 or 6 x 1.
   \return If the input if 3 x 1, the output is 3 x 3, if the input is 6 x 1, the output is 4 x 4.
*/
Eigen::MatrixXd carrot(Eigen::VectorXd x);

/*!
   \brief Returns the output of the circledot operator. carrot(epsilon) * p == circledot(p) * epsilon,
   where epsilon is 6x1 and p is 4 x 1 homogeneous.
   p = [rhobar, eta]^T  circledot(p) = [eta * identity(3), -carrot(rhobar); 0 0 0 0 0 0]
   \param x Input is a 4 x 1 homogeneous 3D vector.
   \return returns the 4 x 6 output of circledot(x)
*/
Eigen::MatrixXd circledot(Eigen::VectorXd x);

/*!
   \brief This function converts from a lie vector to a 4 x 4 SE(3) transform.
   // Lie Vector xi = [rho, phi]^T (6 x 1) --> SE(3) T = [C, R; 0 0 0 1] (4 x 4)
   \param x Input vector is 6 x 1
   \return Output is 4 x SE(3) transform
*/
Eigen::Matrix4d se3ToSE3(Eigen::MatrixXd xi);

/*!
   \brief This function converts from an SE(3) transform into a lie vector
   // SE(3) T = [C, R; 0 0 0 1] (4 x 4) --> Lie Vector xi = [rho, phi]^T (6 x 1)
   \param T Input is a 4x4 SE(3) transform
   \return Output is 6x1 lie vector
*/
Eigen::VectorXd SE3tose3(Eigen::MatrixXd T);

/*!
   \brief Ensures that theta is within [0, 2 * pi)
*/
double wrapto2pi(double theta);

//* Ransac
/**
* \brief This class estimates a single rigid transform between two point clouds using RANSAC and singular value decomp
*/
class Ransac {
public:
    // p1, p2 need to be either (x, y) x N or (x, y, z) x N (must be in homogeneous coordinates)
    Ransac(const np::ndarray& p1_, const np::ndarray& p2_) {
        uint dim = p1_.shape(1);
        uint N = p1_.shape(0);
        assert(N == p2_.shape(0) && dim == p2_.shape(1) && dim >= 2);
        if (dim > 3)
            dim = 3;
        p1 = Eigen::MatrixXd::Zero(dim, N);
        p2 = Eigen::MatrixXd::Zero(dim, N);
        for (uint i = 0; i < dim; ++i) {
            for (uint j = 0; j < N; ++j) {
                p1(i, j) = double(p::extract<float>(p1_[j][i]));
                p2(i, j) = double(p::extract<float>(p2_[j][i]));
            }
        }
        T_best = Eigen::MatrixXd::Identity(dim + 1, dim + 1);
    }
    void setTolerance(double tolerance_) {tolerance = tolerance_;}
    void setInlierRatio(double inlier_ratio_) {inlier_ratio = inlier_ratio_;}
    void setMaxIterations(int iterations_) {iterations = iterations_;}
    void getTransform(Eigen::MatrixXd &Tf) {Tf = T_best;}

    /*!
       \brief Computes the transform that best aligns the two pointclouds such at T * p1 = p2
    */
    double computeModel();

    /*!
       \brief Retrieves the set of point pairs which are inliers given the current transform Tf.
    */
    void getInliers(Eigen::MatrixXd Tf, std::vector<int> &inliers);

private:
    Eigen::MatrixXd p1, p2;
    double tolerance = 0.35;
    double inlier_ratio = 0.9;
    int iterations = 100;
    Eigen::MatrixXd T_best;
};

//* MCRansac
/**
* \brief This class estimates the linear velocity and angular velocity of the sensor in the body-frame.
*
* Assuming constant velocity, the motion vector can be used to estimate the transform between any two pairs of points
* if the delta_t between those points issrand(t1_[i-1][0]); known.
*
* A single transform between the two pointclouds can also be retrieved.
*
* All operations are done in SE(3) even if the input is 2D. The output motion and transforms are in 3D.
*/
class MCRansac {
public:
    MCRansac(const np::ndarray& p1_, const np::ndarray& p2_, const np::ndarray& t1_, const np::ndarray& t2_) {
        assert(p1_.shape(0) == p2_.shape(0) && p1_.shape(1) == p2_.shape(1) && p1_.shape(0) >= p1_.shape(1));
        uint N = p1_.shape(0);
        uint dim = p1_.shape(1);
        if (dim > 3)
            dim = 3;
        p1bar = Eigen::MatrixXd::Zero(4, N);
        p2bar = Eigen::MatrixXd::Zero(4, N);
        p1bar.block(3, 0, 1, N) = Eigen::MatrixXd::Ones(1, N);
        p2bar.block(3, 0, 1, N) = Eigen::MatrixXd::Ones(1, N);
        for (uint i = 0; i < dim; ++i) {
            for (uint j = 0; j < N; ++j) {
                p1bar(i, j) = double(p::extract<float>(p1_[j][i]));
                p2bar(i, j) = double(p::extract<float>(p2_[j][i]));
            }
        }
        std::vector<int64_t> t1(N, 0);
        std::vector<int64_t> t2(N, 0);
        for (uint i = 0; i < N; ++i) {
            t1[i] = int64_t(p::extract<int64_t>(t1_[i]));
            t2[i] = int64_t(p::extract<int64_t>(t2_[i]));
        }
        R_pol << pow(0.25, 2), 0, 0, 0, 0, pow(0.0157, 2), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
        delta_ts = std::vector<double>(N, 0.0);
        for (uint i = 0; i < N; ++i) {
            int64_t delta_t = t2[i] - t1[i];
            delta_ts[i] = double(delta_t) / 1000000.0;
            if (delta_ts[i] > max_delta_t) {
                max_delta_t = delta_ts[i];
            }
            if (delta_ts[i] < min_delta_t) {
                min_delta_t = delta_ts[i];
            }
        }
        double delta_diff = (max_delta_t - min_delta_t) / (num_transforms - 1);
        for (int i = 0; i < num_transforms; ++i) {
            delta_vec.push_back(min_delta_t + i * delta_diff);
        }
    }
    void setTolerance(double tolerance_) {tolerance = tolerance_;}
    void setInlierRatio(double inlier_ratio_) {inlier_ratio = inlier_ratio_;}
    void setMaxIterations(int iterations_) {iterations = iterations_;}
    void setMaxGNIterations(int iterations_) {max_gn_iterations = iterations_;}
    void setConvergenceThreshold(double eps) {epsilon_converge = eps;}
    void correctForDoppler(bool doppler_) {doppler = doppler_;}
    void getTransform(double delta_t, Eigen::MatrixXd &Tf);
    void getMotion(Eigen::VectorXd &w) {w = w_best;}
    void setDopplerParameter(double beta_) {beta = beta_;}

    /*!
       \brief Computes the ego-motion vector that best aligns the two pointclouds
    */
    double computeModel();

    /*!
       \brief Retrieves the set of point pairs which are inliers given the current motion estimate.
    */
    void getInliers(Eigen::VectorXd wbar, std::vector<int> &inliers);

private:
    Eigen::MatrixXd p1bar, p2bar;
    std::vector<double> delta_ts;
    double tolerance = 0.1225;
    double inlier_ratio = 0.9;
    int iterations = 100;
    int max_gn_iterations = 10;
    double epsilon_converge = 0.0001;
    double error_converge = 0.01;
    int dim = 2;
    double beta = -0.049;  // beta = (f_t / (df / dt))
    double r_observable_sq = 0.0625;
    bool doppler = false;
    int num_transforms = 21;
    double max_delta_t = 0.0;
    double min_delta_t = 0.5;
    std::vector<double> delta_vec;
    Eigen::VectorXd w_best = Eigen::VectorXd::Zero(6);
    Eigen::Matrix4d R_pol = Eigen::Matrix4d::Identity();

    /*!
       \brief Given two sets of point pairs (p1small, p2small), this function computes the motion of the sensor
       (linear and angular velocity) in the body frame using nonlinear least squares.
       \pre It's very important that the delt_t_local is accurate. Note that each azimuth in the radar scan is time
       stamped, this should be used to get the more accurate time differences.
    */
    void get_motion_parameters(std::vector<int> subset, Eigen::VectorXd &wbar);

    /*!
       \brief Retrieve the number of inliers corresponding to body motion vector wbar. (6 x 1)
    */
    int getNumInliers(Eigen::VectorXd wbar);

    /*!
       \brief Given a body motion vector wbar (6 x 1), adjust the position of point p to account
       for the Doppler distortion which may be present in the data.
    */
    void dopplerCorrection(Eigen::VectorXd wbar, Eigen::VectorXd &p);
};

// Return the inverse of a 4x4 homogeneous transformation matrix
Eigen::Matrix4d get_inverse_tf(Eigen::Matrix4d T);


// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

// This example consists of a single static target which sits in one
// place and does not move; in effect it has a "GPS" which measures
// its position

#include <Eigen/StdVector>
#include <iostream>
#include <stdint.h>

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/stuff/sampler.h>

#include "targetTypes3D.hpp"

using namespace Eigen;
using namespace std;
using namespace g2o;

int main()
{
  // Set up the optimiser
  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  // Create the block solver - the dimensions are specified because
  // 3D observations marginalise to a 3D estimate
  typedef BlockSolver<BlockSolverTraits<3, 3> > BlockSolver_3_3;
  OptimizationAlgorithmGaussNewton* solver = new OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolver_3_3>(
      g2o::make_unique<LinearSolverCholmod<BlockSolver_3_3::PoseMatrixType>>()));

  optimizer.setAlgorithm(solver);

  // Sample the actual location of the target
  Vector3d truePoint(sampleUniform(-500, 500),
                     sampleUniform(-500, 500),
                     sampleUniform(-500, 500));

  // Construct vertex which corresponds to the actual point of the target
  VertexPosition3D* position = new VertexPosition3D();
  position->setId(0);
  optimizer.addVertex(position);

  // Now generate some noise corrupted measurements; for simplicity
  // these are uniformly distributed about the true target. These are
  // modelled as a unary edge because they do not like to, say,
  // another node in the map.
  int numMeasurements = 10;
  double noiseLimit = sqrt(12.);
  double noiseSigma = noiseLimit*noiseLimit / 12.0;

  for (int i = 0; i < numMeasurements; i++)
    {
      Vector3d measurement = truePoint +
        Vector3d(sampleUniform(-0.5, 0.5) * noiseLimit,
                 sampleUniform(-0.5, 0.5) * noiseLimit,
                 sampleUniform(-0.5, 0.5) * noiseLimit);
      GPSObservationPosition3DEdge* goe = new GPSObservationPosition3DEdge();
      goe->setVertex(0, position);
      goe->setMeasurement(measurement);
      goe->setInformation(Matrix3d::Identity() / noiseSigma);
      optimizer.addEdge(goe);
    }

  // Configure and set things going
  optimizer.initializeOptimization();
  optimizer.setVerbose(true);
  optimizer.optimize(5);

  cout << "truePoint=\n" << truePoint << endl;

  cerr <<  "computed estimate=\n" << dynamic_cast<VertexPosition3D*>(optimizer.vertices().find(0)->second)->estimate() << endl;

  //position->setMarginalized(true);

  SparseBlockMatrix<MatrixXd> spinv;

  optimizer.computeMarginals(spinv, position);

  //optimizer.solver()->computeMarginals();

  // covariance
  //
  cout << "covariance\n" << spinv << endl;

  cout << spinv.block(0,0) << endl;

}


//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <iostream>
#include <boost/lexical_cast.hpp>
#include <rl/math/Constants.h>
#include <rl/math/Quaternion.h>
#include <rl/math/Rotation.h>
#include <rl/math/Vector.h>

int
main(int argc, char** argv)
{
	if (argc < 7)
	{
		std::cout << "Usage: rlEulerAnglesDemo AXIS0 AXIS1 AXIS2 DEG0 DEG1 DEG2" << std::endl;
		return EXIT_FAILURE;
	}

	rl::math::Matrix33 rotation = rl::math::Matrix33::Identity();

	for (std::size_t i = 0; i < 3; ++i)
	{
		rl::math::Real angle = boost::lexical_cast<rl::math::Real>(argv[i + 4]) * rl::math::constants::deg2rad;

		rl::math::Vector3 axis(
			0 == boost::lexical_cast<int>(argv[i + 1]) ? 1 : 0,
			1 == boost::lexical_cast<int>(argv[i + 1]) ? 1 : 0,
			2 == boost::lexical_cast<int>(argv[i + 1]) ? 1 : 0
		);
		std::cout << "angle" << i << ": " << angle << " rad - axis" << i << ": " << axis.transpose() << std::endl;

		rotation = rotation * rl::math::AngleAxis(angle, axis);
	}

	std::cout << std::endl;

	rl::math::Quaternion quaternion(rotation);
	std::cout << "quaternion.w: " << quaternion.w() << " - quaternion.vec: " << quaternion.vec().transpose() << std::endl;

	rl::math::AngleAxis angleAxis(rotation);
	std::cout << "angle: " << angleAxis.angle() << " rad - axis: " << angleAxis.axis().transpose() << std::endl;

	rl::math::Vector3 orientation = rotation.eulerAngles(2, 1, 0).reverse();
	std::cout << "x: " << orientation.x() * rl::math::constants::rad2deg << " deg - y: " << orientation.y() * rl::math::constants::rad2deg << " deg - z: " << orientation.z() * rl::math::constants::rad2deg << " deg" << std::endl;

	return EXIT_SUCCESS;
}


#ifndef LINEAR_LEAST_SQUARE_MODEL_HPP
#define LINEAR_LEAST_SQUARE_MODEL_HPP

#include <iostream>

#include <math.h>
#include <Eigen/QR>
#include <Eigen/Dense>

#include "basic_types.hpp"
#include "regression2d.hpp"

namespace GRANSAC
{

// model paramter number = 3
template<int t_param_num>
class LinearLeastSquaresModel: public AbstractModel<t_param_num>
{
protected:
	//[1  x0  x0^2] [m_a0] = [y0]
    //[1  x1  x1^2] [m_a1]   [y1]
    //[1  x2  x2^2] [m_a2]   [y2]
    //...                    ...
    //[1  xn  xn^2]          [yn]
    // std::vector<VPFloat> m_a;

	// build a lookup table to calculate point to curve distance
	// e.g. target fitting is points in a 400x400 image
	// then grid of lookup table is 40x40 cell, each cell is 10x10 pixel
	int m_grid_size_x; // cell size of grid, e.g. m_grid_size = 10 -> each cell is 10x10 pixel
	int m_grid_size_y;
	std::vector<std::vector<float> > m_occupied_list;

	virtual float computeDistanceMeasure(std::shared_ptr<AbstractParameter> input_data) override
	{
		auto ext_point2D = std::dynamic_pointer_cast<Point2D>(input_data);
		if (ext_point2D == nullptr)
			throw std::runtime_error("PolynomialModel::ComputeDistanceMeasure() - Passed parameter are not of type Point2D.");

		// build a lookup table for distance calculation
		float min_dist = std::numeric_limits<float>::max();
        for (auto each : m_occupied_list){
            float dist = fabs(ext_point2D->m_point2D[0]/m_grid_size_x-each[0]) + fabs(ext_point2D->m_point2D[1]/m_grid_size_y-each[1]); // p-1 distance, 10 is grid size
            if (min_dist > dist)
                min_dist = dist;
        }
		return min_dist; // distance in grid, 10 times smaller than actual distance
	};

public:
	LinearLeastSquaresModel(const std::vector<std::shared_ptr<AbstractParameter>> &input_data,
	               const std::map<std::string, float>& additional_params)
	{
		initialize(input_data, additional_params);
	};

	virtual void initialize(const std::vector<std::shared_ptr<AbstractParameter>> &input_data,
	                        const std::map<std::string, float>& additional_params) override
	{
		int img_width = (additional_params.count("img_width") == 1) ? additional_params.at("img_width") : 400;
		int img_height = (additional_params.count("img_height") == 1) ? additional_params.at("img_height") : 400;
		int grid_num_x = (additional_params.count("grid_num_x") == 1) ? additional_params.at("grid_num_x") : 40;
		int grid_num_y = (additional_params.count("grid_num_y") == 1) ? additional_params.at("grid_num_y") : 40;

		m_grid_size_x = img_width / grid_num_x; // e.g. 10
		m_grid_size_y = img_height / grid_num_y;

		// alway calculate curve with three points, since y = ax^2+bx+c
		if (input_data.size() < t_param_num)
			throw std::runtime_error("PolynomialModel - Number of input parameters does not match minimum number required for this model.");

		AbstractModel<t_param_num>::m_model_def_parameters = input_data;

		// compute deterministic curve parameters with 3 points
		std::vector<float> x_values, y_values, coeff;
		for (int i=0; i< input_data.size(); i++){
			auto point = std::dynamic_pointer_cast<Point2D>(input_data[i]);
			if (point == nullptr)
				throw std::runtime_error("QuadraticModel - InputParams type mismatch. It is not a Point2D.");

			x_values.push_back(point->m_point2D[0]);
			y_values.push_back(point->m_point2D[1]);
		}
		Regression2D::calculateLLS(x_values, y_values, coeff, t_param_num-1);

		std::vector<float> coeff_f(coeff.begin(), coeff.end());
		AbstractModel<t_param_num>::m_model_coeffs = coeff_f;

		m_occupied_list.reserve(grid_num_x*grid_num_y/2);
		//e.g. img_size=400x400, grid_size=40x40, cell_size=10x10
		for (int i=0; i<grid_num_x; i++){ // e.g. i=0; i<40

			float x_0 = float(i*m_grid_size_x); // e.g. i*10
			float x_1 = float((i+1)*m_grid_size_x);

            float y_0 = 0;
			float y_1 = 0;
            for (int j = 0; j < t_param_num; j++){
                y_0 += coeff[j]*std::pow(x_0, float(j));
                y_1 += coeff[j]*std::pow(x_1, float(j));
            }

			bool condi_1 = y_0 <= img_height && y_0 >=0; // e.g. condi_1 = y_0<=400 && y_0>=0
			bool condi_2 = y_1 <= img_height && y_1 >=0;
			if (condi_1 || condi_2){
				y_0 = (y_0 > img_height) ? img_height : y_0;
				y_0 = (y_0 < 0) ? 0 : y_0;
				y_1 = (y_1 > img_height) ? img_height : y_1;
				y_1 = (y_1 < 0) ? 0 : y_1;

				int y_0_idx = floor(y_0 / m_grid_size_y); // e.g. y_0_idx = floor(y_0 / 10)
				int y_1_idx = floor(y_1 / m_grid_size_y);

				if (y_0_idx < y_1_idx){
					for (int j=y_0_idx; j<y_1_idx; j++){
						std::vector<float> xy_pos{float(i), float(j)};
						m_occupied_list.push_back(xy_pos);
					}
				} else if (y_0_idx > y_1_idx){
					for (int j=y_1_idx; j<y_0_idx; j++){
						std::vector<float> xy_pos{float(i), float(j)};
						m_occupied_list.push_back(xy_pos);
					}
				} else{
					std::vector<float> xy_pos{float(i), float(y_0_idx)};
					m_occupied_list.push_back(xy_pos);
				}
			}
		}
		// //if curve go through cell, fill cell = 1, otherwise = 0

	};

	virtual std::pair<float, std::vector<std::shared_ptr<AbstractParameter> > > evaluate(const std::vector<std::shared_ptr<AbstractParameter>>& evaluate_data, float threshold)
	{
		std::vector<std::shared_ptr<AbstractParameter>> inliers;
		int n_total_data = evaluate_data.size();
		int n_inliers = 0;

		for (auto& each : evaluate_data)
		{
			if (computeDistanceMeasure(each) < threshold)
			{
				inliers.push_back(each);
				n_inliers++;
			}
		}
		float inlier_fraction = float(n_inliers) / float(n_total_data); // This is the inlier fraction
		return std::make_pair(inlier_fraction, inliers);
	};

};

}

#endif /* LINEAR_LEAST_SQUARE_MODEL_HPP */


#pragma once

#include <deal.II/base/tensor.h>
#include <complex>
#include <functional>
#include "aux/tensor_helpers.hpp"

namespace boltzmann {

template <int dim>
class QTrapz;
namespace _ {

template <class T>
struct NumberTraits;

template <int dim, int rank, class NUMBER>
struct NumberTraits<dealii::Tensor<dim, rank, NUMBER> >
{
  typedef NUMBER numeric_t;
};

template <>
struct NumberTraits<double>
{
  typedef double numeric_t;
};

template <>
struct NumberTraits<std::complex<double> >
{
  typedef std::complex<double> numeric_t;
};

}  // end namespace

template <>
class QTrapz<1>
{
 public:
  QTrapz(int npts, double a, double b);

  template <class NUMBER>
  NUMBER compute(const std::function<NUMBER(double)>& f) const;

  const std::vector<double>& get_weights() { return weights_; }

  const std::vector<double>& get_points() { return points_; }

 private:
  const double a;
  const double b;
  const int npts;
  std::vector<double> points_;
  std::vector<double> weights_;
};

/**
 * trapezoidal quadrature rule, works also with tensor valued functions
 *
 *
 * @return
 */
template <class NUMBER>
NUMBER
qtrapz1d(const std::function<NUMBER(double)>& f, double a, double b, int npts)
{
  typedef typename _::NumberTraits<NUMBER>::numeric_t numeric_t;
  const double h = (b - a) / double(npts - 1);

  NUMBER sum = 0;
  for (int i = 0; i < npts - 1; ++i) {
    const double x1 = a + h * (i);
    const double x2 = a + h * (i + 1);
    sum += f(x1) + f(x2);
  }
  sum *= numeric_t(0.5 * h);
  return sum;
}

QTrapz<1>::QTrapz(int npts, double a, double b)
    : a(a)
    , b(b)
    , npts(npts)
    , points_(npts)
    , weights_(npts)
{
  double h = (b - a) / (npts - 1);
  for (int i = 0; i < npts; ++i) {
    if (i == 0 || i == npts - 1)
      weights_[i] = 0.5 * h;
    else
      weights_[i] = h;
    points_[i] = a + i * h;
  }
}

}  // end namespace boltzmann


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

/**
 * \file boost/numeric/ublasx/operation/sqrt.hpp
 *
 * \brief Compute the square root of element of a vector or matrix expression.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_SQRT_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_SQRT_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>
//#include <complex>

namespace boost { namespace numeric { namespace ublasx {

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

namespace detail {

template <typename VectorExprT>
struct vector_sqrt_functor_traits
{
    typedef VectorExprT input_expression_type;
    typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
    typedef signature_argument_type signature_result_type;
    typedef vector_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_sqrt_functor_traits
{
    typedef MatrixExprT input_expression_type;
    typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
    typedef signature_argument_type signature_result_type;
    typedef matrix_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

// Note: this wrapper is needed since we have both templated and non-templated
//       overloaded versions of the 'sqrt' function.
//       So whithout this wrapper, the the compiler is not able to infer what
//       overloaded function to use.
template <typename T>
BOOST_UBLAS_INLINE
T sqrt_impl(T const& x)
{
    return ::std::sqrt(x);
}

} // Namespace detail

/**
 * \brief Applies the \c std::sqrt function to a given vector expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::sqrt to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_sqrt_functor_traits<VectorExprT>::result_type sqrt(vector_expression<VectorExprT> const& ve)
{
    typedef typename detail::vector_sqrt_functor_traits<VectorExprT>::expression_type expression_type;
    typedef typename detail::vector_sqrt_functor_traits<VectorExprT>::signature_argument_type signature_argument_type;

    return expression_type(ve(), detail::sqrt_impl<signature_argument_type>);
//  return expression_type(ve(), ::std::sqrt<signature_argument_type>);
//  return expression_type(ve(), ::std::sqrt<signature_result_type>);
//  typedef signature_result_type(*fun_ptr_type)(signature_argument_type);
//  fun_ptr_type ptr_sqrt_fun(&::std::sqrt);
//  return expression_type(ve(), ptr_sqrt_fun);
//  return expression_type(ve(), (signature_result_type (*)(signature_argument_type))&::std::sqrt);
}

/**
 * \brief Applies the \c std::sqrt function to a given matrix expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::sqrt to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_sqrt_functor_traits<MatrixExprT>::result_type sqrt(matrix_expression<MatrixExprT> const& me)
{
    typedef typename detail::matrix_sqrt_functor_traits<MatrixExprT>::expression_type expression_type;
    typedef typename detail::matrix_sqrt_functor_traits<MatrixExprT>::signature_argument_type signature_argument_type;

    return expression_type(me(), detail::sqrt_impl<signature_argument_type>);
//  return expression_type(me(), ::std::sqrt<signature_argument_type>);
//  return expression_type(me(), ::std::sqrt<signature_result_type>);
//  typedef signature_result_type(*fun_ptr_type)(signature_argument_type);
//  fun_ptr_type ptr_sqrt_fun(&::std::sqrt);
//  return expression_type(me(), ptr_sqrt_fun);
//  return expression_type(me(), (signature_result_type (*)(signature_argument_type))&::std::sqrt);
}

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

#endif // BOOST_NUMERIC_UBLASX_OPERATION_SQRT_HPP


/* Boost example/newton-raphson.cpp
 * Newton iteration for intervals (partial: 0/0 is missing)
 *
 * Copyright Guillaume Melquiond 2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation.
 *
 * None of the above authors make any representation about the
 * suitability of this software for any purpose. It is provided "as
 * is" without express or implied warranty.
 *
 * $Id: newton-raphson.cpp,v 1.2 2003/02/05 17:34:35 gmelquio Exp $
 */

#include <boost/numeric/interval.hpp>
#include <boost/numeric/interval/io.hpp>
#include <vector>
#include <algorithm>
#include <utility>
#include <iostream>
#include <iomanip>

template <class I> I f(const I& x)
{ return x * (x - 1.) * (x - 2.) * (x - 3.) * (x - 4.); }
template <class I> I f_diff(const I& x)
{ return (((5. * x - 40.) * x + 105.) * x - 100.) * x + 24.; }

static const double max_width = 1e-10;
static const double alpha = 0.75;

using namespace boost;
using namespace numeric;
using namespace interval_lib;

// First method: no empty intervals

typedef interval<double> I1_aux;
typedef unprotect<I1_aux>::type I1;

std::vector<I1> newton_raphson(const I1& xs) {
  std::vector<I1> l, res;
  I1 vf, vd, x, x1, x2;
  l.push_back(xs);
  while (!l.empty()) {
    x = l.back();
    l.pop_back();
    bool x2_used;
    double xx = median(x);
    vf = f(xx);
    vd = f_diff(x);
    if (in_zero(vf) && in_zero(vd)) {
      x1 = I1::whole();
      x2_used = false;
    } else {
      x1 = xx - division_part1(vf, vd, x2_used);
      if (x2_used) x2 = xx - division_part2(vf, vd);
    }
    if (overlap(x1, x)) x1 = intersect(x, x1);
    else if (x2_used) { x1 = x2; x2_used = false; }
    else continue;
    if (x2_used)
      if (overlap(x2, x)) x2 = intersect(x, x2);
      else x2_used = false;
    if (x2_used && width(x2) > width(x1)) std::swap(x1, x2);
    if (!in_zero(f(x1)))
      if (x2_used) { x1 = x2; x2_used = false; }
      else continue;
    if (width(x1) < max_width) res.push_back(x1);
    else if (width(x1) > alpha * width(x)) {
      std::pair<I1, I1> p = bisect(x);
      if (in_zero(f(p.first))) l.push_back(p.first);
      x2 = p.second;
      x2_used = true;
    } else l.push_back(x1);
    if (x2_used && in_zero(f(x2)))
      if (width(x2) < max_width) res.push_back(x1);
      else l.push_back(x2);
  }
  return res;
}

// Second method: with empty intervals

typedef change_checking<I1_aux, checking_no_nan<double> >::type I2_aux;
typedef unprotect<I2_aux>::type I2;

std::vector<I2> newton_raphson(const I2& xs) {
  std::vector<I2> l, res;
  I2 vf, vd, x, x1, x2;
  l.push_back(xs);
  while (!l.empty()) {
    x = l.back();
    l.pop_back();
    double xx = median(x);
    vf = f(xx);
    vd = f_diff(x);
    if (in_zero(vf) && in_zero(vd)) {
      x1 = x;
      x2 = I2::empty();
    } else {
      bool x2_used;
      x1 = intersect(x, xx - division_part1(vf, vd, x2_used));
      x2 = x2_used ? intersect(x, xx - division_part2(vf, vd)) : I2::empty();
    }
    if (width(x2) > width(x1)) std::swap(x1, x2);
    if (empty(x1) || !in_zero(f(x1)))
      if (!empty(x2)) { x1 = x2; x2 = I2::empty(); }
      else continue;
    if (width(x1) < max_width) res.push_back(x1);
    else if (width(x1) > alpha * width(x)) {
      std::pair<I2, I2> p = bisect(x);
      if (in_zero(f(p.first))) l.push_back(p.first);
      x2 = p.second;
    } else l.push_back(x1);
    if (!empty(x2) && in_zero(f(x2)))
      if (width(x2) < max_width) res.push_back(x1);
      else l.push_back(x2);
  }
  return res;
}

int main() {
  {
    I1_aux::traits_type::rounding rnd;
    std::vector<I1> res = newton_raphson(I1(-1, 5.1));
    std::cout << "Results: " << std::endl << std::setprecision(12);
    for(std::vector<I1>::const_iterator i = res.begin(); i != res.end(); ++i)
      std::cout << "  " << *i << std::endl;
    std::cout << std::endl;
  }
  {
    I2_aux::traits_type::rounding rnd;
    std::vector<I2> res = newton_raphson(I2(-1, 5.1));
    std::cout << "Results: " << std::endl << std::setprecision(12);
    for(std::vector<I2>::const_iterator i = res.begin(); i != res.end(); ++i)
      std::cout << "  " << *i << std::endl;
    std::cout << std::endl;
  }
}


//
// Arun Venkatraman (arunvenk@cs.cmu.edu)
// December 2016
//

#pragma once

#include <utils/math_utils_temp.hh>

#include <Eigen/Dense>

#include <sstream>
#include <vector>

namespace ilqr
{

template<int _rows, int _cols>
using Matrix = Eigen::Matrix<double, _rows, _cols>;

template<int _rows>
using Vector = Eigen::Matrix<double, _rows, 1>;

// Helper function for debugging a sequence of vectors.
template<int _dim>
std::string time_print(const std::vector<Vector<_dim>> &vectors)
{
    std::ostringstream oss;
    for (size_t t = 0; t < vectors.size(); ++t)
    {
        const Vector<_dim> &vec = vectors[t];
        oss << "t=" << t  << ": " << vec.transpose();
        if (t < vectors.size() -1)
        {
            oss << std::endl;
        }
    }
    return oss.str();
}

template<int _xdim, int _udim, typename DynamicsFunc>
void linearize_dynamics(const DynamicsFunc &dynamics_func,
                        const Vector<_xdim> &x,
                        const Vector<_udim> &u,
                        Matrix<_xdim, _xdim> &A,
                        Matrix<_xdim, _udim> &B
                       )
{
    const auto helper = [&dynamics_func](const Vector<_xdim+_udim> &xu) -> Vector<_xdim>
    {
        Vector<_xdim> x = xu.topRows(_xdim);
        Vector<_udim> u = xu.bottomRows(_udim);
        return Vector<_xdim>(dynamics_func(x,u));
    };

    Vector<_xdim + _udim> xu;
    xu.topRows(_xdim) = x;
    xu.bottomRows(_udim) = u;
    const Matrix<_xdim, _xdim+_udim> J
        = math::jacobian<_xdim+_udim, _xdim, decltype(helper)>(helper, xu);

    A = J.leftCols(_xdim);
    B = J.rightCols(_udim);
}

template<int _xdim, int _udim, typename CostFunc>
void quadratize_cost(const CostFunc &cost_func,
                     const int t,
                     const Eigen::VectorXd &x,
                     const Eigen::VectorXd &u,
                     Matrix<_xdim,_xdim> &Q,
                     Matrix<_udim,_udim> &R,
                     Matrix<_xdim,_udim> &P,
                     Vector<_xdim> &g_x,
                     Vector<_udim> &g_u
                     )
{
    const auto helper = [&cost_func, t](const Vector<_xdim+_udim> &xu) -> double
    {
        Vector<_xdim> x = xu.topRows(_xdim);
        Vector<_udim> u = xu.bottomRows(_udim);
        return double(cost_func(x,u,t));
    };

    Vector<_xdim + _udim> xu;
    xu.topRows(_xdim) = x;
    xu.bottomRows(_udim) = u;

    constexpr double ZERO_THRESH = 1e-7;

    Vector<_xdim+_udim> g
        = math::gradient<_xdim+_udim, decltype(helper)>(helper, xu);
    g = g.array() * (g.array().abs() > ZERO_THRESH).template cast<double>();
    g_x = g.topRows(_xdim);
    g_u = g.bottomRows(_udim);

    // Zero out components that are less than this threshold. We do this since
    // finite differencing has numerical issues.
    Matrix<_xdim+_udim,_xdim+_udim> H
        = math::hessian<_xdim+_udim, decltype(helper)>(helper, xu);
    //Eigen::MatrixXd H = g * g.transpose();
    H = H.array() * (H.array().abs() > ZERO_THRESH).template cast<double>();
    Q = H.topLeftCorner(_xdim, _xdim);
    P = H.topRightCorner(_xdim, _udim);
    R = H.bottomRightCorner(_udim, _udim);

    Q = (Q + Q.transpose())/2.0;
    Q = math::project_to_psd(Q, 1e-11);
    //math::check_psd(Q, 1e-12);

    // Control terms.
    R = math::project_to_psd(R, 1e-8);
    //math::check_psd(R, 1e-9);
}

template<int _xdim, typename CostFunc>
void quadratize_cost(const CostFunc &cost_func,
                     const Eigen::VectorXd &x,
                     Matrix<_xdim,_xdim> &Q,
                     Vector<_xdim> &g
                     )
{
    constexpr double ZERO_THRESH = 1e-7;

    g = math::gradient<_xdim, CostFunc>(cost_func, x);
    g = g.array() * (g.array().abs() > ZERO_THRESH).template cast<double>();

    // Zero out components that are less than this threshold. We do this since
    // finite differencing has numerical issues.
    Matrix<_xdim,_xdim> H
        = math::hessian<_xdim, CostFunc>(cost_func, x);
    //Eigen::MatrixXd H = g * g.transpose();
    Q = H.array() * (H.array().abs() > ZERO_THRESH).template cast<double>();

    Q = (Q + Q.transpose())/2.0;
    Q = math::project_to_psd(Q, 1e-11);
    math::check_psd(Q, 1e-12);
}

} // namespace ilqr


/*!
 * \file fitness_metric.cc
 *
 * \author Ethan Adams
 * \date
 *
 * This file contains the cpp version of FitnessMetric.py
 */

#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Core>

#include <unsupported/Eigen/NonLinearOptimization>

#include "BingoCpp/explicit_regression.h"
#include "BingoCpp/fitness_metric.h"

namespace bingo {

int LMFunctor::operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) {
  agraphIndv.set_constants(x);
  fvec = fit->evaluate_fitness_vector(agraphIndv, *train);
  return 0;
}

int LMFunctor::df(const Eigen::VectorXd &x, Eigen::MatrixXd &fjac) {
  double epsilon;
  epsilon = 1e-5f;

  for (int i = 0; i < x.size(); i++) {
    Eigen::VectorXd xPlus(x);
    xPlus(i) += epsilon;
    Eigen::VectorXd xMinus(x);
    xMinus(i) -= epsilon;
    Eigen::VectorXd fvecPlus(values());
    operator()(xPlus, fvecPlus);
    Eigen::VectorXd fvecMinus(values());
    operator()(xMinus, fvecMinus);
    Eigen::VectorXd fvecDiff(values());
    fvecDiff = (fvecPlus - fvecMinus) / (2.0 * epsilon);
    fjac.block(0, i, values(), 1) = fvecDiff;
  }

  return 0;
}

double FitnessMetric::evaluate_fitness(AcyclicGraph &indv,
                                       TrainingData &train) {
  if (indv.needs_optimization()) {
    optimize_constants(indv, train);
  }

  return ((evaluate_fitness_vector(indv, train)).abs()).mean();
}

void FitnessMetric::optimize_constants(AcyclicGraph &indv,
                                       TrainingData &train) {
  LMFunctor functor;
  functor.train = &train;
  functor.fit = this;
  functor.m = functor.train->Size();
  // indv.input_constants();
  functor.n = indv.count_constants();
  functor.agraphIndv = indv;
  Eigen::VectorXd vec = Eigen::VectorXd::Random(functor.n);
  Eigen::LevenbergMarquardt<LMFunctor, double> lm(functor);
  lm.minimize(vec);
  indv.set_constants(vec);
  indv.needs_opt = false;
}
} // namespace bingo


/*
MIT License

Copyright (c) 2021 Yoshifumi Asakura

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <iostream>
#include <boost/math/distributions/skew_normal.hpp>

class max_record {
  // no private
public:
  int    num_grid;
  double width_sig;
  double dmax;
  double inv_max;
  int    search_yet;
         max_record();
  double get_inv_max(
    boost::math::skew_normal_distribution<double> snd,
    double mu,
    double sigma
  );
};

max_record::max_record(){
  num_grid   = 400;
  width_sig  = 10.0;
  search_yet = 1;
  dmax       = 0.0;
  inv_max    = 1.0;
}

double max_record::get_inv_max(
    boost::math::skew_normal_distribution<double> snd,
    double mu,
    double sigma
){
  if(sigma <= 0.0){
    std::cout << ">>> error, sigma <= 0" << std::endl;
    return(0.0);
  }
  if(search_yet){
    search_yet  = 0;
    double cur;
    double x;
    double xmin = mu - sigma * width_sig;
    double xmax = mu + sigma * width_sig;
    double dx   = (xmax - xmin) / num_grid;
    for(x = xmin; x <= xmax; x += dx){
      cur = boost::math::pdf(snd, x);
      if(dmax < cur){
        dmax = cur;
      }
    }
    if(dmax == 0.0){
      inv_max = 0.0;
      std::cout << ">>> error, max of the distribusion is 0" << std::endl;
    } else {
      inv_max = 1.0 / dmax;
    }
    std::cout << ">>> " << dmax << " " << inv_max << std::endl;
  }
  return(inv_max);
}

// use this in skew_normal_1d
max_record mr;

double skew_normal_1d(
  double   mu_t,
  double   x,
  double   sigma_inner,
  double   startx,
  double   gauss_max,
  double   skew_shape
){
  // set sigma as the wave length get similar to the normal gaussian
  double sigma = sigma_inner;

  // a constant parameter
  double alpha = skew_shape * sigma;

  double mu    = mu_t + startx;

  // use below in pdf
  boost::math::skew_normal_distribution<double> snd(mu, sigma, alpha);

  double ratio = mr.get_inv_max(snd, mu, sigma);

  double out   = boost::math::pdf(snd, x) * gauss_max * ratio;
  return(out);
}


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

#include <ndt_mcl/3d_ndt_ukf.h>

#include <iostream>
#include <iomanip>

using namespace std;

int main()
{
    {
        UKF3D ukf;

        std::cout << "ukf.getLambda() : " << ukf.getLambda() << std::endl;
        std::cout << "ukf.getXsi() : " << ukf.getXsi() << std::endl;

        for(int i = 0; i < ukf.getNbSigmaPoints(); i++) {
            std::cout << "mean weight [" << i << "]:" << ukf.getWeightMean(i) << std::endl;
        }

        std::cout << "####################################################" << std::endl;

        UKF3D::Params params;
        params.alpha = 0.4;
        params.beta = 2.;
        params.kappa = 3.;
        ukf.setParams(params);

        std::cout << "ukf.getLambda() : " << ukf.getLambda() << std::endl;
        std::cout << "ukf.getXsi() : " << ukf.getXsi() << std::endl;

        for(int i = 0; i < ukf.getNbSigmaPoints(); i++) {
            std::cout << "mean weight [" << i << "]:" << ukf.getWeightMean(i) << std::endl;
        }

        Eigen::VectorXd x(6);
        x << 1, 2, 3, 0.0, 0.0, 0.0;
        std::cout << "x: " << x << std::endl;
        Eigen::Affine3d T = ndt_generic::vectorToAffine3d(x);

        Eigen::MatrixXd cov(6,6);
        cov.setZero();
        cov(0,0) = 0.1; cov(1,1) = 0.1; cov(2,2) = 0.1;
        cov(3,3) = 0.02; cov(4,4) = 0.02; cov(5,5) = 0.02;

        //ukf.initializeFilter(T, cov);
        ukf.assignSigmas(x, cov);

        std::cout << ukf.getDebugString() << std::endl;

        // Eigen::VectorXd mean = ukf.computePoseMean();
        // std::cout << "mean : " <<  ukf.computePoseMean().transpose() << std::endl;

        Eigen::VectorXd mean(6);
        std::vector<Eigen::Affine3d> T_sigmas = ukf.getSigmasAsAffine3d();
        std::vector<double> weights = ukf.getMeanWeights();
        Eigen::Affine3d T2 = ndt_generic::getAffine3dMean(T_sigmas);
        Eigen::Affine3d T3 = ndt_generic::getAffine3dMeanWeights(T_sigmas, weights);
        Eigen::Affine3d T4 = ndt_generic::getAffine3dMeanWeightsUsingQuat(T_sigmas, weights);
        Eigen::Affine3d T5 = ndt_generic::getAffine3dMeanWeightsUsingQuatNaive(T_sigmas, weights);

        std::cout << "T  -> vec : " << ndt_generic::affine3dToStringRPY(T) << std::endl;
        std::cout << "T2 -> vec : " << ndt_generic::affine3dToStringRPY(T2) << std::endl;
        std::cout << "T3 -> vec : " << ndt_generic::affine3dToStringRPY(T3) << std::endl;
        std::cout << "T4 -> vec : " << ndt_generic::affine3dToStringRPY(T4) << std::endl;
        std::cout << "T5 -> vec : " << ndt_generic::affine3dToStringRPY(T5) << std::endl;

        std::cout << " T : " << ndt_generic::affine3dToStringRotMat(T) << std::endl;
        std::cout << " T2: " << ndt_generic::affine3dToStringRotMat(T2) << std::endl;
        std::cout << " T3: " << ndt_generic::affine3dToStringRotMat(T3) << std::endl;
        std::cout << " T4: " << ndt_generic::affine3dToStringRotMat(T4) << std::endl;
        std::cout << " T5: " << ndt_generic::affine3dToStringRotMat(T5) << std::endl;

        std::cout << ukf.getDebugString() << std::endl;

        Eigen::VectorXd incr(6);
        incr << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        Eigen::Affine3d T_incr = ndt_generic::vectorToAffine3d(incr);

        std::cout << "T_incr : " << ndt_generic::affine3dToStringRotMat(T_incr) << std::endl;

        for (int i = 0; i< 5; i++) {
            std::cout << "------------------------------" << std::endl;
            std::cout << ukf.getDebugString();
            ukf.predict(T_incr, cov);
            mean = ukf.computePoseMean();

            std::cout << "mean[" << i << "]: " <<  mean.transpose() << std::endl;
        }
    }

    // {
    //     UKF3D ukf;

    //     ukf.setParams(0.4, 2., 3);

    //     Eigen::VectorXd x(6), x2(6);
    //     x << 1, 2, 3, 0.0, 0.0, 0.0;

    //     Eigen::MatrixXd cov(6,6), cov2(6,6);
    //     cov.setZero();
    //     cov(0,0) = 0.1; cov(1,1) = 0.1; cov(2,2) = 0.1;
    //     cov(3,3) = 0.02; cov(4,4) = 0.02; cov(5,5) = 0.02;

    //     for (int i = 1; i < 10; i++) {
    //         double alpha = i*0.01;
    //         double kappa = i;
    //         ukf.setParams(alpha, 2., kappa);
    //         std::cout << "x: " << x << std::endl;
    //         std::cout << "cov : " << cov << std::endl;
    //         ukf.assignSigmas(x, cov);

    //         x2 = ukf.computePoseMean();
    //         cov2 = ukf.computePoseCov(x2);

    //         std::cout << "----------------------------" << std::endl;
    //         std::cout << "x2: " << x2 << std::endl;
    //         std::cout << "cov2 : " << cov2 << std::endl;
    //     }
    // }
    {
        UKF3D ukf;

        UKF3D::Params params;
        params.alpha = 0.1;
        params.beta = 2.;
        params.kappa = 3.;
        ukf.setParams(params);

        Eigen::VectorXd x(6), x2(6);
        x << 1, 2, 3, 0.0, 0.0, 0.0;

        Eigen::MatrixXd cov(6,6), cov2(6,6);
        cov.setZero();
        cov(0,0) = 10; cov(1,1) = 1; cov(2,2) = 0.1;
        cov(3,3) = 0.02; cov(4,4) = 0.02; cov(5,5) = 0.02;

        std::cout << "x: " << x << std::endl;
        std::cout << "cov : " << cov << std::endl;
        ukf.assignSigmas(x, cov);

        x2 = ukf.computePoseMean();
        cov2 = ukf.computePoseCov(x2);

        std::cout << "----------------------------" << std::endl;
        std::cout << "x2: " << x2 << std::endl;
        std::cout << "cov2 : " << cov2 << std::endl;
    }
}