// TRENTO: Reduced Thickness Event-by-event Nuclear Topology
// Copyright 2015 Jonah E. Bernhard, J. Scott Moreland
// MIT License

#include "nucleus.h"

#include <algorithm>
#include <cmath>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>

#include <boost/math/constants/constants.hpp>
#ifdef TRENTO_HDF5
// include multi_array for use with ManualNucleus
#ifdef NDEBUG
#define BOOST_DISABLE_ASSERTS
#endif
#include <boost/multi_array.hpp>
#endif

#include "hdf5_utils.h"
#include "random.h"

namespace trento {

NucleusPtr Nucleus::create(const std::string& species, double nucleon_dmin) {
  // W-S params ref. in header
  // XXX: remember to add new species to the help output in main() and the readme
  if (species == "p")
    return NucleusPtr{new Proton{}};
  else if (species == "d")
    return NucleusPtr{new Deuteron{}};
  else if (species == "Cu")
    return NucleusPtr{new WoodsSaxonNucleus{
       63, 4.20, 0.596, 0.0, nucleon_dmin
    }};
  else if (species == "Cu2")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
       63, 4.20, 0.596, 0.162, -0.006, nucleon_dmin
    }};
  else if (species == "O")
      return NucleusPtr{new WoodsSaxonNucleus{
              16, 2.608, 0.513, -0.051, nucleon_dmin
      }};
  else if (species == "Xe")
    return NucleusPtr{new WoodsSaxonNucleus{
      129, 5.36, 0.590, 0.0, nucleon_dmin
    }};
  else if (species == "Xe2")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
      129, 5.36, 0.590, 0.162, -0.003, nucleon_dmin
    }};
  else if (species == "XeTri")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
     129, 5.60, 0.49, 0.21, 0.0, nucleon_dmin
    }};
    else if (species == "XeALICE")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
     129, 5.36, 0.590, 0.18, 0.0, nucleon_dmin
    }};
  else if (species == "Au")
    return NucleusPtr{new WoodsSaxonNucleus{
      197, 6.38, 0.535, 0.0, nucleon_dmin
    }};
  else if (species == "Au2")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
      197, 6.38, 0.535, -0.131, -0.031, nucleon_dmin
    }};
  else if (species == "Pb")
    return NucleusPtr{new WoodsSaxonNucleus{
      208, 6.62, 0.546, 0.0, nucleon_dmin
    }};
  else if (species == "PbTri")
    return NucleusPtr{new WoodsSaxonNucleus{
      208, 6.65, 0.54, 0.0, nucleon_dmin
    }};
  else if (species == "U")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
      238, 6.81, 0.600, 0.280, 0.093, nucleon_dmin
    }};
  else if (species == "U2")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
      238, 6.86, 0.420, 0.265, 0.000, nucleon_dmin
    }};
  else if (species == "U3")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
      238, 6.67, 0.440, 0.280, 0.093, nucleon_dmin
    }};
  // Read nuclear configurations from HDF5.
  else if (hdf5::filename_is_hdf5(species)) {
#ifdef TRENTO_HDF5
    return ManualNucleus::create(species);
#else
    throw std::invalid_argument{"HDF5 output was not compiled"};
#endif  // TRENTO_HDF5
  }
  else
    throw std::invalid_argument{"unknown projectile species: " + species};
}

Nucleus::Nucleus(std::size_t A) : nucleons_(A), offset_(0) {}

void Nucleus::sample_nucleons(double offset) {
  offset_ = offset;
  sample_nucleons_impl();
}

void Nucleus::set_nucleon_position(
    NucleonData& nucleon, double x, double y, double z) {
  nucleon.set_position(x + offset_, y, z);
}

Proton::Proton() : Nucleus(1) {}

/// Always zero.
double Proton::radius() const {
  return 0.;
}

/// Always place the nucleon at the origin.
void Proton::sample_nucleons_impl() {
  set_nucleon_position(*begin(), 0., 0., 0.);
}

// Without loss of generality, let the internal a_ parameter be the minimum of
// the given (a, b) and the internal b_ be the maximum.
Deuteron::Deuteron(double a, double b)
    : Nucleus(2),
      a_(std::fmin(a, b)),
      b_(std::fmax(a, b))
{}

double Deuteron::radius() const {
  // The quantile function for the exponential distribution exp(-2*a*r) is
  // -log(1-q)/(2a).  Return the 99% quantile.
  return -std::log(.01)/(2*a_);
}

void Deuteron::sample_nucleons_impl() {
  // Sample the inter-nucleon radius using rejection sampling with an envelope
  // function.  The Hulthén wavefunction including the r^2 Jacobian expands to
  // three exponential terms:  exp(-2*a*r) + exp(-2*b*r) - 2*exp(-(a+b)*r).
  // This does not have a closed-form inverse CDF, however we can easily sample
  // exponential numbers from the term that falls off the slowest, i.e.
  // exp(-2*min(a,b)*r).  In the ctor initializer list the "a" parameter is
  // always set to the minimum, so we should sample from exp(-2*a*r).
  double r, prob;
  do {
    // Sample a uniform random number, u = exp(-2*a*r).
    auto u = random::canonical<double>();
    // Invert to find the actual radius.
    r = -std::log(u) / (2*a_);
    // The acceptance probability is now the radial wavefunction over the
    // envelope function, both evaluated at the proposal radius r.
    // Conveniently, the envelope evaluated at r is just the uniform random
    // number u.
    prob = std::pow(std::exp(-a_*r) - std::exp(-b_*r), 2) / u;
  } while (prob < random::canonical<double>());

  // Now sample spherical rotation angles.
  auto cos_theta = random::cos_theta<double>();
  auto phi = random::phi<double>();

  // And compute the Cartesian coordinates of one nucleon.
  auto r_sin_theta = r * std::sqrt(1. - cos_theta*cos_theta);
  auto x = r_sin_theta * std::cos(phi);
  auto y = r_sin_theta * std::sin(phi);
  auto z = r * cos_theta;

  // Place the first nucleon at the sampled coordinates.
  set_nucleon_position(*begin(), x, y, z);
  // Place the second nucleon opposite to the first.
  set_nucleon_position(*std::next(begin()), -x, -y, -z);
}

MinDistNucleus::MinDistNucleus(std::size_t A, double dmin)
    : Nucleus(A),
      dminsq_(dmin*dmin)
{}

bool MinDistNucleus::is_too_close(const_iterator nucleon) const {
  if (dminsq_ < 1e-10)
    return false;
  for (const_iterator nucleon2 = begin(); nucleon2 != nucleon; ++nucleon2) {
    auto dx = nucleon->x() - nucleon2->x();
    auto dy = nucleon->y() - nucleon2->y();
    auto dz = nucleon->z() - nucleon2->z();
    if (dx*dx + dy*dy + dz*dz < dminsq_)
      return true;
  }
  return false;
}

// Extend the W-S dist out to R + 10a; for typical values of (R, a), the
// probability of sampling a nucleon beyond this radius is O(10^-5).
WoodsSaxonNucleus::WoodsSaxonNucleus(
    std::size_t A, double R, double a, double w, double dmin)
    : MinDistNucleus(A, dmin),
      R_(R),
      a_(a),
      woods_saxon_dist_(1000, 0., R + 10.*a,
        [R, a, w](double r) { return r*r*(1+w*(r/R)*(r/R))/(1.+std::exp((r-R)/a)); })
{}

/// Return something a bit smaller than the true maximum radius.  The
/// Woods-Saxon distribution falls off very rapidly (exponentially), and since
/// this radius determines the impact parameter range, the true maximum radius
/// would cause far too many events with zero participants.
double WoodsSaxonNucleus::radius() const {
  return R_ + 3.*a_;
}

/// Sample Woods-Saxon nucleon positions.
void WoodsSaxonNucleus::sample_nucleons_impl() {
  // When placing nucleons with a minimum distance criterion, resample spherical
  // angles until the nucleon is not too close to a previously sampled nucleon,
  // but do not resample radius -- this could modify the Woods-Saxon dist.

  // Because of the r^2 Jacobian, there is less available space at smaller
  // radii.  Therefore, pre-sample all radii first, sort them, and then place
  // nucleons starting with the smallest radius and working outwards.  This
  // dramatically reduces the chance that a nucleon cannot be placed.
  std::vector<double> radii(size());
  for (auto&& r : radii)
    r = woods_saxon_dist_(random::engine);
  std::sort(radii.begin(), radii.end());

  // Place each nucleon at a pre-sampled radius.
  auto r_iter = radii.cbegin();
  for (iterator nucleon = begin(); nucleon != end(); ++nucleon) {
    // Get radius and advance iterator.
    auto& r = *r_iter++;

    // Sample angles until the minimum distance criterion is satisfied.
    auto ntries = 0;
    do {
      // Sample isotropic spherical angles.
      auto cos_theta = random::cos_theta<double>();
      auto phi = random::phi<double>();

      // Convert to Cartesian coordinates.
      auto r_sin_theta = r * std::sqrt(1. - cos_theta*cos_theta);
      auto x = r_sin_theta * std::cos(phi);
      auto y = r_sin_theta * std::sin(phi);
      auto z = r * cos_theta;

      set_nucleon_position(*nucleon, x, y, z);

      // Retry sampling a reasonable number of times.  If a nucleon cannot be
      // placed, give up and leave it at its last sampled position.  Some
      // approximate numbers for Pb nuclei:
      //
      //   dmin = 0.5 fm, < 0.001% of nucleons cannot be placed
      //          1.0 fm, ~0.005%
      //          1.5 fm, ~0.1%
      //          1.73 fm, ~1%
    } while (++ntries < 1000 && is_too_close(nucleon));
  }
  // XXX: re-center nucleon positions?
}

// Set rmax like the non-deformed case (R + 10a), but for the maximum
// "effective" radius.  The numerical coefficients for beta2 and beta4 are the
// approximate values of Y20 and Y40 at theta = 0.
DeformedWoodsSaxonNucleus::DeformedWoodsSaxonNucleus(
    std::size_t A, double R, double a, double beta2, double beta4, double dmin)
    : MinDistNucleus(A, dmin),
      R_(R),
      a_(a),
      beta2_(beta2),
      beta4_(beta4),
      rmax_(R*(1. + .63*std::fabs(beta2) + .85*std::fabs(beta4)) + 10.*a)
{}

/// Return something a bit smaller than the true maximum radius.  The
/// Woods-Saxon distribution falls off very rapidly (exponentially), and since
/// this radius determines the impact parameter range, the true maximum radius
/// would cause far too many events with zero participants.
double DeformedWoodsSaxonNucleus::radius() const {
  return rmax_ - 7.*a_;
}

double DeformedWoodsSaxonNucleus::deformed_woods_saxon_dist(
    double r, double cos_theta) const {
  auto cos_theta_sq = cos_theta*cos_theta;

  // spherical harmonics
  using math::double_constants::one_div_root_pi;
  auto Y20 = std::sqrt(5)/4. * one_div_root_pi * (3.*cos_theta_sq - 1.);
  auto Y40 = 3./16. * one_div_root_pi *
             (35.*cos_theta_sq*cos_theta_sq - 30.*cos_theta_sq + 3.);

  // "effective" radius
  auto Reff = R_ * (1. + beta2_*Y20 + beta4_*Y40);

  return 1. / (1. + std::exp((r - Reff) / a_));
}

/// Sample deformed Woods-Saxon nucleon positions.
void DeformedWoodsSaxonNucleus::sample_nucleons_impl() {
  // The deformed W-S distribution is defined so the symmetry axis is aligned
  // with the Z axis, so e.g. the long axis of uranium coincides with Z.
  //
  // After sampling positions, they must be randomly rotated.  In general this
  // requires three Euler rotations, but in this case we only need two
  // because there is no use in rotating about the nuclear symmetry axis.
  //
  // The two rotations are:
  //  - a polar "tilt", i.e. rotation about the X axis
  //  - an azimuthal "spin", i.e. rotation about the original Z axis

  // "tilt" angle
  const auto cos_a = random::cos_theta<double>();
  const auto sin_a = std::sqrt(1. - cos_a*cos_a);

  // "spin" angle
  const auto angle_b = random::phi<double>();
  const auto cos_b = std::cos(angle_b);
  const auto sin_b = std::sin(angle_b);

  // Pre-sample and sort (r, cos_theta) points from the deformed W-S dist.
  // See comments in WoodsSaxonNucleus (above) for rationale.
  struct Sample {
    double r, cos_theta;
  };

  std::vector<Sample> samples(size());

  for (auto&& sample : samples) {
    // Sample (r, cos_theta) using a standard rejection method.
    // Remember to include the phase-space factors.
    do {
      sample.r = rmax_ * std::cbrt(random::canonical<double>());
      sample.cos_theta = random::cos_theta<double>();
    } while (
      random::canonical<double>() >
      deformed_woods_saxon_dist(sample.r, sample.cos_theta)
    );
  }

  // Sort by radius.  Could also sort by e.g. the perpendicular distance from
  // the z-axis, or by descending W-S density.  Empirically, radius leads to the
  // smallest failure rate.
  std::sort(
    samples.begin(), samples.end(),
    [](const Sample& a, const Sample& b) {
      return a.r < b.r;
    }
  );

  // Place each nucleon at a pre-sampled (r, cos_theta).
  auto sample = samples.cbegin();
  for (iterator nucleon = begin(); nucleon != end(); ++nucleon, ++sample) {
    auto& r = sample->r;
    auto& cos_theta = sample->cos_theta;

    auto r_sin_theta = r * std::sqrt(1. - cos_theta*cos_theta);
    auto z = r * cos_theta;

    // Sample azimuthal angle until the minimum distance criterion is satisfied.
    auto ntries = 0;
    do {
      // Choose azimuthal angle.
      auto phi = random::phi<double>();

      // Convert to Cartesian coordinates.
      auto x = r_sin_theta * std::cos(phi);
      auto y = r_sin_theta * std::sin(phi);

      // Rotate.
      // The rotation formula was derived by composing the "tilt" and "spin"
      // rotations described above.
      auto x_rot = x*cos_b - y*cos_a*sin_b + z*sin_a*sin_b;
      auto y_rot = x*sin_b + y*cos_a*cos_b - z*sin_a*cos_b;
      auto z_rot =           y*sin_a       + z*cos_a;

      set_nucleon_position(*nucleon, x_rot, y_rot, z_rot);

      // In addition to resampling phi, flip the z-coordinate each time.  This
      // works because the deformed WS dist is symmetric in z.  Effectively
      // doubles the available space for the nucleon.
      z *= -1;

      // Retry a reasonable number of times.  Unfortunately the failure rate is
      // worse than non-deformed sampling because there is less freedom to place
      // each nucleon.  Some approximate numbers for U nuclei:
      //
      //   dmin = 0.5 fm, < 0.001% of nucleons cannot be placed
      //          1.0 fm, ~0.03%
      //          1.3 fm, ~0.3%
      //          1.5 fm, ~1.2%
    } while (++ntries < 1000 && is_too_close(nucleon));
  }
}

#ifdef TRENTO_HDF5

namespace {

// Read a slice of an HDF5 dataset into a boost::multi_array.
template <typename T, std::size_t FileDims, std::size_t MemDims>
boost::multi_array<T, MemDims>
read_dataset(
    const H5::DataSet& dataset,
    const std::array<hsize_t, FileDims>& count,
    const std::array<hsize_t, FileDims>& start,
    const std::array<hsize_t, MemDims>& shape) {
  boost::multi_array<T, MemDims> array{shape};
  auto filespace = dataset.getSpace();
  filespace.selectHyperslab(H5S_SELECT_SET, count.data(), start.data());
  auto memspace = hdf5::make_dataspace(shape);

  dataset.read(array.data(), hdf5::type<T>(), memspace, filespace);

  return array;
}

}  // unnamed namespace

std::unique_ptr<ManualNucleus> ManualNucleus::create(const std::string& path) {
  auto file = hdf5::try_open_file(path);

  // Check that there is a single dataset in the file.
  // Might relax this constraint in the future.
  if (file.getNumObjs() != 1)
    throw std::invalid_argument{
      "file '" + path + "' must contain exactly one object"
    };

  auto name = file.getObjnameByIdx(0);
#if H5_VERSION_GE(1, 8, 13)
  if (file.childObjType(name) != H5O_TYPE_DATASET)  // added v1.8.13
#else
  if (file.getObjTypeByIdx(0) != H5G_DATASET)  // deprecated fall back
#endif
    throw std::invalid_argument{
      "object '" + name + "' in file '" + path + "' is not a dataset"
    };

  // Make dataset object in a unique_ptr for eventual passing to ctor.
  auto dataset = std::unique_ptr<H5::DataSet>{
    new H5::DataSet{file.openDataSet(name)}
  };

  // Verify that the dataset has the correct dimensionality and shape.
  std::array<hsize_t, 3> shape;
  auto ndim = dataset->getSpace().getSimpleExtentDims(shape.data());

  if (ndim != 3)
    throw std::invalid_argument{
      "dataset '" + name + "' in file '" + path + "' has " +
      std::to_string(ndim) + " dimensions (need 3)"
    };

  if (shape[2] != 3)
    throw std::invalid_argument{
      "dataset '" + name + "' in file '" + path + "' has " +
      std::to_string(shape[2]) + " columns (need 3)"
    };

  // Deduce number of configs and number of nucleons (A) from the shape.
  const auto& nconfigs = shape[0];
  const auto& A = shape[1];

  // Estimate the max radius from at least 500 nucleon positions.
  auto n = std::min(500/A + 1, nconfigs);
  std::array<hsize_t, 3> count = {n, A, 3};
  std::array<hsize_t, 3> start = {0, 0, 0};
  std::array<hsize_t, 2> shape_n = {n*A, 3};
  auto positions = read_dataset<float>(*dataset, count, start, shape_n);

  auto rmax_sq = 0.;

  for (const auto& position : positions) {
    auto& x = position[0];
    auto& y = position[1];
    auto& z = position[2];
    auto r_sq = x*x + y*y + z*z;
    if (r_sq > rmax_sq)
      rmax_sq = r_sq;
  }

  auto rmax = std::sqrt(rmax_sq);

  return std::unique_ptr<ManualNucleus>{
    new ManualNucleus{std::move(dataset), nconfigs, A, rmax}
  };
}

ManualNucleus::ManualNucleus(std::unique_ptr<H5::DataSet> dataset,
                             std::size_t nconfigs, std::size_t A, double rmax)
    : Nucleus(A),
      dataset_(std::move(dataset)),
      rmax_(rmax),
      index_dist_(0, nconfigs - 1)
{}

ManualNucleus::~ManualNucleus() = default;

double ManualNucleus::radius() const {
  return rmax_;
}

void ManualNucleus::sample_nucleons_impl() {
  // Sample Euler rotation angles.
  // First is an azimuthal spin about the Z axis.
  const auto angle_1 = random::phi<double>();
  const auto c1 = std::cos(angle_1);
  const auto s1 = std::sin(angle_1);
  // Then a polar tilt about the original X axis, uniform in cos(theta).
  const auto c2 = random::cos_theta<double>();
  const auto s2 = std::sqrt(1. - c2*c2);
  // Finally another azimuthal spin about the original Z axis.
  const auto angle_3 = random::phi<double>();
  const auto c3 = std::cos(angle_3);
  const auto s3 = std::sin(angle_3);

  // Choose and read a random config from the dataset.
  std::array<hsize_t, 3> count = {1, size(), 3};
  std::array<hsize_t, 3> start = {index_dist_(random::engine), 0, 0};
  std::array<hsize_t, 2> shape = {size(), 3};
  const auto positions = read_dataset<float>(*dataset_, count, start, shape);

  // Loop over positions and nucleons.
  auto positions_iter = positions.begin();
  for (iterator nucleon = begin(); nucleon != end(); ++nucleon) {
    // Extract position vector and increment iterator.
    auto position = *positions_iter++;
    auto& x = position[0];
    auto& y = position[1];
    auto& z = position[2];

    // Rotate.
    auto x_rot = x*(c1*c3 - c2*s1*s3) - y*(c3*s1 + c1*c2*s3) + z*s2*s3;
    auto y_rot = x*(c1*s3 + c2*c3*s1) - y*(s1*s3 - c1*c2*c3) - z*c3*s2;
    auto z_rot = x*s1*s2              + y*c1*s2              + z*c2;

    set_nucleon_position(*nucleon, x_rot, y_rot, z_rot);
  }
}

#endif  // TRENTO_HDF5

}  // namespace trento


/*
Copyright (c) 2013 Daniel Stahlke

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

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

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

// This demonstrates all sorts of data types that can be plotted using send2d().  It is not
// meant as a first tutorial; for that see example-misc.cc or the project wiki.

#define USE_CXX (__cplusplus >= 201103)

#include <vector>
#include <complex>
#include <cmath>

#include <boost/tuple/tuple.hpp>
#include <boost/array.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/irange.hpp>
#include <boost/bind.hpp>

#ifdef USE_ARMA
#include <armadillo>
#endif

#ifdef USE_BLITZ
#include <blitz/array.h>
#endif

#include "gnuplot-iostream.h"

#ifndef M_PI
#	define M_PI 3.14159265358979323846
#endif

// The number of axial points of the torus.
const int num_u = 10;
// The total number of longitudinal points of the torus.  This is set at the beginning of
// main().
int num_v_total;

// This doesn't have to be a template.  It's just a template to show that such things are
// possible.
template <typename T>
struct MyTriple {
	MyTriple() : x(0), y(0), z(0) { }
	MyTriple(T _x, T _y, T _z) : x(_x), y(_y), z(_z) { }

	T x, y, z;
};

// Tells gnuplot-iostream how to print objects of class MyTriple.
namespace gnuplotio {
	template<typename T>
	struct BinfmtSender<MyTriple<T> > {
		static void send(std::ostream &stream) {
			BinfmtSender<T>::send(stream);
			BinfmtSender<T>::send(stream);
			BinfmtSender<T>::send(stream);
		}
	};

	template <typename T>
	struct BinarySender<MyTriple<T> > {
		static void send(std::ostream &stream, const MyTriple<T> &v) {
			BinarySender<T>::send(stream, v.x);
			BinarySender<T>::send(stream, v.y);
			BinarySender<T>::send(stream, v.z);
		}
	};

	// We don't use text mode in this demo.  This is just here to show how it would go.
	template<typename T>
	struct TextSender<MyTriple<T> > {
		static void send(std::ostream &stream, const MyTriple<T> &v) {
			TextSender<T>::send(stream, v.x);
			stream << " ";
			TextSender<T>::send(stream, v.y);
			stream << " ";
			TextSender<T>::send(stream, v.z);
		}
	};
}

MyTriple<double> get_point(int u, int v) {
	double a = 2.0*M_PI*u/(num_u-1);
	double b = 2.0*M_PI*v/(num_v_total-1);
	double z = 0.3*std::cos(a);
	double r = 1 + 0.3*std::sin(a);
	double x = r * std::cos(b);
	double y = r * std::sin(b);
	return MyTriple<double>(x, y, z);
}

int main() {
	Gnuplot gp;
	// for debugging, prints to console
	//Gnuplot gp(stdout);

	int num_examples = 7;
#ifdef USE_ARMA
	num_examples += 3;
#endif
#ifdef USE_BLITZ
	num_examples += 3;
#endif

	int num_v_each = 50 / num_examples + 1;

	num_v_total = (num_v_each-1) * num_examples + 1;
	int shift = 0;

	gp << "set zrange [-1:1]\n";
	gp << "set hidden3d nooffset\n";

	// I use temporary files rather than stdin because the syntax ends up being easier when
	// plotting several datasets.  With the stdin method you have to give the full plot
	// command, then all the data.  But I would rather give the portion of the plot command for
	// the first dataset, then give the data, then the command for the second dataset, then the
	// data, etc.

	gp << "splot ";

	{
		std::vector<std::vector<MyTriple<double> > > pts(num_u);
		for(int u=0; u<num_u; u++) {
			pts[u].resize(num_v_each);
			for(int v=0; v<num_v_each; v++) {
				pts[u][v] = get_point(u, v+shift);
			}
		}
		gp << gp.binFile2d(pts, "record") << "with lines title 'vec of vec of MyTriple'";
	}

	gp << ", ";
	shift += num_v_each-1;

	{
		std::vector<std::vector<boost::tuple<double,double,double> > > pts(num_u);
		for(int u=0; u<num_u; u++) {
			pts[u].resize(num_v_each);
			for(int v=0; v<num_v_each; v++) {
				pts[u][v] = boost::make_tuple(
					get_point(u, v+shift).x,
					get_point(u, v+shift).y,
					get_point(u, v+shift).z
				);
			}
		}
		gp << gp.binFile2d(pts, "record") << "with lines title 'vec of vec of boost::tuple'";
	}

	gp << ", ";
	shift += num_v_each-1;

	{
		std::vector<std::vector<double> > x_pts(num_u);
		std::vector<std::vector<double> > y_pts(num_u);
		std::vector<std::vector<double> > z_pts(num_u);
		for(int u=0; u<num_u; u++) {
			x_pts[u].resize(num_v_each);
			y_pts[u].resize(num_v_each);
			z_pts[u].resize(num_v_each);
			for(int v=0; v<num_v_each; v++) {
				x_pts[u][v] = get_point(u, v+shift).x;
				y_pts[u][v] = get_point(u, v+shift).y;
				z_pts[u][v] = get_point(u, v+shift).z;
			}
		}
		gp << gp.binFile2d(boost::make_tuple(x_pts, y_pts, z_pts), "record") <<
			"with lines title 'boost::tuple of vec of vec'";
	}

	gp << ", ";
	shift += num_v_each-1;

	{
		std::vector<boost::tuple<
				std::vector<double>,
				std::vector<double>,
				std::vector<double>
			> > pts;
		for(int u=0; u<num_u; u++) {
			std::vector<double> x_pts(num_v_each);
			std::vector<double> y_pts(num_v_each);
			std::vector<double> z_pts(num_v_each);
			for(int v=0; v<num_v_each; v++) {
				x_pts[v] = get_point(u, v+shift).x;
				y_pts[v] = get_point(u, v+shift).y;
				z_pts[v] = get_point(u, v+shift).z;
			}
			pts.push_back(boost::make_tuple(x_pts, y_pts, z_pts));
		}
		gp << gp.binFile2d(pts, "record") <<
			"with lines title 'vec of boost::tuple of vec'";
	}

	gp << ", ";
	shift += num_v_each-1;

	{
		std::vector<std::vector<double> > x_pts(num_u);
		std::vector<std::vector<std::pair<double, double> > > yz_pts(num_u);
		for(int u=0; u<num_u; u++) {
			x_pts[u].resize(num_v_each);
			yz_pts[u].resize(num_v_each);
			for(int v=0; v<num_v_each; v++) {
				x_pts [u][v] = get_point(u, v+shift).x;
				yz_pts[u][v] = std::make_pair(
					get_point(u, v+shift).y,
					get_point(u, v+shift).z);
			}
		}
		gp << gp.binFile2d(std::make_pair(x_pts, yz_pts), "record") <<
			"with lines title 'pair(vec(vec(dbl)),vec(vec(pair(dbl,dbl))))'";
	}

	gp << ", ";
	shift += num_v_each-1;

	{
		std::vector<std::vector<std::vector<double> > > pts(num_u);
		for(int u=0; u<num_u; u++) {
			pts[u].resize(num_v_each);
			for(int v=0; v<num_v_each; v++) {
				pts[u][v].resize(3);
				pts[u][v][0] = get_point(u, v+shift).x;
				pts[u][v][1] = get_point(u, v+shift).y;
				pts[u][v][2] = get_point(u, v+shift).z;
			}
		}
		gp << gp.binFile2d(pts, "record") << "with lines title 'vec vec vec'";
	}

	gp << ", ";
	shift += num_v_each-1;

	{
		std::vector<std::vector<std::vector<double> > > pts(3);
		for(int i=0; i<3; i++) pts[i].resize(num_u);
		for(int u=0; u<num_u; u++) {
			for(int i=0; i<3; i++) pts[i][u].resize(num_v_each);
			for(int v=0; v<num_v_each; v++) {
				pts[0][u][v] = get_point(u, v+shift).x;
				pts[1][u][v] = get_point(u, v+shift).y;
				pts[2][u][v] = get_point(u, v+shift).z;
			}
		}
		gp << gp.binFile2d_colmajor(pts, "record") << "with lines title 'vec vec vec (colmajor)'";
	}

#ifdef USE_ARMA
	gp << ", ";
	shift += num_v_each-1;

	{
		arma::cube pts(num_u, num_v_each, 3);
		for(int u=0; u<num_u; u++) {
			for(int v=0; v<num_v_each; v++) {
				pts(u, v, 0) = get_point(u, v+shift).x;
				pts(u, v, 1) = get_point(u, v+shift).y;
				pts(u, v, 2) = get_point(u, v+shift).z;
			}
		}
		gp << gp.file2d(pts) << "with lines title 'arma::cube(U*V*3)'";
	}

	gp << ", ";
	shift += num_v_each-1;

	{
		arma::cube pts(3, num_u, num_v_each);
		for(int u=0; u<num_u; u++) {
			for(int v=0; v<num_v_each; v++) {
				pts(0, u, v) = get_point(u, v+shift).x;
				pts(1, u, v) = get_point(u, v+shift).y;
				pts(2, u, v) = get_point(u, v+shift).z;
			}
		}
		gp << gp.binFile2d_colmajor(pts, "record") << "with lines title 'arma::cube(3*U*V) (colmajor)'";
	}

	gp << ", ";
	shift += num_v_each-1;

	{
		arma::field<MyTriple<double> > pts(num_u, num_v_each);
		for(int u=0; u<num_u; u++) {
			for(int v=0; v<num_v_each; v++) {
				pts(u, v) = get_point(u, v+shift);
			}
		}
		gp << gp.binFile2d(pts, "record") << "with lines title 'arma::field'";
	}
#endif

#ifdef USE_BLITZ
	gp << ", ";
	shift += num_v_each-1;

	{
		blitz::Array<blitz::TinyVector<double, 3>, 2> pts(num_u, num_v_each);
		for(int u=0; u<num_u; u++) {
			for(int v=0; v<num_v_each; v++) {
				pts(u, v)[0] = get_point(u, v+shift).x;
				pts(u, v)[1] = get_point(u, v+shift).y;
				pts(u, v)[2] = get_point(u, v+shift).z;
			}
		}
		gp << gp.binFile2d(pts, "record") << "with lines title 'blitz::Array<blitz::TinyVector<double, 3>, 2>'";
	}

	gp << ", ";
	shift += num_v_each-1;

	{
		blitz::Array<double, 3> pts(num_u, num_v_each, 3);
		for(int u=0; u<num_u; u++) {
			for(int v=0; v<num_v_each; v++) {
				pts(u, v, 0) = get_point(u, v+shift).x;
				pts(u, v, 1) = get_point(u, v+shift).y;
				pts(u, v, 2) = get_point(u, v+shift).z;
			}
		}
		gp << gp.binFile2d(pts, "record") << "with lines title 'blitz<double>(U*V*3)'";
	}

	gp << ", ";
	shift += num_v_each-1;

	{
		blitz::Array<double, 3> pts(3, num_u, num_v_each);
		for(int u=0; u<num_u; u++) {
			for(int v=0; v<num_v_each; v++) {
				pts(0, u, v) = get_point(u, v+shift).x;
				pts(1, u, v) = get_point(u, v+shift).y;
				pts(2, u, v) = get_point(u, v+shift).z;
			}
		}
		gp << gp.binFile2d_colmajor(pts, "record") << "with lines title 'blitz<double>(3*U*V) (colmajor)'";
	}
#endif

	gp << std::endl;

	std::cout << shift+num_v_each << "," << num_v_total << std::endl;
	assert(shift+num_v_each == num_v_total);

#ifdef _WIN32
	// For Windows, prompt for a keystroke before the Gnuplot object goes out of scope so that
	// the gnuplot window doesn't get closed.
	std::cout << "Press enter to exit." << std::endl;
	std::cin.get();
#endif

	return 0;
}


// Boost.Geometry

// Copyright (c) 2016-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_FORMULAS_MAXIMUM_LATITUDE_HPP
#define BOOST_GEOMETRY_FORMULAS_MAXIMUM_LATITUDE_HPP

#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/spherical.hpp>

#include <boost/mpl/assert.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief Algorithm to compute the vertex latitude of a geodesic segment. Vertex is
a point on the geodesic that maximizes (or minimizes) the latitude.
\author See
    [Wood96] Wood - Vertex Latitudes on Ellipsoid Geodesics, SIAM Rev., 38(4),
             637–644, 1996
*/

template <typename CT>
class vertex_latitude_on_sphere
{

public:
    template<typename T1, typename T2>
    static inline CT apply(T1 const& lat1,
                           T2 const& alp1)
    {
        return std::acos( math::abs(cos(lat1) * sin(alp1)) );
    }
};

template <typename CT>
class vertex_latitude_on_spheroid
{

public:
/*
 * formula based on paper
 *   [Wood96] Wood - Vertex Latitudes on Ellipsoid Geodesics, SIAM Rev., 38(4),
 *            637–644, 1996
    template <typename T1, typename T2, typename Spheroid>
    static inline CT apply(T1 const& lat1,
                           T2 const& alp1,
                           Spheroid const& spheroid)
    {
        CT const f = formula::flattening<CT>(spheroid);

        CT const e2 = f * (CT(2) - f);
        CT const sin_alp1 = sin(alp1);
        CT const sin2_lat1 = math::sqr(sin(lat1));
        CT const cos2_lat1 = CT(1) - sin2_lat1;

        CT const e2_sin2 = CT(1) - e2 * sin2_lat1;
        CT const cos2_sin2 = cos2_lat1 * math::sqr(sin_alp1);
        CT const vertex_lat = std::asin( math::sqrt((e2_sin2 - cos2_sin2)
                                                    / (e2_sin2 - e2 * cos2_sin2)));
        return vertex_lat;
    }
*/

    // simpler formula based on Clairaut relation for spheroids
    template <typename T1, typename T2, typename Spheroid>
    static inline CT apply(T1 const& lat1,
                           T2 const& alp1,
                           Spheroid const& spheroid)
    {
        CT const f = formula::flattening<CT>(spheroid);

        CT const one_minus_f = (CT(1) - f);

        //get the reduced latitude
        CT const bet1 = atan( one_minus_f * tan(lat1) );

        //apply Clairaut relation
        CT const betv =  vertex_latitude_on_sphere<CT>::apply(bet1, alp1);

        //return the spheroid latitude
        return atan( tan(betv) / one_minus_f );
    }

    /*
    template <typename T>
    inline static void sign_adjustment(CT lat1, CT lat2, CT vertex_lat, T& vrt_result)
    {
        // signbit returns a non-zero value (true) if the sign is negative;
        // and zero (false) otherwise.
        bool sign = std::signbit(std::abs(lat1) > std::abs(lat2) ? lat1 : lat2);

        vrt_result.north = sign ? std::max(lat1, lat2) : vertex_lat;
        vrt_result.south = sign ? vertex_lat * CT(-1) : std::min(lat1, lat2);
    }

    template <typename T>
    inline static bool vertex_on_segment(CT alp1, CT alp2, CT lat1, CT lat2, T& vrt_result)
    {
        CT const half_pi = math::pi<CT>() / CT(2);

        // if the segment does not contain the vertex of the geodesic
        // then return the endpoint of max (min) latitude
        if ((alp1 < half_pi && alp2 < half_pi)
                || (alp1 > half_pi && alp2 > half_pi))
        {
            vrt_result.north = std::max(lat1, lat2);
            vrt_result.south = std::min(lat1, lat2);
            return false;
        }
        return true;
    }
    */
};

template <typename CT, typename CS_Tag>
struct vertex_latitude
{
    BOOST_MPL_ASSERT_MSG
         (
             false, NOT_IMPLEMENTED_FOR_THIS_COORDINATE_SYSTEM, (types<CS_Tag>)
         );

};

template <typename CT>
struct vertex_latitude<CT, spherical_equatorial_tag>
        : vertex_latitude_on_sphere<CT>
{};

template <typename CT>
struct vertex_latitude<CT, geographic_tag>
        : vertex_latitude_on_spheroid<CT>
{};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_MAXIMUM_LATITUDE_HPP


//=======================================================================
// Copyright (c) 2013 Robert Rosolek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file fractional_winner_determination_in_MUCA_test_utils.hpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2014-06-12
 */
#ifndef PAAL_FRACTIONAL_WINNER_DETERMINATION_IN_MUCA_TEST_UTILS_HPP
#define PAAL_FRACTIONAL_WINNER_DETERMINATION_IN_MUCA_TEST_UTILS_HPP

#include "paal/auctions/auction_components.hpp"
#include "paal/auctions/auction_traits.hpp"
#include "paal/auctions/fractional_winner_determination_in_MUCA/fractional_winner_determination_in_MUCA.hpp"
#include "paal/auctions/xor_bids.hpp"
#include "paal/lp/glp.hpp"
#include "paal/utils/make.hpp"
#include "paal/utils/type_functions.hpp"

#include <boost/function_output_iterator.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/test/unit_test.hpp>

#include <iterator>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <utility>

template <
   class Bidders,
   class Items,
   class GetBids,
   class GetValue,
   class GetItems,
   class GetCopiesNum,
   class ItemToLpIdMap
>
void check_fractional_determine_winners_in_demand_query_auction(
   Bidders&& bidders,
   Items&& items,
   GetBids get_bids,
   GetValue get_value,
   GetItems get_items,
   GetCopiesNum get_copies_num,
   double opt,
   ItemToLpIdMap item_to_id,
   double eps
) {

    namespace pa = paal::auctions;

    auto auction = pa::make_xor_bids_to_demand_query_auction(
            std::forward<Bidders>(bidders),
            std::forward<Items>(items),
            get_bids,
            get_value,
            get_items,
            get_copies_num
            );

    using Traits = pa::demand_query_auction_traits<decltype(auction)>;
    using Bidder = typename Traits::bidder_val_t;
    using Item = typename Traits::item_val_t;
    using ItemsBundle = typename Traits::items_t;
    using Assignment = std::tuple<Bidder, ItemsBundle, double>;

    auto valuation = pa::make_xor_bids_to_value_query_auction(
            // TODO these arguments are copy paste, maybe we need xor_bids_auction_components?
            std::forward<Bidders>(bidders),
            std::forward<Items>(items),
            get_bids,
            get_value,
            get_items,
            get_copies_num
            );

    double social_welfare = 0;
    std::unordered_map<Bidder, double> bidder_count;
    std::unordered_map<Item, double> item_count;
    pa::fractional_determine_winners_in_demand_query_auction(
            auction,
            boost::iterators::make_function_output_iterator([&](Assignment a)
            {
                auto bidder = std::get<0>(a);
                auto& items = std::get<1>(a);
                auto frac = std::get<2>(a);
                auto item_set = paal::make_unordered_set(items);
                social_welfare += frac * valuation.template call<pa::value_query>(bidder, item_set);
                for (auto const & item: items) {
                    auto cnt = item_count[item] += frac;
                    BOOST_CHECK_LE(cnt, get_copies_num(item) + eps);
                }
                auto cnt = bidder_count[bidder] += frac;
                BOOST_CHECK_LE(cnt, 1 + eps);
            }),
            item_to_id,
            eps
            );

    BOOST_CHECK_CLOSE(opt, social_welfare, eps);
}

template<
class Bidders,
      class Items,
      class GetBids,
      class GetValue,
      class GetItems,
      class GetCopiesNum
      >
void check_fractional_determine_winners_in_demand_query_auction(
        Bidders&& bidders,
        Items&& items,
        GetBids get_bids,
        GetValue get_value,
        GetItems get_items,
        GetCopiesNum get_copies_num,
        double opt,
        double epsilon = 1e-8
        ) {
    using ItemVal = paal::range_to_elem_t<Items>;
    using PriceMap = std::unordered_map<ItemVal, paal::lp::col_id>;

    PriceMap map;
    check_fractional_determine_winners_in_demand_query_auction(
            std::forward<Bidders>(bidders),
            std::forward<Items>(items),
            get_bids,
            get_value,
            get_items,
            get_copies_num,
            opt,
            boost::make_assoc_property_map(map),
            epsilon
            );
}

#endif /* PAAL_FRACTIONAL_WINNER_DETERMINATION_IN_MUCA_TEST_UTILS_HPP */


//==================================================================================================
/*!
  @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_TWO_PROD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_TWO_PROD_HPP_INCLUDED

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

  /*!

    @ingroup group-arithmetic
    This function object computes two reals of the type of the inputs
    (in an std::pair)  @c r0 and @c r1 such that:

    @code
    r0 = x * y
    r1 = r0 -(x * y)
    @endcode

    using perfect arithmetic.

    Its main usage is to be able to compute
    prod of reals and the residual error using IEEE 754 arithmetic.

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

    @par Example:

      @snippet two_prod.cpp two_prod

    @par Possible output:

      @snippet two_prod.txt two_prod

  **/
  std::pair<Value, Value> two_prod(Value const& x, Value const& y);

} }
#endif

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

#endif


#include <mi4/Kdtree.hpp>
#include <Eigen/Dense>
#include <vector>
#include <iostream>
int main ( int argc, char** argv )
{

        std::vector<Eigen::Vector3d> p;

        for ( int i = 0 ; i < 10000 ; i++ ) {
                double x = ( rand() % 2001 - 1000 ) * 0.001;
                double y = ( rand() % 2001 - 1000 ) * 0.001;
                double z = ( rand() % 2001 - 1000 ) * 0.001;
                p.push_back ( Eigen::Vector3d ( x, y, z ) );
        }

        mi4::Kdtree<Eigen::Vector3d> kdtree ( p ) ;

        Eigen::Vector3d v ( 0.1, 0.1, 0.1 ) ;
        std::list<Eigen::Vector3d> result;

        kdtree.find ( v, 0.1, result, true );

        for ( std::list<Eigen::Vector3d>::iterator iter = result.begin(); iter != result.end() ; ++iter ) {
                Eigen::Vector3d& v0 = *iter;
                Eigen::Vector3d d = *iter - v;
                double sqrtDist = d.x() * d.x() + d.y() * d.y() + d.z() * d.z();
                std::cerr << v0.x() << " " << v0.y() << " " << v0.z() << "\t " << sqrtDist << std::endl;
        }

        return 0;
}


/* boost random/additive_combine.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: additive_combine.hpp 60755 2010-03-22 00:45:06Z steven_watanabe $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_ADDITIVE_COMBINE_HPP
#define BOOST_RANDOM_ADDITIVE_COMBINE_HPP

#include <iostream>
#include <algorithm> // for std::min and std::max
#include <boost/config.hpp>
#include <boost/cstdint.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/linear_congruential.hpp>

namespace boost {
namespace random {

/**
 * An instantiation of class template \additive_combine model a
 * \pseudo_random_number_generator. It combines two multiplicative
 * \linear_congruential number generators, i.e. those with @c c = 0.
 * It is described in
 *
 *  @blockquote
 *  "Efficient and Portable Combined Random Number Generators", Pierre L'Ecuyer,
 *  Communications of the ACM, Vol. 31, No. 6, June 1988, pp. 742-749, 774
 *  @endblockquote
 *
 * The template parameters MLCG1 and MLCG2 shall denote two different
 * \linear_congruential number generators, each with c = 0. Each invocation
 * returns a random number X(n) := (MLCG1(n) - MLCG2(n)) mod (m1 - 1), where
 * m1 denotes the modulus of MLCG1.
 *
 * The template parameter @c val is the validation value checked by validation.
 */
template<class MLCG1, class MLCG2,
#ifndef BOOST_NO_DEPENDENT_TYPES_IN_TEMPLATE_VALUE_PARAMETERS
  typename MLCG1::result_type
#else
  int32_t
#endif
  val>
class additive_combine
{
public:
  typedef MLCG1 first_base;
  typedef MLCG2 second_base;
  typedef typename MLCG1::result_type result_type;
#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
  static const bool has_fixed_range = true;
  static const result_type min_value = 1;
  static const result_type max_value = MLCG1::max_value-1;
#else
  enum { has_fixed_range = false };
#endif
  /**
   * Returns: The smallest value that the generator can produce
   */
  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return 1; }
  /**
   * Returns: The largest value that the generator can produce
   */
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return (_mlcg1.max)()-1; }

  /**
   * Constructs an \additive_combine generator using the
   * default constructors of the two base generators.
   */
  additive_combine() : _mlcg1(), _mlcg2() { }
  /**
   * Constructs an \additive_combine generator, using aseed as
   * the constructor argument for both base generators.
   */
  explicit additive_combine(result_type aseed)
    : _mlcg1(aseed), _mlcg2(aseed) { }
  /**
   * Constructs an \additive_combine generator, using
   * @c seed1 and @c seed2 as the constructor argument to
   * the first and second base generators, respectively.
   */
  additive_combine(typename MLCG1::result_type seed1,
                   typename MLCG2::result_type seed2)
    : _mlcg1(seed1), _mlcg2(seed2) { }
  /**
   * Contructs an \additive_combine generator with
   * values from the range defined by the input iterators first
   * and last.  first will be modified to point to the element
   * after the last one used.
   *
   * Throws: @c std::invalid_argument if the input range is too small.
   *
   * Exception Safety: Basic
   */
  template<class It> additive_combine(It& first, It last)
    : _mlcg1(first, last), _mlcg2(first, last) { }

  /**
   * Seeds an \additive_combine generator using the default
   * seeds of the two base generators.
   */
  void seed()
  {
    _mlcg1.seed();
    _mlcg2.seed();
  }

  /**
   * Seeds an \additive_combine generator, using @c aseed as the
   * seed for both base generators.
   */
  void seed(result_type aseed)
  {
    _mlcg1.seed(aseed);
    _mlcg2.seed(aseed);
  }

  /**
   * Seeds an \additive_combine generator, using @c seed1 and @c seed2 as
   * the seeds to the first and second base generators, respectively.
   */
  void seed(typename MLCG1::result_type seed1,
            typename MLCG2::result_type seed2)
  {
    _mlcg1.seed(seed1);
    _mlcg2.seed(seed2);
  }

  /**
   * Seeds an \additive_combine generator with
   * values from the range defined by the input iterators first
   * and last.  first will be modified to point to the element
   * after the last one used.
   *
   * Throws: @c std::invalid_argument if the input range is too small.
   *
   * Exception Safety: Basic
   */
  template<class It> void seed(It& first, It last)
  {
    _mlcg1.seed(first, last);
    _mlcg2.seed(first, last);
  }

  /**
   * Returns: the next value of the generator
   */
  result_type operator()() {
    result_type z = _mlcg1() - _mlcg2();
    if(z < 1)
      z += MLCG1::modulus-1;
    return z;
  }

  static bool validation(result_type x) { return val == x; }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  /**
   * Writes the state of an \additive_combine generator to a @c
   * std::ostream.  The textual representation of an \additive_combine
   * generator is the textual representation of the first base
   * generator followed by the textual representation of the
   * second base generator.
   */
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const additive_combine& r)
  { os << r._mlcg1 << " " << r._mlcg2; return os; }

  /**
   * Reads the state of an \additive_combine generator from a
   * @c std::istream.
   */
  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, additive_combine& r)
  { is >> r._mlcg1 >> std::ws >> r._mlcg2; return is; }
#endif

  /**
   * Returns: true iff the two \additive_combine generators will
   * produce the same sequence of values.
   */
  friend bool operator==(const additive_combine& x, const additive_combine& y)
  { return x._mlcg1 == y._mlcg1 && x._mlcg2 == y._mlcg2; }
  /**
   * Returns: true iff the two \additive_combine generators will
   * produce different sequences of values.
   */
  friend bool operator!=(const additive_combine& x, const additive_combine& y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const additive_combine& rhs) const
  { return _mlcg1 == rhs._mlcg1 && _mlcg2 == rhs._mlcg2; }
  bool operator!=(const additive_combine& rhs) const
  { return !(*this == rhs); }
#endif

private:
  MLCG1 _mlcg1;
  MLCG2 _mlcg2;
};

} // namespace random

/**
 * The specialization \ecuyer1988 was suggested in
 *
 *  @blockquote
 *  "Efficient and Portable Combined Random Number Generators", Pierre L'Ecuyer,
 *  Communications of the ACM, Vol. 31, No. 6, June 1988, pp. 742-749, 774
 *  @endblockquote
 */
typedef random::additive_combine<
    random::linear_congruential<int32_t, 40014, 0, 2147483563, 0>,
    random::linear_congruential<int32_t, 40692, 0, 2147483399, 0>,
  2060321752> ecuyer1988;

} // namespace boost

#endif // BOOST_RANDOM_ADDITIVE_COMBINE_HPP


//
// Copyright 1997, 1998, 1999 University of Notre Dame.
// Authors: Andrew Lumsdaine, Jeremy G. Siek, Lie-Quan Lee
//
// This file is part of the Matrix Template Library
//
// You should have received a copy of the License Agreement for the
// Matrix Template Library along with the software;  see the
// file LICENSE.  If not, contact Office of Research, University of Notre
// Dame, Notre Dame, IN  46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//

#include <iostream>
#include <mtl/matrix.h>

int
main()
{
#ifdef MTL_DISABLE_BLOCKING
  std::cout << "Static blocking unsupported for this compiler" << std::endl;
#else
  using namespace mtl;
  //begin
  const int M = 4;
  const int N = 4;
  typedef matrix<double,
                 rectangle<>,
                 dense<>,
                 column_major >::type Matrix;
  Matrix A(M,N);

  for (int i = 0; i < M; ++i)
    for (int j = 0; j < N; ++j)
      A(i, j) = i * N + j;
  print_all_matrix(A);

  block_view<Matrix,2,2>::type
                     bA = blocked(A, blk<2,2>());
  print_partitioned_matrix(bA);

  block_view<Matrix>::type cA = blocked(A, 2, 2);
  print_partitioned_by_column(cA);
  //end
  return 0;
#endif
}


/*
 * pyramid.hpp
 *
 *  Created on: Apr 10, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   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.
 */

#pragma once

//Standard library
#include <vector>
#include <memory>

//Libraries
#include <Eigen/Dense>

//local
#include "../../math/resampling.hpp"

namespace nonrigid_optimization {
namespace hierarchical{

/**
 * A pyramid representation of a discrete scalar field
 */
template <typename Container>
class Pyramid{
public:
	Pyramid(Container field, int maximum_chunk_size=8, math::DownsamplingStrategy downsampling_strategy
			= math::DownsamplingStrategy::AVERAGE);

	const Container& get_level(int i_level) const;
	size_t get_level_count() const;
private:
	std::vector<Container> levels;
};

} //namespace hierarchical
} //namespace nonrigid_optimization


#include <vector>
#include <iostream>
#include <eigen3/Eigen/Sparse>
#include <eigen3/Eigen/IterativeLinearSolvers>
// #include <eigen3/Eigen/Core>
// #include <eigen3/Eigen/SparseCore>
// #include <Eigen/SparseCholesky>
// #include<Eigen/SparseLU>
// #include<Eigen/SparseQR>
// #include <Eigen/IterativeLinearSolvers>

using namespace std;
using namespace Eigen;

int main(){
    typedef Eigen::Triplet<double> T;
    typedef Eigen::SparseMatrix<double> SpMat;

    Eigen::SparseMatrix<std::complex<float> > complex_mat(1000,2000);
    Eigen::SparseMatrix<double> mat(1000,2000);

    cout << "Sparse matrix sizes" << endl;
    cout << mat.rows();
    cout << mat.cols();
    cout << mat.innerSize();
    cout << mat.outerSize();
    cout << mat.nonZeros();

    // use triplet to fill in sparse matrix
    vector<T> triplet_list;
    for (int i = 0;i<1000;i=i+100){
        triplet_list.push_back(T(i, i, i));
    }

    mat.setFromTriplets(triplet_list.begin(), triplet_list.end());

    cout << "Sparse Matrix entries after initialization" << endl;
    for (int k=0; k<mat.outerSize(); ++k)
        for (SparseMatrix<double>::InnerIterator it(mat,k); it; ++it)
        {
            cout << it.value() << endl;
            cout << it.row() << endl;   // row index
            cout << it.col() << endl;   // col index (here it is equal to k)
            cout << it.index() << endl; // inner index, here it is equal to it.row()
        }

    // Supported operators and functions
    // sparse matrices cannot offer the same level of flexibility than dense matrices
    mat.transpose();
    mat.adjoint();

    mat.pruned();
    double ref = 1e-8;
    mat.pruned(ref);

    // // Eigen Sparse matrix solver
    // SparseMatrix<double> A;
    // // fill A
    // VectorXd b, x;
    // // fill b
    // // solve Ax = b
    // ConjugateGradient<SparseMatrix<double> > solver;
    // solver.compute(A);
    // if(solver.info()!=Success) {
    // // decomposition failed
    // return;
    // }
    // x = solver.solve(b);
    // if(solver.info()!=Success) {
    // // solving failed
    // return;
    // }
    // // solve for another right hand side:
    // x = solver.solve(b);

    return 0;
}


#ifndef ALEPH_GEOMETRY_TANGENT_SPACE_HH__
#define ALEPH_GEOMETRY_TANGENT_SPACE_HH__

#include <aleph/config/Eigen.hh>

#include <aleph/geometry/BruteForce.hh>
#include <aleph/geometry/FLANN.hh>

#include <aleph/geometry/distances/Euclidean.hh>

#include <aleph/math/AlgebraicSphere.hh>
#include <aleph/math/KahanSummation.hh>

#ifdef ALEPH_WITH_EIGEN
  #include <Eigen/Core>
  #include <Eigen/Cholesky>
  #include <Eigen/Eigenvalues>
  #include <Eigen/SVD>
#endif

#include <cassert>

#include <set>
#include <vector>

namespace aleph
{

namespace geometry
{

namespace detail
{

/**
  Model of a smooth decreasing weight function according to the
  original paper *Algebraic Point Set Surfaces* by Guennebaud &
  Gross.
*/

template <class T> T phi( T x )
{
  return x < 1 ? std::pow( 1 - x*x, T(4) ) : T();
}

} // namespace detail

#ifdef ALEPH_WITH_EIGEN

// Previous versions of Eigen have a bug that occurs when mixing dynamic
// and fixed-sized vectors:
//
//   http://eigen.tuxfamily.org/bz/show_bug.cgi?id=654
//
// Until a workaround has been identified, tangent space estimation will
// not be enabled for older versions.
#if EIGEN_VERSION_AT_LEAST(3,3,0)

class TangentSpace
{
public:
  using T        = double;

  using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
  using Vector = Eigen::Matrix<T, 1, Eigen::Dynamic>;

  using Sphere = math::AlgebraicSphere<T>;

#if EIGEN_VERSION_AT_LEAST(3,3,0)
  using Index = Eigen::Index;
#else
  using Index = typename Matrix::Index;
#endif

  struct LocalTangentSpace
  {
    Matrix tangents;
    Vector normal;
    Vector position;

    T localFeatureSize;
    std::vector<std::size_t> indices;
  };

  template <class Container> std::vector<T> operator()( const Container& container, unsigned k )
  {
    std::vector<double> curvature;
    curvature.reserve( container.size() );

    auto lts     = localTangentSpaces( container, k );
    auto spheres = fitSpheresWithoutNormals( container, lts );

    std::transform( spheres.begin(), spheres.end(), std::back_inserter( curvature ),
      [] ( const Sphere& sphere )
      {
        return sphere.meanCurvature();
      }
    );

    return curvature;
  }

private:

  /**
    Given a container and a number for determining the local
    neighbourhood of points, this function estimates (!) the
    tangent space structure around every point, resulting in
    a set of normal vectors and tangent vectors.

    @param container Container
    @param k         Local neighbourhood size
  */

  template <class Container> std::vector<LocalTangentSpace> localTangentSpaces( const Container& container, unsigned k )
  {
    using ElementType = typename Container::ElementType;
    using Distance    = distances::Euclidean<ElementType>;

#ifdef ALEPH_WITH_FLANN
    using NearestNeighbours = FLANN<Container, Distance>;
#else
    using NearestNeighbours = BruteForce<Container, Distance>;
#endif

    NearestNeighbours nearestNeighbours( container );
    using IndexType = typename NearestNeighbours::IndexType;

    std::vector< std::vector<IndexType> > indices;
    std::vector< std::vector<ElementType> > distances;

    nearestNeighbours.neighbourSearch( k,
                                       indices,
                                       distances );

    std::vector<LocalTangentSpace> localTangentSpaces;

    auto n = container.size();
    auto d = container.dimension();

    for( std::size_t i = 0; i < n; i++ )
    {
      // This coordinate matrix will contain the differences to the
      // centroid coordinate. The matrix will be transformed via an
      // SVD.
      Matrix M = Matrix::Zero( Index(k), Index(d) );

      // Centroid calculation ------------------------------------------

      Vector centroid  = Vector::Zero(1, Index(d) );

      for( std::size_t j = 0; j < indices[i].size(); j++ )
      {
        auto&& neighbourIndex = indices[i][j];
        auto v                = getPosition( container, neighbourIndex );

        centroid          += v;
        M.row( Index(j) )  = v;
      }

      centroid /= static_cast<T>( indices.size() );

      // Coordinate matrix setup ---------------------------------------

      M = M.rowwise() - centroid;

      Eigen::JacobiSVD<Matrix> svd( M, Eigen::ComputeThinV );

      LocalTangentSpace lts;
      lts.tangents = Matrix::Zero( Index(d), Index(d - 1) );

      // The singular vectors of all but the *smallest* singular value
      // form the tangential directions of the tangent space.

      auto&& V = svd.matrixV();

      // Actual or "effective" dimensionality of the input data. If the
      // matrix is rectangular (and most input matrices will be), there
      // is not a full system of singular vectors available.
      auto dEffective  = std::min( Index(d-1), Index(k-1) );

      for( Index j = 0; j < dEffective; j++ )
        lts.tangents.col(j) = V.col(j);

      lts.normal           = V.col( dEffective  ).normalized();
      lts.position         = getPosition( container, i );
      lts.indices          = indices[i];

      // Take the *maximum distance* in which we can find all of the
      // neighbours as a *rough*  approximation to the local feature
      // size.
      lts.localFeatureSize
        = distances[i].empty() == false ?
            *std::max_element( distances[i].begin(), distances[i].end() )
          : T();

      localTangentSpaces.push_back( lts );

      // TODO:
      //
      // 1. Calculate raw approximation (reconstruction) error by
      //    assessing how well the space fits the original data
      //
      // 2. Make normal orientation consistent. I am unsure as to
      //    whether this will improve the results or not.
    }

    propagateOrientation( localTangentSpaces );
    return localTangentSpaces;
  }

  template <class Container>
    std::vector<Sphere> fitSpheres( const Container& container,
                                    const std::vector<LocalTangentSpace>& localTangentSpaces )
  {
    using namespace detail;

    std::vector<Sphere> spheres;
    spheres.reserve( container.size() );

    for( auto&& lts : localTangentSpaces )
    {
      auto d   = Index( container.dimension() );
      Matrix A = Matrix::Zero( d+2, d+2 );
      Vector b = Vector::Zero( 1,   d+2 );

      auto&& indices = lts.indices;

      // Pre-processing --------------------------------------------------
      //
      // Choose a value for the beta parameter, based on the weighted
      // neighbourhood sizes of *all* points. This requires iterating
      // over all points prior to calculating anything else.

      T beta = T();

      {
        std::vector<T> W;
        std::vector<T> H;
        W.reserve( container.size() );
        H.reserve( container.size() );

        for( auto&& index : indices )
        {
          auto&& neighbour = getPosition( container, index );
          W.emplace_back( phi( ( lts.position - neighbour ).norm() / lts.localFeatureSize ) );
          H.emplace_back( lts.localFeatureSize );
        }

        // Sum of weights *before* applying the local feature size
        // multiplier; we need to save this result because it will
        // be required below.
        auto ws = math::accumulate_kahan_sorted( W.begin(), W.end(), T() );

        // Apply weights to local feature size estimates; afterwards we
        // can finally obtain the scaling factor from this weighted sum
        for( std::size_t i = 0; i < W.size(); i++ )
          W[i] = W[i] * H[i];

        // TODO: make initial guess for beta (10e6) configurable?
        auto h = math::accumulate_kahan_sorted( W.begin(), W.end(), T() ) / ws;
        beta   = 10e6 * h * h;
      }

      Index k  = Index( indices.size() );
      Matrix W = Matrix::Zero( (d+1)*k, (d+1)*k );
      Matrix D = Matrix::Zero( (d+1)*k, (d+2)   );
      Matrix c = Matrix::Zero( (d+1)*k, 1       );

      {
        Index i = Index();

        for( auto&& index : indices )
        {
          auto neighbour       = getPosition( container, index );
          auto w               = phi( ( lts.position - neighbour ).norm() / lts.localFeatureSize );
          W( i*(d+1),i*(d+1) ) = w;

          for( Index(j) = 0; j < d; j++ )
          {
            assert( W( i*(d+1)+j+1, i*(d+1)+j+1 ) == 0 );
            W( i*(d+1)+j+1, i*(d+1)+j+1 ) = beta * w;
          }

          ++i;
        }
      }

      {
        Index i = Index();

        for( auto&& index : indices )
        {
          auto neighbour       = getPosition( container, index );
          D( i*(d+1), 0 )      = 1.0;

          for( Index(j) = 0; j < d; j++ )
            D( i*(d+1), j+1 ) = neighbour(j);

          D( i*(d+1), d+1) = neighbour * neighbour.transpose();

          for( Index(j) = 0; j < d; j++ )
          {
            D( i*(d+1)+j+1, j+1 ) = 1;
            D( i*(d+1)+j+1, d+1 ) = 2 * neighbour(j);
            c( i*(d+1)+j+1, 0   ) = localTangentSpaces.at(index).normal(j);
          }

          ++i;
        }
      }

      for( auto&& index : indices )
      {
        auto neighbour           = getPosition( container, index );
        auto squaredNeigbourNorm = neighbour.squaredNorm();
        auto w                   = phi( ( lts.position - neighbour ).norm() / lts.localFeatureSize );

        A(   0,   0) += w;
        A( d+1,   0) += w * squaredNeigbourNorm;
        A( d+1, d+1) += w * squaredNeigbourNorm * squaredNeigbourNorm;

        for( Index(i) = 1; i < d+1; i++ )
        {
          A(  i,   i) += w * ( neighbour(i-1)*neighbour(i-1) + 1 ) * beta;
          A(  i,   0) += w * ( neighbour(i-1) );
          A(d+1,   i) += w * ( neighbour(i-1)*squaredNeigbourNorm + 2 * beta * neighbour(i-1) );
          A(d+1, d+1) += w * ( 4*neighbour(i-1)*neighbour(i-1) ) * beta;

          // re-establish symmetry
          A(  0,   i)  = A(i,0);
          A(  i, d+1)  = A(d+1, i);

          b(i  ) +=       beta * w * localTangentSpaces.at(index).normal(i-1);
          b(d+1) += 2.0 * beta * w * localTangentSpaces.at(index).normal(i-1) * neighbour(i-1);

          for( Index(j) = i+1; j < d+1; j++ )
          {
            A(j, i) += w * neighbour(i-1) * neighbour(j-1);
            A(i, j)  = A(j,i);
          }
        }

        // re-establish symmetry
        A(0, d+1) = A(d+1, 0);
      }

      // Solve the linear system ---------------------------------------
      //
      // The solution of the system Ax = b is used to obtain the
      // coefficients of the algebraic sphere.

      using Solver = Eigen::LDLT<Matrix>;
      Solver solver(A);

      Vector u = solver.solve( b.transpose() );

      spheres.emplace_back( Sphere( u.data(), u.data() + u.size() ) );
    }

    return spheres;
  }

  template <class Container>
    std::vector<Sphere> fitSpheresWithoutNormals( const Container& container,
                                                  const std::vector<LocalTangentSpace>& localTangentSpaces )
  {
    using namespace detail;

    std::vector<Sphere> spheres;
    spheres.reserve( container.size() );

    for( auto&& lts : localTangentSpaces )
    {
      auto&& indices = lts.indices;
      auto d         = Index( container.dimension() );
      auto k         = Index( indices.size() );

      Matrix W = Matrix::Zero(k,k  );
      Matrix D = Matrix::Zero(k,d+2);
      Matrix C = Matrix::Identity(d+2,d+2);

      C(0  ,  0) =  0;
      C(0  ,d+1) = -2;
      C(d+1,  0) = -2;
      C(d+1,d+1) =  0;

      {
        Index i = Index();
        for( auto&& index : indices )
        {
          auto neighbour = getPosition( container, index );
          W(i,  i)       = phi( ( lts.position - neighbour ).norm() / lts.localFeatureSize );
          D(i,  0)       = 1;
          D(i,d+1)       = neighbour * neighbour.transpose();

          for( Index j = 0; j < d; j++ )
            D(i,j+1) = neighbour(j);

          ++i;
        }
      }

      // Solve the linear system ---------------------------------------
      //
      // The solution of the system Ax = b is used to obtain the
      // coefficients of the algebraic sphere.

      using Solver = Eigen::GeneralizedEigenSolver<Matrix>;
      Solver solver;
      solver.compute( D.transpose() * W * D, C );

      auto eigenvalues = solver.eigenvalues();
      Vector u         = Vector::Zero( 1, d+2 );

      for( Index i = 0; i < d+2; i++ )
      {
        if( eigenvalues(i).real() > 0 && eigenvalues(i).imag() == 0 )
        {
          u = solver.eigenvectors().real().col(i);
          break;
        }
      }

      spheres.emplace_back( Sphere( u.data(), u.data() + u.size() ) );
    }

    return spheres;
  }

  void propagateOrientation( std::vector<LocalTangentSpace>& localTangentSpaces )
  {
    using Edge = std::pair<std::size_t, std::size_t>;

    std::set<Edge> edges;
    for( std::size_t i = 0; i < localTangentSpaces.size(); i++ )
    {
      for( auto&& index : localTangentSpaces.at(i).indices )
      {
        if( i < index )
          edges.insert( std::make_pair(i,index) );
        else
          edges.insert( std::make_pair(index,i) );
      }
    }

    for( auto&& edge : edges )
    {
      auto i    = edge.first;
      auto&& ni = localTangentSpaces.at(i).normal;
      auto j    = edge.second;
      auto&& nj = localTangentSpaces.at(j).normal;

      if( ni * nj.transpose() < 0 )
        nj = -nj;
    }
  }

  /**
    Auxiliary function for extracting and converting a position from
    a given container, storing it as a (mathematical) vector.
  */

  template <class Container> Vector getPosition( const Container& container, std::size_t i )
  {
    auto d   = container.dimension();
    auto p   = container[i];
    Vector v = Vector::Zero(1, Index(d) );

    // copy (and transform!) the vector; there's an implicit type
    // conversion going on here
    for( std::size_t l = 0; l < d; l++ )
      v( Index(l) ) = p[l];

    return v;
  }
};

#endif

#endif

} // namespace geometry

} // namespace aleph

#endif


//==================================================================================================
/*!
  @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_ACSC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACSC_HPP_INCLUDED

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

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse cosecant in radian: \f$\arcsin(1/x)\f$.

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

    @see acscd, acscpi, asin, asin, sin, rec

    @par Example:

      @snippet acsc.cpp acsc

    @par Possible output:

      @snippet acsc.txt acsc

  **/
  IEEEValue acsc(IEEEValue const & x);
} }
#endif

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

#endif


/*!
 * @file     double_integrator.cpp
 * @author   Giuseppe Rizzi
 * @date     23.07.2020
 * @version  1.0
 * @brief    description
 */

#include <Eigen/Dense>
#include <array>
#include <chrono>
#include "mppi/controller/mppi.h"

using namespace Eigen;
using namespace mppi;

// Simple dynamics class representing a point mass subject to a force
class DoubleIntegratorDynamics : public DynamicsBase {
 public:
  DoubleIntegratorDynamics() {
    A << 0.0, 1.0, 0.0, 0.0;
    B << 0.0, 1.0;
    x << 0.0, 0.0;
  };
  ~DoubleIntegratorDynamics() = default;

  size_t get_input_dimension() override { return 1; }
  size_t get_state_dimension() override { return 2; }

  dynamics_ptr create() override { return mppi::DynamicsBase::dynamics_ptr(); }
  dynamics_ptr clone() const override {
    return std::make_shared<DoubleIntegratorDynamics>(*this);
  }

  void reset(const observation_t& xr) override { x = xr; }

  observation_t step(const input_t& u, const double dt) override {
    x += (A * x + B * u) * dt;
    return x;
  }

  // does not make sense, just to test this function is used properly in the
  // control loop
  input_t get_zero_input(const observation_t& x) override {
    return input_t::Ones(1) * x(0);
  }

 private:
  Eigen::Vector2d x;
  Eigen::Matrix<double, 2, 2> A;
  Eigen::Matrix<double, 2, 1> B;
};

// Simple cost class driving the point mass to 1
class DoubleIntegratorCost : public CostBase {
 public:
  DoubleIntegratorCost() = default;
  ~DoubleIntegratorCost() = default;
  cost_ptr create() override { return mppi::CostBase::cost_ptr(); }
  cost_ptr clone() const override { return mppi::CostBase::cost_ptr(); }
  cost_t compute_cost(const observation_t& x, const reference_t& r,
                      const double t) {
    return w * (x(0) - 1.0) * (x(0) - 1.0);
  }

 private:
  double w = 10;
};


/*
 * Copyright (c) Contributors to the Open 3D Engine Project.
 * For complete copyright and license terms please see the LICENSE at the root of this distribution.
 *
 * SPDX-License-Identifier: Apache-2.0 OR MIT
 *
 */

#include <NumericalMethods_precompiled.h>
#include <NumericalMethods/Eigenanalysis.h>
#include <NumericalMethods/Optimization.h>
#include <Optimization/SolverBFGS.h>
#include <Eigenanalysis/Solver3x3.h>

namespace NumericalMethods
{
    namespace Optimization
    {
        SolverResult SolverBFGS(const Function& function, const AZStd::vector<double>& initialGuess)
        {
            return MinimizeBFGS(function, initialGuess);
        }
    }

    namespace Eigenanalysis
    {
        SolverResult<Real, 3> Solver3x3RealSymmetric(const SquareMatrix<Real, 3>& matrix)
        {
            // The matrix must be symmetric.
            if (matrix[0][1] == matrix[1][0] && matrix[0][2] == matrix[2][0] && matrix[1][2] == matrix[2][1])
            {
                return NonIterativeSymmetricEigensolver3x3(
                    matrix[0][0], matrix[0][1], matrix[0][2],
                                  matrix[1][1], matrix[1][2],
                                                matrix[2][2]
                );
            }
            else
            {
                return SolverResult<Real, 3>{SolverOutcome::FailureInvalidInput};
            }
        }
    }
} // namespace NumericalMethods


#ifndef _REGISTRATION_HPP_
#define _REGISTRATION_HPP_

#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>

//その場での比較
Eigen::Matrix4f registration_icp(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src);
Eigen::Matrix4f registration_icp_I(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tgt, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_src);

Eigen::Matrix4f registration_icp_vis(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src,  pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);

Eigen::Matrix4f registration_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src);
Eigen::Matrix4f registration_ndt_vis(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);

//mapを元に算出
Eigen::Matrix4f map_icp_vis(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,nav_msgs::Odometry odo);

Eigen::Matrix4f map_ndt_vis(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,nav_msgs::Odometry odo);

#endif


#ifndef __TWIST_CONTROLLER__
#define __TWIST_CONTROLLER__

#include <kdl/kdl.hpp>
#include <kdl/frames.hpp>
#include <Eigen/Dense>
#include <iostream>

namespace cartesian_controllers{

/**
  Implements a pose controller that outputs a control twist.
**/
class TwistController{
public:
  TwistController(const Eigen::Matrix<double, 6, 1> &twist_gains);
  ~TwistController();

  /**
    Return a twist proportional to the pose error between the two given frames.

    @param current The present frame.
    @param reference The reference frame.
    @return A value proportional to the error "reference - current".
  **/
  KDL::Twist computeError(const KDL::Frame &current, const KDL::Frame &reference);
private:
  Eigen::Matrix<double, 6, 1> gains_;

  /**
    Computes the angle and axis rotation required to rotate v1 along v2.

    @param v1
    @param v2
    @return The angle axis rotation.
  **/
  Eigen::AngleAxisd getAngleAxis(const Eigen::Vector3d &v1, const Eigen::Vector3d &v2);
};
}

#endif


// Copyright (c) 2020 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <gsl/gsl_interp.h>
#include <Eigen/Core>
#include <functional>
#include <memory>
#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/gsl/accelerator.hpp"

namespace pyinterp::detail::gsl {

/// Interpolate a 1-D function
class Interpolate1D {
 public:
  /// Interpolate a 1-D function
  ///
  /// @param size Size of workspace
  /// @param type fitting model
  /// @param acc Accelerator
  Interpolate1D(const size_t size, const gsl_interp_type* type, Accelerator acc)
      : workspace_(
            std::unique_ptr<gsl_interp, std::function<void(gsl_interp*)>>(
                gsl_interp_alloc(type, size),
                [](gsl_interp* ptr) { gsl_interp_free(ptr); })),
        acc_(std::move(acc)) {}

  /// Returns the name of the interpolation type used
  [[nodiscard]] inline auto name() const noexcept -> std::string {
    return gsl_interp_name(workspace_.get());
  }

  /// Return the minimum number of points required by the interpolation
  [[nodiscard]] inline auto min_size() const noexcept -> size_t {
    return gsl_interp_min_size(workspace_.get());
  }

  /// Return the interpolated value of y for a given point x
  inline auto interpolate(const Eigen::VectorXd& xa, const Eigen::VectorXd& ya,
                          const double x) -> double {
    init(xa, ya);
    return gsl_interp_eval(workspace_.get(), xa.data(), ya.data(), x, acc_);
  }

  /// Return the derivative d of an interpolated function for a given point x
  inline auto derivative(const Eigen::VectorXd& xa, const Eigen::VectorXd& ya,
                         const double x) -> double {
    init(xa, ya);
    return gsl_interp_eval_deriv(workspace_.get(), xa.data(), ya.data(), x,
                                 acc_);
  }

  /// Return the second derivative d of an interpolated function for a given
  /// point x
  inline auto second_derivative(const Eigen::VectorXd& xa,
                                const Eigen::VectorXd& ya, const double x)
      -> double {
    init(xa, ya);
    return gsl_interp_eval_deriv2(workspace_.get(), xa.data(), ya.data(), x,
                                  acc_);
  }

  /// Return the numerical integral result of an interpolated function over the
  /// range [a, b],
  inline auto integral(const Eigen::VectorXd& xa, const Eigen::VectorXd& ya,
                       const double a, const double b) -> double {
    init(xa, ya);
    return gsl_interp_eval_integ(workspace_.get(), xa.data(), ya.data(), a, b,
                                 acc_);
  }

 private:
  std::unique_ptr<gsl_interp, std::function<void(gsl_interp*)>> workspace_;
  Accelerator acc_;

  /// Initializes the interpolation object
  void init(const Eigen::VectorXd& xa, const Eigen::VectorXd& ya) {
    acc_.reset();
    gsl_interp_init(workspace_.get(), xa.data(), ya.data(), xa.size());
  }
};

}  // namespace pyinterp::detail::gsl


// Copyright PinaPL
//
// cell.hpp
// PinaPL
//
#ifndef CELL_HPP
#define CELL_HPP

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

#include "weights.hpp"

class Cell {
 public:
    Weights* weights;
    std::vector<Eigen::MatrixXd> inputs;
    // std::vector<Eigen::MatrixXd> forget_gate_out;
    std::vector<Eigen::MatrixXd> input_gate_out;
    std::vector<Eigen::MatrixXd> input_block_out;
    std::vector<Eigen::MatrixXd> output_gate_out;
    std::vector<Eigen::MatrixXd> cell_state;
    std::vector<Eigen::MatrixXd> cell_out;

    std::vector<Eigen::MatrixXd> delta_cell_out;                // dy
    std::vector<Eigen::MatrixXd> delta_output_gate_out;         // do
    std::vector<Eigen::MatrixXd> delta_cell_state;              // dc
    // std::vector<Eigen::MatrixXd> delta_forget_gate_out;      // df
    std::vector<Eigen::MatrixXd> delta_input_gate_out;          // di
    std::vector<Eigen::MatrixXd> delta_input_block_out;         // dz

    explicit Cell(Weights* weights);
    void compute(Eigen::MatrixXd* input);
    Eigen::MatrixXd compute_gate_gradient(Eigen::MatrixXd* deltas, int time);
    void compute_weight_gradient();
    void update_weights(double lambda);
    void reset();
};
#endif


#include <boost/lexical_cast.hpp>

#include "Chi2.hh"

using namespace Eigen;

void Chi2::add(SingleOutput &theory, SingleOutput &data, SingleOutput &errors) {
  t_["chi2"].input(theory);
  t_["chi2"].input(data);
  t_["chi2"].input(errors);
}

void Chi2::checkTypes(TypesFunctionArgs fargs) {
  auto& args=fargs.args;
  auto& rets=fargs.rets;
  if (args.size()%3 != 0) {
    throw args.undefined();
  }
  for (size_t i = 0; i < args.size(); i+=3) {
    auto& theory = args[i+0];
    auto& data   = args[i+1];
    auto& errors = args[i+2];
    if (theory.shape.size() != 1) {
      throw rets.error(rets[0], "non-vector theory");
    }
    if (data.shape != theory.shape) {
      throw rets.error(rets[0], "data and theory have different shape");
    }

    switch(errors.shape.size()){
      case 1:
        /// Errors: uncorrelated uncertainties (first power)
        break;
      case 2:
        /// Errors: L - lower triangular decomposition of covariance matrix
        if (errors.shape[0] != errors.shape[1]) {
          throw rets.error(rets[0], "incompatible covmat shape");
        }
        break;
      default:
        throw rets.error(rets[0], "invalid dimension (errors input)");
        break;
    }

    if (errors.shape[0] != theory.shape[0]) {
      throw rets.error(rets[0], "errors are unconsistent with data");
    }
  }
  rets[0] = DataType().points().shape(1);
}

void Chi2::calculateChi2(FunctionArgs fargs) {
  auto& args=fargs.args;
  double res=0.0;
  for (size_t i = 0; i < args.size(); i+=3) {
    VectorXd diff = args[i+0].vec - args[i+1].vec;
    auto& errors = args[i+2];
    switch(errors.type.shape.size()){
      case 1:
        diff.array()/=errors.arr;
        break;
      case 2:
        errors.mat.triangularView<Eigen::Lower>().solveInPlace(diff);
        break;
      default:
        break;
    }
    res += diff.array().square().sum();
  }
  fargs.rets[0].arr(0)=res;
}


#define BOOST_TEST_MODULE lue core time point
#include <boost/test/unit_test.hpp>
#include "lue/core/time.hpp"

BOOST_AUTO_TEST_CASE(seconds)
{
    namespace ldm = lue::data_model;

    // Time point at 6 ticks in a clock with a tick period of 10 seconds,
    // so the point is located at 60 seconds
    std::size_t const nr_seconds_per_tick = 10;
    std::size_t const nr_ticks = 6;

    using TickPeriod = ldm::time::TickPeriod<ldm::time::Second>;
    using Clock = ldm::time::Clock<TickPeriod>;
    using Duration = Clock::Duration;
    using TimePoint = ldm::time::TimePoint<Clock>;

    TickPeriod tick_period{nr_seconds_per_tick};
    Clock clock{ldm::time::Epoch{}, tick_period};
    Duration duration{nr_ticks};
    TimePoint time_point{duration};

    BOOST_CHECK(time_point.duration() == duration);
}

BOOST_AUTO_TEST_CASE(days)
{
    namespace ldm = lue::data_model;

    // Types for tracking days
    using TickPeriod = ldm::time::TickPeriod<ldm::time::Day>;
    using Clock = ldm::time::Clock<TickPeriod>;
    using Duration = Clock::Duration;
    using TimePoint = ldm::time::TimePoint<Clock>;

    // Clock with a resolution of 2 days
    TickPeriod tick_period{2};
    Clock clock(ldm::time::Epoch{}, tick_period);

    // Select day 6 (3 * 2 days)
    Duration duration{3};
    TimePoint day_6{duration};

    BOOST_CHECK_EQUAL(clock.nr_units(day_6), 6);
}


//#include <lapacke.h>
#include <stdio.h>
#include <Eigen/Dense>
#include <stdexcept>
#include <iostream>

char lower = 'L';

// TODO, remove dependency on lapacke.h
extern "C" void dpotrf_(char *uplo, int *n, double *a, int *lda, int *info);
extern "C" void dpotrs_(char *uplo, int* n, int* nrhs, double* A, int* lda, double* B, int* ldb, int* info);

void chol_decomp(double* A, int n) {
  int info;
  dpotrf_(&lower, &n, A, &n, &info);
  if(info != 0){ throw std::runtime_error("c++ error: Cholesky decomp failed"); }
}

void chol_decomp(Eigen::MatrixXd & A) {
  int info, n = A.rows();
  dpotrf_(&lower, &n, A.data(), &n, &info);
  if(info != 0) {
    throw std::runtime_error(
        std::string("c++ error: Cholesky decomp failed (for ")
        + std::to_string(n)
        + " x "
        + std::to_string(n)
        + " eigen matrix)");
  }
}

void chol_solve(double* A, int n, double* B, int nrhs) {
  int info;
  dpotrs_(&lower, &n, &nrhs, A, &n, B, &n, &info);
  if(info != 0){ throw std::runtime_error("c++ error: Cholesky solve failed");}
}

void chol_solve(Eigen::MatrixXd & A, Eigen::MatrixXd & B) {
  if (A.rows() != B.rows()) {throw std::runtime_error("A.rows() must equal B.rows()");}
  int info;
  int n    = A.rows();
  int nrhs = B.cols();
  dpotrs_(&lower, &n, &nrhs, A.data(), &n, B.data(), &n, &info);
  if(info != 0){ throw std::runtime_error("c++ error: Cholesky solve failed (for eigen matrix)");}
}

/** solves A * X' = B' for X in place */
void chol_solve_t(Eigen::MatrixXd & A, Eigen::MatrixXd & B) {
  if (A.rows() != B.cols()) {throw std::runtime_error("A.rows() must equal B.cols()");}
  B.transposeInPlace();
  chol_solve(A, B);
  B.transposeInPlace();
}