// Copyright 2017 David Wise
#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/KroneckerProduct>
#include "donorClass.h"

using namespace Eigen;

void Donor::setNucSpin(const double value) {
    Donor::coeffs.clear();
    Donor::nucSpin = value;
    setSpinsMats();
}
double Donor::getNucSpin() {
	return nucSpin;
}
void Donor::setHypCoup(const double value) {
	Donor::A = value;
}
double Donor::getHypCoup() {
	return A;
}
void Donor::setSpinsMats() {
//  Set up Electron spin operators
    Donor::coeffs.clear();
    Donor::Sx << 0,1,1,0;
    Donor::Sx *= (h_bar/2);
    Donor::Sy << 0,-i,i,0;
    Donor::Sy *= h_bar/2;
    Donor::Sz << 1,0,0,-1;
    Donor::Sz *= h_bar/2;
    Donor::IdS = MatrixXcd::Identity(2*nucSpin+1, 2*nucSpin+1);
    Donor::IdI = MatrixXcd::Identity(2, 2);

//  Set up Nuclear spin operators

    for (int inc = 1; inc < (2*nucSpin)+1; ++inc) {
        std::complex<double> cInc = sqrt(2*nucSpin*inc + inc*(1-inc));
//        std::cout << "Coeff "<< inc << " is \n" << cInc << "\n";
        Donor::coeffs.push_back(cInc);
    }

    Donor::Icr.resize(nucSpin*2+1, nucSpin*2+1);
    Donor::Ian.resize(nucSpin*2+1, nucSpin*2+1);
    for (int inc = 0; inc <= (2*nucSpin-1); ++inc) {
        Donor::Icr(inc, inc+1) = Donor::coeffs[inc];
        Donor::Ian(inc+1, inc) = Donor::coeffs[inc];
    }

    Donor::Ix = h_bar*(1.0/2.0)*(Icr+Ian);
    Donor::Iy = h_bar*(-i/(2.0))*(Icr-Ian);
    Donor::Iz = (-i/h_bar)*((Ix*Iy) - (Iy*Ix));

    Donor::Sx_f = kroneckerProduct(Sx, IdS);
    Donor::Sy_f = kroneckerProduct(Sy, IdS);
    Donor::Sz_f = kroneckerProduct(Sz, IdS);
    Donor::Ix_f = kroneckerProduct(IdI, Ix);
    Donor::Iy_f = kroneckerProduct(IdI, Iy);
    Donor::Iz_f = kroneckerProduct(IdI, Iz);

    Donor::S_I = kroneckerProduct(Sx, Ix) + kroneckerProduct(Sy, Iy) + kroneckerProduct(Sz, Iz);

}

MatrixXcd Donor::getEigs(const double B_0) {
    Donor::Ham = (ge*mu_e/h_bar)*B_0*Sz_f - (gn*mu_n/h_bar)*B_0*Iz_f + A/(pow(h_bar,2))*S_I;
    ComplexEigenSolver<MatrixXcd> es(Ham);
    es.compute(Ham);
    return es.eigenvalues();
}

void Donor::initialise(double nucVal, double hypVal) {
    if (floor(2*nucVal) != 2*nucVal) {
        throw std::invalid_argument("Please use integer or half-integer value");
    };
    Donor::nucSpin = nucVal;
    Donor::A = hypVal;
    Donor::setSpinsMats();
}


#include <Eigen/Dense>

#include "gravityField.hpp"

using namespace SnowSimulator;

GravityField::GravityField(const Eigen::Vector3f &gravity)
    : m_gravity(gravity) {}

Eigen::Vector3f GravityField::force(const Eigen::Vector3f &pos,
                                    float mass) const {
  return m_gravity * mass;
}

Eigen::Matrix3f GravityField::gradForce(const Eigen::Vector3f &pos,
                                        float mass) const {
  return Eigen::Matrix3f::Zero();
}


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
using namespace boost::math;

TEST( BoostMath, Stats )
{
	// get a normal distribution with mean and standard deviation
	double mean = 0.0;
	double stdev = 1.0;
	normal dist( mean, stdev );

	// 95% of distribution is below q:
	double q = quantile(dist, 0.95);
	EXPECT_FLOAT_EQ( 1.6448536, q );
}


#include "amr_momentum_solver.h"

#include <cmath>
#include <Eigen/Dense>

#include "../../tools/cppitertools/zip.hpp"
#include "../../definitions.h"

using namespace Eigen;
using iter::zip;

/// Get snapshot current J_i^n+1 from momentum distribution
template< typename T, int D, int V>
void vlv::MomentumSolver<T,D,V>::update_future_current( vlv::Tile<D>& tile, T cfl)
{
  //auto& yee = tile.get_yee();
  tile.jx1.clear();

  auto& step0 = tile.steps.get(0);
  for(auto&& block0 : step0) {

    auto Nx = int(block0.Nx),
         Ny = int(block0.Ny),
         Nz = int(block0.Nz);

    for (int s=0; s<Nz; s++) {
      for(int r=0; r<Ny; r++) {
        for(int q=0; q<Nx; q++) {
          const auto& M   = block0.block(q,r,s);   // f_i

          T qm = 1.0 / block0.qm;  // charge to mass ratio

          // Jx current; chi(u) = u/gamma = v
          //
          // NOTE: needs to be given in units of grid speed
          //       so we scale with dt/dx
          //yee.jx1(q,r,s) += qm*
          tile.jx1(q,r,s) += qm*cfl*
            integrate_moment(
                M,
                [](std::array<T,3> uvel) -> T
                { return uvel[0]/gamma<T,3>(uvel); }
                );
        }
      }
    }

  }// end of loop over species

  }

/*! \brief Solve Vlasov tile contents
 *
 * Exposes the actual momentum mesh from the Vlasov Tile containers
 * and feeds those to the mesh solver.
 */
template< typename T, int D, int V>
void vlv::MomentumSolver<T,D,V>::solve( vlv::Tile<D>& tile, T step_size)
{

  // init lock/mutex for mesh.clear()

  // get reference to the Vlasov fluid that we are solving
  auto& step0 = tile.steps.get(0);
  auto& step1 = tile.steps.get(1);

  // get reference to the Yee grid
  auto& yee = tile.get_yee();

  // timestep
  //auto dt   = (T) tile.dt;
  //auto dx   = (T) tile.dx;
  //T cfl  = step_size*dt/dx;
  auto cfl = step_size*tile.cfl;

  // block limits
  auto mins = tile.mins;
  //auto maxs = tile.maxs;

  /// Now get future current
  update_future_current(tile, cfl);

  // param object for solve_mesh
  vlv::tools::Params<T> params = {};
  params.cfl = cfl;

  // loop over different particle species (zips current [0] and new [1] solutions)
  for(auto&& blocks : zip(step0, step1) ) {

    // loop over the tile's internal grid
    auto& block0 = std::get<0>(blocks);
    auto& block1 = std::get<1>(blocks);

    for(int q=0; q<block0.Nx; q++) {
      for(int r=0; r<block0.Ny; r++) {
        for(int s=0; s<block0.Nz; s++) {
          T qm = 1.0 / block0.qm;  // charge to mass ratio

          // Get local field components
          vec
            B =
            {{
               (T) yee.bx(q,r,s),
               (T) yee.by(q,r,s),
               (T) yee.bz(q,r,s)
            }},

            // E-field interpolated to the middle of the tile
            // E_i = (E_i+1/2 + E_i-1/2)
            // XXX
            E =
            {{
               (T) (0.5*(yee.ex(q,r,s) + yee.ex(q-1,r,   s  ))),
               (T) (0.5*(yee.ey(q,r,s) + yee.ey(q,  r-1, s  ))),
               (T) (0.5*(yee.ez(q,r,s) + yee.ez(q,  r,   s-1)))
            }};

          // Now push E field to future temporarily
          //E[0] -= yee.jx1(q,r,s) * 0.5;

          // dig out velomeshes from blocks
          auto& mesh0 = block0.block(q,r,s);
          auto& mesh1 = block1.block(q,r,s);

          // fmt::print("solving for srq ({},{},{})\n",s,r,q);
          params.qm = qm;
          params.xloc = mins[0] + static_cast<T>(q);

          // then the final call to the actual mesh solver
          solve_mesh( mesh0, mesh1, E, B, params);
        }
      }
    }
  }

  // XXX update jx1 for debug
  //update_future_current(tile, cfl);
  //

  }

//--------------------------------------------------
// explicit template instantiation
template class vlv::MomentumSolver<Realf, 1, 1>;


#include "wave/optimization/ceres/odom_gp/point_to_line_gp.hpp"
#include <Eigen/QR>

namespace wave {

SE3PointToLineGP::SE3PointToLineGP(const double *const p,
                                   const double *const pA,
                                   const double *const pB,
                                   SE3PointToLineGPObjects &objects,
                                   const Mat3 &CovZ,
                                   bool calculate_weight)
    : pt(p), ptA(pA), ptB(pB), objects(objects) {
    this->objects.JP_T.setZero();
    this->objects.JP_T.block<3, 3>(0, 3).setIdentity();

    this->diff[0] = this->ptB[0] - this->ptA[0];
    this->diff[1] = this->ptB[1] - this->ptA[1];
    this->diff[2] = this->ptB[2] - this->ptA[2];
    this->bottom = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2];

    if (this->bottom < 1e-10) {
        // The points defining the line are too close to each other
        throw std::out_of_range("Points defining line are too close!");
    }

    this->objects.Jres_P(0, 0) = 1 - (diff[0] * diff[0] / bottom);
    this->objects.Jres_P(0, 1) = -(diff[0] * diff[1] / bottom);
    this->objects.Jres_P(0, 2) = -(diff[0] * diff[2] / bottom);
    this->objects.Jres_P(1, 0) = -(diff[1] * diff[0] / bottom);
    this->objects.Jres_P(1, 1) = 1 - (diff[1] * diff[1] / bottom);
    this->objects.Jres_P(1, 2) = -(diff[1] * diff[2] / bottom);
    this->objects.Jres_P(2, 0) = -(diff[2] * diff[0] / bottom);
    this->objects.Jres_P(2, 1) = -(diff[2] * diff[1] / bottom);
    this->objects.Jres_P(2, 2) = 1 - (diff[2] * diff[2] / bottom);

    Eigen::Vector3d unitdiff;
    double invlength = 1.0 / sqrt(this->bottom);
    if (this->diff[2] > 0) {
        unitdiff[0] = this->diff[0] * invlength;
        unitdiff[1] = this->diff[1] * invlength;
        unitdiff[2] = this->diff[2] * invlength;
    } else {
        unitdiff[0] = -this->diff[0] * invlength;
        unitdiff[1] = -this->diff[1] * invlength;
        unitdiff[2] = -this->diff[2] * invlength;
    }

    Eigen::Vector3d unitz;
    unitz << 0, 0, 1;

    auto v = unitdiff.cross(unitz);
    auto s = v.norm();
    auto c = unitz.dot(unitdiff);
    auto skew = Transformation<>::skewSymmetric3(v);
    this->objects.rotation = Eigen::Matrix3d::Identity() + skew + skew * skew * ((1 - c) / (s * s));

    this->objects.Jres_P = this->objects.rotation * this->objects.Jres_P;

    if (calculate_weight) {
        auto rotated = this->objects.Jres_P * CovZ * this->objects.Jres_P.transpose();
        this->weight_matrix = rotated.block<2, 2>(0, 0).inverse().sqrt();
    } else {
        this->weight_matrix.setIdentity();
    }
}

bool SE3PointToLineGP::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    Eigen::Map<const Mat34> tk_map(parameters[0], 3, 4);
    Eigen::Map<const Mat34> tkp1_map(parameters[1], 3, 4);

    Transformation<Eigen::Map<const Mat34>, true> Tk(tk_map);
    Transformation<Eigen::Map<const Mat34>, true> Tkp1(tkp1_map);

    Eigen::Map<const Vec6> vel_k(parameters[2], 6, 1);
    Eigen::Map<const Vec6> vel_kp1(parameters[3], 6, 1);

    if (jacobians) {
        Transformation<Mat34, true>::interpolateAndJacobians(Tk,
                                                             Tkp1,
                                                             vel_k,
                                                             vel_kp1,
                                                             this->objects.hat,
                                                             this->objects.candle,
                                                             this->objects.T_current,
                                                             this->objects.JT_Ti,
                                                             this->objects.JT_Tip1,
                                                             this->objects.JT_Wi,
                                                             this->objects.JT_Wip1);
    } else {
        Transformation<Mat34, true>::interpolate(
          Tk, Tkp1, vel_k, vel_kp1, this->objects.hat, this->objects.candle, this->objects.T_current);
    }

    Eigen::Map<const Vec3> PT(this->pt, 3, 1);
    Vec3 point = this->objects.T_current.transform(PT);

    double p_A[3] = {point(0) - this->ptA[0], point(1) - this->ptA[1], point(2) - this->ptA[2]};

    double scaling = ceres::DotProduct(p_A, diff);
    // point on line closest to point
    double p_Tl[3] = {this->ptA[0] + (scaling / bottom) * diff[0],
                      this->ptA[1] + (scaling / bottom) * diff[1],
                      this->ptA[2] + (scaling / bottom) * diff[2]};

    Eigen::Map<const Vec3> pt_Tl(p_Tl, 3, 1);
    Eigen::Map<Eigen::Vector2d> reduced(residuals, 2, 1);

    reduced = this->weight_matrix * (this->objects.rotation * (point - pt_Tl)).block<2, 1>(0, 0);

    if (jacobians != nullptr) {
        this->objects.JP_T(0, 1) = point(2);
        this->objects.JP_T(0, 2) = -point(1);
        this->objects.JP_T(1, 0) = -point(2);
        this->objects.JP_T(1, 2) = point(0);
        this->objects.JP_T(2, 0) = point(1);
        this->objects.JP_T(2, 1) = -point(0);

        // Jres_P already has rotation incorporated during construction
        this->objects.Jr_T = this->objects.Jres_P * this->objects.JP_T;

        if (jacobians[0]) {
            Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>> Jr_Tk(jacobians[0], 2, 12);
            Jr_Tk.block<2, 6>(0, 0) = this->weight_matrix * this->objects.Jr_T.block<2, 6>(0, 0) * this->objects.JT_Ti;
            Jr_Tk.block<2, 6>(0, 6).setZero();
        }
        if (jacobians[1]) {
            Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>> Jr_Tkp1(jacobians[1], 2, 12);
            Jr_Tkp1.block<2, 6>(0, 0) = this->weight_matrix * this->objects.Jr_T.block<2, 6>(0, 0) * this->objects.JT_Tip1;
            Jr_Tkp1.block<2, 6>(0, 6).setZero();
        }
        if (jacobians[2]) {
            Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor>> jac_map(jacobians[2], 2, 6);
            jac_map = this->weight_matrix * this->objects.Jr_T.block<2, 6>(0, 0) * this->objects.JT_Wi;
        }
        if (jacobians[3]) {
            Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor>> jac_map(jacobians[3], 2, 6);
            jac_map = this->weight_matrix * this->objects.Jr_T.block<2, 6>(0, 0) * this->objects.JT_Wip1;
        }
    }

    return true;
}

}  // namespace wave


#include "Tests_pcp.h"

#include <fstream>
#include <Eigen/Dense>
#include "Tresca.h"
#include "test_material_models.h"

namespace
{
	enum class AnalysisType : unsigned char
	{
		TriaxialDrained = 0,
		TriaxialUndrained = 1,
		Consolidation = 2,
		Specified = 3
	};

	void output_var(
		std::ostream &os,
		double cohesion,
		const double out_strain[3],
		const double out_stress[6],
		const double out_pstrain[3]
		)
	{
		Eigen::Matrix3d s_mat;
		s_mat << out_stress[0], out_stress[3], out_stress[5],
				 out_stress[3], out_stress[1], out_stress[4],
				 out_stress[5], out_stress[4], out_stress[2];
		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(s_mat);
		const Eigen::Vector3d& pstress = eigen_solver.eigenvalues();
		double smax = pstress[0];
		if (smax < pstress[1])
			smax = pstress[1];
		if (smax < pstress[2])
			smax = pstress[2];
		double smin = pstress[0];
		if (smin > pstress[1])
			smin = pstress[1];
		if (smin > pstress[2])
			smin = pstress[2];
		os << out_strain[0] << ", "
			<< out_strain[1] << ", "
			<< out_strain[2] << ", "
			<< out_stress[0] << ", "
			<< out_stress[1] << ", "
			<< out_stress[2] << ", "
			<< out_stress[3] << ", "
			<< out_stress[4] << ", "
			<< out_stress[5] << ", "
			<< out_pstrain[0] << ", "
			<< out_pstrain[1] << ", "
			<< out_pstrain[2] << ", "
			<< smax - smin - 2.0 * cohesion << "\n";
	}
}

void test_tresca()
{
	double de11, de22, de33;
	//AnalysisType tp = AnalysisType::Specified;
	AnalysisType tp = AnalysisType::TriaxialDrained;
	// tresca 1
	//de11 = -0.05;
	//de22 = 0.0;
	//de33 = 0.0;
	// tresca 2
	de11 = 0.05;
	de22 = 0.0;
	de33 = 0.0;
	// tresca 3
	//de11 = 0.05;
	//de22 = 0.0;
	//de33 = -0.05;
	size_t inc_num = 5000;
	size_t out_num = 100;

	double ini_stress[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
	MatModel::Tresca tc;
	tc.set_param(1000.0, 0.1, 1.0, ini_stress);

	std::fstream res_file;
	res_file.open("Tresca_res.csv", std::ios::out | std::ios::binary);
	res_file << "e11, e22, e33, s11, s22, s33, s12, s23, s31,"
				"dep11, dep22, dep33, f\n";

	de11 /= double(inc_num);
	de22 /= double(inc_num);
	de33 /= double(inc_num);
	size_t out_inv = inc_num / out_num;
	double out_strain[3] = { 0.0, 0.0, 0.0 };
	const double(*Dep_mat)[6];
	double dstrain[6];
	for (size_t i = 0; i < inc_num; ++i)
	{
		if (i % out_inv == 0) // output
			output_var(res_file, tc.get_cohesion(), out_strain, tc.get_stress(), tc.get_dstrain_p());

		switch (tp)
		{
		case AnalysisType::TriaxialDrained:
			Dep_mat = reinterpret_cast<const double(*)[6]>(tc.get_Dep_mat());
			dstrain[0] = de11;
			dstrain[1] = -Dep_mat[1][0] / (Dep_mat[1][1] + Dep_mat[1][2]) * de11;
			dstrain[2] = -Dep_mat[2][0] / (Dep_mat[2][1] + Dep_mat[2][2]) * de11;
			dstrain[3] = 0.0;
			dstrain[4] = 0.0;
			dstrain[5] = 0.0;
			break;
		case AnalysisType::TriaxialUndrained:
			dstrain[0] = de11;
			dstrain[1] = -0.5 * de11;
			dstrain[2] = -0.5 * de11;
			dstrain[3] = 0.0;
			dstrain[4] = 0.0;
			dstrain[5] = 0.0;
			break;
		case AnalysisType::Consolidation:
			dstrain[0] = de11;
			dstrain[1] = 0.0;
			dstrain[2] = 0.0;
			dstrain[3] = 0.0;
			dstrain[4] = 0.0;
			dstrain[5] = 0.0;
			break;
		case AnalysisType::Specified:
			dstrain[0] = de11;
			dstrain[1] = de22;
			dstrain[2] = de33;
			dstrain[3] = 0.0;
			dstrain[4] = 0.0;
			dstrain[5] = 0.0;
			break;
		default:
			res_file.close();
			return;
		}

		int res = tc.integrate(dstrain);

		out_strain[0] += dstrain[0];
		out_strain[1] += dstrain[1];
		out_strain[2] += dstrain[2];
	}

	output_var(res_file, tc.get_cohesion(), out_strain, tc.get_stress(), tc.get_dstrain_p());
	res_file.close();
}


#include <Eigen/Dense>

using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::Vector3d;
using Eigen::Dynamic;

#ifndef HERMITE_H
#define HERMITE_H

#define HERMITE_DATA_ROWS 7

class Hermite
{
private:
  int N; // number of particles
  double t; // current simulation time
  double dt; // integration timestep
  double eps; // softening

  Matrix<double, HERMITE_DATA_ROWS, Dynamic> particles;
  Matrix<double, 3, Dynamic> x0; // current positions
  Matrix<double, 3, Dynamic> v0; // current velocities
  Matrix<double, 3, Dynamic> xp; // predicted next positions
  Matrix<double, 3, Dynamic> vp; // predicted next velocities
  Matrix<double, 3, Dynamic> x1; // corrected next positions
  Matrix<double, 3, Dynamic> v1; // corrected next velocities
  Matrix<double, 3, Dynamic> a0; // predicted accelerations
  Matrix<double, 3, Dynamic> a1; // corrected accelerations
  Matrix<double, 3, Dynamic> jerk0; // predicted jerks
  Matrix<double, 3, Dynamic> jerk1; // corrected jerks

  VectorXd energy;
  std::string filename;
  bool lean; // if set to true solver will not write position files because they can get quite large
  int blockSize;
  Matrix<double, 3, Dynamic> totalData; // positions of all particles over all time steps

  void computeEnergy(int step);
  void step();
  MatrixXd computeForces(const MatrixXd mass, const MatrixXd pos, const MatrixXd vel);
public:
  Hermite();
  void enableLean();
  void disableLean();
  void setBlockSize(int size);
  void setSoftening(double newEps);
  void integrate(double dt, int numSteps);
  bool readData(const std::string &filename);
  const Matrix<double, 3, Dynamic> &data();
};

#endif // HERMITE_H


#include "additional_functions.h"
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

int ipow(int base, int exp)
{
	int result = 1;
	while (exp)
	{
		if (exp & 1)
		{
			result *= base;
		}
		exp >>= 1;
		base *= base;
	}

	return result;
}

bool stationaritycheck(const double n[], int d)
{
	bool indicator = false;
	Eigen::MatrixXd stat_mat(d, d);
	for (int i = 0; i != d; ++i)
	{
		for (int j = 0; j != d; ++j)
			stat_mat(i, j) = n[i * d + j];
	}
	Eigen::EigenSolver<Eigen::MatrixXd> eigsol;
	eigsol.compute(stat_mat, false);
	Eigen::VectorXd eigsol_real = eigsol.eigenvalues().real();
	for (int i = 0; i != d; ++i)
	{
		if (fabs(eigsol_real(i)) >= 1)
		{
			indicator = true;
		}
	}

	return indicator;
}

bool stationaritycheck(const double x[], const double y[], int d)
{
	bool indicator = false;
	Eigen::MatrixXd stat_mat(d, d);
	for (int i = 0; i != d; ++i)
	{
		for (int j = 0; j != d; ++j)
		{
			stat_mat(i, j) = x[i * d + j] / y[i * d + j];
		}
	}
	Eigen::EigenSolver<Eigen::MatrixXd> eigsol;
	eigsol.compute(stat_mat, false);
	Eigen::VectorXd eigsol_real = eigsol.eigenvalues().real();
	for (int i = 0; i != d; ++i)
	{
		if (fabs(eigsol_real(i)) >= 1)
		{
			indicator = true;
		}
	}

	return indicator;
}

double random_check() {
	double random = rand();
	if (random == 0 || random == RAND_MAX)
	{
		return random_check();
	}
	else
	{
		return random;
	}
}


/**
 * Implementation of Rules to eliminate
 * Design points. Check Risk-Base Allocation.pptx
 * for more details.
 */
#include <iostream>
#include <vector>
#include <set>
#include <iterator>
#include <math.h>
#include <chrono>
#include <exception>
#include <queue>
#include <boost/math/distributions/normal.hpp>
#include <sys/time.h>
#include "ptss_dse.hpp"

/* HI - Test Editing */
using namespace std;
using boost::math::normal;

// a == b ?
bool is_eq(const alloc_t &a, const alloc_t &b) {
    return (a == b);
}

// a > b ?
bool is_gt(const alloc_t &a, const alloc_t &b) {
    bool ret = !(is_eq(a,b));
    int idx = 0;
    for(auto it = a.begin();
        it != a.end();
        it++) {
            if (*it >= b[idx++])
                ret = ret && true;
            else
                ret = ret && false;
    }
    return ret;
}

// a < b ?
bool is_lt(const alloc_t &a, const alloc_t &b) {
    bool ret = !(is_eq(a,b));
    int idx = 0;
    for(auto it = a.begin();
        it != a.end();
        it++) {
            if (*it <= b[idx++])
                ret = ret && true;
            else
                ret = ret && false;
    }
    return ret;
}

// a and b are not comparable
bool is_incomparable(const alloc_t &a, const alloc_t &b) {
    bool ret = (!is_gt(a,b)) && \
               (!is_lt(a,b)) && \
               (!is_eq(a,b));
    return ret;
}

// lexicographic comparison of two allocations ( a < b ? )
bool lex_comp(const alloc_t &a, const alloc_t &b) {
    auto it2 = b.begin();
    for (auto it = a.begin(); it != a.end(); it++, it2++) {
        if (*it2 < *it)
            return false;
    }
    return true;
}

// just for the sample -- print the data sets
std::ostream& operator<<(std::ostream& os, const alloc_t& vi) {
  os << "(|";
  std::copy(vi.begin(), vi.end(), std::ostream_iterator<int>(os, "|"));
  os << ")";
  return os;
}

std::ostream& operator<<(std::ostream& os, const all_alloc_t& vvi) {
  os << "{\n";
  for(auto it = vvi.begin();
      it != vvi.end();
      it++) {
      os << "  " << *it << "\n";
  }
  os << "}";
  return os;
}

// Profiled Data
static double mua  [] = {269.568837,	141.985532, 99.508883, 79.006262, 66.454092, 58.649068, 52.804705, 48.960141, 45.73406, 43.455542, 41.386889, 40.046129, 38.695398, 37.750183, 36.840822, 36.41925, 35.715777, 35.350802, 34.767695, 34.493228, 34.033093, 33.944396, 33.676262, 33.871417, 33.654013, 33.737325, 33.825452, 34.021759, 34.223725, 34.479067, 34.694037, 35.157394};
static double stda [] = {0.676473946, 0.335842225, 0.208458629, 0.319806191, 0.313754681, 0.340024999, 0.340440891, 0.441634464, 0.424892928, 0.45393722, 0.470011702, 0.479161768, 0.496199557, 0.493329504, 0.524060111, 0.523248507, 0.564837145, 0.590188106, 0.584813646, 0.602546264, 0.597015913, 0.600979201, 0.59552246, 0.595876665, 0.587283577, 0.645952011, 0.666749578, 0.732030737, 0.836999403, 0.942281274, 1.046115194, 1.103043517};

double compute_risk(const alloc_t &x) {
    double mu = 0.0, var = 0.0;
    for(auto jt = x.begin();
        jt != x.end();
        jt++) {
                mu  += mua[*jt-1];
                var += (stda[*jt-1])*(stda[*jt-1]);
    }
    /* Create a normal distribution and evaluate the risk */
    double sd = sqrt(var);
    normal dist(mu,sd);

    double risk = 1 - cdf(dist,D);
    return risk;
}

double compute_execution_time(const alloc_t &x) {
    double sum = 0.0;
    for(auto jt = x.begin(); jt != x.end(); jt++) {
        sum += mua[*jt-1];
    }
    return sum;
}

double compute_estimated_util(const alloc_t &x) {
    double util = 0;
    for(auto jt = x.begin();
        jt != x.end();
        jt++) {
            util += (*jt) * mua[*jt-1];
    }
    return util;
}

/* recursively construct and add allocations */
void construct_alloc(all_alloc_t &vvi, int ph) {
    alloc_t vi2;
    static long unsigned int cnt = 0;
    long unsigned int r   = M;
    long unsigned int dec;
    long unsigned int j   = 0;

    if (ph == NPH) {
        //cout << "ph : " << ph << ", invoc : " << cnt << endl;
        dec = cnt++;

        /* Resolve cnt into M-radix number */
        for (j = 0; j < NPH; j++) {
            //cout << dec%r + 1 << ",";
            vi2.push_back(dec%r + 1);
            dec = dec/r;
        }
        vvi.insert(vi2);
        //cout << "}\n";
        return;
    }
    for (int m = 1; m <= M; m++) {
        construct_alloc(vvi,ph+1);
    }
}

void ptss_DSE::display() {
    cout << this->search_space << "\n";
    cout << "Lower : ";
    cout << this->lower << "\n";
    cout << "Upper : ";
    cout << this->upper << "\n";
}

// Create All points
ptss_DSE::ptss_DSE() {
    construct_alloc(this->search_space,0);
    this->is_initialized = true;
    this->dmr = 1.0;
}

ptss_DSE::ptss_DSE(double dmr) {
    construct_alloc(this->search_space,0);
    this->init_point(dmr);
    this->is_initialized = true;
    this->dmr = dmr;
}

// Evaluate all points
void ptss_DSE::evaluate_all() {
    this->opt_point = this->lower;
    this->opt_util  = 10e9;
    this->opt_risk  = 1;

    // cout << "Search space size " << search_space.size();
    for(auto it = search_space.begin();
        it != search_space.end();
        it++) {

        double risk = compute_risk(*it);
        double util = compute_estimated_util(*it);

        if (risk <= this->dmr && util <= this->opt_util) {
            this->opt_point = *it;
            this->opt_util  = util;
            this->opt_risk  = risk;
        }
        // cout << *it << "," << risk << "," << util << "\n";
        usleep(50);
    }
    cout << this->opt_point << "," << this->opt_risk << "," << this->opt_util << "\n";
}

void ptss_DSE::init_point(double dmr) {
    alloc_t tmp;
    double risk;
    int m;

    for (m = 1; m <= M; m++) {
        tmp.clear();

        // Create a uniform allocation
        for (int ph = 0; ph < NPH; ph++)
            tmp.push_back(m);

        // Compute the risk
        risk = compute_risk(tmp);

        if (risk <= dmr) {
            this->upper = tmp;
            break;
        }
    }

    tmp.clear();
    for (int ph = 0; ph < NPH; ph++)
        tmp.push_back(m-1);
    this->lower = tmp;

    cout << "lower" << this->lower << ", dmr = " << compute_risk(this->lower) << endl;
    cout << "upper" << this->upper << ", dmr = " << compute_risk(this->upper) << endl;
}

/*
 * The action to generate a
 * set of all (child) points is to
 * select a src and dst from
 * startpoint and
 * transfer a single core
 * from one src to destitation.
 */
void epsilon_move2_rule1(all_alloc_t &children,\
                         const alloc_t startpoint,\
                         const all_alloc_t fbidden) {
    /* Expand all the elements of the frontier */
    for (int idx = 0; idx < NPH; idx++) {
        alloc_t tmp2  = startpoint;
        if (tmp2[idx] > 1) {
            tmp2[idx]--;

            /* Inert only when it doesn't exist in forbidden set */
            // if (fbidden.find(tmp2) == fbidden.end())
                children.insert(tmp2);
        }
    }
}

void epsilon_move2_rule2(all_alloc_t &children,\
                         const alloc_t startpoint,\
                         const all_alloc_t fbidden) {
     /* Expand all the elements of the frontier */
    for (int idx = 0; idx < NPH; idx++) {
        alloc_t tmp2  = startpoint;
        if (tmp2[idx] < M) {
            tmp2[idx]++;

            /* Inert only when it doesn't exist in forbidden set */
            // if (fbidden.find(tmp2) == fbidden.end())
                children.insert(tmp2);
        }
    }
}

void epsilon_move2_rule3(all_alloc_t &children,\
                         const alloc_t startpoint,\
                         const all_alloc_t fbidden) {
    int i, j;

    for (i = 0; i < NPH; i++) {
        for (j = 0; j < NPH; j++) {
            if (i != j) {
                if (startpoint[i] > 1) {
                    /* Transfer a core from i to j */
                    alloc_t new_point = startpoint;
                    new_point[i]--;
                    new_point[j]++;

                    /* Check Execution Time before insert */
                    if (compute_execution_time(new_point) > compute_execution_time(startpoint)) {
                        // if (fbidden.find(new_point) == fbidden.end())
                            children.insert(new_point);

                        /* Also insert other points derived from it */
                        alloc_t new_point2 = new_point;
                        while (--new_point2[i] > 0) {
                            if (++new_point2[j] <= NPH)
                                // if (fbidden.find(new_point2) == fbidden.end())
                                    children.insert(new_point2);
                        }
                    }
                }
            }
        }
    }
}

void epsilon_move2_test() {
    alloc_t startpoint = {5,5,6,4,4};
    all_alloc_t fbidden;
    all_alloc_t children = {};
    epsilon_move2_rule3(children, startpoint,fbidden);
    cout << "Start Point" << startpoint << endl;
    cout << "Rule-34 children" << children << endl;
    // cout << "Number of children " << children.size() << "\n\n";
    // cout << "Valid Actions " << valid_actions << "\n\n";
}

/* Comparison based elimination */
// void ptss_DSE::eliminate_points_rule12() {
//     // if (!this->is_initialized) {
//     //     throw domain_error("Object not initialized correctly");
//     // }
//     // /* Apply recursively */
//     // all_alloc_t children;
//     // set<alloc_t> fset1 = {this->upper};
//     // set<alloc_t> fset2 = {this->upper};
//     // set<alloc_t> aset;

//     // expand_rule1(children,fset1,aset,0);
//     // aset.clear();
//     // expand_rule2(children,fset2,aset,0);
//     // cout << children << endl;

//     // cout << "Eliminated " << children.size() << endl;
// }

/*
 * Whenever the DMR constraint is violated
 * by a point say "init",
 * Rule1 and Rule3 will (recursively) be expanded
 * to create, sibling solutions which are guaranteed
 * to violate the DMR (Check the slides and the [PAPER])
 *
 * fbidden : A set that holds all the discarded points.
 * aset    : Children of "init" point expanded according to rule 1 and rule 3
 * aset2   : An auxiliary aset.
 *
 */
void apply_action_rule13(all_alloc_t &fbidden,\
                         all_alloc_t &fset,\
                         all_alloc_t &aset,\
                         int actv_id) {
    bool no_child = true;
    int i, j;

    /* Expand the children and create the new frontier */
    for (set<alloc_t>::iterator it = fset.begin(); it != fset.end(); it++) {

        // cout << "Inserting to fbidden set (activ-"<<actv_id<<"): " << *it << endl;
        // fbidden.insert(*it); // Insert all the elements of frontier set into fbidden set.

        /* Expand all the elements of the frontier */
        // epsilon_move2_rule1(aset,*it,fbidden);
        // epsilon_move2_rule3(aset,*it,fbidden);

        alloc_t tmp = *it;
        fbidden.insert(tmp);

        /* Rule 1 Expansion */
        for (int idx = 0; idx < NPH; idx++) {
            alloc_t tmp2  = tmp;
            if (tmp2[idx] > 1) {
                tmp2[idx]--;
                if (fbidden.count(tmp2) < 1)
                    aset.insert(tmp2);
            }
        }

        /* Rule 3 Expansion */
        for (i = 0; i < NPH; i++) {
            for (j = 0; j < NPH; j++) {
                // cout << "Expr : ("<<i<<","<<j<<") " << ((i != j) && (tmp[i] > 1) && (tmp[j] < M));
                if ((i != j) && (tmp[i] > 1) && (tmp[j] < M)) {
                    /* Transfer a core from i to j */
                    alloc_t new_point = tmp;
                    new_point[i]--;
                    new_point[j]++;
                    /* Check Execution Time before insert */
                    if (compute_execution_time(new_point) > compute_execution_time(tmp)) {
                        if (fbidden.count(new_point) < 1)
                            aset.insert(new_point);
                        /* Also insert other points derived from it */
                        alloc_t new_point2 = new_point;
                        while ((new_point2[i] > 1) && (new_point2[j] < M)) {
                                new_point2[i]--;
                                new_point2[j]++;
                                if (fbidden.count(new_point) < 1)
                                    aset.insert(new_point2);
                        }
                    }
                    // cout << "Blah : "<< "i = "<< i << ",j = " << j << ", tmp[i] = " << tmp[i] << ", tmp[j] = " << tmp[j] << endl;
                } else {
                    // cout << "i = "<< i << ",j = " << j << ", tmp[i] = " << tmp[i] << ", tmp[j] = " << tmp[j] << endl;
                }
            }
        }
    }
    fset.clear();
    no_child = no_child && (aset.empty()?true:false);

    if (!no_child) {
        apply_action_rule13(fbidden,aset,fset,++actv_id);
    }

}

void ptss_DSE::explore() {
    all_alloc_t tmp1 = {this->lower}, tmp2;
    apply_action_rule13(this->discarded_space,tmp1,tmp2,0);
    cout << "Search points discarded : "<<this->discarded_space.size()<<endl;
}
/********************************************************************/
int main () {

    struct timeval t1, t2;
    gettimeofday(&t1,NULL);
    ptss_DSE obj(0.25);
    // cout << obj;
    // obj.display();
    // obj.explore();
    obj.evaluate_all();
    gettimeofday(&t2,NULL);
    double elapsed  = (t2.tv_sec-t1.tv_sec)*1000000+(t2.tv_usec-t1.tv_usec);

    cout << elapsed << "us\n";

    // alloc_t a = {1,2,3,4,5};
    // alloc_t b = {1,1,3,4,2};
    // cout << is_lt(b,a) << "\n";
}


#include <iostream>
#include <cmath>
#include <boost/random.hpp>

double function(double argument)
{
   return sin(argument);
}

int main(int argc, char** argv)
{
   // random number setup
   //
   typedef boost::mt19937 base_generator_type;

   base_generator_type generator(42u);
   boost::uniform_real<> uniform_distribution(0,1);
   boost::variate_generator<base_generator_type&, boost::uniform_real<> >
      random_uniform(generator, uniform_distribution);

   // integration now from 0 to 1
   //
   long number_of_samples = 1000;

   double sum(0);

   for (long i(0); i < number_of_samples; ++i)
   {
      double value = function(random_uniform());
      sum += value;
   }

   std::cout << "the approximate result is: " << sum / number_of_samples << std::endl;

   return 0;
}


/**
 * Copyright (c) 2015 Carnegie Mellon University, Daniel Maturana <dimatura@cmu.edu>
 *
 * For License information please see the LICENSE file in the root directory.
 *
 */

#ifndef FIXEDGRID2_HPP_UYCWT1KR
#define FIXEDGRID2_HPP_UYCWT1KR

#include <stdint.h>
#include <math.h>

#include <vector>
#include <algorithm>

#include <boost/shared_ptr.hpp>

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

#include <ros/console.h>

#include <geom_cast/geom_cast.hpp>
#include <pcl_util/point_types.hpp>

#include "scrollgrid/grid_types.hpp"
#include "scrollgrid/box.hpp"

namespace ca
{

template<class Scalar>
class FixedGrid2 {
public:
  typedef Scalar ScalarType;
  typedef Eigen::Matrix<Scalar, 2, 1> Vec2;

  typedef boost::shared_ptr<FixedGrid2> Ptr;
  typedef boost::shared_ptr<const FixedGrid2> ConstPtr;

public:
  FixedGrid2() :
      box_(),
      origin_(0, 0),
      dimension_(0, 0),
      num_cells_(0),
      strides_(0, 0),
      resolution_(0)
  { }

  FixedGrid2(const Vec2& center,
             const Vec2Ix& dimension,
             Scalar resolution) :
      box_(),
      origin_(center-box_.radius()),
      dimension_(dimension),
      num_cells_(dimension.prod()),
      strides_(dimension[1], 1),
      resolution_(resolution)
  {
    Vec2 min_pt = center-(dimension.cast<Scalar>()*resolution)/2;
    Vec2 max_pt = center+(dimension.cast<Scalar>()*resolution)/2;
    Vec2 radius = (max_pt-min_pt)/2;

    box_.set_center(center);
    box_.set_radius(radius);
    box_.set_max_pt(max_pt);
    box_.set_min_pt(min_pt);
    origin_ = center-box_.radius();
  }

  FixedGrid2(const FixedGrid2& other) :
      box_(other.box_),
      origin_(other.origin_),
      dimension_(other.dimension_),
      num_cells_(other.num_cells_),
      strides_(other.strides_),
      resolution_(other.resolution_)
  {
  }

  FixedGrid2& operator=(const FixedGrid2& other) {
    if (*this==other) { return *this; }
    box_ = other.box_;
    origin_ = other.origin_;
    dimension_ = other.dimension_;
    num_cells_ = other.num_cells_;
    strides_ = other.strides_;
    resolution_ = other.resolution_;
    return *this;
  }

  virtual ~FixedGrid2() { }

public:
  void reset(const Vec2& center,
             const Vec2Ix& dimension,
             Scalar resolution) {
    box_.set_center(center);
    box_.set_radius((dimension.cast<Scalar>()*resolution)/2);
    origin_ = center - box_.radius();
    dimension_ = dimension;
    num_cells_ = dimension.prod();
    strides_ = Vec2Ix(dimension[1], 1);
    resolution_ = resolution;
  }

  void copy_from(ca::FixedGrid2<Scalar>& other) {
    this->reset(other.center(),
                other.dimension(),
                other.resolution());
  }

  /**
   * Is inside 3D box containing grid?
   * pt is in same frame as center (probably world_view)
   */
  bool is_inside_box(Scalar x, Scalar y) const {
    return box_.contains(Vec2(x, y));
  }

  bool is_inside_box(const Vec2& pt) const {
    return box_.contains(pt);
  }

  template<class PointT>
  bool is_inside_box(const PointT& pt) const {
    return box_.contains(ca::point_cast<Eigen::Vector2d>(pt));
  }

  /**
   * is i, j inside the grid limits?
   */
  bool is_inside_grid(const Vec2Ix& grid_ix) const {
    return ((grid_ix.array() >= 0).all() &&
            (grid_ix.array() < dimension_.array()).all());
  }

  bool is_inside_grid(grid_ix_t i, grid_ix_t j) const {
    return this->is_inside_grid(Vec2Ix(i, j));
  }

  /**
   * Given position in world coordinates, return grid coordinates.
   * Note: does not check if point is inside grid.
   */
  Vec2Ix world_to_grid(const Vec2& xy) const {
    Vec2 tmp = ((xy - origin_).array() - 0.5*resolution_)/resolution_;
    //return tmp.cast<grid_ix_t>();
    return Vec2Ix(round(tmp.x()), round(tmp.y()));
  }

  Vec2Ix world_to_grid(Scalar x, Scalar y) const {
    return this->world_to_grid(Vec2(x, y));
  }

  Vec2 grid_to_world(const Vec2Ix& grid_ix) const {
    Vec2 w((grid_ix.cast<Scalar>()*resolution_ + origin_).array() + 0.5*resolution_);
    return w;
  }

  Vec2 grid_to_world(grid_ix_t i, grid_ix_t j) const {
    return this->grid_to_world(Vec2Ix(i, j));
  }

  mem_ix_t grid_to_mem(grid_ix_t i, grid_ix_t j) const {
    return this->grid_to_mem(Vec2Ix(i, j));
  }

  /**
   * Note that this wraps the z dimension.
   * TODO c-order/f-order config
   */
  mem_ix_t grid_to_mem(const Vec2Ix& grid_ix) const {
    return strides_.dot(grid_ix);
  }

  Vec2Ix mem_to_grid(mem_ix_t mem_ix) const {
    grid_ix_t i = mem_ix/strides_[0];
    mem_ix -= i*strides_[0];
    grid_ix_t j = mem_ix;
    mem_ix -= j;
    return Vec2Ix(i, j);
  }

 public:
  grid_ix_t dim_i() const { return dimension_[0]; }
  grid_ix_t dim_j() const { return dimension_[1]; }
  const Vec2Ix& dimension() const { return dimension_; }
  const Vec2& radius() const { return box_.radius(); }
  const Vec2& origin() const { return origin_; }
  Vec2 min_pt() const { return box_.min_pt(); }
  Vec2 max_pt() const { return box_.max_pt(); }
  const Vec2& center() const { return box_.center(); }
  Scalar resolution() const { return resolution_; }

  grid_ix_t num_cells() const { return num_cells_; }

 private:
  // 3d box enclosing grid. In whatever coordinates were given (probably
  // world_view)
  ca::scrollgrid::Box<Scalar, 2> box_;

  // static origin of the grid coordinate system.
  Vec2 origin_;

  // number of grid cells along each axis
  Vec2Ix dimension_;

  // number of cells
  grid_ix_t num_cells_;

  // grid strides to translate from linear to 3D layout.
  // C-ordering, ie x slowest, z fastest.
  Vec2Ix strides_;

  // size of grid cells
  Scalar resolution_;
};

}

#endif /* end of include guard: FIXEDGRID2_HPP_UYCWT1KR */


/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2) ICP algorithms in C++
 * Copyright (C) 2018-2019 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
/**
 * @file   optimal_tf_olae.cpp
 * @brief  OLAE algorithm to find the SE(3) optimal transformation
 * @author Jose Luis Blanco Claraco
 * @date   Jun 16, 2019
 */

#include <mp2p_icp/optimal_tf_olae.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/tfest/se3.h>
#include <Eigen/Dense>
#include "visit_correspondences.h"

using namespace mp2p_icp;

// Convert to quaternion by normalizing q=[1, optim_rot], then to rot. matrix:
static mrpt::poses::CPose3D gibbs2pose(const Eigen::Vector3d& v)
{
    auto       x = v[0], y = v[1], z = v[2];
    const auto r = 1.0 / std::sqrt(1.0 + x * x + y * y + z * z);
    x *= r;
    y *= r;
    z *= r;
    auto q = mrpt::math::CQuaternionDouble(r, -x, -y, -z);

    // Quaternion to 3x3 rot matrix:
    return mrpt::poses::CPose3D(q, .0, .0, .0);
}

/** The systems built by olae_build_linear_system.
 * The system is: "M g = v".
 *
 * However, if the solution is near the Gibbs vector singularity (|Phi|~= \pi)
 * we may need to use the alternative systems built by the
 * "sequential rotation method" [shuster1981attitude].
 *
 * (Refer to technical report for details)
 */
struct OLAE_LinearSystems
{
    Eigen::Matrix3d M, Mx, My, Mz;
    Eigen::Vector3d v, vx, vy, vz;

    /** Attitude profile matrix */
    Eigen::Matrix3d B;
};

/** Core of the OLAE algorithm  */
static OLAE_LinearSystems olae_build_linear_system(
    const WeightedPairings& in, const mrpt::math::TPoint3D& ct_other,
    const mrpt::math::TPoint3D& ct_this, OutlierIndices& in_out_outliers)
{
    MRPT_START

    using mrpt::math::TPoint3D;
    using mrpt::math::TVector3D;

    OLAE_LinearSystems res;

    // Build the linear system: M g = v
    res.M = Eigen::Matrix3d::Zero();
    res.v = Eigen::Vector3d::Zero();

    // Attitude profile matrix:
    res.B = Eigen::Matrix3d::Zero();

    // Lambda: process each pairing:
    auto lambda_each_pair = [&](const mrpt::math::TVector3D& bi,
                                const mrpt::math::TVector3D& ri,
                                const double                 wi) {
// We will evaluate M from an alternative expression below from the
// attitude profile matrix B instead, since it seems to be slightly more
// stable, numerically. The original code for M is left here for
// reference, though.
#if 0
    // M+=(1/2)* ([s_i]_{x})^2
    // with: s_i = b_i + r_i
    const double sx = bi.x + ri.x, sy = bi.y + ri.y, sz = bi.z + ri.z;

    /* ([s_i]_{x})^2 is:
     *
     *  ⎡    2     2                          ⎤
     *  ⎢- sy  - sz      sx⋅sy        sx⋅sz   ⎥
     *  ⎢                                     ⎥
     *  ⎢                 2     2             ⎥
     *  ⎢   sx⋅sy     - sx  - sz      sy⋅sz   ⎥
     *  ⎢                                     ⎥
     *  ⎢                              2     2⎥
     *  ⎣   sx⋅sz        sy⋅sz     - sx  - sy ⎦
     */
    const double c00 = -sy * sy - sz * sz;
    const double c11 = -sx * sx - sz * sz;
    const double c22 = -sx * sx - sy * sy;
    const double c01 = sx * sy;
    const double c02 = sx * sz;
    const double c12 = sy * sz;

    // clang-format off
    const auto dM = (Eigen::Matrix3d() <<
       c00, c01, c02,
       c01, c11, c12,
       c02, c12, c22 ).finished();
    // clang-format on

    // res.M += wi * dM;

    // The missing (1/2) from the formulas above:
    res.M *= 0.5;
#endif
        /* v-= weight *  [b_i]_{x}  r_i
         *  Each term is:
         *  ⎡by⋅rz - bz⋅ry ⎤   ⎡ B23 - B32 ⎤
         *  ⎢              ⎥   |           ⎥
         *  ⎢-bx⋅rz + bz⋅rx⎥ = | B31 - B13 ⎥
         *  ⎢              ⎥   |           ⎥
         *  ⎣bx⋅ry - by⋅rx ⎦   ⎣ B12 - B21 ⎦
         *
         * B (attitude profile matrix):
         *
         * B+= weight * (b_i * r_i')
         *
         */

        // clang-format off
    const auto dV = (Eigen::Vector3d() <<
       (bi.y * ri.z - bi.z * ri.y),
       (-bi.x * ri.z + bi.z * ri.x),
       (bi.x * ri.y - bi.y * ri.x) ).finished();
        // clang-format on

        res.v -= wi * dV;

        // clang-format off
    const auto dB = (Eigen::Matrix3d() <<
       bi.x * ri.x, bi.x * ri.y, bi.x * ri.z,
       bi.y * ri.x, bi.y * ri.y, bi.y * ri.z,
       bi.z * ri.x, bi.z * ri.y, bi.z * ri.z).finished();
        // clang-format on
        res.B += wi * dB;
    };  // end lambda for visit_correspondences()

    // Lambda for the final stage after visiting all corres:
    auto lambda_final = [&](const double w_sum) {
        // Normalize weights. OLAE assumes \sum(w_i) = 1.0
        if (w_sum > .0)
        {
            const auto f = (1.0 / w_sum);
            // res.M *= f;
            res.v *= f;
            res.B *= f;
        }
        else
        {
            // We either had NO input correspondences, or ALL were detected
            // as outliers... What to do in this case?
        }
    };

    visit_correspondences(
        in, ct_other, ct_this, in_out_outliers, lambda_each_pair, lambda_final,
        true /* DO make unit point vectors for OLAE */);

    // Now, compute the other three sets of linear systems, corresponding
    // to the "sequential rotation method" [shuster1981attitude], so we can
    // later keep the best one (i.e. the one with the largest |M|).
    {
        const Eigen::Matrix3d S = res.B + res.B.transpose();
        const double          p = res.B.trace() + 1;
        const double          m = res.B.trace() - 1;
        // Short cut:
        const auto& v = res.v;

        // Set #0: M g=v, without further rotations (the system built above).
        // clang-format off
        res.M = (Eigen::Matrix3d() <<
           S(0,0)-p,  S(0,1), S(0,2),
           S(0,1),   S(1,1)-p, S(1,2),
           S(0,2),   S(1,2),  S(2,2)-p ).finished();
        // clang-format on

        const auto&  M0 = res.M;  // shortcut
        const double z1 = v[0], z2 = v[1], z3 = v[2];

        // Set #1: rotating 180 deg around "x":
        // clang-format off
        res.Mx = (Eigen::Matrix3d() <<
           m     ,      -z3  ,     z2,
           -z3   ,  M0(2,2),     -S(1,2),
           z2    ,  -S(1,2),    M0(1,1)).finished();
        res.vx = (Eigen::Vector3d() <<
            -z1, S(0,2), -S(0,1)
            ).finished();
        // clang-format on

        // Set #2: rotating 180 deg around "y":
        // clang-format off
        res.My = (Eigen::Matrix3d() <<
           M0(2,2),     z3  ,     -S(0,2),
           z3     ,       m ,     -z1,
         -S(0,2)  ,     -z1 ,   M0(0,0)).finished();
        res.vy = (Eigen::Vector3d() <<
            -S(1,2), -z2, S(0,1)
            ).finished();
        // clang-format on

        // Set #3: rotating 180 deg around "z":
        // clang-format off
        res.Mz = (Eigen::Matrix3d() <<
           M0(1,1),  -S(0,1),     -z2,
          -S(0,1) ,  M0(0,0),      z1,
             -z2  ,      z1 ,      m).finished();
        res.vz = (Eigen::Vector3d() <<
            S(1,2), -S(0,2), -z3
            ).finished();
        // clang-format on
    }

    return res;

    MRPT_END
}

// See .h docs, and associated technical report.
void mp2p_icp::optimal_tf_olae(const WeightedPairings& in, OptimalTF_Result& result)
{
    MRPT_START

    using mrpt::math::TPoint3D;
    using mrpt::math::TVector3D;

    // Note on notation: we are search the relative transformation of
    // the "other" frame wrt to "this", i.e. "this"="global",
    // "other"="local":
    //   p_this = pose \oplus p_other
    //   p_A    = pose \oplus p_B      --> pB = p_A \ominus pose

    // Reset output to defaults:
    result = OptimalTF_Result();

    // Normalize weights for each feature type and for each target (attitude
    // / translation):
    ASSERT_(in.attitude_weights.pt2pt >= .0);
    ASSERT_(in.attitude_weights.l2l >= .0);
    ASSERT_(in.attitude_weights.pl2pl >= .0);

    // Compute the centroids:
    auto [ct_other, ct_this] =
        eval_centroids_robust(in, result.outliers /* empty for now  */);

    // Build the linear system: M g = v
    OLAE_LinearSystems linsys = olae_build_linear_system(
        in, ct_other, ct_this, result.outliers /* empty for now  */);

    MRPT_TODO("Refactor to avoid duplicated code? Is it possible?");

    // Re-evaluate the centroids, now that we have a guess on outliers.
    if (!result.outliers.empty())
    {
        // Re-evaluate the centroids:
        const auto [new_ct_other, new_ct_this] =
            eval_centroids_robust(in, result.outliers);

        ct_other = new_ct_other;
        ct_this  = new_ct_this;

        // And rebuild the linear system with the new values:
        linsys =
            olae_build_linear_system(in, ct_other, ct_this, result.outliers);
    }

    // We are finding the optimal rotation "g", as a Gibbs vector.
    // Solve linear system for optimal rotation: M g = v

    const double detM_orig = std::abs(linsys.M.determinant()),
                 detMx     = std::abs(linsys.Mx.determinant()),
                 detMy     = std::abs(linsys.My.determinant()),
                 detMz     = std::abs(linsys.Mz.determinant());

#if 0
    // clang-format off
    std::cout << " |M_orig|= " << detM_orig << "\n"
                 " |M_x|   = " << detMx << "\n"
                 " |M_t|   = " << detMy << "\n"
                 " |M_z|   = " << detMz << "\n";
    // clang-format on
#endif

    if (detM_orig > mrpt::max3(detMx, detMy, detMz))
    {
        // original rotation is the best numerically-determined problem:
        const auto sol0 =
            gibbs2pose(linsys.M.colPivHouseholderQr().solve(linsys.v));
        result.optimal_pose = sol0;
#if 0
        std::cout << "M   : |M|="
                  << mrpt::format("%16.07f", linsys.M.determinant())
                  << " sol: " << sol0.asString() << "\n";
#endif
    }
    else if (detMx > mrpt::max3(detM_orig, detMy, detMz))
    {
        // rotation wrt X is the best choice:
        auto sol1 =
            gibbs2pose(linsys.Mx.colPivHouseholderQr().solve(linsys.vx));
        sol1                = mrpt::poses::CPose3D(0, 0, 0, 0, 0, M_PI) + sol1;
        result.optimal_pose = sol1;
#if 0
        std::cout << "M_x : |M|="
                  << mrpt::format("%16.07f", linsys.Mx.determinant())
                  << " sol: " << sol1.asString() << "\n";
#endif
    }
    else if (detMy > mrpt::max3(detM_orig, detMx, detMz))
    {
        // rotation wrt Y is the best choice:
        auto sol2 =
            gibbs2pose(linsys.My.colPivHouseholderQr().solve(linsys.vy));
        sol2                = mrpt::poses::CPose3D(0, 0, 0, 0, M_PI, 0) + sol2;
        result.optimal_pose = sol2;
#if 0
        std::cout << "M_y : |M|="
                  << mrpt::format("%16.07f", linsys.My.determinant())
                  << " sol: " << sol2.asString() << "\n";
#endif
    }
    else
    {
        // rotation wrt Z is the best choice:
        auto sol3 =
            gibbs2pose(linsys.Mz.colPivHouseholderQr().solve(linsys.vz));
        sol3                = mrpt::poses::CPose3D(0, 0, 0, M_PI, 0, 0) + sol3;
        result.optimal_pose = sol3;
#if 0
        std::cout << "M_z : |M|="
                  << mrpt::format("%16.07f", linsys.Mz.determinant())
                  << " sol: " << sol3.asString() << "\n";
#endif
    }

    // Use centroids to solve for optimal translation:
    mrpt::math::TPoint3D pp;
    result.optimal_pose.composePoint(
        ct_other.x, ct_other.y, ct_other.z, pp.x, pp.y, pp.z);
    // Scale, if used, was: pp *= s;

    result.optimal_pose.x(ct_this.x - pp.x);
    result.optimal_pose.y(ct_this.y - pp.y);
    result.optimal_pose.z(ct_this.z - pp.z);

    MRPT_END
}


#include <voltu/types/intrinsics.hpp>
#include <Eigen/Eigen>

Eigen::Matrix3d voltu::Intrinsics::matrix() {
	Eigen::Matrix3d K;
	K <<		focal_x,			0.0,	-principle_x,
					0.0,		focal_y,	-principle_y,
					0.0,			0.0,			 1.0;
	return K;
}

Eigen::Vector2i voltu::Intrinsics::size() {
	return { width, height };
}


// http://arma.sourceforge.net/docs.html#example_prog

#include <iostream>
#include <armadillo>
#include <gnuplot-iostream.h>

using namespace std;
using namespace arma;

int main()
{
    arma_rng::set_seed_random();

    mat A = randu<mat>(4, 5);
    mat B = randu<mat>(4, 5);
    mat C = A * B.t();

    cout << C << endl;

    Gnuplot gp;
    gp << "plot " << gp.file1d(vec{C.col(1)}) << "with lines" << endl;

    getchar();

    return 0;
}


/**
 * @file
 *
 * @copyright
 * SPDX-License-Identifier: Apache-2.0
 *
 * @test @b eigen_gemm_double14
 * @parblock
 * This piece of code aims to stress test the exec (in particular FMA)
 * by repetitively solving general matrix matrix multiplication.  The
 * multiplication function is from the 3rd party library Eigen.  A
 * pair of random matrices are generated as inputs and then multiplied
 * using Eigen's gemm. The multiplication result is compared against a
 * golden result that is computed during init.  This particular
 * version of the test goes for double precision input matrices. This
 * variation adds extra copies and consistency checks.
 *
 * @note Although the test should run fine on a single thread, it is
 * only expected to catch defects if run on at least 2 cores.
 * @endparblock
 */

#include <sandstone.h>

#include <Eigen/Core>
using namespace Eigen;

#define M_DIM 256

typedef Matrix < double, Dynamic, Dynamic > Mat;

namespace {
struct eigen_test_data {
    Mat lhs;
    Mat rhs;
    Mat prod;
};
}

#define CAST(_x) static_cast<struct eigen_test_data *>(_x)

static int eigen_gemm_double14_init(struct test *test) {
    test->data = new(eigen_test_data);
    try {
        CAST(test->data)->lhs = Mat::Random(M_DIM, M_DIM);
        CAST(test->data)->rhs = Mat::Random(M_DIM, M_DIM);
        CAST(test->data)->prod = CAST(test->data)->lhs * CAST(test->data)->rhs;
    } catch (...) {
        report_fail_msg("Exception on Eigen code, most probably OOM");
    }
    return EXIT_SUCCESS;
}

static int eigen_gemm_double14_run(struct test *test, int cpu) {
    do {
        auto testdata = CAST(test->data);
        Mat _x;
        _x = testdata->lhs;
        Mat _y;
        _y = testdata->rhs;
        Mat _prod;
        _prod = _x * _y;

        if (!_x.isApprox(testdata->lhs)) {
                report_fail_msg("_x.isApprox failed");
        }
        memcmp_or_fail(_x.data(), testdata->lhs.data(), M_DIM * M_DIM);

        if (!_y.isApprox(testdata->rhs)) {
                report_fail_msg("_y.isApprox failed");
        }
        memcmp_or_fail(_y.data(), testdata->rhs.data(), M_DIM * M_DIM);

        if (!_prod.isApprox(testdata->prod)) {
                report_fail_msg("_prod.isApprox failed");
        }
        memcmp_or_fail(_prod.data(), testdata->prod.data(), M_DIM * M_DIM);
    } while (test_time_condition(test));
    return EXIT_SUCCESS;
}

static int eigen_gemm_double14_finish(struct test *test) {
    delete(CAST(test->data));
    return EXIT_SUCCESS;
}

DECLARE_TEST(eigen_gemm_double14, "Eigen GEMM payload (double, dynamic, square)")
  .groups = DECLARE_TEST_GROUPS(&group_math),
  .test_init = eigen_gemm_double14_init,
  .test_run = eigen_gemm_double14_run,
  .test_cleanup = eigen_gemm_double14_finish,
  .fracture_loop_count = 4,
  .quality_level = TEST_QUALITY_PROD,
END_DECLARE_TEST


/**
 * @brief Partial Least Squares Regression with Eigen
 *
 * Same source as LDA, \cite friedman2001elements
 *
 * @file pls-eigen.hpp
 * @author François-David Collin <Francois-David.Collin@umontpellier.fr>
 * @brief
 * @version 0.1
 * @date 2018-11-08
 *
 * @copyright Copyright (c) 2018
 *
 */
#pragma once

#include "various.hpp"
#include <Eigen/Dense>
#include <list>
#include <algorithm>
#include <range/v3/all.hpp>
#include "tqdm.hpp"

using namespace Eigen;
using namespace std;
using namespace ranges;

/**
 * @brief filters out constant variables
 *
 * @tparam Derived
 * @param xr table to filter
 * @return std::vector<size_t> indexes of valid vars
 */
template<class Derived>
std::vector<size_t> filterConstantVars(const MatrixBase<Derived>& xr) {
    RowVectorXd meanr = xr.colwise().mean();
    VectorXd stdr = ((xr.rowwise() - meanr).array().square().colwise().sum() / (xr.rows() - 1)).sqrt();;
    std::vector<size_t> validvars(xr.cols());
    size_t m = 0;
    for(size_t i = 0; i< xr.cols(); i++) {
        if (stdr(i) >= 1.0e-8) validvars[m++] = i;
    }
    validvars.resize(m);
    return validvars;
}

/**
 * @brief Apply PLS on x regarding y
 *
 * @tparam Derived
 * @tparam OtherDerived
 * @param x input
 * @param y output
 * @param ncomp number of components expected
 * @param Projection the projection matrix
 * @param mean mean of variables in x
 * @param std standard deviation of variables in x
 * @param stopping elbow heuristic enabled
 * @return VectorXd explained variance for each computed components
 */
template<class Derived, class OtherDerived>
VectorXd pls(const MatrixBase<Derived>& x,
         const MatrixBase<OtherDerived>& y,
         size_t ncomp,
         MatrixXd& Projection,
         RowVectorXd& mean,
         RowVectorXd& std,
         bool stopping = false)
{
    size_t n = x.rows();
    size_t p = x.cols();
    mean = x.colwise().mean();
    ncomp = std::min(std::min(n,p),ncomp);
    std = ((x.rowwise() - mean).array().square().colwise().sum() / (x.rows() - 1)).sqrt();;
    MatrixXd X = (x.rowwise() - mean).array().rowwise() / std.array();
    MatrixXd X0 = X;
    MatrixXd Ptilde(ncomp,p);
    MatrixXd Wstar(p,ncomp);
    VectorXd res(ncomp);
    size_t window_size = std::max(2_z,ncomp/10_z);

    std::list<unsigned char> stopping_criterium(window_size,0);
    double ymean = y.mean();
    MatrixXd w_k, t_k, p_k, y_k;
    y_k = y;
    double SSTO = (y.array() - ymean).array().square().sum();
    int m = 0;
    tqdm bar;

    while (m < ncomp)
    {
        bar.progress(m,ncomp);
        // $w_{k}=\frac{X_{k-1}^{T} y_{k-1}}{\left\|X_{k-1}^{T} y_{k-1}\right\|}$
        w_k = X.transpose() * y;   //  (p)
        w_k /= sqrt((w_k.transpose()*w_k)(0,0));
        // $\mathbf{W}^{*}_{p \times K} = [w_1, \ldots, w_K]$
        Wstar.col(m) = w_k;
        // $t_{k}=X_{k-1}w_{k}$
        t_k = X * w_k; // (n)
        double t_k_s = (t_k.transpose() * t_k)(0,0);
        // $p_{k}=\frac{X_{k-1}^{T} t_{k}}{t_{k}^{T} t_{k}}$
        p_k = (X.transpose() * t_k) / t_k_s; // (p)
        // $\widetilde{\mathbf{P}}_{K \times p}=\mathbf{t}\left[p_{1}, \ldots, p_{K}\right]$
        Ptilde.row(m) = p_k.transpose();
        // $q_{k}=\frac{y_{k-1}^{T} t_{k}}{t_{k}^{T} t_{k}}$
        double q_k = (y_k.transpose() * t_k)(0,0) / t_k_s; //(n,n)
        // $y_{k}=y_{k-1}-q_{k} t_{k}$
        y_k -= q_k * t_k;
        // $X_{k}=X_{k-1}-t_{k} p_{k}^{T}$
        X -= (t_k * p_k.transpose());

        // $$Yvar^m = \frac{\sum_{i=1}^{N}{(\hat{y}^{m}_{i}-\bar{y})^2}}{\sum_{i=1}^{N}{(y_{i}-\hat{y})^2}}$$
        res(m) = 1 - (y_k.array() - ymean).array().square().sum() / SSTO;

        // Elbow heuristic
        // $$Yvar^m = \frac{\sum_{i=1}^{N}{(\hat{y}^{m}_{i}-\bar{y})^2}}{\sum_{i=1}^{N}{(y_{i}-\hat{y})^2}}$$
        if ((m >= 2) && stopping) {
            auto lastdiff = res(m) - res(m-1);
            auto lastmean = (res(m) + res(m-1))/2.0;
            size_t remains = ncomp - m;
            stopping_criterium.pop_front();
            stopping_criterium.push_back(lastmean >= 0.99 * remains * lastdiff);
            auto wcrit = ranges::accumulate(stopping_criterium,0);
            if (wcrit == window_size) break;
        }

        m++;
    }
    if (m < ncomp) {
        m--;
        res = res(seq(0,m)).eval();
    }

    // $\mathbf{W}=\mathbf{W}^{*}\left(\widetilde{\mathbf{P}} \mathbf{W}^{*}\right)^{-1}$
    auto solver = (Ptilde*Wstar).completeOrthogonalDecomposition();
    Projection = Wstar*solver.pseudoInverse();
    return res;
}

// Tenenhaus, M. L’approche PLS. Revue de statistique appliquée 47, 5–40 (1999).
// Vancolen, S. La Régression PLS. Mémoire Postgrade en Statistiques, University of Neuchâtel (Switzerland) 1--28 (2004).
// Wold, S., Sjöström, M. & Eriksson, L. PLS-regression: a basic tool of chemometrics. Chemometrics and intelligent laboratory systems 58, 109–130 (2001).
// Mémoire m2 de ghislain : https://plmbox.math.cnrs.fr/f/1192b14f90ea44a1b26a/
// Krämer, N. An overview on the shrinkage properties of partial least squares regression. Computational Statistics 22, 249–273 (2007).


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testPCGSolver.cpp
 * @brief   Unit tests for PCGSolver class
 * @author  Yong-Dian Jian
 * @date    Aug 06, 2014
 */

#include <tests/smallExample.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/Matrix.h>

#include <CppUnitLite/TestHarness.h>

#include <memory>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;

#include <iostream>
#include <fstream>

using namespace std;
using namespace gtsam;

const double tol = 1e-3;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
// Test cholesky decomposition
TEST( PCGSolver, llt ) {
  Matrix R = (Matrix(3,3) <<
                1., -1., -1.,
                0.,  2., -1.,
                0.,  0.,  1.).finished();
  Matrix AtA = R.transpose() * R;

  Vector Rvector = (Vector(9) << 1., -1., -1.,
                                 0.,  2., -1.,
                                 0.,  0.,  1.).finished();
//  Vector Rvector = (Vector(6) << 1., -1., -1.,
//                                      2., -1.,
//                                           1.).finished();

  Vector b = Vector3(1., 2., 3.);

  Vector x = Vector3(6.5, 2.5, 3.) ;

  /* test cholesky */
  Matrix Rhat = AtA.llt().matrixL().transpose();
  EXPECT(assert_equal(R, Rhat, 1e-5));

  /* test backward substitution */
  Vector xhat = Rhat.triangularView<Eigen::Upper>().solve(b);
  EXPECT(assert_equal(x, xhat, 1e-5));

  /* test in-place back substitution */
  xhat = b;
  Rhat.triangularView<Eigen::Upper>().solveInPlace(xhat);
  EXPECT(assert_equal(x, xhat, 1e-5));

  /* test triangular matrix map */
  Eigen::Map<Eigen::MatrixXd> Radapter(Rvector.data(), 3, 3);
  xhat = Radapter.transpose().triangularView<Eigen::Upper>().solve(b);
  EXPECT(assert_equal(x, xhat, 1e-5));

}

/* ************************************************************************* */
// Test GaussianFactorGraphSystem::multiply and getb
TEST( GaussianFactorGraphSystem, multiply_getb)
{
  // Create a Gaussian Factor Graph
  GaussianFactorGraph simpleGFG;
  SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3));
  simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2);
  simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2);
  simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2);
  simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2);
  simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
  simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
  simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);

  // Create a dummy-preconditioner and a GaussianFactorGraphSystem
  DummyPreconditioner dummyPreconditioner;
  KeyInfo keyInfo(simpleGFG);
  std::map<Key,Vector> lambda;
  dummyPreconditioner.build(simpleGFG, keyInfo, lambda);
  GaussianFactorGraphSystem gfgs(simpleGFG, dummyPreconditioner, keyInfo, lambda);

  // Prepare container for each variable
  Vector initial, residual, preconditionedResidual, p, actualAp;
  initial = (Vector(6) << 0., 0., 0., 0., 0., 0.).finished();

  // Calculate values using GaussianFactorGraphSystem same as inside of PCGSolver
  gfgs.residual(initial, residual);                         /* r = b-Ax */
  gfgs.leftPrecondition(residual, preconditionedResidual);  /* pr = L^{-1} (b-Ax) */
  gfgs.rightPrecondition(preconditionedResidual, p);        /* p = L^{-T} pr */
  gfgs.multiply(p, actualAp);                                     /* A p */

  // Expected value of Ap for the first iteration of this example problem
  Vector expectedAp = (Vector(6) << 100400, -249074.074, -2080, 148148.148, -146480, 37962.963).finished();
  EXPECT(assert_equal(expectedAp, actualAp, 1e-3));

  // Expected value of getb
  Vector expectedb = (Vector(6) << 100.0, -194.444, -20.0, 138.889, -120.0, -55.556).finished();
  Vector actualb;
  gfgs.getb(actualb);
  EXPECT(assert_equal(expectedb, actualb, 1e-3));
}

/* ************************************************************************* */
// Test Dummy Preconditioner
TEST( PCGSolver, dummy )
{
  LevenbergMarquardtParams paramsPCG;
  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
  PCGSolverParameters::shared_ptr pcg = std::make_shared<PCGSolverParameters>();
  pcg->preconditioner_ = std::make_shared<DummyPreconditionerParameters>();
  paramsPCG.iterativeParams = pcg;

  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  Point2 x0(10,10);
  Values c0;
  c0.insert(X(1), x0);

  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();

  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}

/* ************************************************************************* */
// Test Block-Jacobi Precondioner
TEST( PCGSolver, blockjacobi )
{
  LevenbergMarquardtParams paramsPCG;
  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
  PCGSolverParameters::shared_ptr pcg = std::make_shared<PCGSolverParameters>();
  pcg->preconditioner_ = std::make_shared<BlockJacobiPreconditionerParameters>();
  paramsPCG.iterativeParams = pcg;

  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  Point2 x0(10,10);
  Values c0;
  c0.insert(X(1), x0);

  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();

  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}

/* ************************************************************************* */
// Test Incremental Subgraph PCG Solver
TEST( PCGSolver, subgraph )
{
  LevenbergMarquardtParams paramsPCG;
  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
  PCGSolverParameters::shared_ptr pcg = std::make_shared<PCGSolverParameters>();
  pcg->preconditioner_ = std::make_shared<SubgraphPreconditionerParameters>();
  paramsPCG.iterativeParams = pcg;

  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  Point2 x0(10,10);
  Values c0;
  c0.insert(X(1), x0);

  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();

  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}


#ifndef FFT_FEATURES_EMISSION_H
#define FFT_FEATURES_EMISSION_H

#include <armadillo>
#include <emissions.hpp>
#include <iostream>
#include <robotics/utils/random.hpp>
#include <vector>

namespace hsmm {

    class MultivariateGaussianEmission : public
                                         AbstractEmissionObsCondIIDgivenState {
        public:
            MultivariateGaussianEmission(
                    std::vector<robotics::random::NormalDist> states_);

            MultivariateGaussianEmission* clone() const;

            double loglikelihood(int state,
                    const arma::vec &single_obs) const;

            void fitFromLabels(const arma::field<arma::mat> &observations_seq,
                    const arma::field<arma::ivec> &labels_seq);

            // TODO: throw a not implemented exception.
            void reestimate(int min_duration,
                    const arma::field<arma::cube>& meta,
                    const arma::field<arma::field<arma::mat>>& mobs) {}

            arma::field<arma::mat> sampleFromState(int state, int size,
                    std::mt19937 &rng) const;

        private:
            std::vector<robotics::random::NormalDist> states_;
    };

};

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
#include <boost/numeric/itl/pc/ilut.hpp>

template <typename Matrix>
double test_factorization(const Matrix& A, unsigned p, double tau)
{
    itl::pc::ilut<Matrix>  P(A, p, tau);
    //itl::pc::ilu_0<Matrix>  P(A);
    Matrix L(P.get_L()), U(P.get_U()), I(num_rows(A), num_cols(A));
    I= 1.0;
    L+= I;
    invert_diagonal(U);
    //std::cout << "L is\n" << L << '\n';
    //std::cout << "U is\n" << U << '\n';
    Matrix LU(L*U);
    //std::cout << "LU is\n" << LU << '\n';
    LU-= A;
    //std::cout << "LU-A is\n" << LU << '\n';
    double diff_norm= frobenius_norm(LU);
    std::cout << "|A-LU|_1 with p = " << p << ", tau = " << tau << " is " << diff_norm << '\n';
    return diff_norm;
}

int main()
{
#ifndef __PGI
    // For a more realistic example set sz to 1000 or larger
    const unsigned size = 4, N = size * size;

    mtl::compressed2D<double>          A(N, N);
    laplacian_setup(A, size, size);

    std::cout << "A is\n" << A << '\n';
    MTL_THROW_IF(test_factorization(A, 3, 0.001) > 0.24, mtl::logic_error("ILUT(3, 0.001) too bad"));

#if 0
    for(unsigned i= 2; i <= size; i++)
	for (double f= 0.5; f > 0.000001; f/= 2)
	    test_factorization(A, i, f);

    itl::pc::ic_0<matrix_type, float>  P(A);
    mtl::dense_vector<double>          x(N), y, yc(N);
    iota(x);
    yc= 1.03194,1.60198,1.45357,2.5258,3.55386,3.16,2.91787,4.09338,3.81335;

    std::cout << x << '\n';
    y= solve(P, x);
    std::cout << y << '\n';

    MTL_THROW_IF(two_norm(mtl::dense_vector<double>(y - yc)) > 0.001,
		 mtl::logic_error("IC(0) doesn't yield expected result"));
#endif

#endif
    return 0;
}


// Copyright 2019 Xanadu Quantum Technologies Inc.

// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at

//     http://www.apache.org/licenses/LICENSE-2.0

// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/**
 * @file
 * Contains functions for approximating the hafnian of a matrix
 * in a classically efficient manner, for certains classes of matrices.
 */

#pragma once
#include <stdafx.h>
#include <numeric>
#include <random>
#include <cmath>

#ifdef LAPACKE
#define EIGEN_SUPERLU_SUPPORT
#define EIGEN_USE_BLAS
#define EIGEN_USE_LAPACKE

#define LAPACK_COMPLEX_CUSTOM
#define lapack_complex_float std::complex<float>
#define lapack_complex_double std::complex<double>
#endif

#include <Eigen/Eigenvalues>

namespace libwalrus {

/**
* Returns the approximation to the hafnian of a matrix with non-negative entries.
*
* The approximation follows an stochastic algorithm according to which the hafnian
* can be approximated as the sum of determinants of matrices.
* The accuracy of the approximation increases with increasing number of iterations.
*
* @param mat vector representing the flattened matrix
* @param nsamples positive integer representing the number of samples to perform
* @return the approximate hafnian
*/
template <typename T>
inline long double hafnian_nonneg(std::vector<T> &mat, int &nsamples) {
    int n = std::sqrt(static_cast<double>(mat.size()));

    long double mean = 0;
    long double stdev = 1;

    namespace eg = Eigen;
    eg::Matrix<T, eg::Dynamic, eg::Dynamic> A = eg::Map<eg::Matrix<T, eg::Dynamic, eg::Dynamic>, eg::Unaligned>(mat.data(), n, n);

#ifdef _OPENMP
    int nthreads = omp_get_max_threads();
    omp_set_num_threads(nthreads);
#else
    int nthreads = 1;
#endif

    std::vector<int> threadbound_low(nthreads);
    std::vector<int> threadbound_hi(nthreads);

    std::default_random_engine generator;
    std::normal_distribution<double> distribution(mean, stdev);

    std::vector<long double> determinants(nsamples);

    #pragma omp parallel for shared(determinants)
    for (int k = 0; k < nsamples; k++) {
        std::vector<T> matrand(n * n, 0);
        std::vector<T> g(n * n, 0);
        std::vector<T> gt(n * n, 0);
        eg::Matrix<T, eg::Dynamic, eg::Dynamic> W;
        W.resize(n, n);

        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                long double randnum = distribution(generator);
                matrand[i * n + j] = static_cast<T>(randnum);
                g[i * n + j] = 0.0;
                gt[i * n + j] = 0.0;
            }
        }

        for (int i = 0; i < n; i++) {
            for (int j = i; j < n; j++) {
                g[i * n + j] = matrand[i * n + j];
                gt[j * n + i] = matrand[i * n + j];
            }
        }

        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                int id = i * n + j;
                W(i, j) = (g[id] - gt[id]) * std::sqrt(std::abs(A(i, j)));
            }
        }

        long double det = std::real(W.determinant());

        determinants[k] = det;
    }

    long double final = 0.0;

    for (int i = 0; i < nsamples; i++) {
        final += determinants[i];
    }

    final = final / (static_cast<long double>(nsamples));

    return final;

}

/**
* Returns the approximation to the hafnian of a matrix with non-negative entries.
*
* The approximation follows an stochastic algorithm according to which the hafnian
* can be approximated as the sum of determinants of matrices.
* The accuracy of the approximation increases with increasing number of iterations.
*
* This is a wrapper around the templated function `libwalrus::hafnian_nonneg` for Python
* integration. It accepts and returns double numeric types, and
* returns sensible values for empty and non-even matrices.
*
* In addition, this wrapper function automatically casts all matrices
* to type `long double`, allowing for greater precision than supported
* by Python and NumPy.
*
* @param mat vector representing the flattened matrix
* @param nsamples positive integer representing the number of samples to perform
* @return the approximate hafnian
*/
double hafnian_approx(std::vector<double> &mat, int &nsamples) {
    std::vector<long double> matq(mat.begin(), mat.end());
    int n = std::sqrt(static_cast<double>(mat.size()));
    long double haf;

    if (n == 0)
        haf = 1.0;
    else if (n % 2 != 0)
        haf = 0.0;
    else
        haf = hafnian_nonneg(matq, nsamples);

    return static_cast<double>(haf);
}

}


//  io_ex1.cpp  ----------------------------------------------------------//

//  Copyright 2010 Howard Hinnant
//  Copyright 2010 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*
This code was adapted by Vicente J. Botet Escriba from Hinnant's html documentation.
Many thanks to Howard for making his code available under the Boost license.

*/

#include <boost/chrono/chrono_io.hpp>
#include <ostream>
#include <iostream>

// format duration as [-]d/hh::mm::ss.cc
template <class CharT, class Traits, class Rep, class Period>
std::basic_ostream<CharT, Traits>&
display(std::basic_ostream<CharT, Traits>& os,
        boost::chrono::duration<Rep, Period> d)
{
    using std::cout;
    using namespace boost;
    using namespace boost::chrono;

    typedef duration<long long, ratio<86400> > days;
    typedef duration<long long, centi> centiseconds;

    // if negative, print negative sign and negate
    if (d < duration<Rep, Period>(0))
    {
        d = -d;
        os << '-';
    }
    // round d to nearest centiseconds, to even on tie
    centiseconds cs = duration_cast<centiseconds>(d);
    if (d - cs > milliseconds(5)
        || (d - cs == milliseconds(5) && cs.count() & 1))
        ++cs;
    // separate seconds from centiseconds
    seconds s = duration_cast<seconds>(cs);
    cs -= s;
    // separate minutes from seconds
    minutes m = duration_cast<minutes>(s);
    s -= m;
    // separate hours from minutes
    hours h = duration_cast<hours>(m);
    m -= h;
    // separate days from hours
    days dy = duration_cast<days>(h);
    h -= dy;
    // print d/hh:mm:ss.cc
    os << dy.count() << '/';
    if (h < hours(10))
        os << '0';
    os << h.count() << ':';
    if (m < minutes(10))
        os << '0';
    os << m.count() << ':';
    if (s < seconds(10))
        os << '0';
    os << s.count() << '.';
    if (cs < centiseconds(10))
        os << '0';
    os << cs.count();
    return os;
}

int main()
{
    using std::cout;
    using namespace boost;
    using namespace boost::chrono;

#ifdef BOOST_CHRONO_HAS_CLOCK_STEADY
    display(cout, steady_clock::now().time_since_epoch()
                  + duration<long, mega>(1)) << '\n';
#endif
    display(cout, -milliseconds(6)) << '\n';
    display(cout, duration<long, mega>(1)) << '\n';
    display(cout, -duration<long, mega>(1)) << '\n';
}

//~ 12/06:03:22.95
//~ -0/00:00:00.01
//~ 11/13:46:40.00
//~ -11/13:46:40.00


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_EXPONENT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_EXPONENT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns the exponent of the floating input.

    @par Header <boost/simd/function/exponent.hpp>

    @par Note:

     The exponent \f$e\f$ and mantissa \f$m\f$ of a floating point entry \f$x\f$ are related by
    \f$x =  m\times 2^e\f$, with  \f$ |m| \in [1, 2[\f$ (except for \f$x = 0\f$,
    where \f$m=0\f$ and \f$e=0\f$ ).

    For integral type inputs exponent is always 0 and mantissa reduces to identity.

    @see mantissa,  frexp, ldexp

    @par Example:

      @snippet exponent.cpp exponent

    @par Possible output:

      @snippet exponent.txt exponent

  **/
  as_integer_t<Value> exponent(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/exponent.hpp>
#include <boost/simd/function/simd/exponent.hpp>

#endif


// Copyright (c) 2013, Manuel Blum
// All rights reserved.

// Define this symbol to enable runtime tests for allocations
//#define EIGEN_RUNTIME_NO_MALLOC

#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <cstdio>
#include <string>

#include "nn.h"

inline void swap(int &val)
{
	val = (val<<24) | ((val<<8) & 0x00ff0000) | ((val>>8) & 0x0000ff00) | (val>>24);
}

matrix_t read_mnist_images(std::string filename)
{
  matrix_t X;
  std::ifstream fs(filename.c_str(), std::ios::binary);
  if(fs) {
    int magic_number, num_images, num_rows, num_columns;
    fs.read((char*)&magic_number, sizeof(magic_number));
    fs.read((char*)&num_images, sizeof(num_images));
    fs.read((char*)&num_rows, sizeof(num_rows));
    fs.read((char*)&num_columns, sizeof(num_columns));
    if (magic_number != 2051) {
      swap(magic_number);
      swap(num_images);
      swap(num_rows);
      swap(num_columns);
    }

    X = matrix_t::Zero(num_images, num_rows*num_columns);

    for (size_t i=0; i<num_images; ++i) {
      for (size_t j=0; j<num_rows*num_columns; ++j) {
        unsigned char temp=0;
        fs.read((char*)&temp,sizeof(temp));
        X(i,j) = (double) temp;
      }
    }
    fs.close();
  } else {
    std::cout << "error reading file: " << filename << std::endl;
    exit(1);
  }
  return X;
}

matrix_t read_mnist_labels(std::string  filename)
{
  matrix_t Y;
  std::ifstream fs(filename.c_str(), std::ios::binary);
  if(fs) {
    int magic_number, num_images, num_rows, num_columns;
    fs.read((char*)&magic_number, sizeof(magic_number));
    fs.read((char*)&num_images, sizeof(num_images));
    if (magic_number != 2049) {
      swap(magic_number);
      swap(num_images);
    }

    Y = matrix_t::Zero(num_images, 10);

    for (size_t i=0; i<num_images; ++i) {
      unsigned char temp=0;
      fs.read((char*)&temp,sizeof(temp));
      Y(i,(int) temp) = 1.0;
    }
    fs.close();
  } else {
    std::cout << "error reading file: " << filename << std::endl;
    exit(1);
  }
  return Y;
}

int main (int argc, const char* argv[]) {

  if (argc != 2) {
    std::cout << "please provide path to mnist data ..." << std::endl;
    std::cout << "you can download the dataset at http://yann.lecun.com/exdb/mnist/" << std::endl;
    std::cout << std::endl << "usage: " << argv[0] << " path_to_data" << std::endl << std::endl;
    return 1;
  }

  std::string path = argv[1];

  std::cout << "reading data" << std::endl;

  matrix_t X_train = read_mnist_images(path + "/train-images-idx3-ubyte");
  matrix_t Y_train = read_mnist_labels(path + "/train-labels-idx1-ubyte");
  matrix_t X_test = read_mnist_images(path + "/t10k-images-idx3-ubyte");
  matrix_t Y_test = read_mnist_labels(path + "/t10k-labels-idx1-ubyte");

  // number of optimization steps
  int max_steps = 600;
  // regularization parameter
  double lambda = 0.0;

  // specify network topology
  Eigen::VectorXi topo(3);
  topo << X_train.cols(), 300, Y_test.cols();
  std::cout << "topology: " << topo.transpose() << std::endl;

  // initialize a neural network with given topology
  std::cout << "initializing network" << std::endl;
  NeuralNet nn(topo);

  std::cout << "scaling the data" << std::endl;
  nn.autoscale(X_train, Y_train);

  // train the network
  std::cout << "starting training" << std::endl;
  std::cout << "iter        error" << std::endl;
  double err;
  for (int i = 0; i < max_steps; ++i) {
    err = nn.loss(X_train, Y_train, lambda);
    nn.rprop();
    printf("%4i   %10.7f\n", i, err);
  }

  // test accuracy
  nn.forward_pass(X_test);
  matrix_t prediction = nn.get_activation();
  int correct = 0;
  int k;
  for (size_t i=0; i<Y_test.rows(); ++i) {
    prediction.row(i).maxCoeff(&k);
    correct += Y_test(i, k);
  }

  std::cout << "test accuracy: " << correct*1.0/Y_test.rows() << std::endl;

  nn.write("mnist.nn");

  return 0;
}


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#include <cstdlib>
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <cmath>
#include <limits>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include "NAOS/constants.hpp"
#include "NAOS/basicMath.hpp"
#include "NAOS/basicAstro.hpp"
#include "NAOS/misc.hpp"
#include "NAOS/ellipsoidGravitationalAcceleration.hpp"

namespace naos
{

//! equations of motion (for a particle around the asteroid modelled as a spheroid)
/*!
 * first order differential equations describing the motion of a particle or spacecraft around a
 * central body modeled as a spheroid using ellipsoid gravitational model.
 */
class equationsOfMotionParticleAroundSpheroid
{
    // declare parameters, gravitational parameter and the radius of the spheroid
    const double gravParameter;
    const double alpha;
    const double zRotation;

public:
    // Default constructor with member initializer list, get the gravitational parameter
    equationsOfMotionParticleAroundSpheroid( const double aGravParameter,
                       const double aAlpha,
                       const double aZRotation )
                    : gravParameter( aGravParameter ),
                      alpha( aAlpha ),
                      zRotation( aZRotation )
    { }
    void operator() ( const std::vector< double > &stateVector,
                      std::vector< double > &dXdt,
                      const double currentTime )
    {
        // calculate the gravitational accelerations first
        std::vector< double > gravAcceleration( 3, 0.0 );

        computeEllipsoidGravitationalAcceleration( alpha,
                                                   alpha,
                                                   alpha,
                                                   gravParameter,
                                                   stateVector[ xPositionIndex ],
                                                   stateVector[ yPositionIndex ],
                                                   stateVector[ zPositionIndex ],
                                                   gravAcceleration );

        // now calculate the derivatives
        dXdt[ xPositionIndex ] = stateVector[ xVelocityIndex ];
        dXdt[ yPositionIndex ] = stateVector[ yVelocityIndex ];
        dXdt[ zPositionIndex ] = stateVector[ zVelocityIndex ];

        dXdt[ xVelocityIndex ] = gravAcceleration[ xPositionIndex ]
                                + 2.0 * zRotation * stateVector[ yVelocityIndex ]
                                + zRotation * zRotation * stateVector[ xPositionIndex ];

        dXdt[ yVelocityIndex ] = gravAcceleration[ yPositionIndex ]
                                - 2.0 * zRotation * stateVector[ xVelocityIndex ]
                                + zRotation * zRotation * stateVector[ yPositionIndex ];

        dXdt[ zVelocityIndex ] = gravAcceleration[ zPositionIndex ];
    }
};

//! Store intermediate state values and time( if needed )
/*!
 * This structure contains members that will save all intermediate state values and times when
 * an object of this structure is passed as an argument to the integrator function.
 */
struct pushBackStateAndTime
{
    // declare containers to store state and time
    std::vector< std::vector< double > > &stateContainer;
    std::vector< double > &timeContainer;

    //member initializer list
    pushBackStateAndTime( std::vector< std::vector< double > > &aState,
                          std::vector< double > &aTime )
                : stateContainer( aState ),
                  timeContainer( aTime )
    { }

    void operator() ( const std::vector< double > &singleStateVector, const double singleTime )
    {
        // store the intermediate state and time values in the containers
        stateContainer.push_back( singleStateVector );
        timeContainer.push_back( singleTime );
    }
};

//! particle around spheroid integration
/*!
 * integrate the equations of motion for a particle around a spheroid. The gravitational accelerations
 * calculated using the ellipsoid gravitational potential model.
 */
void executeParticleAroundSpheroid( const double alpha,
                                    const double gravParameter,
                                    std::vector< double > asteroidRotationVector,
                                    std::vector< double > &initialOrbitalElements,
                                    const double initialStepSize,
                                    const double startTime,
                                    const double endTime,
                                    std::ostringstream &outputFilePath,
                                    const int dataSaveIntervals )
{
    //! open the output csv file to save data. Declare file headers.
    std::ofstream outputFile;
    outputFile.open( outputFilePath.str( ) );
    outputFile << "x" << ",";
    outputFile << "y" << ",";
    outputFile << "z" << ",";
    outputFile << "vx" << ",";
    outputFile << "vy" << ",";
    outputFile << "vz" << ",";
    outputFile << "t" << std::endl;
    outputFile.precision( 16 );

    //! convert the initial orbital elements to cartesian state
    std::vector< double > initialStateInertial( 6, 0.0 );
    initialStateInertial = convertKeplerianElementsToCartesianCoordinates( initialOrbitalElements,
                                                                           gravParameter );

    std::vector< double > inertialPositionVector = { initialStateInertial[ xPositionIndex ],
                                                     initialStateInertial[ yPositionIndex ],
                                                     initialStateInertial[ zPositionIndex ] };
    std::vector< double > omegaCrossPosition( 3, 0.0 );
    omegaCrossPosition = crossProduct( asteroidRotationVector, inertialPositionVector );

    std::vector< double > initialState( 6, 0.0 );
    initialState[ xPositionIndex ] = initialStateInertial[ xPositionIndex ];
    initialState[ yPositionIndex ] = initialStateInertial[ yPositionIndex ];
    initialState[ zPositionIndex ] = initialStateInertial[ zPositionIndex ];

    initialState[ xVelocityIndex ]
                = initialStateInertial[ xVelocityIndex ] - omegaCrossPosition[ 0 ];

    initialState[ yVelocityIndex ]
                = initialStateInertial[ yVelocityIndex ] - omegaCrossPosition[ 1 ];

    initialState[ zVelocityIndex ]
                = initialStateInertial[ zVelocityIndex ] - omegaCrossPosition[ 2 ];

    // set up boost odeint
    const double absoluteTolerance = 1.0e-15;
    const double relativeTolerance = 1.0e-15;
    typedef boost::numeric::odeint::runge_kutta_fehlberg78< std::vector< double > > stepperType;

    // state step size guess (at each step this initial guess will be used)
    double stepSizeGuess = initialStepSize;

    // initialize the ode system
    const double zRotation = asteroidRotationVector[ zPositionIndex ];
    equationsOfMotionParticleAroundSpheroid particleAroundSpheroidProblem( gravParameter, alpha, zRotation );

    // initialize current state vector and time
    std::vector< double > currentStateVector = initialState;
    double currentTime = startTime;
    double intermediateEndTime = currentTime + dataSaveIntervals;

    // save the initial state vector
    outputFile << currentStateVector[ xPositionIndex ] << ",";
    outputFile << currentStateVector[ yPositionIndex ] << ",";
    outputFile << currentStateVector[ zPositionIndex ] << ",";
    outputFile << currentStateVector[ xVelocityIndex ] << ",";
    outputFile << currentStateVector[ yVelocityIndex ] << ",";
    outputFile << currentStateVector[ zVelocityIndex ] << ",";
    outputFile << currentTime << std::endl;

    // start the integration outer loop
    while( intermediateEndTime <= endTime )
    {
        // perform integration, integrated result stored in currentStateVector
        size_t steps = boost::numeric::odeint::integrate_adaptive(
                            make_controlled( absoluteTolerance, relativeTolerance, stepperType( ) ),
                            particleAroundSpheroidProblem,
                            currentStateVector,
                            currentTime,
                            intermediateEndTime,
                            stepSizeGuess );

        // update the time variables
        currentTime = intermediateEndTime;
        intermediateEndTime = currentTime + dataSaveIntervals;

        // save data
        outputFile << currentStateVector[ xPositionIndex ] << ",";
        outputFile << currentStateVector[ yPositionIndex ] << ",";
        outputFile << currentStateVector[ zPositionIndex ] << ",";
        outputFile << currentStateVector[ xVelocityIndex ] << ",";
        outputFile << currentStateVector[ yVelocityIndex ] << ",";
        outputFile << currentStateVector[ zVelocityIndex ] << ",";
        outputFile << currentTime << std::endl;

    } // end of outer while loop for integration

    outputFile.close( );
}

} // namespace naos


#include <algorithm>
#include <cmath>
#include <cstdint>
#include <limits>
#include <numeric>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include "losslessops.h"

using namespace crolol;
using namespace backend;
namespace mp = boost::multiprecision;
using std::int64_t;
using slims = std::numeric_limits<int64_t>;
using int128 = mp::int128_t;
using float128 = mp::cpp_bin_float_quad;

static constexpr int64_t slmax = slims::max();
static constexpr int64_t slmin = slims::min();
static constexpr int64_t scale = 1000;
static const float128 slmaxlog2 = mp::log2(static_cast<float128>(slmax) / scale);
static const float128 slminlog2 = mp::log2(-static_cast<float128>(slmax) / scale);

static saferet clamp(const int128& n)
{
	saferet out;
	out.val = static_cast<int64_t>(n);

	if (n > static_cast<int128>(slmax))
		out.flow = overflow;
	else if (n < static_cast<int128>(slmin))
		out.flow = underflow;

	return out;
}

static saferet make_error(const flowstatus flow)
{
	saferet out;
	out.flow = flow;

	return out;
}

static saferet make_badarg(const int64_t val)
{
	saferet out = make_error(badarg);
	out.val = val;

	return out;
}

saferet backend::multiply(int64_t n, int64_t m)
{
	return clamp((static_cast<int128>(n) * static_cast<int128>(m))
		/ static_cast<int128>(scale));
}

saferet backend::divide(int64_t n, int64_t m)
{
	if (m == 0) return make_badarg(n);
	else return clamp((static_cast<int128>(n) * scale)
		/ static_cast<int128>(m));
}

saferet backend::pow(int64_t n, int64_t m)
{
	if (n < 0 && m % scale != 0) return make_badarg(m);
	else if (n == 0 && m == 0) return clamp(scale);
	else if (n == 0 && m < 0) return make_badarg(m);
	else if (n == scale || m == 0) return clamp(scale);
	else if (m == scale) return clamp(n);

	const float128 nf = static_cast<float128>(n) / scale;
	const float128 mf = static_cast<float128>(m) / scale;
	const float128 size = mp::log2(mp::abs(nf)) * mf;

	if (size > slmaxlog2) return clamp(slmax);
	else if (mp::abs(size) > slminlog2) return clamp(slmin);
	else return clamp(static_cast<int128>(
		mp::llrint(mp::trunc(mp::pow(nf, mf) * scale))
	));
}

saferet backend::factorial(int64_t n)
{
	if (n < 0 or n % scale != 0) return make_badarg(n);

	saferet acc = clamp(scale);

	for (int64_t i = 0; i < n && acc.flow == noflow; i += scale)
		acc = multiply(acc.val, i);

	return acc;
}

static const int64_t cr2pi = std::llrintl(std::truncl(M_PI * 2.L * scale));
static constexpr long double radtodeg = 360.L / (M_PI * 2.L);
static constexpr long double degtorad = (M_PI * 2.L) / 360.L;

static long double trigmod(int64_t n)
{
	return static_cast<long double>(n % cr2pi) * degtorad / scale;
}

saferet backend::sin(int64_t n)
{
	return clamp(static_cast<int128>(
		std::truncl(std::sin(trigmod(n))) * scale
	));
}

saferet backend::cos(int64_t n)
{
	return clamp(static_cast<int128>(
		std::truncl(std::cos(trigmod(n))) * scale
	));
}

saferet backend::tan(int64_t n)
{
	return clamp(static_cast<int128>(
		std::truncl(std::tan(trigmod(n))) * scale
	));
}

saferet backend::asin(int64_t n)
{
	if (n > scale or n < -scale) return make_badarg(n);
	else return clamp(static_cast<int128>(
		std::truncl(std::asin(
			static_cast<long double>(n) / scale
		)) * scale * radtodeg
	));
}

saferet backend::acos(int64_t n)
{
	if (n > scale or n < -scale) return make_badarg(n);
	else return clamp(static_cast<int128>(
		std::truncl(std::acos(
			static_cast<long double>(n) / scale
		)) * scale * radtodeg
	));
}

saferet backend::atan(int64_t n)
{
	return clamp(static_cast<int128>(
		std::truncl(std::atan(
			static_cast<long double>(n) / scale
		)) * scale * radtodeg
	));
}


#include <bitset>
#include <boost/dynamic_bitset.hpp>
#include <chrono>
#include <vector>

#include <stdio.h>

bool verbose = true;
const int bits = 30;
const int64_t mask = (1<<30) - 1;

void sieve(int64_t n)
{
  auto start = std::chrono::system_clock::now();
  std::vector<boost::dynamic_bitset<>> notprime(1+(n>>bits), boost::dynamic_bitset<>((1LL<<bits)+1));
  printf("constructed %lld %lld\n", 1+(n>>bits), (1LL<<bits)+1);
  for (int64_t p = 2; p*p <= n; p++) {
    if (notprime[p>>bits][p&mask] == 0) {
      for (int64_t q = p*2; q <= n; q += p)
	notprime[q>>bits][q&mask] = 1;
    }
  }
  auto end = std::chrono::system_clock::now();
  std::chrono::duration<double> elapsed_seconds = end-start;
  printf("Computation took %f sec\n", elapsed_seconds.count());

  int64_t k = 0;
  printf("Primes <= %lld\n", n);
  for (int64_t p = 2; p <= n; p++)
    if (notprime[p>>bits][p&mask] == 0) {
      ++k;
      if (verbose) {
	printf("%lld ", p);
      }
    }
  if (verbose)
    printf("\n");
  printf("%lld primes\n", k);
}

int main(int argc, char* argv[]) {
  if (argc < 2) {
    printf("era [-quiet] <n>: computes primes <= n\n");
    return -1;
  }
  if (!strcmp(argv[1], "-quiet")) {
    verbose = false;
  }
  int64_t n = std::stoll(argv[argc-1]);
  sieve(n);
  return 0;
}