#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/miller/match_indices.h>
#include <boost/python/class.hpp>

namespace cctbx { namespace miller { namespace boost_python {

namespace {

  struct match_indices_wrappers
  {
    typedef match_indices w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("match_indices", no_init)
        .def(init<af::shared<index<> > const&,
                  af::shared<index<> > const&>())
        .def(init<af::shared<index<> > const&>())
        .def("match_cached", &w_t::match_cached,
           "To be used when the match_indices instance was constructed from \n \
           one Miller array h0. Upon calling match_indices.match_cached(h1),\n \
           the matches between h0 and h1 are found and stored as tuples\n \
           (i0, i1) where i0 and i1 are indices into the arrays h0 and h1.\n \
           Retrieve the array of (i0, i1) tuples by calling\n \
           match_indices.pairs()."
            )
        .def("match_cached_fast", &w_t::match_cached_fast,
           "See match_indices.match_cached. This is faster when only \n \
           the pairs (i0, i1) are needed. Other member functions such as \n \
           match_cached.singles(i) are not available when the matching was \n \
           done via match_cached_fast. Note that match_cached_fast \n \
           populates the pairs array in ascending order of indices i1 \n \
           instead of i0."
            )
        .def("pairs", &w_t::pairs)
        .def("singles", &w_t::singles)
        .def("have_singles", &w_t::have_singles)
        .def("pair_selection", &w_t::pair_selection)
        .def("single_selection", &w_t::single_selection)
        .def("paired_miller_indices", &w_t::paired_miller_indices)
        .def("permutation", &w_t::permutation)
#define CCTBX_DEF(function_name) \
        .def(# function_name, \
          (af::shared<double>(w_t::*)(af::const_ref<double> const&, \
                                      af::const_ref<double> const&) const) \
          &w_t::function_name) \
        .def(# function_name, \
          (af::shared<std::complex<double> >(w_t::*)( \
            af::const_ref<std::complex<double> > const&, \
            af::const_ref<std::complex<double> > const&) const) \
          &w_t::function_name)
        CCTBX_DEF(plus)
        CCTBX_DEF(minus)
        CCTBX_DEF(divides)
        CCTBX_DEF(multiplies)
        CCTBX_DEF(additive_sigmas)
#undef CCTBX_DEF
      ;
    }
  };

} // namespace <anoymous>

  void wrap_match_indices()
  {
    match_indices_wrappers::wrap();
  }

}}} // namespace cctbx::miller::boost_python


/**
 * @file test_robust_distributed_mapper.cpp
 * @author Pierre-Yves Lajoie (lajoie.py@gmail.com)
 * @brief unit tests for test multi robot case with outliers simulated by loading the datasets
 */
#include <distributed_mapper/run_distributed_mapper.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <cmath>

using namespace std;
using namespace gtsam;
using namespace distributed_mapper;
using namespace evaluation_utils;

/******************************************************************************/

TEST(DistributedMapper, test_distributed_estimation_without_outliers_2robots) {
    // Parameters
    size_t nr_robots = 2; // number of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/pairwise_consistency_maximization/clean/simulation/example_2robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = true; // Use pairwise consistency maximization.

    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 1e-1;
    EXPECT(assert_equal(0.0, std::get<0>(results), tolerance));
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance));
    EXPECT(std::get<2>(results) == 10);
}

TEST(DistributedMapper, test_distributed_estimation_2robots) {
    // Parameters
    size_t nr_robots = 2; // number of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/pairwise_consistency_maximization/spoiled/simulation/example_2robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = true; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 2e-1;
    EXPECT(assert_equal(0.0, std::get<0>(results), tolerance));
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance));
    EXPECT(std::get<2>(results) == 10);
}

/* Test where the loop closure measurements are mixed with the odometry measurements
 * and not in the same order in the 2 files as we can expect in a asynchronous system */
TEST(DistributedMapper, test_distributed_estimation_not_in_order_2robots) {
    // Parameters
    size_t nr_robots = 2; // number of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/pairwise_consistency_maximization/spoiled/simulation_not_in_order/example_2robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = true; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 2e-1;
    EXPECT(assert_equal(0.0, std::get<0>(results), tolerance));
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance));
    EXPECT(std::get<2>(results) == 10);
}

TEST(DistributedMapper, test_distributed_estimation_no_PCM_2robots) {
    // Parameters
    size_t nr_robots = 2; // number of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/pairwise_consistency_maximization/spoiled/simulation/example_2robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = false; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 1e-1;
    EXPECT(assert_equal(27.8, std::get<0>(results), tolerance));
    EXPECT(assert_equal(27.8, std::get<1>(results), tolerance));
    EXPECT(std::get<2>(results) == 0);
}

TEST(DistributedMapper, test_distributed_estimation_4robots) {
    // Parameters
    size_t nr_robots = 4; // number of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/pairwise_consistency_maximization/spoiled/simulation/example_4robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = true; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 1e-1;
    EXPECT(assert_equal(0.0, std::get<0>(results), tolerance));
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance));
    EXPECT(std::get<2>(results) == 24);
}

TEST(DistributedMapper, test_distributed_estimation_8robots) {
    // Parameters
    size_t nr_robots = 8; // number of robots
    size_t nr_separators_by_pair = 4; // number of separators by pair of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/pairwise_consistency_maximization/spoiled/simulation/example_8robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = true; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 1e-1;
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance));
    EXPECT(std::get<2>(results) == (nr_robots-1)*nr_robots*nr_separators_by_pair/2);
}

TEST(DistributedMapper, test_distributed_estimation_16robots) {
    // Parameters
    size_t nr_robots = 16; // number of robots
    size_t nr_separators_by_pair = 4; // number of separators by pair of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/pairwise_consistency_maximization/spoiled/simulation/example_16robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = true; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 2e0;
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance));
    EXPECT(std::abs((int)(std::get<2>(results)-(nr_robots-1)*nr_robots*nr_separators_by_pair/2)) < 5);
}

TEST(DistributedMapper, test_distributed_estimation_no_outliers_argos_2robots) {
    // Parameters
    size_t nr_robots = 2; // number of robots
    // size_t nr_separators_by_pair = 4; // number of separators by pair of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/argos_simulation/clean/example_2robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = false; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 1e-1;
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance));
}

TEST(DistributedMapper, test_distributed_estimation_no_outliers_argos_4robots) {
    // Parameters
    size_t nr_robots = 4; // number of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/argos_simulation/clean/example_4robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = false; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 5e-1;
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance*std::get<0>(results)));
}

TEST(DistributedMapper, test_distributed_estimation_no_outliers_argos_10robots) {
    // Parameters
    size_t nr_robots = 10; // number of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/argos_simulation/clean/example_10robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = false; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 10; // That's the best I can get right now with that many poses.
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance*std::get<0>(results)));
}

TEST(DistributedMapper, test_distributed_estimation_argos_2robots) {
    // Parameters
    size_t nr_robots = 2; // number of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/argos_simulation/spoiled/example_2robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = true; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 10; // That's the best I can get right now with that many poses.
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance*std::get<0>(results)));
    std::cout << "Max clique size: " << std::get<2>(results) << std::endl;
}

TEST(DistributedMapper, test_distributed_estimation_argos_4robots) {
    // Parameters
    size_t nr_robots = 4; // number of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/argos_simulation/spoiled/example_4robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-12); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = true; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 10; // That's the best I can get right now with that many poses.
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance*std::get<0>(results)));
    std::cout << "Max clique size: " << std::get<2>(results) << std::endl;
}

TEST(DistributedMapper, test_distributed_estimation_argos_10robots) {
    // Parameters
    size_t nr_robots = 10; // number of robots
    string log_dir("/tmp/"); // log directory
    string data_dir("../../../test_data/argos_simulation/spoiled/example_10robots/"); // data directory
    string trace_file("/tmp/runG2o"); // data directory
    bool use_XY = false;
    bool use_OP = false;
    bool debug = false;
    noiseModel::Diagonal::shared_ptr prior_model = noiseModel::Isotropic::Variance(6, 1e-2); // prior noise
    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);
    size_t max_iter = 1000; // Maximum number of iterations of optimizer
    double rotation_estimate_change_threshold = 1e-1; // Difference between rotation estimate provides an early stopping condition
    double pose_estimate_change_threshold = 1e-1; // Difference between pose estimate provides an early stopping condition
    double gamma = 1.0f; // Gamma value for over relaxation methods
    bool use_flagged_init = true; // to use flagged initialization or not
    distributed_mapper::DistributedMapper::UpdateType update_type = distributed_mapper::DistributedMapper::incUpdate; // update_type differetiates between Distributed Jacobi/Jacobi OverRelaxation (postUpdate) and Gauss-Seidel/Successive OverRelaxation (incUpdate)
    bool use_between_noise = false; // use between factor noise or not
    bool use_chr_less_full_graph = false; // whether full graph has character indexes or not
    bool use_landmarks = false; // use landmarks -- landmarks are given symbols as upper case of robot name, for eg: if robot is 'a', landmark will be 'A'
    double confidence_probability = 0.99; // Confidence probability for the pairwise consistency computation
    bool use_covariance = false; // use covariance in dataset file.
    bool use_PCM = true; // Use pairwise consistency maximization.
    // Call distributed optimization
    std::tuple<double, double, int> results = runDistributedMapper(nr_robots, log_dir, data_dir, trace_file,
                                                                   use_XY, use_OP, debug, prior_model, model,
                                                                   max_iter, rotation_estimate_change_threshold, pose_estimate_change_threshold,
                                                                   gamma, use_flagged_init, update_type, use_between_noise,
                                                                   use_chr_less_full_graph, use_landmarks, confidence_probability, use_covariance, use_PCM);
    // Compare centralized and distributed pose estimates
    double tolerance = 10; // That's the best I can get right now with that many poses.
    EXPECT(assert_equal(std::get<0>(results), std::get<1>(results), tolerance*std::get<0>(results)));
    std::cout << "Max clique size: " << std::get<2>(results) << std::endl;
}

/****************************************************************************** */
int main() {
    TestResult tr;
    return TestRegistry::runAllTests(tr);
}
//******************************************************************************


#ifndef CORE_PPE_CONTEXT_HPP
#define CORE_PPE_CONTEXT_HPP

#include <memory>
#include <vector>
#include <NTL/ZZ.h>
class FHEcontext;
typedef std::shared_ptr<FHEcontext> raw_context_ptr;
namespace ppe {
class Context {
public:
    // dummy constructor
    Context() {}

	Context(const std::vector<long> &Ms,
			const std::vector<long> &Ps,
			const std::vector<long> &Rs);

    Context(long m, long p, long r) : Context(std::vector<long>(1, m),
											  std::vector<long>(1, p),
											  std::vector<long>(1, r)) {}

	void buildModChain(long L);

	/// @return the specific FHEcontext
	const raw_context_ptr get(int index) const { return contexts_.at(index); }

	size_t partsNum() const { return contexts_.size(); }

	double precision() const;

    NTL::ZZ plainSpace() const { return plainSpace_; }

    bool dump(std::ostream &ostream) const;

    bool restore(std::istream &istream);

private:
    NTL::ZZ plainSpace_;
	std::vector<raw_context_ptr> contexts_;
};
}
#endif // CORE_PPE_CONTEXT_HPP


#include <boost/math/complex/details.hpp>


#ifdef USE_PANGOLIN_VIEWER
#include <pangolin_viewer/viewer.h>
#elif USE_SOCKET_PUBLISHER
#include <socket_publisher/publisher.h>
#endif

#include <openvslam/system.h>
#include <openvslam/config.h>

#include <iostream>
#include <chrono>
#include <numeric>

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <spdlog/spdlog.h>
#include <popl.hpp>

//-------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------
#include "MsgSync/MsgSynchronizer.h"
#include "MsgSync/MsgSynchronizer.cpp"
#include "openvslam/IMU/imudata.h"
#include "openvslam/IMU/configparam.h"
#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <boost/foreach.hpp>

#include "openvslam/viorb_msg.h"
#include "openvslam/util/converter.h"

//#undef RUN_REALTIME
//#undef TRACK_WITH_IMU
//-------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------

#ifdef USE_STACK_TRACE_LOGGER
#include <glog/logging.h>
#endif

#ifdef USE_GOOGLE_PERFTOOLS
#include <gperftools/profiler.h>
#endif

//-------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------
void PublishSLAMData(ros::Publisher& slamdatapub, const openvslam::SLAMData& slamdata)
{
    openvslam::viorb_msg slamdatamsg;
    slamdatamsg.header.stamp = ros::Time(slamdata.Timestamp);
    // VINSInitFlag
    slamdatamsg.VINSInitFlag.data = slamdata.VINSInitFlag;
    // TrackStatus
    slamdatamsg.TrackStatus.data = slamdata.TrackingStatus;
    // -1-notready,0-noimage,1-initing,2-OK
    if(slamdata.TrackingStatus > 1 && slamdata.VINSInitFlag==true)
    {
        cv::Mat Rwi = slamdata.Rwi;

        // Qwi
        Eigen::Matrix<double,3,3> eigRwi = openvslam::util::converter::toMatrix3d(Rwi);
        Eigen::Quaterniond qwi(eigRwi);
        geometry_msgs::Quaternion qwimsg;
        qwimsg.x = qwi.x();
        qwimsg.y = qwi.y();
        qwimsg.z = qwi.z();
        qwimsg.w = qwi.w();
        slamdatamsg.Qwi = qwimsg;

        // gw
        geometry_msgs::Point gw;
        gw.x = slamdata.gw.at<float>(0);
        gw.y = slamdata.gw.at<float>(1);
        gw.z = slamdata.gw.at<float>(2);
        slamdatamsg.gw = gw;

        // Tic(Ric&tic)
        cv::Mat Riw = Rwi.t();

        cv::Mat Rcw = slamdata.Tcw.rowRange(0,3).colRange(0,3).clone();
        cv::Mat tcw = slamdata.Tcw.rowRange(0,3).col(3).clone();
        cv::Mat Rwc = Rcw.t();
        cv::Mat twc = -Rwc*tcw;

        cv::Mat tic = Riw*twc;
        cv::Mat Ric = Riw*Rwc;

        Eigen::Matrix<double,3,3> eigRic = openvslam::util::converter::toMatrix3d(Ric);
        Eigen::Quaterniond qic(eigRic);

        geometry_msgs::Quaternion qicmsg;
        qicmsg.x = qic.x();
        qicmsg.y = qic.y();
        qicmsg.z = qic.z();
        qicmsg.w = qic.w();

        geometry_msgs::Point ticmsg;
        ticmsg.x = tic.at<float>(0);
        ticmsg.y = tic.at<float>(1);
        ticmsg.z = tic.at<float>(2);

        geometry_msgs::Pose Ticmsg;
        Ticmsg.orientation = qicmsg;
        Ticmsg.position = ticmsg;

        slamdatamsg.Tic = Ticmsg;
    }

    slamdatapub.publish(slamdatamsg);
}
//-------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------

void mono_tracking(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path,
                   const std::string& mask_img_path, const bool eval_log, const std::string& map_db_path) {
    // load the mask image
    const cv::Mat mask = mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE);

    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    openvslam::ConfigParam config(cfg->config_file_path_);
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------

    // build a SLAM system
    openvslam::system SLAM(cfg, vocab_file_path);
    // startup the SLAM process
    SLAM.startup();

    // create a viewer object
    // and pass the frame_publisher and the map_publisher
#ifdef USE_PANGOLIN_VIEWER
    pangolin_viewer::viewer viewer(cfg, &SLAM, SLAM.get_frame_publisher(), SLAM.get_map_publisher());
#elif USE_SOCKET_PUBLISHER
    socket_publisher::publisher publisher(cfg, &SLAM, SLAM.get_frame_publisher(), SLAM.get_map_publisher());
#endif

    // run the viewer in another thread
#ifdef USE_PANGOLIN_VIEWER
    std::thread thread([&]() {
        viewer.run();
        if (SLAM.terminate_is_requested()) {
            // wait until the loop BA is finished
            while (SLAM.loop_BA_is_running()) {
                std::this_thread::sleep_for(std::chrono::microseconds(5000));
            }
            ros::shutdown();
        }
    });
#elif USE_SOCKET_PUBLISHER
    std::thread thread([&]() {
        publisher.run();
        if (SLAM.terminate_is_requested()) {
            // wait until the loop BA is finished
            while (SLAM.loop_BA_is_running()) {
                std::this_thread::sleep_for(std::chrono::microseconds(5000));
            }
            ros::shutdown();
        }
    });
#endif
    /*std::vector<double> track_times;
    const auto tp_0 = std::chrono::steady_clock::now();*/

    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    /**
     * @brief added data sync
     */
    ros::NodeHandle nh;
    double imageMsgDelaySec = config.GetImageDelayToIMU();
    openvslam::MsgSynchronizer msgsync(imageMsgDelaySec);

#ifdef RUN_REALTIME
    ros::Subscriber imagesub = nh.subscribe(config._imageTopic, 2, &openvslam::MsgSynchronizer::imageCallback, &msgsync);
    ros::Subscriber imusub = nh.subscribe(config._imuTopic, 200, &openvslam::MsgSynchronizer::imuCallback, &msgsync);
#endif
    sensor_msgs::ImageConstPtr imageMsg;
    std::vector<sensor_msgs::ImuConstPtr> vimuMsg;

    // 3dm imu output per g. 1g=9.80665 according to datasheet
    const double g3dm = 9.80665;
    const bool bAccMultiply98 = config.GetAccMultiply9p8();

    #ifndef RUN_REALTIME
        std::string bagfile = config._bagfile;
        rosbag::Bag bag;
        bag.open(bagfile,rosbag::bagmode::Read);

        std::vector<std::string> topics;
        std::string imutopic = config._imuTopic;
        std::string imagetopic = config._imageTopic;
        topics.push_back(imagetopic);
        topics.push_back(imutopic);

        rosbag::View view(bag, rosbag::TopicQuery(topics));
    #endif

    openvslam::SLAMData slamdata;
    ros::Publisher slamdatapub = nh.advertise<openvslam::viorb_msg>("VIORB/SLAMData", 10);

    ros::Rate r(1000);

    #ifdef RUN_REALTIME
    ROS_WARN("Run realtime");
    while(ros::ok())
    {
#else
    ROS_WARN("Run non-realtime");
    BOOST_FOREACH(rosbag::MessageInstance const m, view)
    {
        sensor_msgs::ImuConstPtr simu = m.instantiate<sensor_msgs::Imu>();
        if(simu!=NULL)
            msgsync.imuCallback(simu);
        sensor_msgs::ImageConstPtr simage = m.instantiate<sensor_msgs::Image>();
        if(simage!=NULL)
            msgsync.imageCallback(simage);
#endif

        bool bdata = msgsync.getRecentMsgs(imageMsg,vimuMsg);
        if(bdata)
        {
            //if(msgsync.getImageMsgSize()>=2) ROS_ERROR("image queue size: %d",msgsync.getImageMsgSize());
            std::vector<openvslam::IMUData> vimuData;
            //ROS_INFO("image time: %.3f",imageMsg->header.stamp.toSec());
            for(unsigned int i=0;i<vimuMsg.size();i++)
            {
                sensor_msgs::ImuConstPtr imuMsg = vimuMsg[i];
                double ax = imuMsg->linear_acceleration.x;
                double ay = imuMsg->linear_acceleration.y;
                double az = imuMsg->linear_acceleration.z;
                if(bAccMultiply98)
                {
                    ax *= g3dm;
                    ay *= g3dm;
                    az *= g3dm;
                }
                openvslam::IMUData imudata(imuMsg->angular_velocity.x,imuMsg->angular_velocity.y,imuMsg->angular_velocity.z,
                                ax,ay,az,imuMsg->header.stamp.toSec());
                vimuData.push_back(imudata);
                //ROS_INFO("imu time: %.3f",vimuMsg[i]->header.stamp.toSec());
            }

            // Copy the ros image message to cv::Mat.
            cv_bridge::CvImageConstPtr cv_ptr;
            try
            {
                cv_ptr = cv_bridge::toCvShare(imageMsg);
            }
            catch (cv_bridge::Exception& e)
            {
                ROS_ERROR("cv_bridge exception: %s", e.what());
                return;
            }

            // Consider delay of image message
            //SLAM.TrackMonocular(cv_ptr->image, imageMsg->header.stamp.toSec() - imageMsgDelaySec);
            // Below to test relocalizaiton
            cv::Mat im = cv_ptr->image.clone();
            {
                // To test relocalization
                static double startT=-1;
                if(startT<0)
                    startT = imageMsg->header.stamp.toSec();
                //if(imageMsg->header.stamp.toSec() > startT+25 && imageMsg->header.stamp.toSec() < startT+25.3)
                if(imageMsg->header.stamp.toSec() < startT+config._testDiscardTime)
                    im = cv::Mat::zeros(im.rows,im.cols,im.type());
            }
            //***C++11 Style:***
            //std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
            SLAM.feed_monocular_frame_VI(im, vimuData, imageMsg->header.stamp.toSec() - imageMsgDelaySec);
            //std::chrono::steady_clock::time_point end= std::chrono::steady_clock::now();
            //ROS_INFO_STREAM( "Time difference = " << std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count() <<std::endl );

            SLAM.GetSLAMData(slamdata);
            PublishSLAMData(slamdatapub,slamdata);

            //SLAM.TrackMonoVI(cv_ptr->image, vimuData, imageMsg->header.stamp.toSec() - imageMsgDelaySec);
            //cv::imshow("image",cv_ptr->image);//
#ifndef RUN_REALTIME
            // Wait local mapping end.
            bool bstop = false;

            std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
            while(!SLAM.bLocalMapAcceptKF())
            {
                if(!ros::ok())
                {
                    bstop=true;
                }
            };
            std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
            double vlocalbatime2 = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
            ROS_INFO("Waited: %f", vlocalbatime2);
            if(bstop)
                break;
#endif
        }

        //cv::waitKey(1);

        ros::spinOnce();
        r.sleep();
        if(!ros::ok())
            break;
    }

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

    //ros::spin();

    // automatically close the viewer
#ifdef USE_PANGOLIN_VIEWER
    viewer.request_terminate();
    thread.join();
#elif USE_SOCKET_PUBLISHER
    publisher.request_terminate();
    thread.join();
#endif

    // shutdown the SLAM process
    SLAM.shutdown();
/*
    if (eval_log) {
        // output the trajectories for evaluation
        SLAM.save_frame_trajectory("frame_trajectory.txt", "TUM");
        SLAM.save_keyframe_trajectory("keyframe_trajectory.txt", "TUM");
        // output the tracking times for evaluation
        std::ofstream ofs("track_times.txt", std::ios::out);
        if (ofs.is_open()) {
            for (const auto track_time : track_times) {
                ofs << track_time << std::endl;
            }
            ofs.close();
        }
    }

    if (!map_db_path.empty()) {
        // output the map database
        SLAM.save_map_database(map_db_path);
    }

    if (track_times.size()) {
        std::sort(track_times.begin(), track_times.end());
        const auto total_track_time = std::accumulate(track_times.begin(), track_times.end(), 0.0);
        std::cout << "median tracking time: " << track_times.at(track_times.size() / 2) << "[s]" << std::endl;
        std::cout << "mean tracking time: " << total_track_time / track_times.size() << "[s]" << std::endl;
    }*/
}

int main(int argc, char* argv[]) {
#ifdef USE_STACK_TRACE_LOGGER
    google::InitGoogleLogging(argv[0]);
    google::InstallFailureSignalHandler();
#endif
    ros::init(argc, argv, "run_slam");

    // create options
    popl::OptionParser op("Allowed options");
    auto help = op.add<popl::Switch>("h", "help", "produce help message");
    auto vocab_file_path = op.add<popl::Value<std::string>>("v", "vocab", "vocabulary file path");
    auto setting_file_path = op.add<popl::Value<std::string>>("c", "config", "setting file path");
    auto mask_img_path = op.add<popl::Value<std::string>>("", "mask", "mask image path", "");
    auto debug_mode = op.add<popl::Switch>("", "debug", "debug mode");
    auto eval_log = op.add<popl::Switch>("", "eval-log", "store trajectory and tracking times for evaluation");
    auto map_db_path = op.add<popl::Value<std::string>>("", "map-db", "store a map database at this path after SLAM", "");
    try {
        op.parse(argc, argv);
    }
    catch (const std::exception& e) {
        std::cerr << e.what() << std::endl;
        std::cerr << std::endl;
        std::cerr << op << std::endl;
        return EXIT_FAILURE;
    }

    // check validness of options
    if (help->is_set()) {
        std::cerr << op << std::endl;
        return EXIT_FAILURE;
    }
    if (!vocab_file_path->is_set() || !setting_file_path->is_set()) {
        std::cerr << "invalid arguments" << std::endl;
        std::cerr << std::endl;
        std::cerr << op << std::endl;
        return EXIT_FAILURE;
    }

    // setup logger
    spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] %^[%L] %v%$");
    if (debug_mode->is_set()) {
        spdlog::set_level(spdlog::level::debug);
    }
    else {
        spdlog::set_level(spdlog::level::info);
    }

    // load configuration
    std::shared_ptr<openvslam::config> cfg;
    try {
        cfg = std::make_shared<openvslam::config>(setting_file_path->value());
    }
    catch (const std::exception& e) {
        std::cerr << e.what() << std::endl;
        return EXIT_FAILURE;
    }

#ifdef USE_GOOGLE_PERFTOOLS
    ProfilerStart("slam.prof");
#endif

    // run tracking
    if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::Monocular) {
        mono_tracking(cfg, vocab_file_path->value(), mask_img_path->value(), eval_log->is_set(), map_db_path->value());
    }
    else {
        throw std::runtime_error("Invalid setup type: " + cfg->camera_->get_setup_type_string());
    }

#ifdef USE_GOOGLE_PERFTOOLS
    ProfilerStop();
#endif

    return EXIT_SUCCESS;
}


/*    Copyright (c) 2010-2020, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "tudat/basics/testMacros.h"

#include "tudat/astro/ephemerides/tleEphemeris.h"

namespace tudat
{
namespace unit_tests
{

//! Test the functionality of the two-line elements ephemeris
BOOST_AUTO_TEST_SUITE( test_two_line_elements_ephemeris )

//! Test the functionality of the two-line elements ephemeris
BOOST_AUTO_TEST_CASE( testTwoLineElementsEphemeris )
{
	using namespace tudat;
	using namespace tudat::ephemerides;

	// Dummy two line element set from Vallado (2013), page 234
	std::string elements =  "1 00005U 58002B   00179.78495062  .00000023  00000-0  28098-4 0  4753\n"
							"2 00005  34.2682 348.7242 1859667 331.7664  19.3264 10.82419157413667";

	// Create TLE object from element set
	std::shared_ptr< Tle > tlePtr = std::make_shared< Tle >( elements );

	// Create TLE Ephemeris object; output state in J2000 frame fixed at the centre of the Earth
	std::shared_ptr< TleEphemeris > tleEphemeris = std::make_shared< TleEphemeris >( "Earth", "J2000", tlePtr, false );

	// Propagate TLE for 3 Julian days to be able to compare state to the one given in Vallado
	Eigen::Vector6d propagatedState = tleEphemeris->getCartesianState( 3.0 * tudat::physical_constants::JULIAN_DAY + tlePtr->getEpoch() );
	Eigen::Vector3d propagatedPosition = propagatedState.head( 3 );
	Eigen::Vector3d propagatedVelocity = propagatedState.tail( 3 );

	// Position and velocity vectors as found in Vallado
	Eigen::Vector3d verificationPosition;
	Eigen::Vector3d verificationVelocity;
	verificationPosition << -9059941.3786, 4659697.2000, 813958.8875;
	verificationVelocity << -2233.348094, -4110.136162, -3157.394074;

	// Check if difference within tolerances
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( propagatedPosition, verificationPosition, 5.0e-5 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( propagatedVelocity, verificationVelocity, 5.0e-6);

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// Copyright 2015 Cloudera, Inc.
//
// 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.

#include "kudu/common/row_key-util.h"

#include <boost/type_traits/is_unsigned.hpp>

#include "kudu/common/row.h"

namespace kudu {
namespace row_key_util {

namespace {

template<DataType type>
bool IncrementIntCell(void* cell_ptr) {
  typedef DataTypeTraits<type> traits;
  typedef typename traits::cpp_type cpp_type;

  cpp_type orig;
  memcpy(&orig, cell_ptr, sizeof(cpp_type));

  cpp_type inc;
  if (boost::is_unsigned<cpp_type>::value) {
    inc = orig + 1;
  } else {
    // Signed overflow is undefined in C. So, we'll use a branch here
    // instead of counting on undefined behavior.
    if (orig == MathLimits<cpp_type>::kMax) {
      inc = MathLimits<cpp_type>::kMin;
    } else {
      inc = orig + 1;
    }
  }
  memcpy(cell_ptr, &inc, sizeof(cpp_type));
  return inc > orig;
}

bool IncrementStringCell(void* cell_ptr, Arena* arena) {
  Slice orig;
  memcpy(&orig, cell_ptr, sizeof(orig));
  uint8_t* new_buf = CHECK_NOTNULL(
      static_cast<uint8_t*>(arena->AllocateBytes(orig.size() + 1)));
  memcpy(new_buf, orig.data(), orig.size());
  new_buf[orig.size()] = '\0';

  Slice inc(new_buf, orig.size() + 1);
  memcpy(cell_ptr, &inc, sizeof(inc));
  return true;
}

bool IncrementCell(const ColumnSchema& col, void* cell_ptr, Arena* arena) {
  DataType type = col.type_info()->physical_type();
  switch (type) {
#define HANDLE_TYPE(t) case t: return IncrementIntCell<t>(cell_ptr);
    HANDLE_TYPE(UINT8);
    HANDLE_TYPE(UINT16);
    HANDLE_TYPE(UINT32);
    HANDLE_TYPE(UINT64);
    HANDLE_TYPE(INT8);
    HANDLE_TYPE(INT16);
    HANDLE_TYPE(INT32);
    HANDLE_TYPE(TIMESTAMP);
    HANDLE_TYPE(INT64);
    case UNKNOWN_DATA:
    case BOOL:
    case FLOAT:
    case DOUBLE:
      LOG(FATAL) << "Unable to handle type " << type << " in row keys";
    case STRING:
    case BINARY:
      return IncrementStringCell(cell_ptr, arena);
    default: CHECK(false) << "Unknown data type: " << type;
  }
  return false; // unreachable
#undef HANDLE_TYPE
}

} // anonymous namespace

void SetKeyToMinValues(ContiguousRow* row) {
  for (int i = 0; i < row->schema()->num_key_columns(); i++) {
    const ColumnSchema& col = row->schema()->column(i);
    col.type_info()->CopyMinValue(row->mutable_cell_ptr(i));
  }
}

bool IncrementKey(ContiguousRow* row, Arena* arena) {
  return IncrementKeyPrefix(row, row->schema()->num_key_columns(), arena);
}

bool IncrementKeyPrefix(ContiguousRow* row, int prefix_len, Arena* arena) {
  for (int i = prefix_len - 1; i >= 0; --i) {
    if (IncrementCell(row->schema()->column(i),
                                row->mutable_cell_ptr(i),
                                arena)) {
      return true;
    }
  }
  return false;
}

} // namespace row_key_util
} // namespace kudu


#include <iostream>
#include <boost/cstdint.hpp>

inline const boost::uint_least32_t UnSynch(const boost::uint_least32_t & uValue)
{
    return (uValue & 0x7Fu) | ((uValue & (0x7Fu<<8u))>>1) | ((uValue & (0x7Fu<<16u))>>2) | ((uValue & (0x7Fu<<24u))>>3);
}
inline const boost::uint_least32_t Synch(const boost::uint_least32_t & uValue)
{
    return (uValue & 0x7Fu) | ((uValue & (0x7Fu<<7u))<<1) | ((uValue & (0x7Fu<<14u))<<2) | ((uValue & (0x7Fu<<21u))<<3);
}
inline const bool ValidSynch(const boost::uint_least32_t & uValue)
{
    return !(uValue & 0x80808080u);
}
int main()
{
    boost::uint_least32_t uValue = 0x0FFFFFFFu;
    std::cout << "Value: " << uValue << std::endl;
    uValue = Synch(uValue);
    std::cout << "Value: " << uValue << std::endl;
    uValue = UnSynch(uValue);
    std::cout << "Value: " << uValue << std::endl;
    return 0;
}


/**
 * \ file OutPointerFilter.cpp
 */

#include <cmath>

#include <boost/math/constants/constants.hpp>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

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

#define PROCESSSIZE (10)

BOOST_AUTO_TEST_CASE( OutPointerFloat_sin1k_test )
{
  std::array<float, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<float, PROCESSSIZE> outdata;

  ATK::OutPointerFilter<float> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);

  output.process(2);
  output.process(PROCESSSIZE - 2);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE( OutPointerFloat_sin1k_test_set_pointer )
{
  std::array<float, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<float, PROCESSSIZE> outdata;

  ATK::OutPointerFilter<float> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);
  output.set_pointer(outdata.data(), PROCESSSIZE);

  output.process(2);
  output.process(PROCESSSIZE - 2);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE( OutPointerDouble_sin1k_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);

  output.process(2);
  output.process(PROCESSSIZE - 2);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE(OutPointerDouble_check_bound_test)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i + 1.) / 48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, 2*PROCESSSIZE> outdata;
  std::fill(outdata.begin(), outdata.end(), -1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, 3*PROCESSSIZE/2, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);

  output.process(2*PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i], outdata[i]);
  }
  for (gsl::index i = 0; i < PROCESSSIZE / 2; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[PROCESSSIZE + i]); // check that input is now 0
  }
  for (gsl::index i = 0; i < PROCESSSIZE / 2; ++i)
  {
    BOOST_REQUIRE_EQUAL(-1, outdata[3 * PROCESSSIZE /2 + i]); // check that we don't write after what we declare was allocated
  }
}

BOOST_AUTO_TEST_CASE( OutPointerFloat_sin1k2k_interleaved_test )
{
  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[2*i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
    data[2*i+1] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 2000);
  }

  ATK::InPointerFilter<float> generator(data.data(), PROCESSSIZE, 2, true);
  generator.set_output_sampling_rate(48000);

  std::array<double, 2*PROCESSSIZE> outdata;

  ATK::OutPointerFilter<double> output(outdata.data(), PROCESSSIZE, 2, true);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);
  output.set_input_port(1, &generator, 1);

  output.process(2);
  output.process(PROCESSSIZE - 2);

  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE( OutPointerFloat_sin1k2k_noninterleaved_test )
{
  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i+PROCESSSIZE] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 2000);
  }

  ATK::InPointerFilter<float> generator(data.data(), 2, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, 2*PROCESSSIZE> outdata;

  ATK::OutPointerFilter<double> output(outdata.data(), 2, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);
  output.set_input_port(1, &generator, 1);

  output.process(2);
  output.process(PROCESSSIZE);
  output.process(PROCESSSIZE);
  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i], outdata[i]);
  }
}


//Note : could take advantage of sorting straw hits by time in the mergeCluster module,
//
// Object to perform helix fit to straw hits
//
#include "TrkReco/inc/TNTClusterer.hh"
#include "ConditionsService/inc/ConditionsHandle.hh"
#include "TrackerGeom/inc/Tracker.hh"
#include "TrackerGeom/inc/Straw.hh"
#include "GeometryService/inc/GeometryService.hh"
#include "GeometryService/inc/GeomHandle.hh"
#include "BFieldGeom/inc/BFieldConfig.hh"
#include "GeometryService/inc/GeomHandle.hh"
#include "CLHEP/Units/PhysicalConstants.h"
#include "GeneralUtilities/inc/Angles.hh"

#include "art_root_io/TFileService.h"
#include "art/Framework/Services/Registry/ServiceHandle.h"
#include "cetlib_except/exception.h"

#include <iostream>
#include <vector>
#include <algorithm>
#include <iterator>
#include <functional>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/weighted_median.hpp>

#include "TTree.h"
#include "Math/VectorUtil.h"

using namespace boost::accumulators;
using namespace ROOT::Math::VectorUtil;

// Note 1: The list allows you to take pointer of its elements, a vector will reallocate and invalidate the pointers
//         The code with the vector version is much more complicated and has a tiny performwnce improvement.
//         The initDB also makes use of the list specificities to delete empty clusters

namespace mu2e
{
// std::sort sorts in ascending order, so the following will put the best hits first
  struct BkgComp : public std::binary_function<BkgClusterHit,BkgClusterHit, bool> {
    BkgComp(ComboHitCollection const& chcol) : _chcol(chcol){}
    bool operator()(BkgClusterHit const& x, BkgClusterHit const& y) {
      return _chcol[x.index()].wireRes() < _chcol[y.index()].wireRes();
//      return _chcol[x.index()].nStrawHits() > _chcol[y.index()].nStrawHits();
    }
    ComboHitCollection const& _chcol;
  };

  int ClusterStraw::idCounter_=0;

   ClusterStraw::ClusterStraw(BkgClusterHit& hit, const ComboHit& chit) :
      _id(idCounter_++),_pos(chit.pos()),_time(chit.time()),_hasChanged(true),_hitsPtr(std::vector<BkgClusterHit*>(1,&hit))
   {}

   void ClusterStraw::updateCache(const ComboHitCollection& chcol, float maxwt)
   {
       if (_hitsPtr.size()==1)
       {
          int idx = _hitsPtr.at(0)->index();
          _pos = chcol[idx].pos();
          _pos.SetZ(0);
          _time = chcol[idx].time();
          return;
       }

       accumulator_set<float, stats<tag::weighted_median>, unsigned> racc, pacc, tacc;

       float crho  = sqrtf(_pos.perp2());
       float cphi  = _pos.phi();
//       XYZVec rdir = PerpVector(_pos,Geom::ZDir()).unit();
//       XYZVec pdir(-rdir.y(),rdir.x(),0.0);

       for (auto& hitPtr : _hitsPtr)
       {
          int idx         = hitPtr->index();
          float hphi      = chcol[idx].phi();

          float dt  = chcol[idx].time() -_time;
	  float dr  = sqrtf(chcol[idx].pos().perp2()) - crho;
	  float dp = Angles::deltaPhi(hphi,cphi);

          // weight according to the # of hits

	  tacc(dt,weight=chcol[idx].nStrawHits());
	  racc(dr,weight=chcol[idx].nStrawHits());
	  pacc(dp,weight=chcol[idx].nStrawHits());
       }

       crho  += extract_result<tag::weighted_median>(racc);
       cphi  += extract_result<tag::weighted_median>(pacc);
       _time += extract_result<tag::weighted_median>(tacc);
       _pos   = XYZVec(crho*cos(cphi),crho*sin(cphi),0.0);
   }

   //---------------------------------------------------------------------------------------
   TNTClusterer::TNTClusterer(const fhicl::ParameterSet& pset) :
     _diag(pset.get<int>(                         "diagLevel",0)),
     _testflag(pset.get<bool>(                    "TestFlag")),
     _bkgmask(pset.get<std::vector<std::string> >("BackgroundMask",std::vector<std::string>())),
     _sigmask(pset.get<std::vector<std::string> >("SignalMask",std::vector<std::string>())),
     _comboInit(pset.get<bool>(                   "ComboInit",true)),
     _mergeInit(pset.get<bool>(                   "MergeInit",false)),
     _dseed(pset.get<float>(                      "SeedDistance")),
     _dhit(pset.get<unsigned>(                    "HitDistance")),
     _dd(pset.get<float>(                         "ClusterDiameter",10.0)),
     _dt(pset.get<float>(                         "TimeDifference",30.0)),
     _maxdt(pset.get<float>(                      "MaxTimeDifference")),
     _maxdsum(pset.get<float>(                    "MaxDistanceSum",100.0)),
     _maxNiter(pset.get<unsigned>(                "MaxNIterations")),
     _maxNchanged(pset.get<unsigned>(             "MaxNChanged",2)),
     _hitIndex(200,std::vector<ClusterStraw*>()),
     _cptrs()
   {
       // cache some values
       float minerr(pset.get<float>( "MinHitError",5.0));
       float maxdist(pset.get<float>("MaxDistance",50.0));
       float trms(pset.get<float>(   "TimeRMS",2.0));

       _trms2inv  = 1.0/trms/trms;
       _ditime = int(_maxdt/10.0)+1;
       _dd2 = _dd*_dd;
       _maxwt = 1.0/minerr;
       _md2 = maxdist*maxdist;
   }

   //---------------------------------------------------------------------------------------
   void TNTClusterer::init()
   {
      if (_diag > 0)
      {
	art::ServiceHandle<art::TFileService> tfs;
	_idiag = tfs->make<TTree>("idiag","iteration diagnostics");
	_idiag->Branch("nhits",      &nhits_,        "nhits/I");
	_idiag->Branch("hitRad",     &hitRad_,       "hitRad[nhits]/F");
	_idiag->Branch("hitPhi",     &hitPhi_,       "hitPhi[nhits]/F");
	_idiag->Branch("hitTime",    &hitTime_,      "hitTime[nhits]/F");
	_idiag->Branch("hitNcombo",  &hitNcombo_,    "hitNcombo[nhits]/I");

	_idiag->Branch("ncluIter",   &ncluIter_,     "ncluIter/I");
	_idiag->Branch("cluId",      &cluId_,        "cluId[ncluIter]/I");
	_idiag->Branch("cluNpass",   &cluNpass_,     "cluNpass[ncluIter]/I");
	_idiag->Branch("cnhi",       &cluNhit_,      "cnhi[ncluIter]/I");
	_idiag->Branch("cluRad",     &cluRad_,       "cluRad[ncluIter]/F");
	_idiag->Branch("cluPhi",     &cluPhi_,       "cluPhi[ncluIter]/F");
	_idiag->Branch("cluTime",    &cluTime_,      "cluTime[ncluIter]/F");
	_idiag->Branch("cluR2diff",  &cluR2diff_,    "cluR2diff[ncluIter]/F");
	_idiag->Branch("cluRdiff",   &cluRdiff_,     "cluRdiff[ncluIter]/F");
	_idiag->Branch("cluPdiff",   &cluPdiff_,     "cluPdiff[ncluIter]/F");
	_idiag->Branch("cluTdiff",   &cluTdiff_,     "cluTdiff[ncluIter]/F");
	_idiag->Branch("cluTRdiff",  &cluTRdiff_,    "cluTRdiff[ncluIter]/F");

	_idiag->Branch("nhitClu",    &nhitClu_,      "nhitClu/I");
	_idiag->Branch("hcIdxClu",   &hcIdxClu_,     "hcIdxClu[nhitClu]/I");
	_idiag->Branch("hcIdxHit",   &hcIdxHit_,     "hcIdxHit[nhitClu]/I");
	_idiag->Branch("hcNpass",    &hcNpass_,      "hcNpass[nhitClu]/I");

	_idiag->Branch("niter",      &niter_,        "niter/I");
        _idiag->Branch("nclu",       &nclu_,         "nclu[niter]/I");
	_idiag->Branch("nChanged",   &nChanged_,     "nChanged[niter]/I");
	_idiag->Branch("odist",      &odist_,        "odist[niter]/F");
	_idiag->Branch("tdist",      &tdist_,        "tdist[niter]/F");

      }
   }

   //-------------------------------------------------------------------------------------------------------------------
   void TNTClusterer::findClusters(BkgClusterCollection& clusterColl, const ComboHitCollection& chcol)
   {

      ClusterStraw::resetCounter();
      if (_diag) {ncluIter_ = nhitClu_ = 0;fillHitTree(chcol);}

      //reset stuff
      _cptrs = std::vector<ClusterStraw*>(chcol.size(),nullptr);
      for (auto& vec: _hitIndex) vec.clear();
      std::list<ClusterStraw> clusters; //see Note 1

      // loop over the straw hits and create ClusterHits
      std::vector<BkgClusterHit> chits;
      chits.reserve(chcol.size());

      algo1(chcol, clusters, chits);
      //algo2(chcol, clusters, chits);

      if (_diag) _idiag->Fill();

      //form and insert final BkgClusters here
      for (auto& cluster: clusters)
      {
         if (cluster.hits().size()==0) continue;
         clusterColl.emplace_back(BkgCluster(cluster.pos(), cluster.time()));

         BkgCluster& sclust = clusterColl.back();
         sclust._hits.reserve(cluster.hits().size());
         for (const auto& hitPtr : cluster.hits()) sclust._hits.emplace_back(std::move(*hitPtr));
      }
   }

   //----------------------------------------------------------------------------------------------
   void TNTClusterer::algo1(const ComboHitCollection& chcol, std::list<ClusterStraw>& clusters, std::vector<BkgClusterHit>& chits)
   {

       if (_mergeInit) initCluMerge(chcol,chits, clusters);
       else            initClu(chcol,chits);

       float odist(2.0*_maxdsum);
       float tdist(0.0);

       unsigned  niter(0);
       while (std::abs(odist - tdist) > _maxdsum  && niter < _maxNiter)
       {
	   unsigned nChanged = formClusters(chcol, chits, clusters);

           odist = tdist;
           tdist = 0.0;
           for (auto& cluster: clusters)
             for ( auto& hitPtr : cluster.hits()) tdist += hitPtr->distance();

	   if (_diag) fillCluTree(chcol,clusters,niter,odist,tdist,nChanged);
	   ++niter;
       }
      //if you want to do a final merge only, and comment the merging step in formclusters
      //mergeClusters(clusters, _dt, _dd2);

   }

   //----------------------------------------------------------------------------------------------
   void TNTClusterer::algo2(const ComboHitCollection& chcol, std::list<ClusterStraw>& clusters, std::vector<BkgClusterHit>& chits)
   {
       if (_mergeInit) initCluMerge(chcol,chits, clusters);
       else            initClu(chcol,chits);

       unsigned  niter(0),nChanged(clusters.size());
       while (nChanged > _maxNchanged  && niter < _maxNiter)
       {
	   nChanged = formClusters(chcol, chits,clusters);
	   if (_diag) fillCluTree(chcol, clusters, niter, 0, 0, nChanged);
	   ++niter;
       }
      //if you want to do a final merge only, and comment the merging step in formclusters
      //mergeClusters(clusters, _dt, _dd2);
   }

   //-------------------------------------------------------------------------------------------------------------------
   void TNTClusterer::initClu(const ComboHitCollection& chcol, std::vector<BkgClusterHit>& chits)
   {
     for (size_t ish=0; ish<chcol.size(); ++ish){
       if (_testflag && (!chcol[ish].flag().hasAllProperties(_sigmask) || chcol[ish].flag().hasAnyProperty(_bkgmask))) continue;
       chits.emplace_back(BkgClusterHit(ish,chcol[ish].flag()));
     }
     if (_comboInit)
     {
       BkgComp chsort(chcol);
       std::sort(chits.begin(),chits.end(),chsort);
     }
   }

   //-------------------------------------------------------------------------------------------------------------------
   void TNTClusterer::initCluMerge(const ComboHitCollection& chcol, std::vector<BkgClusterHit>& chits, std::list<ClusterStraw>& clusters)
   {
       //merge init only uses combohit with two hits as starting point
       for (size_t ish=0;ish<chcol.size();++ish)
       {
          if (_testflag && (!chcol[ish].flag().hasAllProperties(_sigmask) || chcol[ish].flag().hasAnyProperty(_bkgmask))) continue;
          chits.emplace_back(BkgClusterHit(ish,chcol[ish].flag()));
          if (chcol.at(ish).nCombo()>1) clusters.emplace_back(ClusterStraw(chits.back(),chcol[ish]));
       }

       mergeClusters(clusters, chcol, _maxdt, _md2);

       for (auto& cluster : clusters)
       {
           _hitIndex[cluster.itime()].emplace_back(&cluster);

           if (cluster.hits().size()==1)
             cluster.hits().at(0)->distance(0);
           else
             for (auto& hit : cluster.hits()) {hit->distance(distance(cluster,chcol[hit->index()])); _cptrs[hit->index()] = &cluster;}
       }
   }

   //-------------------------------------------------------------------------------------------------------------------
   void TNTClusterer::mergeClusters(std::list<ClusterStraw>& clusters, const ComboHitCollection& chcol, float dt, float dd2)
   {
       unsigned niter(0);
       while (niter < _maxNiter)
       {
          int nchanged(0);
          for (auto it1 = clusters.begin(); it1 != std::prev(clusters.end()); ++it1)
          {
              for (auto it2 = std::next(it1); it2!=clusters.end(); ++it2)
              {
                 //if (niter==0 && it2->time() - it1->time() >dt) break; //for sorted times
                 if (std::abs(it1->time() - it2->time()) > dt) continue;
		 if ((it1->pos() - it2->pos()).perp2() > dd2) continue;

                 it1->flagChanged(true);
                 it2->flagChanged(true);
                 ++nchanged;
                 if (it1->hits().size() >= it2->hits().size())
                     {mergeTwoClu(*it1,*it2); it2=clusters.erase(it2); --it2;} //remove it2 from list
                 else
                     {mergeTwoClu(*it2,*it1); it1=clusters.erase(it1); --it1; break;} //remove it1 from list
	      }
           }

           ++niter;
           if (nchanged==0) break;

           clusters.remove_if([](auto& cluster){return cluster.hits().empty();});
           for (auto& cluster : clusters )
           {
              if (cluster.hasChanged()) cluster.updateCache(chcol, _maxwt);
              cluster.flagChanged(false);
           }
           if (_diag>0) std::cout<<"Merge "<<niter<<" "<<nchanged<<std::endl;
       }

       return;
   }

   void TNTClusterer::mergeTwoClu(ClusterStraw& clu1, ClusterStraw& clu2 )
   {
       if (clu1.hits().empty() || clu2.hits().empty()) return;
       clu1.hits().insert(clu1.hits().end(),clu2.hits().begin(), clu2.hits().end());
       clu2.hits().clear();
       clu1.flagChanged(true);
   }

   //-------------------------------------------------------------------------------------------------------------------
   // loop over hits, re-affect them to their original cluster if they are still within the radius, otherwise look at
   // candidate clusters to check if they could be added. If not, make a new cluster.
   // to speed up, do not update clusters who haven't changed and keep a list of clusters within a given time window
   unsigned  TNTClusterer::formClusters(const ComboHitCollection& chcol, std::vector<BkgClusterHit>& chits, std::list<ClusterStraw>& clusters)
   {
       unsigned  nchanged(0);
       for (auto& cluster : clusters) cluster.hits().clear();

       for (auto& chit : chits)
       {
          //if (chit._nchanged > 5) continue;

          //check if the current hit is still ok or find the best cluster
          if (_cptrs[chit.index()] != nullptr && chit.distance() < _dhit)
          {
              _cptrs[chit.index()]->hits().emplace_back(&chit);
              continue;
          }

          ClusterStraw* minc(nullptr);
          float mindist(FLT_MAX);
          int hitIdx  = chit.index();
          int itime = int(chcol[hitIdx].time()/10.0);

          for (int i=std::max(0,itime-_ditime);i<itime+_ditime;++i)
          {
             for (const auto& ic : _hitIndex[i])
             {
                 float dist = distance(*ic,chcol[hitIdx]);
                 if (dist < mindist) {mindist = dist; minc = ic;}
                 if (mindist < _dhit) break;
             }
             if (mindist < _dhit) break;
          }

          if (mindist < _dhit)
          {
              minc->hits().emplace_back(&chit); //add to best cluster
          }
          else if (mindist > _dseed)
          {
              clusters.emplace_back(ClusterStraw(chit, chcol[hitIdx]));
              minc = &clusters.back();
              _hitIndex[minc->itime()].emplace_back(minc);
          }
          else
          {
               mindist =10000;
               minc = nullptr;
          }

          if (minc != nullptr)
          {
               //associated to a new cluster, need to flag old/new  cluster accordingly
               if (_cptrs[hitIdx] != minc)
               {
                  ++nchanged;
                  //++chit._nchanged;
                  if (_cptrs[hitIdx] != nullptr) _cptrs[hitIdx]->flagChanged(true);
                  minc->flagChanged(true);
               }
               _cptrs[hitIdx] = minc;
           }
           else
           {
               //unassociated to previous cluster, need to flag old cluster accordingly
               if (_cptrs[hitIdx] != nullptr)
               {
                  ++nchanged;
                  //++chit._nchanged;
                  _cptrs[hitIdx]->flagChanged(true);
               }
               _cptrs[hitIdx] = nullptr;
           }
       }

       //clear cluster timing vector, update clusters and hit distance and refil timing vector
       for (auto& vec: _hitIndex) vec.clear();

       for(auto& cluster : clusters)
       {
           if (cluster.hasChanged())
           {
              cluster.updateCache(chcol, _maxwt);
              cluster.flagChanged(false);

              if (cluster.hits().size()==1)
                cluster.hits().at(0)->distance(0);
              else
                for (auto& hit : cluster.hits()) hit->distance(distance(cluster,chcol[hit->index()]));
           }

           _hitIndex[cluster.itime()].emplace_back(&cluster);
       }

       return nchanged;
   }

   //---------------------------------------------------------------------------------------
   // only count differences if they are above the natural hit size (drift time, straw size)
   float TNTClusterer::distance(const ClusterStraw& cluster, const ComboHit& hit) const
   {
       float dt = std::abs(hit.time()-cluster.time());
       if (dt > _maxdt) return _dseed+1.0;

       XYZVec psep = PerpVector(hit.pos()-cluster.pos(),Geom::ZDir());
       float d2 = psep.mag2();
       if (d2 > _md2) return _dseed+1.0;

       float retval(0.0);
       if (dt > _dt) {float tdist = dt -_dt;  retval = tdist*tdist*_trms2inv;}
       if (d2 > _dd2)
       {
	   XYZVec that(-hit.wdir().y(),hit.wdir().x(),0.0);
           float dw = std::max(0.0f,hit.wdir().Dot(psep)-_dd)/hit.posRes(ComboHit::wire);
	   float dp = std::max(0.0f,that.Dot(psep)-_dd)*_maxwt;  //maxwt = 1/minerr
	   retval += dw*dw + dp*dp;
       }
       return retval;
   }

   //-------------------------------------------------------------------------------------------------------------------
   void TNTClusterer::dump(std::list<ClusterStraw> clusters)
   {
       int iclu(0);
       clusters.sort([](ClusterStraw& a, ClusterStraw& b){return a.hits().at(0)->index() < b.hits().at(0)->index() ;});
       for (auto& cluster: clusters)
       {
           std::cout<<"Cluster "<<iclu<<" "<<cluster.pos()<<" "<<cluster.time()<<"  "<<cluster.hits().size()<<"  - ";
           for (auto& hit : cluster.hits()) std::cout<<hit->index()<<" ";
           std::cout<<std::endl;
           ++iclu;
       }
   }

   //---------------------------------------------------------------------------------------
   void TNTClusterer::fillHitTree(const ComboHitCollection& chcol)
   {
      nhits_ = 0;

      for (size_t ish = 0; ish < chcol.size(); ++ish)
      {
	 hitRad_[nhits_]    = sqrtf(chcol.at(ish).pos().perp2());
	 hitPhi_[nhits_]    = chcol.at(ish).pos().phi();
	 hitTime_[nhits_]   = chcol.at(ish).time();
	 hitNcombo_[nhits_] = chcol.at(ish).nCombo();
	 hmap_[ish]         = nhits_;
	 ++nhits_;
      }

   }

   //---------------------------------------------------------------------------------------
   void TNTClusterer::fillCluTree(const ComboHitCollection& chcol, std::list<ClusterStraw>& clusters,
                                   int npass, float odist, float tdist, int nChanged)
   {

      int nclu(0);
      for (const auto& cluster : clusters)
      {
	 float rdiff(-1.0),r2diff(-1.0),pdiff(-1.0),tdiff(-1.0),trdiff(-1.0);
	 for (unsigned i=0;i<cluster.hits().size() ;++i)
	 {
	    int ishIdx = cluster.hits().at(i)->index();

            float dr = sqrtf(chcol.at(ishIdx).pos().perp2()) - sqrtf(cluster.pos().perp2());
	    float dp = chcol.at(ishIdx).pos().phi()-cluster.pos().phi();
	    float dt = chcol.at(ishIdx).time()-cluster.time();
	    rdiff    = std::max(rdiff,std::abs(dr));
  	    pdiff    = std::max(pdiff,std::abs(dp));
  	    tdiff    = std::max(tdiff,std::abs(dt));
  	    r2diff   = std::max(r2diff,sqrt((chcol.at(ishIdx).pos()-cluster.pos()).perp2()));

            XYZVec psep = PerpVector(chcol.at(ishIdx).pos()-cluster.pos(),Geom::ZDir());
            float dw    = std::max(float(0.0),chcol.at(ishIdx).wdir().Dot(psep)-_dd)/chcol.at(ishIdx).posRes(ComboHit::wire);
	    XYZVec that(-chcol.at(ishIdx).wdir().y(),chcol.at(ishIdx).wdir().x(),0.0);
	    float dpe   = std::max(float(0.0),that.Dot(psep)-_dd)*_maxwt;  //maxwt = 1/minerr
	    float dtr   =  dw*dw + dpe*dpe;
            trdiff = std::max(trdiff,dtr);

	    hcIdxClu_[nhitClu_] = ncluIter_;
	    hcIdxHit_[nhitClu_] = hmap_[ishIdx];
	    hcNpass_[nhitClu_]  = npass;
	    ++nhitClu_;
         }
	 if (cluster.hits().size()>0) ++nclu;

         cluId_[ncluIter_]    = cluster.id();
         cluNpass_[ncluIter_] = npass;
	 cluNhit_[ncluIter_]  = cluster.hits().size();
         cluRad_[ncluIter_]   = sqrtf(cluster.pos().perp2());
         cluPhi_[ncluIter_]   = cluster.pos().phi();
         cluTime_[ncluIter_]  = cluster.time();
         cluR2diff_[ncluIter_] = r2diff;
         cluRdiff_[ncluIter_]  = rdiff;
         cluPdiff_[ncluIter_]  = pdiff;
         cluTdiff_[ncluIter_]  = tdiff;
         cluTRdiff_[ncluIter_] = trdiff;

         ++ncluIter_;
      }
      niter_           = npass;
      nclu_[npass]     = nclu;
      odist_[npass]    = odist;
      tdist_[npass]    = tdist;
      nChanged_[npass] = nChanged;

   }

}

// Note 1: The list allows you to take pointer of its elements, a vector will reallocate and invalidate the pointers
//         The code with the vector version is much more complicated and has a very small performwnce improvement.


#include "IALSLearningConfig.hpp"
#include "IALSTrainer.hpp"
#include "pybind11/cast.h"
#include <Eigen/Sparse>
#include <cstddef>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <sstream>
#include <stdexcept>
#include <vector>

namespace py = pybind11;
using namespace ials11;
using std::vector;

PYBIND11_MODULE(_ials, m) {
  std::stringstream doc_stream;
  doc_stream << "irspack's core module for \"IALSRecommender\"." << std::endl
             << "Built to use" << std::endl
             << "\t" << Eigen::SimdInstructionSetsInUse();

  m.doc() = doc_stream.str();
  py::class_<IALSLearningConfig>(m, "IALSLearningConfig")
      .def(py::init<size_t, Real, Real, Real, int, size_t, bool, size_t>())
      .def(py::pickle(
          [](const IALSLearningConfig &config) {
            return py::make_tuple(config.K, config.alpha, config.reg,
                                  config.init_stdev, config.n_threads,
                                  config.random_seed, config.use_cg,
                                  config.max_cg_steps);
          },
          [](py::tuple t) {
            if (t.size() != 8)
              throw std::runtime_error("invalid state");

            size_t K = t[0].cast<size_t>();
            Real alpha = t[1].cast<Real>();
            Real reg = t[2].cast<Real>();
            Real init_stdev = t[3].cast<Real>();
            size_t n_threads = t[4].cast<size_t>();
            int random_seed = t[5].cast<int>();
            bool use_cg = t[6].cast<bool>();
            size_t max_cg_steps = t[7].cast<size_t>();
            return IALSLearningConfig(K, alpha, reg, init_stdev, n_threads,
                                      random_seed, use_cg, max_cg_steps);
          }));
  py::class_<IALSLearningConfig::Builder>(m, "IALSLearningConfigBuilder")
      .def(py::init<>())
      .def("build", &IALSLearningConfig::Builder::build)
      .def("set_K", &IALSLearningConfig::Builder::set_K)
      .def("set_alpha", &IALSLearningConfig::Builder::set_alpha)
      .def("set_reg", &IALSLearningConfig::Builder::set_reg)
      .def("set_init_stdev", &IALSLearningConfig::Builder::set_init_stdev)
      .def("set_random_seed", &IALSLearningConfig::Builder::set_random_seed)
      .def("set_n_threads", &IALSLearningConfig::Builder::set_n_threads)
      .def("set_use_cg", &IALSLearningConfig::Builder::set_use_cg)
      .def("set_max_cg_steps", &IALSLearningConfig::Builder::set_max_cg_steps);

  py::class_<IALSTrainer>(m, "IALSTrainer")
      .def(py::init<IALSLearningConfig, const SparseMatrix &>())
      .def("step", &IALSTrainer::step)
      .def("user_scores", &IALSTrainer::user_scores)
      .def("transform_user", &IALSTrainer::transform_user)
      .def("transform_item", &IALSTrainer::transform_item)
      .def_readwrite("user", &IALSTrainer::user)
      .def_readwrite("item", &IALSTrainer::item)
      .def(py::pickle(
          [](const IALSTrainer &trainer) {
            return py::make_tuple(trainer.config_, trainer.user, trainer.item);
          },
          [](py::tuple t) {
            if (t.size() != 3)
              throw std::runtime_error("Invalid state!");
            IALSTrainer trainer(t[0].cast<IALSLearningConfig>(),
                                t[1].cast<DenseMatrix>(),
                                t[2].cast<DenseMatrix>());
            return trainer;
          }));
}