#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/maptbx/fft.h>
#include <cctbx/maptbx/average_densities.h>
#include <cctbx/maptbx/standard_deviations_around_sites.hpp>
#include <scitbx/boost_python/utils.h>
#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <cctbx/maptbx/histogram.h>
#include <cctbx/maptbx/resolution.h>
#include <cctbx/maptbx/fsc.h>
#include <cctbx/maptbx/sphericity.h>
#include <cctbx/maptbx/mask.h>
#include <cctbx/maptbx/utils.h>
#include <cctbx/maptbx/connectivity.h>
#include <cctbx/maptbx/marked_grid_points.h>
#include <cctbx/maptbx/mask_utils.h>
#include <cctbx/maptbx/map_accumulator.h>
#include <cctbx/maptbx/ft_analytical_1d_point_scatterer_at_origin.h>
#include <cctbx/maptbx/target_and_gradients.h>

namespace cctbx { namespace maptbx { namespace boost_python {

  void wrap_grid_indices_around_sites();
  void wrap_grid_tags();
  void wrap_gridding();
  void wrap_misc();
  void wrap_peak_list();
  void wrap_pymol_interface();
  void wrap_statistics();
  void wrap_structure_factors();
  void wrap_coordinate_transformers();

  template <typename FloatType, typename GridType>
  struct map_accumulator_wrapper
  {
    //typedef map_accumulator<FloatType, GridType> w_t; // works too
    typedef cctbx::maptbx::map_accumulator<FloatType, GridType> w_t;
    static void wrap() {
      using namespace boost::python;
      class_<w_t>("map_accumulator", no_init)
        .def(init<af::int3 const&,
             double const&,
             double const&,
             int const&,
             bool,
             bool >((arg("n_real"),
                     arg("smearing_b"),
                     arg("max_peak_scale"),
                     arg("smearing_span"),
                     arg("use_exp_table"),
                     arg("use_max_map"))))
        .def("as_median_map", &w_t::as_median_map)
        .def("add", &w_t::add, (arg("map_data")))
        .def("at_index", &w_t::at_index, (arg("n")))
        .def("int_to_float_at_index", &w_t::int_to_float_at_index, (arg("n")))
      ;
    }
  };

  template <typename FloatType>
  struct ft_analytical_1d_point_scatterer_at_origin_wrapper
  {
    typedef ft_analytical_1d_point_scatterer_at_origin<FloatType> w_t;
    static void wrap() {
      using namespace boost::python;
      class_<w_t>("ft_analytical_1d_point_scatterer_at_origin", no_init)
        .def(init<int const& >(
                    (arg("N"))))
        .def("distances", &w_t::distances)
        .def("rho", &w_t::rho)
        .def("compute", &w_t::compute,
          (arg("miller_indices"),arg("step"),arg("left"),
            arg("right"),arg("u_frac")))
      ;
    }
  };

namespace {

  void init_module()
  {
    using namespace boost::python;

    map_accumulator_wrapper<double, af::c_grid<3> >::wrap();
    ft_analytical_1d_point_scatterer_at_origin_wrapper<double>::wrap();
    wrap_grid_indices_around_sites();
    wrap_grid_tags();
    wrap_gridding();
    wrap_misc();
    wrap_peak_list();
    wrap_pymol_interface();
    wrap_statistics();
    wrap_structure_factors();
    wrap_coordinate_transformers();

// Real-space target and gradients ---------------------------------------------
    {
      typedef cctbx::maptbx::target_and_gradients::diffmap::compute w_t;
      class_<w_t>("target_and_gradients_diffmap", no_init)
        .def(init<uctbx::unit_cell const&,
             af::const_ref<double, af::c_grid_padded<3> > const&,
             af::const_ref<double, af::c_grid_padded<3> > const&,
             double const&,
             af::const_ref<scitbx::vec3<double> > const& >((
                                    arg("unit_cell"),
                                    arg("map_target"),
                                    arg("map_current"),
                                    arg("step"),
                                    arg("sites_frac"))))
        .def("target", &w_t::target)
        .def("gradients", &w_t::gradients)
      ;
    }

    //- Magnification begin ----------------------------------------------------
    {
      typedef cctbx::maptbx::target_and_gradients::simple::magnification<double> w_t;
      class_<w_t>("target_and_gradients_simple_magnification", no_init)
        .def(init<uctbx::unit_cell const&,
             af::const_ref<double, af::c_grid_padded<3> > const&,
             af::const_ref<scitbx::vec3<double> > const&,
             scitbx::mat3<double> const& >((
                                    arg("unit_cell"),
                                    arg("map_target"),
                                    arg("sites_cart"),
                                    arg("K"))))
        .def(init<uctbx::unit_cell const&,
             af::const_ref<double, af::c_grid_padded<3> > const&,
             af::const_ref<scitbx::vec3<double> > const&,
             scitbx::vec3<double> const& >((
                                    arg("unit_cell"),
                                    arg("map_target"),
                                    arg("sites_cart"),
                                    arg("K"))))
        .def("target", &w_t::target)
        .def("gradients", &w_t::gradients)
      ;
    }

    def("magnification_isotropic",
      (double(*)
        (uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid_padded<3> > const&,
         af::const_ref<scitbx::vec3<double> > const&))
           cctbx::maptbx::target_and_gradients::simple::magnification_isotropic, (
             arg("unit_cell"),
             arg("density_map"),
             arg("sites_cart")));

    def("magnification_anisotropic",
      (scitbx::vec3<double>(*)
        (uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid_padded<3> > const&,
         af::const_ref<scitbx::vec3<double> > const&))
           cctbx::maptbx::target_and_gradients::simple::magnification_anisotropic, (
             arg("unit_cell"),
             arg("density_map"),
             arg("sites_cart")));
    //- Magnification end ------------------------------------------------------

    {
      typedef cctbx::maptbx::target_and_gradients::simple::compute<double> w_t;
      class_<w_t>("target_and_gradients_simple", no_init)
        .def(init<uctbx::unit_cell const&,
             af::const_ref<double, af::c_grid_padded<3> > const&,
             af::const_ref<scitbx::vec3<double> > const&,
             double const&,
             af::const_ref<bool> const& >((
                                    arg("unit_cell"),
                                    arg("map_target"),
                                    arg("sites_cart"),
                                    arg("delta"),
                                    arg("selection"))))
        .def(init<uctbx::unit_cell const&,
             af::const_ref<double, af::c_grid_padded<3> > const&,
             af::const_ref<scitbx::vec3<double> > const&,
             af::const_ref<bool> const&,
             std::string const& >((
                                    arg("unit_cell"),
                                    arg("map_target"),
                                    arg("sites_cart"),
                                    arg("selection"),
                                    arg("interpolation"))))
        .def("target", &w_t::target)
        .def("gradients", &w_t::gradients)
      ;
    }

    def("real_space_target_simple",
      (double(*)
        (uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid_padded<3> > const&,
         af::const_ref<scitbx::vec3<double> > const&,
         af::const_ref<bool> const&))
           cctbx::maptbx::target_and_gradients::simple::target, (
             arg("unit_cell"),
             arg("density_map"),
             arg("sites_cart"),
             arg("selection")));

    def("real_space_target_simple_with_adjacent_similarity",
      (double(*)
        (uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid_padded<3> > const&,
         af::const_ref<scitbx::vec3<double> > const&,
         af::const_ref<std::size_t > const&,
         af::const_ref<double> const&))
           cctbx::maptbx::target_and_gradients::simple::target_with_adjacent_similarity, (
             arg("unit_cell"),
             arg("density_map"),
             arg("sites_cart"),
             arg("selection"),
             arg("weights")));

    def("real_space_target_simple",
      (double(*)
        (uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid_padded<3> > const&,
         af::const_ref<scitbx::vec3<double> > const&))
           cctbx::maptbx::target_and_gradients::simple::target, (
             arg("unit_cell"),
             arg("density_map"),
             arg("sites_cart")));

    def("real_space_target_simple",
      (double(*)
        (uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid_padded<3> > const&,
         af::const_ref<scitbx::vec3<double> > const&,
         af::const_ref<std::size_t> const&))
           cctbx::maptbx::target_and_gradients::simple::target, (
             arg("unit_cell"),
             arg("density_map"),
             arg("sites_cart"),
             arg("selection")));

    def("real_space_target_simple",
      (double(*)
        (af::const_ref<double, af::c_grid<3> > const&,
         af::const_ref<scitbx::vec3<double> > const&))
           cctbx::maptbx::target_and_gradients::simple::target, (
             arg("density_map"),
             arg("sites_frac")));

    def("real_space_target_simple_per_site",
      (af::shared<double>(*)
        (uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid_padded<3> > const&,
         af::const_ref<scitbx::vec3<double> > const&))
           cctbx::maptbx::target_and_gradients::simple::target_per_site, (
             arg("unit_cell"),
             arg("density_map"),
             arg("sites_cart")));

    def("real_space_gradients_simple",
      (af::shared<scitbx::vec3<double> >(*)
        (uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid_padded<3> > const&,
         af::const_ref<scitbx::vec3<double> > const&,
         double,
         af::const_ref<bool> const&))
           cctbx::maptbx::target_and_gradients::simple::gradients, (
             arg("unit_cell"),
             arg("density_map"),
             arg("sites_cart"),
             arg("delta"),
             arg("selection")));

// -----------------------------------------------------------------------------

    {
      typedef grid_points_in_sphere_around_atom_and_distances w_t;

      class_<w_t>("grid_points_in_sphere_around_atom_and_distances", no_init)
        .def(init<cctbx::uctbx::unit_cell const&,
                  af::const_ref<double, af::c_grid<3> > const&,
                  double const&,
                  double const&,
                  scitbx::vec3<double> const& >(
                    (arg("unit_cell"),
                     arg("data"),
                     arg("radius"),
                     arg("shell"),
                     arg("site_frac"))))
        .def("data_at_grid_points", &w_t::data_at_grid_points)
        .def("data_at_grid_points_averaged", &w_t::data_at_grid_points_averaged)
        .def("distances", &w_t::distances)
      ;
    }

    {
      typedef non_linear_map_modification_to_match_average_cumulative_histogram w_t;

      class_<w_t>("non_linear_map_modification_to_match_average_cumulative_histogram", no_init)
        .def(init<af::const_ref<double, af::c_grid<3> > const&,
                  af::const_ref<double, af::c_grid<3> > const& >(
                    (arg("map_1"),
                     arg("map_2"))))
        .def("map_1", &w_t::map_1)
        .def("map_2", &w_t::map_2)
        .def("histogram_1", &w_t::histogram_1)
        .def("histogram_2", &w_t::histogram_2)
        .def("histogram_12", &w_t::histogram_12)
        .def("histogram_values", &w_t::histogram_values)
      ;
    }

    {
      typedef d99 w_t;

      class_<w_t>("d99", no_init)
        .def(init<
          af::const_ref<std::complex<double> > const&,
          af::const_ref<double> const&,
          af::const_ref<miller::index<> > const&,
          double const&,
          double const& >(
                    (arg("f"),
                     arg("d_spacings"),
                     arg("hkl"),
                     arg("d_min"),
                     arg("d_max"))))
        .def("d_min_cc9",      &w_t::d_min_cc9)
        .def("d_min_cc99",     &w_t::d_min_cc99)
        .def("d_min_cc999",    &w_t::d_min_cc999)
        .def("d_min_cc9999",   &w_t::d_min_cc9999)
        .def("d_min_cc99999",  &w_t::d_min_cc99999)
        .def("d_min_cc999999", &w_t::d_min_cc999999)
      ;
    }

    {
      typedef fsc w_t;

      class_<w_t>("fsc", no_init)
        .def(init<
          af::const_ref<std::complex<double> > const&,
          af::const_ref<std::complex<double> > const&,
          af::const_ref<double> const&,
          int const& >(
                    (arg("f1"),
                     arg("f2"),
                     arg("d_spacings"),
                     arg("step"))))
        .def("fsc",   &w_t::cc)
        .def("d",     &w_t::d)
        .def("d_inv", &w_t::d_inv)
      ;
    }

    {
      typedef histogram w_t;

      class_<w_t>("histogram", no_init)
        .def(init<af::const_ref<double, af::c_grid<3> > const&,
                  int const& >(
                    (arg("map"),
                     arg("n_bins"))))
        .def(init<af::const_ref<double> const&,
                  int const& >(
                    (arg("map"),
                     arg("n_bins"))))
        .def("values",    &w_t::values)
        .def("c_values",  &w_t::c_values)
        .def("v_values",  &w_t::v_values)
        .def("bin_width", &w_t::bin_width)
        .def("arguments", &w_t::arguments)
      ;
    }

    {
      typedef volume_scale w_t;

      class_<w_t>("volume_scale", no_init)
        .def(init<af::const_ref<double, af::c_grid<3> > const&,
                  int const& >(
                    (arg("map"),
                     arg("n_bins"))))
        .def("map_data", &w_t::map_data)
        .def("v_values", &w_t::v_values)
      ;
    }

    {
      typedef volume_scale_1d w_t;

      class_<w_t>("volume_scale_1d", no_init)
        .def(init<af::const_ref<double> const&,
                  int const& >(
                    (arg("map"),
                     arg("n_bins"))))
        .def("map_data", &w_t::map_data)
        .def("v_values", &w_t::v_values)
      ;
    }

    {
      typedef one_gaussian_peak_approximation w_t;

      class_<w_t>("one_gaussian_peak_approximation", no_init)
        .def(init<af::const_ref<double> const&,
                  af::const_ref<double> const&,
                  bool const&,
                  bool const& >(
                    (arg("data_at_grid_points"),
                     arg("distances"),
                     arg("use_weights"),
                     arg("optimize_cutoff_radius"))))
        .def("a_real_space", &w_t::a_real_space)
        .def("b_real_space", &w_t::b_real_space)
        .def("a_reciprocal_space", &w_t::a_reciprocal_space)
        .def("b_reciprocal_space", &w_t::b_reciprocal_space)
        .def("gof", &w_t::gof)
        .def("cutoff_radius", &w_t::cutoff_radius)
        .def("weight_power", &w_t::weight_power)
        .def("first_zero_radius", &w_t::first_zero_radius)
      ;
    }

    {
      typedef connectivity w_t;

      class_<w_t>("connectivity", no_init)
        .def(init<af::const_ref<float, af::flex_grid<> > const&,
                  float const&,
                  bool >(
                    (arg("map_data"),
                     arg("threshold"),
                     arg("wrapping")=true)))
        .def(init<af::const_ref<double, af::flex_grid<> > const&,
                  double const&,
                  bool >(
                    (arg("map_data"),
                     arg("threshold"),
                     arg("wrapping")=true)))
        .def(init<af::const_ref<int, af::flex_grid<> > const&,
                  int const&,
                  bool >(
                    (arg("map_data"),
                     arg("threshold"),
                     arg("wrapping")=true)))
        .def("result",    &w_t::result)
        .def("regions",   &w_t::regions)
        .def("volume_cutoff_mask", &w_t::volume_cutoff_mask,
                    (arg("volume_cutoff")))
        .def("get_blobs_boundaries", &w_t::get_blobs_boundaries)
        .def("expand_mask", &w_t::expand_mask,
                  (arg("id_to_expand"),
                   arg("expand_size")))
        .def("noise_elimination_two_cutoffs",
             &w_t::noise_elimination_two_cutoffs,
                    (arg("connectivity_object_at_t1"),
                     arg("elimination_volume_threshold_at_t1"),
                     arg("zero_all_interblob_region")=true))
        .def("maximum_coors", &w_t::maximum_coors)
        .def("maximum_values", &w_t::maximum_values)
      ;
    }
    {
      typedef marked_grid_points w_t;

      class_<w_t>("marked_grid_points", no_init)
        .def(init<af::const_ref<bool, af::flex_grid<> > const&,
                  int const&
                  >(
                    (arg("map_data"),
                     arg("every_nth_point")
                     )))
        .def("result",    &w_t::result)
      ;
    }

    {
      typedef sample_all_mask_regions w_t;
      class_<w_t>("sample_all_mask_regions", no_init)
        .def(init<af::const_ref<int, af::flex_grid<> > const&,
            af::shared<int> const&,
            af::shared<int> const&,
            cctbx::uctbx::unit_cell const& > ((
              arg("mask"),
              arg("volumes"),
              arg("sampling_rates"),
              arg("unit_cell"))))
        .def("get_array", &w_t::get_array,
          (arg("n")))
      ;
    }

    {
      typedef zero_boundary_box_map w_t;
      class_<w_t>("zero_boundary_box_map", no_init)
        .def(init<af::const_ref<double, af::flex_grid<> > const&,
            int const& > ((
              arg("mask"),
              arg("boundary"))))
        .def("result",    &w_t::result)
      ;
    }

    def("copy",
      (af::versa<float, af::flex_grid<> >(*)
        (af::const_ref<float, af::flex_grid<> > const&,
         af::flex_grid<> const&)) maptbx::copy, (
      arg("map"),
      arg("result_grid")));
    def("copy",
      (af::versa<double, af::flex_grid<> >(*)
        (af::const_ref<double, af::flex_grid<> > const&,
         af::flex_grid<> const&)) maptbx::copy, (
      arg("map"),
      arg("result_grid")));
    def("copy",
      (af::versa<float, af::flex_grid<> >(*)
        (af::const_ref<float, c_grid_padded_p1<3> > const&,
         af::int3 const&,
         af::int3 const&)) maptbx::copy, (
      arg("map_unit_cell"),
      arg("first"),
      arg("last")));
    def("copy",
      (af::versa<double, af::flex_grid<> >(*)
        (af::const_ref<double, c_grid_padded_p1<3> > const&,
         af::int3 const&,
         af::int3 const&)) maptbx::copy, (
      arg("map_unit_cell"),
      arg("first"),
      arg("last")));
    def("copy_box",
      (af::versa<float, af::flex_grid<> >(*)
        (af::const_ref<float, af::flex_grid<> > const&,
         af::int3 const&,
         af::int3 const&)) maptbx::copy_box, (
      arg("map"),
      arg("first"),
      arg("last")));
    def("copy_box",
      (af::versa<double, af::flex_grid<> >(*)
        (af::const_ref<double, af::flex_grid<> > const&,
         af::int3 const&,
         af::int3 const&)) maptbx::copy_box, (
      arg("map"),
      arg("first"),
      arg("last")));
    def("unpad_in_place",
      (void(*)(af::versa<float, af::flex_grid<> >&))
        maptbx::unpad_in_place, (arg("map")));
    def("unpad_in_place",
      (void(*)(af::versa<double, af::flex_grid<> >&))
        maptbx::unpad_in_place, (arg("map")));

    def("fft_to_real_map_unpadded",
      (af::versa<double, af::c_grid<3> >(*)(
        sgtbx::space_group const&,
        af::tiny<int, 3> const&,
        af::const_ref<miller::index<> > const&,
        af::const_ref<std::complex<double> > const&))
          maptbx::fft_to_real_map_unpadded, (
            arg("space_group"),
            arg("n_real"),
            arg("miller_indices"),
            arg("data")));

    def("direct_summation_at_point",
      (std::complex<double>(*)(
        af::const_ref<miller::index<> > const&,
        af::const_ref<std::complex<double> > const&,
        scitbx::vec3<double>))
          maptbx::direct_summation_at_point, (
            arg("miller_indices"),
            arg("data"),
            arg("site_frac")));

    def("cc_weighted_maps",cc_weighted_maps);

    def("kuwahara_filter",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         int const&)) kuwahara_filter, (
      arg("map_data"),
      arg("index_span")));

    def("median_filter",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         int const&)) median_filter, (
      arg("map_data"),
      arg("index_span")));

    def("remove_single_node_peaks",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         af::ref<double, af::c_grid_padded<3> >,
         double const&,
         int const&)) remove_single_node_peaks, (
      arg("map_data"),
      arg("mask_data"),
      arg("cutoff"),
      arg("index_span")));

    def("map_box_average",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         double const&,
         int const&)) map_box_average, (
      arg("map_data"),
      arg("cutoff"),
      arg("index_span")));

    def("map_box_average",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         int const&)) map_box_average, (
      arg("map_data"),
      arg("index_span")));

    def("center_of_mass",
      (cctbx::cartesian<>(*)
        (af::const_ref<double, af::c_grid<3> > const&,
         uctbx::unit_cell const&,
         double const&)) center_of_mass, (
      arg("map_data"),
      arg("unit_cell"),
      arg("cutoff")));

    def("fit_point_3d_grid_search",
      (cctbx::cartesian<>(*)
        (cctbx::cartesian<> const&,
         af::const_ref<double, af::c_grid<3> > const&,
         uctbx::unit_cell const&,
         double const&,
         double const&)) fit_point_3d_grid_search, (
      arg("site_cart"),
      arg("map_data"),
      arg("unit_cell"),
      arg("amplitude"),
      arg("increment")));

    def("sharpen",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         int const&,
         int const&,
         bool)) sharpen, (
      arg("map_data"),
      arg("index_span"),
      arg("n_averages"),
      arg("allow_negatives")));

    def("gamma_compression",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         double const&)) gamma_compression, (
      arg("map_data"),
      arg("gamma")));

    def("map_box_average",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         cctbx::uctbx::unit_cell const&,
         double const&)) map_box_average, (
      arg("map_data"),
      arg("unit_cell"),
      arg("radius")));

    def("hoppe_gassman_modification",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         double,
         int)) hoppe_gassman_modification, (
      arg("data"),
      arg("mean_scale"),
      arg("n_iterations")));

    def("hoppe_gassman_modification2",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         int)) hoppe_gassman_modification2, (
      arg("data"),
      arg("n_iterations")));

    def("sphericity_tensor",
      (scitbx::sym_mat3<double>(*)
        (af::const_ref<double, af::c_grid<3> > const&,
          uctbx::unit_cell const&,
         double const&,
         cctbx::fractional<> const&)) sphericity_tensor, (
      arg("map_data"),
      arg("unit_cell"),
      arg("radius"),
      arg("site_frac")));

    def("sphericity",
      (af::shared<double>(*)
        (af::const_ref<double, af::c_grid<3> > const&,
          uctbx::unit_cell const&,
         double const&,
         af::const_ref<scitbx::vec3<double> > const&)) sphericity, (
      arg("map_data"),
      arg("unit_cell"),
      arg("radius"),
      arg("sites_frac")));

    def("average_densities",
      (af::shared<double>(*)
        (uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid<3> > const&,
         af::const_ref<scitbx::vec3<double> > const&,
         float)) average_densities, (
      arg("unit_cell"),
      arg("data"),
      arg("sites_frac"),
      arg("radius")));

    def("mask",
      (af::versa<double, af::c_grid<3> >(*)
        (af::const_ref<scitbx::vec3<double> > const&,
          uctbx::unit_cell const&,
          af::tiny<int, 3> const&,
          double const&,
          double const&,
          af::const_ref<double> const&,
          bool const&)) mask, (
      arg("sites_frac"),
      arg("unit_cell"),
      arg("n_real"),
      arg("mask_value_inside_molecule"),
      arg("mask_value_outside_molecule"),
      arg("radii"),
      arg("wrapping")=true
       ));

    def("convert_to_non_negative",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         double)) convert_to_non_negative, (
      arg("data"),
      arg("substitute_value")));

    def("flexible_boundary_mask",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         af::ref<double, af::c_grid<3> >)) flexible_boundary_mask, (
      arg("data"),
      arg("mask")));

    def("negate_selected_in_place",
      (af::versa<double, af::c_grid_padded<3> >(*)
        (af::const_ref<double, af::c_grid_padded<3> > const&,
         std::vector<unsigned> const&)) negate_selected_in_place, (
      arg("map_data"),
      arg("selection")));

    def("reset",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         double,
         double,
         double,
         bool)) reset, (
      arg("data"),
      arg("substitute_value"),
      arg("less_than_threshold"),
      arg("greater_than_threshold"),
      arg("use_and")));

    def("intersection",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         af::ref<double, af::c_grid<3> >,
         af::ref<double> const&,
         bool)) intersection, (
      arg("map_data_1"),
      arg("map_data_2"),
      arg("thresholds"),
      arg("average")));

    def("rotate_translate_map",
      (af::versa<double, af::c_grid<3> >(*)
        (uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid<3> > const&,
         scitbx::mat3<double> const&,
         scitbx::vec3<double> const&,
         af::tiny<int, 3> const&,
         af::tiny<int, 3> const&)) rotate_translate_map, (
      arg("unit_cell"),
      arg("map_data"),
      arg("rotation_matrix"),
      arg("translation_vector"),
      arg("start"),
      arg("end")));

    def("rotate_translate_map",
      (af::versa<double, af::c_grid<3> >(*)
        (uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid<3> > const&,
         scitbx::mat3<double> const&,
         scitbx::vec3<double> const& )) rotate_translate_map, (
      arg("unit_cell"),
      arg("map_data"),
      arg("rotation_matrix"),
      arg("translation_vector")));

    def("superpose_maps",
      (af::versa<double, af::c_grid<3> >(*)
        (uctbx::unit_cell const&,
         uctbx::unit_cell const&,
         af::const_ref<double, af::c_grid<3> > const&,
         af::tiny<int, 3> const&,
         scitbx::mat3<double> const&,
         scitbx::vec3<double> const&,
         bool wrap )) superpose_maps, (
      arg("unit_cell_1"),
      arg("unit_cell_2"),
      arg("map_data_1"),
      arg("n_real_2"),
      arg("rotation_matrix"),
      arg("translation_vector"),
      arg("wrapping")=true));

    def("combine_and_maximize_maps",
      (af::versa<double, af::c_grid<3> >(*)
        (af::const_ref<double, af::c_grid<3> > const&,
         af::const_ref<double, af::c_grid<3> > const&,
         af::tiny<int, 3> const& )) combine_and_maximize_maps, (
      arg("map_data_1"),
      arg("map_data_2"),
      arg("n_real")));

    def("denmod_simple",
      (af::versa<double, af::c_grid<3> >(*)
        (af::const_ref<double, af::c_grid<3> > const&,
         af::tiny<int, 3> const&,
         double,double)) denmod_simple, (
      arg("map_data"),
      arg("n_real"),
      arg("cutoffp"),
      arg("cutoffm")));

    def("binarize",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         double const&,
         double const&,
         double const&)) binarize, (
      arg("map_data"),
      arg("threshold"),
      arg("substitute_value_below"),
      arg("substitute_value_above")));

    def("truncate_special",
      (void(*)
        (af::ref<int, af::c_grid<3> >,
         af::ref<double, af::c_grid<3> >)) truncate_special, (
      arg("mask"),
      arg("map_data")));

    def("combine_1",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         af::ref<double, af::c_grid<3> >)) combine_1, (
      arg("map_data"),
      arg("diff_map")));

    def("truncate_between_min_max",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         double const&,
         double const&)) truncate_between_min_max, (
      arg("map_data"),
      arg("min"),
      arg("max")));

    def("truncate",
      (void(*)
        (af::ref<double, af::c_grid<3> >,
         double const&,
         double const&,
         double const&,
         double const&)) truncate, (
      arg("map_data"),
      arg("standard_deviation"),
      arg("by_sigma_less_than"),
      arg("scale_by"),
      arg("set_value")));

    def("fem_averaging_loop",
      (af::shared<std::complex<double> >(*)
        (af::const_ref<std::complex<double> > const&,
         af::const_ref<double> const&,
         af::const_ref<double> const&,
         double const&,
         int const&,
         int const&)) fem_averaging_loop, (
      arg("map_coefficients"),
      arg("r_factors"),
      arg("sigma_over_f_obs"),
      arg("random_scale"),
      arg("random_seed"),
      arg("n_cycles")));

    def("conditional_solvent_region_filter",
      (af::versa<double, af::c_grid<3> >(*)
        (af::const_ref<double, af::c_grid_padded<3> > const&,
         af::const_ref<double, af::c_grid<3> > const&,
         double const&)) conditional_solvent_region_filter, (
      arg("bulk_solvent_mask"),
      arg("map_data"),
      arg("threshold")));

    def("update_f_part1_helper",
      (af::versa<double, af::c_grid<3> >(*)
        (af::const_ref<int, af::c_grid_padded<3> > const&,
         af::const_ref<double, af::c_grid<3> > const&,
         int const&)) update_f_part1_helper, (
      arg("connectivity_map"),
      arg("map_data"),
      arg("region_id")));

    def("map_sum_at_sites_frac",
      (double(*)
        (af::const_ref<double, af::c_grid<3> > const&,
         af::const_ref<scitbx::vec3<double> > const&)) map_sum_at_sites_frac, (
      arg("map_data"),
      arg("sites_frac")));

    def("discrepancy_function",
      (af::shared<double>(*)
        (af::const_ref<double> const&,
         af::const_ref<double> const&,
         af::const_ref<double> const&)) discrepancy_function, (
      arg("map_1"),
      arg("map_2"),
      arg("cutoffs")));

    def("discrepancy_function",
      (af::shared<double>(*)
        (af::const_ref<double, af::c_grid<3> > const&,
         af::const_ref<double, af::c_grid<3> > const&,
         af::const_ref<double> const&)) discrepancy_function, (
      arg("map_1"),
      arg("map_2"),
      arg("cutoffs")));

    def("cc_complex_complex",
      (af::shared<double>(*)
        (af::const_ref<std::complex<double> > const&,
         af::const_ref<std::complex<double> > const&,
         af::const_ref<double> const&,
         af::const_ref<double> const&,
         af::const_ref<double> const&,
         double const&)) cc_complex_complex, (
      arg("f_1"),
      arg("f_2"),
      arg("d_spacings"),
      arg("ss"),
      arg("d_mins"),
      arg("b_iso")));

    def("cc_complex_complex",
      (double(*)
        (af::const_ref<std::complex<double> > const&,
         af::const_ref<std::complex<double> > const&)) cc_complex_complex, (
      arg("f_1"),
      arg("f_2")));

    def("cc_peak",
      (double(*)
        (af::const_ref<double, af::c_grid<3> > const&,
         af::const_ref<double, af::c_grid<3> > const&,
         double const&)) cc_peak, (
      arg("map_1"),
      arg("map_2"),
      arg("cutoff")));

    def("set_box_copy",
      (af::versa<double, af::c_grid<3> >(*)
        (double const&,
         af::ref<double, af::c_grid<3> >,
         af::tiny<int, 3> const&,
         af::tiny<int, 3> const&)) set_box_copy, (
      arg("value"),
      arg("map_data_to"),
      arg("start"),
      arg("end")));

    def("set_box",
      (void(*)
        (double const&,
         af::ref<double, af::c_grid<3> >,
         af::tiny<int, 3> const&,
         af::tiny<int, 3> const&)) set_box, (
      arg("value"),
      arg("map_data_to"),
      arg("start"),
      arg("end")));

    def("set_box",
      (void(*)
        (af::const_ref<double, af::c_grid<3> > const&,
         af::ref<double, af::c_grid<3> >,
         af::tiny<int, 3> const&,
         af::tiny<int, 3> const&)) set_box, (
      arg("map_data_from"),
      arg("map_data_to"),
      arg("start"),
      arg("end")));

    def("copy_box",
      (void(*)
        (af::const_ref<double, af::c_grid<3> > const&,
         af::ref<double, af::c_grid<3> >,
         af::tiny<int, 3> const&,
         af::tiny<int, 3> const&)) copy_box, (
      arg("map_data_from"),
      arg("map_data_to"),
      arg("start"),
      arg("end")));

    def("eight_point_interpolation",
      (double(*)
        (af::const_ref<double, af::flex_grid<> > const&,
         scitbx::vec3<double> const&)) eight_point_interpolation);
    def("eight_point_interpolation",
      (double(*)
        (af::const_ref<double, af::c_grid_padded<3> > const&,
         scitbx::vec3<double> const&)) eight_point_interpolation);
    def("eight_point_interpolation",
      (double(*)
        (af::const_ref<double, af::c_grid<3> > const&,
         scitbx::vec3<double> const&)) eight_point_interpolation);
    def("eight_point_interpolation_with_gradients",
      (af::tiny<double, 4>(*)
        (af::const_ref<double, af::c_grid_padded<3> > const&,
         scitbx::vec3<double> const&,
         scitbx::vec3<double> const&)) eight_point_interpolation_with_gradients);
    def("quadratic_interpolation_with_gradients",
      (af::tiny<double, 4>(*)
        (//af::const_ref<double, af::flex_grid<> > const&,
         af::const_ref<double, af::c_grid_padded<3> > const&,
         scitbx::vec3<double> const&,
         scitbx::vec3<double> const&)) quadratic_interpolation_with_gradients);
    def("closest_grid_point",
      (af::c_grid_padded<3>::index_type(*)
        (af::flex_grid<> const&,
         fractional<double> const&)) closest_grid_point);
    def("tricubic_interpolation_with_gradients",
      (af::tiny<double, 4>(*)
        (af::const_ref<double, af::c_grid_padded<3> > const&,
         scitbx::vec3<double> const&,
         scitbx::vec3<double> const&)) tricubic_interpolation_with_gradients);
    def("tricubic_interpolation",
      (double(*)
        (af::const_ref<double, af::c_grid_padded<3> > const&,
         scitbx::vec3<double> const&)) tricubic_interpolation);
    def("non_crystallographic_eight_point_interpolation",
      (double(*)
        (af::const_ref<double, af::flex_grid<> > const&,
         scitbx::mat3<double> const&,
         scitbx::vec3<double> const&,
         bool,
         double const&))
           non_crystallographic_eight_point_interpolation, (
             arg("map"),
             arg("gridding_matrix"),
             arg("site_cart"),
             arg("allow_out_of_bounds")=false,
             arg("out_of_bounds_substitute_value")=0));
    def("asu_eight_point_interpolation",
      (double(*)
        (af::const_ref<double, af::flex_grid<> > const&,
         crystal::direct_space_asu::asu_mappings<double> &,
         fractional<double> const&)) asu_eight_point_interpolation);

    def("standard_deviations_around_sites",
      standard_deviations_around_sites, (
        arg("unit_cell"),
        arg("density_map"),
        arg("sites_cart"),
        arg("site_radii")));
  }

} // namespace <anonymous>
}}} // namespace cctbx::maptbx::boost_python

BOOST_PYTHON_MODULE(cctbx_maptbx_ext)
{
  cctbx::maptbx::boost_python::init_module();
}


/* Copyright (C) 2012-2020 IBM Corp.
 * This program 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. See accompanying LICENSE file.
 */
/**
 * @file binaryCompare.cpp
 * @brief Implementing integer comparison in binary representation.
 */
#include <algorithm>

#include <NTL/BasicThreadPool.h>
#include <helib/binaryArith.h>

#define BPL_ESTIMATE (30)
// FIXME: this should really be dynamic

#ifdef HELIB_DEBUG
#include <helib/debugging.h>
#endif

namespace helib {

// returns new v[i] = \sum_{j>=i} old v[i]
void runningSums(CtPtrs& v)
{
  HELIB_TIMER_START;
  for (long i = lsize(v) - 1; i > 0; i--)
    *v[i - 1] += *v[i];
}

// a recursive function that computes
//      e*[i] = prod_{j>=i} e[i]  and  g*[i] = e*[i+1] \cdot g[i]
// This function is optimized, so that instead of all the e*[i]'s
// it only computes e*[0] and the e*[i]'s that are used in the
// computation of the g*[i]'d
static void compProducts(const CtPtrs_slice& e, const CtPtrs_slice& g)
{
  long n = lsize(e);
  if (n <= 1)
    return; // nothing to do
#ifdef HELIB_DEBUG
  std::cout << "compProducts(g[" << g.start << ".." << (g.start + g.sz - 1)
            << "],e[" << e.start << ".." << (e.start + e.sz - 1) << "])"
            << std::endl;
#endif

  // split the array in two, second part has size the largest 2^l < n,
  // and first part is the rest

  long ell = NTL::NumBits(n - 1) - 1; // n/2 <= 2^l < n
  long n1 = n - (1UL << ell);         // n1 \in [1, n/2]

  // Call the recursive procedure separately on the first and second parts
  compProducts(CtPtrs_slice(e, 0, n1), CtPtrs_slice(g, 0, n1)); // first half
  compProducts(CtPtrs_slice(e, n1, n - n1),
               CtPtrs_slice(g, n1, n - n1)); // second half

  // Multiply the first product in the 2nd part into every product in the 1st
  NTL_EXEC_RANGE(1 + n1, first, last)
  for (long i = first; i < last; i++) {
    if (i == 0)
      e[0]->multiplyBy(*e[n1]);
    else if (i - 1 < g.size())
      g[i - 1]->multiplyBy(*e[n1]);
  }
  NTL_EXEC_RANGE_END
#ifdef HELIB_DEBUG
  std::cout << " g[" << g.start << ".." << (g.start + g.sz - 1) << "], "
            << " e[" << e.start << ".." << (e.start + e.sz - 1)
            << "]:" << std::endl;
  for (long i = 0; i < g.size(); i++)
    decryptAndPrint((std::cout << "   g[" << (i + g.start) << "] ("
                               << ((void*)g[i]) << "): "),
                    *g[i],
                    *dbgKey,
                    *dbgEa,
                    FLAG_PRINT_POLY);
  for (long i = 0; i < e.size(); i++)
    decryptAndPrint((std::cout << "   e[" << (i + e.start) << "] ("
                               << ((void*)e[i]) << "): "),
                    *e[i],
                    *dbgKey,
                    *dbgEa,
                    FLAG_PRINT_POLY);

  std::cout << std::endl;
#endif
}

// Compute aeqb[i] = (a==b upto bit i), agtb[i] = (aeqb[i+1] and ai>bi)
// We assume that b.size()>a.size()
static void compEqGt(CtPtrs& aeqb,
                     CtPtrs& agtb,
                     const CtPtrs& a,
                     const CtPtrs& b)
{
  HELIB_TIMER_START;
  const Ctxt zeroCtxt(ZeroCtxtLike, *(b.ptr2nonNull()));
  const Context& context = zeroCtxt.getContext();
  DoubleCRT one(context, context.allPrimes());
  one += 1L;

  resize(aeqb, lsize(b), zeroCtxt);
  resize(agtb, lsize(a), zeroCtxt);

  // First compute the local bits e[i]=(a[i]==b[i]), gt[i]=(a[i]>b[i])
  HELIB_NTIMER_START(compEqGt1);
  long aSize = lsize(a);
  NTL_EXEC_RANGE(aSize, first, last)
  for (long i = first; i < last; i++) {
    *aeqb[i] = *b[i];               // b
    aeqb[i]->addConstant(one, 1.0); // b+1
    *agtb[i] = *aeqb[i];            // b+1
    *aeqb[i] += *a[i];              // a+b+1
    agtb[i]->multiplyBy(*a[i]);     // a(b+1)
  }
  NTL_EXEC_RANGE_END
  HELIB_NTIMER_STOP(compEqGt1);

  // NOTE: Usually there isn't much gain in multi-threading the loop below,
  //    but computing b[i] can be expensive in some implementations of CtPtrs
  HELIB_NTIMER_START(compEqGt2);
  if (lsize(b) - aSize > 1) {
    NTL_EXEC_RANGE(lsize(b) - aSize, first, last)
    for (long i = first; i < last; i++) {
      *aeqb[i + aSize] = *b[i + aSize];       // b
      aeqb[i + aSize]->addConstant(one, 1.0); // b+1
    }
    NTL_EXEC_RANGE_END
  } else if (lsize(b) - aSize == 1) {
    *aeqb[aSize] = *b[aSize];           // b
    aeqb[aSize]->addConstant(one, 1.0); // b+1
  }
  HELIB_NTIMER_STOP(compEqGt2);

#ifdef HELIB_DEBUG
  for (long i = 0; i < lsize(b); i++)
    decryptAndPrint((std::cout << " e[" << i << "]: "),
                    *aeqb[i],
                    *dbgKey,
                    *dbgEa,
                    FLAG_PRINT_POLY);
  for (long i = 0; i < lsize(a); i++)
    decryptAndPrint((std::cout << " ag[" << i << "]: "),
                    *agtb[i],
                    *dbgKey,
                    *dbgEa,
                    FLAG_PRINT_POLY);
  std::cout << std::endl;
#endif

  // Call a recursive function to compute:
  // e*_i = \prod_{j>=i} aeqb_i, g*_i = aeqb*_{i+1} \cdot agtb_i
  HELIB_NTIMER_START(compEqGt3);
  compProducts(CtPtrs_slice(aeqb, 0), CtPtrs_slice(agtb, 0));
  runningSums(agtb); // now ag[i] = (a>b upto bit i)
  HELIB_NTIMER_STOP(compEqGt3);
}

// Compares two integers in binary a,b.
// Returns max(a,b), min(a,b) and indicator bits mu=(a>b) and ni=(a<b)
void compareTwoNumbersImplementation(CtPtrs& max,
                                     CtPtrs& min,
                                     Ctxt& mu,
                                     Ctxt& ni,
                                     const CtPtrs& aa,
                                     const CtPtrs& bb,
                                     bool twosComplement,
                                     std::vector<zzX>* unpackSlotEncoding,
                                     bool cmp_only)
{
  HELIB_TIMER_START;
  // make sure that lsize(b) >= lsize(a)
  const CtPtrs& a = (lsize(bb) >= lsize(aa)) ? aa : bb;
  const CtPtrs& b = (lsize(bb) >= lsize(aa)) ? bb : aa;
  long aSize = lsize(a);
  long bSize = lsize(b);
  if (aSize < 1) { // a is empty
    mu.clear();
    ni.clear();
    ni.addConstant(NTL::ZZ(1L));
    vecCopy(max, b);
    setLengthZero(min);
    return;
  }

  // Check that we have enough levels, try to bootstrap otherwise
  if (findMinBitCapacity({&a, &b}) <
      (NTL::NumBits(bSize + 1) + 2) * mu.getContext().BPL())
    packedRecrypt(a, b, unpackSlotEncoding);
  if (findMinBitCapacity({&a, &b}) <
      (NTL::NumBits(bSize) + 1) * mu.getContext().BPL())
    // the bare minimum
    throw LogicError("not enough levels for comparison");

  // NOTE: this procedure minimizes the number of multiplications,
  //       but it may use one level too many. Can we optimize it?

  /* We first compute for each position i the values
   *   e[i] = (a==b upto position i)
   *   ag[i] = (a>b upto position i)
   */

  // We use max, min to hold the intermediate values e, ag
  CtPtrs& e = max;
  CtPtrs& ag = min;
  compEqGt(e, ag, a, b);

  // We are now ready to compute the bits of the result.

  HELIB_NTIMER_START(compResults);
  mu = *ag[0]; // a > b
  ni = *ag[0];
  ni.addConstant(NTL::ZZ(1L)); // a <= b
  ni += *e[0];                 // a < b

  if (twosComplement) {
    // mu, ni and ag need to be inverted iff the sign bits of a and b differ.
    // Perform this by adding both sign bits.
    const auto flipIfDifferentSign = [&](Ctxt& ctxt) {
      (ctxt += *aa[aa.size() - 1]) += *bb[bb.size() - 1];
    };
    flipIfDifferentSign(mu);
    flipIfDifferentSign(ni);
    for (long i = 0; i < ag.size(); ++i)
      flipIfDifferentSign(*ag[i]);
  }
  if (cmp_only) {
    return;
  }

  NTL_EXEC_RANGE(aSize, first, last)
  for (long i = first; i < last; i++) {
    *max[i] = *a[i];
    *max[i] -= *b[i];
    max[i]->multiplyBy(*ag[i]);

    *min[i] = *max[i];
    *max[i] += *b[i];
    *min[i] -= *a[i];
  }
  NTL_EXEC_RANGE_END
  for (long i = aSize; i < bSize; i++)
    *max[i] = *b[i];
  HELIB_NTIMER_STOP(compResults);
}

void compareTwoNumbers(CtPtrs& max,
                       CtPtrs& min,
                       Ctxt& mu,
                       Ctxt& ni,
                       const CtPtrs& aa,
                       const CtPtrs& bb,
                       bool twosComplement,
                       std::vector<zzX>* unpackSlotEncoding)
{
  compareTwoNumbersImplementation(max,
                                  min,
                                  mu,
                                  ni,
                                  aa,
                                  bb,
                                  twosComplement,
                                  unpackSlotEncoding,
                                  false);
}

void compareTwoNumbers(Ctxt& mu,
                       Ctxt& ni,
                       const CtPtrs& aa,
                       const CtPtrs& bb,
                       bool twosComplement,
                       std::vector<zzX>* unpackSlotEncoding)
{
  NTL::Vec<Ctxt> aeqb;
  NTL::Vec<Ctxt> agtb;
  CtPtrs_VecCt eq(aeqb);
  CtPtrs_VecCt gr(agtb);
  compareTwoNumbersImplementation(eq,
                                  gr,
                                  mu,
                                  ni,
                                  aa,
                                  bb,
                                  twosComplement,
                                  unpackSlotEncoding,
                                  true);
}

} // namespace helib


#include "gaussfft.hpp"

#include <boost/python/numpy.hpp>

//#include <chrono>
#include <iostream>

#include "nrlib/iotools/stringtools.hpp"
#include "nrlib/grid/grid.hpp"
#include "nrlib/grid/grid2d.hpp"
#include "nrlib/math/constants.hpp"
#include "nrlib/random/random.hpp"
#include "nrlib/variogram/variogram.hpp"
#include "nrlib/variogram/gaussianfield.hpp"

namespace bp = boost::python;

/***************************/
std::string GaussFFT::Quote()
{
  return "Arc, amplitude, and curvature sustain a similar relation to each other as time, motion, and velocity, or as volume, mass, and density.";
}

/**********************************************************************************/
std::vector<size_t> GaussFFT::FindGridSizeAfterPadding(NRLib::Variogram * variogram,
                                                       size_t             nx,
                                                       double             dx,
                                                       size_t             ny,
                                                       double             dy,
                                                       size_t             nz,
                                                       double             dz)
{
  std::vector<size_t> out = NRLib::FindNDimPadding(*variogram,
                                                   nx,
                                                   dx,
                                                   ny,
                                                   dy,
                                                   nz,
                                                   dz);
  // Add simulation grid size to that output reflects final grid size
  // and not padding only
  out[0] += nx;
  if (out.size() > 1) {
    out[1] += ny;
    if (out.size() > 2) {
      out[2] += nz;
    }
  }
  return out;
}

/*********************************************************************/
NRLib::Variogram * GaussFFT::CreateVariogram(const std::string & type,
                                             double              range_x,
                                             double              range_y,
                                             double              range_z,
                                             double              azimuth_angle,
                                             double              dip_angle,
                                             double              power)
{
  if (range_y < 0.0)
    range_y = range_x;
  if (range_z < 0.0)
    range_z = range_x;

  azimuth_angle *= NRLib::Degree;
  dip_angle *= NRLib::Degree;

  NRLib::Variogram::Type t;
  std::string utype = NRLib::Uppercase(type);
  if (utype == "CONSTANT")
    t = NRLib::Variogram::CONSTANT;
  else if (utype == "EXPONENTIAL")
    t = NRLib::Variogram::EXPONENTIAL;
  else if (utype == "GAUSSIAN")
    t = NRLib::Variogram::GAUSSIAN;
  else if (utype == "GENERAL_EXPONENTIAL")
    t = NRLib::Variogram::GENERAL_EXPONENTIAL;
  else if (utype == "MATERN32")
    t = NRLib::Variogram::MATERN32;
  else if (utype == "MATERN52")
    t = NRLib::Variogram::MATERN52;
  else if (utype == "MATERN72")
    t = NRLib::Variogram::MATERN72;
  else if (utype == "SPHERICAL")
    t = NRLib::Variogram::SPHERICAL;
  else
    t = NRLib::Variogram::SPHERICAL;

  return NRLib::Variogram::Create(t, power, range_x, range_y, range_z, azimuth_angle, dip_angle, 1.0);
}

/******************************************************************/
bp::numpy::ndarray  GaussFFT::Simulate(NRLib::Variogram * variogram,
                                       size_t             nx,
                                       double             dx,
                                       size_t             ny,
                                       double             dy,
                                       size_t             nz,
                                       double             dz)
{
  return SimulateWithAdvancedSettings(variogram, nx, dx, ny, dy, nz, dz, -1, -1,-1, 1.0, 1.0, 1.0);
}

/***********************************************************************************/
bp::numpy::ndarray  GaussFFT::SimulateWithAdvancedSettings(NRLib::Variogram * variogram,
                                                           size_t             nx,
                                                           double             dx,
                                                           size_t             ny,
                                                           double             dy,
                                                           size_t             nz,
                                                           double             dz,
                                                           int                padding_x,
                                                           int                padding_y,
                                                           int                padding_z,
                                                           double             scaling_x,
                                                           double             scaling_y,
                                                           double             scaling_z)
{
  try {
    NRLib::Random::GetStartSeed();
  }
  catch (NRLib::Exception e) {
    // NRLib::Random is not initialized yet. Use empty initializer:
    NRLib::Random::Initialize();
  }
  std::vector<double> result;
  if (ny <= 1U || dy < 0.0) {
    result = GaussFFT::Simulate1D(variogram, nx, dx,                 padding_x,                       scaling_x                      );
  }
  else if (nz <= 1U || dz < 0.0) {
    result = GaussFFT::Simulate2D(variogram, nx, dx, ny, dy,         padding_x, padding_y,            scaling_x, scaling_y           );
  }
  else {
    result = GaussFFT::Simulate3D(variogram, nx, dx, ny, dy, nz, dz, padding_x, padding_y, padding_z, scaling_x, scaling_y, scaling_z);
  }

  bp::numpy::dtype dt = bp::numpy::dtype::get_builtin<double>();
  bp::tuple shape = bp::make_tuple(result.size());
  bp::tuple stride = bp::make_tuple(sizeof(double));
  bp::object owner;
  bp::numpy::ndarray np_result = bp::numpy::from_data(&result[0], dt, shape, stride, owner);
  return np_result.copy();
}

/********************************************************************/
std::vector<double> GaussFFT::Simulate1D(NRLib::Variogram * variogram,
                                         size_t             nx,
                                         double             dx,
                                         int                padding_x,
                                         double             scaling_x)
{
  std::vector<double> values;
  // Have to allocate memory (for some reason) before running the simulation
  values.resize(nx);
  NRLib::Simulate1DGaussianField(*variogram,
                                 nx,
                                 dx,
                                 values,
                                 NULL, // Will use NRLib::Random state
                                 padding_x,
                                 scaling_x);
  return values;
}

/********************************************************************/
std::vector<double> GaussFFT::Simulate2D(NRLib::Variogram * variogram,
                                         size_t             nx,
                                         double             dx,
                                         size_t             ny,
                                         double             dy,
                                         int                padding_x,
                                         int                padding_y,
                                         double             scaling_x,
                                         double             scaling_y)
{
  std::vector<NRLib::Grid2D<double> > fields;
  NRLib::Simulate2DGaussianField(*variogram,
                                 nx,
                                 dx,
                                 ny,
                                 dy,
                                 1,
                                 fields,
                                 NULL, // Will use NRLib::Random state
                                 padding_x,
                                 padding_y,
                                 scaling_x,
                                 scaling_y);
  return fields[0].GetStorage();
}

/*********************************************************************/
std::vector<double> GaussFFT::Simulate3D(NRLib::Variogram * variogram,
                                         size_t             nx,
                                         double             dx,
                                         size_t             ny,
                                         double             dy,
                                         size_t             nz,
                                         double             dz,
                                         int                padding_x,
                                         int                padding_y,
                                         int                padding_z,
                                         double             scaling_x,
                                         double             scaling_y,
                                         double             scaling_z)
{
  std::vector<NRLib::Grid<double> > fields;
  NRLib::Simulate3DGaussianField(*variogram,
                                 nx,
                                 dx,
                                 ny,
                                 dy,
                                 nz,
                                 dz,
                                 1,
                                 fields,
                                 padding_x,
                                 padding_y,
                                 padding_z,
                                 scaling_x,
                                 scaling_y,
                                 scaling_z);
  return fields[0].GetStorage();
}


#include "comfi.hpp"
#include <armadillo>
#include "viennacl/matrix.hpp"
#include "viennacl/matrix_proxy.hpp"
#include "viennacl/forwards.h"

using namespace arma;

vcl_mat comfi::operators::jm1(const vcl_mat &xn, comfi::types::Context ctx) {
  //make sure the dimension exists
  if ((ctx.bc_down == comfi::types::DIMENSIONLESS) ||
      (ctx.nz == 1)) {
    return xn;
  }
  //shift values
  vcl_mat xn_jm1(xn.size1(), xn.size2());
  viennacl::range eq(0, ctx.num_of_eq);
  static viennacl::range jm1(0, ctx.num_of_grid()-ctx.nx);
  static viennacl::range j(ctx.nx, ctx.num_of_grid());
  viennacl::project(xn_jm1, j, eq) = viennacl::project(xn, jm1, eq);

  //boundary conditions
  if (ctx.bc_down == comfi::types::NEUMANN) {
    static viennacl::range edge(0, ctx.nx);
    viennacl::project(xn_jm1, edge, eq) = viennacl::project(xn, edge, eq);
  }
  if (ctx.bc_down == comfi::types::PERIODIC) {
    static viennacl::range start(ctx.num_of_grid()-ctx.nx, ctx.num_of_grid());
    static viennacl::range edge(0, ctx.nx);
    viennacl::project(xn_jm1, edge, eq) = viennacl::project(xn, start, eq);
  }

  return xn_jm1;
}

vcl_mat comfi::operators::jp1(const vcl_mat &xn, comfi::types::Context ctx) {

  //make sure the dimension exists
  if ((ctx.bc_up == comfi::types::DIMENSIONLESS) ||
      (ctx.nz == 1)) {
    return xn;
  }

  //shift values
  vcl_mat xn_jp1(xn.size1(), xn.size2());
  static viennacl::range eq(0, ctx.num_of_eq);
  static viennacl::range jp1(inds(0, 1, ctx), ctx.num_of_grid());
  static viennacl::range j(0, ctx.num_of_grid()-ctx.nx);
  viennacl::project(xn_jp1, j, eq) = viennacl::project(xn, jp1, eq);

  //boundary conditions
  if (ctx.bc_up == comfi::types::NEUMANN) {
    static viennacl::range edge(ctx.num_of_grid()-ctx.nx, ctx.num_of_grid());
    viennacl::project(xn_jp1, edge, eq) = viennacl::project(xn, edge, eq);
  }
  if (ctx.bc_up == comfi::types::PERIODIC) {
    static viennacl::range edge(ctx.num_of_grid()-ctx.nx, ctx.num_of_grid());
    static viennacl::range start(0, ctx.nx);
    viennacl::project(xn_jp1, edge, eq) = viennacl::project(xn, start, eq);
  }

  return xn_jp1;
}

vcl_mat comfi::operators::ip1(const vcl_mat &xn, comfi::types::Context ctx) {

  //make sure the dimension exists
  if ((ctx.bc_right == comfi::types::DIMENSIONLESS) ||
      (ctx.nx == 1)) {
    return xn;
  }

  /* // TODO: Fix viennacl so that this method is working (preferred) */
  /* //shift values */
  /* vcl_mat xn_ip1(xn.size1(), xn.size2()); */
  /* viennacl::range eq(0, xn.size1()); */
  /* viennacl::range ip1[ctx.nz]; */
  /* viennacl::range i[ctx.nz]; */
  /* #pragma omp parallel for schedule(dynamic) */
  /* for(uint j = 0; j < ctx.nz; j++) { */
  /*    ip1[j] = viennacl::range(inds(1, j, ctx), inds(ctx.nx-1, j, ctx)+1); */
  /*    i[j] = viennacl::range(inds(0, j, ctx), inds(ctx.nx-2, j, ctx)+1); */
  /* } */
  /* /1* #pragma omp parallel for *1/ */
  /* for(uint j = 0; j < ctx.nz; j++) { */
  /*   viennacl::project(xn_ip1, eq, i[j]) = viennacl::project(xn, eq, ip1[j]); */
  /* } */

  /* //boundary conditions */
  /* if (ctx.bc_right == comfi::types::NEUMANN) { */
  /*   viennacl::slice eq(0, 1, ctx.num_of_eq); */
  /*   viennacl::slice edge(inds(ctx.nx-1, 0, ctx), ctx.nx, ctx.nz); */
  /*   viennacl::project(xn_ip1, edge, eq) = viennacl::project(xn, edge, eq); */
  /* } */
  /* if (ctx.bc_right == comfi::types::PERIODIC) { */
  /*   viennacl::slice eq(0, 1, ctx.num_of_eq); */
  /*   viennacl::slice edge(inds(ctx.nx-1, 0, ctx), ctx.nx, ctx.nz); */
  /*   viennacl::slice start(0, ctx.nx, ctx.nz); */
  /*   viennacl::project(xn_ip1, edge, eq) = viennacl::project(xn, start, eq); */
  /* } */

  static const sp_mat cpu_Pip1 = comfi::operators::buildPip1(ctx);
  static vcl_sp_mat Pip1(ctx.num_of_grid(), ctx.num_of_grid());
  static bool created = false;
  if (!created) { viennacl::copy(cpu_Pip1, Pip1); created = true; }
  vcl_mat xn_ip1 = viennacl::linalg::prod(Pip1, xn);

  return xn_ip1;
}

vcl_mat comfi::operators::im1(const vcl_mat &xn, comfi::types::Context ctx) {
  //make sure the dimension exists
  if ((ctx.bc_left == comfi::types::DIMENSIONLESS) ||
      (ctx.nx == 1)) {
    return xn;
  }

  /* // TODO: Make this method work through changing viennacl (preferred) */
  /* //shift values */
  /* vcl_mat xn_im1(xn.size1(), xn.size2()); */
  /* viennacl::range eq(0, xn.size1()); */
  /* viennacl::range im1[ctx.nz]; */
  /* viennacl::range i[ctx.nz]; */
  /* #pragma omp parallel for schedule(dynamic) */
  /* for(uint j = 0; j < ctx.nz; j++) { */
  /*   im1[j] = viennacl::range(inds(0, j, ctx), inds(ctx.nx()-2, j, ctx)+1); */
  /*   i[j] = viennacl::range(inds(1, j, ctx), inds(ctx.nx()-1, j, ctx)+1); */
  /* } */
  /* for(uint j = 0; j < ctx.nz(); j++) { */
  /*   viennacl::project(xn_im1, eq, i[j]) = viennacl::project(xn, eq, im1[j]); */
  /* } */

  /* //boundary conditions */
  /* if (ctx.bc_left == comfi::types::NEUMANN) { */
  /*   viennacl::slice eq(0, 1, ctx.num_of_eq); */
  /*   viennacl::slice start(0, ctx.nx, ctx.nz); */
  /*   viennacl::project(xn_im1, start, eq) = viennacl::project(xn, start, eq); */
  /* } */
  /* if (ctx.bc_left == comfi::types::PERIODIC) { */
  /*   viennacl::slice eq(0, 1, ctx.num_of_eq); */
  /*   viennacl::slice edge(inds(ctx.nx-1, 0, ctx), ctx.nx, ctx.nz); */
  /*   viennacl::slice start(0, ctx.nx, ctx.nz); */
  /*   viennacl::project(xn_im1, start, eq) = viennacl::project(xn, edge, eq); */
  /* } */

  static const sp_mat cpu_Pim1 = comfi::operators::buildPim1(ctx);
  static vcl_sp_mat Pim1(ctx.num_of_grid(), ctx.num_of_grid());
  static bool created = false;
  if (!created) { viennacl::copy(cpu_Pim1, Pim1); created = true; }
  vcl_mat xn_im1 = viennacl::linalg::prod(Pim1, xn);

  return xn_im1;
}

const sp_mat comfi::operators::buildPip1(comfi::types::Context &ctx) {
  umat locations;
  urowvec loci = zeros<urowvec>(ctx.num_of_grid());
  urowvec locj = zeros<urowvec>(ctx.num_of_grid());
  vec values = ones<vec>(ctx.num_of_grid());
  const comfi::types::BoundaryCondition BC = ctx.bc_right;

  #pragma omp parallel for collapse(2)
  for (uint i=0; i<ctx.nx; i++){ for(uint j=0; j<ctx.nz; j++) {
    // indexing
    const int           ij = inds(i, j, ctx);
    int                 ip1j = inds(i+1, j, ctx);

    //BC
    if (i==ctx.nx-1 && (BC==comfi::types::MIRROR || BC==comfi::types::NEUMANN)) { ip1j = ij; }
    else if (i==ctx.nx-1 && BC==comfi::types::PERIODIC) { ip1j = inds(0, j, ctx); }
    else if (BC==comfi::types::DIMENSIONLESS) { ip1j = inds(0, j, ctx); }

    loci(ij) = ip1j;
    locj(ij) = ij;
  }}

  locations.insert_rows(0, loci);
  locations.insert_rows(1, locj);
  return sp_mat(true, locations, values, ctx.num_of_grid(), ctx.num_of_grid());
}

const sp_mat comfi::operators::buildPim1(comfi::types::Context &ctx) {
  umat locations;
  urowvec loci = zeros<urowvec>(ctx.num_of_grid());
  urowvec locj = zeros<urowvec>(ctx.num_of_grid());
  vec values = ones<vec>(ctx.num_of_grid());
  const comfi::types::BoundaryCondition BC = ctx.bc_left;

  #pragma omp parallel for collapse(2)
  for (uint i=0; i<ctx.nx; i++){ for(uint j=0; j<ctx.nz; j++) {
    // indexing
    const int           ij = inds(i, j, ctx);
    int                 im1j = inds(i-1, j, ctx);

    //BC
    if (i==0 && (BC==comfi::types::MIRROR || BC==comfi::types::NEUMANN)) { im1j = ij; }
    else if (i==0 && BC==comfi::types::PERIODIC) { im1j = inds(ctx.nx-1, j, ctx);}
    else if (BC==comfi::types::DIMENSIONLESS) { im1j = inds(0, j, ctx);}

    loci(ij) = im1j;
    locj(ij) = ij;
  }}

  locations.insert_rows(0, loci);
  locations.insert_rows(1, locj);
  return sp_mat(true, locations, values, ctx.num_of_grid(), ctx.num_of_grid());
}

/*
vim: tabstop=2
vim: shiftwidth=2
vim: smarttab
vim: expandtab
*/


#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;
}


#include <QApplication>
#include <QFileInfo>
#include <QDir>
#include "GLPointCloudViewer.h"
#include "QImageWidget.h"
#include "QKinectGrabber.h"
#include "QKinectIO.h"
#include <iostream>
#include <iterator>
#include <array>
#include <memory>
#include <time.h>

#include "Volumetric_helper.h"
#include "Timer.h"
#include <Eigen/Dense>
#include <cuda_runtime.h>
#include <vector_types.h>
#include "cuda_kernels/cuda_kernels.h"

#include "GLPointCloud.h"

#include "helper_cuda.h"
#include "helper_image.h"

static QVector3D colors[7] = {
	QVector3D(1, 0, 0), QVector3D(0, 1, 0), QVector3D(0, 0, 1),
	QVector3D(1, 1, 0), QVector3D(1, 0, 1), QVector3D(0, 1, 1),
	QVector3D(1, 1, 1) };

static void export_obj_float4(const std::string& filename, const std::vector<float4>& vertices, const std::vector<float4>& normals)
{
	std::ofstream file;
	file.open(filename);
	//for (const float4 v : vertices)
	//	file << std::fixed << "v " << v.x << ' ' << v.y << ' ' << v.z << std::endl;
	//for (const float4 n : normals)
	//	file << std::fixed << "vn " << n.x << ' ' << n.y << ' ' << n.z << std::endl;

	for (int i = 0; i < vertices.size(); ++i)
	{
		const float4& v = vertices[i];
		const float4& n = normals[i];
		file << std::fixed << "v "
			<< v.x << ' ' << v.y << ' ' << v.z << ' '
			<< ((n.x * 0.5) + 0.5) * 255 << ' ' << ((n.y * 0.5) + 0.5) * 255 << ' ' << ((n.z * 0.5) + 0.5) * 255
			<< std::endl;
	}

	file.close();
}

void convert_normal_to_rgba(std::vector<float4>& normals)
{
	for (float4& n : normals)
	{
		n.x = (n.x * 0.5) + 0.5;
		n.y = (n.y * 0.5) + 0.5;
		n.z = (n.z * 0.5) + 0.5;
		n.w = (n.w * 0.5) + 0.5;
	}
}

void run_back_projection_with_normal_estimate(
	std::vector<float4>& vertices,
	std::vector<float4>& normals,
	const std::vector<ushort>& depth_buffer,
	uint width,
	uint height,
	ushort max_depth)
{
	StopWatchInterface *kernel_timer = nullptr;

	ushort* h_depth_buffer = (ushort*)depth_buffer.data();

	size_t in_pitch, out_pitch;

	ushort* d_depth_buffer = nullptr;
	// copy image data to array
	checkCudaErrors(cudaMallocPitch(&d_depth_buffer, &in_pitch, sizeof(ushort) * width, height));
	checkCudaErrors(cudaMemcpy2D(
		d_depth_buffer,
		in_pitch,
		h_depth_buffer,
		sizeof(ushort) * width,
		sizeof(ushort) * width,
		height,
		cudaMemcpyHostToDevice));

	float4* d_vertex_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_vertex_buffer,
		&out_pitch,
		width * sizeof(float4),
		height));

	float4* d_normal_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_normal_buffer,
		&out_pitch,
		width * sizeof(float4),
		height));

	sdkCreateTimer(&kernel_timer);
	sdkStartTimer(&kernel_timer);

	Eigen::Matrix4f h_inverse_projection = perspective_matrix_inverse<float>(fov_y, aspect_ratio, near_plane, far_plane);
	//bilateralFilter_normal_estimate_float4((OutputPixelType*)dOutputImage, (InputPixelType*)dInputImage, width, height, in_pitch, out_pitch, max_depth, euclidean_delta, filter_radius, iterations, kernel_timer);
	back_projection_with_normal_estimation(d_vertex_buffer, d_normal_buffer, d_depth_buffer, width, height, max_depth, in_pitch, out_pitch, h_inverse_projection.data());

	checkCudaErrors(cudaDeviceSynchronize());
	sdkStopTimer(&kernel_timer);
	std::cout << "Kernel Timer                              : " << kernel_timer->getTime() << " msec" << std::endl;
	sdkDeleteTimer(&kernel_timer);

	vertices.resize(depth_buffer.size());
	normals.resize(depth_buffer.size());

	cudaMemcpy2D(
		vertices.data(),
		sizeof(float4) * width,
		d_vertex_buffer,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	cudaMemcpy2D(
		normals.data(),
		sizeof(float4) * width,
		d_normal_buffer,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	checkCudaErrors(cudaFree(d_depth_buffer));
	checkCudaErrors(cudaFree(d_vertex_buffer));
	checkCudaErrors(cudaFree(d_normal_buffer));
}

void run_icp_with_normal_estimate(
	std::vector<float4>& vertices_0,
	std::vector<float4>& vertices_1,
	std::vector<float4>& normals,
	std::vector<ushort2>& indices,
	std::vector<float>& distances,
	const std::vector<ushort>& depth_buffer_0,
	const std::vector<ushort>& depth_buffer_1,
	uint width,
	uint height,
	ushort max_depth,
	const ushort half_window_search_size)
{
	StopWatchInterface *kernel_timer = nullptr;

	ushort* h_depth_buffer_0 = (ushort*)depth_buffer_0.data();
	ushort* h_depth_buffer_1 = (ushort*)depth_buffer_1.data();

	size_t in_pitch, out_pitch, index_pitch, distance_pitch;

	//
	// copy depth buffer data to array
	//
	ushort* d_depth_buffer_0 = nullptr;
	checkCudaErrors(cudaMallocPitch(&d_depth_buffer_0, &in_pitch, sizeof(ushort) * width, height));
	checkCudaErrors(cudaMemcpy2D(
		d_depth_buffer_0,
		in_pitch,
		h_depth_buffer_0,
		sizeof(ushort) * width,
		sizeof(ushort) * width,
		height,
		cudaMemcpyHostToDevice));

	ushort* d_depth_buffer_1 = nullptr;
	checkCudaErrors(cudaMallocPitch(&d_depth_buffer_1, &in_pitch, sizeof(ushort) * width, height));
	checkCudaErrors(cudaMemcpy2D(
		d_depth_buffer_1,
		in_pitch,
		h_depth_buffer_1,
		sizeof(ushort) * width,
		sizeof(ushort) * width,
		height,
		cudaMemcpyHostToDevice));

	//
	// allocate vertex buffer in gpu
	//
	float4* d_vertex_buffer_0;
	checkCudaErrors(cudaMallocPitch(
		&d_vertex_buffer_0,
		&out_pitch,
		width * sizeof(float4),
		height));

	float4* d_vertex_buffer_1;
	checkCudaErrors(cudaMallocPitch(
		&d_vertex_buffer_1,
		&out_pitch,
		width * sizeof(float4),
		height));

	//
	//  allocate normal buffer in gpu
	//
	float4* d_normal_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_normal_buffer,
		&out_pitch,
		width * sizeof(float4),
		height));

	sdkCreateTimer(&kernel_timer);
	sdkStartTimer(&kernel_timer);

	Eigen::Matrix4f h_inverse_projection = perspective_matrix_inverse<float>(fov_y, aspect_ratio, near_plane, far_plane);
	back_projection_with_normal_estimation(d_vertex_buffer_0, d_normal_buffer, d_depth_buffer_0, width, height, max_depth, in_pitch, out_pitch, h_inverse_projection.data());
	back_projection_with_normal_estimation(d_vertex_buffer_1, d_normal_buffer, d_depth_buffer_1, width, height, max_depth, in_pitch, out_pitch, h_inverse_projection.data());

	//
	// allocate index buffer in gpu
	//
	ushort2* d_index_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_index_buffer,
		&index_pitch,
		width * sizeof(ushort2),
		height));

	//
	// allocate index buffer in gpu
	//
	float* d_distances_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_distances_buffer,
		&distance_pitch,
		width * sizeof(float),
		height));

	icp_matching_vertices(d_index_buffer, d_distances_buffer, d_vertex_buffer_0, d_vertex_buffer_1, width, height, out_pitch, index_pitch, half_window_search_size);

	checkCudaErrors(cudaDeviceSynchronize());
	sdkStopTimer(&kernel_timer);
	std::cout << "Kernel Timer                              : " << kernel_timer->getTime() << " msec" << std::endl;
	sdkDeleteTimer(&kernel_timer);

	vertices_0.resize(depth_buffer_0.size());
	vertices_1.resize(depth_buffer_1.size());
	normals.resize(depth_buffer_0.size());
	indices.resize(depth_buffer_0.size());
	distances.resize(depth_buffer_0.size());

	cudaMemcpy2D(
		vertices_0.data(),
		sizeof(float4) * width,
		d_vertex_buffer_0,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	cudaMemcpy2D(
		vertices_1.data(),
		sizeof(float4) * width,
		d_vertex_buffer_1,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	cudaMemcpy2D(
		normals.data(),
		sizeof(float4) * width,
		d_normal_buffer,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	cudaMemcpy2D(
		indices.data(),
		sizeof(ushort2) * width,
		d_index_buffer,
		index_pitch,
		sizeof(ushort2) * width,
		height,
		cudaMemcpyDeviceToHost);

	cudaMemcpy2D(
		distances.data(),
		sizeof(float) * width,
		d_distances_buffer,
		distance_pitch,
		sizeof(float) * width,
		height,
		cudaMemcpyDeviceToHost);

	for (int i = 0; i < width * height; ++i)
	{
		ushort2 index = indices[i];
		int ii = index.y * width + index.x;
		//std::cout << index.x << ", " << index.y << std::endl;

		if (ii > width * height)
		{
			std::cout << ii << " ---> " << width * height << std::endl;
		}
	}

	checkCudaErrors(cudaFree(d_depth_buffer_0));
	checkCudaErrors(cudaFree(d_depth_buffer_1));
	checkCudaErrors(cudaFree(d_vertex_buffer_0));
	checkCudaErrors(cudaFree(d_vertex_buffer_1));
	checkCudaErrors(cudaFree(d_normal_buffer));
	checkCudaErrors(cudaFree(d_index_buffer));
	checkCudaErrors(cudaFree(d_distances_buffer));
}

void convertKinectFrame2QImage(const KinectFrame& frame, QImage& depthImage)
{
	QVector<QRgb> colorTable;
	for (int i = 0; i < 256; ++i)
		colorTable.push_back(qRgb(i, i, i));
	depthImage = QImage(frame.depth_width(), frame.depth_height(), QImage::Format::Format_Indexed8);
	depthImage.setColorTable(colorTable);

	// set pixels to depth image
	for (int y = 0; y < depthImage.height(); y++)
	{
		for (int x = 0; x < depthImage.width(); x++)
		{
			const unsigned short depth = frame.depth[y * frame.depth_width() + x];
			depthImage.scanLine(y)[x] = static_cast<uchar>((float)depth / (float)frame.depth_max_distance() * 255.f);;
		}
	}
}

template<typename Type>
void cpu_test(const KinectFrame frame_0, const KinectFrame& frame_1)//, std::vector<Eigen::Vector3f>& vertices_0, std::vector<Eigen::Vector3f>& vertices_1)
{
	Type window_width = 640.0f;
	Type window_height = 480.0f;
	Type near_plane = 0.1f;
	Type far_plane = 10240.0f;
	Type fovy = 60.0f;
	Type aspect_ratio = window_width / window_height;

	Eigen::Matrix<Type, 4, 4> proj_inv = perspective_matrix_inverse(fovy, aspect_ratio, near_plane, far_plane);

	std::vector<Eigen::Matrix<Type, 3, 1>> vertices_0;
	std::vector<Eigen::Matrix<Type, 3, 1>> vertices_1;
	vertices_0.clear();
	vertices_1.clear();

	int d = 0;
	for (int y = 0; y < frame_0.depth_height(); ++y)
	{
		for (int x = 0; x < frame_0.depth_width(); ++x)
		{
			Eigen::Matrix<Type, 3, 1> v = window_coord_to_3d(Eigen::Matrix<Type, 2, 1>(x, y), (Type)frame_0.depth.at(d), proj_inv, (int)window_width, (int)window_height);
			++d;

			vertices_0.push_back(v);
		}
	}

	d = 0;
	for (int y = 0; y < frame_1.depth_height(); ++y)
	{
		for (int x = 0; x < frame_1.depth_width(); ++x)
		{
			Eigen::Matrix<Type, 3, 1> v = window_coord_to_3d(Eigen::Matrix<Type, 2, 1>(x, y), (Type)frame_1.depth.at(d), proj_inv, (int)window_width, (int)window_height);
			++d;

			vertices_1.push_back(v);
		}
	}

	Eigen::Matrix<Type, 3, 3> R = Eigen::Matrix<Type, 3, 3>::Zero();
	Eigen::Matrix<Type, 3, 1> t = Eigen::Matrix<Type, 3, 1>::Zero();

	ComputeRigidTransform(vertices_0, vertices_1, R, t);

	std::cout << std::fixed
		<< "ComputeRigidTransform: " << std::endl
		<< "Rotate" << std::endl << R << std::endl
		<< "Translate" << std::endl << t.transpose() << std::endl
		<< std::endl;
}

int main(int argc, char **argv)
{
	srand(time(NULL));

	std::string filename_0 = "../../data/knt_frames/frame_27.knt";
	std::string filename_1 = "../../data/knt_frames/frame_27.knt";
	ushort half_window_search_size = 3;
	ushort max_distance = 10;

	if (argc < 3)
	{
		std::cerr << "Usage: ICP_gpu.exe ../../data/knt_frames/frame_27.knt ../../data/knt_frames/frame_27.knt 3 10"
		<< std::endl
		<< "The app will continue with default parameters."
		<< std::endl;
	}
	else
	{
		filename_0 = argv[1];
		filename_1 = argv[2];
	}

	if (argc > 3)
		half_window_search_size = atoi(argv[3]);

	if (argc > 4)
		max_distance = atoi(argv[4]);

	Timer timer;

	timer.start();
	KinectFrame frame_0, frame_1;
	QKinectIO::loadFrame(QString::fromStdString(filename_0), frame_0);
	QKinectIO::loadFrame(QString::fromStdString(filename_1), frame_1);
	timer.print_interval("Importing kinect frames (.knt)            : ");

	//std::vector<Eigen::Vector3f> verts_0, verts_1;
	//cpu_test<float>(frame_0, frame_1);
	//return 0;

	std::vector<float4> vertices_0, vertices_1, normals;
	std::vector<ushort2> indices;
	std::vector<float> distances;
	std::vector<Eigen::Vector3f> vertices_00, vertices_11;

	timer.start();
	run_icp_with_normal_estimate(
		vertices_0,
		vertices_1,
		normals,
		indices,
		distances,
		frame_0.depth,
		frame_1.depth,
		frame_0.depth_width(),
		frame_0.depth_height(),
		frame_0.depth_max_distance(),
		half_window_search_size);
	timer.print_interval("Running normal estimate in GPU            : ");

	vertices_1.clear();
	for (const ushort2 index : indices)
	{
		float4 v = vertices_0.at(index.y * frame_0.depth_width() + index.x);
		vertices_1.push_back(v);
	}

	//for (int i = 0; i < vertices_0.size(); ++i)
	//{
	//	const float4& v0 = vertices_0.at(i);
	//	const float4& v1 = vertices_1.at(i);

	//	if (v0.z > 0.1 && v1.z > 0.1)
	//	{
	//		vertices_00.push_back(Eigen::Vector3f((Eigen::Vector4f(v0.x, v0.y, v0.z, v0.w) / v0.w).head<3>()));
	//		vertices_11.push_back(Eigen::Vector3f((Eigen::Vector4f(v1.x, v1.y, v1.z, v1.w) / v1.w).head<3>()));
	//	}
	//}

	for (int y = 0; y < frame_0.depth_height(); ++y)
	{
		for (int x = 0; x < frame_0.depth_width(); ++x)
		{
			int i = y * frame_0.depth_width() + x;

			float distance = distances[i];

			if (distance > max_distance)
				continue;

			ushort2 index = indices[i];
			int ii = index.y * frame_0.depth_width() + index.x;

			const float4& v0 = vertices_0.at(ii);
			const float4& v1 = vertices_1.at(ii);

			vertices_00.push_back(Eigen::Vector3f((Eigen::Vector4f(v0.x, v0.y, v0.z, v0.w) / v0.w).head<3>()));
			vertices_11.push_back(Eigen::Vector3f((Eigen::Vector4f(v1.x, v1.y, v1.z, v1.w) / v1.w).head<3>()));

		}

	}

	std::cout << "Vertices size " << vertices_0.size() << ", " << vertices_1.size() << std::endl;
	std::cout << "Vertices size " << vertices_00.size() << ", " << vertices_11.size() << std::endl;

	Eigen::Matrix<float, 3, 3> R = Eigen::Matrix<float, 3, 3>::Zero();
	Eigen::Matrix<float, 3, 1> t = Eigen::Matrix<float, 3, 1>::Zero();

	ComputeRigidTransform(vertices_00, vertices_11, R, t);

	std::cout << std::fixed
		<< "ComputeRigidTransform: " << std::endl
		<< "Rotate" << std::endl << R << std::endl
		<< "Translate" << std::endl << t.transpose() << std::endl
		<< std::endl;

	return 0;

	//
	// Viewer
	//
	QApplication app(argc, argv);

	QSurfaceFormat format;
	format.setDepthBufferSize(24);
	QSurfaceFormat::setDefaultFormat(format);

	GLPointCloudViewer glwidget;
	glwidget.resize(1024, 848);
	glwidget.move(0, 0);
	glwidget.setWindowTitle("Point Cloud");
	glwidget.show();

	std::shared_ptr<GLPointCloud> cloud_0(new GLPointCloud);
	cloud_0->initGL();
	//cloud_0->setVertices((float*)&vertices_0.data()[0], (uint)vertices_0.size(), 4);
	cloud_0->setVertices((float*)&vertices_00.data()[0], (uint)vertices_00.size(), 4);
	cloud_0->setColor(QVector3D(1, 0, 0));

	std::shared_ptr<GLPointCloud> cloud_1(new GLPointCloud);
	cloud_1->initGL();
	//cloud_1->setVertices((float*)&vertices_1.data()[0], (uint)vertices_1.size(), 4);
	cloud_1->setVertices((float*)&vertices_11.data()[0], (uint)vertices_11.size(), 4);
	cloud_1->setColor(QVector3D(0, 0, 1));

	glwidget.addPointCloud(cloud_0);
	glwidget.addPointCloud(cloud_1);

	glwidget.setWeelSpeed(0.1f);
	glwidget.setPosition(0, 0, -0.5f);

	return app.exec();
}