/**
 * \ file RIAAFilter.cpp
 */

#include <ATK/EQ/RIAAFilter.h>
#include <ATK/EQ/IIRFilter.h>

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

const int PROCESSSIZE = 1024*64;
const int SAMPLINGRATE = 96000;

BOOST_AUTO_TEST_CASE( IIRFilter_RIAACoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::RIAACoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 2.01060845));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(PROCESSSIZE);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_RIAACoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::RIAACoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 1.89328793));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(PROCESSSIZE);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_RIAACoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1420);

  ATK::IIRFilter<ATK::RIAACoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1420, 1.00011655));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(PROCESSSIZE);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_RIAACoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::RIAACoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.87855133));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(PROCESSSIZE);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_RIAACoefficients_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::RIAACoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.59615476));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(PROCESSSIZE);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_InverseRIAACoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::InverseRIAACoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 1/2.01060845));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(PROCESSSIZE);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_InverseRIAACoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::InverseRIAACoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 1/1.89328793));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(PROCESSSIZE);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_InverseRIAACoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1420);

  ATK::IIRFilter<ATK::InverseRIAACoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1420, 1/1.00011655));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(PROCESSSIZE);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_InverseRIAACoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::InverseRIAACoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 1/0.87855133));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(PROCESSSIZE);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_InverseRIAACoefficients_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::InverseRIAACoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 1/0.59615476));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(PROCESSSIZE);

  checker.process(PROCESSSIZE);
}


////////////////////////////////////////////////////////////////////////////////
// distribution::survival::failure_time::meta::random_distribution.hpp 	      //
//                                                                            //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                 //
//  Software License, Version 1.0. (See accompanying file                     //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)          //
////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_FAILURE_TIME_META_RANDOM_DISTRIBUTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_FAILURE_TIME_META_RANDOM_DISTRIBUTION_HPP_ER_2009
#include <boost/mpl/bool.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/is_event.hpp>
#include <boost/statistics/detail/distribution/survival/failure_time/meta/detail/traits.hpp>
#include <boost/statistics/detail/distribution/survival/failure_time/meta/distribution.hpp>
#include <boost/statistics/detail/distribution_common/meta/random/distribution.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace distribution{
namespace survival {
namespace failure_time{

    // Returns a model of RandomDistribution
    // See http://www.boost.org/doc/libs/1_40_0/libs/random/random-concepts.html
    template<typename Model>
    struct random_distribution
    {
        typedef failure_time::distribution<Model> meta_;
        typedef typename meta_::type dist_;
        typedef typename distribution::meta::random_distribution<dist_> meta1_;
        typedef typename meta1_::type type;

        template<typename X>
        static type call(const X& x,const Model& m)
        {
            return distribution::make_random_distribution( meta_::call(x,m) );
        }
    };

	template<typename X,typename M>
    typename random_distribution<M>::type
    make_random_distribution(const X& x,const M& m)
    {
        typedef random_distribution<M> meta_;
        return meta_::call(x,m);
    }

	template<typename Map>
    typename random_distribution<
    	typename detail::traits<Map>::model::type
    >::type
    make_random_distribution(const Map& map)
    {
    	typedef model::tag::covariate tag_x_;
    	typedef model::tag::model tag_m_;
    	typedef typename detail::traits<Map>::model::type model_;
        typedef random_distribution<model_> meta_;
        return meta_::call(
        	boost::fusion::at_key<tag_x_>(map),
        	boost::fusion::at_key<tag_m_>(map)
        );
    }

}// failure_time
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


//
// Copyright (c) 2015-2018 CNRS
//

#include <iostream>
#include <iomanip>

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"

#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/parsers/urdf.hpp"

#include <vector>
#include <boost/test/unit_test.hpp>

using namespace pinocchio;

typedef std::map <std::string, pinocchio::SE3> PositionsMap_t;
typedef std::map <std::string, pinocchio::SE3> JointPositionsMap_t;
typedef std::map <std::string, pinocchio::SE3> GeometryPositionsMap_t;
typedef std::map <std::pair < std::string , std::string >, fcl::DistanceResult > PairDistanceMap_t;
JointPositionsMap_t fillPinocchioJointPositions(const pinocchio::Model& model, const pinocchio::Data & data);
GeometryPositionsMap_t fillPinocchioGeometryPositions(const pinocchio::GeometryModel & geomModel,
                                                      const pinocchio::GeometryData & geomData);

std::vector<std::string> getBodiesList();

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( simple_boxes )
{
  using namespace pinocchio;
  Model model;
  GeometryModel geomModel;

  Model::JointIndex idx;
  idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar1_joint");
  model.addJointFrame(idx);
  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
  model.addBodyFrame("planar1_body", idx, SE3::Identity());

  idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar2_joint");
  model.addJointFrame(idx);
  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
  model.addBodyFrame("planar2_body", idx, SE3::Identity());

  boost::shared_ptr<fcl::Box> sample(new fcl::Box(1, 1, 1));
  Model::FrameIndex body_id_1 = model.getBodyId("planar1_body");
  Model::JointIndex joint_parent_1 = model.frames[body_id_1].parent;
  Model::JointIndex idx_geom1 = geomModel.addGeometryObject(GeometryObject("ff1_collision_object",
                                                                           model.getBodyId("planar1_body"),joint_parent_1,
                                                                           sample,SE3::Identity(), "", Eigen::Vector3d::Ones())
                                                            );
  geomModel.geometryObjects[idx_geom1].parentJoint = model.frames[body_id_1].parent;

  boost::shared_ptr<fcl::Box> sample2(new fcl::Box(1, 1, 1));
  Model::FrameIndex body_id_2 = model.getBodyId("planar2_body");
  Model::JointIndex joint_parent_2 = model.frames[body_id_2].parent;
  Model::JointIndex idx_geom2 = geomModel.addGeometryObject(GeometryObject("ff2_collision_object",
                                                                           model.getBodyId("planar2_body"),joint_parent_2,
                                                                           sample2,SE3::Identity(), "", Eigen::Vector3d::Ones()),
                                                            model);
  BOOST_CHECK(geomModel.geometryObjects[idx_geom2].parentJoint == model.frames[body_id_2].parent);

  geomModel.addAllCollisionPairs();
  pinocchio::Data data(model);
  pinocchio::GeometryData geomData(geomModel);

  BOOST_CHECK(CollisionPair(0,1) == geomModel.collisionPairs[0]);

  std::cout << "------ Model ------ " << std::endl;
  std::cout << model;
  std::cout << "------ Geom ------ " << std::endl;
  std::cout << geomModel;
  std::cout << "------ DataGeom ------ " << std::endl;
  std::cout << geomData;

  Eigen::VectorXd q(model.nq);
  q <<  0, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  BOOST_CHECK(computeCollision(geomModel,geomData,0) == true);

  q <<  2, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  BOOST_CHECK(computeCollision(geomModel,geomData,0) == false);

  q <<  0.99, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  BOOST_CHECK(computeCollision(geomModel,geomData,0) == true);

  q <<  1.01, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  BOOST_CHECK(computeCollision(geomModel,geomData,0) == false);
}

BOOST_AUTO_TEST_CASE ( loading_model )
{
  typedef pinocchio::Model Model;
  typedef pinocchio::GeometryModel GeometryModel;
  typedef pinocchio::Data Data;
  typedef pinocchio::GeometryData GeometryData;

  std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo/romeo_description/urdf/romeo_small.urdf";
  std::vector < std::string > packageDirs;
  std::string meshDir  = PINOCCHIO_SOURCE_DIR"/models/romeo/";
  packageDirs.push_back(meshDir);

  Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
  GeometryModel geomModel;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs );
  geomModel.addAllCollisionPairs();

  Data data(model);
  GeometryData geomData(geomModel);
  fcl::CollisionResult result;

  Eigen::VectorXd q(model.nq);
  q << 0, 0, 0.840252, 0, 0, 0, 1, 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, 0, -0.3490658,
       0.6981317, -0.3490658, 0, 0, 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, 0, 0, 0, 0,
       1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  pinocchio::Index idx = geomModel.findCollisionPair(CollisionPair(1,10));
  BOOST_CHECK(computeCollision(geomModel,geomData,idx) == false);
}

#if defined(PINOCCHIO_WITH_URDFDOM) && defined(PINOCCHIO_WITH_HPP_FCL)
BOOST_AUTO_TEST_CASE (radius)
{
  std::vector < std::string > packageDirs;

  std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo/romeo_description/urdf/romeo_small.urdf";
  std::string meshDir  = PINOCCHIO_SOURCE_DIR"/models/romeo/";
  packageDirs.push_back(meshDir);

  pinocchio::Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
  pinocchio::GeometryModel geom;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom, packageDirs);
  Data data(model);
  GeometryData geomData(geom);

  // Test that the algorithm does not crash
  pinocchio::computeBodyRadius(model, geom, geomData);
  BOOST_FOREACH( double radius, geomData.radius) BOOST_CHECK(radius>=0.);
}
#endif // if defined(PINOCCHIO_WITH_URDFDOM) && defined(PINOCCHIO_WITH_HPP_FCL)

BOOST_AUTO_TEST_SUITE_END ()

JointPositionsMap_t fillPinocchioJointPositions(const pinocchio::Model& model, const pinocchio::Data & data)
{
  JointPositionsMap_t result;
  for (pinocchio::Model::Index i = 0; i < (pinocchio::Model::Index)model.njoints; ++i)
  {
    result[model.names[i]] = data.oMi[i];
  }
  return result;
}

GeometryPositionsMap_t fillPinocchioGeometryPositions(const pinocchio::GeometryModel & geomModel,
                                                      const pinocchio::GeometryData & geomData)
{
  GeometryPositionsMap_t result;
  for (std::size_t i = 0; i < geomModel.ngeoms ; ++i)
  {
    result[geomModel.geometryObjects[i].name] = geomData.oMg[i];
  }
  return result;
}

std::vector<std::string> getBodiesList()
{
  std::vector<std::string> result;

  result.push_back( "CHEST_LINK0");
  result.push_back( "torso");
  result.push_back( "HEAD_LINK0");
  result.push_back( "HEAD_LINK1");
  result.push_back( "LARM_LINK0");
  result.push_back( "LARM_LINK1");
  result.push_back( "LARM_LINK2");
  result.push_back( "LARM_LINK3");
  result.push_back( "LARM_LINK4");
  result.push_back( "l_wrist");
  result.push_back( "LARM_LINK6");
  result.push_back( "LHAND_LINK0");
  result.push_back( "LHAND_LINK1");
  result.push_back( "LHAND_LINK2");
  result.push_back( "LHAND_LINK3");
  result.push_back( "LHAND_LINK4");
  result.push_back( "RARM_LINK0");
  result.push_back( "RARM_LINK1");
  result.push_back( "RARM_LINK2");
  result.push_back( "RARM_LINK3");
  result.push_back( "RARM_LINK4");
  result.push_back( "r_wrist");
  result.push_back( "RARM_LINK6");
  result.push_back( "RHAND_LINK0");
  result.push_back( "RHAND_LINK1");
  result.push_back( "RHAND_LINK2");
  result.push_back( "RHAND_LINK3");
  result.push_back( "RHAND_LINK4");
  result.push_back( "LLEG_LINK0");
  result.push_back( "LLEG_LINK1");
  result.push_back( "LLEG_LINK2");
  result.push_back( "LLEG_LINK3");
  result.push_back( "LLEG_LINK4");
  result.push_back( "l_ankle");
  result.push_back( "RLEG_LINK0");
  result.push_back( "RLEG_LINK1");
  result.push_back( "RLEG_LINK2");
  result.push_back( "RLEG_LINK3");
  result.push_back( "RLEG_LINK4");
  result.push_back( "r_ankle");

  return result;
}


/**
 *  $Id: CORSIKAResampler.cxx 150715 2016-10-12 20:23:40Z hpandya $
 *
 *  Copyright (C) 2012
 *  The IceCube Collaboration <http://www.icecube.wisc.edu>
 *
 */

#include <icetray/open.h>
#include <icetray/I3Frame.h>
#include <icetray/I3Module.h>
#include <dataio/I3FileStager.h>

#include <dataclasses/physics/I3MCTreeUtils.h>
#include <dataclasses/physics/I3Particle.h>
#include <dataclasses/I3Double.h>
#include <simclasses/I3CorsikaInfo.h>
#include <simclasses/I3CorsikaShowerInfo.h>
#include <phys-services/I3RandomService.h>
#include <phys-services/surfaces/Cylinder.h>

#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/assign.hpp>
#include <boost/python.hpp>
#include <boost/scoped_ptr.hpp>

#include <cmath>

const double EarthRadius = 637131500*I3Units::cm; // as in CORSIKA

typedef I3Map<I3ParticleID, double> I3ParticleIDMap;
typedef boost::shared_ptr<I3ParticleIDMap> I3ParticleIDMapPtr;

class CORSIKAResampler : public I3Module
{
  public:
	CORSIKAResampler(const I3Context& context);
	void Configure();
	void Simulation(I3FramePtr frame);
	void DAQ(I3FramePtr frame);

  private:
	SET_LOGGER("I3CorsikaReader");

	I3MCTreePtr ResampleEvent(const I3MCTree, const I3CorsikaShowerInfo);

	I3RandomServicePtr rng_;
	std::string weightdictname_;
	std::string orig_mctree_name_;
	std::string mctree_name_;
	int oversampling_;
	I3Surfaces::Cylinder surface_;
	bool volumeCorr_;

};

I3_MODULE(CORSIKAResampler);

CORSIKAResampler::CORSIKAResampler(const I3Context& context)
    : I3Module(context), surface_(1600*I3Units::m, 800*I3Units::m)
{

	AddParameter("OverSampling", "number of times to oversample showers ",1);
	AddParameter("CylinderHeight", "Height of detector cylinder",
	    surface_.GetLength());
	AddParameter("CylinderRadius", "Radius of detector cylinder",
	    surface_.GetRadius());
	AddParameter("ZenithBias", "True if CORSIKA was compiled with the "
	    "VOLUMECORR option, False if the VOLUMEDET option was used. The "
	    "default zenith bias (proportional to sin(theta) for surface "
	    "detectors) is not supported.", true);
	AddParameter("WeightMapName", "Name of weights map to store in frame",
	    "CorsikaWeightMap");
	AddParameter("InputMCTreeName", "Name of mctree in frame", "I3MCTree_preSampling");
	AddParameter("OutputMCTreeName", "Name of mctree in frame", "I3MCTree");

	AddOutBox("OutBox");
}

void CORSIKAResampler::Configure()
{
	std::string filename;

	GetParameter("OverSampling", oversampling_);
	// Oversampling by reading input files multiple times
	double length, radius;
	GetParameter("CylinderHeight", length);
	GetParameter("CylinderRadius", radius);
	surface_ = I3Surfaces::Cylinder(length, radius);
	GetParameter("ZenithBias", volumeCorr_);
	GetParameter("WeightMapName", weightdictname_ );
	GetParameter("InputMCTreeName", orig_mctree_name_);
	GetParameter("OutputMCTreeName", mctree_name_);

	rng_ = context_.Get<I3RandomServicePtr>();
	if (rng_ == NULL)
		log_fatal("No I3RandomService in context!");

}

void CORSIKAResampler::Simulation(I3FramePtr frame)
{
  const I3CorsikaInfo old_info = frame->Get<I3CorsikaInfo>();
  I3CorsikaInfoPtr new_info(new I3CorsikaInfo(old_info));
  new_info->cylinder_height = surface_.GetLength();
  new_info->cylinder_radius = surface_.GetRadius();
  new_info->oversampling = oversampling_;
  frame->Delete("I3CorsikaInfo");
  frame->Put(new_info);
  log_info_stream (GetName() << " " << __PRETTY_FUNCTION__ << " " << *new_info);
  PushFrame(frame);
}

void CORSIKAResampler::DAQ(I3FramePtr frame)
{
	log_debug("%s: %s", GetName().c_str(), __PRETTY_FUNCTION__);
	log_trace("%s: %s", GetName().c_str(), __PRETTY_FUNCTION__);
	if (!frame->Has(orig_mctree_name_) )
		log_fatal("No tree %s found in frame!",orig_mctree_name_.c_str());
	if (frame->Has(mctree_name_) )
		log_fatal("Found %s: This event has already been sampled!",mctree_name_.c_str());
	const I3MCTree mctree = frame->Get<I3MCTree>(orig_mctree_name_);
	const I3MapStringDouble weightMap = frame->Get<I3MapStringDouble>(weightdictname_);
	const I3CorsikaShowerInfo showerInfo = frame->Get<I3CorsikaShowerInfo>("ShowerInfo");

	for ( int f_i = 0; f_i < oversampling_ ; f_i++)
	{
		I3FramePtr newframe(new I3Frame(I3Frame::DAQ));
		I3MCTreePtr o = I3MCTreePtr(new I3MCTree(mctree));
		newframe->Put(orig_mctree_name_,o);

		I3MCTreePtr t = ResampleEvent(mctree,showerInfo);
		const I3Particle primary = I3MCTreeUtils::GetPrimaries(*t)[0];
		newframe->Put(mctree_name_,t);

		// Update weights
		I3MapStringDoublePtr weightdict(new I3MapStringDouble(weightMap));

		double thetaMin = (*weightdict)["ThetaMin"];
		double thetaMax = (*weightdict)["ThetaMax"];

		(*weightdict)["OverSampling"] = (double)oversampling_;
		(*weightdict)["AreaSum"] = surface_.GetAcceptance(cos(thetaMax), cos(thetaMin));
		(*weightdict)["CylinderRadius"] = surface_.GetRadius();
		(*weightdict)["CylinderLength"] = surface_.GetLength();

		if (!volumeCorr_) {
			(*weightdict)["AreaSum"] = 2*M_PI*
		    surface_.GetArea(primary.GetDir())
		    /(cos(thetaMin)-cos(thetaMax));
		}
		newframe->Put(weightdictname_, weightdict);

		I3DoublePtr height(new I3Double(showerInfo.firstIntHeight));
		newframe->Put("CorsikaInteractionHeight", height);

		PushFrame(newframe);
	}
}

// Distance to the surface
double
GetSlantDepth(const I3Direction &dir, const I3Position &pos)
{
	double d = I3Constants::SurfaceElev-I3Constants::OriginElev-pos.GetZ();
	double r = EarthRadius;
	double ct = cos(dir.GetZenith());
	return sqrt(2*r*d + ct*ct*(r-d)*(r-d) - d*d) - (r-d)*ct;
}

I3MCTreePtr
CORSIKAResampler::ResampleEvent(I3MCTree mctree,I3CorsikaShowerInfo shower_info)
{
	I3MCTreePtr newtree = I3MCTreePtr(new I3MCTree());
	const I3Particle orig_primary = I3MCTreeUtils::GetPrimaries(mctree)[0];
	I3Particle primary = orig_primary.Clone();

	// Randomly shift shower core around
	I3Position shower_intersection =
	    surface_.SampleImpactPosition(primary.GetDir(), *rng_);

	double xspeed, yspeed, zspeed;
	xspeed = primary.GetSpeed()*sin(primary.GetZenith() / I3Units::radian)*
	    cos(primary.GetAzimuth() / I3Units::radian);
	yspeed = primary.GetSpeed()*sin(primary.GetZenith() / I3Units::radian)*
	    sin(primary.GetAzimuth() / I3Units::radian);
	zspeed = primary.GetSpeed()*cos(primary.GetZenith() / I3Units::radian);

	// Time 0 is when the particle intersects the observation level
	double time_to_intersection;
	if (shower_info.curved) {
		double entry_altitude = EarthRadius +
		    shower_info.entryHeight;
		double detection_altitude = EarthRadius +
			shower_info.obsLevelHeight + I3Constants::OriginElev;
		double detection_angle = primary.GetZenith();

		double flight_distance =
		    -detection_altitude*cos(detection_angle) +
		    sqrt(pow(entry_altitude, 2) -
		    pow(detection_altitude*sin(detection_angle), 2));
		primary.SetTime(-flight_distance/primary.GetSpeed());
		time_to_intersection = primary.GetTime() - GetSlantDepth(
		    primary.GetDir(), shower_intersection)/primary.GetSpeed();
	} else {
		primary.SetTime(-(shower_info.entryHeight -
		    I3Constants::SurfaceElev)/zspeed);
		time_to_intersection = primary.GetTime() -
		    (I3Constants::SurfaceElev - I3Constants::OriginElev
		    - shower_intersection.GetZ())/zspeed;
	}

	double time_to_interaction =
		(shower_info.entryHeight - shower_info.firstIntHeight)/zspeed;
	primary.SetLength(primary.GetSpeed()*time_to_interaction);
	primary.SetPos(
             shower_intersection.GetX() - time_to_intersection*xspeed,
             shower_intersection.GetY() - time_to_intersection*yspeed,
             shower_intersection.GetZ() - time_to_intersection*zspeed
             );

        double distance = primary.GetTime()*primary.GetSpeed();
	I3Position surface_pos = primary.GetPos() - distance*primary.GetDir();

	I3MCTreeUtils::AddPrimary(*newtree, primary);

	//I3MCTree::iterator iter;
	std::vector<I3Particle>::iterator iter;
	std::vector<I3Particle> daughters = I3MCTreeUtils::GetDaughters(mctree,orig_primary);
	std::vector<I3Particle> new_daughters;

	for (iter = daughters.begin(); iter != daughters.end();iter++) {
		I3Particle part = iter->Clone();
		I3Position temp_pos = iter->GetPos();
		if (shower_info.curvedObs) {
			double theta, phi, D;
			// Normal particles are recorded at the observation
			// level, extended-history entries at production level
			D = EarthRadius + I3Constants::OriginElev +
				shower_info.obsLevelHeight;
			theta = hypot(
			  temp_pos.GetX() + surface_pos.GetX(),
			  temp_pos.GetY() + surface_pos.GetY()
			  )/D;
			phi = atan2(temp_pos.GetY() + surface_pos.GetY(),
			  temp_pos.GetX() + surface_pos.GetX());

			part.SetPos(
				    D*sin(theta)*cos(phi),
				    D*sin(theta)*sin(phi),
				    D*cos(theta) - EarthRadius -
				    I3Constants::OriginElev
				);
		} else {
			part.SetPos(temp_pos.GetX() + surface_pos.GetX(),
			  temp_pos.GetY() + surface_pos.GetY(),
			  shower_info.obsLevelHeight);
		}
		part.SetTime(iter->GetTime()+primary.GetTime());

		new_daughters.push_back(part);
	}
	newtree->append_children(primary, new_daughters);

	return newtree;
}


#pragma once

#include <boost/optional/optional.hpp>

#include "Vector.hpp"
#include "Vector2.hpp"
#include "Color.hpp"

namespace col
{
    /// Single vertex.
	class Vertex
	{
	public:
		const Vector3& getPosition() const { return m_position; }
		void setPosition(const col::Vector3& position) { m_position = position; }

        const Vector3& getNormal() const { return m_normal; }
        void setNormal(const Vector3& normal) { m_normal = normal; }

        const boost::optional<Vector2>& getTextureCoordinates() const { return m_textureCoordinates; }
        void setTextureCoordinates(const Vector2& textureCoordinates) { m_textureCoordinates = textureCoordinates; }

	private:
		Vector3 m_position;
        Vector3 m_normal;
        boost::optional<Vector2> m_textureCoordinates;
    };
}


#include <boost/graph/spectral_embedding_3d_layout.hpp>
#include "graph_types.hpp"
#include <boost/graph/python/point2d.hpp>
#include <boost/graph/python/point3d.hpp>
#include <boost/graph/python/graph.hpp>
#include <boost/python.hpp>

namespace boost { namespace graph { namespace python {

template <typename Graph>
void
spectral_embedding_3d_layout
  (Graph& g,
   const vector_property_map<point3d, typename Graph::VertexIndexMap>* in_pos,
   double rel_tol = 1000,
   double abs_tol = 10000)
{
  using boost::python::object;

  typedef typename property_map<Graph, vertex_index_t>::const_type VertexIndexMap;
  typedef vector_property_map<point3d, VertexIndexMap> PositionMap;
  typedef vector_property_map<vertex_index_t, VertexIndexMap> IndexMap;

  PositionMap pos = *in_pos;

  boost::spectral_embedding_layout(g, pos);
}

void export_spectral_embedding_3d_layout()
{
  using boost::python::arg;
  using boost::python::def;
  using boost::python::object;

  def("spectral_embedding_3d_layout",
      &spectral_embedding_3d_layout<Graph>,
      (arg("graph"),
       arg("position") =
       (vector_property_map<point3d, Graph::VertexIndexMap>*)0,
       arg("rel_tol") = (double)1000,
       arg("abs_tol") = (double)10000)
      );

}

} } } // end namespace boost::graph::python


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/ext/std/ratio.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/comparable.hpp>
#include <laws/euclidean_ring.hpp>
#include <laws/group.hpp>
#include <laws/monoid.hpp>
#include <laws/orderable.hpp>
#include <laws/ring.hpp>

#include <ratio>
namespace hana = boost::hana;

int main() {
    auto ratios = hana::make_tuple(
          std::ratio<0>{}
        , std::ratio<1, 3>{}
        , std::ratio<1, 2>{}
        , std::ratio<2, 6>{}
        , std::ratio<3, 1>{}
        , std::ratio<7, 8>{}
        , std::ratio<3, 5>{}
        , std::ratio<2, 1>{}
    );

    hana::test::TestComparable<hana::ext::std::ratio_tag>{ratios};
    hana::test::TestOrderable<hana::ext::std::ratio_tag>{ratios};
    hana::test::TestMonoid<hana::ext::std::ratio_tag>{ratios};
    hana::test::TestGroup<hana::ext::std::ratio_tag>{ratios};
    hana::test::TestRing<hana::ext::std::ratio_tag>{ratios};
    hana::test::TestEuclideanRing<hana::ext::std::ratio_tag>{ratios};
}


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, PAL Robotics, S.L.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the PAL Robotics 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: Luca Marchionni
 * Author: Bence Magyar
 * Author: Enrique Fernández
 * Author: Paul Mathieu
 * Author: Luke Liao
 */

#include <odometry.h>

#include <boost/bind.hpp>

namespace omni_triangle_controller
{
namespace bacc = boost::accumulators;

Odometry::Odometry(size_t velocity_rolling_window_size)
: timestamp_(0.0)
, x_(0.0)
, y_(0.0)
, heading_(0.0)
, linear_x_(0.0)
, linear_y_(0.0)
, angular_(0.0)
, wheel_center_(0.0)
, wheel_radius_(0.0)
, wheel1_pos_(0.0)
, wheel2_pos_(0.0)
, wheel3_pos_(0.0)
, velocity_rolling_window_size_(velocity_rolling_window_size)
, linear_x_acc_(RollingWindow::window_size = velocity_rolling_window_size)
, linear_y_acc_(RollingWindow::window_size = velocity_rolling_window_size)
, angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
, integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2, _3))
{
}

void Odometry::init(const ros::Time& time)
{
	// Reset accumulators:
	linear_x_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
	linear_y_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
	angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);

	// Reset timestamp:
	timestamp_ = time;
}

bool Odometry::update(double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time)
{
	/// Get current wheel joint positions:
	//because of the matrix the value's squence is a little different than us
	const double v1 = wheel2_vel * wheel_radius_;
	const double v2 = wheel3_vel * wheel_radius_;
	const double v3 = wheel1_vel * wheel_radius_;
	const double L = wheel_center_;
	const double th = heading_;
	const double psi = 2 * 3.1415926 / 3;

	/// Update old position with current:
	wheel1_pos_ += v1;
	wheel2_pos_ += v2;
	wheel3_pos_ += v3;

	/// Compute linear and angular omni:
//	const double linear_x  = (v1*(cos(psi/2 + th) + cos(th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th)) - (v3*(cos(psi/2 + th) - cos(psi/2 - th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th)) - (v2*(cos(psi/2 - th) + cos(th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th));
//	const double linear_y  = (v2*(sin(psi/2 - th) - sin(th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th)) - (v3*(sin(psi/2 + th) + sin(psi/2 - th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th)) + (v1*(sin(psi/2 + th) + sin(th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th));
//	const double angular   = (v3*(cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th)))/(L*cos(psi/2 + th)*sin(psi/2 - th) + L*sin(psi/2 + th)*cos(psi/2 - th) - L*cos(psi/2 + th)*sin(th) + L*sin(psi/2 + th)*cos(th) + L*cos(psi/2 - th)*sin(th) + L*sin(psi/2 - th)*cos(th)) - (v1*(cos(psi/2 + th)*sin(th) - sin(psi/2 + th)*cos(th)))/(L*cos(psi/2 + th)*sin(psi/2 - th) + L*sin(psi/2 + th)*cos(psi/2 - th) - L*cos(psi/2 + th)*sin(th) + L*sin(psi/2 + th)*cos(th) + L*cos(psi/2 - th)*sin(th) + L*sin(psi/2 - th)*cos(th)) + (v2*(cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th)))/(L*cos(psi/2 + th)*sin(psi/2 - th) + L*sin(psi/2 + th)*cos(psi/2 - th) - L*cos(psi/2 + th)*sin(th) + L*sin(psi/2 + th)*cos(th) + L*cos(psi/2 - th)*sin(th) + L*sin(psi/2 - th)*cos(th));

	const double linear_x = (v1 / 2 / sin(psi / 2) - v2 / 2 / sin(psi / 2));
	const double linear_y = -v3 * 2 / 3 + v1 / 3 + v2 / 3;
	const double angular = v1 / 2 / (L + L / cos(psi / 2)) / cos(psi / 2) + v2 / 2 / (L + L / cos(psi / 2)) / cos(psi / 2) + v3 / (L + L / cos(psi / 2));
	ROS_INFO("velocity is %lf %lf %lf", wheel1_vel, wheel2_vel, wheel3_vel);

	/// We cannot estimate the speed with very small time intervals:
	const double dt = (time - timestamp_).toSec();
	if(dt < 0.0001)
		return false; // Interval too small to integrate with
	timestamp_ = time;

	/// Integrate odometry:
	integrate_fun_(linear_x, -linear_y, angular);
	/// Estimate speeds using a rolling mean to filter them out:
	// attention!!!!!! becaunse of the wrong direction of the axis
	linear_x_acc_(linear_x / dt);
	linear_y_acc_(linear_y / dt);
	angular_acc_(angular / dt);

	linear_x_ = bacc::rolling_mean(linear_x_acc_);
	linear_y_ = bacc::rolling_mean(linear_y_acc_);
	angular_ = bacc::rolling_mean(angular_acc_);

	return true;
}

void Odometry::setWheelParams(double wheel_center, double wheel_radius)
{
	wheel_center_ = wheel_center;
	wheel_radius_     = wheel_radius;
}

// TODO:: to be proof something wrong when using this
void Odometry::integrateRungeKutta2(double linear_x, double linear_y, double angular)
{
	const double direction = heading_ + angular * 0.5;

	/// Runge-Kutta 2nd order integration:
	x_       += linear_x * cos(direction) + linear_y * sin(direction);
	y_       += linear_y * sin(direction) + linear_x * cos(direction);
	heading_ += angular;
}

/**
 * \brief Other possible integration method provided by the class
 * \param linear_x
 * \param linear_y
 * \param angular
 */
void Odometry::integrateExact(double linear_x, double linear_y, double angular)
{
//	if(fabs(angular) < 10e-3)
//		integrateRungeKutta2(linear_x, linear_y, angular);
//	else
	{
		/// Exact integration (should solve problems when angular is zero):
		const double heading_old = heading_;
		heading_ += angular;
		x_       +=  linear_x * cos(heading_old) + linear_y * sin(heading_old);
		y_       +=  -linear_y * cos(heading_old) + linear_x * sin(heading_old);
	}
}

} // namespace diff_drive_controller


/**
 * This program tests the range image likelihood library.
 *
 * You can move around using
 *  w - forward
 *  a - backward
 *  s - left
 *  d - right
 *  q - up
 *  z - down
 *  f - write point cloud file
 *
 * The mouse controls the direction of movement
 *
 *  Esc - quit
 *
 */

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <boost/shared_ptr.hpp>
#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#endif
#include <GL/glew.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include "pcl/common/common.h"
#include "pcl/common/transforms.h"

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>

// define the following in order to eliminate the deprecated headers warning
#define VTK_EXCLUDE_STRSTREAM_HEADERS
#include <pcl/io/vtk_lib_io.h>

#include "pcl/simulation/camera.h"
#include "pcl/simulation/model.h"
#include "pcl/simulation/scene.h"
#include "pcl/simulation/range_likelihood.h"

#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>

// RangeImage:
#include <pcl/range_image/range_image_planar.h>

// Pop-up viewer
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>

using namespace Eigen;
using namespace pcl;
using namespace pcl::console;
using namespace pcl::io;
using namespace pcl::simulation;

using namespace std;

uint16_t t_gamma[2048];

Scene::Ptr scene_;
Camera::Ptr camera_;
RangeLikelihood::Ptr range_likelihood_;

int window_width_;
int window_height_;
bool paused_;
bool write_file_;

void printHelp(int argc, char **argv) {
    print_error("Syntax is: %s <filename>\n", argv[0]);
    print_info("acceptable filenames include vtk, obj and ply. ply can support "
               "colour\n");
}

void wait() {
    std::cout << "Press enter to continue";
    getchar();
    std::cout << "\n\n";
}

void display_score_image(const float *score_buffer) {
    int npixels =
        range_likelihood_->getWidth() * range_likelihood_->getHeight();
    uint8_t *score_img = new uint8_t[npixels * 3];

    float min_score = score_buffer[0];
    float max_score = score_buffer[0];
    for (int i = 1; i < npixels; i++) {
        if (score_buffer[i] < min_score)
            min_score = score_buffer[i];
        if (score_buffer[i] > max_score)
            max_score = score_buffer[i];
    }

    for (int i = 0; i < npixels; i++) {
        float d = (score_buffer[i] - min_score) / (max_score - min_score);
        score_img[3 * i + 0] = 0;
        score_img[3 * i + 1] = d * 255;
        score_img[3 * i + 2] = 0;
    }

    glRasterPos2i(-1, -1);
    glDrawPixels(range_likelihood_->getWidth(), range_likelihood_->getHeight(),
                 GL_RGB, GL_UNSIGNED_BYTE, score_img);

    delete[] score_img;
}

void display_depth_image(const float *depth_buffer, int width, int height) {
    int npixels = width * height;
    uint8_t *depth_img = new uint8_t[npixels * 3];

    float min_depth = depth_buffer[0];
    float max_depth = depth_buffer[0];
    for (int i = 1; i < npixels; ++i) {
        if (depth_buffer[i] < min_depth)
            min_depth = depth_buffer[i];
        if (depth_buffer[i] > max_depth)
            max_depth = depth_buffer[i];
    }

    for (int i = 0; i < npixels; ++i) {
        float zn = 0.7f;
        float zf = 20.0f;
        float d = depth_buffer[i];
        float z = -zf * zn / ((zf - zn) * (d - zf / (zf - zn)));
        float b = 0.075f;
        float f = 580.0f;
        uint16_t kd = static_cast<uint16_t>(1090 - b * f / z * 8);
        if (kd < 0)
            kd = 0;
        else if (kd > 2047)
            kd = 2047;

        int pval = t_gamma[kd];
        int lb = pval & 0xff;
        switch (pval >> 8) {
        case 0:
            depth_img[3 * i + 0] = 255;
            depth_img[3 * i + 1] = 255 - lb;
            depth_img[3 * i + 2] = 255 - lb;
            break;
        case 1:
            depth_img[3 * i + 0] = 255;
            depth_img[3 * i + 1] = lb;
            depth_img[3 * i + 2] = 0;
            break;
        case 2:
            depth_img[3 * i + 0] = 255 - lb;
            depth_img[3 * i + 1] = 255;
            depth_img[3 * i + 2] = 0;
            break;
        case 3:
            depth_img[3 * i + 0] = 0;
            depth_img[3 * i + 1] = 255;
            depth_img[3 * i + 2] = lb;
            break;
        case 4:
            depth_img[3 * i + 0] = 0;
            depth_img[3 * i + 1] = 255 - lb;
            depth_img[3 * i + 2] = 255;
            break;
        case 5:
            depth_img[3 * i + 0] = 0;
            depth_img[3 * i + 1] = 0;
            depth_img[3 * i + 2] = 255 - lb;
            break;
        default:
            depth_img[3 * i + 0] = 0;
            depth_img[3 * i + 1] = 0;
            depth_img[3 * i + 2] = 0;
            break;
        }
    }

    glRasterPos2i(-1, -1);
    glDrawPixels(width, height, GL_RGB, GL_UNSIGNED_BYTE, depth_img);

    delete[] depth_img;
}

/*
void display_depth_image(const float* depth_buffer)
{
  int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
  uint8_t* depth_img = new uint8_t[npixels * 3];

  float min_depth = depth_buffer[0];
  float max_depth = depth_buffer[0];
  for (int i=1; i<npixels; i++)
  {
    if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
    if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
  }

  for (int i=0; i<npixels; i++)
  {
    float zn = 0.7;
    float zf = 20.0;
    float d = depth_buffer[i];
    float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
    float b = 0.075;
    float f = 580.0;
    uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
    if (kd < 0) kd = 0;
    else if (kd>2047) kd = 2047;

    int pval = t_gamma[kd];
    int lb = pval & 0xff;
    switch (pval>>8) {
      case 0:
          depth_img[3*i+0] = 255;
          depth_img[3*i+1] = 255-lb;
          depth_img[3*i+2] = 255-lb;
          break;
      case 1:
          depth_img[3*i+0] = 255;
          depth_img[3*i+1] = lb;
          depth_img[3*i+2] = 0;
          break;
      case 2:
          depth_img[3*i+0] = 255-lb;
          depth_img[3*i+1] = 255;
          depth_img[3*i+2] = 0;
          break;
      case 3:
          depth_img[3*i+0] = 0;
          depth_img[3*i+1] = 255;
          depth_img[3*i+2] = lb;
          break;
      case 4:
          depth_img[3*i+0] = 0;
          depth_img[3*i+1] = 255-lb;
          depth_img[3*i+2] = 255;
          break;
      case 5:
          depth_img[3*i+0] = 0;
          depth_img[3*i+1] = 0;
          depth_img[3*i+2] = 255-lb;
          break;
      default:
          depth_img[3*i+0] = 0;
          depth_img[3*i+1] = 0;
          depth_img[3*i+2] = 0;
          break;
    }
  }

  glRasterPos2i(-1,-1);
  glDrawPixels(range_likelihood_->getWidth(), range_likelihood_->getHeight(),
GL_RGB, GL_UNSIGNED_BYTE, depth_img);

  delete [] depth_img;
}
*/

boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud) {
    // --------------------------------------------
    // -----Open 3D viewer and add point cloud-----
    // --------------------------------------------
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
        new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(
        cloud);
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud");
    viewer->setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    return (viewer);
}

void display() {
    float *reference = new float[range_likelihood_->getRowHeight() *
                                 range_likelihood_->getColWidth()];
    const float *depth_buffer = range_likelihood_->getDepthBuffer();
    // Copy one image from our last as a reference.
    for (int i = 0, n = 0; i < range_likelihood_->getRowHeight(); ++i) {
        for (int j = 0; j < range_likelihood_->getColWidth(); ++j) {
            reference[n++] =
                depth_buffer[i * range_likelihood_->getWidth() + j];
        }
    }

    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>
        poses;
    std::vector<float> scores;
    int n = range_likelihood_->getRows() * range_likelihood_->getCols();
    for (int i = 0; i < n; ++i) {
        Camera camera(*camera_);
        camera.move(0.0, i * 0.02, 0.0);
        // camera.move(0.0,i*0.02,0.0);
        poses.push_back(camera.getPose());
    }

    range_likelihood_->computeLikelihoods(reference, poses, scores);

    range_likelihood_->computeLikelihoods(reference, poses, scores);
    std::cout << "score: ";
    for (size_t i = 0; i < scores.size(); ++i) {
        std::cout << " " << scores[i];
    }
    std::cout << std::endl;

    std::cout << "camera: " << camera_->getX() << " " << camera_->getY() << " "
              << camera_->getZ() << " " << camera_->getRoll() << " "
              << camera_->getPitch() << " " << camera_->getYaw() << std::endl;

    delete[] reference;

    glDrawBuffer(GL_BACK);
    glReadBuffer(GL_BACK);

    // Draw the resulting images from the range_likelihood
    glViewport(range_likelihood_->getWidth(), 0, range_likelihood_->getWidth(),
               range_likelihood_->getHeight());
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    // Draw the color image
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glColorMask(true, true, true, true);
    glDisable(GL_DEPTH_TEST);

    glRasterPos2i(-1, -1);
    glDrawPixels(range_likelihood_->getWidth(), range_likelihood_->getHeight(),
                 GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_->getColorBuffer());

    // Draw the depth image
    glViewport(0, 0, range_likelihood_->getWidth(),
               range_likelihood_->getHeight());

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    //  display_depth_image (range_likelihood_->getDepthBuffer ());
    display_depth_image(range_likelihood_->getDepthBuffer(),
                        range_likelihood_->getWidth(),
                        range_likelihood_->getHeight());

    // Draw the score image
    glViewport(0, range_likelihood_->getHeight(), range_likelihood_->getWidth(),
               range_likelihood_->getHeight());
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    display_score_image(range_likelihood_->getScoreBuffer());

    glutSwapBuffers();

    if (write_file_) {
        range_likelihood_->addNoise();
        pcl::RangeImagePlanar rangeImage;
        range_likelihood_->getRangeImagePlanar(rangeImage);

        pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out(
            new pcl::PointCloud<pcl::PointXYZRGB>);

        // Optional argument to save point cloud in global frame:
        // Save camera relative:
        // range_likelihood_->getPointCloud(pc_out);
        // Save in global frame - applying the camera frame:
        // range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
        // Save in local frame
        range_likelihood_->getPointCloud(pc_out, false, camera_->getPose());
        // TODO: what to do when there are more than one simulated view?

        pcl::PCDWriter writer;
        writer.write("simulated_range_image.pcd", *pc_out, false);
        cout << "finished writing file\n";

        //     pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
        //     viewer.showCloud (pc_out);

        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
        viewer = simpleVis(pc_out);
        while (!viewer->wasStopped()) {
            viewer->spinOnce(100);
            boost::this_thread::sleep(boost::posix_time::microseconds(100000));
        }

        // doesnt work:
        //    viewer->~PCLVisualizer();
        //    viewer.reset();

        cout << "done\n";
        // Problem: vtk and opengl dont seem to play very well together
        // vtk seems to misbehave after a little while and wont keep the window
        // on the screen

        // method1: kill with [x] - but eventually it crashes:
        // while (!viewer.wasStopped ()){
        //}

        // method2: eventually starts ignoring cin and pops up on screen and
        // closes almost immediately
        //  cout << "enter 1 to cont\n";
        //  cin >> pause;
        //  viewer.wasStopped ();

        // method 3: if you interact with the window with keys, the window is
        // not closed properly
        // TODO: use pcl methods as this time stuff is probably not cross
        // playform
        //     struct timespec t;
        //     t.tv_sec = 100;
        //     //t.tv_nsec = (time_t)(20000000); // short sleep
        //     t.tv_nsec = (time_t)(0);  // long sleep - normal speed
        //     nanosleep (&t, NULL);
        write_file_ = 0;
    }
}

// Handle normal keys
void on_keyboard(unsigned char key, int x, int y) {
    double speed = 0.1;

    if (key == 27)
        exit(0);
    else if (key == 'w' || key == 'W')
        camera_->move(speed, 0, 0);
    else if (key == 's' || key == 'S')
        camera_->move(-speed, 0, 0);
    else if (key == 'a' || key == 'A')
        camera_->move(0, speed, 0);
    else if (key == 'd' || key == 'D')
        camera_->move(0, -speed, 0);
    else if (key == 'q' || key == 'Q')
        camera_->move(0, 0, speed);
    else if (key == 'z' || key == 'Z')
        camera_->move(0, 0, -speed);
    else if (key == 'p' || key == 'P')
        paused_ = !paused_;
    else if (key == 'v' || key == 'V')
        write_file_ = 1;

    // Use glutGetModifiers for modifiers
    // GLUT_ACTIVE_SHIFT, GLUT_ACTIVE_CTRL, GLUT_ACTIVE_ALT
}

// Handle special keys, e.g. F1, F2, ...
void on_special(int key, int x, int y) {
    switch (key) {
    case GLUT_KEY_F1:
        break;
    case GLUT_KEY_HOME:
        break;
    }
}

void on_reshape(int w, int h) {
    // Window size changed
    window_width_ = w;
    window_height_ = h;
}

void on_mouse(int button, int state, int x, int y) {
    // button:
    // GLUT_LEFT_BUTTON
    // GLUT_MIDDLE_BUTTON
    // GLUT_RIGHT_BUTTON
    //
    // state:
    // GLUT_UP
    // GLUT_DOWN
}

void on_motion(int x, int y) {}

void on_passive_motion(int x, int y) {
    if (paused_)
        return;

    double pitch = -(0.5 - (double)y / window_height_) * M_PI *
                   4; // in window coordinates positive y-axis is down
    double yaw = (0.5 - (double)x / window_width_) * M_PI * 2 * 4;

    camera_->setPitch(pitch);
    camera_->setYaw(yaw);
}

void on_entry(int state) {
    // state:
    // GLUT_LEFT
    // GLUT_ENTERED
}

// Read in a 3D model
void load_PolygonMesh_model(char *polygon_file) {
    pcl::PolygonMesh mesh; // (new pcl::PolygonMesh);
    // pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",mesh);
    pcl::io::loadPolygonFile(polygon_file, mesh);
    pcl::PolygonMesh::Ptr cloud(new pcl::PolygonMesh(mesh));

    // Not sure if PolygonMesh assumes triangles if to
    // TODO: Ask a developer
    PolygonMeshModel::Ptr model =
        PolygonMeshModel::Ptr(new PolygonMeshModel(GL_POLYGON, cloud));
    scene_->add(model);

    std::cout << "Just read " << polygon_file << std::endl;
    std::cout << mesh.polygons.size() << " polygons and "
              << mesh.cloud.data.size() << " triangles\n";
}

void initialize(int argc, char **argv) {
    const GLubyte *version = glGetString(GL_VERSION);
    std::cout << "OpenGL Version: " << version << std::endl;

    // works well for MIT CSAIL model 3rd floor:
    // camera_->set(4.04454, 44.9377, 1.1, 0.0, 0.0, -2.00352);

    // works well for MIT CSAIL model 2nd floor:
    //  camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);

    // works for small files:
    // camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
    camera_->set(1.31762, 0.382931, 1.89533, 0, 0.20944, -9.14989);
    camera_->setPitch(0.20944); // not sure why this is here:
    // camera_->set(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);

    cout << "About to read: " << argv[1] << endl;
    load_PolygonMesh_model(argv[1]);

    paused_ = false;
}

int main(int argc, char **argv) {
    int width = 640;
    int height = 480;

    window_width_ = width * 2;
    window_height_ = height * 2;

    print_info("Manually generate a simulated RGB-D point cloud using "
               "pcl::simulation. For more information, use: %s -h\n",
               argv[0]);

    if (argc < 2) {
        printHelp(argc, argv);
        return (-1);
    }

    int i;
    for (i = 0; i < 2048; i++) {
        float v = i / 2048.0;
        v = powf(v, 3) * 6;
        t_gamma[i] = v * 6 * 256;
    }

    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB); // was GLUT_RGBA
    glutInitWindowPosition(10, 10);
    glutInitWindowSize(window_width_, window_height_);
    glutCreateWindow("OpenGL range likelihood");

    GLenum err = glewInit();
    if (GLEW_OK != err) {
        std::cerr << "Error: " << glewGetErrorString(err) << std::endl;
        exit(-1);
    }

    std::cout << "Status: Using GLEW " << glewGetString(GLEW_VERSION)
              << std::endl;

    if (glewIsSupported("GL_VERSION_2_0"))
        std::cout << "OpenGL 2.0 supported" << std::endl;
    else {
        std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
        exit(1);
    }
    std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;

    camera_ = Camera::Ptr(new Camera());
    scene_ = Scene::Ptr(new Scene());

    // range_likelihood_ = RangeLikelihoodGLSL::Ptr(new RangeLikelihoodGLSL(1,
    // 1, height, width, scene_, 0));

    range_likelihood_ = RangeLikelihood::Ptr(
        new RangeLikelihood(2, 2, height / 2, width / 2, scene_));
    // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96,
    // 96, scene_)); range_likelihood_ = RangeLikelihood::Ptr(new
    // RangeLikelihood(1, 1, 480, 640, scene_));

    // Actually corresponds to default parameters:
    range_likelihood_->setCameraIntrinsicsParameters(
        640, 480, 576.09757860, 576.09757860, 321.06398107, 242.97676897);
    range_likelihood_->setComputeOnCPU(false);
    range_likelihood_->setSumOnCPU(true);
    range_likelihood_->setUseColor(true);

    initialize(argc, argv);

    glutReshapeFunc(on_reshape);
    glutDisplayFunc(display);
    glutIdleFunc(display);
    glutKeyboardFunc(on_keyboard);
    glutMouseFunc(on_mouse);
    glutMotionFunc(on_motion);
    glutPassiveMotionFunc(on_passive_motion);
    glutEntryFunc(on_entry);
    glutMainLoop();

    return 0;
}


//==============================================================================
//         Copyright 2016        NumScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/function/acosd.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/valmax.hpp>

#include <exhaustive.hpp>

#include <cmath>
#include <cstdlib>

struct raw_acosd
{
  float operator()(float x) const
  {
    return bs::acosd(double(x));
  }
};

int main(int argc, char* argv[])
{
  float mini = bs::Zero<float>();
  float maxi = bs::Valmax<float>();
  if(argc >= 2) mini = std::atof(argv[1]);
  if(argc >= 3) maxi = std::atof(argv[2]);
  bs::exhaustive_test<float> ( mini
                              , maxi
                              , bs::acosd
                              , raw_acosd()
                              );

  return 0;
}


#include "ros/ros.h"
#include "cv_bridge/cv_bridge.h"
#include "image_transport/image_transport.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/image_encodings.h"
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Dense"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <boost/foreach.hpp>
#include <chrono>
#include <pcl/io/pcd_io.h>
// #include <pcl/common/impl/io.hpp>
#include "pcl_ros/point_cloud.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <sophus/se3.hpp>
#include <iostream>
#include <fstream>
#include <pangolin/pangolin.h>
#include "geometry_msgs/PoseStamped.h"

using namespace std;
using namespace Eigen;
using namespace cv;
class getXYZfromPoint
{

private:
    ros::NodeHandle nh;
    ros::Subscriber sub;
    ros::Publisher pub;
    sensor_msgs::Image image_;
    geometry_msgs::PoseStamped robot_pose;
    cv::Mat cvColorImgMat;
    cv::Mat cvColorImgMat2;
    cv::Mat r, t;
    pcl::PCLPointCloud2 pcl_pc2;
    pcl::PCLPointCloud2 pcl_pc2l;
    uint8_t flag=0;
    long int cnt = 0;

    typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
    typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;
// cv::Mat color = cv::Mat::zeros(cv::Size(640,480,3),CV_64FC1);
public:
    getXYZfromPoint(){
        // sub = nh.subscribe<pcl::PointCloud<pcl::PointXYZ>>("/camera/depth_registered/points",1,&getXYZfromPoint::callback,this);
        sub = nh.subscribe("/camera/depth_registered/points",5,&getXYZfromPoint::callback,this);
        pub = nh.advertise<geometry_msgs::PoseStamped>("robot_pose",1);
    }
    void callback(const sensor_msgs::PointCloud2ConstPtr& point){
        cv::Point3f d;
        std::vector<cv::Point3f> pts_3d,pts_3d_last;
        std::vector<cv::Point2f> pts_2d,pts_2d_last;
        vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
        cnt = 0;
        //当前帧数据
        cv_bridge::CvImagePtr cvImagePtr;
        pcl_conversions::toPCL(*point,pcl_pc2);
        pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud_last(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);
        //now we know image(x,y) → point2(y*640+x)
        // BOOST_FOREACH(const pcl::PointXYZ& pt,temp_cloud->points){
        //     cnt++;
        //     if(isnan(pt.x)||isnan(pt.y)||isnan(pt.z))
        //     continue;
        //     ROS_INFO("cnt %d",cnt);
        //     ROS_INFO("%f %f %f",pt.x,pt.y,pt.z);
        // }
        ROS_INFO("Cloud:width = %d,height = %d",temp_cloud->width,temp_cloud->height);
        try
        {
            pcl::toROSMsg(*point,image_);
        }
        catch(std::runtime_error e)
        {
            ROS_ERROR_STREAM("Error in converting cloud to image message: " << e.what());
        }
        try{
            cvImagePtr = cv_bridge::toCvCopy(image_,sensor_msgs::image_encodings::BGR8);
        }catch(cv_bridge::Exception e){
            ROS_ERROR_STREAM("Cv_bridge Exception:" << e.what());
            return;
        }
        cvColorImgMat = cvImagePtr->image;

        //上一帧的数据
        if(flag==0){//如果是第一次进入，则和第一帧解算相同
            // pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud_last(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud_last);
            cvColorImgMat2 = cvImagePtr->image;
            flag =2;
        }else{
            // pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud_last(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::fromPCLPointCloud2(pcl_pc2l,*temp_cloud_last);
            // cvColorImgMat2 = cvImagePtr->image;
        }
        // cvColorImgMat2 = cvImagePtr->image;
        std::vector<cv::KeyPoint> keypoints,keypoints2;
        cv::Mat descriptors,descriptors2;
        cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
        cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
        cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
        detector->detect(cvColorImgMat,keypoints);
        detector->detect(cvColorImgMat2,keypoints2);
        descriptor->compute(cvColorImgMat,keypoints,descriptors);
        descriptor->compute(cvColorImgMat2,keypoints2,descriptors2);
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
        std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
        ROS_INFO("extract ORB cost = %f seconds",time_used.count());

        //match
        std::vector<cv::DMatch> matches;
        t1 = std::chrono::steady_clock::now();
        try
        {
            matcher->match(descriptors,descriptors2,matches);
        }
        catch(const cv::Exception& e)
        {
            cout<< e.what() <<endl;
        }

        t2 = std::chrono::steady_clock::now();
        time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
        ROS_INFO("match ORB cost %f seconds",time_used.count());

        //chose better
        auto min_max = std::minmax_element(matches.begin(),matches.end(),
            [](const cv::DMatch &m1,const cv::DMatch &m2){return m1.distance < m2.distance;});
        double min_dist = min_max.first->distance;
        double max_dist = min_max.second->distance;
        std::vector<cv::DMatch> good_matches;
        for (int i = 0; i < descriptors.rows; i++){
            if(matches[i].distance <= std::max(2 * min_dist,30.0))
                good_matches.push_back(matches[i]);
        }
        cout << "一共找到了 %d 组匹配点" << good_matches.size() <<endl;
        ofstream ofs;
        ofs.open("/home/bonoy/data/ORBfeature.txt",ios::app);
        ofs << "time " << time_used.count() << "num" << good_matches.size() <<endl;
        ofs.close();
        //获取3D点
        // BOOST_FOREACH(cv::DMatch m,good_matches){
            cv::DMatch m;
            for(int i=0;i<good_matches.size();i++){
            m = good_matches[i];
            d.x = temp_cloud_last->points[int(keypoints2[m.queryIdx].pt.y)*640 + int(keypoints2[m.queryIdx].pt.x)].x;
            d.y = temp_cloud_last->points[int(keypoints2[m.queryIdx].pt.y)*640 + int(keypoints2[m.queryIdx].pt.x)].y;
            d.z = temp_cloud_last->points[int(keypoints2[m.queryIdx].pt.y)*640 + int(keypoints2[m.queryIdx].pt.x)].z;
            if(isnan(d.x)||isnan(d.y)||isnan(d.z))
                continue;
            pts_3d.push_back(d);
            pts_2d.push_back(keypoints[m.trainIdx].pt);
            // cout << "X: "<<d.x << " Y: " << d.y << " Z: "<< d.z << endl;
            // cout << "x:" << int(keypoints2[m.queryIdx].pt.x) << " y:" << int(keypoints2[m.queryIdx].pt.y) <<endl;
            // cout << "u:" << int(525.0*(d.x/d.z)+319.5) << " v:" << int(525*(d.y/d.z)+239.5) <<endl;
            // // ROS_INFO("u %d v %d",i,121);
        }
        std::cout << "3d-2s pairs: " << pts_3d.size() << std::endl;
        cv::Mat K = (cv::Mat_<double>(3, 3) << 525.0, 0, 319.5, 0, 525.0, 239.5, 0, 0, 1);
        //Opencv 解位姿
        t1 = std::chrono::steady_clock::now();
        try{
            cv::solvePnP(pts_3d, pts_2d, K, cv::Mat(), r, t, false,CV_ITERATIVE);
        }catch(const cv::Exception& e)
        {
            cout<< e.what() <<endl;
        }
        // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
        cv::Mat R(3,3,CV_64FC1);
        Eigen::Matrix3d rotation_matrix;
        Eigen::Vector3d eigent;
        cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
        t2 = std::chrono::steady_clock::now();
        time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
        std::cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << std::endl;

        std::cout << "R=" << std::endl << R << std::endl;
        std::cout << "t=" << std::endl << t << std::endl;
        cv::cv2eigen(R,rotation_matrix);
        cv::cv2eigen(t,eigent);
        Eigen::Quaterniond qua(rotation_matrix);
        robot_pose.header.stamp = point->header.stamp;
        robot_pose.header.frame_id = "robot_pose";
        robot_pose.pose.position.x = eigent(0);
        robot_pose.pose.position.y = eigent(1);
        robot_pose.pose.position.z = eigent(2);
        robot_pose.pose.orientation.x = qua.x();
        robot_pose.pose.orientation.y = qua.y();
        robot_pose.pose.orientation.z = qua.z();
        robot_pose.pose.orientation.w = qua.w();
        pub.publish(robot_pose);

        // Isometry3d Twr(qua);
        // Twr.pretranslate(eigent);
        // poses.push_back(Twr);
        // DrawTrajectory(poses);
        //gaussNewton 解位姿
        // getXYZfromPoint::VecVector3d pts_3d_eigen;
        // getXYZfromPoint::VecVector2d pts_2d_eigen;
        // for (size_t i = 0; i < pts_3d.size(); ++i) {
        //     pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
        //     pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
        // }
        // std::cout << "calling bundle adjustment by gauss newton " << std::endl;
        // Sophus::SE3d pose_gn;
        // t1 = std::chrono::steady_clock::now();
        // getXYZfromPoint::bundleAdjustmentGaussNewton(pts_3d_eigen,pts_2d_eigen,K,pose_gn);
        // t2 = std::chrono::steady_clock::now();
        // time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
        // std::cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << std::endl;
        //保存上一帧的数据
        // pcl_pc2l = pcl_pc2;
        pcl::copyPointCloud(*pcl_pc2,*pcl_pc2l);
        cvColorImgMat.copyTo(cvColorImgMat2);
        //draw answer
        cv::Mat img_match;
        cv::Mat img_goodmatch;
        cv::drawMatches(cvColorImgMat,keypoints,cvColorImgMat2,keypoints2,good_matches,img_goodmatch);
        cv::imshow("good matches",img_goodmatch);
        // cv::imshow("grayview",cvGrayImgMat);
        cv::waitKey(5);
        // ROS_INFO("%f %f %f",point->points[240000].b,point->points[240000].g,point->points[240000].r);
    }
void bundleAdjustmentGaussNewton(const getXYZfromPoint::VecVector3d &points_3d,const getXYZfromPoint::VecVector2d &points_2d,const cv::Mat &K,Sophus::SE3d &pose) {
        typedef Eigen::Matrix<double,6,1> Vector6d;
        const int iterations = 10;
        double cost = 0,lastCost = 0;
        double fx = K.at<double>(0,0);
        double fy = K.at<double>(1,1);
        double cx = K.at<double>(0,2);
        double cy = K.at<double>(1,2);
        for(int iter = 0;iter < iterations; iter++){
            Eigen::Matrix<double,6,6> H = Eigen::Matrix<double,6,6>::Zero();
            Vector6d b = Vector6d::Zero();
            cost = 0;
            //compute cost
            for(int i = 0;i < points_3d.size(); i++){
                Eigen::Vector3d pc = pose * points_3d[i];
                double inv_z = 1.0 / pc[2];
                double inv_z2 = inv_z * inv_z;
                Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx,fy * pc[1] / pc[2] + cy);
                Eigen::Vector2d e = points_2d[i] - proj;
                cost+=e.squaredNorm();

                Eigen::Matrix<double,2,6> J;
                J << -fx * inv_z,
                0,
                fx * pc[0] * inv_z2,
                fx * pc[0] * pc[1] * inv_z2,
                -fx - fx * pc[0] * pc[0] * inv_z2,
                fx * pc[1] * inv_z,
                0,
                -fy * inv_z,
                fy * pc[1] * inv_z2,
                fy + fy * pc[1] * pc[1] * inv_z2,
                -fy * pc[0] * pc[1] * inv_z2,
                -fy * pc[0] * inv_z;

                H += J.transpose() * J;
                b += -J.transpose() * e;
            }
            Vector6d dx;
            dx = H.ldlt().solve(b);
            if(isnan(dx[0])){
                std::cout << "result is nan!" << std::endl;
                break;
            }

            if(iter > 0 && cost >= lastCost){
                // cost increase, update is not good
                std::cout << "cost: " << cost << ", last cost: " << lastCost << std::endl;
                break;
            }

            //update estimation
            pose = Sophus::SE3d::exp(dx) * pose;
            lastCost = cost;
            std::cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << std::endl;
            if(dx.norm() < 1e-6){
                break;
            }
        }
        std::cout << "pose by g-n \n" << pose.matrix() <<std::endl;
        // return pose;
    }
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses){
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));
  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    pangolin::Quit();
    //usleep(5000);   // sleep 5 ms
  }
}
};
int main(int argc, char *argv[])
{
    ros::init(argc,argv,"getXYZRGBfrompoints");
    getXYZfromPoint get;
    ros::spin();
    return 0;
}


#ifndef QUADRATIC_B_SPLINE_3D_HPP
#define QUADRATIC_B_SPLINE_3D_HPP

#include <polyfem/QuadraticBSpline.hpp>

#include <array>
#include <cassert>

#include <Eigen/Dense>

namespace polyfem {
	class QuadraticBSpline3d {
	public:
		QuadraticBSpline3d()
		{ }

		QuadraticBSpline3d(const std::array<double, 4> &knots_u, const std::array<double, 4> &knots_v, const std::array<double, 4> &knots_w)
		: spline_u_(knots_u), spline_v_(knots_v), spline_w_(knots_w)
		{ }

		void init(const std::array<double, 4> &knots_u, const std::array<double, 4> &knots_v, const std::array<double, 4> &knots_w);

		void interpolate(const Eigen::MatrixXd &ts, Eigen::MatrixXd &result) const;
		double interpolate(const double u, const double v, const double w) const;

		void derivative(const Eigen::MatrixXd &ts, Eigen::MatrixXd &result) const;
	private:
		QuadraticBSpline spline_u_;
		QuadraticBSpline spline_v_;
		QuadraticBSpline spline_w_;
	};
}
#endif //QUADRATIC_B_SPLINE_3D_HPP


#pragma once

#include <boost/multiprecision/cpp_int.hpp>

#include <istream>
#include <ostream>

namespace base
{

template<typename T>
using BigInteger = boost::multiprecision::number<T>;

template<typename T>
std::ostream& operator<<(std::ostream& output, const BigInteger<T>& big_int);

template<typename T>
std::istream& operator>>(std::istream& input, BigInteger<T>& big_int);

using Uint256 = BigInteger<
  boost::multiprecision::backends::cpp_int_backend<256,
                                                   256,
                                                   boost::multiprecision::cpp_integer_type::unsigned_magnitude,
                                                   boost::multiprecision::cpp_int_check_type::checked,
                                                   void>>;

using Uint512 = BigInteger<
  boost::multiprecision::backends::cpp_int_backend<512,
                                                   512,
                                                   boost::multiprecision::cpp_integer_type::unsigned_magnitude,
                                                   boost::multiprecision::cpp_int_check_type::checked,
                                                   void>>;

} // namespace base


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

/// \file datum_convert.cc
///

#include <vw/FileIO.h>
#include <vw/Image.h>
#include <vw/Cartography.h>
#include <vw/Math.h>
#include <asp/Core/Macros.h>
#include <asp/Core/Common.h>
#include <asp/Core/PointUtils.h>

#include <boost/filesystem.hpp>
namespace po = boost::program_options;
namespace fs = boost::filesystem;

using namespace vw;
using namespace vw::cartography;
using namespace std;

template <class ImageT>
class DatumConvertView : public ImageViewBase<DatumConvertView<ImageT> >
{
  ImageT        m_input_dem;
  GeoReference const& m_input_georef;
  GeoReference const& m_output_georef;
  double              m_nodata_val;

public:

  typedef double pixel_type;
  typedef double result_type;
  typedef ProceduralPixelAccessor<DatumConvertView> pixel_accessor;

  /// Image view which adds or subtracts the ellipsoid/geoid difference
  ///  from elevations in a DEM image.
  DatumConvertView(ImageT       const& input_dem,
                   GeoReference const& input_georef,
                   GeoReference const& output_georef,
                   double nodata_val):
    m_input_dem(input_dem), m_input_georef(input_georef), m_output_georef(output_georef),
    m_nodata_val(nodata_val){}

  inline int32 cols  () const { return m_input_dem.cols(); }
  inline int32 rows  () const { return m_input_dem.rows(); }
  inline int32 planes() const { return 1; }

  inline pixel_accessor origin() const { return pixel_accessor(*this); }

  inline result_type operator()( size_t col, size_t row, size_t p=0 ) const {

    // Handle nodata
    if ( m_input_dem(col, row) == m_nodata_val )
      return m_nodata_val;

    // Compute the elevation in the output datum
    double  current_height  = m_input_dem(col, row);
    Vector2 input_lonlat    = m_input_georef.pixel_to_lonlat(Vector2(col, row));
    Vector3 input_llh(input_lonlat[0], input_lonlat[1], current_height);
    Vector3 gcc_coord       = m_input_georef.datum().geodetic_to_cartesian(input_llh);
    Vector3 output_lonlat   = m_output_georef.datum().cartesian_to_geodetic(gcc_coord);
    double output_height = output_lonlat[2];
/*
    if ( (current_height < -50) || (current_height > 700))
    {
      std::cout << "pixel      = " << Vector2(col, row) << std::endl;
      std::cout << "height      = " << current_height << std::endl;
      //std::cout << m_input_dem(col, row)  << std::endl;
      //std::cout << "dem lonlat = " << input_llh     << std::endl;
      //std::cout << "gcc_coord  = " << gcc_coord     << std::endl;
      //std::cout << "out lonlat = " << output_lonlat << std::endl;
    }
*/
    return output_height;
  }

  /// \cond INTERNAL
  typedef DatumConvertView<typename ImageT::prerasterize_type> prerasterize_type;
  inline prerasterize_type prerasterize( BBox2i const& bbox ) const {
    return prerasterize_type( m_input_dem.prerasterize(bbox), m_input_georef,
                              m_output_georef, m_nodata_val );
  }
  template <class DestT> inline void rasterize( DestT const& dest, BBox2i const& bbox ) const {
    vw::rasterize( prerasterize(bbox), dest, bbox );
  }
  /// \endcond
};

// Helper function which uses the class above.
template <class ImageT>
DatumConvertView<ImageT>
datum_convert( ImageViewBase<ImageT> const& input_dem,
               GeoReference          const& input_georef,
               GeoReference          const& output_georef,
               double                       nodata_val) {
  return DatumConvertView<ImageT>( input_dem.impl(), input_georef, output_georef, nodata_val );
}

/// Convert input pixel location to output projection location
Vector2 get_output_loc(Vector2      const& input_pixel,
                       double       const  dem_height,
                       GeoReference const& input_georef,
                       GeoReference const& output_georef) {
  Vector2 input_lonlat     = input_georef.pixel_to_lonlat(input_pixel);
  Vector3 input_llh(input_lonlat[0], input_lonlat[1], dem_height);
  Vector3 gcc_coord        = input_georef.datum().geodetic_to_cartesian(input_llh);
  Vector3 output_lonlat    = output_georef.datum().cartesian_to_geodetic(gcc_coord);
  Vector2 output_projected = output_georef.lonlat_to_point(Vector2(output_lonlat[0], output_lonlat[1]));
  return output_projected;
}

/// Find the bounding box in the output projected space that contains all the input data
template <typename T>
BBox2 get_output_projected_bbox(GeoReference     const& input_georef,
                                GeoReference     const& output_georef,
                                DiskImageView<T> const& input_dem) {

  const int num_rows = input_dem.rows();
  const int num_cols = input_dem.cols();

  vw_out() << "Image size = " << Vector2(num_cols, num_rows) << std::endl;

  // Expand along sides
  BBox2 output_bbox;
  for (int r=0; r<num_rows; ++r) {
    Vector2 pixel_left (0,          r);
    Vector2 pixel_right(num_cols-1, r);
    double height_left  = input_dem(pixel_left[0],  pixel_left[1]);
    double height_right = input_dem(pixel_right[0], pixel_right[1]);

    output_bbox.grow(get_output_loc(pixel_left,  height_left,  input_georef, output_georef));
    output_bbox.grow(get_output_loc(pixel_right, height_right, input_georef, output_georef));
  }

  // Expand along the top and bottom
  for (int c=1; c<num_cols-1; ++c) {
    Vector2 pixel_top(c, 0         );
    Vector2 pixel_bot(c, num_rows-1);
    double height_top = input_dem(pixel_top[0], pixel_top[1]);
    double height_bot = input_dem(pixel_bot[0], pixel_bot[1]);

    output_bbox.grow(get_output_loc(pixel_top, height_top, input_georef, output_georef));
    output_bbox.grow(get_output_loc(pixel_bot, height_bot, input_georef, output_georef));
  }
  return output_bbox;
}

struct Options : vw::cartography::GdalWriteOptions {
  string input_dem, output_dem, output_datum, target_srs_string;
  double nodata_value;
  bool   use_double;
};

void handle_arguments( int argc, char *argv[], Options& opt ){

  po::options_description general_options("");
  general_options.add_options()
    ("output-datum", po::value(&opt.output_datum), " The datum to convert to. Supported options: WGS_1984, NAD83, WGS72, and NAD27.")
    ("t_srs",        po::value(&opt.target_srs_string)->default_value(""), "Specify the output datum via the PROJ.4 string.")
    ("nodata_value", po::value(&opt.nodata_value)->default_value(-32768),
     "The value of no-data pixels, unless specified in the DEM.")
    ("double", po::bool_switch(&opt.use_double)->default_value(false)->implicit_value(true),
     "Output using double precision (64 bit) instead of float (32 bit).");

  general_options.add( vw::cartography::GdalWriteOptionsDescription(opt) );

  po::options_description positional("");
  positional.add_options()
    ("input-dem",    po::value(&opt.input_dem  ), "The path to the input DEM file.")
    ("output-dem",   po::value(&opt.output_dem ), "The path to the output DEM file.");

  po::positional_options_description positional_desc;
  positional_desc.add("input-dem",    1);
  positional_desc.add("output-dem",   1);

  string usage("[options] <input dem> <output dem>");
  bool allow_unregistered = false;
  vector<string> unregistered;
  po::variables_map vm =
    asp::check_command_line( argc, argv, opt, general_options, general_options,
                             positional, positional_desc, usage,
                             allow_unregistered, unregistered);

  if ( opt.input_dem.empty() )
    vw_throw( ArgumentErr() << "Missing input arguments.\n\n"     << usage << general_options );
  if ( opt.output_dem.empty() )
    vw_throw( ArgumentErr() << "Requires <output dem> in order to proceed.\n\n"  << usage << general_options );
  if ( opt.output_datum.empty() && opt.target_srs_string.empty())
    vw_throw( ArgumentErr() << "Requires <output datum> or PROJ.4 string in order to proceed.\n\n" << usage << general_options );

  if ( !opt.output_datum.empty() && !opt.target_srs_string.empty())
    vw_out(WarningMessage) << "Both the output datum and the PROJ.4 string were specified. The former takes precedence.\n";

  boost::to_lower(opt.output_datum);

  vw::create_out_dir(opt.output_dem);

  //// Turn on logging to file
  //asp::log_to_file(argc, argv, "", opt.out_prefix);

}

/// Perform vertical datum conversion.
/// - gdalwarp should be used after this tool to perform the horizontal conversion
/// - TODO: This tool does everything?

// --> Option 1: Run this tool before gdalwarp, all conversions handled.
// --> Option 2: This tool does everything, but only handles datum conversions
//               within the same projection type.

template <typename T>
void do_work(Options const& opt) {

  // Read the DEM to adjust
  DiskImageResourceGDAL dem_rsrc(opt.input_dem);
  double dem_nodata_val = opt.nodata_value;
  if ( dem_rsrc.has_nodata_read() ) {
    dem_nodata_val = dem_rsrc.nodata_read();
    vw_out() << "\tFound input nodata value for " << opt.input_dem << ": " << dem_nodata_val << endl;
  }
  DiskImageView<T> dem_img(dem_rsrc);
  GeoReference dem_georef;
  bool has_georef = read_georeference(dem_georef, dem_rsrc);
  if (!has_georef)
    vw_throw( ArgumentErr() << "Missing georeference for DEM: " << opt.input_dem << "\n" );

  // Handle the datums supported by our Datum class:
  // WGS84, WGS72, NAD83, NAD27, D_MOON, D_MARS

  // Create an output GeoReference object
  cartography::Datum user_datum;
  bool have_user_datum = asp::read_user_datum(0, 0, opt.output_datum, user_datum);
  GeoReference output_georef = dem_georef;
  if (opt.target_srs_string.empty()) {
    if (have_user_datum)
      output_georef.set_well_known_geogcs(opt.output_datum);
  } else {
    // The user specified the target srs_string
    // Set the srs string into georef.
    asp::set_srs_string(opt.target_srs_string, have_user_datum, user_datum, output_georef);
  }

  vw_out() << "Input georef:\n"  << dem_georef    << std::endl;

  BBox2 output_proj_box = get_output_projected_bbox(dem_georef, output_georef, dem_img);

  vw_out() << "Computed output projected box:\n" << output_proj_box << std::endl;

  // Overwrite the projected bounds in the output bounding box
  Matrix<double,3,3> affine = output_georef.transform();
  affine(0, 2) = output_proj_box.min()[0];
  if (affine(1, 1) > 0)
    affine(1, 2) = output_proj_box.min()[1];
  else // Decreasing Y as row increases
    affine(1, 2) = output_proj_box.max()[1];
  output_georef.set_transform(affine);

  vw_out() << "Output georef:\n" << output_georef << std::endl;

  BBox2i output_pixel_box = output_georef.point_to_pixel_bbox(output_proj_box);
  vw_out() << "Computed output pixel box:\n" << output_pixel_box << std::endl;

  // Update the elevation values in the image to account for the new datum.
  ImageViewRef<double> dem_new_heights = datum_convert(pixel_cast<double>(dem_img),
                                                       dem_georef,
                                                       output_georef,
                                                       dem_nodata_val
                                                      );

  // Apply the horizontal warping to the image on account of the new datum.
  ImageViewRef<double> output_dem = apply_mask(geo_transform(create_mask(dem_new_heights,
                                                                         dem_nodata_val),
                                                             dem_georef, output_georef,
                                                             output_pixel_box.width(),
                                                             output_pixel_box.height(),
                                                             ConstantEdgeExtension()
                                                            ),
                                               dem_nodata_val
                                              );

  vw_out() << "Writing adjusted DEM: " << opt.output_dem << endl;

  if ( opt.use_double ) {
    // Output as double
    block_write_gdal_image(opt.output_dem, output_dem,
                           true, output_georef,
                           true, dem_nodata_val, opt,
                           TerminalProgressCallback("asp", "\t--> Converting datum: ") );
  }else{
    // Output as float
    block_write_gdal_image(opt.output_dem,  pixel_cast<float>(output_dem),
                           true, output_georef,
                           true, dem_nodata_val, opt,
                           TerminalProgressCallback("asp", "\t--> Converting datum: ") );
  }

}

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

  Options opt;
  try {
    handle_arguments( argc, argv, opt );

    // Check the input data type
    DiskImageResourceGDAL dem_rsrc(opt.input_dem);
    ChannelTypeEnum input_data_type = dem_rsrc.channel_type();

    // Redirect to another function with the correct template type
    switch(input_data_type) {
      //case VW_CHANNEL_INT8   : do_work<vw::int8   >(opt);  break;
      //case VW_CHANNEL_UINT8  : do_work<vw::uint8  >(opt);  break;
      case VW_CHANNEL_INT16  : do_work<vw::int16  >(opt);  break;
      case VW_CHANNEL_UINT16 : do_work<vw::uint16 >(opt);  break;
      case VW_CHANNEL_INT32  : do_work<vw::int32  >(opt);  break;
      case VW_CHANNEL_UINT32 : do_work<vw::uint32 >(opt);  break;
      case VW_CHANNEL_FLOAT32: do_work<vw::float32>(opt);  break;
      case VW_CHANNEL_FLOAT64: do_work<vw::float64>(opt);  break;
      default : vw_throw(ArgumentErr() << "Input image format " << input_data_type << " is not supported!\n");
    };

  } ASP_STANDARD_CATCHES;

  return 0;
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <iomanip>
#include <armadillo>

#include "geom.hpp"
#include "pes.hpp"
#include "message.hpp"

using namespace willow;
//
// Unit : AU
//
int main (int argc, char *argv[])
{

  // mpi start
  mpi::init (argc, argv);
  const auto sys_me = mpi::rank ();

  std::cout << std::setprecision (8);
  std::cout << std::fixed;

  // xyz file name
  const std::string fname_geom  = (argc > 1) ? argv[1] : "cage.xyz";
  // RHF vs MP2
  const std::string theory      = (argc > 2) ? argv[2] : "RHF";
  // BasisSet: aug-cc-pvdz, 6-311g**
  const std::string basis       = (argc > 3) ? argv[3] : "aug-cc-pvdz";
  //

  std::vector<std::string>  at_names;
  arma::mat  pos = read_geom (fname_geom, at_names);
  arma::ivec Zs  = atom_to_Z (at_names);

  // -----

  if (sys_me == 0) {
    std::cout << "START Embedded Many Body Expansion Method " << std::endl;
  }
  const std::string method_embe = "EMBE";
  // Epol (Q): what is Q for Epol?
  // ESP, RESP, ESP_SCF, RESP_SCF
  // Here, ESP_SCF and RESP_SCF provide the atomic point charges using the self-consistent field scheme.

  const std::string epol_resp_scf = "RESP_SCF";
  willow::PES embe_pot_grad (pos.n_cols, theory, basis, method_embe, epol_resp_scf);
  embe_pot_grad.compute (pos, Zs);

  if (sys_me == 0) {
    std::cout << "START Many Body Expansion Method " << std::endl;
  }

  const std::string method_mbe = "MBE";
  const std::string epol_resp  = "RESP";
  willow::PES mbe_pot_grad (pos.n_cols, theory, basis, method_mbe, epol_resp);
  mbe_pot_grad.compute (pos, Zs);

  mpi::finalize ();

  return 0;

}