// Copyright (c) 2021 George E. Brown and Rahul Narain
//
// WRAPD uses the MIT License (https://opensource.org/licenses/MIT)
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is furnished
// to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
// By George E. Brown (https://www-users.cse.umn.edu/~brow2327/)

#ifndef SRC_MATH_HPP_
#define SRC_MATH_HPP_

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

#include <iostream>
#include <memory>
#include <string>
#include <vector>

namespace wrapd {
namespace math {

using VecX = Eigen::VectorXd;
using RowVecX = Eigen::RowVectorXd;
using VecXi = Eigen::VectorXi;
using MatX = Eigen::MatrixXd;
using MatXi = Eigen::MatrixXi;
using MatX2 = Eigen::MatrixX2d;
using Mat2X = Eigen::Matrix2Xd;
using MatX3 = Eigen::MatrixX3d;
using Mat3X = Eigen::Matrix3Xd;
using SpMat = Eigen::SparseMatrix<double, Eigen::RowMajor>;
using DiagMat = Eigen::DiagonalMatrix<double, Eigen::Dynamic>;
using RowVec3 = Eigen::RowVector3d;
using RowVec4 = Eigen::RowVector4d;

using Triplet = Eigen::Triplet<double>;
using Triplets = std::vector<Triplet>;

using Doubles = std::vector<double>;

using Cholesky = Eigen::SimplicialLDLT< Eigen::SparseMatrix<double>, Eigen::Lower>;

using ConjugateGradient = Eigen::ConjugateGradient<Eigen::SparseMatrix<double>, Eigen::Lower|Eigen::Upper>;

using AlignedBox = Eigen::AlignedBox<double, 3>;

// Singular value decomposition
template <typename TYPE>
using JacobiSVD = Eigen::JacobiSVD<TYPE>;
static constexpr int ComputeFullU = Eigen::ComputeFullU;
static constexpr int ComputeFullV = Eigen::ComputeFullV;

// Mapping between vectors and matrices
template <typename TYPE>
using Map = Eigen::Map<TYPE>;

// Vectors of doubles
template <int DIM>
using Vec = Eigen::Matrix<double, DIM, 1>;
using Vec2 = Vec<2>;
using Vec3 = Vec<3>;
using Vec4 = Vec<4>;
using Vec5 = Vec<5>;
using Vec6 = Vec<6>;
using Vec7 = Vec<7>;
using Vec8 = Vec<8>;
using Vec9 = Vec<9>;
using Vec10 = Vec<10>;
using Vec11 = Vec<11>;
using Vec12 = Vec<12>;

// Vectors of floats
template <int DIM>
using Vecf = Eigen::Matrix<float, DIM, 1>;
using Vec2f = Vecf<2>;
using Vec3f = Vecf<3>;
using Vec4f = Vecf<4>;

// Vectors of integers
template <int DIM>
using Veci = Eigen::Matrix<int, DIM, 1>;
using Vec1i = Veci<1>;
using Vec2i = Veci<2>;
using Vec3i = Veci<3>;
using Vec4i = Veci<4>;

template <int ROWS, int COLS>
using Mat = Eigen::Matrix<double, ROWS, COLS>;

using Mat1x3 = Mat<1, 3>;
using Mat2x2 = Mat<2, 2>;
using Mat2x3 = Mat<2, 3>;
using Mat3x2 = Mat<3, 2>;
using Mat3x3 = Mat<3, 3>;
using Mat3x4 = Mat<3, 4>;
using Mat3x9 = Mat<3, 9>;
using Mat3x12 = Mat<3, 12>;
using Mat4x3 = Mat<4, 3>;
using Mat4x4 = Mat<4, 4>;
using Mat4x6 = Mat<4, 6>;
using Mat4x9 = Mat<4, 9>;
using Mat6x4 = Mat<6, 4>;
using Mat6x6 = Mat<6, 6>;
using Mat6x9 = Mat<6, 9>;
using Mat9x4 = Mat<9, 4>;
using Mat9x6 = Mat<9, 6>;
using Mat9x9 = Mat<9, 9>;
using Mat9x12 = Mat<9, 12>;
using Mat12x3 = Mat<12, 3>;
using Mat12x9 = Mat<12, 9>;
using Mat12x12 = Mat<12, 12>;

inline double clamp(const double minval, const double val, const double maxval) {
    double retval = val;
    if (val < minval) {
        retval = minval;
    }
    if (val > maxval) {
        retval = maxval;
    }
    return retval;
}

inline void svd(const math::Mat3x3& F, math::Vec3& sigma, math::Mat3x3& U, math::Mat3x3& V, bool identify_flips = true) {

    Eigen::JacobiSVD<math::Mat3x3, Eigen::FullPivHouseholderQRPreconditioner> svd(F, math::ComputeFullU | math::ComputeFullV);
    sigma = svd.singularValues();
    U = svd.matrixU();
    V = svd.matrixV();

    if (identify_flips) {
        math::Mat3x3 J = math::Mat3x3::Identity();
        J(2, 2) = -1.0;
        if (U.determinant() < 0.) {
            U = U * J;
            sigma[2] = -sigma[2];
        }
        if (V.determinant() < 0.0) {
            math::Mat3x3 Vt = V.transpose();
            Vt = J * Vt;
            V = Vt.transpose();
            sigma[2] = -sigma[2];
        }
    }
}

inline void svd(const std::vector<math::Mat3x3> &A, std::vector<math::Vec3> &sigma,
        std::vector<math::Mat3x3> &U, std::vector<math::Mat3x3> &V) {

    const int num_elements = A.size();

    using EigenSVD = Eigen::JacobiSVD<math::Mat3x3, Eigen::FullPivHouseholderQRPreconditioner>;

    #pragma omp parallel
    {
        EigenSVD svd(3, 3, math::ComputeFullU | math::ComputeFullV);
        #pragma omp for
        for (int e = 0; e < num_elements; e++) {
            svd.compute(A[e]);
            sigma[e] = svd.singularValues();
            U[e] = svd.matrixU();
            V[e] = svd.matrixV();
            math::Mat3x3 J = math::Mat3x3::Identity();
            J(2, 2) = -1.0;
            if (U[e].determinant() < 0.) {
                U[e] = U[e] * J;
                sigma[e][2] = -sigma[e][2];
            }
            if (V[e].determinant() < 0.0) {
                math::Mat3x3 Vt = V[e].transpose();
                Vt = J * Vt;
                V[e] = Vt.transpose();
                sigma[e][2] = -sigma[e][2];
            }
        }
    }
}

inline void svd(const math::Mat2x2 &A, math::Vec2 &sigma, math::Mat2x2 &U,
        math::Mat2x2 &V, bool identify_flips = true) {
    Eigen::JacobiSVD<math::Mat2x2, Eigen::FullPivHouseholderQRPreconditioner> svd(A, math::ComputeFullU | math::ComputeFullV);
    sigma = svd.singularValues();
    U = svd.matrixU();
    V = svd.matrixV();

    if (identify_flips) {
        math::Mat2x2 J = math::Mat2x2::Identity();
        J(1, 1) = -1.0;
        if (U.determinant() < 0.) {
            U = U * J;
            sigma[1] = -sigma[1];
        }
        if (V.determinant() < 0.0) {
            math::Mat2x2 Vt = V.transpose();
            Vt = J * Vt;
            V = Vt.transpose();
            sigma[1] = -sigma[1];
        }
    }
}

inline void polar(const math::Mat3x3& F, math::Mat3x3& R, math::Mat3x3& S, bool identify_flips = true) {
    math::Vec3 sigma;
    math::Mat3x3 U;
    math::Mat3x3 V;
    svd(F, sigma, U, V, identify_flips);
    R = U * V.transpose();
    S = V * sigma.asDiagonal() * V.transpose();
}

inline math::Mat3x3 rot(const math::Mat3x3& A) {
    math::Mat3x3 R;
    math::Mat3x3 S;
    polar(A, R, S, true);
    return R;
}

inline math::Mat3x3 sym(const math::Mat3x3& A) {
    math::Mat3x3 R;
    math::Mat3x3 S;
    polar(A, R, S, true);
    return S;
}

// Project a symmetric real matrix to the nearest SPD matrix
// Source: https://github.com/liminchen/OptCuts/blob/master/src/Utils/IglUtils.hpp#L72
template<typename Scalar, int size>
inline void makePD(Eigen::Matrix<Scalar, size, size>& symMtr) {
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, size, size>> eigenSolver(symMtr);
    if(eigenSolver.eigenvalues()[0] >= 0.0) {
        return;
    }
    Eigen::DiagonalMatrix<Scalar, size> D(eigenSolver.eigenvalues());
    int rows = ((size == Eigen::Dynamic) ? symMtr.rows() : size);
    int i = 0;
    for(; i < rows; i++) {
        if(D.diagonal()[i] < 0.0) {
            D.diagonal()[i] = 0.0;
        }
        else {
            break;
        }
    }
    symMtr = eigenSolver.eigenvectors() * D * eigenSolver.eigenvectors().transpose();
}

}  // namespace math
}  // namespace wrapd

#endif  // SRC_MATH_HPP_


#include "BatchAST.h"
#include "InterpreterContext.h"
#include "StringPool.h"
#include "VariablesScope.h"
#include <iostream>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/range/algorithm.hpp>
#include <cmath>

class CSinFunction : public IFunctionAst
{
public:
    double Call(CInterpreterContext &context, const std::vector<double> &arguments) const override
    {
        (void)context;
        if (arguments.size() != 1)
        {
            std::cerr << "sin needs exactly 1 argument" << std::endl;
            return std::numeric_limits<double>::quiet_NaN();
        }
        return std::sin(arguments[0]);
    }

    unsigned GetNameId() const override
    {
        return 0;
    }
};

class CRandFunction : public IFunctionAst
{
public:
    double Call(CInterpreterContext &context, const std::vector<double> &arguments) const override
    {
        (void)context;
        if (arguments.size() != 2)
        {
            std::cerr << "rand needs exactly 2 argument" << std::endl;
            return std::numeric_limits<double>::quiet_NaN();
        }
        if (arguments[0] > arguments[1])
        {
            std::cerr << "rand: first argument must be less than or equal to second" << std::endl;
            return std::numeric_limits<double>::quiet_NaN();
        }
        double rand0to1 = double(std::rand()) / std::numeric_limits<unsigned>::max();
        return (arguments[1] - arguments[0]) * rand0to1 + arguments[0];
    }

    unsigned GetNameId() const override
    {
        return 0;
    }
};

CInterpreterContext::CInterpreterContext(CStringPool &pool)
    : m_pool(pool)
{
    AddBuiltin("sin", std::unique_ptr<IFunctionAst>(new CSinFunction));
    AddBuiltin("rand", std::unique_ptr<IFunctionAst>(new CRandFunction));
}

CInterpreterContext::~CInterpreterContext()
{
}

void CInterpreterContext::DefineVariable(unsigned nameId, double value)
{
    m_scopes.back()->AssignVariable(nameId, value);
}

void CInterpreterContext::AssignVariable(unsigned nameId, double value)
{
    if (CVariablesScope *pScope = FindScopeWithVariable(nameId))
    {
        pScope->AssignVariable(nameId, value);
    }
    else
    {
        DefineVariable(nameId, value);
    }
}

double CInterpreterContext::GetVariableValue(unsigned nameId) const
{
    if (CVariablesScope *pScope = FindScopeWithVariable(nameId))
    {
        return *pScope->GetVariableValue(nameId);
    }
    std::cerr << "error: unknown variable " << m_pool.GetString(nameId) << std::endl;
    return std::numeric_limits<double>::quiet_NaN();
}

void CInterpreterContext::PushScope(std::unique_ptr<CVariablesScope> &&scope)
{
    m_scopes.emplace_back(std::move(scope));
}

std::unique_ptr<CVariablesScope> CInterpreterContext::PopScope()
{
    std::unique_ptr<CVariablesScope> ret(m_scopes.back().release());
    m_scopes.pop_back();
    return ret;
}

size_t CInterpreterContext::GetScopesCount() const
{
    return m_scopes.size();
}

IFunctionAst *CInterpreterContext::GetFunction(unsigned nameId) const
{
    try
    {
        return m_functions.at(nameId);
    }
    catch (std::exception const&)
    {
        std::cerr << "error: unknown function " << m_pool.GetString(nameId) << std::endl;
        return nullptr;
    }
}

void CInterpreterContext::AddFunction(unsigned nameId, IFunctionAst *function)
{
    if (function)
    {
        m_functions[nameId] = function;
    }
}

void CInterpreterContext::PrintResult(double value)
{
    std::cout << "result: " << value << std::endl;
}

void CInterpreterContext::SetReturnValue(boost::optional<double> const& valueOpt)
{
    m_returnValueOpt = valueOpt;
}

boost::optional<double> CInterpreterContext::GetReturnValue() const
{
    return m_returnValueOpt;
}

CVariablesScope *CInterpreterContext::FindScopeWithVariable(unsigned nameId) const
{
    auto range = boost::adaptors::reverse(m_scopes);
    auto it = boost::find_if(range, [=](const auto &pScope) {
        return pScope->HasVariable(nameId);
    });
    if (it != range.end())
    {
        return it->get();
    }
    return nullptr;
}

void CInterpreterContext::AddBuiltin(const std::string &name, std::unique_ptr<IFunctionAst> &&function)
{
    m_builtins.emplace_back(std::move(function));
    unsigned nameRand = m_pool.Insert(name);
    m_functions[nameRand] = m_builtins.back().get();
}


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

  @copyright 2012-2015 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant log10_2lo.This constant is coupled with Log10_2hi and is
    used in the float logarithms computations
    We have abs( double(log10_2lo<float>())+double(Log10_2hi<float>()) - Log10(2.0))) < 3e-11

    @par Semantic:

    @code
    T r = log10_2lo<T>();
    @endcode

**/
  template<typename T> T Log10_2lo();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant log10_2lo.This constant is coupled with Log10_2hi and is
      used in the float logarithms computations
      We have abs( double(log10_2lo<float>())+double(Log10_2hi<float>()) - Log10(2.0))) < 3e-11

      Generate the  constant log10_2lo.

      @return The Log10_2lo constant for the proper type
    **/
    const boost::dispatch::functor<tag::log10_2lo_> log10_2lo = {};
  }
} }
#endif

#include <boost/simd/constant/definition/log10_2lo.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_FreeGasElasticScatteringKernelFactor.hpp
//! \author Alex Robinson
//! \brief  Free gas elastic scattering gkq_set factor decl.
//!
//---------------------------------------------------------------------------//

#ifndef DATA_GEN_FREE_GAS_ELASTIC_SCATTERING_KERNEL_FACTOR_HPP
#define DATA_GEN_FREE_GAS_ELASTIC_SCATTERING_KERNEL_FACTOR_HPP

// Boost Includes
#include <boost/math/policies/policy.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/multiprecision/float128.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

// FRENSIE Includes
#include "MonteCarlo_NuclearScatteringAngularDistribution.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_UnivariateDistribution.hpp"

namespace DataGen{

//! The free gas elastic scattering gkq_set factor class
class FreeGasElasticScatteringKernelFactor
{

private:

  // Typedef for bessel function error policy
  typedef boost::math::policies::domain_error<
    boost::math::policies::throw_on_error> domain_error;
  typedef boost::math::policies::pole_error<
    boost::math::policies::throw_on_error> pole_error;
  typedef boost::math::policies::overflow_error<
    boost::math::policies::throw_on_error> overflow_error;
  typedef boost::math::policies::evaluation_error<
    boost::math::policies::throw_on_error> evaluation_error;

  typedef boost::math::policies::policy<
    domain_error,
    pole_error,
    overflow_error,
    evaluation_error> Policy;

  // Typedef for extended precision floating point values
  typedef boost::multiprecision::float128 BoostLongDouble;

public:

  //! Constructor
  FreeGasElasticScatteringKernelFactor(
	  const std::shared_ptr<Utility::UnivariateDistribution>&
	  zero_temp_elastic_cross_section,
          const std::shared_ptr<MonteCarlo::NuclearScatteringAngularDistribution>&
	  cm_scattering_distribution,
	  const double A,
	  const double kT,
	  const double alpha,
	  const double beta,
	  const double E );

  //! Destructor
  ~FreeGasElasticScatteringKernelFactor()
  { /* ... */ }

  //! Set the alpha, beta, and energy values
  void setIndependentVariables( const double alpha,
				const double beta,
				const double E );

  //! Evaluate the factor at a desired value of the center-of-mass angle cosine
  double operator()( const double mu_cm ) const;

  //! Get the integrated value (over all mu_cm => [-1,1])
  double getIntegratedValue( double& error_estimate ) const;

private:

  // Calculate the cached values
  void calculateCachedValues();

  // Find limits to integrate over
  void findLimits( double& lower_limit, double& upper_limit ) const;

  // Find a CM scattering angle cosine where the function is non-zero
  double findCMScatteringAngleCosineWithNonZeroFunctionValue(
					std::list<double>& grid_points ) const;

  // The neutron kinetic energy multiplier
  static double neutron_kinetic_energy_multiplier;

  // The min exponential argument that can be used before returning zero
  static double min_exp_arg;

  // The integration gkq_set
  Utility::GaussKronrodIntegrator<double> d_gkq_set;

  // The zero temperature cross section
  std::shared_ptr<Utility::UnivariateDistribution> d_zero_temp_elastic_cross_section;

  // The cm scattering angle PDF
  std::shared_ptr<MonteCarlo::NuclearScatteringAngularDistribution>
  d_cm_scattering_distribution;

  // The atomic weight ratio
  double d_A;

  // The temperature value (MeV)
  double d_kT;

  // The alpha value
  double d_alpha;

  // The beta value
  double d_beta;

  // The energy value (MeV)
  double d_E;

  // Cached energy ratio
  double d_energy_ratio;

  // Cached exponential argument multiplier
  double d_exponential_arg_mult;

  // Cached exponential arg constant
  double d_exponential_arg_const;

  // Cached bessel argument multiplier
  double d_bessel_arg_mult;

  // Cached relative velocity multiplier
  double d_relative_velocity_mult;
};

} // end DataGen namespace

#endif // end DATA_GEN_FREE_GAS_ELASTIC_SCATTERING_KERNEL_FACTOR_HPP

//---------------------------------------------------------------------------//
// end DataGen_FreeGasElasticScatteringKernelFactor.hpp
//---------------------------------------------------------------------------//


///John Baker NASA-GSFC (2019)
///
///This file provides a class for managing testing of proposal distributions.
/// FIXME: SO FAR THIS IS JUST A SKETCH OF A PROPOSED IDEA

#ifndef PTMCMC_TEST_PROPOSAL_HH
#define PTMCMC_TEST_PROPOSAL_HH

#include "states.hh"
#include "probability_function.hh"
#include "proposal_distribution.hh"
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <Eigen/Dense>
#include "Mrpt.h"
#include <algorithm>

using namespace std;

///Class for managing testing of proposal distributions.
///
///This is to allow unit testing to maintain existing proposals, and most
///crucially to allow a systematic way of testing planned user-defined proposals.
///Tests are based on verifying that the image of a set of samples that have been
///drawn from some target distribution and then transformed by a MH step [wrt the
///target distribution] on each point of the sample set using the proposal
///distribution does still match the target distribution.  The match can be
///measured by the Kullback-Leibler (KL) divergence.
///
///A sampleable_probability_function can be provided as the target distribution.
///Eventually plan for the option of auto-generating of an appropriate target
///distribution.
///
///  Functioning version of proposal testing. This is class which defines a
/// test of a proposal_distribution. The test applies the proposal multiple
/// times and then compares the resulting distribution against the original
/// sample distirbution and also against an independently redrawn sample.
/// The first test is based on the KLdivergence among the sample distributions.
/// Since KLdiv is asymmetric, this is tried both ways.  To quantify the result
/// the same test is applied to multiple re-draws of the original sample
/// distribution.  The test is considered to pass if the transformed sample
/// KLdiv value does not exceed some percentile of the re-draw comparison
/// values, stated explicitly in the output.  Since the KLdiv test is evidently
/// prone to a fair amount of noise a second "fake" KL test is performed which
/// is computed from only the mean and variance of each distribution.  This
/// test statistic reduces to the KL test for sufficiently large Gaussian
/// distributions, but it is much faster and less noisy, thus more sensitive
/// to variance and mean differences, though it would be insensitive to
/// differences which don't show up in the mean and variance.
class test_proposal {
  shared_ptr<Random> rng;
  proposal_distribution *proposal=nullptr;
  sampleable_probability_function *target_dist=nullptr;
  bool loop;
  chain ch;
  string path;
public:
  //test_proposal(proposal_distribution &proposal, shared_ptr<Random> rng){cout<<"The idea is to provide a ready-made test distribution, perhaps a gaussian mixture, perhaps augmented by gaussians located at proposal-step images of the initial gaussian centers.  This is not yet implemented."<<endl;exit(-1);};
  test_proposal(proposal_distribution &proposalref, sampleable_probability_function &target, bool loop=false, string path_="", vector<int> index=vector<int>()):
    rng(new MotherOfAll(ProbabilityDist::getPRNG()->Next())),
    proposal(&proposalref),
    target_dist(&target),
    loop(loop),
    path(path_)
  {
    //Note: if proposal needs a chain, we may need to make one up with samples from the target dist.
    //configure output name
    if(path!=""){
      //cout<<"path.back()='"<<path.back()<<"'"<<endl;
      //cout<<(path.back()=='/'?"match":"non-match")<<endl;
      if(path.back()=='/')path=path+"test_prop";
      if(index.size()>0)path=path+"_"+to_string(index[0]);
      if(index.size()>1)for(int i: vector<int>(index.begin()+1,index.end()))path+="-"+to_string(i);
      cout<<"path='"<<path<<"'"<<endl;
    }

    //If proposal is a proposal_set and index is provided, then we access only the sub proposal
    while(index.size()>0){
      proposal_distribution_set *set=dynamic_cast<proposal_distribution_set*>(proposal);
      if(set){
	proposal=set->members()[index[0]];
	vector<int> subindex(index.begin()+1,index.end());
	index=subindex;
      }else break;
    }
  };

  test_proposal(){};//Trivial constructor for development and testing...
  vector<state> sample_target_dist(int nsamp, double maxfail=1e100){
    //make a set of samples from the target distribution
    vector<state> samples(nsamp);
    bool done=false;
    for( auto &sample : samples){
      int failcount=0;
      done=false;
      while(not (done or failcount>maxfail)){
	sample=target_dist->drawSample(*rng);
	sample.enforce();
	if(not sample.invalid())done=true;
	else failcount++;
	//if(sample.get_params_vector()==copy.get_params_vector())done=true;
	//else cout<<"rejected state "<<copy.get_string()<<endl;
	//No we are using rejection sampling to reject any part of the
	//target distribution outside of the stateSpace domain
      }
      if(not done){
	cout<<"test_proposal::sample_target_dist: Struggling to find draw valid samples. Bailing out."<<endl;
	return vector<state>(0);//
      }
    }
    return samples;
  };
  void write_samples(vector<state> samps, string tag){
    if(path!=""){
      string file=path+"_"+tag+".dat";
      cout<<"Will write to "<<file<<endl;
      ofstream out(file);
      for(auto s : samps)out<<s.get_string()<<endl;
    }
  };
  vector<state> transform_samples(vector<state> &samples, int & Naccept,double hastings_err=0){
    //Apply the proposal via Metropolis-Hastings step, to each sample and return result.
    //Number of acceptances goes in Naccept
    //set hastings_err!=0 to force a nontrivial test (even with a good proposal)
    Naccept=0;
    int nsame=0;double report_thresh=0.01,threshstep=0.01;
    size_t N=samples.size();
    vector<state> transformed(N);

    for(size_t i=0;i<N;i++){
      //cout<<i<<endl;
      state &oldstate=samples[i];
      double oldlpdf=target_dist->evaluate_log(oldstate);
      oldstate.enforce();//this shouldn't be needed
      if(oldstate.invalid())cout<<"enforcement failure for oldstate "<<oldstate.get_string()<<endl;
      state newstate=proposal->draw(oldstate,&ch);
      newstate.enforce();
      double newlpdf=target_dist->evaluate_log(newstate);

      //Now the test:
      double log_hastings_ratio=proposal->log_hastings_ratio();
      log_hastings_ratio+=newlpdf-oldlpdf+hastings_err;
      bool accept=true;
      if(newstate.invalid())accept=false;
      if(accept and log_hastings_ratio<0){
	double x=rng->Next(); //pick a number
	accept=(log(x)<log_hastings_ratio);
      }

      //cout<<"old: "<<oldstate.get_string()<<" --> "<<oldlpdf<<endl;
      //cout<<"new: "<<newstate.get_string()<<" --> "<<newlpdf<<endl;
      //cout<<"    "<<(accept?"ACCEPTED    ":    "REJECTED")<<endl;
      if(accept){
	if(newstate.invalid())cout<<"enforcement failure for newstate "<<newstate.get_string()<<endl;
	Naccept++;
	proposal->accept(); //As with MCMC, this allows some proposals to 'adapt'
	transformed[i]=newstate;
	if(false and oldstate.dist2(newstate)<1e-16){
	  nsame++;
	  if(nsame>Naccept*report_thresh){
	    report_thresh+=threshstep;
	    cout<<"test_proposal: Transformed state not significantly different."<<endl;
	    cout<<" Nsame/Naccept"<<nsame<<"/"<<Naccept<<"="<<nsame*1.0/Naccept<<endl;
	    cout<<"                s="<<oldstate.get_string()<<endl;
	    cout<<"               s'="<<newstate.get_string()<<"  dist2="<<oldstate.dist2(newstate)<<endl;

	    vector<state> v={oldstate};
	    involution_proposal *invprop = dynamic_cast<involution_proposal*>(proposal);
	    if(invprop){
	      stateSpaceInvolution involution=invprop->get_involution();
	      cout<<" Intrinsic test:"<<endl;
	      involution.test_involution(oldstate,-1);
	    }
	  }
	}
      }
      else {
	proposal->reject();
	transformed[i]=oldstate;
      }
    }
    return transformed;
  };
  ///This is the core test.
  ///
  ///  1. Construct Nsamp samples from the target distribution.
  ///  2. Apply M-H steps based on the proposal to each sample, through as many cycles
  ///     as needed so the total number of accepted proposals is at least Nsamp*ncyc
  ///  3. Compute the KL divergence between the orginal and transformed set distributions
  ///     -KLdiv is generally asymmetric, but here we are interested in near equivalent distributions
  ///      where it is approximately symmetric.
  ///     -Note: For insight, Gaussians differing in 1 dimensions, yield:
  ///        KL(A||B) = (1/2) [ (mu(A)-mu(B))^2 / sigma(B)^2  + sigma(A)^2/sigma(B)^2 - 1 - log(sigma(A)^2/sigma(B)^2) ]
  ///     -Thus for small differences:
  ///        KL( A[mu+dmu*sigma,sigma*(1+dlnsig)] || B[mu,sigma]) ~ dmu^2 + dlnsig^2
  ///  4. A challenge is knowing what level of difference is significant, and what level is measurable.
  ///     One approach to this is to compare KL measurements of redraws of the original sample with
  ///     those of the transformed set.
  ///  5. Note ncyc will be cut back if sampling is too inefficient
  ///  6. If acceptance rate is < min_accept then will return with failure, otherwise returns true.
  bool test(int Nsamp=10000, double ncyc=5, int ntry=10, double hastings_err=0, double min_accept=0){
    cout<<"\n\nTesting proposal: "<<proposal->show()<<endl;
    cout<<"ncyc="<<ncyc<<endl;
    //If loop==true, then we make a new test_proposal object for the sub_proposal, and loop;
    if(loop){
      proposal_distribution_set *set=dynamic_cast<proposal_distribution_set*>(proposal);
      if(set){
	auto members=set->members();
	int n=members.size();
	cout<<n<<" members in proposal set"<<endl;
	int i=0;
	for(auto member : members){
	  cout<<"Preparing and testing member "<<i+1<<" of "<<n<<endl;
	  string subpath=path;
	  if(path!="")subpath=subpath+"-"+to_string(i);
	  cout<<"subpath='"<<subpath<<"'"<<endl;
	  test_proposal subtest(*member, *target_dist,true, subpath);
	  bool ok=subtest.test(Nsamp,ncyc,ntry,hastings_err,min_accept);
	  if(not ok)return false;
	  i++;
	}
	return true;
      }
    }

    //Draw reference samples
    vector<state> target_samples=sample_target_dist(Nsamp,100);
    if(target_samples.size()<Nsamp)return false;
    write_samples(target_samples,"target");

    //Perform intrinsic test if any
    string intrinsic_test_result=proposal->test(target_samples,*rng);
    if(intrinsic_test_result!="")cout<<" Intrinsic test:\n"<<intrinsic_test_result<<endl;

    //Transform by application of the proposal
    int Naccept_cum=0;
    int Ngoal=ncyc*Nsamp;
    vector<state> transformed=target_samples;
    cout<<"Applying transform:"<<endl;
    int icyc=1;
    while(Naccept_cum<Ngoal){
      int Naccept=0;
      vector<state> old=transformed;
      transformed=transform_samples(transformed, Naccept,hastings_err);
      Naccept_cum+=Naccept;
      if(icyc%int(ncyc+1)==0)
	cout<<100.0*Naccept/Nsamp<<"\% accepted,  "<<100.0*Naccept_cum/Ngoal<<" percent done."<<endl;
      if(icyc==2000 and Naccept_cum<Ngoal*0.01){//Not sampling fast enough
	//Reduce goal for number of cycles
	ncyc=ncyc/2.0;
	if(Naccept*1.0/Nsamp<min_accept){
	  cout<<"Acceptance rate too low. Giving up."<<endl;
	  return false;
	}
	cout<<"Sampling poorly.  Reducing to ncyc="<<ncyc<<endl;
	//start over
	Ngoal=ncyc*Nsamp;
	icyc=0;
	Naccept_cum=0;
      }
      icyc++;
    }
    write_samples(transformed,"transformed");

    vector<double> mean,var;
    var=get_sample_var(target_samples,mean);
    cout<<"target mean:      ";for(auto x:mean)cout<<x<<" ";cout<<endl;
    cout<<"target var:       ";for(auto x:var)cout<<x<<" ";cout<<endl;
    var=get_sample_var(transformed,mean);
    cout<<"transformed mean: ";for(auto x:mean)cout<<x<<" ";cout<<endl;
    cout<<"transformed var:  ";for(auto x:var)cout<<x<<" ";cout<<endl;

    //Compute KL divergence
    //First decide whether to use approx NN:
    double dim=target_samples[0].size();
    bool use_approxNN=(Nsamp*pow(dim,0.7)>4500); //estimate of when approx is faster
    cout<<"Computing KL divergences:"<<endl;
    double KLtransformed=KL_divergence(transformed,target_samples,use_approxNN);
    double KLtransformedX=KL_divergence(target_samples,transformed,use_approxNN);
    double fKLtransformed=fake_KL_divergence(transformed,target_samples);
    double fKLtransformedX=fake_KL_divergence(target_samples,transformed);

    //For comparison estimate statistics of redraws of samples (expected value is 0)
    double sum=0;
    int fntry=30*ntry;
    vector<double> KLdiffs(ntry),fKLdiffs(fntry);
    for(int i=0;i<ntry;i++){
      //draw alternative samples;
      vector<state> alt_samples=sample_target_dist(Nsamp);
      KLdiffs[i]=KL_divergence(alt_samples,target_samples,use_approxNN);
      //cout<<"("<<KLdiffs[i]<<","<<fKLdiffs[i]<<")"<<(i<ntry-1?",":"");
      //cout<<KLdiffs[i]<<(i<ntry-1?",":"");
    }
    //cout<<endl;
    cout<<"Computing 'fake' KL divergences:"<<endl;
    for(int i=0;i<fntry;i++){
      //draw alternative samples;
      vector<state> alt_samples=sample_target_dist(Nsamp);
      fKLdiffs[i]=fake_KL_divergence(alt_samples,target_samples);
      //cout<<"("<<KLdiffs[i]<<","<<fKLdiffs[i]<<")"<<(i<fntry-1?",":"");
    }
    //cout<<endl;
    //draw alternative samples;
    vector<state> alt_samples=sample_target_dist(Nsamp);
    double altKLtransformed=KL_divergence(transformed,alt_samples,use_approxNN);
    double altKLtransformedX=KL_divergence(alt_samples,transformed,use_approxNN);
    double altfKLtransformed=fake_KL_divergence(transformed,alt_samples);
    double altfKLtransformedX=fake_KL_divergence(alt_samples,transformed);
    for(auto &diff:KLdiffs)diff=fabs(diff);
    for(auto &diff:fKLdiffs)diff=fabs(diff);
    sort(KLdiffs.begin(),KLdiffs.end());
    sort(fKLdiffs.begin(),fKLdiffs.end());
    int Ncut=sqrt(ntry);
    double KLcut=0;
    double cut_frac=Ncut*1.0/ntry;
    if(ntry>1){
      KLcut=(KLdiffs[ntry-Ncut]+KLdiffs[ntry-Ncut-1])/2.0;
      //cout<<"KLdiffs";for(auto diff:KLdiffs)cout<<diff<<" ";cout<<endl;
      cout<<"\nEstimate that only "<<cut_frac*100<<"\% of KLdiff measurements are likely to exceed "<<KLcut<<" for matching distributions."<<endl;
    }
    cout<<"Transformed KLdiv="<<KLtransformed<<" <-> "<<KLtransformedX<<endl;

    cout<<"Alt-transformed KLdiv="<<altKLtransformed<<" <-> "<<altKLtransformedX<<endl;
    if(altKLtransformed-KLtransformed>fabs(KLcut))cout<<"When 'Transformed' is signficantly less than 'alt-Transformed' it may indicate that the effect of the proposal (after ncyc="<<ncyc<<" applications) is too small to measure at this level."<<endl;
    if(altKLtransformed<KLcut)cout<<"PASS"<<endl;
    else cout<<"FAIL\nKL value is "<<altKLtransformed/KLcut<<" times the stated threshold."<<endl;
    //Report fake KL tests
    Ncut=sqrt(fntry);
    KLcut=0;
    cut_frac=Ncut*1.0/fntry;
    if(fntry>1){
      //cout<<"fKLdiffs";for(auto diff:fKLdiffs)cout<<diff<<" ";cout<<endl;
      KLcut=(fKLdiffs[fntry-Ncut]+fKLdiffs[fntry-Ncut-1])/2.0;
      cout<<"\nEstimate that only "<<cut_frac*100<<"\% of fKLdiff measurements are likely to exceed "<<KLcut<<" for matching distributions."<<endl;
    }
    cout<<"Transformed fKLdiv="<<fKLtransformed<<" <-> "<<fKLtransformedX<<endl;

    cout<<"Alt-transformed fKLdiv="<<altfKLtransformed<<" <-> "<<altfKLtransformedX<<endl;
    if(altfKLtransformed-fKLtransformed>fabs(KLcut))cout<<"When 'Transformed' is signficantly less than 'alt-Transformed' it may indicate that the effect of the proposal (after ncyc="<<ncyc<<" applications) is too small to measure at this level."<<endl;
    if(altfKLtransformed<KLcut)cout<<"PASS"<<endl;
    else cout<<"FAIL\nKL value is "<<altfKLtransformed/KLcut<<" times the stated threshold."<<endl;
    //double result=altKLtransformed;
    //if(result<0)result=0;
    //result+=altfKLtransformed;
    return true;
  };

    double KL_divergence(vector<state> &samplesP,vector<state> &samplesQ,bool approx_nn=true){
    ///Interesting to apply to both target samples and transformed samples
    /// Implements eqn 2 of Perez-Cruz 2008 "Kullback-Leibler Divergence Estimation of Continuous Distributions"
    /// with k=1. We infer that the sign is wrong on the sum term there.
    ///Notes:
    /// -We see faster convergence when the Q set has the larger variance (more specifically when all points in P
    ///  are in regions with some Q support) as noted near Fig2 of Perez-Cruz.
    /// -We see faster runs for the approximate method when N>~3000. (On a laptop example eval takes a few 100ms there in 2D).
    /// -With k=2,target_recall=0.99 we get not much difference in biases for approx/exact (diff << stat err for 2D tests)
    /// -More tests varying d<=20 suggests crossing point is about N*d^0.7~4500
    /// -exact method cost seems to scale with n  and  with (1+d)           [near n~1000]
    /// -approx method cost seems to scale with n^1/2 and with (1+d)^.2
    int k=2;double target_recall=0.99;//For internal use of Mrpt;
    bool useMrptP=approx_nn,useMrptQ=approx_nn;
    double result=0;
    double N=samplesP.size();
    //cout<<"KL:about to call all_nnd2"<<endl;
    //vector<double> s1sqs=all_nnd2(samplesP);
    vector<double> s1sqs,r1sqs;
    if(useMrptP)r1sqs=all_nnd2_mrpt(samplesP,samplesP,true,k,target_recall);
    else r1sqs=all_nnd2(samplesP);
    if(useMrptQ)s1sqs=all_nnd2_mrpt(samplesP,samplesQ,false,k,target_recall);
    else {
      s1sqs.resize(N);
      for(int i=0;i<N;i++)s1sqs[i]=one_nnd2(samplesP[i],samplesQ);
    }
    if(useMrptQ and false){//testing
      vector<double>tests1sqs(N);
      for(int i=0;i<N;i++)tests1sqs[i]=one_nnd2(samplesP[i],samplesQ);
      //Copied/adapted from all_nnd2_mrpt::
      Eigen::MatrixXf X=get_X(samplesQ);
      Mrpt mrpt=get_mrpt(X,k);
      for(int i=0;i<N;i+=20){
	cout<<"test s1sqs["<<i<<"]: "<<tests1sqs[i]<<" -> "<<s1sqs[i]<<endl;
	//get i0 byt the non-mrpt method
	int i0;
	double nnd2=one_nnd2(samplesP[i],samplesQ,&i0);
	cout<<"  "<<i0<<": "<<nnd2<<endl;
	//get i0 and two version of d^2 by mrpt method:
	i0=one_knn(samplesP[i],mrpt,k,0);
	nnd2=samplesP[i].dist2(samplesQ[i0]);
	vector<double> Pveci=samplesP[i].get_params_vector();
	double nnd2_raw=0;
	for(int j=0;j<Pveci.size();j++){
	  double diff=Pveci[j]-X(j,i0);
	  nnd2_raw+=diff*diff;
	}
	cout<<"  "<<i0<<": "<<nnd2_raw<<" -> "<<nnd2<<endl;
      }
    }
    //Here we put a floor on the smallest value of all NN distances
    //based on the kfloorth smallest distance within the P set
    int kfloor=5;
    if(kfloor>0){
      auto dists=r1sqs;
      sort(dists.begin(),dists.end());
      double floor=dists[kfloor+1];
      for(auto&d2:r1sqs)if(d2<floor)d2=floor;//this is pretty inefficient!
      for(auto&d2:s1sqs)if(d2<floor)d2=floor;//this is pretty inefficient!
    }

    for(int i=0;i<N;i++){
      //double r1sq=one_nnd2(samplesP[i],samplesQ);
      result+=-log(r1sqs[i]/s1sqs[i]);
      //cout<<i<<" "<<r1sqs[i]<<" "<<s1sqs[i]<<" -> "<<result<<endl;
    }
    double dim=samplesP[0].size();
    int M=samplesQ.size();
    result *= (0.5*dim)/N;//factor of 1/2 because we use nearest neighbor dist^2
    result += log(M/(N-1.0));
    //cout<<"KL about to return"<<endl;
    return result;

  };
  double one_nnd2(state &s,vector<state> &samples,int *out_i0=nullptr){//Computation of nearest neighbor distance to point, brute force.

    size_t N=samples.size();
    double nnd2=-1;
    int i0=-1;
    for(size_t i=0;i<N;i++){
      double dist2=samples[i].dist2(s);
      //double dist2ji=s.dist2(samples[i]);//debug
      //if(fabs((dist2-dist2ji)/dist2)>1e-14)cout<<"distance not symmetric "<<dist2<<"!="<<dist2ji<<endl;
      if(nnd2<0 or nnd2>dist2){
	i0=i;
	nnd2=dist2;
      }
      if(out_i0)*out_i0=i0;
      //cout<<"  "<<i<<": "<<nnd2<<endl;
    }
    return nnd2;
  };
  vector<double> all_nnd2(vector<state> &samples){//Computation of all nearest neighbor distances, brute force.
    size_t N=samples.size();
    vector<int> nni(N);
    vector<double> nnd2(N,-1);
    for(size_t i=0;i<N;i++){
      //cout<<"state i="<<i<<":"<<samples[i].show()<<endl;
      for(size_t j=i+1;j<N;j++){
	//cout<<"  state j="<<j<<":"<<samples[j].show()<<endl;
	double dist2=samples[i].dist2(samples[j]);
	//double dist2ji=samples[j].dist2(samples[i]);//debug
	//if(fabs((dist2-dist2ji)/dist2)>1e-14)cout<<"distance not symmetric "<<dist2<<"!="<<dist2ji<<endl;
	if(nnd2[i]<0 or nnd2[i]>dist2){
	  nni[i]=j;
	  nnd2[i]=dist2;
	}
	if(nnd2[j]<0 or nnd2[j]>dist2){
	  nni[j]=i;
	  nnd2[j]=dist2;
	}
      }
    }
    return nnd2;
  };

  double fake_KL_divergence(vector<state> &samplesP,vector<state> &samplesQ){
    //This applies a simplified alternative to the KL divergence (which is difficult to compute accurately from samples).
    //The calculation is based on the means and variances of the two samples and would agree with the KL diverences
    //for large samples of Gaussian distributions.
    //The KL-divergence between two Gaussians is
    // 2 KLdiv(P,Q) = Tr [ cov(P) cov(Q)^-1 ]- dim - log | cov(P) cov(Q)^-1 | - (mu(P)-mu(Q))^t cov(Q)^-1 (mu(P)-mu(Q))^t
    Eigen::MatrixXd covP,covQ,invCovQ,covPinvCovQ;
    Eigen::VectorXd meanP,meanQ,dmu;
    covP=get_sample_cov(samplesP,meanP);
    covQ=get_sample_cov(samplesQ,meanQ);
    int dim=covP.rows();
    int nQ=samplesP.size();
    double unbiasing_factor=(nQ-dim-2.0)/(nQ-1.0);//The final factor is to make unbiased for finite nQ
    invCovQ=covQ.inverse()*unbiasing_factor;
    covPinvCovQ=covP*invCovQ;
    dmu=meanP-meanQ;
    double result=0;
    //cout<<"\n\ncovPdiag:\n"<<covP.diagonal()<<endl;
    //cout<<"\n\ncovQdiag:\n"<<covQ.diagonal()<<endl;
    //Eigen::MatrixXd covP2=covP*covP;
    //cout<<"\n\ncovP^2:\n"<<covP*covP<<endl;
    //cout<<"\n\ncovP^2:\n"<<covP2<<endl;
    //cout<<"|covP|^2:"<<(covP*covP).trace()<<endl;
    //cout<<"covPdiag: "<<covP.diagonal().transpose()<<endl;
    //cout<<"|covQ|^2:"<<(covQ*covQ).trace()<<endl;
    //cout<<"covQdiag: "<<covQ.diagonal().transpose()<<endl;
    //cout<<"invCovQdiag: "<<invCovQ.diagonal().transpose()<<endl;
    result += -dim + covPinvCovQ.trace();
    //cout<<"result A:"<<result<<endl;
    result += -log((covPinvCovQ/unbiasing_factor).determinant());  //If nP != nQ then we need another digamma-based term to make unbiased
    //cout<<"result AB:"<<result<<endl;
    result += dmu.transpose()*invCovQ*dmu - (dim + covPinvCovQ.trace())/nQ;
    //cout<<"result ABC:"<<result<<endl;

    return 0.5*result;
    //return 0.5*(covPinvCovQ.trace()-dim -log(covPinvCovQ.determinant()) +dmu.transpose()*invCovQ*dmu);
  };

  Eigen::MatrixXf get_X(vector<state> &samples){
    size_t N=samples.size();;
    vector<double> statevec=samples[0].get_params_vector();
    int dim=statevec.size();
    Eigen::MatrixXf X(dim, N);
    Eigen::VectorXf v(dim);

    for(size_t i=0;i<N;i++){
      statevec=samples[i].get_params_vector();
      for(size_t j=0;j<dim;j++)
	X(j,i)=statevec[j];
    }
    return X;
  };

  Mrpt get_mrpt(Eigen::MatrixXf &X,int k=8,double recall=0.9){
    Mrpt mrpt(X);
    mrpt.grow_autotune(recall, k);
    return mrpt;
  };

  int one_knn(state &s,Mrpt &mrpt,int k=8,int ik=0){//Computation of nearest neighbor distance to point, brute force.
    //ik is index of which kth nearest neighbor to take (not nec same as how many are found)
    //note that when state is s among those used to define mrpt, then we often want to exclude that one, hence ik=1
    vector<double> statevec=s.get_params_vector();
    int dim=statevec.size();
    Eigen::VectorXf v(dim);

    for(size_t j=0;j<dim;j++) v(j)=statevec[j];

    int nni=-1,index[k];
    for(int i=0;i<=ik;i++)index[i]=-1;
    bool exact=false;
    //cout<<"k-"<<k<<endl;
    if(!exact){
      mrpt.query(v,index);
      nni=index[ik];
    }
    if(nni<0){//approx search failed
      mrpt.exact_knn(v, ik+1, index);
      nni=index[ik];
      //cout<<nni<<" ";
    }
    //cout<<nni<<endl;
    return nni;
  };

  vector<double>all_nnd2_mrpt(vector<state> &teststates,vector<state>&samples,bool same_set=false,int k=8,double target_recall=0.9,bool nonzero=true){
    //Computation of all nearest neighbor distances, using mrpt header-library.
    //This version doesn't know the parameter space topology in identifying nearest neighbors,
    //but does use the state.dist2 function for reporting the nearest distance.
    //
    Eigen::MatrixXf X=get_X(samples);
    Mrpt mrpt=get_mrpt(X,k,target_recall);
    size_t N=teststates.size();
    vector<double> nnd2(N);
    int ik=0;
    if(same_set)ik=1;
    if(k<=ik)k=ik+1;
    //cout<<k<<">"<<ik<<endl;
    for(size_t i=0;i<N;i++){
      double d2=0;
      size_t i0=one_knn(teststates[i],mrpt,k,ik);
      //cout<<"i,i0:"<<i<<" "<<i0<<endl;
      nnd2[i]=teststates[i].dist2(samples[i0]);
    }
    if(false and nonzero){//replace zeros with the next smallest value
      double eps=1e100;
      for(auto d2 :nnd2)if(d2<eps and d2>0)eps=d2;
      for(auto &d2 :nnd2)if(d2<eps)d2=eps;
    }
    return nnd2;
  };
  vector<double>all_nnd2_mrpt_v2_orig(vector<state> &samples){
    //Computation of all nearest neighbor distances, using mrpt header-library.
    //This version doesn't know the parameter space topology in identifying nearest neighbors,
    //but does use the state.dist2 function for reporting the nearest distance.
    int k=8;
    Eigen::MatrixXf X=get_X(samples);
    Mrpt mrpt=get_mrpt(X,k);
    size_t N=samples.size();
    vector<double> nnd2(N);
    for(size_t i=0;i<N;i++){
      size_t i0=one_knn(samples[i],mrpt,k,1);
      nnd2[i]=samples[i].dist2(samples[i0]);
    }
    return nnd2;
  };
  vector<double> get_sample_mean(const vector<state>&samples){
    size_t N=samples.size();
    size_t dim=samples[0].size();
    vector<double>sum(dim,0);
    for(auto s:samples)for(int j=0;j<dim;j++)sum[j]+=s.get_param(j);
    for(auto &x :sum)x/=N;
    return sum;
  };
  vector<double> get_sample_var(const vector<state>&samples){
    vector<double>mean;
    return get_sample_var(samples,mean);
  }
  vector<double> get_sample_var(const vector<state>&samples,vector<double>&out_mean){
    size_t N=samples.size();
    size_t dim=samples[0].size();
    out_mean=get_sample_mean(samples);
    vector<double>sum(dim,0);
    for(auto s:samples)for(int j=0;j<dim;j++){
	double diff=s.get_param(j)-out_mean[j];
	sum[j]+=diff*diff;
      }
    for(auto &x :sum)x/=(N-1.0);
    return sum;
  };
  Eigen::MatrixXd get_sample_cov(const vector<state>&samples){
    Eigen::VectorXd mean;
    return get_sample_cov(samples,mean);
  }
  Eigen::MatrixXd get_sample_cov(const vector<state>&samples, Eigen::VectorXd &out_mean){
    size_t N=samples.size();
    size_t dim=samples[0].size();
    Eigen::MatrixXd cov(dim,dim);
    cov = Eigen::MatrixXd::Zero(dim,dim);
    vector<double> mean=get_sample_mean(samples);
    out_mean=Eigen::VectorXd::Zero(dim);
    for(size_t i=0;i<dim;i++)out_mean(i)=mean[i];
    vector<double>sum(dim,0);
    for(auto s:samples)for(int j=0;j<dim;j++){
	double jdiff=s.get_param(j)-mean[j];
	cov(j,j)+=jdiff*jdiff;
	for(int i=j+1;i<dim;i++){
	  double idiff=s.get_param(i)-mean[i];
	  double val=idiff*jdiff;
	  cov(i,j)+=val;
	  cov(j,i)+=val; //Eigen is finicky so cov(j,i)=cov(i,j) doesn't work right!
	  if(cov(i,j)*cov(i,j)>1e50){
	    cout<<i<<","<<j<<":"<<idiff<<"*"<<jdiff<<"="<<val<<"->"<<cov(i,j)<<endl;
	}
	}
      }
    cov/=(N-1.0);
    return cov;
  };

};

void mrpt_knn() {
  int n = 10000, d = 200, k = 10;
  double target_recall = 0.9;
  Eigen::MatrixXf X = Eigen::MatrixXf::Random(d, n);
  Eigen::MatrixXf q = Eigen::VectorXf::Random(d);

  Eigen::VectorXi indices(k), indices_exact(k);

  Mrpt::exact_knn(q, X, k, indices_exact.data());
  std::cout << indices_exact.transpose() << std::endl;

  Mrpt mrpt(X);
  mrpt.grow_autotune(target_recall, k);

  mrpt.query(q, indices.data());
  std::cout << indices.transpose() << std::endl;
}

#endif


// BSD 3-Clause License

// Copyright (c) 2021, Chenyu
// All rights reserved.

// 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.

// 3. Neither the name of the copyright holder 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 HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "graph/view_graph.h"

#include <Eigen/Geometry>

#include "rotation_averaging/lagrange_dual_rotation_estimator.h"
#include "rotation_averaging/robust_l1l2_rotation_estimator.h"
#include "rotation_averaging/hybrid_rotation_estimator.h"
#include "translation_averaging/lud_position_estimator.h"
#include "util/random.h"

namespace gopt {
namespace graph {

ViewGraph::ViewGraph() {}

bool ViewGraph::ReadG2OFile(const std::string &filename) {
  // A string used to contain the contents of a single line.
  std::string line;

  // A string used to extract tokens from each line one-by-one.
  std::string token;

  // Preallocate various useful quantities.
  double tx, ty, tz, qx, qy, qz, qw;
  double I11, I12, I13, I14, I15, I16, I22, I23, I24, I25, I26,
         I33, I34, I35, I36, I44, I45, I46, I55, I56, I66;

  node_t i, j;
  ViewEdge edge;

  std::ifstream infile(filename);
  if (!infile.is_open()) {
    LOG(ERROR) << "Cannot read g2o file: " << filename;
    return false;
  }

  while (std::getline(infile, line)) {
    // Construct a stream from the string.
    std::stringstream strstrm(line);

    // Extract the first token from the string.
    strstrm >> token;

    if (token == "EDGE_SE3:QUAT") {
      // The g2o format specifies a 3D relative pose measurement in the
      // following form:
      // EDGE_SE3:QUAT id1, id2, tx, ty, tz, qx, qy, qz, qw
      // I11 I12 I13 I14 I15 I16
      //     I22 I23 I24 I25 I26
      //         I33 I34 I35 I36
      //             I44 I45 I46
      //                 I55 I56
      //                     I66
      //

      // Extract formatted output.
      strstrm >> i >> j >> tx >> ty >> tz >> qx >> qy >> qz >> qw >> I11 >>
          I12 >> I13 >> I14 >> I15 >> I16 >> I22 >> I23 >> I24 >> I25 >> I26 >>
          I33 >> I34 >> I35 >> I36 >> I44 >> I45 >> I46 >> I55 >> I56 >> I66;

      edge.src = (i > j) ? j : i;
      edge.dst = (i > j) ? i : j;

      // Fill in elements of the measurement.
      const Eigen::Quaterniond quat(qw, qx, qy, qz);
      const Eigen::AngleAxisd angle_axis =
          (i < j) ? Eigen::AngleAxisd(quat) : Eigen::AngleAxisd(quat.conjugate());

      edge.translation_2 = Eigen::Vector3d(tx, ty, tz);
      if (i > j) {
        edge.translation_2 = -angle_axis.toRotationMatrix() * edge.translation_2;
      }
      edge.rotation_2 = angle_axis.angle() * angle_axis.axis();
      AddEdge(edge);
    } else if (token == "VERTEX_SE3:QUAT") {
      // This is just initialization information, so do nothing.
      continue;
    } else {
      LOG(ERROR) << "Unrecognized type: " << token << "!" << std::endl;
      return false;
    }
  }

  infile.close();

  return true;
}

bool ViewGraph::MotionAveraging(
    const RotationEstimatorOptions& rotation_estimator_options,
    const PositionEstimatorOptions& position_estimator_options,
    std::unordered_map<image_t, Eigen::Vector3d>* global_rotations,
    std::unordered_map<image_t, Eigen::Vector3d>* positions) {
  return RotationAveraging(rotation_estimator_options, global_rotations) &&
         TranslationAveraging(position_estimator_options, positions);
}

bool ViewGraph::RotationAveraging(
    const RotationEstimatorOptions& options,
    std::unordered_map<image_t, Eigen::Vector3d>* global_rotations) {
  std::unique_ptr<RotationEstimator> rotation_estimator =
      CreateRotationEstimator(options);

  InitializeGlobalRotations(options, global_rotations);

  std::unordered_map<ImagePair, TwoViewGeometry> view_pairs;
  ViewEdgesToViewPairs(&view_pairs);

  bool success =
      rotation_estimator->EstimateRotations(view_pairs, global_rotations);

  // Assign global rotations to each node.
  if (success) {
    for (const auto& rotation_iter : *global_rotations) {
      const node_t node_id = static_cast<node_t>(rotation_iter.first);
      nodes_[node_id].rotation = rotation_iter.second;
    }
  }

  return success;
}

bool ViewGraph::TranslationAveraging(
    const PositionEstimatorOptions& options,
    std::unordered_map<image_t, Eigen::Vector3d>* positions) {
  std::unique_ptr<PositionEstimator> position_estimator =
      CreatePositionEstimator(options);

  InitializeGlobalPositions(positions);

  std::unordered_map<ImagePair, TwoViewGeometry> view_pairs;
  ViewEdgesToViewPairs(&view_pairs);

  std::unordered_map<image_t, Eigen::Vector3d> global_rotations;
  for (const auto& node_iter : nodes_) {
    const node_t node_id = node_iter.second.id;
    global_rotations[node_id] = node_iter.second.rotation;
  }

  bool success =
      position_estimator->EstimatePositions(view_pairs, global_rotations, positions);

  // Assing global positions to each node.
  if (success) {
    for (const auto& position_iter : *positions) {
      const node_t node_id = static_cast<node_t>(position_iter.first);
      nodes_[node_id].translation = position_iter.second;
    }
  }

  return true;
}

void ViewGraph::ViewEdgesToViewPairs(
    std::unordered_map<ImagePair, TwoViewGeometry>* view_pairs) {
  for (const auto& edge_iter : edges_) {
    const auto& em = edge_iter.second;
    for (const auto& em_iter : em) {
      const ViewEdge& view_edge = em_iter.second;

      const node_t src = static_cast<image_t>(view_edge.src);
      const node_t dst = static_cast<image_t>(view_edge.dst);

      const ImagePair image_pair =
          (src > dst) ? ImagePair(dst, src) : ImagePair(src, dst);

      TwoViewGeometry twoview_geometry;
      twoview_geometry.visibility_score = static_cast<int>(view_edge.weight);
      twoview_geometry.rotation_2 = view_edge.rotation_2;
      twoview_geometry.translation_2 = view_edge.translation_2;

      (*view_pairs)[image_pair] = twoview_geometry;
    }
  }
}

void ViewGraph::InitializeGlobalRotations(
    const RotationEstimatorOptions& options,
    std::unordered_map<image_t, Eigen::Vector3d>* global_rotations) {
  switch (options.init_method) {
    case GlobalRotationEstimatorInitMethod::RANDOM:
      InitializeGlobalRotationsRandomly(global_rotations);
      break;
    case GlobalRotationEstimatorInitMethod::MAXIMUM_SPANNING_TREE:
      InitializeGlobalRotationsFromMST(global_rotations);
      break;
    default:
      LOG(FATAL) << "Initialize method invalid!";
      break;
  }
}

void ViewGraph::InitializeGlobalRotationsRandomly(
    std::unordered_map<image_t, Eigen::Vector3d>* global_rotations) {
  RandomNumberGenerator rng;
  for (size_t i = 0; i < size_; i++) {
    // (*global_rotations)[i] = rng.RandVector3d();
    (*global_rotations)[i] = Eigen::Vector3d::Zero();
  }
}

void ViewGraph::InitializeGlobalRotationsFromMST(
    std::unordered_map<image_t, Eigen::Vector3d>* global_rotations) {
  // TODO(chenyu): implementation here.
}

void ViewGraph::InitializeGlobalPositions(
    std::unordered_map<image_t, Eigen::Vector3d>* positions) {
  for (size_t i = 0; i < size_; i++) {
    (*positions)[i] = Eigen::Vector3d::Zero();
  }
}

std::unique_ptr<RotationEstimator> ViewGraph::CreateRotationEstimator(
    const RotationEstimatorOptions& options) {
  std::unique_ptr<RotationEstimator> rotation_estimator = nullptr;
  switch (options.estimator_type) {
    case GlobalRotationEstimatorType::LAGRANGIAN_DUAL: {
      rotation_estimator.reset(new LagrangeDualRotationEstimator(
          size_, 3, options.sdp_solver_options));
      break;
    }
    case GlobalRotationEstimatorType::HYBRID: {
      HybridRotationEstimator::HybridRotationEstimatorOptions hybrid_options;
      hybrid_options.sdp_solver_options = options.sdp_solver_options;
      hybrid_options.irls_options = options.irls_options;
      rotation_estimator.reset(
          new HybridRotationEstimator(size_, 3, hybrid_options));
      break;
    }
    case GlobalRotationEstimatorType::ROBUST_L1L2: {
      RobustL1L2RotationEstimator::RobustL1L2RotationEstimatorOptions
          robust_l1l2_options;
      robust_l1l2_options.l1_options = options.l1_options;
      robust_l1l2_options.irls_options = options.irls_options;
      rotation_estimator.reset(
          new RobustL1L2RotationEstimator(robust_l1l2_options));
      break;
    }
    default:
      break;
  }

  return rotation_estimator;
}

std::unique_ptr<PositionEstimator> ViewGraph::CreatePositionEstimator(
    const PositionEstimatorOptions& options) {
  std::unique_ptr<PositionEstimator> position_estimator = nullptr;
  switch (options.estimator_type) {
    case PositionEstimatorType::LUD: {
      LUDPositionEstimator::Options lud_options;
      lud_options.max_num_iterations = options.max_num_iterations;
      lud_options.max_num_reweighted_iterations = options.max_num_reweighted_iterations;
      lud_options.convergence_criterion = options.convergence_criterion;
      position_estimator.reset(new LUDPositionEstimator(lud_options));
      break;
    }
    case PositionEstimatorType::BATA: {
      // TODO(chenyu): implement BATA position estimator.
      break;
    }
    default:
      break;
  }
  return position_estimator;
}

}  // namespace graph
}  // namespace gopt


#include "teca_cartesian_mesh_regrid.h"

#include "teca_cartesian_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_coordinate_util.h"

#include <algorithm>
#include <iostream>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::cerr;
using std::endl;

//#define TECA_DEBUG

template<typename NT1, typename NT2, typename NT3, class interp_t>
int interpolate(unsigned long target_nx, unsigned long target_ny,
    unsigned long target_nz, const NT1 *p_target_xc, const NT1 *p_target_yc,
    const NT1 *p_target_zc, NT3 *p_target_a, const NT2 *p_source_xc,
    const NT2 *p_source_yc, const NT2 *p_source_zc, const NT3 *p_source_a,
    unsigned long source_ihi, unsigned long source_jhi, unsigned long source_khi,
    unsigned long source_nx, unsigned long source_nxy)
{
    interp_t f;
    unsigned long q = 0;
    for (unsigned long k = 0; k < target_nz; ++k)
    {
        NT2 tz = static_cast<NT2>(p_target_zc[k]);
        for (unsigned long j = 0; j < target_ny; ++j)
        {
            NT2 ty = static_cast<NT2>(p_target_yc[j]);
            for (unsigned long i = 0; i < target_nx; ++i, ++q)
            {
                NT2 tx = static_cast<NT2>(p_target_xc[i]);
                if (f(tx,ty,tz,
                    p_source_xc, p_source_yc, p_source_zc,
                    p_source_a, source_ihi, source_jhi, source_khi,
                    source_nx, source_nxy,
                    p_target_a[q]))
                {
                    TECA_ERROR("failed to interpolate i=(" << i << ", " << j << ", " << k
                        << ") x=(" << tx << ", " << ty << ", " << tz << ")")
                    return -1;
                }
            }
        }
    }
    return 0;
}

template<typename taget_coord_t, typename source_coord_t, typename array_t>
int interpolate(int mode, unsigned long target_nx, unsigned long target_ny,
    unsigned long target_nz, const taget_coord_t *p_target_xc, const taget_coord_t *p_target_yc,
    const taget_coord_t *p_target_zc, array_t *p_target_a, const source_coord_t *p_source_xc,
    const source_coord_t *p_source_yc, const source_coord_t *p_source_zc, const array_t *p_source_a,
    unsigned long source_ihi, unsigned long source_jhi, unsigned long source_khi,
    unsigned long source_nx, unsigned long source_nxy)
{
    using nearest_interp_t = teca_coordinate_util::interpolate_t<0>;
    using linear_interp_t = teca_coordinate_util::interpolate_t<1>;

    switch (mode)
    {
        case teca_cartesian_mesh_regrid::nearest:
            return interpolate<taget_coord_t,source_coord_t,array_t,nearest_interp_t>(
                target_nx, target_ny, target_nz, p_target_xc, p_target_yc, p_target_zc,
                p_target_a, p_source_xc, p_source_yc, p_source_zc, p_source_a,
                source_ihi, source_jhi, source_khi, source_nx, source_nxy);
            break;
        case teca_cartesian_mesh_regrid::linear:
            return interpolate<taget_coord_t,source_coord_t,array_t,linear_interp_t>(
                target_nx, target_ny, target_nz, p_target_xc, p_target_yc, p_target_zc,
                p_target_a, p_source_xc, p_source_yc, p_source_zc, p_source_a,
                source_ihi, source_jhi, source_khi, source_nx, source_nxy);
            break;
    }

    TECA_ERROR("invalid interpolation mode \"" << mode << "\"")
    return -1;
}

// --------------------------------------------------------------------------
teca_cartesian_mesh_regrid::teca_cartesian_mesh_regrid()
    : target_input(0), interpolation_mode(nearest)
{
    this->set_number_of_input_connections(2);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_cartesian_mesh_regrid::~teca_cartesian_mesh_regrid()
{}

#if defined(TECA_HAS_BOOST)

// --------------------------------------------------------------------------
void teca_cartesian_mesh_regrid::get_properties_description(
    const std::string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_cartesian_mesh_regrid":prefix));

    opts.add_options()
        TECA_POPTS_GET(int, prefix, target_input,
            "select input connection that contains metadata (0)")
        TECA_POPTS_GET(std::vector<std::string>, prefix, arrays,
            "list of arrays to move from source to target mesh ("")")
        TECA_POPTS_GET(int, prefix, interpolation_mode,
            "linear or nearest interpolation (1)")
        ;

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_cartesian_mesh_regrid::set_properties(
    const std::string &prefix, variables_map &opts)
{
    TECA_POPTS_SET(opts, int, prefix, target_input)
    TECA_POPTS_SET(opts, std::vector<std::string>, prefix, arrays)
    TECA_POPTS_SET(opts, int, prefix, interpolation_mode)
}

#endif

// --------------------------------------------------------------------------
teca_metadata teca_cartesian_mesh_regrid::get_output_metadata(
    unsigned int port, const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_cf_reader::get_output_metadata" << endl;
#endif
    (void)port;

    int md_src = this->target_input ? 0 : 1;
    int md_tgt = this->target_input ? 1 : 0;

    // start with a copy of metadata from the target
    teca_metadata output_md(input_md[md_tgt]);

    // get target metadata
    std::vector<std::string> target_vars;
    input_md[md_tgt].get("variables", target_vars);

    std::vector<std::string> target_time_vars;
    input_md[md_tgt].get("time variables", target_time_vars);

    teca_metadata target_atts;
    input_md[md_tgt].get("attributes", target_atts);

    // get source metadata
    std::vector<std::string> source_vars;
    input_md[md_src].get("variables", source_vars);

    std::vector<std::string> source_time_vars;
    input_md[md_src].get("time variables", source_time_vars);

    teca_metadata source_atts;
    input_md[md_src].get("attributes", source_atts);

    // merge metadata from source and target
    // variables and time variables should be unique lists.
    // attributes are indexed by variable names
    // in the case of collisions, the target variable
    // is kept, the source variable is ignored
    size_t n_source_vars = source_vars.size();
    for (size_t i = 0; i < n_source_vars; ++i)
    {
        const std::string &source = source_vars[i];

        // check that there's not a variable of that same name in target
        std::vector<std::string>::iterator first = target_vars.begin();
        std::vector<std::string>::iterator last = target_vars.end();
        if (find(first, last, source) == last)
        {
            // not present in target, ok to add it
            target_vars.push_back(source);

            teca_metadata atts;
            source_atts.get(source, atts);
            target_atts.set(source, atts);

            // check if it's a time var as well
            first = source_time_vars.begin();
            last = source_time_vars.end();
            if (find(first, last, source) != last)
                target_time_vars.push_back(source);
        }

    }

    // update with merged lists
    output_md.set("variables", target_vars);
    output_md.set("time variables", target_time_vars);
    output_md.set("attributes", target_atts);

    return output_md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_cartesian_mesh_regrid::get_upstream_request(
    unsigned int port, const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
    (void)port;
    std::vector<teca_metadata> up_reqs(2);

    // route requests for arrays to either target or the source.
    // if the array exists in both then it is take from the target
    teca_metadata target_req(request);
    teca_metadata source_req(request);

    // get target metadata
    int md_tgt = this->target_input ? 1 : 0;
    std::vector<std::string> target_vars;
    input_md[md_tgt].get("variables", target_vars);
    std::vector<std::string>::iterator tgt_0 = target_vars.begin();
    std::vector<std::string>::iterator tgt_1 = target_vars.end();

    // get source metadata
    int md_src = this->target_input ? 0 : 1;
    std::vector<std::string> source_vars;
    input_md[md_src].get("variables", source_vars);
    std::vector<std::string>::iterator src_0 = source_vars.begin();
    std::vector<std::string>::iterator src_1 = source_vars.end();

    // get the requested arrays
    std::vector<std::string> req_arrays;
    request.get("arrays", req_arrays);

    // add any explicitly named
    std::copy(this->arrays.begin(), this->arrays.end(),
        std::back_inserter(req_arrays));

    // route. if found in the target then  routed there, else
    // if found in the source routed there, else skipped
    std::vector<std::string> target_arrays;
    std::vector<std::string> source_arrays;

    std::vector<std::string>::iterator it = req_arrays.begin();
    std::vector<std::string>::iterator end = req_arrays.end();
    for (; it != end; ++it)
    {
        if (find(tgt_0, tgt_1, *it) != tgt_1)
            target_arrays.push_back(*it);
        else if (find(src_0, src_1, *it) != src_1)
            source_arrays.push_back(*it);
        else
            TECA_ERROR("\"" << *it << "\" was not in target nor source")
    }

    target_req.set("arrays", target_arrays);
    source_req.set("arrays", source_arrays);

    // get the target extent and coordinates
    teca_metadata target_coords;
    p_teca_variant_array target_x, target_y, target_z;

    if (input_md[md_tgt].get("coordinates", target_coords)
        || !(target_x = target_coords.get("x"))
        || !(target_y = target_coords.get("y"))
        || !(target_z = target_coords.get("z")))
    {
        TECA_ERROR("failed to locate target mesh coordinates")
        return up_reqs;
    }

    // get the actual bounds of what we will be served with. this will be a
    // region covering the requested bounds. we need to insure that source data
    // covers this region, not just the requested bounds.
    double request_bounds[6] = {0.0};
    unsigned long target_extent[6] = {0l};
    if (request.get("bounds", request_bounds, 6))
    {
        if (request.get("extent", target_extent, 6))
        {
            TECA_ERROR("neither \"bounds\" nor \"extent\" has been requested")
            return up_reqs;
        }
    }
    else
    {
        if (teca_coordinate_util::bounds_to_extent(request_bounds,
            target_x, target_y, target_z, target_extent))
        {
            TECA_ERROR("invalid bounds requested [" << request_bounds[0]  << ", "
                << request_bounds[1] << ", " << request_bounds[2] << ", "
                << request_bounds[3] << ", " << request_bounds[4] << ", "
                << request_bounds[5] << "]")
            return up_reqs;
        }
    }

    double target_bounds[6] = {0.0};
    target_x->get(target_extent[0], target_bounds[0]);
    target_x->get(target_extent[1], target_bounds[1]);
    target_y->get(target_extent[2], target_bounds[2]);
    target_y->get(target_extent[3], target_bounds[3]);
    target_z->get(target_extent[4], target_bounds[4]);
    target_z->get(target_extent[5], target_bounds[5]);

    // send the target bounds to the source as well
    source_req.set("bounds", target_bounds, 6);

    // send the requests up
    up_reqs[md_tgt] = target_req;
    up_reqs[md_src] = source_req;

    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_cartesian_mesh_regrid::execute(
    unsigned int port, const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_cartesian_mesh_regrid::execute" << endl;
#endif
    (void)port;

    int md_src = this->target_input ? 0 : 1;
    int md_tgt = this->target_input ? 1 : 0;

    p_teca_cartesian_mesh in_target
        = std::dynamic_pointer_cast<teca_cartesian_mesh>(
            std::const_pointer_cast<teca_dataset>(input_data[md_tgt]));

    const_p_teca_cartesian_mesh source
        = std::dynamic_pointer_cast<const teca_cartesian_mesh>(
            input_data[md_src]);

    if (!in_target || !source)
    {
        TECA_ERROR("invalid input. target invalid "
            << !in_target << " source invalid " << !source )
        return nullptr;
    }

    // create the output
    p_teca_cartesian_mesh target = teca_cartesian_mesh::New();
    target->shallow_copy(in_target);

    // get the source and targent extents
    std::vector<unsigned long> source_ext;
    source->get_extent(source_ext);

    std::vector<unsigned long> target_ext;
    target->get_extent(target_ext);

    // get the list of arrays to move
    std::vector<std::string> req_arrays;
    request.get("regrid_arrays", req_arrays);

    // add any explicitly named
    std::copy(this->arrays.begin(), this->arrays.end(),
        std::back_inserter(req_arrays));

    // route. if found in the target then  routed there, else
    // if found in the source routed there, else skipped
    std::vector<std::string> source_arrays;

    std::vector<std::string>::iterator it = req_arrays.begin();
    std::vector<std::string>::iterator end = req_arrays.end();
    for (; it != end; ++it)
    {
        if (!target->get_point_arrays()->has(*it))
            source_arrays.push_back(*it);
    }

    // move the arrays
    const_p_teca_variant_array target_xc = target->get_x_coordinates();
    const_p_teca_variant_array target_yc = target->get_y_coordinates();
    const_p_teca_variant_array target_zc = target->get_z_coordinates();
    p_teca_array_collection target_ac = target->get_point_arrays();

    unsigned long target_nx = target_xc->size();
    unsigned long target_ny = target_yc->size();
    unsigned long target_nz = target_zc->size();
    unsigned long target_size = target_nx*target_ny*target_nz;

    const_p_teca_variant_array source_xc = source->get_x_coordinates();
    const_p_teca_variant_array source_yc = source->get_y_coordinates();
    const_p_teca_variant_array source_zc = source->get_z_coordinates();
    const_p_teca_array_collection source_ac = source->get_point_arrays();

    unsigned long source_nx = source_xc->size();
    unsigned long source_ny = source_yc->size();
    unsigned long source_nz = source_zc->size();
    unsigned long source_nxy = source_nx*source_ny;
    unsigned long source_ihi = source_nx - 1;
    unsigned long source_jhi = source_ny - 1;
    unsigned long source_khi = source_nz - 1;

    NESTED_TEMPLATE_DISPATCH_FP(
        const teca_variant_array_impl,
        target_xc.get(),
        1,

        const NT1 *p_target_xc = std::dynamic_pointer_cast<TT1>(target_xc)->get();
        const NT1 *p_target_yc = std::dynamic_pointer_cast<TT1>(target_yc)->get();
        const NT1 *p_target_zc = std::dynamic_pointer_cast<TT1>(target_zc)->get();

        NESTED_TEMPLATE_DISPATCH_FP(
            const teca_variant_array_impl,
            source_xc.get(),
            2,

            const NT2 *p_source_xc = std::dynamic_pointer_cast<TT2>(source_xc)->get();
            const NT2 *p_source_yc = std::dynamic_pointer_cast<TT2>(source_yc)->get();
            const NT2 *p_source_zc = std::dynamic_pointer_cast<TT2>(source_zc)->get();

            size_t n_arrays = source_arrays.size();
            for (size_t i = 0; i < n_arrays; ++i)
            {
                const_p_teca_variant_array source_a = source_ac->get(source_arrays[i]);
                p_teca_variant_array target_a = source_a->new_instance();
                target_a->resize(target_size);

                NESTED_TEMPLATE_DISPATCH(
                    teca_variant_array_impl,
                    target_a.get(),
                    3,

                    const NT3 *p_source_a = std::static_pointer_cast<const TT3>(source_a)->get();
                    NT3 *p_target_a = std::static_pointer_cast<TT3>(target_a)->get();

                    if (interpolate(this->interpolation_mode, target_nx, target_ny, target_nz,
                        p_target_xc, p_target_yc, p_target_zc, p_target_a, p_source_xc,
                        p_source_yc, p_source_zc, p_source_a, source_ihi, source_jhi,
                        source_khi, source_nx, source_nxy))
                    {
                        TECA_ERROR("Failed to move \"" << source_arrays[i] << "\"")
                        return nullptr;
                    }

                    target_ac->set(source_arrays[i], target_a);
                    )
                }
                )
            )

    return target;
}


/*
 * Copyright (c) 2010, Hozefa Indorewala <indorewala@ias.in.tum.de>
 * 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 Willow Garage, Inc. 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 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.
 */

/*
 * Author: Hozefa Indorewala
 * This node subscribes to a PointCloud2 topic, receives point clouds and then registers them and publishes
 * the result on /merged_pointcloud (PointCloud2)
 */

#include <pcl/point_types.h>
#include <pcl/features/feature.h>
#include <pcl/ros/conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pointcloud_registration/pointcloud_registration_point_types.h>
#include <pcl/io/pcd_io.h>
#include <Eigen/SVD>
#include <pcl_conversions/pcl_conversions.h>

#include "pcl/filters/statistical_outlier_removal.h" // to filter outliers

#include "pcl/filters/voxel_grid.h" //for downsampling the point cloud

#include "pcl/kdtree/kdtree_flann.h" //for the kdtree

#include "pcl/registration/transforms.h" //for the transformation function

#include <pcl/features/normal_3d_omp.h>

#include <pointcloud_registration/icp/icp_correspondences_check.h> //for icp
#include <algorithm> //for the sort and unique functions

#include <ctime>

const float PI = 3.14159265;

typedef pcl::PointNormal  PointT;

bool pclSort (pcl::PointNormal i, pcl::PointNormal j)
{
  return (i.x < j.x);
}

bool pclUnique (pcl::PointNormal i, pcl::PointNormal j)
{
  double x_diff = fabs(i.x - j.x);
  double y_diff = fabs(i.y - j.y);
  double z_diff = fabs(i.z - j.z);
  if(x_diff < 0.0001 && y_diff < 0.0001 && z_diff < 0.0001 )
    return 1;
  else
    return 0;
}

class PointCloudRegistration
{
  public:
    PointCloudRegistration();
    ~PointCloudRegistration();
    void pointcloudRegistrationCallBack(const sensor_msgs::PointCloud2& msg);
    Eigen::Matrix4f getOverlapTransformation();
    void publishPointCloud(pcl::PointCloud<pcl::PointNormal> &pointcloud);
    pcl::PointCloud<pcl::PointNormal> convertFromMsgToPointCloud(const sensor_msgs::PointCloud2& pointcloud_msg);
    void spin (int argc, char** argv);

  private:
    ros::NodeHandle nh_;
  std::string merged_pointcloud_topic_, subscribe_pointcloud_topic_, frame_id_, field_;
    int max_number_of_iterations_icp_, max_nn_icp_, max_nn_overlap_;
    double downsample_leafsize_, epsilon_z_, epsilon_curvature_, epsilon_transformation_, radius_icp_, radius_overlap_;
    bool downsample_pointcloud_before_, downsample_pointcloud_after_, filter_outliers_, curvature_check_;
    int scan_index_, counter_;
    time_t start, end;
    Eigen::Matrix4f final_transformation_;
    ros::Subscriber pointcloud_subscriber_;
    ros::Publisher pointcloud_merged_publisher_;

    pcl::IterativeClosestPointCorrespondencesCheck<pcl::PointNormal, pcl::PointNormal> icp_; // for icp
    pcl::KdTreeFLANN<pcl::PointNormal> kdtree_;  // for kdtree
    bool firstCloudReceived_, secondCloudReceived_;
    pcl::PointCloud<pcl::PointNormal> pointcloud2_current_, pointcloud2_merged_, pointcloud2_transformed_;
};

Eigen::Matrix4f PointCloudRegistration::getOverlapTransformation()
{
  // In this function we extract the overlapped region of the two points cloud and compute
  // the transformation and return it.

  if(firstCloudReceived_ == false && secondCloudReceived_ == false )
  {
    ROS_ERROR("[PointCloudRegistration:] getOverlapTransformation called before receiving atleast 2 point clouds");
    exit(1);
  }
  else
  {
    //searching for overlapped points in the point cloud
    // Allocate enough space to hold the results
    std::vector<int> nn_indices (max_nn_overlap_);
    std::vector<float> nn_dists (max_nn_overlap_);

    pcl::PointCloud<pcl::PointNormal> overlap_model, overlap_current;
    Eigen::Matrix4f transformation;
    ROS_INFO("[PointCloudRegistration:] finding overlapping points");
    start = time(NULL);
    std::vector<pcl:: PointNormal, Eigen::aligned_allocator<pcl:: PointNormal> >::iterator it;
    for(size_t idx = 0 ; idx < pointcloud2_current_.points.size(); idx++ )
    {
      kdtree_.radiusSearch(pointcloud2_current_, idx, radius_overlap_, nn_indices, nn_dists, max_nn_overlap_);

      if(nn_indices.size() > 0 )
      {
        overlap_current.points.push_back(pointcloud2_current_.points[idx]);
              for(size_t i = 0 ; i < nn_indices.size(); i++)
        {
          overlap_model.points.push_back (kdtree_.getInputCloud()->points[nn_indices[i]]);
        }
      }
    }
    end = time(NULL);
    ROS_INFO("[PointCloudRegistration:] found overlapping points in %d seconds with points %ld",
             (int)(end - start), overlap_current.points.size());

    //Getting rid of duplicate points in model
    ROS_INFO("[PointCloudRegistration:] removing duplicate points");
    start = time(NULL);
    std::sort(overlap_model.points.begin(), overlap_model.points.end(), pclSort);
    it = std::unique(overlap_model.points.begin(), overlap_model.points.end(), pclUnique);
    overlap_model.points.resize(it - overlap_model.points.begin());
    end = time(NULL);
    ROS_INFO("[PointCloudRegistration:] removed duplicate points %d seconds", (int)(end - start));

    icp_.setInputTarget(boost::make_shared< pcl::PointCloud < pcl::PointNormal> > (overlap_model));
    icp_.setInputCloud(boost::make_shared< pcl::PointCloud < pcl::PointNormal> > (overlap_current));

    ROS_INFO("[PointCloudRegistration: ] aligning");
    icp_.align(pointcloud2_transformed_);

    ROS_INFO("[PointCloudRegistration:] getting final transformation");
    transformation = icp_.getFinalTransformation();
    return (transformation);
  }
}

void PointCloudRegistration::publishPointCloud(pcl::PointCloud<pcl::PointNormal> &pointcloud)
{
  pcl::PCLPointCloud2 pointcloud2;

  if( downsample_pointcloud_after_ == true)
  {
    // for downsampling of pointclouds
    pcl::VoxelGrid<pcl::PointNormal> sor_;
    pcl::PointCloud<pcl::PointNormal>::Ptr mycloud_downsampled(new pcl::PointCloud<pcl::PointNormal>);
    // pcl::PointCloud<pcl::PointNormal> mycloud_downsampled;

    //Now we will downsample the point cloud
    sor_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointNormal>> (pointcloud));
    sor_.setLeafSize (downsample_leafsize_, downsample_leafsize_, downsample_leafsize_);
    sor_.filter (*mycloud_downsampled);

    pcl::toPCLPointCloud2(*mycloud_downsampled, pointcloud2);
  }
  else
  {
    pcl::toPCLPointCloud2(pointcloud, pointcloud2);
  }
  sensor_msgs::PointCloud2 my_cloud;
  pcl_conversions::fromPCL(pointcloud2, my_cloud);

  my_cloud.header.frame_id = frame_id_;
  my_cloud.header.stamp = ros::Time::now();

  pointcloud_merged_publisher_.publish(my_cloud);

  ROS_INFO("[PointCloudRegistration:] Merged Point cloud published");
}

pcl::PointCloud<pcl::PointNormal> PointCloudRegistration::convertFromMsgToPointCloud(const sensor_msgs::PointCloud2& pointcloud_msg)
{
  pcl::PointCloud<pcl::PointNormal> pointcloud_pcl_normals;
  pcl::fromROSMsg(pointcloud_msg, pointcloud_pcl_normals);
  return (pointcloud_pcl_normals);
  //TODO: make this work again with Hokuyo scans
  // //incrementing the scan index
  // scan_index_++;
  // // Declaring some variables required in this function
  // sensor_msgs::PointCloud2 pointcloud2_msg = pointcloud_msg;
  // pcl::PointCloud<pcl::PointXYZ> pointcloud_pcl_step01, pointcloud_pcl_step02;
  // pcl::PointCloud<pcl::PointNormal> pointcloud_pcl_normals;
  // pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
  // pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree_ptr_;
  // tree_ptr_ = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
  // std::vector<int> indices;

  // // Converting from PointCloud msg format to PointCloud2 msg format

  // //sensor_msgs::convertPointCloudToPointCloud2(pointcloud_msg, pointcloud2_msg);

  // //This is done because there is a bug in PCL.
  // // for(u_int i = 0 ; i < pointcloud2_msg.fields.size(); i++)
  // // {
  // //   if(pointcloud2_msg.fields[i].name == "intensities")
  // //   {
  // //     pointcloud2_msg.fields[i].name = "intensity";
  // //   }
  // // }

  // // STEP 01: Check if we should downsample the input cloud or not
  // if(downsample_pointcloud_before_ == true)
  // {
  //   ROS_INFO ("PointCloud before downsampling: %d .", pointcloud2_msg.width * pointcloud2_msg.height);
  //   sensor_msgs::PointCloud2 pointcloud_downsampled_msg;
  //   pcl::VoxelGrid<sensor_msgs::PointCloud2> sor_; // for downsampling of pointclouds

  //   //Now we will downsample the point cloud
  //   sor_.setInputCloud (boost::make_shared<sensor_msgs::PointCloud2> (pointcloud2_msg));
  //   sor_.setLeafSize (downsample_leafsize_, downsample_leafsize_, downsample_leafsize_);
  //   sor_.filter (pointcloud_downsampled_msg);
  //   ROS_INFO ("PointCloud after downsampling: %d .", pointcloud_downsampled_msg.width * pointcloud_downsampled_msg.height);

  //   // Converting from PointCloud2 msg format to pcl pointcloud format
  //   pcl::fromROSMsg(pointcloud_downsampled_msg, pointcloud_pcl_step01);
  // }
  // else
  // {
  //   // Converting from PointCloud2 msg format to pcl pointcloud format
  //   pcl::fromROSMsg(pointcloud2_msg, pointcloud_pcl_step01);
  // }

  // // STEP 02: Check if we should filter the outliers or not

  // if(filter_outliers_)
  // {
  //   // Removing outliers
  //   pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  //   sor.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(pointcloud_pcl_step01));
  //   sor.setMeanK (50);
  //   sor.setStddevMulThresh (1.0);
  //   sor.filter (pointcloud_pcl_step02);
  // }
  // else
  // {
  //   pointcloud_pcl_step02 = pointcloud_pcl_step01;
  // }
  // tree_ptr_->setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (pointcloud_pcl_step02));
  // indices.resize (pointcloud_pcl_step02.points.size ());
  // for (size_t i = 0; i < indices.size (); ++i)
  // {
  //   indices[i] = i;
  // }

  // return (pointcloud_pcl_normals);
  // // STEP 03: Here we perform Normal Estimation on the input cloud

  // // Object
  // pcl::PointCloud<pcl::Normal> normals;
  // // set parameters

  // n.setInputCloud (boost::make_shared <const pcl::PointCloud<pcl::PointXYZ> > (pointcloud_pcl_step02));
  // n.setIndices (boost::make_shared <std::vector<int> > (indices));
  // n.setSearchMethod (tree_ptr_);
  // n.setKSearch (10);

  // // estimate
  // n.compute (normals);

  // // STEP 04: Here we copy data from the normals and the input cloud into the pcl::PointNormal cloud

  // pointcloud_pcl_normals.points.resize(pointcloud_pcl_step02.points.size());

  // for(u_int i = 0 ; i < pointcloud_pcl_step02.points.size(); i++)
  // {
  //   pointcloud_pcl_normals.points[i].x = pointcloud_pcl_step02.points[i].x;
  //   pointcloud_pcl_normals.points[i].y = pointcloud_pcl_step02.points[i].y;
  //   pointcloud_pcl_normals.points[i].z = pointcloud_pcl_step02.points[i].z;
  //   //pointcloud_pcl_normals.points[i].rgb = pointcloud_pcl_step02.points[i].rgb;
  //   //    pointcloud_pcl_normals.points[i].intensity = pointcloud_pcl_step02.points[i].intensity;
  //   pointcloud_pcl_normals.points[i].normal[0] = normals.points[i].normal[0];
  //   pointcloud_pcl_normals.points[i].normal[1] = normals.points[i].normal[1];
  //   pointcloud_pcl_normals.points[i].normal[2] = normals.points[i].normal[2];
  //   pointcloud_pcl_normals.points[i].curvature = normals.points[i].curvature;
  //   //pointcloud_pcl_normals.points[i].scan_index = scan_index_;
  // }

  // pointcloud_pcl_normals.header.frame_id = pointcloud_pcl_normals.header.frame_id;
  // pointcloud_pcl_normals.header.stamp = pointcloud_pcl_normals.header.stamp;
  // pointcloud_pcl_normals.width    = pointcloud_pcl_normals.points.size ();
  // pointcloud_pcl_normals.height   = 1;
  // pointcloud_pcl_normals.is_dense = false;

  // return (pointcloud_pcl_normals);
}

PointCloudRegistration::PointCloudRegistration(): nh_("~")
{
  nh_.param("publish_merged_pointcloud_topic", merged_pointcloud_topic_, std::string("/merged_pointcloud"));
  nh_.param("subscribe_pointcloud_topic", subscribe_pointcloud_topic_, std::string("/shoulder_cloud"));
  nh_.param("max_number_of_iterations_icp", max_number_of_iterations_icp_, 100);
  nh_.param("max_nn_icp", max_nn_icp_, 100);
  nh_.param("max_nn_overlap", max_nn_overlap_, 10);
  nh_.param("radius_icp", radius_icp_, 0.005);
  nh_.param("radius_overlap", radius_overlap_, 0.005);
  nh_.param("curvature_check", curvature_check_, true);
  nh_.param("downsample_pointcloud_before", downsample_pointcloud_before_, false);
  nh_.param("downsample_pointcloud_after", downsample_pointcloud_after_, false);
  nh_.param("filter_outliers", filter_outliers_, true);
  nh_.param("downsample_leafsize", downsample_leafsize_, 0.05);
  nh_.param("epsilon_z", epsilon_z_, 0.001);
  nh_.param("epsilon_curvature", epsilon_curvature_, 0.001);
  nh_.param("epsilon_transformation", epsilon_transformation_, 1e-8);
  nh_.param("field", field_, std::string("x"));
  firstCloudReceived_ = false;
  secondCloudReceived_ = false;
  scan_index_ = 0;
  counter_ = 0;
  icp_.setMaximumIterations(max_number_of_iterations_icp_);
  icp_.setTransformationEpsilon(epsilon_transformation_);
  icp_.setParameters(radius_icp_, max_nn_icp_, epsilon_z_, epsilon_curvature_, curvature_check_, field_);
  ROS_INFO("[PointCloudRegistration:] pointcloud_registration node is up and running.");
  pointcloud_subscriber_ = nh_.subscribe(subscribe_pointcloud_topic_, 100, &PointCloudRegistration::pointcloudRegistrationCallBack, this);
  pointcloud_merged_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>(merged_pointcloud_topic_, 100);
}

PointCloudRegistration::~PointCloudRegistration()
{
  ROS_INFO("[PointCloudRegistration:] Shutting down pointcloud_registration node!.");
}

void PointCloudRegistration::pointcloudRegistrationCallBack(const sensor_msgs::PointCloud2& pointcloud_msg)
{
  counter_++;
  frame_id_ = pointcloud_msg.header.frame_id;
  start = time(NULL);
  if( firstCloudReceived_ == false)
  {
    pointcloud2_current_ = convertFromMsgToPointCloud(pointcloud_msg);
    ROS_INFO("[PointCloudRegistration:] Size of point cloud received = %d", (int) pointcloud2_current_.points.size());
    firstCloudReceived_ = true;
    ROS_INFO("[PointCloudRegistration:] Received first point cloud with points %ld:", pointcloud2_current_.points.size());
    kdtree_.setInputCloud(boost::make_shared< pcl::PointCloud < pcl::PointNormal> > (pointcloud2_current_));

    pointcloud2_merged_ = pointcloud2_current_;
  }
  else if( secondCloudReceived_ == false)
  {
    secondCloudReceived_ = true;
    pointcloud2_current_ = convertFromMsgToPointCloud(pointcloud_msg);
    ROS_INFO("[PointCloudRegistration:] Received second point cloud with points %ld.", pointcloud2_current_.points.size());

    //Now we get the transformation from the overlapped regions of the 2 point clouds
    final_transformation_= getOverlapTransformation();
    pcl::transformPointCloud(pointcloud2_current_, pointcloud2_transformed_, final_transformation_);
    pointcloud2_merged_ += pointcloud2_transformed_;

  }
  else
  {
    pointcloud2_current_ = convertFromMsgToPointCloud(pointcloud_msg);
    ROS_INFO("[PointCloudRegistration:] Received point cloud number: %d with points %ld.", counter_, pointcloud2_current_.points.size());
    kdtree_.setInputCloud(boost::make_shared< pcl::PointCloud < pcl::PointNormal> > (pointcloud2_merged_));

    //Now we get the transformation from the overlapped regions of the 2 point clouds
    final_transformation_= getOverlapTransformation();
    pcl::transformPointCloud(pointcloud2_current_, pointcloud2_transformed_, final_transformation_);

    pointcloud2_merged_ += pointcloud2_transformed_;
  }

  publishPointCloud(pointcloud2_merged_);
  end = time(NULL);
  ROS_INFO("[PointCloudRegistration:] Time taken: %d seconds", (int)(end - start));

}

   void PointCloudRegistration::spin (int argc, char** argv)
    {
      for (int i = 1; i < argc; i++)
      {
        ROS_INFO("[PointCloudRegistration:] New Cloud ===========================");
        ROS_INFO("[PointCloudRegistration:] Reading pcd %s.", argv[i]);
        pcl::PCDReader reader;
        pcl::PointCloud<PointT> input_cloud;
        sensor_msgs::PointCloud2 input_cloud_msg;
        reader.read (argv[i], input_cloud);
        if (input_cloud.points.size() == 0)
        {
          ROS_ERROR("[PointCloudRegistration:] input_cloud.points.size(): %ld", input_cloud.points.size());
          continue;
        }
        pcl::toROSMsg(input_cloud, input_cloud_msg);
        pointcloudRegistrationCallBack (input_cloud_msg);
        ros::spinOnce();
      }
      ROS_INFO("[PointCloudRegistration:] Done and Exiting!");
      ros::shutdown();
    }

int main(int argc, char** argv)
{
    ros::init(argc, argv, "pointcloud_registration");
    PointCloudRegistration pointcloud_registration;

    if (argc == 1)
    {
      ros::spin();
    }
    else if (argc >= 2  && std::string(argv[1]) != "-h")
    {
      pointcloud_registration.spin(argc, argv);
    }
    else
    {
      std::cerr << "Usage: " << std::endl
                << argv[0] << " - get the pointcloud from the topic" << std::endl
                << argv[0] << " <input_cloud.pcd> - get the pointcloud on cmd" << std::endl;
    }
    return(0);
}