#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>

#include "simulator.hpp"
#include "spdlog/spdlog.h"

using namespace SnowSimulator;

Simulator::Simulator(MaterialPoints &materialPoints, Grid *grid,
                     SnowModel &snowModel,
                     std::vector<CollisionObject *> colliders)
    : m_materialPoints(materialPoints), m_grid(grid), m_snowModel(snowModel),
      m_colliders(colliders) {
  m_stepCount = 0;
  logger = spdlog::get("snowsim");
}

/**
 * The simulation proceeds as follows:
 *
 * 1. Rasterize particle data to the grid (rasterizeParticlesToGrid)
 * 2. Compute particle volumes and densities (setParticleVolumesAndDensities)
 *    NOTE: This is done only on the first timestep!
 * 3. Compute grid forces (computeGridForces)
 * 4. Update velocities on grid to v_i' (updateGridVelocities)
 * 5. Grid-based body collisions (detectGridCollisions)
 * 6. Solve linear system for semi-implicit integration (solveLinearSystem)
 * 7. Update deformation gradient (updateDeformationGradient)
 * 8. Update particle velocities (updateParticleVelocities)
 * 9. Particle-based body collisions (detectParticleCollisions)
 * 10. Update particle positions (updateParticlePostions)
 **/
void Simulator::advance(double delta_t) {
  logger->info("[Beginning step {} -> advancing by {}]", stepCount(), delta_t);

  rasterizeParticlesToGrid();

  // On the first timestep, we need to compute particle volumes and densities

  if (m_stepCount == 0) {
    setParticleVolumesAndDensities();
  }

  computeGridForces();
  updateGridVelocities(delta_t);

  detectGridCollisions(delta_t);

  explicitIntegration();
  // solveLinearSystem();

  updateDeformationGradient(delta_t);
  updateParticleVelocities(delta_t);
  detectParticleCollisions(delta_t);
  updateParticlePositions(delta_t);

  m_stepCount++;
}

void Simulator::rasterizeParticlesToGrid() {
  // Reset all node properties

  for (auto &node : m_grid->nodes()) {
    node->m_mass = 0;
    node->m_velocity.setZero();
    node->m_nextVelocity.setZero();
    node->m_velocityStar.setZero();
    node->m_force.setZero();
  }

  // Place all particles in their updated cells

  // TODO(kvchen): Comment this out once we fix this bug

  // for (auto &mp : m_materialPoints.particles()) {
  //   Vector3f idx = (mp->m_position.array() / m_grid->m_spacing).floor();
  //   int i = m_grid->vectorToIdx(idx.cast<int>());
  //
  //   if (i < 0 || i >= m_grid->m_gridCells.size()) {
  //     logger->error("Particle index {} exceeds grid boundaries", i);
  //     std::cout << *mp << std::endl;
  //
  //     logger->error("Other particles in the same cell as invalid particle:");
  //     for (auto &other : mp->cell()->particles()) {
  //       if (other == mp) {
  //         continue;
  //       }
  //       logger->error("Distance to invalid particle: {}",
  //                     (other->position() - mp->position()).norm());
  //       std::cout << *other << std::endl;
  //     }
  //   }
  // }

  for (auto &cell : m_grid->cells()) {
    cell->clear();
  }

  for (auto &mp : m_materialPoints.particles()) {
    int idx = m_grid->getParticleIdx(mp);
    if (idx < 0 || idx >= m_grid->m_gridCells.size()) {
      logger->error("Particle index {} exceeds grid boundaries", idx);
      std::cout << *mp << std::endl;
    }

    m_grid->cells()[idx]->addMaterialPoint(mp);
  }

  for (auto &mp : m_materialPoints.particles()) {
    m_grid->forEachNeighbor(mp, [&mp](GridNode *node) {
      double w = node->basisFunction(mp->position());
      node->m_mass += mp->mass() * w;
      node->m_velocity += mp->velocity() * mp->mass() * w;
    });
  }

  for (auto &node : m_grid->nodes()) {
    if (node->m_mass > 0) {
      node->m_velocity /= node->m_mass;
    }
  }
}

void Simulator::setParticleVolumesAndDensities() {
  logger->info("Setting initial particle volumes and densities");

  for (auto const &mp : m_materialPoints.particles()) {
    mp->m_volume = 0;
    mp->m_density = 0;

    m_grid->forEachNeighbor(mp, [&](GridNode *node) {
      mp->m_density += node->m_mass * node->basisFunction(mp->m_position) /
                       pow(m_grid->m_spacing, 3);
    });

    if (mp->m_density != 0) {
      mp->m_volume = mp->m_mass / mp->m_density;
    }
  }
}

void Simulator::computeGridForces() {
  for (auto &mp : m_materialPoints.particles()) {
    JacobiSVD<Matrix3f> svd(mp->m_defElastic, ComputeFullU | ComputeFullV);

    double Jp = mp->m_defPlastic.determinant();
    double Je = mp->m_defElastic.determinant();

    Matrix3f Re = svd.matrixU() * svd.matrixV().transpose();

    double epsilon =
        exp(fmin(m_snowModel.hardeningCoefficient * (1 - Jp), 1e3));

    // Compute the Cauchy stress
    // mu * 2 * (fe - re)_f

    Matrix3f stress =
        epsilon *
        (2 * m_snowModel.initialMu * (mp->m_defElastic - Re) *
             mp->m_defElastic.transpose() +
         Matrix3f::Identity() * (m_snowModel.initialLambda * (Je - 1) * Je));

    m_grid->forEachNeighbor(mp, [mp, &stress](GridNode *node) {
      node->m_force -=
          mp->m_volume * stress * node->gradBasisFunction(mp->m_position);
    });
  }
}

void Simulator::updateGridVelocities(double delta_t) {
  for (auto &node : m_grid->getAllNodes()) {
    if (node->m_mass > 0) {
      node->m_nextVelocity =
          node->m_velocity + delta_t * node->m_force / node->m_mass;
    }

    // Add gravitational forces

    node->m_nextVelocity += Vector3f(0, -9.8, 0) * delta_t;
  }
}

void Simulator::detectGridCollisions(double delta_t) {
  for (auto &node : m_grid->getAllNodes()) {
    node->m_velocityStar = node->m_nextVelocity;

    for (auto &collider : m_colliders) {
      Vector3f position = node->position();
      if (collider->phi(position) > 0) {
        continue;
      }

      Vector3f normal = collider->normal(position);
      Vector3f relVelocity = node->m_velocityStar - collider->velocity();
      double magnitude = relVelocity.dot(normal);

      if (magnitude >= 0) {
        continue;
      }

      Vector3f tangentialVelocity = relVelocity - magnitude * normal;

      // Only apply dynamic friction if the tangential velocity is large
      // compared to the normal

      double normComponent = collider->friction() * magnitude;
      double tangentNorm = tangentialVelocity.norm();

      if (collider->sticky() || tangentNorm <= -normComponent) {
        relVelocity.setZero();
      } else {
        relVelocity = tangentialVelocity * (1 + normComponent / tangentNorm);
      }

      node->m_velocityStar = relVelocity + collider->velocity();
    }
  }
}

void Simulator::explicitIntegration() {
  for (auto &node : m_grid->getAllNodes()) {
    node->m_nextVelocity = node->m_velocityStar;
    // std::cout << node->m_nextVelocity << std::endl;
  }
}

void Simulator::updateDeformationGradient(double delta_t) {
  for (auto &mp : m_materialPoints.particles()) {
    Matrix3f gradVelocity = Matrix3f::Identity();
    m_grid->forEachNeighbor(mp, [mp, delta_t, &gradVelocity](GridNode *node) {
      gradVelocity += delta_t * node->nextVelocity() *
                      node->gradBasisFunction(mp->m_position).transpose();
    });

    Matrix3f defNext = gradVelocity * mp->m_defElastic * mp->m_defPlastic;
    mp->m_defElastic = gradVelocity * mp->m_defElastic;

    // Push deformations exceeding critical deformation thresholds into Fp

    JacobiSVD<Matrix3f> svd(mp->m_defElastic, ComputeFullU | ComputeFullV);
    Matrix3f sigma = svd.singularValues()
                         .cwiseMax(1 - m_snowModel.criticalCompression)
                         .cwiseMin(1 + m_snowModel.criticalStretch)
                         .asDiagonal();

    mp->m_defPlastic =
        svd.matrixV() * sigma.inverse() * svd.matrixU().transpose() * defNext;
    mp->m_defElastic = svd.matrixU() * sigma * svd.matrixV().transpose();
    if (!defNext.isApprox(mp->m_defElastic * mp->m_defPlastic)) {
      logger->warn("Deformation update is incorrect");
    }
  }
}

void Simulator::updateParticleVelocities(double delta_t, float alpha) {
  for (auto &mp : m_materialPoints.particles()) {
    Vector3f velocityPIC = Vector3f::Zero();
    Vector3f velocityFLIP = mp->m_velocity;

    m_grid->forEachNeighbor(
        mp, [mp, &velocityPIC, &velocityFLIP](GridNode *node) {
          float weight = node->basisFunction(mp->m_position);

          velocityPIC += node->m_nextVelocity * weight;
          velocityFLIP += (node->m_nextVelocity - node->m_velocity) * weight;
        });

    mp->m_velocity = (1 - alpha) * velocityPIC + alpha * velocityFLIP;
  }
}

void Simulator::detectParticleCollisions(double delta_t) {
  for (auto &mp : m_materialPoints.particles()) {
    for (auto &collider : m_colliders) {
      Vector3f position = mp->m_position;
      if (collider->phi(position) > 0) {
        continue;
      }

      // std::cout << mp->position() << std::endl;

      Vector3f normal = collider->normal(position);
      Vector3f relVelocity = mp->velocity() - collider->velocity();

      // std::cout << collider->velocity() << std::endl;

      double magnitude = relVelocity.dot(normal);
      if (magnitude >= 0) {
        continue;
      }

      // std::cout << "GOT HERE" << std::endl;

      Vector3f tangentialVelocity = relVelocity - magnitude * normal;

      // Only apply dynamic friction if the tangential velocity is large
      // compared to the normal

      double normComponent = collider->friction() * magnitude;
      double tangentNorm = tangentialVelocity.norm();

      if (collider->sticky() || tangentNorm <= -normComponent) {
        relVelocity.setZero();
      } else {
        relVelocity = tangentialVelocity * (1 + normComponent / tangentNorm);
      }

      mp->m_velocity = relVelocity + collider->velocity();
    }
  }
}

/**
 * Uses backwards Euler integration to update the particle position for each
 * delta_t. At this point, the velocities should already have been updated
 * to the next delta_t.
 */
void Simulator::updateParticlePositions(double delta_t) {
  for (auto &mp : m_materialPoints.particles()) {
    mp->m_position += delta_t * mp->m_velocity;

    // mp->m_position = mp->m_position.array().min(
    //     (m_grid->m_dim.array()).cast<float>() * m_grid->m_spacing);
    // mp->m_position = mp->m_position.cwiseMax(0);
  }
}

std::string Simulator::exportVolumeData() {
  std::stringstream ss;
  ss << "../volumes/density-" << std::setfill('0') << std::setw(6)
     << m_stepCount << ".vol";

  std::ofstream os(ss.str());

  Vector3i dim = m_grid->dim();
  Vector3f bboxMin = m_grid->origin();
  Vector3f bboxMax = bboxMin + m_grid->extent();

  bboxMin = Vector3f(-0.5, -0.5, -0.5);
  bboxMax = Vector3f(0.5, 0.5, 0.5);

  os.write("VOL", 3);

  char version = 3;
  os.write((char *)&version, sizeof(char));

  int value = 1;
  os.write((char *)&value, sizeof(int));

  os.write((char *)&dim.x(), sizeof(int));
  os.write((char *)&dim.y(), sizeof(int));
  os.write((char *)&dim.z(), sizeof(int));

  value = 1;
  os.write((char *)&value, sizeof(int));

  os.write((char *)&bboxMin.x(), sizeof(float));
  os.write((char *)&bboxMin.y(), sizeof(float));
  os.write((char *)&bboxMin.z(), sizeof(float));

  os.write((char *)&bboxMax.x(), sizeof(float));
  os.write((char *)&bboxMax.y(), sizeof(float));
  os.write((char *)&bboxMax.z(), sizeof(float));

  auto nodes = m_grid->nodes();
  for (unsigned i = nodes.size(); i-- > 0;) {
    auto node = nodes[i];
    float value = (float)node->mass();
    os.write((char *)&value, sizeof(float));
  }

  return ss.str();
}


#include <iostream>
#include <vector>

#include <Eigen/Dense>

#include "funnels/utils.hh"

#include "funnels/distances.hh"
#include "funnels/dynamics.hh"
#include "funnels/funnels.hh"
#include "aut/ta.hh"
#include "aut/aut_collision.hh"

using namespace std;
using namespace Eigen;
using namespace funnels;
using namespace lyapunov;

void test_dist(){

  Matrix3d M;
  M(0,0) = 1.;
  M(1,1) = 2.;
  M(2,2) = 3.;

  VectorXd v(3);
  v(0)=11.;
  v(1)=12.;
  v(2)=13.;

  partial_so2_dist_t my_dist;
  std::cout << my_dist.cp_Mv(M, v) << std::endl;
  std::cout << my_dist.cp_vM(v, M) << std::endl;
  std::cout << my_dist.cp_vv(v, v) << std::endl;
  std::cout << my_dist.cp_MM(M, M) << std::endl;
}

void test_dyn(){

  kinematic_2d_sys_t my_sys;
  VectorXd u(2);
  VectorXd t(10);
  MatrixXd x(4,10);

  t.setLinSpaced(10, 0., 10.);
  u(0)=1.;
  u(1)=1.33;
  x(0,0) = -5.55;
  x(1,0) = 6.55;

  my_sys.compute(x,u,t);

  std::cout << x << std::endl;
  std::cout << u << std::endl;
  std::cout << t << std::endl;

}

void test_funnel1(){

  std::cout << "funnel 1" << std::endl;

  clock_ta_t &ctrl_clk = utils_ext::clock_map.create_and_get("c_t");
  clock_ta_t &lcl_clk = utils_ext::clock_map.create_and_get("c_h");

  kinematic_2d_sys_t my_dyn;
  partial_so2_dist_t my_dist;

  MatrixXd P(my_dyn._dimx, my_dyn._dimx);
  for (size_t i=0; i<my_dyn._dimx; i++){
    P(i,i) = (double) i+1.;
  }

  fixed_ellipsoidal_lyap_t my_lyap(P, 0.5, my_dist);

  process_t &my_proc = utils_ext::process_map.create_and_get("proc_0");

  // Init
  Vector4d x0;
  Vector2d u0;
  x0(0) = 0.;
  x0(1) = 2.;
  u0(0) = 1.1;
  u0(1) = 0.66;
  const double t0=0., t1=10.;

  funnel_t<fixed_ellipsoidal_lyap_t<partial_so2_dist_t>, kinematic_2d_sys_t>
      my_fun(100, "fun_0", my_proc, my_dyn, P, 0.5, my_dist);

  my_fun.compute(x0, t0, t1, u0);

  std::cout << my_fun.x() << std::endl;
  std::cout << my_fun.u() << std::endl;
  std::cout << my_fun.t() << std::endl;

}

void test_funnel_sys(){

  std::cout << "funnel sys" << std::endl;

  using fun_t = funnel_t<fixed_ellipsoidal_lyap_t<partial_so2_dist_t>,
      kinematic_2d_sys_t>;

  using fun_ptr_t = shared_ptr<fun_t>;

  utils_ext::clear_all_maps();
  utils_ext::clock_map.create_and_get("c_t");
  utils_ext::clock_map.create_and_get("c_h");
  utils_ext::process_map.create_and_get("proc_0");

  // Dummy event
  event_t &dummy = utils_ext::event_map.create_and_get("no_action");

  clock_ta_t &ctrl_clk = utils_ext::clock_map["c_t"];
  clock_ta_t &lcl_clk = utils_ext::clock_map["c_h"];

  kinematic_2d_sys_t my_dyn;
  partial_so2_dist_t my_dist;

  MatrixXd P(my_dyn._dimx, my_dyn._dimx);
//  for (size_t i=0; i<my_dyn._dimx; i++){
//    P(i,i) = (double) 1.;
//  }
  P(0,0) = 1.;
  P(1,1) = 1.;
  P(2,2) = 1.e-10;
  P(3,3) = 1.e-10;

  fixed_ellipsoidal_lyap_t my_lyap(P, 0.5, my_dist);

  process_t &my_proc = utils_ext::process_map["proc_0"];

  // Init
  Vector4d x0, x1;
  Vector2d u0, u1;
  double sq2i = 1./std::sqrt(2.);
  x0(0) = -1.; x1(0) = -sq2i;
  x0(1) =  0.; x1(1) = -sq2i;
  u0(0) = 1.; u1(0) = sq2i;
  u0(1) = 0.; u1(1) = sq2i;
  const double t0=0., t1=2.;
  const size_t N=1000;
  const double t_step = 20000;

  fun_ptr_t my_fun0_0 = make_shared<fun_t>(N, "fun_0_0", my_proc, my_dyn, 50.*P,
      0.5, my_dist);

  fun_ptr_t my_fun1_0 = make_shared<fun_t>(N, "fun_1_0", my_proc, my_dyn,
      100.*P, 0.5, my_dist);

  fun_ptr_t my_fun1_1 = make_shared<fun_t>(N, "fun_1_1", my_proc, my_dyn,
      200.*P, 0.5, my_dist);

  my_fun0_0->compute(x0, t0, t1, u0);
  my_fun1_0->compute(x1, t0, t1, u1);

  fun_t::set_parent(my_fun1_0, my_fun1_1);

  std::cout << my_fun0_0.use_count() << " ; " << my_fun1_0.use_count()
      << " ; " << my_fun1_1.use_count() << std::endl;

  using fun_sys_t = funnel_sys_t<fun_t>;
  using fun_sys_ptr_t = std::shared_ptr<fun_sys_t>;

  fun_sys_ptr_t my_fun_sys = std::make_shared<fun_sys_t>(ctrl_clk, lcl_clk);

  my_fun_sys->add_funnel(my_fun0_0, true);
  my_fun_sys->add_funnel(my_fun1_0, true);
  my_fun_sys->add_funnel(my_fun1_1, false);

  my_fun_sys->generate_transitions(t_step, true, true);

  std::cout << my_fun_sys->declare_proc() << std::endl;
  std::cout << my_fun_sys->declare_edge() << std::endl;

  ta::ta_t my_ta("");

  my_fun_sys->register_self(my_ta);

  std::cout << my_ta.declare() << std::endl;

  Vector4d x_obs;
  Vector2d u_obs;
  x_obs(0) = -1.;
  x_obs(1) = -1.;
  u_obs(0) = 1.;
  u_obs(1) = 1.;

  const process_t &my_proc_obs = utils_ext::process_map.create_and_get
      ("obs_col");
  const clock_ta_t &obs_ctrl = utils_ext::clock_map.create_and_get("c_o");
  fun_ptr_t my_obs_fun = make_shared<fun_t>(N, "obs", my_proc_obs, my_dyn,
      100.*P, 0.5, my_dist);

  my_obs_fun->compute(x_obs, t0, t1, u_obs);

  aut_col::aut_col<fun_t> my_aut_col(my_obs_fun, obs_ctrl);

  my_aut_col.register_self(my_ta);

  my_aut_col.compute_collisions(my_fun_sys, t_step);

  std::cout << my_ta.declare() << std::endl;
}

int main() {

//  test_dist();
//  test_dyn();
//  test_funnel1();
  test_funnel_sys();

  return 0;
}


#include "GridFilter.hh"
#include <Eigen/Core>
#include <fstream>
#include <cmath>
#include <string>
using namespace std;
using namespace Eigen;

//#define DEBUG_GRIDFILTER

void GridFilter::ComputeCrossSectionOriginal(double value) {
    auto lambda = [&value, this](double x){ return std::abs(x - value) <= m_AllowableError ? 1.0 : 0.0; };
    m_CrossSectionModified = Eigen::MatrixXd::Zero(m_PMrows, m_PMcols);
    #ifdef DEBUG_GRIDFILTER
    std::cout << "I am computed!!!" << std::endl;
    #endif
    m_CrossSecOriginal = m_ParaboloidMatrix.unaryExpr(lambda);
}

void GridFilter::ComputeGradient(double xStep, double yStep) {
    /**
     *
     * Computes dxPM and dyPM (components of gradient)
     * Gradient matrixes are the folowing
     *
     *	- dx - size [NxM-1]
     *	- dy - size [N-1xM]
     *
     * as gradient is computed with the neighbour elements in matrix
     *
     */
    m_dxPM = (m_ParaboloidMatrix.rightCols(m_PMcols - 1) - m_ParaboloidMatrix.leftCols(m_PMcols - 1)) / xStep;
    m_dyPM = (m_ParaboloidMatrix.bottomRows(m_PMrows - 1) - m_ParaboloidMatrix.topRows(m_PMrows - 1)) / yStep;
}

int GridFilter::ComputeCurrentDeviation() {
    /**
     *
     * Algorithm:
     *	- Compute non-zero elements in original cross-section
     *	- For each element in matrixes compute respectivetly:
     *	  dx[i, j]*CrossSectionOriginal[i, j] and dy[i, j]*CrossSectionOriginal[i, j]
     *	  (this will leave only contour's gradient points)
     *	- Find the sqrt of sum of squares (to fing the length of gradient vector)
     *	- Sum all this values and divide by the number of non-zero values to find the avarage value of contour's gradient
     *	- Product with multiplier GradientInfluence, ceil and product with InitialDeviation
     *
     * \return Deviation from the original coutour - the number of points that will be included in extended contour.
     *
     */

    int numOfNonZero = (m_CrossSecOriginal.array() != 0).count();
    if (numOfNonZero == 0) {
        std::cerr << "No contour found on this level. If you are sure that it shold be here, try the following:" << std::endl
            << "- make grid step smaller " << std::endl
            << "- make tolerance higher" << std::endl;
        return 0;
    }
    #ifdef DEBUG_GRIDFILTER
    std::cout << "numOfNonZero = "  << numOfNonZero << std::endl;
    #endif
    ComputeAbsGradMatrix();
    double  tmp =  m_AbsGrad.sum() / numOfNonZero;
    #ifdef DEBUG_GRIDFILTER
    std::cout << "grad_len = "  << tmp << std::endl;
    #endif
    return std::ceil(tmp) * m_InitialDeviation;
}

void GridFilter::ComputeAbsGradMatrix() {
    int rowsnum = m_PMrows - 1, colsnum = m_PMcols - 1;
    m_AbsGrad = ((m_dxPM.block(0, 0, rowsnum, colsnum).array() *
                     m_CrossSecOriginal.block(0, 0, rowsnum, colsnum).array()).square() +
                    (m_dyPM.block(0, 0, rowsnum, colsnum).array() *
                     m_CrossSecOriginal.block(0, 0, rowsnum, colsnum).array()).square())
                    .sqrt() * m_GradientInfluence ;

}

void GridFilter::GetCrossSectionOriginal(Eigen::MatrixXd& CSOmatTarget, double value, bool isCScomputed ) {
    /**
     *
     * Returns cross-section z = value of ParaboloidMatrix: the plain contains contour
     * \param[in] CSOmatTarget The matrix where result will be written
     * \param[in] value The value for compute cross-section plane z = value
     * \param[in] isCScomputed It is false if CrossSecOriginal is not computed yet. It is necessary to avoid computing twice and ensure that it is not rubbish in this matrix
     *
     */
    if (! isCScomputed)  ComputeCrossSectionOriginal(value);
    CSOmatTarget = m_CrossSecOriginal;
}

void GridFilter::GetCrossSectionExtended(MatrixXd & CSEmatTarget,
                                         double value, int deviation, bool isCScomputed, bool isDevConst) {
    /**
     *
     * Returns cross-section plane z = value of ParaboloidMatrix with the extended contour
     * \param[in] CSEmatTarget The matrix where result will be written
     * \param[in] value The value for compute cross-section plane z = value
     * \param[in] deviation Deviation of the original contour
     * \param[in] isCScomputed It is false if CrossSecOriginal is not computed yet. It is necessary to avoid computing twice and ensure that it is not rubbish in this matrix
     *
     */

    Eigen::MatrixXd tmpMat;
    GetCrossSectionOriginal(tmpMat, value, isCScomputed);
    #ifdef DEBUG_GRIDFILTER
    std::cout << " deviation = " << deviation << std::endl;
    #endif
    if (deviation != 0) addPoints(deviation, isDevConst);
    else {
        CSEmatTarget = MatrixXd::Zero(m_PMrows, m_PMcols);
    }
    GetModifiedCrossSection(CSEmatTarget);
}

void GridFilter::GetCrossSectionExtendedAutoDev(Eigen::MatrixXd& CSEADmatTarget, double value) {
    /**
     *
     * Returns cross-section z = value of ParaboloidMatrix (the plain contains contour) using value only. The deviation is computed automaticly and depends on gradient at contour points.
     * \param[in] CSEADmatTarget The matrix where result will be written
     * \param[in] value The value for compute cross-section plane z = value
     *
     */
    ComputeCrossSectionOriginal(value);
    GetCrossSectionExtended(CSEADmatTarget, value, ComputeCurrentDeviation(), true);
}

void GridFilter::GetCrossSectionExtendedIrregular(Eigen::MatrixXd& CSEImatTarget, double value) {
     /**
     *
     * Returns cross-section z = value of ParaboloidMatrix (the plain contains contour) using value only. The deviation is computed automaticly,  depends on gradient at contour points and not constant for the level.
     * \param[in] CSEImatTarget The matrix where result will be written
     * \param[in] value The value for compute cross-section plane z = value
     *
     */
    ComputeCrossSectionOriginal(value);
    ComputeAbsGradMatrix();
    GetCrossSectionExtended(CSEImatTarget, value, 1, true, false);
}

void GridFilter::makeCorridor(int curr_x, int curr_y, int deviation) {
    /**
     * Adds neighbour points to the InterestingPoints matrix for point (curr_x, curr_y)
     * \param[in] curr_x x-coordinate of the considered point
     * \param[in] curr_y y-coordinate of the considered point
     * \param[in] deviation Deviation from the original contour
     */
    for (int i = curr_x - deviation; i < curr_x + deviation + 1; i++) {
        if (i < 0 || i >=  m_PMrows) continue;
        for (int j = curr_y - deviation; j < curr_y + deviation + 1; j++) {
            if (j <  0 || j >= m_PMcols) continue;
            if (m_CrossSecOriginal(i, j) == 1.0) continue;
            m_InterestingPoints.conservativeResize(2, m_InterestingPoints.cols() + 1);
            m_InterestingPoints(0, m_InterestingPoints.cols() - 1) = i;
            m_InterestingPoints(1, m_InterestingPoints.cols() - 1) = j;
            m_CrossSectionModified(i, j) = 1.0;
        }
    }

}

void GridFilter::addPoints(int dev, bool isDevConst) {
    /**
     * Fills GridFilter#InterestingPoints matrix by extended contour points.
     */
    int deviation = 0;
    if(isDevConst) deviation = dev;
    m_InterestingPoints.resize(2, 0);
    for(int i = 0; i < m_PMrows; i++) {
        for (int j = 0; j < m_PMcols; j++) {
            if (m_CrossSecOriginal(i, j) == 1.0) {
                if(!isDevConst)  deviation = std::ceil(m_AbsGrad(i, j)) * m_InitialDeviation;
                m_InterestingPoints.conservativeResize(2, m_InterestingPoints.cols() + 1);
                m_InterestingPoints(0, m_InterestingPoints.cols() - 1) = i;
                m_InterestingPoints(1, m_InterestingPoints.cols() - 1) = j;
                m_CrossSectionModified(i, j) = 1.0;
                makeCorridor(i, j, deviation);
            }
        }
    }
}


/*
 * @Description: IMU integration activity
 * @Author: Ge Yao
 * @Date: 2020-11-10 14:25:03
 */
#ifndef IMU_INTEGRATION_ACTIVITY_HPP_
#define IMU_INTEGRATION_ACTIVITY_HPP_

// common:
#include <ros/ros.h>

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

// config:
#include "imu_integration/config/config.hpp"

// subscribers:
#include "imu_integration/subscriber/imu_subscriber.hpp"
#include "imu_integration/subscriber/odom_subscriber.hpp"

#include <nav_msgs/Odometry.h>

namespace imu_integration {

namespace estimator {

class Activity {
  public:
    Activity(void);
    void Init(void);
    bool Run(void);
  private:
    // workflow:
    bool ReadData(void);
    bool HasData(void);
    bool UpdatePose(void);
    bool PublishPose(void);

    // utils:
    /**
     * @brief  get unbiased angular velocity in body frame
     * @param  angular_vel, angular velocity measurement
     * @return unbiased angular velocity in body frame
     */
    Eigen::Vector3d GetUnbiasedAngularVel(const Eigen::Vector3d &angular_vel);
    /**
     * @brief  get unbiased linear acceleration in navigation frame
     * @param  linear_acc, linear acceleration measurement
     * @param  R, corresponding orientation of measurement
     * @return unbiased linear acceleration in navigation frame
     */
    Eigen::Vector3d GetUnbiasedLinearAcc(
        const Eigen::Vector3d &linear_acc,
        const Eigen::Matrix3d &R
    );
    /**
     * @brief  get angular delta
     * @param  index_curr, current imu measurement buffer index
     * @param  index_prev, previous imu measurement buffer index
     * @param  angular_delta, angular delta output
     * @return true if success false otherwise
     */
    bool GetAngularDelta(
        const size_t index_curr, const size_t index_prev,
        Eigen::Vector3d &angular_delta
    );
    /**
     * @brief  get velocity delta
     * @param  index_curr, current imu measurement buffer index
     * @param  index_prev, previous imu measurement buffer index
     * @param  R_curr, corresponding orientation of current imu measurement
     * @param  R_prev, corresponding orientation of previous imu measurement
     * @param  velocity_delta, velocity delta output
     * @return true if success false otherwise
     */
    bool GetVelocityDelta(
        const size_t index_curr, const size_t index_prev,
        const Eigen::Matrix3d &R_curr, const Eigen::Matrix3d &R_prev,
        double &delta_t, Eigen::Vector3d &velocity_delta
    );
    /**
     * @brief  update orientation with effective rotation angular_delta
     * @param  angular_delta, effective rotation
     * @param  R_curr, current orientation
     * @param  R_prev, previous orientation
     * @return void
     */
    void UpdateOrientation(
        const Eigen::Vector3d &angular_delta,
        Eigen::Matrix3d &R_curr, Eigen::Matrix3d &R_prev
    );
    /**
     * @brief  update orientation with effective velocity change velocity_delta
     * @param  velocity_delta, effective velocity change
     * @return void
     */
    void UpdatePosition(const double &delta_t, const Eigen::Vector3d &velocity_delta);

  private:
    // node handler:
    ros::NodeHandle private_nh_;

    // subscriber:
    std::shared_ptr<IMUSubscriber> imu_sub_ptr_;
    std::shared_ptr<OdomSubscriber> odom_ground_truth_sub_ptr;
    ros::Publisher odom_estimation_pub_;

    // data buffer:
    std::deque<IMUData> imu_data_buff_;
    std::deque<OdomData> odom_data_buff_;

    // config:
    bool initialized_ = false;

    IMUConfig imu_config_;
    OdomConfig odom_config_;

    // a. gravity constant:
    Eigen::Vector3d G_;
    // b. angular velocity:
    Eigen::Vector3d angular_vel_bias_;
    // c. linear acceleration:
    Eigen::Vector3d linear_acc_bias_;

    // IMU pose estimation:
    Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity();
    Eigen::Vector3d vel_ = Eigen::Vector3d::Zero();

    nav_msgs::Odometry message_odom_;
};

} // namespace estimator

} // namespace imu_integration

#endif


#ifndef LIGHT_HPP
#define	LIGHT_HPP
#include <armadillo>

using namespace arma;
using namespace std;

class Light{

private:
    vec posicao, I;

public:

    Light();

    Light(vec luz){
	this->posicao << luz(0) << luz(1) << luz(2);
	this->I << luz(3) << luz(4) << luz(5);
    }

    void SetI(vec I) {
        this->I = I;
    }

    vec GetI() const {
        return I;
    }

    void SetPosicao(vec posicao) {
        this->posicao = posicao;
    }

    vec GetPosicao() const {
        return posicao;
    }

};

#endif	/* LIGHT_HPP */


#include <igl/readOFF.h>
//#undef IGL_STATIC_LIBRARY
#include <igl/copyleft/cgal/mesh_boolean.h>
#include <igl/opengl/glfw/Viewer.h>

#include <Eigen/Core>
#include <iostream>

#include "tutorial_shared_path.h"

Eigen::MatrixXd VA,VB,VC;
Eigen::VectorXi J,I;
Eigen::MatrixXi FA,FB,FC;
igl::MeshBooleanType boolean_type(
  igl::MESH_BOOLEAN_TYPE_UNION);

const char * MESH_BOOLEAN_TYPE_NAMES[] =
{
  "Union",
  "Intersect",
  "Minus",
  "XOR",
  "Resolve",
};

void update(igl::opengl::glfw::Viewer &viewer)
{
  igl::copyleft::cgal::mesh_boolean(VA,FA,VB,FB,boolean_type,VC,FC,J);
  Eigen::MatrixXd C(FC.rows(),3);
  for(size_t f = 0;f<C.rows();f++)
  {
    if(J(f)<FA.rows())
    {
      C.row(f) = Eigen::RowVector3d(1,0,0);
    }else
    {
      C.row(f) = Eigen::RowVector3d(0,1,0);
    }
  }
  viewer.data().clear();
  viewer.data().set_mesh(VC,FC);
  viewer.data().set_colors(C);
  std::cout<<"A "<<MESH_BOOLEAN_TYPE_NAMES[boolean_type]<<" B."<<std::endl;
}

bool key_down(igl::opengl::glfw::Viewer &viewer, unsigned char key, int mods)
{
  switch(key)
  {
    default:
      return false;
    case '.':
      boolean_type =
        static_cast<igl::MeshBooleanType>(
          (boolean_type+1)% igl::NUM_MESH_BOOLEAN_TYPES);
      break;
    case ',':
      boolean_type =
        static_cast<igl::MeshBooleanType>(
          (boolean_type+igl::NUM_MESH_BOOLEAN_TYPES-1)%
          igl::NUM_MESH_BOOLEAN_TYPES);
      break;
    case '[':
      viewer.core.camera_dnear -= 0.1;
      return true;
    case ']':
      viewer.core.camera_dnear += 0.1;
      return true;
  }
  update(viewer);
  return true;
}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  igl::readOFF(TUTORIAL_SHARED_PATH "/cheburashka.off",VA,FA);
  igl::readOFF(TUTORIAL_SHARED_PATH "/decimated-knight.off",VB,FB);
  // Plot the mesh with pseudocolors
  igl::opengl::glfw::Viewer viewer;

  // Initialize
  update(viewer);

  viewer.data().show_lines = true;
  viewer.callback_key_down = &key_down;
  viewer.core.camera_dnear = 3.9;
  cout<<
    "Press '.' to switch to next boolean operation type."<<endl<<
    "Press ',' to switch to previous boolean operation type."<<endl<<
    "Press ']' to push near cutting plane away from camera."<<endl<<
    "Press '[' to pull near cutting plane closer to camera."<<endl<<
    "Hint: investigate _inside_ the model to see orientation changes."<<endl;
  viewer.launch();
}


// Boost.Geometry
// Unit Test

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

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

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include <geometry_test_common.hpp>

#include <boost/geometry/geometries/geometries.hpp>

#include <boost/geometry/algorithms/densify.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/algorithms/num_points.hpp>
#include <boost/geometry/algorithms/perimeter.hpp>

#include <boost/geometry/iterators/segment_iterator.hpp>

#include <boost/geometry/strategies/cartesian/densify.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/geographic/densify.hpp>
#include <boost/geometry/strategies/geographic/distance.hpp>
#include <boost/geometry/strategies/spherical/densify.hpp>
#include <boost/geometry/strategies/spherical/distance_haversine.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

struct check_lengths
{
    template <typename G, typename S>
    void operator()(G const& g, G const& o, S const& s) const
    {
        double d1 = bg::length(g, s);
        double d2 = bg::length(o, s);

        BOOST_CHECK_CLOSE(d1, d2, 0.0001);
    }
};

struct check_perimeters
{
    template <typename G, typename S>
    void operator()(G const& g, G const& o, S const& s) const
    {
        double d1 = bg::perimeter(g, s);
        double d2 = bg::perimeter(o, s);

        BOOST_CHECK_CLOSE(d1, d2, 0.0001);
    }
};

template <typename G, typename DistS>
double inline shortest_length(G const& g, DistS const& dist_s)
{
    double min_len = (std::numeric_limits<double>::max)();
    for (bg::segment_iterator<G const> it = bg::segments_begin(g);
            it != bg::segments_end(g); ++it)
    {
        double len = bg::length(*it, dist_s);
        min_len = (std::min)(min_len, len);
    }
    return min_len;
}

template <typename G, typename DistS>
double inline greatest_length(G const& o, DistS const& dist_s)
{
    double max_len = 0.0;
    for (bg::segment_iterator<G const> it = bg::segments_begin(o);
            it != bg::segments_end(o); ++it)
    {
        double len = bg::length(*it, dist_s);
        max_len = (std::max)(max_len, len);
    }
    return max_len;
}

template <typename G, typename CSTag = typename bg::cs_tag<G>::type>
struct cs_data
{};

template <typename G>
struct cs_data<G, bg::cartesian_tag>
{
    bg::strategy::densify::cartesian<> compl_s;
    bg::strategy::distance::pythagoras<> dist_s;
};

template <typename G>
struct cs_data<G, bg::spherical_equatorial_tag>
{
    cs_data()
        : model(6378137.0)
        , compl_s(model)
        , dist_s(6378137.0)
    {}

    bg::srs::sphere<double> model;
    bg::strategy::densify::spherical<> compl_s;
    bg::strategy::distance::haversine<double> dist_s;
};

template <typename G>
struct cs_data<G, bg::geographic_tag>
{
    cs_data()
        : model(6378137.0, 6356752.3142451793)
        , compl_s(model)
        , dist_s(model)
    {}

    bg::srs::spheroid<double> model;
    bg::strategy::densify::geographic<> compl_s;
    bg::strategy::distance::geographic<> dist_s;
};

template <typename G, typename DistS, typename Check>
inline void check_result(G const& g, G const& o, double max_distance,
                         DistS const& dist_s, Check const& check)
{
    // geometry was indeed densified
    std::size_t g_count = bg::num_points(g);
    std::size_t o_count = bg::num_points(o);
    BOOST_CHECK(g_count < o_count);

    // all segments have lengths smaller or equal to max_distance
    double gr_len = greatest_length(o, dist_s);
    // NOTE: Currently geographic strategies can generate segments that have
    //       lengths slightly greater than max_distance. In order to change
    //       this the generation of new points should e.g. be recursive with
    //       stop condition comparing the current distance calculated by
    //       inverse strategy.
    // NOTE: Closeness value tweaked for Andoyer
    bool is_close = (gr_len - max_distance) / (std::max)(gr_len, max_distance) < 0.0001;
    BOOST_CHECK(gr_len <= max_distance || is_close);

    // the overall length or perimeter didn't change
    check(g, o, dist_s);
}

template <typename G, typename Check>
inline void test_geometry(std::string const& wkt, Check const& check)
{
    cs_data<G> d;

    G g;
    bg::read_wkt(wkt, g);

    {
        bg::default_strategy def_s;
        double max_distance = shortest_length(g, def_s) / 3.0;

        G o;
        bg::densify(g, o, max_distance);

        check_result(g, o, max_distance, def_s, check);
    }

    {
        double max_distance = shortest_length(g, d.dist_s) / 3.0;

        G o;
        bg::densify(g, o, max_distance, d.compl_s);

        check_result(g, o, max_distance, d.dist_s, check);
    }
}

template <typename G>
inline void test_linear(std::string const& wkt)
{
    test_geometry<G>(wkt, check_lengths());
}

template <typename G>
inline void test_areal(std::string const& wkt)
{
    test_geometry<G>(wkt, check_perimeters());
}

template <typename P>
void test_all()
{
    typedef bg::model::linestring<P> ls_t;
    typedef bg::model::multi_linestring<ls_t> mls_t;

    typedef bg::model::ring<P> ring_t;
    typedef bg::model::polygon<P> poly_t;
    typedef bg::model::multi_polygon<poly_t> mpoly_t;

    typedef bg::model::ring<P, true, false> oring_t;
    typedef bg::model::polygon<P, true, false> opoly_t;
    typedef bg::model::multi_polygon<opoly_t> ompoly_t;

    test_linear<ls_t>("LINESTRING(4 -4, 4 -1)");
    test_linear<ls_t>("LINESTRING(4 4, 4 1)");
    test_linear<ls_t>("LINESTRING(0 0, 180 0)");
    test_linear<ls_t>("LINESTRING(1 1, -179 -1)");

    test_linear<ls_t>("LINESTRING(1 1, 2 2, 4 2)");
    test_linear<mls_t>("MULTILINESTRING((1 1, 2 2),(2 2, 4 2))");

    test_areal<ring_t>("POLYGON((1 1, 1 2, 2 2, 1 1))");
    test_areal<poly_t>("POLYGON((1 1, 1 4, 4 4, 4 1, 1 1),(1 1, 2 2, 2 3, 1 1))");
    test_areal<mpoly_t>("MULTIPOLYGON(((1 1, 1 4, 4 4, 4 1, 1 1),(1 1, 2 2, 2 3, 1 1)),((4 4, 5 5, 5 4, 4 4)))");

    test_areal<oring_t>("POLYGON((1 1, 1 2, 2 2))");
    test_areal<opoly_t>("POLYGON((1 1, 1 4, 4 4, 4 1),(1 1, 2 2, 2 3))");
    test_areal<ompoly_t>("MULTIPOLYGON(((1 1, 1 4, 4 4, 4 1),(1 1, 2 2, 2 3)),((4 4, 5 5, 5 4)))");

    test_areal<ring_t>("POLYGON((0 0,0 40,40 40,40 0,0 0))");
    test_areal<oring_t>("POLYGON((0 0,0 40,40 40,40 0))");
}

int test_main(int, char* [])
{
    test_all< bg::model::point<double, 2, bg::cs::cartesian> >();
    test_all< bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> > >();
    test_all< bg::model::point<double, 2, bg::cs::geographic<bg::degree> > >();

    return 0;
}


/**
 * @file
 * @author Denise Ratasich
 * @date 17.09.2013
 *
 * @brief Implementation of an Unscented Transform.
 */

#include "estimation/UnscentedTransform.h"

#include <stdexcept>
#include <vector>
#include <cmath>
#include <Eigen/Cholesky>

namespace estimation
{
  UnscentedTransform::UnscentedTransform (VectorXd x, MatrixXd Px, ITransformer* transformer)
  {
    // validation of params
    if (x.size() != Px.rows()  ||  x.size() != Px.cols())
      throw std::length_error("Initialization of Unscented Transform failed. "
			      "Invalid size of covariance, doesn't match that of vector x.");

    if (transformer == 0)
      throw std::runtime_error("Initialization of Unscented Transform failed. "
			       "No transformer passed.");

    // initializes parameters
    this->x = x;
    this->Px = Px;
    this->transformer = transformer;

    // set default scaling factors
    alpha = 1;			// spread
    beta = 2;			// distribution = Gaussian
    kappa = 3 - x.size();

    // initialize output; to get the size of y, we must propagate a
    // vector through the transformer
    VectorXd testY = transformer->transform(VectorXd::Zero(x.size()));
    y = VectorXd::Zero(testY.size());
    Py = MatrixXd::Zero(testY.size(),testY.size());
    Pxy = MatrixXd::Zero(x.size(),testY.size());

    locked = false;	// compute() not called yet
  }

  UnscentedTransform::~UnscentedTransform ()
  {
    // no space to free
  }

  void UnscentedTransform::compute (void)
  {
    std::vector<double> weights;
    std::vector<VectorXd> sigmaPoints_prior;
    std::vector<VectorXd> sigmaPoints_post;

    if (locked)
      throw new std::runtime_error("Unscented Transform. Called compute a second time - not allowed.");

    locked = true;

    try
    {
      // fill arrays: sigma points with associated weights
      generateSigmaPoints(sigmaPoints_prior, weights);

      // pop last element because its not for calculating the mean;
      // popped value will replace first element when calculating the
      // covariance
      double w0_cov = weights.back();
      weights.pop_back();

      // calculate mean y -----------------------------------
      for (int i = 0; i < sigmaPoints_prior.size(); i++)
      {
	// propagate the sigma points through the function and save
	// (needed for covariance) and covariance cannot be accumulated
	// here too because the mean is needed :(
	VectorXd post = transformer->transform(sigmaPoints_prior[i]);
	sigmaPoints_post.push_back(post);

	// accumulate to the approximated mean
	y += weights[i] * post;
      }

      // calculate covariances Py, Pxy ----------------------
      weights[0] = w0_cov;			// prepare weights
      VectorXd t(sigmaPoints_post[0].size());	// help vector (for transform)

      for (int i = 0; i < sigmaPoints_post.size(); i++)
      {
	t = sigmaPoints_post[i] - y;
	Py += weights[i] * t * t.transpose();
	Pxy += weights[i] * (sigmaPoints_prior[i] - x) * t.transpose();
      }
    }
    catch (std::exception& e)
    {
      std::string additionalInfo = "Computation of Unscented Transform failed. ";
      throw std::runtime_error(additionalInfo + e.what());
    }
  }

  // -----------------------------------------
  // getters and setters
  // -----------------------------------------

  VectorXd UnscentedTransform::mean (void) const
  {
    return y;
  }

  MatrixXd UnscentedTransform::covariance (void) const
  {
    return Py;
  }

  MatrixXd UnscentedTransform::crossCovarianceXY (void) const
  {
    return Pxy;
  }

  // -----------------------------------------
  // private functions
  // -----------------------------------------
  void UnscentedTransform::generateSigmaPoints (std::vector<VectorXd>& sigmaPoints, std::vector<double>& weights)
  {
    int L = x.size();

    // calculate values which are needed more often
    double alphaPow2 = alpha*alpha;
    double lambda = alphaPow2 * (L + kappa) - L;
    double lPlusLambda = L + lambda;

    // calculate matrix square root ----------------

    // matrix which should form the sigma points (matrix to take the
    // root of)
    MatrixXd msr(L,L);
    msr = lPlusLambda * Px;

    // calculate the root of the matrix; this is done here by standard
    // Cholesky decomposition (this works for symmetric,
    // positive-definite matrices, which Px is like)
    msr = msr.llt().matrixL();

    // generate sigma points -----------------------
    VectorXd col(x.size());

    // chi_0
    sigmaPoints.push_back(x);

    // chi_i = x + msr_i-1, i=1,..,L
    for (int i = 1; i <= L; i++)
      sigmaPoints.push_back(x + msr.col(i-1));

    // chi_i = x - msr_i-L-1, i=L+1,..,2L
    for (int i = 1; i <= L; i++)
      sigmaPoints.push_back(x - msr.col(i-1));

    // set weights --------------------------------

    // W_0 for mean
    weights.push_back(lambda / lPlusLambda);

    // W_1, W_2, ..., W_2L for mean AND covariance
    for (int i = 1; i <= 2*L; i++)
      weights.push_back(1 / (2 * lPlusLambda));

    // W_0 for covariance
    weights.push_back(lambda/lPlusLambda + 1 - alphaPow2 + beta);
  }
}


//==================================================================================================
/*!
  @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_MAJORITY_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MAJORITY_HPP_INCLUDED

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

  /*!
    @ingroup group-predicates
    This function object returns @ref True if at least two inputs
    are not @ref Zero else @ref False.

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

    @par Note

     Using `majority(x,y,z)` is similar to: `(x!= 0)+(y!= 0)+(z!= 0) >= 2`

    @par Example:

      @snippet majority.cpp majority

    @par Possible output:

      @snippet majority.txt majority

  **/
  as_logical_t<Value> majority(Value const& x, Value const& y, Value const& z);
} }
#endif

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

#endif


//=======================================================================
// Copyright (c)
//
// 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 winner_determination_in_MUCA_example.cpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2014-1-9
 */

#include "paal/auctions/auction_components.hpp"
#include "paal/auctions/xor_bids.hpp"
#include "paal/auctions/winner_determination_in_MUCA/winner_determination_in_MUCA.hpp"

#include <boost/function_output_iterator.hpp>
#include <boost/range/algorithm/copy.hpp>

#include <algorithm>
#include <iostream>
#include <iterator>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

//! [Winner Determination In MUCA Example]

int main()
{
   using Bidder = std::string;
   using Item = std::string;
   using Items = std::unordered_set<Item>;
   using Value = double;
   using Bid = std::pair<Items, Value>;
   using Bids = std::vector<Bid>;

   // create auction
   const std::unordered_map<Bidder, Bids> bids {
      {"John", {
         {{"ball", "kite"}, 2},
         {{"umbrella"}, 3},
         {{"orange"}, 1.75},
         {{"ball", "kite", "umbrella"}, 5},
         {{"ball", "kite", "orange", "umbrella"}, 6.75},
      }},
      {"Bob", {
         {{"orange"}, 1.5},
         {{"apple"}, 2.0},
         {{"apple", "orange"}, 4},
      }},
      {"Steve", {
         {{"apple"}, 1},
         {{"umbrella"}, 4},
         {{"apple", "umbrella"}, 5},
      }},
   };
   const std::vector<Bidder> bidders {"John", "Bob", "Steve"};
   const std::vector<Item> items {"apple", "ball", "orange", "kite", "umbrella"};
   auto get_bids = [&](const Bidder& bidder) -> const Bids& { return bids.at(bidder); };
   auto get_value = [](const Bid& bid) { return bid.second; };
   auto get_items = [](const Bid& bid) -> const Items& { return bid.first; };
   auto auction = paal::auctions::make_xor_bids_to_gamma_oracle_auction(
      bidders, items, get_bids, get_value, get_items
   );

   // determine winners
   Value social_welfare = 0;
   auto valuation = paal::auctions::make_xor_bids_to_value_query_auction(
      bidders, items, get_bids, get_value, get_items
   );
   paal::auctions::determine_winners_in_gamma_oracle_auction(
      auction,
      boost::make_function_output_iterator([&](std::pair<Bidder, Items> p)
      {
         auto bidder = p.first;
         auto& cur_items = p.second;
         social_welfare += valuation.call<paal::auctions::value_query>(bidder, cur_items);
         std::cout << bidder << " got bundle: ";
         boost::copy(cur_items, std::ostream_iterator<Item>(std::cout, ", "));
         std::cout << std::endl;
      })
   );
   std::cout << "social welfare: " << social_welfare << std::endl;

   return 0;
}

//! [Winner Determination In MUCA Example]


#include <Engine/MeshEdit/ASAP.h>

#include <Engine/MeshEdit/ARAP.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>

#include <Engine/MeshEdit/Paramaterize.h>

using namespace Ubpa;
using namespace std;
using namespace Eigen;

ARAP::ARAP(Ptr<TriMesh> triMesh)
	: heMesh(make_shared<HEMesh<V>>())
{
	Init(triMesh);
}

void ARAP::Clear() {
	heMesh->Clear();
	triMesh = nullptr;
}

bool ARAP::Init(Ptr<TriMesh> triMesh) {
	Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::ARAP::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	/*auto paramaterize = Paramaterize::New(triMesh);
	paramaterize->setShape(1);
	paramaterize->setWeight(0);
	paramaterize->Run();*/
	auto asap = ASAP::New(triMesh);
	asap->Run();

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::ARAP::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		return false;
	}

	// triangle mesh's texcoords and positions ->  half-edge structure's texcoords and positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->coord = triMesh->GetTexcoords()[i].cast_to<vecf2>();
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	// flatten the triangles to the plane
	flatten();

	// set two fixed vertices
	if (nV < 2) {
		printf("ERROR::ARAP::Init:\n"
			"\t""need more vertices\n");
		heMesh->Clear();
		return false;
	}
	size_t a = heMesh->Boundaries()[0].size() / 2;
	auto v1 = heMesh->Boundaries()[0][0]->Origin();
	auto v2 = heMesh->Boundaries()[0][a]->End();
	fixed_vertices_.push_back(heMesh->Index(v1));
	fixed_coords_.push_back(vecf2(0, 0));

	this->triMesh = triMesh;
	return true;
}

bool ARAP::Run() {
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::MinSurf::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}
	kernelARAP();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();

	vector<pointf2> texcoords;
	for (auto v : heMesh->Vertices())
		texcoords.push_back(v->coord.cast_to<pointf2>());
	triMesh->Update(texcoords);

	cout << "ARAP done" << endl;
	return true;
}

bool ARAP::Show()
{
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::ARAP::Show\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	kernelARAP();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);

	// Set positions
	for (auto v : heMesh->Vertices()) {
		positions.push_back(v->coord.cast_to<pointf3>());
	}

	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) { // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
		}
	}

	triMesh->Init(indice, positions);

	// Set texcoords
	vector<pointf2> texcoords;
	for (auto v : heMesh->Vertices())
		texcoords.push_back(v->coord.cast_to<pointf2>());
	triMesh->Update(texcoords);

	cout << "ARAP done" << endl;
	return true;
}

void ARAP::kernelARAP()
{
	size_t nV = heMesh->NumVertices();
	SparseMatrix<double> A(nV, nV);
	GlobalMatrixA(A);
	//cout << A << endl;

	SimplicialLLT<SparseMatrix<double>> solver;
	solver.compute(A);

	if (times < 0 || times > 100)
		times = 10;
	cout << "time = " << times << endl;

	for (int count = 0; count < times; count++)
	{
		LocalSetL();
		GlobalSolveU(solver);
	}
}

void ARAP::flatten()
{
	size_t nT = heMesh->NumPolygons();
	auto triangles = heMesh->Polygons();

	for (int i = 0; i < nT; i++) {
		auto x_tmp = triangles[i]->BoundaryVertice();

		if (Paramaterize::EqualVector(x_tmp[0]->pos, x_tmp[1]->pos) || Paramaterize::EqualVector(x_tmp[1]->pos, x_tmp[2]->pos) || Paramaterize::EqualVector(x_tmp[2]->pos, x_tmp[0]->pos))
		{
			triangles[i]->cot = { 1, 1, 1 };
			triangles[i]->x_flatten = { vecf2(1, 1), vecf2(1, 1), vecf2(1, 1) };
			continue;
		}

		triangles[i]->x_flatten.push_back(vecf2(0, 0));
		triangles[i]->x_flatten.push_back(vecf2((x_tmp[1]->pos - x_tmp[0]->pos).norm(), 0));
		float x = (x_tmp[2]->pos - x_tmp[0]->pos).dot(x_tmp[1]->pos - x_tmp[0]->pos) / (x_tmp[1]->pos - x_tmp[0]->pos).norm();
		triangles[i]->x_flatten.push_back(vecf2(x, pow(pow((x_tmp[2]->pos - x_tmp[0]->pos).norm(), 2) - pow(x, 2), 0.5)));

		//cout << triangles[i]->x_flatten[0] << triangles[i]->x_flatten[1] << triangles[i]->x_flatten[2] << endl;

		for (int j = 0; j < 3; j++) {
			double cos_theta = (triangles[i]->x_flatten[j] - triangles[i]->x_flatten[j == 0 ? 2 : j - 1])
				.cos_theta(triangles[i]->x_flatten[j == 2 ? 0 : j + 1] - triangles[i]->x_flatten[j == 0 ? 2 : j - 1]);
			double abs_cot = pow(cos_theta * cos_theta / (1 - cos_theta * cos_theta), 0.5);

			triangles[i]->cot.push_back(cos_theta > 0 ? abs_cot : -abs_cot);
			//cout << triangles[i]->cot[j] << endl;
		}
	}
}

void ARAP::LocalSetL()
{
	for (auto triangle : heMesh->Polygons())
	{
		auto vertices = triangle->BoundaryVertice();
		Matrix2d X, U;
		X << (triangle->x_flatten[0] - triangle->x_flatten[1])[0], (triangle->x_flatten[1] - triangle->x_flatten[2])[0],
			(triangle->x_flatten[0] - triangle->x_flatten[1])[1], (triangle->x_flatten[1] - triangle->x_flatten[2])[1];
		U << (vertices[0]->coord - vertices[1]->coord)[0], (vertices[1]->coord - vertices[2]->coord)[0],
			(vertices[0]->coord - vertices[1]->coord)[1], (vertices[1]->coord - vertices[2]->coord)[1];
		Matrix2d J = U * X.inverse();

		JacobiSVD<Matrix2d> svd(J, ComputeFullU | ComputeFullV);
		if (J.determinant() > 0)
			triangle->L = svd.matrixU() * svd.matrixV().transpose();
		else
		{
			Matrix2d D;
			D(0, 0) = 1; D(0, 1) = 0; D(1, 0) = 0; D(1, 1) = -1;
			triangle->L = svd.matrixU() * D * svd.matrixV().transpose();
		}
		//cout << "L:" << triangle->L << endl;
	}
}

void ARAP::GlobalSolveU(SimplicialLLT<SparseMatrix<double>>& solver)
{
	size_t nV = heMesh->NumVertices();
	VectorXd b_x = VectorXd::Zero(nV);
	VectorXd b_y = VectorXd::Zero(nV);

	auto vertice_list_ = heMesh->Vertices();

	for (int i = 0; i < nV; i++)
	{
		if (i == fixed_vertices_[0])
		{
			b_x(i) += fixed_coords_[0][0];
			b_y(i) += fixed_coords_[0][1];
			continue;
		}
		/*if (i == fixed_vertices_[1])
		{
			b_x(i) += fixed_coords_[1][0];
			b_y(i) += fixed_coords_[1][1];
			continue;
		}*/
		Vector2d sum = Vector2d::Zero();
		for (auto he : vertice_list_[i]->OutHEs())
		{
			// Left Triangle
			auto triangle = he->Polygon();
			size_t t = 0;
			int index = 0;
			double coe_ij = 0;
			if (triangle != nullptr)
			{
				for (index = 0; heMesh->Index(triangle->BoundaryVertice()[index]) != i; index++);
				auto diffx = triangle->x_flatten[index] - triangle->x_flatten[index == 2 ? 0 : index + 1];
				coe_ij += triangle->cot[index];
				sum += triangle->cot[index] * triangle->L * Vector2d(diffx[0], diffx[1]);
			}
			// Right Triangle
			triangle = he->Pair()->Polygon();
			if (triangle != nullptr)
			{
				for (index = 0; heMesh->Index(triangle->BoundaryVertice()[index]) != i; index++);
				auto diffx = triangle->x_flatten[index] - triangle->x_flatten[index == 0 ? 2 : index - 1];
				coe_ij += triangle->cot[index == 0 ? 2 : index - 1];
				sum += triangle->cot[index == 0 ? 2 : index - 1] * triangle->L * Vector2d(diffx[0], diffx[1]);
			}
			// Set
			int j = static_cast<int>(heMesh->Index(he->End()));
			if (j == fixed_vertices_[0])
			{
				b_x(i) -= (-coe_ij) * fixed_coords_[0][0];
				b_y(i) -= (-coe_ij) * fixed_coords_[0][1];
			}
			/*else if (j == fixed_vertices_[1])
			{
				b_x(i) -= (-coe_ij) * fixed_coords_[1][0];
				b_y(i) -= (-coe_ij) * fixed_coords_[1][1];
			}*/

		}

		b_x(i) += sum(0);
		b_y(i) += sum(1);
	}
	//cout << b_x << endl << b_y << endl;
	VectorXd u_x = solver.solve(b_x);
	VectorXd u_y = solver.solve(b_y);
	//cout << u_x << endl << u_y << endl;

	for (int i = 0; i < nV; i++)
	{
		vertice_list_[i]->coord = vecf2(u_x(i), u_y(i));
	}
}

void ARAP::GlobalMatrixA(SparseMatrix<double>& A)
{
	vector<Triplet<double>> A_Triplet;
	size_t nV = heMesh->NumVertices();
	auto vertice_list_ = heMesh->Vertices();

	for (int i = 0; i < nV; i++)
	{
		//if (i == fixed_vertices_[0] || i == fixed_vertices_[1])
		if (i == fixed_vertices_[0])
		{
			A_Triplet.push_back(Triplet<double>(i, i, 1));
			continue;
		}
		double sum = 0;
		for (auto he : vertice_list_[i]->OutHEs())
		{
			double coe_ij = 0;
			// Left Triangle
			auto triangle = he->Polygon();
			size_t t = 0;
			int index = 0;
			if (triangle != nullptr)
			{
				for (index = 0; heMesh->Index(triangle->BoundaryVertice()[index]) != i; index++);
				coe_ij += triangle->cot[index];
			}
			// Right Triangle
			triangle = he->Pair()->Polygon();
			if (triangle != nullptr)
			{
				for (index = 0; heMesh->Index(triangle->BoundaryVertice()[index]) != i; index++);
				coe_ij += triangle->cot[index == 0 ? 2 : index - 1];
			}
			// Set
			sum += coe_ij;
			int j = static_cast<int>(heMesh->Index(he->End()));
			//if (j != fixed_vertices_[0] && j != fixed_vertices_[1])
			if (j != fixed_vertices_[0])
				A_Triplet.push_back(Triplet<double>(i, j, -coe_ij));
		}
		A_Triplet.push_back(Triplet<double>(i, i, sum));
	}

	A.setFromTriplets(A_Triplet.begin(), A_Triplet.end());
}

void ARAP::setTimes(int time)
{
	times = time;
}


/**
 * @file lsh_test.cpp
 *
 * Unit tests for the 'LSHSearch' class.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/metrics/lmetric.hpp>
#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

#include <mlpack/methods/lsh/lsh_search.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>

using namespace std;
using namespace mlpack;
using namespace mlpack::neighbor;

double ComputeRecall(
    const arma::Mat<size_t>& lshNeighbors,
    const arma::Mat<size_t>& groundTruth)
{
  const size_t queries = lshNeighbors.n_cols;
  const size_t neigh = lshNeighbors.n_rows;

  const double same = arma::accu(lshNeighbors == groundTruth);
  return same / (static_cast<double>(queries * neigh));
}

BOOST_AUTO_TEST_SUITE(LSHTest);

/**
 * Test: Run LSH with varying number of tables, keeping all other parameters
 * constant. Compute the recall, i.e. the number of reported neighbors that
 * are real neighbors of the query.
 * LSH's property is that (with high probability), increasing the number of
 * tables will increase recall. Epsilon ensures that if noise lightly affects
 * the projections, the test will not fail.
 * This produces false negatives, so we attempt the test numTries times and
 * only declare failure if all of them fail.
 */
BOOST_AUTO_TEST_CASE(NumTablesTest)
{
  // kNN and LSH parameters (use LSH default parameters).
  const int k = 4;
  const int numProj = 10;
  const double hashWidth = 0;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  // Test parameters.
  const double epsilon = 0.1; // Allowed deviation from expected monotonicity.
  const int numTries = 5; // Tries for each test before declaring failure.

  // Read iris training and testing data as reference and query sets.
  const string trainSet = "iris_train.csv";
  const string testSet = "iris_test.csv";
  arma::mat rdata;
  arma::mat qdata;
  data::Load(trainSet, rdata, true);
  data::Load(testSet, qdata, true);

  // Run classic knn on reference data.
  AllkNN knn(rdata);
  arma::Mat<size_t> groundTruth;
  arma::mat groundDistances;
  knn.Search(qdata, k, groundTruth, groundDistances);

  bool fail;
  for (int t = 0; t < numTries; ++t)
  {
    fail = false;

    const int lSize = 6; // Number of runs.
    const int lValue[] = {1, 8, 16, 32, 64, 128}; // Number of tables.
    double lValueRecall[lSize] = {0.0}; // Recall of each LSH run.

    for (size_t l = 0; l < lSize; ++l)
    {
      // Run LSH with only numTables varying (other values are defaults).
      LSHSearch<> lshTest(rdata, numProj, lValue[l], hashWidth, secondHashSize,
          bucketSize);
      arma::Mat<size_t> lshNeighbors;
      arma::mat lshDistances;
      lshTest.Search(qdata, k, lshNeighbors, lshDistances);

      // Compute recall for each query.
      lValueRecall[l] = ComputeRecall(lshNeighbors, groundTruth);

      if (l > 0)
      {
        if (lValueRecall[l] < lValueRecall[l - 1] - epsilon)
        {
          fail = true; // If test fails at one point, stop and retry.
          break;
        }
      }
    }

    if (!fail)
      break; // If test passes one time, it is sufficient.
  }

  BOOST_REQUIRE(fail == false);
}

/**
 * Test: Run LSH with varying hash width, keeping all other parameters
 * constant. Compute the recall, i.e. the number of reported neighbors that
 * are real neighbors of the query.
 * LSH's property is that (with high probability), increasing the hash width
 * will increase recall. Epsilon ensures that if noise lightly affects the
 * projections, the test will not fail.
 */
BOOST_AUTO_TEST_CASE(HashWidthTest)
{
  // kNN and LSH parameters (use LSH default parameters).
  const int k = 4;
  const int numTables = 30;
  const int numProj = 10;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  // Test parameters.
  const double epsilon = 0.1; // Allowed deviation from expected monotonicity.

  // Read iris training and testing data as reference and query.
  const string trainSet = "iris_train.csv";
  const string testSet = "iris_test.csv";
  arma::mat rdata;
  arma::mat qdata;
  data::Load(trainSet, rdata, true);
  data::Load(testSet, qdata, true);

  // Run classic knn on reference data.
  AllkNN knn(rdata);
  arma::Mat<size_t> groundTruth;
  arma::mat groundDistances;
  knn.Search(qdata, k, groundTruth, groundDistances);
  const int hSize = 7; // Number of runs.
  const double hValue[] = {0.1, 0.5, 1, 5, 10, 50, 500}; // Hash width.
  double hValueRecall[hSize] = {0.0}; // Recall of each run.

  for (size_t h = 0; h < hSize; ++h)
  {
    // Run LSH with only hashWidth varying (other values are defaults).
    LSHSearch<> lshTest(
        rdata,
        numProj,
        numTables,
        hValue[h],
        secondHashSize,
        bucketSize);

    arma::Mat<size_t> lshNeighbors;
    arma::mat lshDistances;
    lshTest.Search(qdata, k, lshNeighbors, lshDistances);

    // Compute recall for each query.
    hValueRecall[h] = ComputeRecall(lshNeighbors, groundTruth);

    if (h > 0)
      BOOST_REQUIRE_GE(hValueRecall[h], hValueRecall[h - 1] - epsilon);
  }
}

/**
 * Test: Run LSH with varying number of projections, keeping other parameters
 * constant. Compute the recall, i.e. the number of reported neighbors that
 * are real neighbors of the query.
 * LSH's property is that (with high probability), increasing the number of
 * projections per table will decrease recall. Epsilon ensures that if noise
 * lightly affects the projections, the test will not fail.
 */
BOOST_AUTO_TEST_CASE(NumProjTest)
{
  // kNN and LSH parameters (use LSH default parameters).
  const int k = 4;
  const int numTables = 30;
  const double hashWidth = 0;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  // Test parameters.
  const double epsilon = 0.1; // Allowed deviation from expected monotonicity.

  // Read iris training and testing data as reference and query sets.
  const string trainSet = "iris_train.csv";
  const string testSet = "iris_test.csv";
  arma::mat rdata;
  arma::mat qdata;
  data::Load(trainSet, rdata, true);
  data::Load(testSet, qdata, true);

  // Run classic knn on reference data.
  AllkNN knn(rdata);
  arma::Mat<size_t> groundTruth;
  arma::mat groundDistances;
  knn.Search(qdata, k, groundTruth, groundDistances);

  // LSH test parameters for numProj.
  const int pSize = 5; // Number of runs.
  const int pValue[] = {1, 10, 20, 50, 100}; // Number of projections.
  double pValueRecall[pSize] = {0.0}; // Recall of each run.

  for (size_t p = 0; p < pSize; ++p)
  {
    // Run LSH with only numProj varying (other values are defaults).
    LSHSearch<> lshTest(
        rdata,
        pValue[p],
        numTables,
        hashWidth,
        secondHashSize,
        bucketSize);

    arma::Mat<size_t> lshNeighbors;
    arma::mat lshDistances;
    lshTest.Search(qdata, k, lshNeighbors, lshDistances);

    // Compute recall for each query.
    pValueRecall[p] = ComputeRecall(lshNeighbors, groundTruth);

    // Don't check the first run; only check that increasing P decreases recall.
    if (p > 0)
      BOOST_REQUIRE_LE(pValueRecall[p] - epsilon, pValueRecall[p - 1]);
  }
}

/**
 * Test: Run two LSH searches:
 * First, a very expensive LSH search, with a large number of hash tables
 * and a large hash width. This run should return an acceptable recall. We set
 * the bar very low (recall >= 50%) to make sure that a test fail means bad
 * implementation.
 * Second, a very cheap LSH search, with parameters that should cause recall
 * to be very low. Set the threshhold very high (recall <= 25%) to make sure
 * that a test fail means bad implementation.
 */
BOOST_AUTO_TEST_CASE(RecallTest)
{
  // kNN and LSH parameters (use LSH default parameters).
  const int k = 4;
  const int secondHashSize = 99901;
  const int bucketSize = 500;

  // Read iris training and testing data as reference and query sets.
  const string trainSet = "iris_train.csv";
  const string testSet = "iris_test.csv";
  arma::mat rdata;
  arma::mat qdata;
  data::Load(trainSet, rdata, true);
  data::Load(testSet, qdata, true);

  // Run classic knn on reference data.
  AllkNN knn(rdata);
  arma::Mat<size_t> groundTruth;
  arma::mat groundDistances;
  knn.Search(qdata, k, groundTruth, groundDistances);

  // Expensive LSH run.
  const int hExp = 10000; // First-level hash width.
  const int kExp = 1; // Projections per table.
  const int tExp = 128; // Number of tables.
  const double recallThreshExp = 0.5;

  LSHSearch<> lshTestExp(
      rdata,
      kExp,
      tExp,
      hExp,
      secondHashSize,
      bucketSize);
  arma::Mat<size_t> lshNeighborsExp;
  arma::mat lshDistancesExp;
  lshTestExp.Search(qdata, k, lshNeighborsExp, lshDistancesExp);

  const double recallExp = ComputeRecall(lshNeighborsExp, groundTruth);

  // This run should have recall higher than the threshold.
  BOOST_REQUIRE_GE(recallExp, recallThreshExp);

  // Cheap LSH run.
  const int hChp = 1; // Small first-level hash width.
  const int kChp = 1000; // Large number of projections per table.
  const int tChp = 1; // Only one table.
  const double recallThreshChp = 0.25; // Recall threshold.

  LSHSearch<> lshTestChp(
      rdata,
      kChp,
      tChp,
      hChp,
      secondHashSize,
      bucketSize);
  arma::Mat<size_t> lshNeighborsChp;
  arma::mat lshDistancesChp;
  lshTestChp.Search(qdata, k, lshNeighborsChp, lshDistancesChp);

  const double recallChp = ComputeRecall(lshNeighborsChp, groundTruth);

  // This run should have recall lower than the threshold.
  BOOST_REQUIRE_LE(recallChp, recallThreshChp);
}

BOOST_AUTO_TEST_CASE(LSHTrainTest)
{
  // This is a not very good test that simply checks that the re-trained LSH
  // model operates on the correct dimensionality and returns the correct number
  // of results.
  arma::mat referenceData = arma::randu<arma::mat>(3, 100);
  arma::mat newReferenceData = arma::randu<arma::mat>(10, 400);
  arma::mat queryData = arma::randu<arma::mat>(10, 200);

  LSHSearch<> lsh(referenceData, 3, 2, 2.0, 11, 3);

  lsh.Train(newReferenceData, 4, 3, 3.0, 12, 4);

  arma::Mat<size_t> neighbors;
  arma::mat distances;

  lsh.Search(queryData, 3, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 200);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 3);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 200);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 3);
}

BOOST_AUTO_TEST_CASE(EmptyConstructorTest)
{
  // If we create an empty LSH model and then call Search(), it should throw an
  // exception.
  LSHSearch<> lsh;

  arma::mat dataset = arma::randu<arma::mat>(5, 50);
  arma::mat distances;
  arma::Mat<size_t> neighbors;
  BOOST_REQUIRE_THROW(lsh.Search(dataset, 2, neighbors, distances),
      std::invalid_argument);

  // Now, train.
  lsh.Train(dataset, 4, 3, 3.0, 12, 4);

  lsh.Search(dataset, 3, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 50);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 3);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 50);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 3);
}

BOOST_AUTO_TEST_SUITE_END();


#include <NTL/mat_lzz_p.h>

NTL_CLIENT

void FillRandom(Mat<zz_p>& A)
{
   long n = A.NumRows();
   long m = A.NumCols();
   for (long i = 0; i < n; i++)
      for (long j = 0; j < m; j++)
         random(A[i][j]);
}

void FillRandom1(Mat<zz_p>& A)
{
   long n = A.NumRows();
   long m = A.NumCols();
   for (long j = 0; j < m; j++) {
      if (j > 0 && RandomBnd(2)) {
	 for (long i = 0; i < n; i++)
            A[i][j] = A[i][j-1];
      }
      else {
	 for (long i = 0; i < n; i++)
	    random(A[i][j]);
      }
   }
}

void FillRandom(Vec<zz_p>& A)
{
   long n = A.length();
   for (long i = 0; i < n; i++)
      random(A[i]);
}

long old_gauss(mat_zz_p& M, long w)
{
   using NTL_NAMESPACE::negate;
   long k, l;
   long i, j;
   long pos;
   zz_p t1, t2, t3;
   zz_p *x, *y;

   long n = M.NumRows();
   long m = M.NumCols();

   if (w < 0 || w > m)
      LogicError("gauss: bad args");

   long p = zz_p::modulus();
   mulmod_t pinv = zz_p::ModulusInverse();
   long T1, T2;

   l = 0;
   for (k = 0; k < w && l < n; k++) {

      pos = -1;
      for (i = l; i < n; i++) {
         if (!IsZero(M[i][k])) {
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         swap(M[pos], M[l]);

         inv(t3, M[l][k]);
         negate(t3, t3);

         for (i = l+1; i < n; i++) {
            // M[i] = M[i] + M[l]*M[i,k]*t3

            mul(t1, M[i][k], t3);

            T1 = rep(t1);
            mulmod_precon_t T1pinv = PrepMulModPrecon(T1, p, pinv);

            clear(M[i][k]);

            x = M[i].elts() + (k+1);
            y = M[l].elts() + (k+1);

            for (j = k+1; j < m; j++, x++, y++) {
               // *x = *x + (*y)*t1

               T2 = MulModPrecon(rep(*y), T1, p, T1pinv);
               T2 = AddMod(T2, rep(*x), p);
               (*x).LoopHole() = T2;
            }
         }

         l++;
      }
   }

   return l;
}

long old_gauss(mat_zz_p& M)
{
   return old_gauss(M, M.NumCols());
}

void old_image(mat_zz_p& X, const mat_zz_p& A)
{
   mat_zz_p M;
   M = A;
   long r = old_gauss(M);
   M.SetDims(r, M.NumCols());
   X = M;
}

int main(int argc, char **argv)
{
   ZZ seed;
   RandomLen(seed, 30);
   SetSeed(seed);
   cerr << "\nseed=" << seed << "\n";

   long iters = 100;

#if 1
   cerr << "testing multiplication";
   for (long cnt = 0; cnt < iters; cnt++) {
      cerr << ".";

      long bnd = (cnt%2) ? 25 : 2000;

      long len = RandomBnd(NTL_SP_NBITS-3)+4;
      long n = RandomBnd(bnd);
      long l = RandomBnd(bnd);
      long m = RandomBnd(bnd);

      long p = RandomPrime_long(len);
      zz_p::init(p);

      Mat<zz_p> A, B, X;

      A.SetDims(n, l);
      B.SetDims(l, m);

      FillRandom(A);
      FillRandom(B);

      X.SetDims(n, m);

      vec_zz_p R;

      R.SetLength(m);
      for (long i = 0; i < m; i++) random(R[i]);

      mul(X, A, B);

      if (X*R != A*(B*R))
         cerr << "*\n*\n*\n*\n*\n*********** oops " << len << " " << n << " " << l << " "
              << m << "\n";
   }
#endif

#if 1
   cerr << "\ntesting inversion";
   for (long cnt = 0; cnt < iters; cnt++) {
      cerr << ".";
      long bnd = (cnt%2) ? 25 : 1500;

      long len = RandomBnd(NTL_SP_NBITS-3)+4;
      long n = RandomBnd(bnd);

      long p = RandomPrime_long(len);
      zz_p::init(p);

      Mat<zz_p> A, X;

      A.SetDims(n, n);

      FillRandom(A);

      vec_zz_p R;

      R.SetLength(n);
      for (long i = 0; i < n; i++) random(R[i]);

      zz_p d;

      inv(d, X, A);

      if (d != 0) {
	 if (R != A*(X*R))
	    cerr << "\n*\n*\n*\n*\n*********** oops " << len << " " << n << "\n";
      }
      else {
         cerr << "[singular]";
      }
   }
#endif

#if 1
   cerr << "\ntesting solve";
   for (long cnt = 0; cnt < iters; cnt++) {
      cerr << ".";
      long bnd = (cnt%2) ? 25 : 2000;

      long len = RandomBnd(NTL_SP_NBITS-3)+4;
      long n = RandomBnd(bnd);

      long p = RandomPrime_long(len);
      zz_p::init(p);

      Mat<zz_p> A;

      A.SetDims(n, n);
      FillRandom(A);

      Vec<zz_p> x, b;
      b.SetLength(n);
      FillRandom(b);

      zz_p d;

      solve(d, A, x, b);

      if (d != 0) {
	 if (A*x != b)
	    cerr << "\n*\n*\n*\n*\n*********** oops " << len << " " << n << "\n";
      }
      else {
         cerr << "[singular]";
      }
   }
#endif

#if 1
   cerr << "\ntesting image and kernel";
   for (long cnt = 0; cnt < iters; cnt++) {
      cerr << ".";
      long bnd = (cnt%2) ? 25 : 1500;

      long len = RandomBnd(NTL_SP_NBITS-3)+4;
      long n = RandomBnd(bnd);
      long m = RandomBnd(bnd);

      long p = RandomPrime_long(len);
      zz_p::init(p);

      Mat<zz_p> A;

      A.SetDims(n, m);
      FillRandom1(A);

      Mat<zz_p> im, im1, ker1;

      old_image(im, A);
      image(im1, A);
      kernel(ker1, A);

      if (im != im1 || !IsZero(ker1*A) || im1.NumRows() + ker1.NumRows() != n) {
         cerr << "\n*\n*\n*\n*\n*********** oops " << len << " " << n << m << "\n";
      }
   }
#endif

   cerr << "\n";

}


#include <cmath>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include "ctkernel.h"


/*
 * Polygon.cpp
 *
 *  Created on: Nov 7, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#include <grid_map_core/Polygon.hpp>

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

#include <limits>
#include <algorithm>

namespace grid_map {

Polygon::Polygon()
    : timestamp_(0)
{
}

Polygon::Polygon(std::vector<Position> vertices)
    : Polygon()
{
  vertices_ = vertices;
}

Polygon::~Polygon() {}

bool Polygon::isInside(const Position& point) const
{
  int cross = 0;
  for (int i = 0, j = vertices_.size() - 1; i < vertices_.size(); j = i++) {
    if ( ((vertices_[i].y() > point.y()) != (vertices_[j].y() > point.y()))
           && (point.x() < (vertices_[j].x() - vertices_[i].x()) * (point.y() - vertices_[i].y()) /
            (vertices_[j].y() - vertices_[i].y()) + vertices_[i].x()) )
    {
      cross++;
    }
  }
  return bool(cross % 2);
}

void Polygon::addVertex(const Position& vertex)
{
  vertices_.push_back(vertex);
}

const Position& Polygon::getVertex(const size_t index) const
{
  return vertices_.at(index);
}

void Polygon::removeVertices()
{
  vertices_.clear();
}

const Position& Polygon::operator [](const size_t index) const
{
  return getVertex(index);
}

const std::vector<Position>& Polygon::getVertices() const
{
  return vertices_;
}

size_t Polygon::nVertices() const
{
  return vertices_.size();
}

const std::string& Polygon::getFrameId() const
{
  return frameId_;
}

void Polygon::setFrameId(const std::string& frameId)
{
  frameId_ = frameId;
}

uint64_t Polygon::getTimestamp() const
{
  return timestamp_;
}

void Polygon::setTimestamp(const uint64_t timestamp)
{
  timestamp_ = timestamp;
}

void Polygon::resetTimestamp()
{
  timestamp_ = 0.0;
}

double Polygon::getArea() const
{
  double area = 0.0;
  int j = vertices_.size() - 1;
  for (int i = 0; i < vertices_.size(); i++) {
    area += (vertices_.at(j).x() + vertices_.at(i).x())
        * (vertices_.at(j).y() - vertices_.at(i).y());
    j = i;
  }
  return std::abs(area / 2.0);
}

Position Polygon::getCentroid() const
{
  Position centroid = Position::Zero();
  std::vector<Position> vertices = getVertices();
  vertices.push_back(vertices.at(0));
  double area = 0.0;
  for (int i = 0; i < vertices.size() - 1; i++) {
    const double a = vertices[i].x() * vertices[i+1].y() - vertices[i+1].x() * vertices[i].y();
    area += a;
    centroid.x() += a * (vertices[i].x() + vertices[i+1].x());
    centroid.y() += a * (vertices[i].y() + vertices[i+1].y());
  }
  area *= 0.5;
  centroid /= (6.0 * area);
  return centroid;
}

void Polygon::getBoundingBox(Position& center, Length& length) const
{
  double minX = std::numeric_limits<double>::infinity();
  double maxX = -std::numeric_limits<double>::infinity();
  double minY = std::numeric_limits<double>::infinity();
  double maxY = -std::numeric_limits<double>::infinity();
  for (const auto& vertex : vertices_) {
    if (vertex.x() > maxX) maxX = vertex.x();
    if (vertex.y() > maxY) maxY = vertex.y();
    if (vertex.x() < minX) minX = vertex.x();
    if (vertex.y() < minY) minY = vertex.y();
  }
  center.x() = (minX + maxX) / 2.0;
  center.y() = (minY + maxY) / 2.0;
  length.x() = (maxX - minX);
  length.y() = (maxY - minY);
}

bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd& b) const
{
  Eigen::MatrixXd V(nVertices(), 2);
  for (unsigned int i = 0; i < nVertices(); ++i)
    V.row(i) = vertices_[i];

  // Create k, a list of indices from V forming the convex hull.
  // TODO: Assuming counter-clockwise ordered convex polygon.
  // MATLAB: k = convhulln(V);
  Eigen::MatrixXi k;
  k.resizeLike(V);
  for (unsigned int i = 0; i < V.rows(); ++i)
    k.row(i) << i, (i+1) % V.rows();
  Eigen::RowVectorXd c = V.colwise().mean();
  V.rowwise() -= c;
  A = Eigen::MatrixXd::Constant(k.rows(), V.cols(), NAN);

  unsigned int rc = 0;
  for (unsigned int ix = 0; ix < k.rows(); ++ix) {
    Eigen::MatrixXd F(2, V.cols());
    F.row(0) << V.row(k(ix, 0));
    F.row(1) << V.row(k(ix, 1));
    Eigen::FullPivLU<Eigen::MatrixXd> luDecomp(F);
    if (luDecomp.rank() == F.rows()) {
      A.row(rc) = F.colPivHouseholderQr().solve(Eigen::VectorXd::Ones(F.rows()));
      ++rc;
    }
  }

  A = A.topRows(rc);
  b = Eigen::VectorXd::Ones(A.rows());
  b = b + A * c.transpose();

  return true;
}

bool Polygon::thickenLine(const double thickness)
{
  if (vertices_.size() != 2) return false;
  const Vector connection(vertices_[1] - vertices_[0]);
  const Vector orthogonal = thickness * Vector(connection.y(), -connection.x()).normalized();
  std::vector<Position> newVertices;
  newVertices.reserve(4);
  newVertices.push_back(vertices_[0] + orthogonal);
  newVertices.push_back(vertices_[0] - orthogonal);
  newVertices.push_back(vertices_[1] - orthogonal);
  newVertices.push_back(vertices_[1] + orthogonal);
  vertices_ = newVertices;
  return true;
}

bool Polygon::offsetInward(const double margin)
{
  // Create a list of indices of the neighbours of each vertex.
  // TODO: Assuming counter-clockwise ordered convex polygon.
  std::vector<Eigen::Array2i> neighbourIndices;
  const unsigned int n = nVertices();
  neighbourIndices.resize(n);
  for (unsigned int i = 0; i < n; ++i) {
    neighbourIndices[i] << (i > 0 ? (i-1)%n : n-1), (i + 1) % n;
  }

  std::vector<Position> copy(vertices_);
  for (unsigned int i = 0; i < neighbourIndices.size(); ++i) {
    Eigen::Vector2d v1 = vertices_[neighbourIndices[i](0)] - vertices_[i];
    Eigen::Vector2d v2 = vertices_[neighbourIndices[i](1)] - vertices_[i];
    v1.normalize();
    v2.normalize();
    const double angle = acos(v1.dot(v2));
    copy[i] += margin / sin(angle) * (v1 + v2);
  }
  vertices_ = copy;
  return true;
}

std::vector<Polygon> Polygon::triangulate(const TriangulationMethods& method) const
{
  // TODO Add more triangulation methods.
  // https://en.wikipedia.org/wiki/Polygon_triangulation
  std::vector<Polygon> polygons;
  if (vertices_.size() < 3)
    return polygons;

  size_t nPolygons = vertices_.size() - 2;
  polygons.reserve(nPolygons);

  if (nPolygons < 1) {
    // Special case.
    polygons.push_back(*this);
  } else {
    // General case.
    for (size_t i = 0; i < nPolygons; ++i) {
      Polygon polygon({vertices_[0], vertices_[i + 1], vertices_[i + 2]});
      polygons.push_back((polygon));
    }
  }

  return polygons;
}

Polygon Polygon::fromCircle(const Position center, const double radius,
                                  const int nVertices)
{
  Eigen::Vector2d centerToVertex(radius, 0.0), centerToVertexTemp;

  Polygon polygon;
  for (int j = 0; j < nVertices; j++) {
    double theta = j * 2 * M_PI / (nVertices - 1);
    Eigen::Rotation2D<double> rot2d(theta);
    centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
    polygon.addVertex(center + centerToVertexTemp);
  }
  return polygon;
}

Polygon Polygon::convexHullOfTwoCircles(const Position center1,
                                   const Position center2, const double radius,
                                   const int nVertices)
{
  if (center1 == center2) return fromCircle(center1, radius, nVertices);
  Eigen::Vector2d centerToVertex, centerToVertexTemp;
  centerToVertex = center2 - center1;
  centerToVertex.normalize();
  centerToVertex *= radius;

  grid_map::Polygon polygon;
  for (int j = 0; j < ceil(nVertices / 2.0); j++) {
    double theta = M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
    Eigen::Rotation2D<double> rot2d(theta);
    centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
    polygon.addVertex(center1 + centerToVertexTemp);
  }
  for (int j = 0; j < ceil(nVertices / 2.0); j++) {
    double theta = 3 * M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
    Eigen::Rotation2D<double> rot2d(theta);
    centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
    polygon.addVertex(center2 + centerToVertexTemp);
  }
  return polygon;
}

Polygon Polygon::convexHull(Polygon& polygon1, Polygon& polygon2)
{
  std::vector<Position> vertices;
  vertices.reserve(polygon1.nVertices() + polygon2.nVertices());
  vertices.insert(vertices.end(), polygon1.getVertices().begin(), polygon1.getVertices().end());
  vertices.insert(vertices.end(), polygon2.getVertices().begin(), polygon2.getVertices().end());

  return monotoneChainConvexHullOfPoints(vertices);
}

Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector<Position>& points)
{
  // Adapted from https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain
  if (points.size() <= 3) {
    return Polygon(points);
  }
  std::vector<Position> pointsConvexHull(2 * points.size());

  // Sort points lexicographically.
  auto sortedPoints(points);
  std::sort(sortedPoints.begin(), sortedPoints.end(), sortVertices);

  int k = 0;
  // Build lower hull
  for (int i = 0; i < sortedPoints.size(); ++i) {
    while (k >= 2 && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
      k--;
    }
    pointsConvexHull.at(k++) = sortedPoints.at(i);
  }

  // Build upper hull.
  for (int i = sortedPoints.size() - 2, t = k + 1; i >= 0; i--) {
    while (k >= t && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
      k--;
    }
    pointsConvexHull.at(k++) = sortedPoints.at(i);
  }
  pointsConvexHull.resize(k - 1);

  Polygon polygon(pointsConvexHull);
  return polygon;
}

bool Polygon::sortVertices(const Eigen::Vector2d& vector1,
                           const Eigen::Vector2d& vector2)
{
  return (vector1.x() < vector2.x()
      || (vector1.x() == vector2.x() && vector1.y() < vector2.y()));
}

double Polygon::computeCrossProduct2D(const Eigen::Vector2d& vector1,
                                      const Eigen::Vector2d& vector2)
{
  return (vector1.x() * vector2.y() - vector1.y() * vector2.x());
}

double Polygon::vectorsMakeClockwiseTurn(const Eigen::Vector2d &pointOrigin,
                                         const Eigen::Vector2d &pointA,
                                         const Eigen::Vector2d &pointB)
{
  return computeCrossProduct2D(pointA - pointOrigin, pointB - pointOrigin) <= 0;
}

const std::vector<Position> Polygon::getInterpolatedConvexHull(const double stepSize) {
  std::vector<Position> result;
  Polygon hull = monotoneChainConvexHullOfPoints(this->getVertices());

  for (int curIdx = 0; curIdx < hull.nVertices(); curIdx++){
    int nextIdx = curIdx < hull.nVertices() -1 ? curIdx + 1 : 0;

    Position curPos = hull.getVertex(curIdx);
    Position nextPos = hull.getVertex(nextIdx);

    Position diff = nextPos - curPos;
    double diffDistance = diff.norm();
    int steps = std::max(1, (int) ceil(diffDistance / stepSize));
    Position direction(
      diff[0] / steps,
      diff[1] / steps
    );

    result.push_back(curPos);
    for (int i = 0; i < steps; i++) {
      double factor = ((double) i) / steps;
      Position tmp = curPos + factor * diff;
      result.push_back(tmp);
    }
  }

  return result;
}

} /* namespace grid_map */


#pragma once

#include <iostream>

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdeprecated-register"
#include <Eigen/Dense>
#include <g2o/core/base_vertex.h>
#pragma clang diagnostic pop

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
        Eigen::RowMajor> Mat;
using Eigen::Vector3d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 6, 6> Matrix6d;

//===========================================================================
class VertexPositionVelocity3D : public g2o::BaseVertex<6, Vector6d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexPositionVelocity3D() {
      _estimate.setZero();
    }

  virtual void setToOriginImpl() {
    _estimate.setZero();
  }

  virtual void oplusImpl(const double* update) {
    for (int k = 0; k < 6; k++)
      _estimate[k] += update[k];
  }

  virtual bool read(std::istream& /*is*/) { return false; }
  virtual bool write(std::ostream& /*os*/) const { return false; }
};


//==================================================================================================
/*
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
*/
//==================================================================================================
#include <eve/function/cyl_bessel_j1.hpp>
#include <eve/constant/valmin.hpp>
#include <eve/constant/valmax.hpp>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>

int main()
{
  auto lmin = EVE_VALUE(0);
  auto lmax = EVE_VALUE(10);

  auto arg0 = eve::bench::random_<EVE_VALUE>(lmin,lmax);
//  auto stdj1 = [](auto x){return std::cyl_bessel_j(0, x);};
  auto boostj1= [](auto x){return boost::math::detail::bessel_j1(x);};
  eve::bench::experiment xp;
  run<EVE_TYPE> (EVE_NAME(cyl_bessel_j1) , xp, eve::cyl_bessel_j1 , arg0);
  run<EVE_VALUE>(EVE_NAME(cyl_bessel_j1) , xp, eve::cyl_bessel_j1 , arg0);
//  run<EVE_VALUE>(EVE_NAME(stdj1) , xp, stdj1 , arg0);
  run<EVE_VALUE>(EVE_NAME(boostj1), xp, boostj1 , arg0);
}


// Main code for Bayesian Inference with DIAMONDS
// Created by Enrico Corsaro @ OACT - February 2017
// e-mail: emncorsaro@gmail.com
// Source code file "SuperGaussianModel.cpp"

// To compile in Mac OS:
// clang++ -o SuperGaussianFit SuperGaussianFit.cpp -L../../build/ -I ../../include/ -l diamonds -stdlib=libc++ -std=c++11 -Wno-deprecated-register
// To compile in Linux OS:
// g++ -o SuperGaussianFit SuperGaussianFit.cpp -L../../build/ -I../../include/ -ldiamonds -std=c++11

#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <Eigen/Dense>
#include "Functions.h"
#include "File.h"
#include "MultiEllipsoidSampler.h"
#include "KmeansClusterer.h"
#include "EuclideanMetric.h"
#include "Prior.h"
#include "UniformPrior.h"
#include "NormalPrior.h"
#include "SuperGaussianModel.h"
#include "NormalLikelihood.h"
#include "FerozReducer.h"
#include "PowerlawReducer.h"
#include "Results.h"
#include "Ellipsoid.h"
#include "PrincipalComponentProjector.h"

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

    // Check number of arguments for main function

    if (argc != 3)
    {
        cerr << "Usage: ./SuperGaussianFit <data filename> <prior filename>" << endl;
        exit(EXIT_FAILURE);
    }

    // ---------------------------
    // ----- Read input data -----
    // ---------------------------

    unsigned long Nrows;
    int Ndimensions;              // Number of parameters for which prior distributions are defined
    int Ncols;
    ArrayXXd data;

    string baseInputDirName = "";
    string inputFileName(argv[1]);
    string outputPathPrefix = "SuperGaussianFit_";

    ifstream inputFile;
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nrows, Ncols);
    data = File::arrayXXdFromFile(inputFile, Nrows, Ncols);
    inputFile.close();

    // Creating arrays for each data type

    ArrayXd covariates = data.col(0);
    ArrayXd observations = data.col(1);
    ArrayXd uncertainties = data.col(2);

    // -------------------------------------------------------
    // ----- First step. Set up all prior distributions -----
    // -------------------------------------------------------

    // Uniform Prior
    unsigned long Nparameters;

    // ---- Read prior hyper parameters for resolved modes -----
    string inputFileNamePrior(argv[2]);
    File::openInputFile(inputFile, inputFileNamePrior);
    File::sniffFile(inputFile, Nparameters, Ncols);
    ArrayXXd hyperParameters;
    Ndimensions = Nparameters;

    if (Ncols == 1)
    {
        Ncols = 2;
        hyperParameters.conservativeResize(Nparameters, Ncols);
    }

    hyperParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    ArrayXd hyperParametersMinima = hyperParameters.col(0);
    ArrayXd hyperParametersMaxima = hyperParameters.col(1);

    int NpriorTypes = 1;                                        // Total number of prior types included in the computation
    vector<Prior*> ptrPriors(NpriorTypes);
    ArrayXd parametersMinima(Ndimensions);
    ArrayXd parametersMaxima(Ndimensions);
    parametersMinima << hyperParametersMinima;      // Minima values for the free parameters (free parameter #1, free parameter #2, ..., etc.)
    parametersMaxima << hyperParametersMaxima;      // Maxima values for the free parameters (same order as minima)
    UniformPrior uniformPrior(parametersMinima, parametersMaxima);
    ptrPriors[0] = &uniformPrior;

    string fullPathHyperParameters = outputPathPrefix + "hyperParametersUniform.txt";       // Print prior hyper parameters as output
    uniformPrior.writeHyperParametersToFile(fullPathHyperParameters);

    // -------------------------------------------------------------------
    // ---- Second step. Set up the models for the inference problem -----
    // -------------------------------------------------------------------

    SuperGaussianModel model(covariates);      // Super Gaussian function

    // -----------------------------------------------------------------
    // ----- Third step. Set up the likelihood function to be used -----
    // -----------------------------------------------------------------

    NormalLikelihood likelihood(observations, uncertainties, model);

    // -------------------------------------------------------------------------------
    // ----- Fourth step. Set up the K-means clusterer using an Euclidean metric -----
    // -------------------------------------------------------------------------------

    inputFileName = "Xmeans_configuringParameters.txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);

    if (Nparameters != 2)
    {
        cerr << "Wrong number of input parameters for X-means algorithm." << endl;
        exit(EXIT_FAILURE);
    }

    ArrayXd configuringParameters;
    configuringParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    int minNclusters = configuringParameters(0);
    int maxNclusters = configuringParameters(1);

    if ((minNclusters <= 0) || (maxNclusters <= 0) || (maxNclusters < minNclusters))
    {
        cerr << "Minimum or maximum number of clusters cannot be <= 0, and " << endl;
        cerr << "minimum number of clusters cannot be larger than maximum number of clusters." << endl;
        exit(EXIT_FAILURE);
    }

    int Ntrials = 10;
    double relTolerance = 0.01;

    bool printNdimensions = false;
    PrincipalComponentProjector projector(printNdimensions);
    bool featureProjectionActivated = true;

    EuclideanMetric myMetric;
    KmeansClusterer kmeans(myMetric, projector, featureProjectionActivated,
                           minNclusters, maxNclusters, Ntrials, relTolerance);

    // ---------------------------------------------------------------------
    // ----- Sixth step. Configure and start nested sampling inference -----
    // ---------------------------------------------------------------------

    inputFileName = "NSMC_configuringParameters.txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);
    configuringParameters.setZero();
    configuringParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    if (Nparameters != 8)
    {
        cerr << "Wrong number of input parameters for NSMC algorithm." << endl;
        exit(EXIT_FAILURE);
    }

    bool printOnTheScreen = true;                       // Print results on the screen
    int initialNobjects = configuringParameters(0);     // Initial number of live points
    int minNobjects = configuringParameters(1);         // Minimum number of live points
    int maxNdrawAttempts = configuringParameters(2);    // Maximum number of attempts when trying to draw a new sampling point
    int NinitialIterationsWithoutClustering = configuringParameters(3); // The first N iterations, we assume that there is only 1 cluster
    int NiterationsWithSameClustering = configuringParameters(4);       // Clustering is only happening every N iterations.
    double initialEnlargementFraction = configuringParameters(5);   // Fraction by which each axis in an ellipsoid has to be enlarged.
                                                                    // It can be a number >= 0, where 0 means no enlargement.
    double shrinkingRate = configuringParameters(6);        // Exponent for remaining prior mass in ellipsoid enlargement fraction.
                                                            // It is a number between 0 and 1. The smaller the slower the shrinkage
                                                            // of the ellipsoids.
    double terminationFactor = configuringParameters(7);    // Termination factor for nested sampling process.

    MultiEllipsoidSampler nestedSampler(printOnTheScreen, ptrPriors, likelihood, myMetric, kmeans,
                                        initialNobjects, minNobjects, initialEnlargementFraction, shrinkingRate);

    double tolerance = 1.e2;
    double exponent = 0.4;
    PowerlawReducer livePointsReducer(nestedSampler, tolerance, exponent, terminationFactor);

    nestedSampler.run(livePointsReducer, NinitialIterationsWithoutClustering, NiterationsWithSameClustering,
                      maxNdrawAttempts, terminationFactor, 0, outputPathPrefix);

    nestedSampler.outputFile << "# List of configuring parameters used for the ellipsoidal sampler and X-means" << endl;
    nestedSampler.outputFile << "# Row #1: Minimum Nclusters" << endl;
    nestedSampler.outputFile << "# Row #2: Maximum Nclusters" << endl;
    nestedSampler.outputFile << "# Row #3: Initial Enlargement Fraction" << endl;
    nestedSampler.outputFile << "# Row #4: Shrinking Rate" << endl;
    nestedSampler.outputFile << minNclusters << endl;
    nestedSampler.outputFile << maxNclusters << endl;
    nestedSampler.outputFile << initialEnlargementFraction << endl;
    nestedSampler.outputFile << shrinkingRate << endl;
    nestedSampler.outputFile.close();

    // -------------------------------------------------------
    // ----- Last step. Save the results in output files -----
    // -------------------------------------------------------

    Results results(nestedSampler);
    results.writeParametersToFile("parameter");
    results.writeLogLikelihoodToFile("logLikelihood.txt");
    results.writeLogWeightsToFile("logWeights.txt");
    results.writeEvidenceInformationToFile("evidenceInformation.txt");
    results.writePosteriorProbabilityToFile("posteriorDistribution.txt");
    results.writeLogEvidenceToFile("logEvidence.txt");
    results.writeLogMeanLiveEvidenceToFile("logMeanLiveEvidence.txt");

    double credibleLevel = 68.3;
    bool writeMarginalDistributionToFile = true;
    results.writeParametersSummaryToFile("parameterSummary.txt", credibleLevel, writeMarginalDistributionToFile);

    cout << "Process completed." << endl;

    return EXIT_SUCCESS;
}


#include "PG/core/RectUtils.h"

#include <cmath>

#ifdef __APPLE__

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wsign-conversion"
#pragma clang diagnostic ignored "-Wconversion"
#pragma clang diagnostic ignored "-Wshadow"

#endif

#include <boost/geometry.hpp>

#ifdef __APPLE__

#pragma clang diagnostic pop

#endif

namespace PG {

namespace
{
    using bPoint = boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian>;
    using bRect = boost::geometry::model::box<bPoint>;

    //--------------------------------------------------------
    bPoint toBPoint(const Point& pt)
    {
        return bPoint(pt.x, pt.y);
    }

    //--------------------------------------------------------
    bRect toBRect(const Rect& r)
    {
        return bRect(bPoint(r.origin.x - (r.size.width / 2.0), r.origin.y - (r.size.height / 2.0)),
                     bPoint(r.origin.x + (r.size.width / 2.0), r.origin.y + r.size.height / 2.0));
    }

    //--------------------------------------------------------
    Rect toPGRect(const bRect& r)
    {
        auto ox = r.min_corner().get<0>();
        auto oy = r.min_corner().get<1>();

        auto w = r.max_corner().get<0>() - ox;
        auto h = r.max_corner().get<1>() - oy;

        return Rect(Point(ox, oy), Size(w, h));
    }
}

//--------------------------------------------------------
namespace RectUtils
{
    //--------------------------------------------------------
    Rect getIntersection(const Rect& rectOne, const Rect& rectTwo)
    {
        auto bRectOne = toBRect(rectOne);
        auto bRectTwo = toBRect(rectTwo);

        if (!boost::geometry::intersects(bRectOne, bRectTwo))
        {
            return Rect();
        }

        bRect intersection;
        boost::geometry::intersection(bRectOne, bRectTwo, intersection);

        return toPGRect(intersection);
   }

   //--------------------------------------------------------
   bool isEmpty(const Rect& r)
   {
       return isEmpty(r.size);
   }

   //--------------------------------------------------------
   bool isEmpty(const Size& s)
   {
       return s.width == 0 || s.height == 0;
   }

   //--------------------------------------------------------
   bool containsPoint(const Rect& r, const Point& pt)
   {
        return boost::geometry::covered_by(toBPoint(pt), toBRect(r));
   }

   //--------------------------------------------------------
   Rect combineRects(const Rect& r1, const Rect& r2)
   {
       if (isEmpty(r1))
       {
           return r2;
       }

       if (isEmpty(r2))
       {
           return r1;
       }

       auto minLeft = std::min(r1.left(), r2.left());
       auto maxRight = std::max(r1.right(), r2.right());
       auto minTop = std::min(r1.top(), r2.top());
       auto maxBottom = std::max(r1.bottom(), r2.bottom());

       Size combinedSize(maxRight - minLeft, maxBottom - minTop);
       Point newOrigin(minLeft + (combinedSize.width / 2.0), maxBottom - (combinedSize.height / 2.0));

       return Rect(newOrigin, combinedSize);
   }
}

}