// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "five_point_relative_pose.h"

#include <Eigen/Dense>

#include <cmath>
#include <ctime>
#include <vector>

using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::RowVector3d;
using Eigen::RowVector4d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;
using Eigen::VectorXd;

typedef Matrix<double, 10, 10> Matrix10d;

// Multiply two degree one polynomials of variables x, y, z.
// E.g. p1 = a[0]x + a[1]y + a[2]z + a[3]
// Output order: x^2 xy y^2 xz yz z^2 x y z 1 (GrevLex)
Matrix<double, 1, 10> MultiplyDegOnePoly(const RowVector4d& a,
                                         const RowVector4d& b) {
  Matrix<double, 1, 10> output;
  // x^2
  output(0) = a(0) * b(0);
  // xy
  output(1) = a(0) * b(1) + a(1) * b(0);
  // y^2
  output(2) = a(1) * b(1);
  // xz
  output(3) = a(0) * b(2) + a(2) * b(0);
  // yz
  output(4) = a(1) * b(2) + a(2) * b(1);
  // z^2
  output(5) = a(2) * b(2);
  // x
  output(6) = a(0) * b(3) + a(3) * b(0);
  // y
  output(7) = a(1) * b(3) + a(3) * b(1);
  // z
  output(8) = a(2) * b(3) + a(3) * b(2);
  // 1
  output(9) = a(3) * b(3);
  return output;
}

// Multiply a 2 deg poly (in x, y, z) and a one deg poly in GrevLex order.
// x^3 x^2y xy^2 y^3 x^2z xyz y^2z xz^2 yz^2 z^3 x^2 xy y^2 xz yz z^2 x y z 1
Matrix<double, 1, 20> MultiplyDegTwoDegOnePoly(const Matrix<double, 1, 10>& a,
                                               const RowVector4d& b) {
  Matrix<double, 1, 20> output;
  // x^3
  output(0) = a(0) * b(0);
  // x^2y
  output(1) = a(0) * b(1) + a(1) * b(0);
  // xy^2
  output(2) = a(1) * b(1) + a(2) * b(0);
  // y^3
  output(3) = a(2) * b(1);
  // x^2z
  output(4) = a(0) * b(2) + a(3) * b(0);
  // xyz
  output(5) = a(1) * b(2) + a(3) * b(1) + a(4) * b(0);
  // y^2z
  output(6) = a(2) * b(2) + a(4) * b(1);
  // xz^2
  output(7) = a(3) * b(2) + a(5) * b(0);
  // yz^2
  output(8) = a(4) * b(2) + a(5) * b(1);
  // z^3
  output(9) = a(5) * b(2);
  // x^2
  output(10) = a(0) * b(3) + a(6) * b(0);
  // xy
  output(11) = a(1) * b(3) + a(6) * b(1) + a(7) * b(0);
  // y^2
  output(12) = a(2) * b(3) + a(7) * b(1);
  // xz
  output(13) = a(3) * b(3) + a(6) * b(2) + a(8) * b(0);
  // yz
  output(14) = a(4) * b(3) + a(7) * b(2) + a(8) * b(1);
  // z^2
  output(15) = a(5) * b(3) + a(8) * b(2);
  // x
  output(16) = a(6) * b(3) + a(9) * b(0);
  // y
  output(17) = a(7) * b(3) + a(9) * b(1);
  // z
  output(18) = a(8) * b(3) + a(9) * b(2);
  // 1
  output(19) = a(9) * b(3);
  return output;
}

Matrix<double, 1, 20> GetDeterminantConstraint(
    const Matrix<double, 1, 4> null_space[3][3]) {
  // Singularity constraint.
  const Matrix<double, 1, 20> determinant =
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space[0][1], null_space[1][2]) -
          MultiplyDegOnePoly(null_space[0][2], null_space[1][1]),
          null_space[2][0]) +
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space[0][2], null_space[1][0]) -
          MultiplyDegOnePoly(null_space[0][0], null_space[1][2]),
          null_space[2][1]) +
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space[0][0], null_space[1][1]) -
          MultiplyDegOnePoly(null_space[0][1], null_space[1][0]),
          null_space[2][2]);
  return determinant;
}

// Shorthand for multiplying the Essential matrix with its transpose.
Matrix<double, 1, 10> EETranspose(
    const Matrix<double, 1, 4> null_space[3][3], int i, int j) {
  return MultiplyDegOnePoly(null_space[i][0], null_space[j][0]) +
      MultiplyDegOnePoly(null_space[i][1], null_space[j][1]) +
      MultiplyDegOnePoly(null_space[i][2], null_space[j][2]);
}

// Builds the trace constraint: EEtE - 1/2 trace(EEt)E = 0
Matrix<double, 9, 20> GetTraceConstraint(
    const Matrix<double, 1, 4> null_space[3][3]) {
  Matrix<double, 9, 20> trace_constraint;

  // Comput EEt.
  Matrix<double, 1, 10> eet[3][3];
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      eet[i][j] = 2 * EETranspose(null_space, i, j);
    }
  }

  // Compute the trace.
  const Matrix<double, 1, 10> trace = eet[0][0] + eet[1][1] + eet[2][2];

  // Multiply EEt with E.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      trace_constraint.row(3 * i + j) =
          MultiplyDegTwoDegOnePoly(eet[i][0], null_space[0][j]) +
          MultiplyDegTwoDegOnePoly(eet[i][1], null_space[1][j]) +
          MultiplyDegTwoDegOnePoly(eet[i][2], null_space[2][j]) -
          0.5 * MultiplyDegTwoDegOnePoly(trace, null_space[i][j]);
    }
  }

  return trace_constraint;
}

Matrix<double, 10, 20> BuildConstraintMatrix(
    const Matrix<double, 1, 4> null_space[3][3]) {
  Matrix<double, 10, 20> constraint_matrix;
  constraint_matrix.block<9, 20>(0, 0) = GetTraceConstraint(null_space);
  constraint_matrix.row(9) = GetDeterminantConstraint(null_space);
  return constraint_matrix;
}

// Implementation of Nister from "An Efficient Solution to the Five-Point
// Relative Pose Problem"
bool FivePointRelativePose(const std::vector<Vector2d>& image1_points,
                           const std::vector<Vector2d>& image2_points,
                           std::vector<Matrix3d>* essential_matrices) {

  // Step 1. Create the nx9 matrix containing epipolar constraints.
  //   Essential matrix is a linear combination of the 4 vectors spanning the
  //   null space of this matrix.
  MatrixXd epipolar_constraint(image1_points.size(), 9);
  for (int i = 0; i < image1_points.size(); i++) {
    // Fill matrix with the epipolar constraint from q'_t*E*q = 0. Where q is
    // from the first image, and q' is from the second.
    epipolar_constraint.row(i) <<
        image2_points[i].x() * image1_points[i].x(),
        image2_points[i].y() * image1_points[i].x(),
        image1_points[i].x(),
        image2_points[i].x() * image1_points[i].y(),
        image2_points[i].y() * image1_points[i].y(),
        image1_points[i].y(),
        image2_points[i].x(),
        image2_points[i].y(),
        1.0;
  }

  Matrix<double, 9, 4> null_space;

  // Extract the null space from a minimal sampling (using LU) or non-minimal
  // sampling (using SVD).
  if (image1_points.size() == 5) {
    const Eigen::FullPivLU<MatrixXd> lu(epipolar_constraint);
    if (lu.dimensionOfKernel() != 4) {
      return false;
    }
    null_space = lu.kernel();
  } else {
    const Eigen::JacobiSVD<MatrixXd> svd(
        epipolar_constraint.transpose() * epipolar_constraint,
        Eigen::ComputeFullV);
    null_space = svd.matrixV().rightCols<4>();
  }

  const Matrix<double, 1, 4> null_space_matrix[3][3] = {
    { null_space.row(0), null_space.row(3), null_space.row(6) },
    { null_space.row(1), null_space.row(4), null_space.row(7) },
    { null_space.row(2), null_space.row(5), null_space.row(8) }
  };

  // Step 2. Expansion of the epipolar constraints on the determinant and trace.
  const Matrix<double, 10, 20> constraint_matrix =
      BuildConstraintMatrix(null_space_matrix);

  // Step 3. Eliminate part of the matrix to isolate polynomials in z.
  Eigen::FullPivLU<Matrix10d> c_lu(constraint_matrix.block<10, 10>(0, 0));
  Matrix10d eliminated_matrix =
      c_lu.solve(constraint_matrix.block<10, 10>(0, 10));

  Matrix10d action_matrix = Matrix10d::Zero();
  action_matrix.block<3, 10>(0, 0) = eliminated_matrix.block<3, 10>(0, 0);
  action_matrix.row(3) = eliminated_matrix.row(4);
  action_matrix.row(4) = eliminated_matrix.row(5);
  action_matrix.row(5) = eliminated_matrix.row(7);
  action_matrix(6, 0) = -1.0;
  action_matrix(7, 1) = -1.0;
  action_matrix(8, 3) = -1.0;
  action_matrix(9, 6) = -1.0;

  Eigen::EigenSolver<Matrix10d> eigensolver(action_matrix);
  const auto& eigenvectors = eigensolver.eigenvectors();
  const auto& eigenvalues = eigensolver.eigenvalues();

  // Now that we have x, y, and z we need to substitute them back into the null
  // space to get a valid essential matrix solution.
  for (int i = 0; i < 10; i++) {
    // Only consider real solutions.
    if (eigenvalues(i).imag() != 0) {
      continue;
    }
    Matrix3d ematrix;
    Map<Matrix<double, 9, 1> >(ematrix.data()) =
        null_space * eigenvectors.col(i).tail<4>().real();
    essential_matrices->emplace_back(ematrix);
  }

  return essential_matrices->size() > 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <vector>
#include <Eigen/Dense>

#include <dpMM/gpuMatrix.hpp>
#include <dpMM/sphere.hpp>
#include <dpMM/clGMMDataGpu.hpp>
#include <dpMM/timer.hpp>

using namespace Eigen;
using std::vector;

extern  void Log_p_gpu(double *p, double *q, double *Rs, uint32_t *z,
  uint32_t K, uint32_t N, double *x);
extern void meanInTpS2_gpu(double *d_p, double *d_mu_karch, double *d_q,
    uint32_t *d_z , uint32_t N, uint32_t K);
extern void sufficientStatisticsOnTpS2_gpu(double *d_p, double *d_Rnorths,
    double *d_q, uint32_t *d_z , uint32_t N, uint32_t k0, uint32_t K,
    double *d_SSs);
extern void sphereGMMPdf(double *d_p, double *d_Rnorths, double * d_q,
    double *d_invSigmas, double *d_logNormalizer, double *d_logPi,
    double* d_logPdf, uint32_t N, uint32_t K);

extern  void Log_p_gpu(float *p, float *q, float *Rs, uint32_t *z,
  uint32_t K, uint32_t N, float *x);
extern void meanInTpS2_gpu(float *d_p, float *d_mu_karch, float *d_q,
    uint32_t *d_z , uint32_t N, uint32_t K);
extern void sufficientStatisticsOnTpS2_gpu(float *d_p, float *d_Rnorths,
    float *d_q, uint32_t *d_z , uint32_t N, uint32_t k0, uint32_t K,
    float *d_SSs);
extern void sphereGMMPdf(float *d_p, float *d_Rnorths, float * d_q,
    float *d_invSigmas, float *d_logNormalizer, float *d_logPi,
    float* d_logPdf, uint32_t N, uint32_t K);

template<typename T>
class ClTGMMDataGpu : public ClGMMDataGpu<T>
{
protected:
  GpuMatrix<T> d_q_; // normals on GPU
//  GpuMatrix<uint32_t> d_z_; // indicators on GPU
//  GpuMatrix<T> d_x_; // points in tangent plane (dim. is 1 less than q)

  //uint32_t K_; // number of different tangent planes
  GpuMatrix<T> d_ps_; // points around which we have tangent spaces
  GpuMatrix<T> d_muKarch_; // karcher means
//  GpuMatrix<T> d_Ss_; // sufficient statistics in tangent spaces
  GpuMatrix<T> d_northRps_; // rotations from tangent spaces to north

//  GpuMatrix<T> d_pdfs_; //
//  GpuMatrix<T> d_logPi_; //
//  GpuMatrix<T> d_logNormalizers_; //
//  GpuMatrix<T> d_invSigmas_; //

  Matrix<T,Dynamic,Dynamic> ps_;

  Sphere<T> sphere_;

public:
  ClTGMMDataGpu(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& q,
      const spVectorXu& z, uint32_t K);
  ~ClTGMMDataGpu(){;};
  virtual void init(const Matrix<T,Dynamic,Dynamic>& ps);

  /* normal in tangent space around p rotate to the north pole
   * -> the last dimension will always be 0
   * -> return only first 2 dims
   */
  void Log_p_north(const Matrix<T,Dynamic,Dynamic>& ps,
      const VectorXu& z, Matrix<T,Dynamic,Dynamic>& x, int32_t K=-1);
  void Log_p_north(const Matrix<T,Dynamic,Dynamic>& ps,
      const VectorXu& z, int32_t K=-1);

  Matrix<T,Dynamic,Dynamic> Log_p_north(const Matrix<T,Dynamic,Dynamic>& ps,
      int32_t K=-1)
  {
    Matrix<T,Dynamic,Dynamic> x(this->d_x_.rows(),this->d_x_.cols());
    Log_p_north(ps, *this->z_, x, K);
    return x;
  };

  virtual void update(uint32_t K);

  void relinearize(const Matrix<T,Dynamic,Dynamic>& ps);

  Matrix<T,Dynamic,Dynamic> karcherMeans(const Matrix<T,Dynamic,Dynamic>& p0,
      uint32_t maxIter = 50);

  virtual void computeLogLikelihoods(const Matrix<T,Dynamic,1>& pi,
    const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
    const Matrix<T,Dynamic,1>& logNormalizers);
  virtual void sampleGMMpdf(const Matrix<T,Dynamic,1>& pi,
      const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
      const Matrix<T,Dynamic,1>& logNormalizers, Sampler<T> *sampler);

  const Matrix<T,Dynamic,Dynamic>& ps() const {return ps_;};
  const Matrix<T,Dynamic,1>& p(uint32_t k) const {return ps_.col(k);};
  // TODO: broke compatibility! this used to be ps
//  Matrix<T,Dynamic,1> mean(uint32_t k) const {return ps_.col(k);};
//  const Matrix<T,Dynamic,Dynamic>& means() const {return ps_;};

  virtual const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& q() const
  {return this->x_;};
  virtual const Matrix<T,Dynamic,Dynamic>& qMat() const
  {return (*this->x_);};

  GpuMatrix<T>& d_q(){ return d_q_;};

protected:
  Matrix<T,Dynamic,Dynamic> meanInTpS2(const Matrix<T,Dynamic,Dynamic>& ps);
  Matrix<T,Dynamic,Dynamic> karcherMeans__(const Matrix<T,Dynamic,Dynamic>& p0,
      uint32_t maxIter = 50);
};

typedef ClTGMMDataGpu<double> ClTGMMDataGpud;
typedef ClTGMMDataGpu<float> ClTGMMDataGpuf;

// ------------------------------------ impl --------------------------------
template<typename T>
ClTGMMDataGpu<T>::ClTGMMDataGpu(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& q,
    const spVectorXu& z, uint32_t K)
  : ClGMMDataGpu<T>(q,z,K), d_q_(this->D_,this->N_),
    d_ps_(this->D_,this->K_), d_muKarch_(this->D_+1,this->K_),
    d_northRps_(this->D_-1,this->K_*this->D_),
    ps_(Matrix<T,Dynamic,Dynamic>::Zero(this->D_,this->K_)),
    sphere_(this->D_)
{};

template<typename T>
void ClTGMMDataGpu<T>::init(const Matrix<T,Dynamic,Dynamic>& ps)
{
  for(uint32_t i=0; i<this->N_; ++i)
    if(fabs(this->x_->col(i).norm()-1.0) > 1e-5 )
    {
        cout<<"ClTGMMDataGpu<T>::ClTGMMDataGpu: warning: renormalizing normal "
          <<this->x_->col(i).norm()<<endl;
      this->x_->col(i) /= this->x_->col(i).norm();
    }
  d_q_.set(*this->x_);
  this->d_x_.setZero();

  // resize the statistyics to be one less dim, since we are in
  // the tangent planes
  this->Ns_.setZero(this->K_);
  this->means_.setZero(this->D_-1,this->K_);
  for(uint32_t k=0; k<this->K_; ++k)
    this->Ss_[k].setZero(this->D_-1,this->D_-1);

  ps_ = ps;
//  for(uint32_t k=0; k<this->K_; ++k)
//    ps_.col(k) = sphere_.sampleUnif(this->pRndGen_);
};

template<typename T>
void ClTGMMDataGpu<T>::update(uint32_t K)
{
  this->K_ = K>0?K:this->z_->maxCoeff()+1;
  assert(this->z_->maxCoeff() < this->K_); // no indicators \geq K
  assert(this->z_->minCoeff() >= 0); // no indicators \le 0
  assert((this->z_->array() < this->K_).all());
  // update the labels on the GPU
  this->d_z_.set(this->z_);
  //TODO shouldnt need to do this everytime?
  //d_q_.set(*this->x_);
  // run karcher means to obtain ps
//  Matrix<T,Dynamic,Dynamic> q = d_q_.get();
//  cout<<q<<endl;
  Timer t;
  ps_ = karcherMeans__(ps_);
  t.toctic("karcherMeansFull");
  cout<<"linearize around: \n"<<ps_<<endl;
  // relinearize around the new centers
  t.tic();
  relinearize(ps_);
  t.toctic("relinearize");

//  // put points into tangent spaces - relinearize
//  Log_p_gpu(d_ps_.data(), d_q_.data(), d_northRps_.data(), this->d_z_.data(), this->K_,
//      d_q_.cols(),d_this->x_.data());
  // compute moments
  this->computeSufficientStatistics();
  t.toctic("computeSufficientStatistics");
}

template<typename T>
void ClTGMMDataGpu<T>::relinearize(const Matrix<T,Dynamic,Dynamic>& ps)
{
  // TODO: need to call udapte Labels before this function

  // compute rotations to north and push them to GPU
  Matrix<T,Dynamic,Dynamic> Rs(ps.rows()-1, this->K_*3);
  for(uint32_t k=0; k<this->K_; ++k)
    Rs.middleCols(k*3,3) = sphere_.north_R_TpS2(ps.col(k)).topRows(2);

  d_ps_.set(ps);
  d_northRps_.set(Rs);

  // q is already in GPU since construction
//  d_ps_.print(); d_q_.print(); d_northRps_.print(); this->d_z_.print(); d_q_.print();
//  this->d_x_.print();

  Log_p_gpu(d_ps_.data(), d_q_.data(), d_northRps_.data(), this->d_z_.data(),
      this->K_, d_q_.cols(), this->d_x_.data());

//  d_northRps_.get(Rs);
//  cout<<Rs<<endl;
//  Matrix<T,Dynamic,Dynamic> pps(ps.rows(),ps.cols());
//  d_ps_.get(pps);
//  cout<<pps<<endl;
};

template<typename T>
Matrix<T,Dynamic,Dynamic> ClTGMMDataGpu<T>::meanInTpS2(
    const Matrix<T,Dynamic,Dynamic>& ps)
{
  assert(ps.cols() == this->K_);
  assert(ps.rows() == this->D_);

  // one more dimension to hold the counts
  Matrix<T,Dynamic,Dynamic> muKarch =
    Matrix<T,Dynamic,Dynamic>::Zero(this->D_+1,this->K_);
  d_muKarch_.set(muKarch);
  d_ps_.set(ps);

//  cout<<"ClTGMMDataGpu<T>::meanInTpS2: mean starting from "<<endl<<ps<<endl;
//  cout<<d_ps_.rows()<<" "<<d_ps_.cols()<<endl;
//  cout<<d_muKarch_.rows()<<" "<<d_muKarch_.cols()<<endl;
//  cout<<d_q_.rows()<<" "<<d_q_.cols()<<endl;
//  cout<<this->d_z_.rows()<<" "<<this->d_z_.cols()<<endl;

//  cout<<d_ps_.get()<<endl;
//  cout<<d_muKarch_.get()<<endl;
//  cout<<d_q_.get()<<endl;
//  cout<<this->d_z_.get()<<endl;

  meanInTpS2_gpu(d_ps_.data(), d_muKarch_.data(), d_q_.data(), this->d_z_.data(),
      this->N_,this->K_);
  //meanInTpS2GPU(h_p, d_p_, h_mu_karch, d_mu_karch_, d_q_, d_z, w_, h_);
  d_muKarch_.get(muKarch);

  Matrix<T,Dynamic,Dynamic> mu = muKarch.topRows(this->D_);
  for(uint32_t k=0; k<this->K_; ++k)
    if(muKarch(this->D_,k) > 0)
    {
      mu.col(k) /= muKarch(this->D_,k);
    }

//  cout<<muKarch<<endl<<endl;
//  cout<<mu<<endl;
  return mu;
}

template<typename T>
Matrix<T,Dynamic,Dynamic> ClTGMMDataGpu<T>::karcherMeans(const Matrix<T,Dynamic,Dynamic>& p0, uint32_t maxIter)
{
  // stand alone karcher means
  assert(this->z_->maxCoeff() < this->K_); // no indicators \geq K
  assert(this->z_->minCoeff() >= 0); // no indicators \le 0
  assert((this->z_->array() < this->K_).all());
  // update the labels on the GPU
  this->d_z_.set(this->z_);

  Matrix<T,Dynamic,Dynamic> p(p0.rows(),p0.cols());
  Timer t;
  p = karcherMeans__(p0);
  t.toctic("karcherMeansFull");
  return p;
};

template<typename T>
Matrix<T,Dynamic,Dynamic> ClTGMMDataGpu<T>::karcherMeans__(const Matrix<T,Dynamic,Dynamic>& p0, uint32_t maxIter)
{

  assert(p0.rows() == d_ps_.rows()); // we dont want dimension change
  Matrix<T,Dynamic,Dynamic> p = p0;
//  cout<<"p0"<<endl<<p<<endl;

  Matrix<T,Dynamic,1> residual(this->K_);
  residual.setOnes(this->K_);
//  cout<<(this->z_->transpose())<<endl;
  for(uint32_t i=0; i< maxIter; ++i)
  {
//    Timer t0;
    Matrix<T,Dynamic,Dynamic> mu_karch = meanInTpS2(p);
//    t0.toctic("meanInTpS2_GPU");
//    cout<<"mu_karch"<<endl<<mu_karch<<endl;
//    cout<<"p"<<endl<<p<<endl;
    for (uint32_t k=0; k<this->K_; ++k)
    {
      p.col(k) = sphere_.Exp_p(p.col(k),mu_karch.col(k));
//      cout<<p.col(k).norm()<<endl;
      residual(k) = mu_karch.col(k).norm();
    }
//    cout<<"p"<<endl<<p<<endl;
    //cout<<"karcherMeans "<<i<<" residual="<<residual<<endl;
//    cout<<"@"<<i<<" residual = "<<residual.transpose()<<endl;
    if((residual.array() < 1e-5).all())
    {
      cout<<"ClTGMMDataGpu<T>::karcherMeans__: converged after "<<i
        <<" residual = "<<residual.transpose()<<endl;
      assert((residual.array() != 0.0).any());
//      assert(i>0); // first itaration convergence is rare
      break;
    }
  }
//  cout<<"p"<<endl<<p<<endl;
  return p;
}

template<typename T>
void ClTGMMDataGpu<T>::Log_p_north(const Matrix<T,Dynamic,Dynamic>& ps,
    const VectorXu& z, Matrix<T,Dynamic,Dynamic>& x, int32_t K)
{
  Log_p_north(ps,z,K);
  this->d_x_.get(x);
};

template<typename T>
void ClTGMMDataGpu<T>::Log_p_north(const Matrix<T,Dynamic,Dynamic>& ps,
    const VectorXu& z,  int32_t K)
{
  if(K > 0){
    this->K_ = K>0?K:this->z_->maxCoeff()+1;
//    cout<<this->K_<<" "<< uint32_t(this->z_->maxCoeff()) <<endl;
    assert(this->z_->maxCoeff() < this->K_); // no indicators \geq K
    assert(this->z_->minCoeff() >= 0); // no indicators \le 0
    assert((this->z_->array() < this->K_).all());
  }
  // updates d_ps_, d_northRps_, this->d_z_, this->K_
  relinearize(ps);
  this->d_z_.set(z);
  //TODO shouldnt need to do this everytime?
  d_q_.set(*this->x_);
  // q is already in GPU since construction
//  d_ps_.print();
//  d_q_.print();
//  d_northRps_.print();
//  this->d_z_.print();
//  d_q_.print();
//  this->d_x_.print();

  Log_p_gpu(d_ps_.data(), d_q_.data(), d_northRps_.data(), this->d_z_.data(),
      this->K_, d_q_.cols(),this->d_x_.data());
};

template<typename T>
void ClTGMMDataGpu<T>::computeLogLikelihoods(const Matrix<T,Dynamic,1>& pi,
    const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
    const Matrix<T,Dynamic,1>& logNormalizers)
{
//  cout<<"ClGMMDataGpu<T>::sampleGMMpdf"<<endl;
  assert(pi.size() == this->K_);
  assert(logNormalizers.size() == this->K_);

  Matrix<T,Dynamic,Dynamic> invSigmas((this->D_-1)*(this->D_-1),this->K_);
//  Matrix<T,Dynamic,1> logNormalizer(this->K_);
  for(uint32_t k=0; k<this->K_; ++k)
  {
    Matrix<T,Dynamic,Dynamic> invS =Sigmas[k].inverse();
    for(uint32_t i=0; i<invS.cols(); ++i)
      for(uint32_t j=0; j<invS.rows(); ++j)
        invSigmas(i*invS.rows()+j,k) = invS(j,i);
//    logNormalizer(k) = -0.5*Sigmas[k]
  }
//  assert(fabs(pi.sum()-1.0) <1e-6);
  Matrix<T,Dynamic,1> logPi = pi.array().log().matrix();
  //copy parameters into memory
  this->d_logPi_.set(logPi);
  this->d_invSigmas_.set(invSigmas);
  this->d_logNormalizers_.set(logNormalizers);

//  cout<<"sphereGMMPdf"<<endl;
//  cout<<"logNormaloizers"<<endl<<logNormalizers.transpose()<<endl;
//  cout<<"logPi"<<endl<<logPi.transpose()<<endl;
//  cout<<invSigmas<<endl;
//
//  d_ps_.data();
//  d_northRps_.data();
//  d_q_.data();
//  d_invSigmas_.data();
//  d_logNormalizers_.data();
//  d_logPi_.data();

  Matrix<T,Dynamic,Dynamic> pdfs(this->N_,this->K_);
  if(!this->d_pdfs_->isInit())
  {
    this->d_pdfs_->setZero();
//    pdfs = Matrix<T,Dynamic,Dynamic>::Zero(this->N_,this->K_);
//    d_pdfs_->set(pdfs);
  }

  sphereGMMPdf(d_ps_.data(), d_northRps_.data(), d_q_.data(),
    this->d_invSigmas_.data(), this->d_logNormalizers_.data(),
    this->d_logPi_.data(), this->d_pdfs_->data(), this->N_, this->K_);
};

template<typename T>
void ClTGMMDataGpu<T>::sampleGMMpdf(const Matrix<T,Dynamic,1>& pi,
    const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
    const Matrix<T,Dynamic,1>& logNormalizers, Sampler<T> *sampler)
{
  computeLogLikelihoods(pi,Sigmas,logNormalizers);

  sampler->sampleDiscPdf(this->d_pdfs_->data(),this->z_);

//  cout<< this->z_->transpose()<<endl;
//  d_pdfs_->get(pdfs);
//  cout<<pdfs<<endl;

};


#include <Eigen/Core>
#include <Eigen/Sparse>
#include <iostream>
#include <igl/readDMAT.h>
#include <igl/writeDMAT.h>
#include <unordered_map>
#include <array>
#include <vector>
#include <queue>

void sparse_continuation(const Eigen::RowVector3d p0, const std::vector<Eigen::RowVector3i> init_voxels, const std::vector<double> t0, const std::function<double(const Eigen::RowVector3d &, double &, std::vector<std::vector<double>> &, std::vector<std::vector<double>> &, std::vector<std::vector<double>> &)> scalarFunc, const double eps, const int expected_number_of_cubes, Eigen::VectorXd & CS, Eigen::MatrixXd & CV, Eigen::MatrixXi & CI, Eigen::VectorXd & CV_argmins_vector){

    struct IndexRowVectorHash  {
        std::size_t operator()(const Eigen::RowVector3i& key) const {
            std::size_t seed = 0;
            std::hash<int> hasher;
            for (int i = 0; i < 3; i++) {
                seed ^= hasher(key[i]) + 0x9e3779b9 + (seed<<6) + (seed>>2); // Copied from boost::hash_combine
            }
            return seed;
        }
    };

    auto sgn = [](double val) -> int {
        return (double(0) < val) - (val < double(0));
    };

    double half_eps = 0.5 * eps;

    std::vector<Eigen::Matrix<int,1,8>> CI_vector;
    std::vector<Eigen::RowVector3d> CV_vector;
    std::vector<std::vector<std::vector<double>>> CV_intervals;
    std::vector<std::vector<std::vector<double>>> CV_values;
    std::vector<std::vector<std::vector<double>>> CV_minima;
    std::vector<double> CS_vector;
    CI_vector.reserve(expected_number_of_cubes);
    CV_vector.reserve(8 * expected_number_of_cubes);
    CS_vector.reserve(8 * expected_number_of_cubes);
    std::vector<std::vector<double>> argmins;
    std::vector<double> CV_argmins;

    argmins.reserve(32 * expected_number_of_cubes);
    int counter = 0;

    // Track visisted neighbors
    std::unordered_map<Eigen::RowVector3i, int, IndexRowVectorHash> visited;
    visited.reserve(6 * expected_number_of_cubes);
    visited.max_load_factor(0.5);

    // BFS Queue
    auto cmp = [](std::tuple<Eigen::RowVector3i, double, int, double> left, std::tuple<Eigen::RowVector3i, double, int, double> right) {
        double pleft, pright;
        pleft = std::get<3>(left);
        pright = std::get<3>(right);
        return pleft > pright;
    };

    std::priority_queue<std::tuple<Eigen::RowVector3i, double, int, double>, std::vector<std::tuple<Eigen::RowVector3i, double, int, double>>, decltype(cmp)> p_queue(cmp);

    std::vector<Eigen::RowVector3i> queue;
    queue.reserve(expected_number_of_cubes * 8);
    std::vector<double> time_queue;
    time_queue.reserve(expected_number_of_cubes * 8);
    std::vector<int> correspondence_queue;
    correspondence_queue.reserve(expected_number_of_cubes * 8);
    std::vector<double> intervals_turn, values_turn, minima_turn;

    for (int seed_ind = 0; seed_ind < init_voxels.size(); seed_ind++) {

        double min_turn = 1000.0;
        double final_seed = t0[seed_ind];
//        for (double tt = 0; tt <=1.0; tt = tt + 0.1) {
//            intervals_turn.resize(0);
//            values_turn.resize(0);
//            minima_turn.resize(0);
//            double seed_turn = tt;
//            Eigen::RowVector3i pi_turn = init_voxels[seed_ind];
//            Eigen::RowVector3d ctr_turn = p0 + eps*pi_turn.cast<double>();
//            double val_turn = scalarFunc(ctr_turn,seed_turn,intervals_turn,values_turn,minima_turn);
//            if (val_turn < min_turn) {
//                final_seed = seed_turn;
//                min_turn = val_turn;
//            }
//        }
        queue.push_back(init_voxels[seed_ind]);
        time_queue.push_back(final_seed);
        correspondence_queue.push_back(-1);
        auto bar = std::make_tuple(init_voxels[seed_ind], final_seed, -1, 0.0);
        p_queue.push(bar);
    }
    //queue.push_back(Eigen::RowVector3i(0, 0, 0));
    //time_queue.push_back(t0);
    //std::cout << "test" << std::endl;

    int additions_normal, additions_corrections, additions_self;
    additions_normal = 0;
    additions_corrections = 0;
    additions_self = 0;
    while (queue.size() > 0)
    {
        Eigen::RowVector3i pi = queue.back();
        queue.pop_back();
        double time_seed = time_queue.back();
        time_queue.pop_back();
        int correspondence = correspondence_queue.back();
        correspondence_queue.pop_back();

//        std::tuple<Eigen::RowVector3i, double, int, double> val;
//        val = p_queue.top();
//        p_queue.pop();
//        pi = std::get<0>(val);
//        time_seed = std::get<1>(val);
//        correspondence = std::get<2>(val);

        Eigen::RowVector3d ctr = p0 + eps*pi.cast<double>(); // R^3 center of this cube

        // X, Y, Z basis vectors, and array of neighbor offsets used to construct cubes
        const Eigen::RowVector3i bx(1, 0, 0), by(0, 1, 0), bz(0, 0, -1);
        const std::array<Eigen::RowVector3i, 30> neighbors = {
            bx, -bx, by, -by, bz, -bz,
            by-bz, -by+bz, // 1-2 4-7
            bx+by, -bx-by, // 0-1 7-6
            by+bz, -by-bz,  // 0-3 6-5
            by-bx, -by+bx,  // 2-3 5-4
            bx-bz, -bx+bz, // 1-5 3-7
            bx+bz, -bx-bz, // 0-4 2-6
            -bx+by+bz, bx-by-bz, // 3 5
            bx+by+bz, -bx-by-bz, // 0 6
            bx+by-bz, -bx-by+bz, //1 7
            -bx+by-bz, bx-by+bz, // 2 4,
            bx-bx, bx-bx,
            bx-bx, bx-bx
        };

        // Compute the position of the cube corners and the scalar values at those corners
        std::array<Eigen::RowVector3d, 8> cubeCorners = {
            ctr+half_eps*(bx+by+bz).cast<double>(), ctr+half_eps*(bx+by-bz).cast<double>(), ctr+half_eps*(-bx+by-bz).cast<double>(), ctr+half_eps*(-bx+by+bz).cast<double>(),
            ctr+half_eps*(bx-by+bz).cast<double>(), ctr+half_eps*(bx-by-bz).cast<double>(), ctr+half_eps*(-bx-by-bz).cast<double>(), ctr+half_eps*(-bx-by+bz).cast<double>()
        };
        std::array<double, 8> cubeScalars;
        //double time_seed = 0.0;
        //std::cout << time_seed << std::endl;
        double time_test;
        argmins[CI_vector.size()].resize(8);
        std::vector<double> argmins_cube;

        // Add the cube vertices and indices to the output arrays if they are not there already

        uint8_t vertexAlreadyAdded = 0; // This is a bimask. If a bit is 1, it has been visited already by the BFS
        constexpr std::array<uint8_t, 30> zv = {
            (1 << 0) | (1 << 1) | (1 << 4) | (1 << 5),
            (1 << 2) | (1 << 3) | (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 3) | (1 << 4) | (1 << 7),
            (1 << 1) | (1 << 2) | (1 << 5) | (1 << 6),
            (1 << 1) | (1 << 2),
            (1 << 4) | (1 << 7),
            (1 << 0) | (1 << 1),
            (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 3),
            (1 << 5) | (1 << 6),
            (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5),
            (1 << 1) | (1 << 5),
            (1 << 3) | (1 << 7),
            (1 << 0) | (1 << 4),
            (1 << 2) | (1 << 6),
            (1 << 3), (1 << 5), // diagonals
            (1 << 0), (1 << 6),
            (1 << 1), (1 << 7),
            (1 << 2), (1 << 4),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
        };
        constexpr std::array<std::array<int, 4>, 30> zvv {{
            {{0, 1, 4, 5}}, {{3, 2, 7, 6}}, {{0, 1, 2, 3}},
            {{4, 5, 6, 7}}, {{0, 3, 4, 7}}, {{1, 2, 5, 6}},
            {{-1,-1,1,2}}, {{-1,-1,4,7}}, {{-1,-1,0,1}},{{-1,-1,7,6}},
            {{-1,-1,0,3}}, {{-1,-1,5,6}}, {{-1,-1,2,3}}, {{-1,-1,5,4}},
            {{-1,-1,1,5}}, {{-1,-1,3,7}}, {{-1,-1,0,4}}, {{-1,-1,2,6}},
            {{-1,-1,-1,3}}, {{-1,-1,-1,5}}, {{-1,-1,-1,0}}, {{-1,-1,-1,6}},
            {{-1,-1,-1,1}}, {{-1,-1,-1,7}}, {{-1,-1,-1,2}}, {{-1,-1,-1,4}},
            {{0,1,2,3}}, {{0,1,2,3}}, {{4,5,6,7}}, {{4,5,6,7}}
        }};
        bool flag = false;

        Eigen::Matrix<int,1,8> cube;
        cube << -1, -1, -1, -1, -1, -1, -1, -1;
        for (int n = 0; n < 30; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            if (nbr != visited.end()) {
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                    }
                }
            }
        }

        // do we already know we're inside?
        bool we_in = true;
        for (int i = 0; i<8; i++) {
            if(cube[i]==-1){
                we_in = false;
                break;
            }
            if (CS_vector[cube[i]] > 0.0) {
                we_in = false;
                break;
            }
        }

        if (we_in) {
            continue;
        }

        std::vector<std::vector<std::vector<double>>> intervals;
        intervals.resize(8);
        std::vector<std::vector<std::vector<double>>> values;
        values.resize(8);
        std::vector<std::vector<std::vector<double>>> minima;
        minima.resize(8);

        bool debug_flag = false;
        bool in_existing_interval = false;
        bool intersecting_interval = false;
        time_test = time_seed;
        double running_argmin = 0.0;
        for (int i = 0; i < 8; i++){
            time_test = time_seed;
            //            cubeScalars[i] = scalarFunc(cubeCorners[i],time_test);
            //            argmins[CI_vector.size()][i] = time_test;
            //   running_argmin = running_argmin + (time_test/8.0);

            if (cube[i] >= 0) {
                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,CV_intervals[cube[i]], CV_values[cube[i]], CV_minima[cube[i]]);
                argmins[CI_vector.size()][i] = time_test;

                if (correspondence==-1) {
                double temp = cubeScalars[i];
                    //std::cout << "We got " << temp << " at " << time_seed << "...";
                int temp_i = -1;
                    int temp_s = -1;
                    for (int s = 0; s < CV_intervals[cube[i]].size(); s++) {
                for (int mm = 0; mm < (CV_intervals[cube[i]][s].size()/2); mm++){
                    if ( (CV_values[cube[i]][s][mm]+1e-3) < temp) {
                        temp = CV_values[cube[i]][s][mm];
                        temp_i = mm;
                        temp_s = s;
                    }
                }}
                if (temp_i > -1) {
                     //JAN 24 change this
                    queue.push_back(pi);
                    //std::cout << " but we found " << temp << " at " << CV_minima[cube[i]][temp_s][temp_i] << " (correspondence " << correspondence << std::endl;
                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    // time_queue.push_back(time_test);
                    time_queue.push_back(CV_minima[cube[i]][temp_s][temp_i]);
                    correspondence_queue.push_back(1);
                    auto bar = std::make_tuple(pi, CV_minima[cube[i]][temp_s][temp_i], 1, temp);
                    p_queue.push(bar);
                    additions_self++;

                    //time_seed = CV_minima[cube[i]][temp_s][temp_i];
//                    cubeScalars[i] = temp;
//                    argmins[CI_vector.size()][i] = CV_minima[cube[i]][temp_s][temp_i];
                }else{
                    //std::cout << std::endl;
                }
                }
                // DEBUGGING
                //                if (cube[i]==10) {
//                std::cout << "______________";
//                std::cout << "Time seed: " << time_seed << std::endl;
//                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++){
//                    std::cout << "Existing interval: " << CV_intervals[cube[i]][2*mm] << " " <<  CV_intervals[cube[i]][2*mm + 1] << " value: " << CV_values[cube[i]][mm] << std::endl;
//                }
                //                }
                //                            in_existing_interval = false;
                //                            for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++) {
                //                                if ((time_test >= CV_intervals[cube[i]][2*mm]) && (time_test <= CV_intervals[cube[i]][2*mm+1]) ){
                //                                    cubeScalars[i] = CV_values[cube[i]][mm];
                //                                    argmins[CI_vector.size()][i] = CV_minima[cube[i]][mm];
                //                                    in_existing_interval = true;
                //                                }
                //                            }
                //                            if (!in_existing_interval) {
                //                                intersecting_interval = false;
                //                                time_test = time_seed;
                //                                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,CV_intervals[cube[i]], CV_values[cube[i]], CV_minima[cube[i]]);
                //                                argmins[CI_vector.size()][i] = time_test;
                //                                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++) {
                //                                    if ((time_test >= CV_intervals[cube[i]][2*mm]) && (time_test <= CV_intervals[cube[i]][2*mm+1]) ){
                //                                        intersecting_interval = true;
                //                                        CV_intervals[cube[i]][2*mm] = std::min(CV_intervals[cube[i]][2*mm],time_seed);
                //                                        CV_intervals[cube[i]][2*mm+1] = std::max(CV_intervals[cube[i]][2*mm+1],time_seed);
                //                                        CV_values[cube[i]][mm] = std::min(CV_values[cube[i]][mm],cubeScalars[i]); //  ??
                //                                    }
                //                                }
                //                            }
                //                            if (!in_existing_interval && !intersecting_interval) {
                //                                std::vector<double> interval_i;
                //                                interval_i.push_back(time_test);
                //                                interval_i.push_back(time_seed);
                //                                std::sort(interval_i.begin(), interval_i.end());
                //                                CV_intervals[cube[i]].push_back(interval_i[0]);
                //                                CV_intervals[cube[i]].push_back(interval_i[1]);
                //                                CV_values[cube[i]].push_back(cubeScalars[i]);
                //                                CV_minima[cube[i]].push_back(time_test);
                //                            }
                //                time_test = time_seed;
                //                double debug = scalarFunc(cubeCorners[i],time_test);
                //                if (fabs(debug - cubeScalars[i])>1e-3 ) {
                //                                std::cout << "______________";
                ////                                std::cout << "Seed: " << time_seed << " value " << debug << " at time " << time_test << std::endl;
                //                                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++){
                //                                    std::cout << "Existing interval :" << CV_intervals[cube[i]][2*mm] << " " <<  CV_intervals[cube[i]][2*mm + 1] << " value: " << CV_values[cube[i]][mm] << std::endl;
                //                                }
                //                                std::cout << "Point: " << CV_vector[cube[i]] << std::endl;
                //                }

            }else{
                time_test = time_seed;
                intervals[i].resize(0);
                values[i].resize(0);
                minima[i].resize(0);
                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,intervals[i],values[i],minima[i]);
                argmins[CI_vector.size()][i] = time_test;
            }

            running_argmin = running_argmin + (argmins[CI_vector.size()][i]/8.0);
            //            if (cubeScalars[i]>0.3) {
            //                debug_flag = true;
            //            }

        }
        //std::cout << time_test << std::endl;
        // If this cube doesn't intersect the surface, disregard it
        bool validCube = false;
        int sign = sgn(cubeScalars[0]);
        for (int i = 1; i < 8; i++) {
            if (sign != sgn(cubeScalars[i])) {
                validCube = true;
                break;
            }
        }

        //    if (!validCube) {
        //continue;
        //  }

        // Debugging
        //        if (debug_flag) {
        //            std::cout << "voxel" << std::endl;
        //            for (int i = 0; i<8; i++) {
        //                std::cout << "value: " << cubeScalars[i] << " at time" << argmins[CI_vector.size()][i] << " with seed" << time_seed << std::endl;
        //
        //            }
        //        }

        for (int n = 0; n < 30; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr != visited.end()) { // We've already visited this neighbor, use references to its vertices instead of duplicating them
                vertexAlreadyAdded |= zv[n];
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                        //              if( CS_vector[cube[zvv[n][i]]] < cubeScalars[zvv[n][i]] ){
                        //                  // It was better before
                        //                  cubeScalars[zvv[n][i]] = CS_vector[cube[zvv[n][i]]];
                        //              }else
                        if((CS_vector[cube[zvv[n][i]]]>cubeScalars[zvv[n][i]] && (CS_vector[cube[zvv[n][i]]]*cubeScalars[zvv[n][i]])<0) || CS_vector[cube[zvv[n][i]]]>(cubeScalars[zvv[n][i]] + 1e-3)){
                            if (!flag) {
                                queue.push_back(nkey);
                                //time_queue.push_back(time_test);
                                // JAN 24 CHANGE THIS
                                time_queue.push_back(running_argmin);
                                correspondence_queue.push_back(nbr->second);
                                additions_corrections++;
                                flag = true;
                                auto bar = std::make_tuple(nkey, running_argmin, nbr->second, cubeScalars[zvv[n][i]]);
                                p_queue.push(bar);
                            }
                        }

                        // WARNING: THIS BELOW IS WRONG, HAVE TO CHANGE
                        //cubeScalars[zvv[n][i]] = std::min(CS_vector[cube[zvv[n][i]]],cubeScalars[zvv[n][i]]);
                        //CS_vector[cube[zvv[n][i]]] = cubeScalars[zvv[n][i]];
                    }
                }
                //} else if(correspondence==-1) {
            }
        }

        validCube = false;
        sign = sgn(cubeScalars[0]);
        for (int i = 1; i < 8; i++) {
            if (sign != sgn(cubeScalars[i])) {
                validCube = true;
            }
        }
        bool validCube_before = validCube;

        for (int n = 0; n < 30; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr != visited.end()) { // We've already visited this neighbor, use references to its vertices instead of duplicating them
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                        // WARNING: THIS BELOW IS WRONG, HAVE TO CHANGE
                        cubeScalars[zvv[n][i]] = std::min(CS_vector[cube[zvv[n][i]]],cubeScalars[zvv[n][i]]);
                        CS_vector[cube[zvv[n][i]]] = cubeScalars[zvv[n][i]];
                    }
                }
                //} else if(correspondence==-1) {
            }else{
                //                validCube = false;
                //                sign = sgn(cubeScalars[0]);
                //                for (int i = 1; i < 8; i++) {
                //                    if (sign != sgn(cubeScalars[i])) {
                //                        validCube = true;
                //                    }
                //                }
                if (validCube) {
                    //                    if (debug_flag) {
                    //                        std::cout << "BAD THING HAPPENED" << std::endl;
                    //                    }
                    //                    queue.push_back(nkey);
                    //                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    //                   // time_queue.push_back(time_test);
                    //                    time_queue.push_back(running_argmin);
                    //                    correspondence_queue.push_back(-1);
                }

            }
        }
  // WARNING THIS SHOULDNT BE COMMENTED!!
        validCube = false;
        sign = sgn(cubeScalars[0]);
        for (int i = 1; i < 8; i++) {
            if (sign != sgn(cubeScalars[i])) {
                validCube = true;
            }
        }

        for (int n = 0; n < 6; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr == visited.end()) {
                if(validCube && validCube_before){
                    queue.push_back(nkey);
                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    // time_queue.push_back(time_test);

                    time_queue.push_back(running_argmin);
                    correspondence_queue.push_back(-1);
                    auto bar = std::make_tuple(nkey, running_argmin, -1, 0.0);
                    p_queue.push(bar);
                    additions_normal++;
                }
            }
        }

        auto did_we_visit_this_one = visited.find(pi);
        if (correspondence==-1 && did_we_visit_this_one==visited.end()) {
            for (int i = 0; i < 8; i++) { // Add new, non-visited,2 vertices to the arrays
                //if (0 == ((1 << i) & vertexAlreadyAdded)) {
                if (0 == ((1 << i) & vertexAlreadyAdded)) {
//                    std::vector<double> interval;
//                    std::vector<double> interval_values;
//                    std::vector<double> interval_minima;
//                    std::vector<double> interval_big;
//                    std::vector<double> interval_values_big;
//                    std::vector<double> interval_minima_big;
//                    interval_minima.push_back(argmins[CI_vector.size()][i]);
//                    interval.push_back(argmins[CI_vector.size()][i]);
//                    interval.push_back(time_seed);
//                    std::sort(interval.begin(), interval.end());
//                    interval_values.push_back(cubeScalars[i]);

                    CV_intervals.push_back(intervals[i]);
                    CV_values.push_back(values[i]);
                    CV_minima.push_back(minima[i]);
                    cube[i] = CS_vector.size();
                    CV_vector.push_back(cubeCorners[i]);
                    CS_vector.push_back(cubeScalars[i]);
                    CV_argmins.push_back(argmins[CI_vector.size()][i]);
                }
            }

            visited[pi] = CI_vector.size();
            CI_vector.push_back(cube);
        }

        //std::cout << queue.size() << std::endl;

        bool debug = false;
        if (debug) {
            CV.resize(CV_vector.size(), 3);
            CV_argmins_vector.resize(CV_vector.size(), 1);
            CS.resize(CS_vector.size(), 1);
            CI.resize(CI_vector.size(), 8);
            Eigen::MatrixXi Q;
            Q.resize(queue.size(),3);
            for (int i = 0; i < queue.size(); i++) {
                Q.row(i) = queue[i];
            }
            // If you pass in column-major matrices, this is going to be slooooowwwww
            for (int i = 0; i < CV_vector.size(); i++) {
                CV.row(i) = CV_vector[i];
            }
            for (int i = 0; i < CS_vector.size(); i++) {
                CS(i) = CS_vector[i];
            }
            for (int i = 0; i < CI_vector.size(); i++) {
                CI.row(i) = CI_vector[i];
            }
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CS" + std::to_string(counter) + ".dmat",CS);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CV" + std::to_string(counter) + ".dmat",CV);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CI" + std::to_string(counter) + ".dmat",CI);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/Q" + std::to_string(counter) + ".dmat",Q);
            counter = counter + 1;
        }

    }
    //std::cout << "test" << std::endl;
//    std::cout << " Normal: " << additions_normal << std::endl;
//    std::cout << " Corrections: " << additions_corrections << std::endl;
//    std::cout << " Self: " << additions_self << std::endl;

    CV.conservativeResize(CV_vector.size(), 3);
    CV_argmins_vector.conservativeResize(CV_vector.size(), 1);
    CS.conservativeResize(CS_vector.size(), 1);
    CI.conservativeResize(CI_vector.size(), 8);
//    // If you pass in column-major matrices, this is going to be slooooowwwww
//    for (int j = 0; j < CV_vector.size(); j++) {
//        std::cout << "______________" << std::endl;
//        for (int mm = 0; mm < (CV_intervals[j].size()/2); mm++){
//            std::cout << "Existing interval: " << CV_intervals[j][2*mm] << " " <<  CV_intervals[j][2*mm + 1] << " value: " << CV_values[j][mm] << std::endl;
//        }
//    }

    for (int i = 0; i < CV_vector.size(); i++) {
        CV.row(i) = CV_vector[i];
    }
    for (int i = 0; i < CS_vector.size(); i++) {
        CS(i) = CS_vector[i];
        //CS(i) = std::min_element(CV_values[i].begin(), CV_values[i].end());
    }
    for (int i = 0; i < CI_vector.size(); i++) {
        CI.row(i) = CI_vector[i];
    }
    for (int i = 0; i < CV_argmins.size(); i++) {
        double val = 100.0;
        double argmin = 0.0;
        for(int s = 0; s<CV_values[i].size(); s++){
        for (int mm = 0; mm<CV_values[i][s].size(); mm++) {
            if (CV_values[i][s][mm]<val) {
                val = CV_values[i][s][mm];
                argmin = CV_minima[i][s][mm];
            }
        }
        }
        CV_argmins_vector(i) = argmin;
    }
}

void sparse_continuation(const Eigen::RowVector3d p0, const std::vector<Eigen::RowVector3i> init_voxels, const std::vector<Eigen::RowVectorXd> t0, const  std::function<double(const Eigen::RowVector3d &, Eigen::RowVectorXd &, std::vector<std::vector<Eigen::RowVectorXd>> &, std::vector<std::vector<double>> &, std::vector<std::vector<Eigen::RowVectorXd>> &)> scalarFunc, const double eps, const int expected_number_of_cubes, Eigen::VectorXd & CS, Eigen::MatrixXd & CV, Eigen::MatrixXi & CI, Eigen::MatrixXd & CV_argmins_vector){

    struct IndexRowVectorHash  {
        std::size_t operator()(const Eigen::RowVector3i& key) const {
            std::size_t seed = 0;
            std::hash<int> hasher;
            for (int i = 0; i < 3; i++) {
            seed ^= hasher(key[i]) + 0x9e3779b9 + (seed<<6) + (seed>>2); // Copied from boost::hash_combine
            }
            return seed;
        }
    };

    auto sgn = [](double val) -> int {
        return (double(0) < val) - (val < double(0));
    };

    double half_eps = 0.5 * eps;

    std::vector<Eigen::Matrix<int,1,8>> CI_vector;
    std::vector<Eigen::RowVector3d> CV_vector;
    std::vector<std::vector<std::vector<Eigen::RowVectorXd>>> CV_intervals;
    std::vector<std::vector<std::vector<double>>> CV_values;
    std::vector<std::vector<std::vector<Eigen::RowVectorXd>>> CV_minima;
    std::vector<double> CS_vector;
    CI_vector.reserve(expected_number_of_cubes);
    CV_vector.reserve(8 * expected_number_of_cubes);
    CS_vector.reserve(8 * expected_number_of_cubes);
    std::vector<std::vector<Eigen::RowVectorXd>> argmins;
    std::vector<Eigen::RowVectorXd> CV_argmins;

    argmins.reserve(32 * expected_number_of_cubes);
    int counter = 0;

    // Track visisted neighbors
    std::unordered_map<Eigen::RowVector3i, int, IndexRowVectorHash> visited;
    visited.reserve(6 * expected_number_of_cubes);
    visited.max_load_factor(0.5);

    // BFS Queue
    std::vector<Eigen::RowVector3i> queue;
    queue.reserve(expected_number_of_cubes * 8);
    std::vector<Eigen::RowVectorXd> time_queue;
    time_queue.reserve(expected_number_of_cubes * 8);
    std::vector<int> correspondence_queue;
    correspondence_queue.reserve(expected_number_of_cubes * 8);
    std::vector<double> intervals_turn, values_turn, minima_turn;

    for (int seed_ind = 0; seed_ind < init_voxels.size(); seed_ind++) {

        double min_turn = 1000.0;
        Eigen::VectorXd final_seed = t0[seed_ind];

        queue.push_back(init_voxels[seed_ind]);
        time_queue.push_back(final_seed);
        correspondence_queue.push_back(-1);
    }

    while (queue.size() > 0)
    {
        Eigen::RowVector3i pi = queue.back();
        queue.pop_back();
        Eigen::VectorXd time_seed = time_queue.back();
        time_queue.pop_back();
        int correspondence = correspondence_queue.back();
        correspondence_queue.pop_back();

        Eigen::RowVector3d ctr = p0 + eps*pi.cast<double>(); // R^3 center of this cube

        // X, Y, Z basis vectors, and array of neighbor offsets used to construct cubes
        const Eigen::RowVector3i bx(1, 0, 0), by(0, 1, 0), bz(0, 0, -1);
        const std::array<Eigen::RowVector3i, 30> neighbors = {
            bx, -bx, by, -by, bz, -bz,
            by-bz, -by+bz, // 1-2 4-7
            bx+by, -bx-by, // 0-1 7-6
            by+bz, -by-bz,  // 0-3 6-5
            by-bx, -by+bx,  // 2-3 5-4
            bx-bz, -bx+bz, // 1-5 3-7
            bx+bz, -bx-bz, // 0-4 2-6
            -bx+by+bz, bx-by-bz, // 3 5
            bx+by+bz, -bx-by-bz, // 0 6
            bx+by-bz, -bx-by+bz, //1 7
            -bx+by-bz, bx-by+bz, // 2 4,
            bx-bx, bx-bx,
            bx-bx, bx-bx
        };

        // Compute the position of the cube corners and the scalar values at those corners
        std::array<Eigen::RowVector3d, 8> cubeCorners = {
            ctr+half_eps*(bx+by+bz).cast<double>(), ctr+half_eps*(bx+by-bz).cast<double>(), ctr+half_eps*(-bx+by-bz).cast<double>(), ctr+half_eps*(-bx+by+bz).cast<double>(),
            ctr+half_eps*(bx-by+bz).cast<double>(), ctr+half_eps*(bx-by-bz).cast<double>(), ctr+half_eps*(-bx-by-bz).cast<double>(), ctr+half_eps*(-bx-by+bz).cast<double>()
        };
        std::array<double, 8> cubeScalars;
        //double time_seed = 0.0;
        //std::cout << time_seed << std::endl;
        Eigen::RowVectorXd time_test;
        argmins[CI_vector.size()].resize(8);
        std::vector<Eigen::RowVectorXd> argmins_cube;

        // Add the cube vertices and indices to the output arrays if they are not there already

        uint8_t vertexAlreadyAdded = 0; // This is a bimask. If a bit is 1, it has been visited already by the BFS
        constexpr std::array<uint8_t, 30> zv = {
            (1 << 0) | (1 << 1) | (1 << 4) | (1 << 5),
            (1 << 2) | (1 << 3) | (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 3) | (1 << 4) | (1 << 7),
            (1 << 1) | (1 << 2) | (1 << 5) | (1 << 6),
            (1 << 1) | (1 << 2),
            (1 << 4) | (1 << 7),
            (1 << 0) | (1 << 1),
            (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 3),
            (1 << 5) | (1 << 6),
            (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5),
            (1 << 1) | (1 << 5),
            (1 << 3) | (1 << 7),
            (1 << 0) | (1 << 4),
            (1 << 2) | (1 << 6),
            (1 << 3), (1 << 5), // diagonals
            (1 << 0), (1 << 6),
            (1 << 1), (1 << 7),
            (1 << 2), (1 << 4),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
        };
        constexpr std::array<std::array<int, 4>, 30> zvv {{
            {{0, 1, 4, 5}}, {{3, 2, 7, 6}}, {{0, 1, 2, 3}},
            {{4, 5, 6, 7}}, {{0, 3, 4, 7}}, {{1, 2, 5, 6}},
            {{-1,-1,1,2}}, {{-1,-1,4,7}}, {{-1,-1,0,1}},{{-1,-1,7,6}},
            {{-1,-1,0,3}}, {{-1,-1,5,6}}, {{-1,-1,2,3}}, {{-1,-1,5,4}},
            {{-1,-1,1,5}}, {{-1,-1,3,7}}, {{-1,-1,0,4}}, {{-1,-1,2,6}},
            {{-1,-1,-1,3}}, {{-1,-1,-1,5}}, {{-1,-1,-1,0}}, {{-1,-1,-1,6}},
            {{-1,-1,-1,1}}, {{-1,-1,-1,7}}, {{-1,-1,-1,2}}, {{-1,-1,-1,4}},
            {{0,1,2,3}}, {{0,1,2,3}}, {{4,5,6,7}}, {{4,5,6,7}}
        }};
        bool flag = false;

        Eigen::Matrix<int,1,8> cube;
        cube << -1, -1, -1, -1, -1, -1, -1, -1;
        for (int n = 0; n < 30; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            if (nbr != visited.end()) {
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                    }
                }
            }
        }

        std::vector<std::vector<std::vector<Eigen::RowVectorXd>>> intervals;
        intervals.resize(8);
        std::vector<std::vector<std::vector<double>>> values;
        values.resize(8);
        std::vector<std::vector<std::vector<Eigen::RowVectorXd>>> minima;
        minima.resize(8);

        bool debug_flag = false;
        bool in_existing_interval = false;
        bool intersecting_interval = false;
        time_test = time_seed;
        Eigen::RowVectorXd running_argmin;
        running_argmin.resize(time_seed.size());
        running_argmin.setZero();
        for (int i = 0; i < 8; i++){
            time_test = time_seed;
            //            cubeScalars[i] = scalarFunc(cubeCorners[i],time_test);
            //            argmins[CI_vector.size()][i] = time_test;
            //   running_argmin = running_argmin + (time_test/8.0);

            if (cube[i] >= 0) {
               // std::cout << " CALL FUNC " << std::endl;
                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,CV_intervals[cube[i]], CV_values[cube[i]], CV_minima[cube[i]]);
               // std::cout << " end CALL FUNC " << std::endl;
                argmins[CI_vector.size()][i] = time_test;
                minima[i] = CV_minima[cube[i]];
                values[i] = CV_values[cube[i]];
                intervals[i] = CV_intervals[cube[i]];

                if (correspondence==-1) {
                double temp = cubeScalars[i];
                int temp_i = -1;
                    int temp_s = -1;
                    for (int s = 0; s < CV_intervals[cube[i]].size(); s++) {
                for (int mm = 0; mm < (CV_intervals[cube[i]][s].size()/2); mm++){
                    if (CV_values[cube[i]][s][mm]+1e-3 < temp) {
                        temp = CV_values[cube[i]][s][mm];
                        temp_i = mm;
                        temp_s = s;
                    }
                }}
                if (temp_i > -1) {
                    queue.push_back(pi);
                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    // time_queue.push_back(time_test);
                    time_queue.push_back(CV_minima[cube[i]][temp_s][temp_i]);
                    correspondence_queue.push_back(1);
                }
                }
                // DEBUGGING
                //                if (cube[i]==10) {
//                std::cout << "______________";
//                std::cout << "Time seed: " << time_seed << std::endl;
//                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++){
//                    std::cout << "Existing interval: " << CV_intervals[cube[i]][2*mm] << " " <<  CV_intervals[cube[i]][2*mm + 1] << " value: " << CV_values[cube[i]][mm] << std::endl;
//                }
                //                }
                //                            in_existing_interval = false;
                //                            for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++) {
                //                                if ((time_test >= CV_intervals[cube[i]][2*mm]) && (time_test <= CV_intervals[cube[i]][2*mm+1]) ){
                //                                    cubeScalars[i] = CV_values[cube[i]][mm];
                //                                    argmins[CI_vector.size()][i] = CV_minima[cube[i]][mm];
                //                                    in_existing_interval = true;
                //                                }
                //                            }
                //                            if (!in_existing_interval) {
                //                                intersecting_interval = false;
                //                                time_test = time_seed;
                //                                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,CV_intervals[cube[i]], CV_values[cube[i]], CV_minima[cube[i]]);
                //                                argmins[CI_vector.size()][i] = time_test;
                //                                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++) {
                //                                    if ((time_test >= CV_intervals[cube[i]][2*mm]) && (time_test <= CV_intervals[cube[i]][2*mm+1]) ){
                //                                        intersecting_interval = true;
                //                                        CV_intervals[cube[i]][2*mm] = std::min(CV_intervals[cube[i]][2*mm],time_seed);
                //                                        CV_intervals[cube[i]][2*mm+1] = std::max(CV_intervals[cube[i]][2*mm+1],time_seed);
                //                                        CV_values[cube[i]][mm] = std::min(CV_values[cube[i]][mm],cubeScalars[i]); //  ??
                //                                    }
                //                                }
                //                            }
                //                            if (!in_existing_interval && !intersecting_interval) {
                //                                std::vector<double> interval_i;
                //                                interval_i.push_back(time_test);
                //                                interval_i.push_back(time_seed);
                //                                std::sort(interval_i.begin(), interval_i.end());
                //                                CV_intervals[cube[i]].push_back(interval_i[0]);
                //                                CV_intervals[cube[i]].push_back(interval_i[1]);
                //                                CV_values[cube[i]].push_back(cubeScalars[i]);
                //                                CV_minima[cube[i]].push_back(time_test);
                //                            }
                //                time_test = time_seed;
                //                double debug = scalarFunc(cubeCorners[i],time_test);
                //                if (fabs(debug - cubeScalars[i])>1e-3 ) {
                //                                std::cout << "______________";
                ////                                std::cout << "Seed: " << time_seed << " value " << debug << " at time " << time_test << std::endl;
                //                                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++){
                //                                    std::cout << "Existing interval :" << CV_intervals[cube[i]][2*mm] << " " <<  CV_intervals[cube[i]][2*mm + 1] << " value: " << CV_values[cube[i]][mm] << std::endl;
                //                                }
                //                                std::cout << "Point: " << CV_vector[cube[i]] << std::endl;
                //                }

            }else{
                time_test = time_seed;
                intervals[i].resize(0);
                values[i].resize(0);
                minima[i].resize(0);
          //      std::cout << " CALL FUNC " << std::endl;
                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,intervals[i],values[i],minima[i]);
          //      std::cout << " end CALL FUNC " << std::endl;
                argmins[CI_vector.size()][i] = time_test;
            }

            running_argmin = running_argmin + (argmins[CI_vector.size()][i]/8.0);
            //            if (cubeScalars[i]>0.3) {
            //                debug_flag = true;
            //            }

        }
        //std::cout << time_test << std::endl;
        // If this cube doesn't intersect the surface, disregard it
        bool validCube = false;
        int sign = sgn(cubeScalars[0]);
        for (int i = 1; i < 8; i++) {
            if (sign != sgn(cubeScalars[i])) {
                validCube = true;
                break;
            }
        }

        //    if (!validCube) {
        //continue;
        //  }

        // Debugging
        //        if (debug_flag) {
        //            std::cout << "voxel" << std::endl;
        //            for (int i = 0; i<8; i++) {
        //                std::cout << "value: " << cubeScalars[i] << " at time" << argmins[CI_vector.size()][i] << " with seed" << time_seed << std::endl;
        //
        //            }
        //        }

        for (int n = 0; n < 6; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr != visited.end()) { // We've already visited this neighbor, use references to its vertices instead of duplicating them
                vertexAlreadyAdded |= zv[n];
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                        //              if( CS_vector[cube[zvv[n][i]]] < cubeScalars[zvv[n][i]] ){
                        //                  // It was better before
                        //                  cubeScalars[zvv[n][i]] = CS_vector[cube[zvv[n][i]]];
                        //              }else
                        if((CS_vector[cube[zvv[n][i]]]>cubeScalars[zvv[n][i]] && (CS_vector[cube[zvv[n][i]]]*cubeScalars[zvv[n][i]])<0) || CS_vector[cube[zvv[n][i]]]>(cubeScalars[zvv[n][i]] + 1e-3)){
                            if (!flag) {
                                queue.push_back(nkey);
                                //time_queue.push_back(time_test);
                                time_queue.push_back(running_argmin);
                                correspondence_queue.push_back(nbr->second);
                                flag = true;
                            }
                        }

                        // WARNING: THIS BELOW IS WRONG, HAVE TO CHANGE
                        //cubeScalars[zvv[n][i]] = std::min(CS_vector[cube[zvv[n][i]]],cubeScalars[zvv[n][i]]);
                        //CS_vector[cube[zvv[n][i]]] = cubeScalars[zvv[n][i]];
                    }
                }
                //} else if(correspondence==-1) {
            }
        }

        validCube = false;
        sign = sgn(cubeScalars[0]);
        for (int i = 1; i < 8; i++) {
            if (sign != sgn(cubeScalars[i])) {
                validCube = true;
            }
        }

        for (int n = 0; n < 30; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr != visited.end()) { // We've already visited this neighbor, use references to its vertices instead of duplicating them
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                        // WARNING: THIS BELOW IS WRONG, HAVE TO CHANGE
                        cubeScalars[zvv[n][i]] = std::min(CS_vector[cube[zvv[n][i]]],cubeScalars[zvv[n][i]]);
                        CS_vector[cube[zvv[n][i]]] = cubeScalars[zvv[n][i]];
                    }
                }
                //} else if(correspondence==-1) {
            }else{
                //                validCube = false;
                //                sign = sgn(cubeScalars[0]);
                //                for (int i = 1; i < 8; i++) {
                //                    if (sign != sgn(cubeScalars[i])) {
                //                        validCube = true;
                //                    }
                //                }
                if (validCube) {
                    //                    if (debug_flag) {
                    //                        std::cout << "BAD THING HAPPENED" << std::endl;
                    //                    }
                    //                    queue.push_back(nkey);
                    //                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    //                   // time_queue.push_back(time_test);
                    //                    time_queue.push_back(running_argmin);
                    //                    correspondence_queue.push_back(-1);
                }

            }
        }
  // WARNING THIS SHOULDNT BE COMMENTED!!
//        validCube = false;
//        sign = sgn(cubeScalars[0]);
//        for (int i = 1; i < 8; i++) {
//            if (sign != sgn(cubeScalars[i])) {
//                validCube = true;
//            }
//        }
        for (int n = 0; n < 6; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr == visited.end()) {
                if(validCube){
                    queue.push_back(nkey);
                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    // time_queue.push_back(time_test);
                    time_queue.push_back(running_argmin);
                    correspondence_queue.push_back(-1);
                }
            }
        }

        auto did_we_visit_this_one = visited.find(pi);
        if (correspondence==-1 && did_we_visit_this_one==visited.end()) {
            for (int i = 0; i < 8; i++) { // Add new, non-visited,2 vertices to the arrays
                //if (0 == ((1 << i) & vertexAlreadyAdded)) {
                if (0 == ((1 << i) & vertexAlreadyAdded)) {
//                    std::vector<double> interval;
//                    std::vector<double> interval_values;
//                    std::vector<double> interval_minima;
//                    std::vector<double> interval_big;
//                    std::vector<double> interval_values_big;
//                    std::vector<double> interval_minima_big;
//                    interval_minima.push_back(argmins[CI_vector.size()][i]);
//                    interval.push_back(argmins[CI_vector.size()][i]);
//                    interval.push_back(time_seed);
//                    std::sort(interval.begin(), interval.end());
//                    interval_values.push_back(cubeScalars[i]);

                    CV_intervals.push_back(intervals[i]);
                    CV_values.push_back(values[i]);
                    CV_minima.push_back(minima[i]);
                    cube[i] = CS_vector.size();
                    CV_vector.push_back(cubeCorners[i]);
                    CS_vector.push_back(cubeScalars[i]);
                    CV_argmins.push_back(argmins[CI_vector.size()][i]);
                }
            }

            visited[pi] = CI_vector.size();
            CI_vector.push_back(cube);
        }

        //std::cout << queue.size() << std::endl;

        bool debug = false;
        if (debug) {
            CV.resize(CV_vector.size(), 3);
            CV_argmins_vector.resize(CV_vector.size(), 1);
            CS.resize(CS_vector.size(), 1);
            CI.resize(CI_vector.size(), 8);
            Eigen::MatrixXi Q;
            Q.resize(queue.size(),3);
            for (int i = 0; i < queue.size(); i++) {
                Q.row(i) = queue[i];
            }
            // If you pass in column-major matrices, this is going to be slooooowwwww
            for (int i = 0; i < CV_vector.size(); i++) {
                CV.row(i) = CV_vector[i];
            }
            for (int i = 0; i < CS_vector.size(); i++) {
                CS(i) = CS_vector[i];
            }
            for (int i = 0; i < CI_vector.size(); i++) {
                CI.row(i) = CI_vector[i];
            }
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CS" + std::to_string(counter) + ".dmat",CS);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CV" + std::to_string(counter) + ".dmat",CV);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CI" + std::to_string(counter) + ".dmat",CI);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/Q" + std::to_string(counter) + ".dmat",Q);
            counter = counter + 1;
        }

    }
    //std::cout << "test" << std::endl;

    CV.conservativeResize(CV_vector.size(), 3);
    CV_argmins_vector.conservativeResize(CV_vector.size(), 2);
    CS.conservativeResize(CS_vector.size(), 1);
    CI.conservativeResize(CI_vector.size(), 8);
//    // If you pass in column-major matrices, this is going to be slooooowwwww
//    for (int j = 0; j < CV_vector.size(); j++) {
//        std::cout << "______________" << std::endl;
//        for (int mm = 0; mm < (CV_intervals[j].size()/2); mm++){
//            std::cout << "Existing interval: " << CV_intervals[j][2*mm] << " " <<  CV_intervals[j][2*mm + 1] << " value: " << CV_values[j][mm] << std::endl;
//        }
//    }

    for (int i = 0; i < CV_vector.size(); i++) {
        CV.row(i) = CV_vector[i];
    }
    for (int i = 0; i < CS_vector.size(); i++) {
        CS(i) = CS_vector[i];
        //CS(i) = std::min_element(CV_values[i].begin(), CV_values[i].end());
    }
    for (int i = 0; i < CI_vector.size(); i++) {
        CI.row(i) = CI_vector[i];
    }
    for (int i = 0; i < CV_vector.size(); i++) {
        std::cout << "1" << std::endl;
        double val = CV_values[i][0][0];
        Eigen::RowVectorXd argmin;
        argmin = CV_minima[i][0][0];
        std::cout << "2" << std::endl;
        for(int s = 0; s<CV_values[i].size(); s++){
        for (int mm = 0; mm<CV_values[i][s].size(); mm++) {
            if (CV_values[i][s][mm]<val) {
                std::cout << "3" << std::endl;
                val = CV_values[i][s][mm];
                std::cout << "4" << std::endl;
                argmin = CV_minima[i][s][mm];
            }
        }
        }
        std::cout << "5" << std::endl;
        std::cout << argmin << std::endl;
        CV_argmins_vector.row(i) = argmin;
        std::cout << "5b" << std::endl;
    }
    std::cout << "6" << std::endl;
}


// Copyright 2010-2013 The Trustees of Indiana University.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//  Authors: Jeremiah Willcock
//           Andrew Lumsdaine

#ifndef BOOST_SPLITTABLE_ECUYER1988_HPP
#define BOOST_SPLITTABLE_ECUYER1988_HPP

#include <boost/cstdint.hpp>
#include <boost/iterator.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <utility>

namespace boost {
  namespace random {

    template <typename Base> // To get around incomplete class issues
    class generic_split_iterator: public boost::iterator_facade<generic_split_iterator<Base>, Base, std::input_iterator_tag, Base> {
      public:
      Base base;
      uint32_t new_multiplier1, new_multiplier2;

      generic_split_iterator(Base base, uint32_t new_multiplier1, uint32_t new_multiplier2)
	: base(base), new_multiplier1(new_multiplier1), new_multiplier2(new_multiplier2) {}

      generic_split_iterator(): base(), new_multiplier1(0), new_multiplier2(0) {}

      Base dereference() const {
	Base result = base;
	result.multiplier1 = new_multiplier1;
	result.multiplier2 = new_multiplier2;
	return result;
      }

      bool equal(const generic_split_iterator& it) const {
	return base.state1 == it.base.state1 && base.state2 == it.base.state2;
      }

      void increment() {
	base(); // Advance seeds
      }
    };

    class splittable_ecuyer1988 {
      uint32_t state1, state2, multiplier1, multiplier2;

      static const uint64_t modulus1 = 2147483563;
      static const uint64_t modulus2 = 2147483399;

      template <uint64_t Modulus>
      static uint32_t mul_mod(uint32_t a, uint32_t b) {
	return uint32_t((uint64_t(a) * b) % Modulus);
      }

      template <uint64_t Modulus>
      static uint32_t exp_mod(uint32_t a, uint32_t b) {
	if (b == 0) {
	  return 1;
	} else if (b == 1) {
	  return a;
	} else {
	  uint32_t temp = exp_mod<Modulus>(a, b>>1);
	  temp = mul_mod<Modulus>(temp, temp);
	  if (b % 2) temp = mul_mod<Modulus>(a, temp);
	  return temp;
	}
      }

      public:
      splittable_ecuyer1988(): state1(1), state2(1), multiplier1(40014), multiplier2(40692) {}
      splittable_ecuyer1988(uint32_t seed1, uint32_t seed2): state1(seed1), state2(seed2), multiplier1(40014), multiplier2(40692) {}

      void seed(uint32_t seed1, uint32_t seed2) {
	state1 = seed1;
	state2 = seed2;
      }

      void split(splittable_ecuyer1988& a, splittable_ecuyer1988& b) const {
	a = *this;
	a.multiplier1 = mul_mod<modulus1>(a.multiplier1, a.multiplier1);
	a.multiplier2 = mul_mod<modulus2>(a.multiplier2, a.multiplier2);
	b = *this;
	b();
	b.multiplier1 = a.multiplier1;
	b.multiplier2 = a.multiplier2;
      }

      typedef generic_split_iterator<splittable_ecuyer1988> split_iterator;
      typedef std::pair<split_iterator, split_iterator> split_iterator_pair;
      friend class generic_split_iterator<splittable_ecuyer1988>;

      split_iterator_pair split_n(uint32_t n) const { // This RNG is not usable after the split
	split_iterator begin(*this, exp_mod<modulus1>(multiplier1, n), exp_mod<modulus2>(multiplier2, n));
	split_iterator end(*this, begin.new_multiplier1, begin.new_multiplier2);
	end.base.state1 = mul_mod<modulus1>(end.base.state1, end.new_multiplier1);
	end.base.state2 = mul_mod<modulus2>(end.base.state2, end.new_multiplier2);
	return std::make_pair(begin, end);
      }

      split_iterator_pair split_off_n(uint32_t n) { // Allows this RNG to keep being used
	split_iterator_pair p = split_n(n + 1);
	*this = *p.first;
	++p.first;
	return p;
      }

      typedef uint32_t result_type;

      uint32_t operator()() {
	state1 = mul_mod<modulus1>(state1, multiplier1);
	state2 = mul_mod<modulus2>(state2, multiplier2);
	return (state1 + modulus1 - 1 - state2) % (modulus1 - 1);
      }

      BOOST_STATIC_CONSTANT(bool, has_fixed_range = true);
      BOOST_STATIC_CONSTANT(uint32_t, min_value = 0);
      BOOST_STATIC_CONSTANT(uint32_t, max_value = modulus1 - 1);
      uint32_t min BOOST_PREVENT_MACRO_SUBSTITUTION () const {return min_value;}
      uint32_t max BOOST_PREVENT_MACRO_SUBSTITUTION () const {return max_value;}

      static bool validation(uint32_t x) {
	return x == 831582319;
      }
    };

    template <typename Gen>
    class uniform_01_wrapper {
      Gen& gen;

      public:
      uniform_01_wrapper(Gen& gen): gen(gen) {}

      double operator()() const {return boost::uniform_01<double>()(gen);}

      typedef double result_type;
      BOOST_STATIC_CONSTANT(bool, has_fixed_range = true);
      static const double min_value, max_value;
      double min BOOST_PREVENT_MACRO_SUBSTITUTION () const {return min_value;}
      double max BOOST_PREVENT_MACRO_SUBSTITUTION () const {return max_value;}
    };

    template <typename Gen>
    const double uniform_01_wrapper<Gen>::min_value = 0.;
    template <typename Gen>
    const double uniform_01_wrapper<Gen>::max_value = 1.;
  }
}

#endif // BOOST_SPLITTABLE_ECUYER1988_HPP


/**
 * particle_filter.cpp
 *
 * Created on: Dec 12, 2016
 * Author: Tiffany Huang
 */

#include "particle_filter.h"

#include <algorithm>
#include <chrono>
#include <array>
#include <boost/format.hpp>
#include <boost/log/trivial.hpp>
#include <cmath>
#include <ctime>
#include <eigen3/Eigen/Geometry>
#include <iostream>
#include <iterator>
#include <numeric>
#include <random>
#include <string>
#include <vector>
#ifdef _OPENMP
#include <omp.h>
#endif

#include "helper_functions.h"

using Eigen::Isometry2d;
using Eigen::Matrix2Xd;
using Eigen::Vector2d;
using Eigen::VectorXd;
using std::cout;
using std::endl;
using std::string;
using std::vector;

namespace {

/**
 * @brief      Trivial implementation of nearest neighborhood using euclidean
 * distances
 *
 * @param[in]  obs        The obs
 * @param      landmarks  The landmarks
 *
 * @return     the min distance indices in landmark set
 */
int findNN(const Vector2d &obs, Matrix2Xd &predicted) {
  int minIndex = -1;

  if (predicted.cols() < 1) {
    return minIndex;
  }

  VectorXd distances = (predicted.colwise() - obs).colwise().norm();
  double minDist = distances.minCoeff(&minIndex);
  if (not std::isinf(minDist)) {
    predicted.col(minIndex) = Vector2d(std::numeric_limits<double>::infinity(),
                                       std::numeric_limits<double>::infinity());
    return minIndex;
  } else {
    return -1;
  }
}

double Gaussian(const Vector2d &obs, const Vector2d &landmark,
                const Vector2d &std) {
  using std::exp;
  double dx = obs[0] - landmark[0];
  double dy = obs[1] - landmark[1];
  double gaussianNormalizer = 1.0 / (2 * M_PI * std[0] * std[1]);
  return gaussianNormalizer *
         exp(-(dx * dx / (2 * std[0] * std[0]) + (dy * dy)) /
             (2 * std[1] * std[1]));
}

}  // end of namespace

std::pair<Eigen::Matrix2Xd, std::vector<int>>
Particle::computePredictedObservations(const Map &map, double sensorRange,
                                       const Eigen::Vector2d &sensorStd) {
  std::default_random_engine gen;
  gen.seed(std::time(0));
  std::normal_distribution<double> x_d{0.0, sensorStd[0]};
  std::normal_distribution<double> y_d{0.0, sensorStd[1]};

  vector<int> indices;
  const Vector2d pos = {x, y};

  std::vector<Vector2d, Eigen::aligned_allocator<Vector2d>> landmarks;
  for (size_t i = 0; i < map.landmark_list.size(); ++i) {
    Vector2d landmark = {map.landmark_list[i].x_f + x_d(gen),
                         map.landmark_list[i].y_f + y_d(gen)};
    if ((pos - landmark).norm() < sensorRange) {
      indices.push_back(i);
      landmarks.push_back(landmark);
    }
  }
  return std::make_pair(Eigen::Map<const Matrix2Xd>(
                            reinterpret_cast<const double *>(landmarks.data()),
                            2, landmarks.size()),
                        indices);
}

std::vector<int> Particle::observationAssociation(
    const Matrix2Xd &observationsInWorld,
    const Matrix2Xd &predictedObservationsInWorld) {
  associations.clear();

  // Make a local copy
  vector<int> matches;
  Matrix2Xd predictedObservationsInWorld2 = predictedObservationsInWorld;
  for (int i = 0; i < observationsInWorld.cols(); ++i) {
    int nearestPredicted =
        findNN(observationsInWorld.col(i), predictedObservationsInWorld2);
    if (nearestPredicted != -1) {
      matches.push_back(nearestPredicted);
    }
  }
  return matches;
}

void Particle::updateWeight(const Eigen::Matrix2Xd &observationsInWorld,
                            const Eigen::Matrix2Xd &predictedLandmarks,
                            const std::vector<int> &matchedIndices,
                            const Eigen::Vector2d &sensorStd) {
  // BOOST_LOG_TRIVIAL(debug)
  //     << (boost::format(" --- Updating weight of particle %d --- ") % id).str();

  // Reset weight
  weight = 1.0;

  for (size_t i = 0; i < matchedIndices.size(); ++i) {
    Vector2d observation = observationsInWorld.col(i);
    Vector2d predicted = predictedLandmarks.col(matchedIndices[i]);

    const double likelihood = Gaussian(observation, predicted, sensorStd);

    // BOOST_LOG_TRIVIAL(debug)
    //     << (boost::format("observation=%s, landmark=%s, likelihood=%.3e") %
    //         observation.transpose() % predicted.transpose() % likelihood)
    //            .str();
    weight *= likelihood;
  }

  // BOOST_LOG_TRIVIAL(debug)
  //     << (boost::format("Updating weight to %.3e") % weight).str();
}

void ParticleFilter::init(double x, double y, double theta,
                          const Eigen::Vector3d &std) {
  /**
   * TODO: Set the number of particles. Initialize all particles to
   *   first position (based on estimates of x, y, theta and their uncertainties
   *   from GPS) and all weights to 1.
   * TODO: Add random Gaussian noise to each particle.
   * NOTE: Consult particle_filter.h for more information about this method
   *   (and others in this file).
   */
  BOOST_LOG_TRIVIAL(info) << "Initializing particle filters.";
  std::default_random_engine gen;
  gen.seed(std::time(0));

  std::normal_distribution<double> x_d{x, std[0]};
  std::normal_distribution<double> y_d{y, std[1]};
  std::normal_distribution<double> s_d{theta, std[2]};

  particles = vector<Particle>(num_particles);
  weights = vector<double>(num_particles);
  for (int i = 0; i < num_particles; ++i) {
    Particle &p = particles[i];

    p.id = i;
    p.x = x_d(gen);
    p.y = y_d(gen);
    p.theta = s_d(gen);
    p.weight = 1.0;

    weights[i] = 1.0;
  }

  // Set to true after initialization
  is_initialized = true;
}

void ParticleFilter::prediction(double delta_t, const Eigen::Vector3d &std_pose,
                                double velocity, double yaw_rate) {
  /**
   * TODO: Add measurements to each particle and add random Gaussian noise.
   * NOTE: When adding noise you may find std::normal_distribution
   *   and std::default_random_engine useful.
   *  http://en.cppreference.com/w/cpp/numeric/random/normal_distribution
   *  http://www.cplusplus.com/reference/random/default_random_engine/
   */
  using std::abs;
  using std::cos;
  using std::sin;

  // BOOST_LOG_TRIVIAL(info) << "Run prediction.";
  std::default_random_engine gen;
  gen.seed(std::time(0));

  std::normal_distribution<double> x_d{0.0, std_pose[0]};
  std::normal_distribution<double> y_d{0.0, std_pose[1]};
  std::normal_distribution<double> s_d{0.0, std_pose[2]};

  for (size_t i = 0; i < particles.size(); ++i) {
    Particle &p = particles[i];
    double delta_yaw = 0.0;
    double delta_x = 0.0;
    double delta_y = 0.0;
    if (abs(yaw_rate) < 1e-10) {
      delta_x = velocity * delta_t * cos(p.theta);
      delta_y = velocity * delta_t * sin(p.theta);
    } else {
      delta_yaw = yaw_rate * delta_t;
      delta_x = velocity / yaw_rate * (sin(p.theta + delta_yaw) - sin(p.theta));
      delta_y = velocity / yaw_rate * (cos(p.theta) - cos(p.theta + delta_yaw));
    }
    p.x += delta_x + x_d(gen);
    p.y += delta_y + y_d(gen);
    p.theta += delta_yaw + s_d(gen);
  }
}

Eigen::Matrix2Xd Particle::computeObsvervationsInWorld(
    const std::vector<LandmarkObs> &observations) {
  // Construct eigen matrices for convenient computation
  vector<double> obsBuffer(observations.size() * 2);
  for (size_t i = 0, j = 0; i < observations.size(); ++i, j += 2) {
    obsBuffer[j] = observations[i].x;
    obsBuffer[j + 1] = observations[i].y;
  }

  Matrix2Xd obs =
      Eigen::Map<Matrix2Xd>(obsBuffer.data(), 2, observations.size());
  Isometry2d Tparticle = getIsometry2d(x, y, theta);
  return (Tparticle * obs.colwise().homogeneous()).topRows<2>();
}

void ParticleFilter::updateWeights(double sensor_range,
                                   const Eigen::Vector2d &std_landmark,
                                   const vector<LandmarkObs> &observations,
                                   const Map &map_landmarks) {
  /**
   * NOTE: The observations are given in the VEHICLE'S coordinate system.
   *   Your particles are located according to the MAP'S coordinate system.
   *   You will need to transform between the two systems. Keep in mind that
   *   this transformation requires both rotation AND translation (but no
   * scaling). The following is a good resource for the theory:
   *   https://www.willamette.edu/~gorr/classes/GeneralGraphics/Transforms/transforms2d.htm
   *   and the following is a good resource for the actual equation to implement
   *   (look at equation 3.33) http://planning.cs.uiuc.edu/node99.html
   */
  // BOOST_LOG_TRIVIAL(debug) << "Updating particle weights.";
  using Eigen::Isometry2d;
  using Eigen::Vector2f;
  using namespace std::chrono;

  // Update sense_x, sense_y and associations.
  auto start = high_resolution_clock::now();

#ifdef _OPENMP
  #pragma omp parallel for
#endif
  for (size_t i = 0; i < particles.size(); ++i) {
    Particle &p = particles[i];

    const Eigen::Matrix2Xd observationsInWorld =
        p.computeObsvervationsInWorld(observations);

    Matrix2Xd predictedObservations;
    vector<int> predictedLandmarkIndices;
    std::tie(predictedObservations, predictedLandmarkIndices) =
        p.computePredictedObservations(map_landmarks, sensor_range,
                                       std_landmark);

    std::vector<int> matchedIndices =
        p.observationAssociation(observationsInWorld, predictedObservations);

    // Update associations
    p.associations.clear();
    p.sense_x.clear();
    p.sense_y.clear();
    for (int matchedIndex : matchedIndices) {
      // +1 to output the landmark id
      p.associations.push_back(predictedLandmarkIndices[matchedIndex] + 1);
      p.sense_x.push_back(predictedObservations.col(matchedIndex).x());
      p.sense_y.push_back(predictedObservations.col(matchedIndex).y());
    }

    p.updateWeight(observationsInWorld, predictedObservations, matchedIndices,
                   std_landmark);

    weights[i] = p.weight;
  }

  auto elapsedTime = duration_cast<milliseconds>(high_resolution_clock::now() - start).count();
  BOOST_LOG_TRIVIAL(debug) << "updateWeights took " << elapsedTime << " ms";
}

void ParticleFilter::resample() {
  using namespace std;
  if (weights.empty()) {
    return;
  }

  VectorXd weights_ =
      Eigen::Map<VectorXd>(weights.data(), weights.size(), 1);
  weights_ /= weights_.sum();

  // BOOST_LOG_TRIVIAL(info) << "Weights: " << weights_.transpose();

  // First index initialization
  std::default_random_engine gen;
  gen.seed(std::time(0));
  std::uniform_real_distribution<> urd(0, 2.0 * weights_.maxCoeff());
  std::uniform_int_distribution<> rid(0, num_particles - 1);

  int sampledIndex = rid(gen);

  // Resampling the particles
  vector<Particle> newParticles(num_particles);
  // Eigen::VectorXi samples(num_particles);
  // samples[0] = sampledIndex;
  newParticles[0] = particles[sampledIndex];

  double beta = 0.0;

  for (int i = 1; i < num_particles; ++i) {
    beta += urd(gen);
    while (weights_[sampledIndex] < beta) {
      beta -= weights_[sampledIndex];
      sampledIndex = (sampledIndex + 1) % num_particles;
    }
    // samples[i] = sampledIndex; // DEBUG
    newParticles[i] = particles[sampledIndex];
    newParticles[i].weight = weights_[sampledIndex];
    newParticles[i].id = i;
  }

  particles = newParticles;
  // BOOST_LOG_TRIVIAL(info) << "samples: " << samples.transpose();
}

void ParticleFilter::setAssociations(Particle &particle,
                                     const vector<int> &associations,
                                     const vector<double> &sense_x,
                                     const vector<double> &sense_y) {
  // particle: the particle to which assign each listed association,
  //   and association's (x,y) world coordinates mapping
  // associations: The landmark id that goes along with each listed association
  // sense_x: the associations x mapping already converted to world coordinates
  // sense_y: the associations y mapping already converted to world coordinates
  particle.associations = associations;
  particle.sense_x = sense_x;
  particle.sense_y = sense_y;
}

string ParticleFilter::getAssociations(Particle best) {
  vector<int> v = best.associations;
  std::stringstream ss;
  copy(v.begin(), v.end(), std::ostream_iterator<int>(ss, " "));
  string s = ss.str();
  s = s.substr(0, s.length() - 1);  // get rid of the trailing space
  return s;
}

string ParticleFilter::getSenseCoord(Particle best, string coord) {
  vector<double> v;

  if (coord == "X") {
    v = best.sense_x;
  } else {
    v = best.sense_y;
  }

  std::stringstream ss;
  copy(v.begin(), v.end(), std::ostream_iterator<float>(ss, " "));
  string s = ss.str();
  s = s.substr(0, s.length() - 1);  // get rid of the trailing space
  return s;
}