#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <string>
#include <random>
#include <set>
#include <sstream>
#include <vector>
#include <array>
#include <algorithm>
#include <util/cityhash_tc.hpp>

// Eigen
#include <Eigen/Core>
#include <Eigen/SparseCore>

// SFrame and Flex type
#include <unity/lib/flex_dict_view.hpp>
#include <random/random.hpp>

// ML-Data Utils
#include <ml_data/ml_data.hpp>
#include <ml_data/ml_data_entry.hpp>
#include <ml_data/metadata.hpp>

// Testing utils common to all of ml_data_iterator
#include <sframe/testing_utils.hpp>
#include <ml_data/testing_utils.hpp>
#include <util/testing_utils.hpp>

#include <globals/globals.hpp>

using namespace turi;

typedef Eigen::Matrix<double, Eigen::Dynamic,1>  DenseVector;
typedef Eigen::SparseVector<double> SparseVector;

struct test_row_bounds  {
 public:

  // Test the block iterator by stress-testing a large number of
  // combinations of bounds, threads, sizes, and types

  enum class target_column_type {NONE, NUMERICAL, CATEGORICAL};

  void run_row_bounds_check_test(size_t n, std::string run_string, target_column_type target_type,
                                 std::vector<size_t> untranslated_columns = {}) {

    globals::set_global("TURI_ML_DATA_TARGET_ROW_BYTE_MINIMUM", 29);
    globals::set_global("TURI_ML_DATA_STATS_PARALLEL_ACCESS_THRESHOLD", 7);

    // This must hold if we are going to have deterministic indexing
    ASSERT_LE(n, 10000);

    random::seed(0);

    bool target_column;

    if(target_type == target_column_type::CATEGORICAL) {
      target_column = true;
      run_string = std::string("Z") + run_string;
    } else if(target_type == target_column_type::NUMERICAL) {
      target_column = true;
      run_string = std::string("n") + run_string;
    } else {
      target_column = false;
    }

    sframe raw_data = make_random_sframe(n, run_string, false);

    if(target_column)
      raw_data.set_column_name(0, "target");

    std::vector<std::vector<flexible_type> > ref_data = testing_extract_sframe_data(raw_data);

    std::map<std::string, ml_column_mode> mode_overrides;

    for(size_t c_idx : untranslated_columns)
      mode_overrides[raw_data.column_name(c_idx)] = ml_column_mode::UNTRANSLATED;

    for(size_t c_idx = target_column ? 1 : 0; c_idx < raw_data.num_columns(); ++c_idx) {
      if(raw_data.column_type(c_idx) == flex_type_enum::INTEGER)
        mode_overrides[raw_data.column_name(c_idx)] = ml_column_mode::CATEGORICAL;
    }

    std::string print_str = run_string;

    if(target_column)
      print_str += ":target";

    bool target_column_categorical = (target_type == target_column_type::CATEGORICAL);

    if(target_column_categorical)
      mode_overrides["target"] = ml_column_mode::CATEGORICAL;
    else
      mode_overrides["target"] = ml_column_mode::NUMERIC;

    ml_data data;

    data.fill(raw_data, (target_column ? "target" : ""), mode_overrides);

    ////////////////////////////////////////////////////////////////////////////////

    std::vector<std::pair<size_t, size_t> > row_segments
    { {0, n},
      {0, n / 3},
      {n / 3, 2*n / 3},
      {2*n / 3, n} };

    for(const std::pair<size_t, size_t>& row_bounds : row_segments) {

      sframe sliced_raw_data = slice_sframe(raw_data, row_bounds.first, row_bounds.second);

      ml_data data_row_sliced;
      data_row_sliced.fill(raw_data, row_bounds, (target_column ? "target" : ""), mode_overrides);

      ml_data data_true;
      data_true.fill(sliced_raw_data, (target_column ? "target" : ""), mode_overrides);

      ml_data data_sliced = data.slice(row_bounds.first, row_bounds.second);

      // Now go through and make sure they are equal.
      data_row_sliced.metadata()->_debug_is_equal(data_true.metadata());

      ASSERT_EQ(data_row_sliced.num_rows(), data_true.num_rows());
      ASSERT_EQ(data_sliced.num_rows(), data_true.num_rows());

      in_parallel([&](size_t thread_idx, size_t num_threads) {

          std::vector<ml_data_entry> x1, x2, x3;
          std::vector<flexible_type> xf1, xf2, xf3;
          std::vector<flexible_type> row_x1, row_x2, row_x3;

          auto it_1 = data_row_sliced.get_iterator(thread_idx, num_threads);
          auto it_2 = data_true.get_iterator(thread_idx, num_threads);
          auto it_3 = data_sliced.get_iterator(thread_idx, num_threads);

          for(;!it_1.done();++it_1, ++it_2, ++it_3) {
            ASSERT_FALSE(it_2.done());
            ASSERT_FALSE(it_3.done());

            ASSERT_EQ(it_1.row_index(), it_2.row_index());
            ASSERT_EQ(it_1.row_index(), it_3.row_index());

            ////////////////////////////////////////////////////////////////////////////////
            // Test that the sliced creation and the creation from sliced are identical

            it_1->fill(x1);
            it_2->fill(x2);
            it_3->fill(x3);

            ASSERT_TRUE(x1 == x2);

            ASSERT_TRUE(it_1->target_index() == it_2->target_index());
            ASSERT_TRUE(it_1->target_value() == it_2->target_value());

            ASSERT_TRUE(it_1->target_value() == it_3->target_value());

            ////////////////////////////////////////////////////////////////////////////////
            // Test reference to the original data, including the sliced data.

            row_x1 = translate_row_to_original(data_row_sliced.metadata(), x1);
            row_x2 = translate_row_to_original(data_true.metadata(), x2);
            row_x3 = translate_row_to_original(data_sliced.metadata(), x3);

            ASSERT_EQ(row_x1.size(), row_x2.size());
            ASSERT_EQ(row_x1.size(), row_x3.size());

            for(size_t ri = 0; ri < row_x1.size(); ++ri) {
              ASSERT_TRUE(ml_testing_equals(row_x1.at(ri), row_x2.at(ri)));
              ASSERT_TRUE(ml_testing_equals(row_x1.at(ri), row_x3.at(ri)));
            }

            ////////////////////////////////////////////////////////////////////////////////
            // Test the unobserved values

            it_1->fill_untranslated_values(xf1);
            it_2->fill_untranslated_values(xf2);
            it_3->fill_untranslated_values(xf3);

            ASSERT_TRUE(xf1 == xf2);
            ASSERT_TRUE(xf1 == xf3);
          }
          ASSERT_TRUE(it_2.done());
          ASSERT_TRUE(it_3.done());
        });
    }
  }

  ////////////////////////////////////////////////////////////////////////////////

  void test_row_bounds_000() {
    // All unique
    run_row_bounds_check_test(0, "n", target_column_type::NONE);
  }

  void test_row_bounds_0n() {
    // All unique
    run_row_bounds_check_test(5, "n", target_column_type::NONE);
  }

  void test_row_bounds_0b() {
    run_row_bounds_check_test(5, "b", target_column_type::NONE);
  }

  void test_row_bounds_0c() {
    // All unique
    run_row_bounds_check_test(5, "c", target_column_type::NONE);
  }

  void test_row_bounds_0C() {
    // All unique
    run_row_bounds_check_test(5, "C", target_column_type::NONE);
  }

  void test_row_bounds_1b_unsorted() {
    run_row_bounds_check_test(13, "b", target_column_type::NONE);
  }

  void test_row_bounds_1() {
    run_row_bounds_check_test(13, "bc", target_column_type::NONE);
  }

  void test_row_bounds_2() {
    run_row_bounds_check_test(13, "zc", target_column_type::NONE);
  }

  void test_row_bounds_3() {
    run_row_bounds_check_test(100, "Zc", target_column_type::NONE);
  }

  void test_row_bounds_4() {
    // Pretty much gonna be unique
    run_row_bounds_check_test(100, "Cc", target_column_type::NONE);
  }

  void test_row_bounds_5() {
    // 10 blocks of values.
    run_row_bounds_check_test(1000, "Zc", target_column_type::NONE);
  }

  void test_row_bounds_6() {
    // two large blocks of values
    run_row_bounds_check_test(1000, "bc", target_column_type::NONE);
  }

  void test_row_bounds_10() {
    // Yeah, a corner case
    run_row_bounds_check_test(1, "bc", target_column_type::NONE);
  }

  void test_row_bounds_11() {
    // One with just a lot of stuff
    run_row_bounds_check_test(200, "u", target_column_type::NONE);
  }

  void test_row_bounds_12() {
    // One with just a lot of stuff
    run_row_bounds_check_test(200, "d", target_column_type::NONE);
  }

  void test_row_bounds_13() {
    // One with just a lot of stuff
    run_row_bounds_check_test(1000, "cnv", target_column_type::NONE);
  }

  void test_row_bounds_14() {
    // One with just a lot of stuff
    run_row_bounds_check_test(1000, "du", target_column_type::NONE);
  }

  void test_row_bounds_15() {
    // One with just a lot of stuff
    run_row_bounds_check_test(3, "UDccccV", target_column_type::NONE);
  }

  void test_row_bounds_100() {
    // One with just a lot of stuff
    run_row_bounds_check_test(10, "Zcuvd", target_column_type::NONE);
  }

  void test_untranslated_columns_nn_1() {
    run_row_bounds_check_test(109, "nn", target_column_type::NONE, {1});
  }

  void test_untranslated_columns_nn_2() {
    run_row_bounds_check_test(109, "nn", target_column_type::NONE, {0});
  }

  void test_untranslated_columns_nn_3() {
    run_row_bounds_check_test(109, "nn", target_column_type::NONE, {0, 1});
  }

  void test_untranslated_columns_ssss_1() {
    run_row_bounds_check_test(109, "ssss", target_column_type::NONE, {1,3});
  }

  void test_untranslated_columns_ssss_2() {
    run_row_bounds_check_test(109, "ssss", target_column_type::NONE, {0,1,2,3});
  }

  void test_untranslated_columns_dd_1() {
    run_row_bounds_check_test(109, "dd", target_column_type::NONE, {1});
  }

  void test_untranslated_columns_dd_2() {
    run_row_bounds_check_test(109, "dd", target_column_type::NONE, {0});
  }

  void test_untranslated_columns_dd_3() {
    run_row_bounds_check_test(109, "dd", target_column_type::NONE, {0,1});
  }

  void test_untranslated_columns_v_1() {
    run_row_bounds_check_test(109, "v", target_column_type::NONE, {0});
  }

  void test_untranslated_columns_many_1() {
    run_row_bounds_check_test(109, "cnsnscsnccccccccncss", target_column_type::NONE,
                              {0, 2, 4, 6, 8, 10, 12, 14, 16, 18});
  }

  void test_untranslated_columns_many_2() {
    run_row_bounds_check_test(109, "cnsnscsnccccccccncss", target_column_type::NONE, {19});
  }

  ////////////////////////////////////////////////////////////////////////////////
  // All the ones with targets

  void test_row_bounds_000_tn() {
    // All unique
    run_row_bounds_check_test(0, "n", target_column_type::NUMERICAL);
  }

  void test_row_bounds_0n_tn() {
    // All unique
    run_row_bounds_check_test(5, "n", target_column_type::NUMERICAL);
  }

  void test_row_bounds_0C_tn() {
    // All unique
    run_row_bounds_check_test(5, "c", target_column_type::NUMERICAL);
  }

  void test_row_bounds_1_unsorted_tn() {
    run_row_bounds_check_test(5, "b", target_column_type::NUMERICAL);
  }

  void test_row_bounds_0b_tn() {
    // All unique
    run_row_bounds_check_test(13, "C", target_column_type::NUMERICAL);
  }

  void test_row_bounds_1b_unsorted_tn() {
    run_row_bounds_check_test(13, "b", target_column_type::NUMERICAL);
  }

  void test_row_bounds_1_tn() {
    run_row_bounds_check_test(13, "bc", target_column_type::NUMERICAL);
  }

  void test_row_bounds_2_tn() {
    run_row_bounds_check_test(13, "zc", target_column_type::NUMERICAL);
  }

  void test_row_bounds_3_tn() {
    run_row_bounds_check_test(100, "Zc", target_column_type::NUMERICAL);
  }

  void test_row_bounds_4_tn() {
    // Pretty much gonna be unique
    run_row_bounds_check_test(100, "Cc", target_column_type::NUMERICAL);
  }

  void test_row_bounds_5_tn() {
    // 10 blocks of values.
    run_row_bounds_check_test(1000, "Zc", target_column_type::NUMERICAL);
  }

  void test_row_bounds_6_tn() {
    // two large blocks of values
    run_row_bounds_check_test(1000, "bc", target_column_type::NUMERICAL);
  }

  void test_row_bounds_10_tn() {
    // Yeah, a corner case
    run_row_bounds_check_test(1, "bc", target_column_type::NUMERICAL);
  }

  void test_row_bounds_11_tn() {
    // One with just a lot of stuff
    run_row_bounds_check_test(200, "u", target_column_type::NUMERICAL);
  }

  void test_row_bounds_12_tn() {
    // One with just a lot of stuff
    run_row_bounds_check_test(200, "d", target_column_type::NUMERICAL);
  }

  void test_row_bounds_13_tn() {
    // One with just a lot of stuff
    run_row_bounds_check_test(1000, "cnv", target_column_type::NUMERICAL);
  }

  void test_row_bounds_14_tn() {
    // One with just a lot of stuff
    run_row_bounds_check_test(1000, "du", target_column_type::NUMERICAL);
  }

  void test_row_bounds_15_tn() {
    // One with just a lot of stuff
    run_row_bounds_check_test(3, "UDccccV", target_column_type::NUMERICAL);
  }

  void test_row_bounds_100_tn() {
    // One with just a lot of stuff
    run_row_bounds_check_test(10, "Zcuvd", target_column_type::NUMERICAL);
  }

  void test_row_bounds_16_null_tn() {
    // two large blocks of values
    run_row_bounds_check_test(1000, "", target_column_type::NUMERICAL);
  }

  void test_untranslated_columns_nn_1_num() {
    run_row_bounds_check_test(109, "nn", target_column_type::NUMERICAL, {1});
  }

  void test_untranslated_columns_nn_2_num() {
    run_row_bounds_check_test(109, "nn", target_column_type::NUMERICAL, {0});
  }

  void test_untranslated_columns_nn_3_num() {
    run_row_bounds_check_test(109, "nn", target_column_type::NUMERICAL, {0, 1});
  }

  void test_untranslated_columns_ssss_1_num() {
    run_row_bounds_check_test(109, "ssss", target_column_type::NUMERICAL, {1,3});
  }

  void test_untranslated_columns_ssss_2_num() {
    run_row_bounds_check_test(109, "ssss", target_column_type::NUMERICAL, {0,1,2,3});
  }

  void test_untranslated_columns_dd_1_num() {
    run_row_bounds_check_test(109, "dd", target_column_type::NUMERICAL, {1});
  }

  void test_untranslated_columns_dd_2_num() {
    run_row_bounds_check_test(109, "dd", target_column_type::NUMERICAL, {0});
  }

  void test_untranslated_columns_dd_3_num() {
    run_row_bounds_check_test(109, "dd", target_column_type::NUMERICAL, {0,1});
  }

  void test_untranslated_columns_v_1_num() {
    run_row_bounds_check_test(109, "v", target_column_type::NUMERICAL, {0});
  }

  void test_untranslated_columns_many_1_num() {
    run_row_bounds_check_test(109, "cnsnscsnccccccccncss", target_column_type::NUMERICAL,
                              {0, 2, 4, 6, 8, 10, 12, 14, 16, 18});
  }

  void test_untranslated_columns_many_2_num() {
    run_row_bounds_check_test(109, "cnsnscsnccccccccncss", target_column_type::NUMERICAL, {19});
  }

  ////////////////////////////////////////////////////////////////////////////////
  // All the ones with targets

  void test_row_bounds_000_tc() {
    // All unique
    run_row_bounds_check_test(0, "n", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_0n_tc() {
    // All unique
    run_row_bounds_check_test(5, "n", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_0C_tc() {
    // All unique
    run_row_bounds_check_test(5, "c", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_1_unsorted_tc() {
    run_row_bounds_check_test(5, "b", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_0b_tc() {
    // All unique
    run_row_bounds_check_test(13, "C", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_1b_unsorted_tc() {
    run_row_bounds_check_test(13, "b", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_1_tc() {
    run_row_bounds_check_test(13, "bc", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_2_tc() {
    run_row_bounds_check_test(13, "zc", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_3_tc() {
    run_row_bounds_check_test(100, "Zc", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_4_tc() {
    // Pretty much gonna be unique
    run_row_bounds_check_test(100, "Cc", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_5_tc() {
    // 10 blocks of values.
    run_row_bounds_check_test(1000, "Zc", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_6_tc() {
    // two large blocks of values
    run_row_bounds_check_test(1000, "bc", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_10_tc() {
    // Yeah, a corner case
    run_row_bounds_check_test(1, "bc", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_11_tc() {
    // One with just a lot of stuff
    run_row_bounds_check_test(200, "u", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_12_tc() {
    // One with just a lot of stuff
    run_row_bounds_check_test(200, "d", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_13_tc() {
    // One with just a lot of stuff
    run_row_bounds_check_test(1000, "cnv", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_14_tc() {
    // One with just a lot of stuff
    run_row_bounds_check_test(1000, "du", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_15_tc() {
    // One with just a lot of stuff
    run_row_bounds_check_test(3, "UDccccV", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_100_tc() {
    // One with just a lot of stuff
    run_row_bounds_check_test(10, "Zcuvd", target_column_type::CATEGORICAL);
  }

  void test_row_bounds_16_null_tc() {
    // two large blocks of values
    run_row_bounds_check_test(1000, "", target_column_type::CATEGORICAL);
  }

  void test_untranslated_columns_nn_1_cat() {
    run_row_bounds_check_test(109, "nn", target_column_type::CATEGORICAL, {1});
  }

  void test_untranslated_columns_nn_2_cat() {
    run_row_bounds_check_test(109, "nn", target_column_type::CATEGORICAL, {0});
  }

  void test_untranslated_columns_nn_3_cat() {
    run_row_bounds_check_test(109, "nn", target_column_type::CATEGORICAL, {0, 1});
  }

  void test_untranslated_columns_ssss_1_cat() {
    run_row_bounds_check_test(109, "ssss", target_column_type::CATEGORICAL, {1,3});
  }

  void test_untranslated_columns_ssss_2_cat() {
    run_row_bounds_check_test(109, "ssss", target_column_type::CATEGORICAL, {0,1,2,3});
  }

  void test_untranslated_columns_dd_1_cat() {
    run_row_bounds_check_test(109, "dd", target_column_type::CATEGORICAL, {1});
  }

  void test_untranslated_columns_dd_2_cat() {
    run_row_bounds_check_test(109, "dd", target_column_type::CATEGORICAL, {0});
  }

  void test_untranslated_columns_dd_3_cat() {
    run_row_bounds_check_test(109, "dd", target_column_type::CATEGORICAL, {0,1});
  }

  void test_untranslated_columns_v_1_cat() {
    run_row_bounds_check_test(109, "v", target_column_type::CATEGORICAL, {0});
  }

  void test_untranslated_columns_many_1_cat() {
    run_row_bounds_check_test(109, "cnsnscsnccccccccncss", target_column_type::CATEGORICAL,
                              {0, 2, 4, 6, 8, 10, 12, 14, 16, 18});
  }

  void test_untranslated_columns_many_2_cat() {
    run_row_bounds_check_test(109, "cnsnscsnccccccccncss", target_column_type::CATEGORICAL, {19});
  }

};

BOOST_FIXTURE_TEST_SUITE(_test_row_bounds, test_row_bounds)
BOOST_AUTO_TEST_CASE(test_row_bounds_000) {
  test_row_bounds::test_row_bounds_000();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_0n) {
  test_row_bounds::test_row_bounds_0n();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_0b) {
  test_row_bounds::test_row_bounds_0b();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_0c) {
  test_row_bounds::test_row_bounds_0c();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_0C) {
  test_row_bounds::test_row_bounds_0C();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_1b_unsorted) {
  test_row_bounds::test_row_bounds_1b_unsorted();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_1) {
  test_row_bounds::test_row_bounds_1();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_2) {
  test_row_bounds::test_row_bounds_2();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_3) {
  test_row_bounds::test_row_bounds_3();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_4) {
  test_row_bounds::test_row_bounds_4();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_5) {
  test_row_bounds::test_row_bounds_5();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_6) {
  test_row_bounds::test_row_bounds_6();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_10) {
  test_row_bounds::test_row_bounds_10();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_11) {
  test_row_bounds::test_row_bounds_11();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_12) {
  test_row_bounds::test_row_bounds_12();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_13) {
  test_row_bounds::test_row_bounds_13();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_14) {
  test_row_bounds::test_row_bounds_14();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_15) {
  test_row_bounds::test_row_bounds_15();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_100) {
  test_row_bounds::test_row_bounds_100();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_nn_1) {
  test_row_bounds::test_untranslated_columns_nn_1();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_nn_2) {
  test_row_bounds::test_untranslated_columns_nn_2();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_nn_3) {
  test_row_bounds::test_untranslated_columns_nn_3();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_ssss_1) {
  test_row_bounds::test_untranslated_columns_ssss_1();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_ssss_2) {
  test_row_bounds::test_untranslated_columns_ssss_2();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_dd_1) {
  test_row_bounds::test_untranslated_columns_dd_1();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_dd_2) {
  test_row_bounds::test_untranslated_columns_dd_2();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_dd_3) {
  test_row_bounds::test_untranslated_columns_dd_3();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_v_1) {
  test_row_bounds::test_untranslated_columns_v_1();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_many_1) {
  test_row_bounds::test_untranslated_columns_many_1();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_many_2) {
  test_row_bounds::test_untranslated_columns_many_2();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_000_tn) {
  test_row_bounds::test_row_bounds_000_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_0n_tn) {
  test_row_bounds::test_row_bounds_0n_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_0C_tn) {
  test_row_bounds::test_row_bounds_0C_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_1_unsorted_tn) {
  test_row_bounds::test_row_bounds_1_unsorted_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_0b_tn) {
  test_row_bounds::test_row_bounds_0b_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_1b_unsorted_tn) {
  test_row_bounds::test_row_bounds_1b_unsorted_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_1_tn) {
  test_row_bounds::test_row_bounds_1_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_2_tn) {
  test_row_bounds::test_row_bounds_2_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_3_tn) {
  test_row_bounds::test_row_bounds_3_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_4_tn) {
  test_row_bounds::test_row_bounds_4_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_5_tn) {
  test_row_bounds::test_row_bounds_5_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_6_tn) {
  test_row_bounds::test_row_bounds_6_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_10_tn) {
  test_row_bounds::test_row_bounds_10_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_11_tn) {
  test_row_bounds::test_row_bounds_11_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_12_tn) {
  test_row_bounds::test_row_bounds_12_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_13_tn) {
  test_row_bounds::test_row_bounds_13_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_14_tn) {
  test_row_bounds::test_row_bounds_14_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_15_tn) {
  test_row_bounds::test_row_bounds_15_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_100_tn) {
  test_row_bounds::test_row_bounds_100_tn();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_16_null_tn) {
  test_row_bounds::test_row_bounds_16_null_tn();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_nn_1_num) {
  test_row_bounds::test_untranslated_columns_nn_1_num();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_nn_2_num) {
  test_row_bounds::test_untranslated_columns_nn_2_num();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_nn_3_num) {
  test_row_bounds::test_untranslated_columns_nn_3_num();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_ssss_1_num) {
  test_row_bounds::test_untranslated_columns_ssss_1_num();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_ssss_2_num) {
  test_row_bounds::test_untranslated_columns_ssss_2_num();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_dd_1_num) {
  test_row_bounds::test_untranslated_columns_dd_1_num();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_dd_2_num) {
  test_row_bounds::test_untranslated_columns_dd_2_num();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_dd_3_num) {
  test_row_bounds::test_untranslated_columns_dd_3_num();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_v_1_num) {
  test_row_bounds::test_untranslated_columns_v_1_num();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_many_1_num) {
  test_row_bounds::test_untranslated_columns_many_1_num();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_many_2_num) {
  test_row_bounds::test_untranslated_columns_many_2_num();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_000_tc) {
  test_row_bounds::test_row_bounds_000_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_0n_tc) {
  test_row_bounds::test_row_bounds_0n_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_0C_tc) {
  test_row_bounds::test_row_bounds_0C_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_1_unsorted_tc) {
  test_row_bounds::test_row_bounds_1_unsorted_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_0b_tc) {
  test_row_bounds::test_row_bounds_0b_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_1b_unsorted_tc) {
  test_row_bounds::test_row_bounds_1b_unsorted_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_1_tc) {
  test_row_bounds::test_row_bounds_1_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_2_tc) {
  test_row_bounds::test_row_bounds_2_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_3_tc) {
  test_row_bounds::test_row_bounds_3_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_4_tc) {
  test_row_bounds::test_row_bounds_4_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_5_tc) {
  test_row_bounds::test_row_bounds_5_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_6_tc) {
  test_row_bounds::test_row_bounds_6_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_10_tc) {
  test_row_bounds::test_row_bounds_10_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_11_tc) {
  test_row_bounds::test_row_bounds_11_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_12_tc) {
  test_row_bounds::test_row_bounds_12_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_13_tc) {
  test_row_bounds::test_row_bounds_13_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_14_tc) {
  test_row_bounds::test_row_bounds_14_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_15_tc) {
  test_row_bounds::test_row_bounds_15_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_100_tc) {
  test_row_bounds::test_row_bounds_100_tc();
}
BOOST_AUTO_TEST_CASE(test_row_bounds_16_null_tc) {
  test_row_bounds::test_row_bounds_16_null_tc();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_nn_1_cat) {
  test_row_bounds::test_untranslated_columns_nn_1_cat();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_nn_2_cat) {
  test_row_bounds::test_untranslated_columns_nn_2_cat();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_nn_3_cat) {
  test_row_bounds::test_untranslated_columns_nn_3_cat();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_ssss_1_cat) {
  test_row_bounds::test_untranslated_columns_ssss_1_cat();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_ssss_2_cat) {
  test_row_bounds::test_untranslated_columns_ssss_2_cat();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_dd_1_cat) {
  test_row_bounds::test_untranslated_columns_dd_1_cat();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_dd_2_cat) {
  test_row_bounds::test_untranslated_columns_dd_2_cat();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_dd_3_cat) {
  test_row_bounds::test_untranslated_columns_dd_3_cat();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_v_1_cat) {
  test_row_bounds::test_untranslated_columns_v_1_cat();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_many_1_cat) {
  test_row_bounds::test_untranslated_columns_many_1_cat();
}
BOOST_AUTO_TEST_CASE(test_untranslated_columns_many_2_cat) {
  test_row_bounds::test_untranslated_columns_many_2_cat();
}
BOOST_AUTO_TEST_SUITE_END()


// Copyright 2018-2020 Petr Petrovich Petrov. All rights reserved.
// License: https://github.com/PetrPPetrov/gkm-world/blob/master/LICENSE

#include <cassert>
#include <cstdint>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <string>
#include <sstream>
#include <vector>
#include <boost/polygon/polygon.hpp>
#include <QPainter>
#include <QApplication>
#include <QStatusBar>
#include "main_monitor_window.h"
#include "balancer_monitor_widget.h"

BalancerMonitorWidget::BalancerMonitorWidget(QWidget* parent) : QWidget(parent) {
    setMouseTracking(true);
    cached_zoom = zoom();
}

void BalancerMonitorWidget::paintEvent(QPaintEvent* event) {
    QPainter painter(this);

    const QRect viewport = painter.viewport();
    screen_center_x = viewport.center().x();
    screen_center_y = viewport.center().y();

    const double world_to_draw_width = viewport.width() / cached_zoom;
    const double world_to_draw_height = viewport.height() / cached_zoom;
    const double actual_cell_size = CELL_SIZE * cached_zoom;
    bool draw_cell_lines = true;
    if (actual_cell_size < 8.0) {
        draw_cell_lines = false;
    }
    bool print_neighbors = true;
    if (actual_cell_size < 50.0) {
        print_neighbors = false;
    }

    const CoordinateType cells_to_draw_width = static_cast<CoordinateType>(world_to_draw_width / CELL_SIZE) + 1;
    const CoordinateType cells_to_draw_height = static_cast<CoordinateType>(world_to_draw_height / CELL_SIZE) + 1;
    const CoordinateType cur_cell_x = static_cast<CoordinateType>(view_point_x / CELL_SIZE);
    const CoordinateType cur_cell_y = static_cast<CoordinateType>(view_point_y / CELL_SIZE);
    const CoordinateType start_cell_x = cur_cell_x - cells_to_draw_width / 2 - 1;
    const CoordinateType start_cell_y = cur_cell_y - cells_to_draw_height / 2 - 1;
    const CoordinateType end_cell_x = cur_cell_x + cells_to_draw_width / 2 + 1;
    const CoordinateType end_cell_y = cur_cell_y + cells_to_draw_height / 2 + 1;

    auto server_info = g_main_window->getServerInfo();
    if (server_info) {
        typedef boost::polygon::point_data<CoordinateType> Point;
        typedef boost::polygon::rectangle_data<CoordinateType> Rectangle;
        typedef boost::polygon::polygon_90_data<CoordinateType> Polygon90;
        typedef boost::polygon::polygon_90_set_data<CoordinateType> Polygon90Set;
        using namespace boost::polygon::operators;

        Polygon90Set result_outside_polygon;

        Rectangle outer_rectangle(start_cell_x, start_cell_y, end_cell_x + 1, end_cell_y + 1);
        result_outside_polygon += outer_rectangle;

        Square2D global_box = server_info->bounding_box;
        Rectangle global_box_rectangle(global_box.start.x, global_box.start.y, global_box.start.x + global_box.size, global_box.start.y + global_box.size);
        result_outside_polygon -= global_box_rectangle;

        QColor out_of_global_box(150, 0, 0);
        QBrush out_of_global_box_brush(out_of_global_box, Qt::BDiagPattern);

        std::vector<Rectangle> rectangles;
        result_outside_polygon.get(rectangles);
        const size_t rectangle_count = rectangles.size();
        for (size_t i = 0; i < rectangle_count; ++i) {
            Rectangle& rectangle = rectangles[i];
            painter.fillRect(
                to_screen_x(xl(rectangle) * CELL_SIZE),
                to_screen_y(yl(rectangle) * CELL_SIZE),
                to_screen_w((xh(rectangle) - xl(rectangle)) * CELL_SIZE),
                to_screen_h((yh(rectangle) - yl(rectangle)) * CELL_SIZE),
                out_of_global_box_brush
            );
        }

        if (g_main_window->isShowLeafNodes()) {
            BalancerTreeInfo* cur_node = server_info->token_to_tree_node.find(server_info->tree_root_token);
            std::list<BalancerTreeInfo*> parent_stack;
            parent_stack.push_back(cur_node);
            while (!parent_stack.empty()) {
                cur_node = parent_stack.back();
                parent_stack.pop_back();
                if (cur_node) {
                    if (cur_node->leaf_node) {
                        QBrush inside_node_brush(getColor(cur_node->token), Qt::SolidPattern);
                        Polygon90Set result_polygon;
                        Square2D node_box = cur_node->bounding_box;
                        Rectangle node_box_rectangle(node_box.start.x, node_box.start.y, node_box.start.x + node_box.size, node_box.start.y + node_box.size);
                        result_polygon += node_box_rectangle;
                        result_polygon *= outer_rectangle;

                        rectangles.clear();
                        result_polygon.get(rectangles);
                        const size_t rectangle_count = rectangles.size();
                        for (size_t i = 0; i < rectangle_count; ++i) {
                            Rectangle& rectangle = rectangles[i];
                            painter.fillRect(
                                to_screen_x(xl(rectangle) * CELL_SIZE),
                                to_screen_y(yl(rectangle) * CELL_SIZE),
                                to_screen_w((xh(rectangle) - xl(rectangle)) * CELL_SIZE),
                                to_screen_h((yh(rectangle) - yl(rectangle)) * CELL_SIZE),
                                inside_node_brush
                            );
                            painter.drawText(
                                to_screen_x(xl(rectangle) * CELL_SIZE),
                                to_screen_y(yl(rectangle) * CELL_SIZE),
                                to_screen_w((xh(rectangle) - xl(rectangle)) * CELL_SIZE),
                                to_screen_h((yh(rectangle) - yl(rectangle)) * CELL_SIZE),
                                Qt::AlignCenter,
                                tr("Node Server %1").arg(cur_node->token)
                            );
                        }
                    } else {
                        for (std::uint8_t i = ChildFirst; i < ChildLast; ++i) {
                            IndexType child_token = cur_node->children[i];
                            if (child_token) {
                                parent_stack.push_back(server_info->token_to_tree_node.find(child_token));
                            }
                        }
                    }
                }
            }
        }

        if (g_main_window->isShowSelectedNode() && server_info->selected_node) {
            Polygon90Set result_selection_polygon;
            Square2D selection_box = server_info->selected_node->bounding_box;
            Rectangle selection_box_rectangle(selection_box.start.x, selection_box.start.y, selection_box.start.x + selection_box.size, selection_box.start.y + selection_box.size);
            result_selection_polygon += selection_box_rectangle;
            result_selection_polygon *= outer_rectangle;

            QColor inside_selected_node(0, 250, 0);
            QBrush inside_selected_node_brush(inside_selected_node, Qt::DiagCrossPattern);

            rectangles.clear();
            result_selection_polygon.get(rectangles);
            const size_t rectangle_count = rectangles.size();
            for (size_t i = 0; i < rectangle_count; ++i) {
                Rectangle& rectangle = rectangles[i];
                painter.fillRect(
                    to_screen_x(xl(rectangle) * CELL_SIZE),
                    to_screen_y(yl(rectangle) * CELL_SIZE),
                    to_screen_w((xh(rectangle) - xl(rectangle)) * CELL_SIZE),
                    to_screen_h((yh(rectangle) - yl(rectangle)) * CELL_SIZE),
                    inside_selected_node_brush
                );
            }

            if (print_neighbors && g_main_window->isShowNeighbor()) {
                for (CoordinateType x = start_cell_x; x <= end_cell_x; ++x) {
                    for (CoordinateType y = start_cell_y; y <= end_cell_y; ++y) {
                        std::pair<CoordinateType, CoordinateType> coordinate(x, y);
                        auto find_neighbor = server_info->selected_node->neighbor_nodes.find(coordinate);
                        if (find_neighbor != server_info->selected_node->neighbor_nodes.end()) {
                            painter.drawText(
                                to_screen_x(x * CELL_SIZE),
                                to_screen_y(y * CELL_SIZE),
                                to_screen_w(CELL_SIZE),
                                to_screen_h(CELL_SIZE),
                                Qt::AlignCenter,
                                tr("Neighbor %1").arg(find_neighbor->second)
                            );
                        }
                    }
                }
            }
        }
    }

    if (draw_cell_lines) {
        QPen thin_pen;
        thin_pen.setColor(QColor(0, 0, 0));
        thin_pen.setStyle(Qt::DotLine);
        painter.setPen(thin_pen);

        for (CoordinateType x = start_cell_x; x <= end_cell_x; ++x) {
            painter.drawLine(
                to_screen_x(x * CELL_SIZE), to_screen_y(start_cell_y * CELL_SIZE),
                to_screen_x(x * CELL_SIZE), to_screen_y((end_cell_y + 1) * CELL_SIZE)
            );
        }
        for (CoordinateType y = start_cell_y; y <= end_cell_y; ++y) {
            painter.drawLine(
                to_screen_x(start_cell_x * CELL_SIZE), to_screen_y(y * CELL_SIZE),
                to_screen_x((end_cell_x + 1) * CELL_SIZE), to_screen_y(y * CELL_SIZE)
            );
        }
    }
}

void BalancerMonitorWidget::wheelEvent(QWheelEvent* event) {
    zoom_step += (event->angleDelta().y() / 120);
    if (zoom_step < MIN_ZOOM_STEP) {
        zoom_step = MIN_ZOOM_STEP;
    }
    if (zoom_step > MAX_ZOOM_STEP) {
        zoom_step = MAX_ZOOM_STEP;
    }
    cached_zoom = zoom();
    update();
}

void BalancerMonitorWidget::mouseMoveEvent(QMouseEvent* event) {
    const QPointF current_point = event->localPos();

    if (left_mouse_pressed) {
        const QPointF delta = previous_point - current_point;
        const double actual_zoom = zoom();
        view_point_x = previous_view_point_x + delta.x() / actual_zoom;
        view_point_y = previous_view_point_y - delta.y() / actual_zoom;
        update();
    }

    const QPoint center = rect().center();
    const QPoint delta = current_point.toPoint() - center;
    const double world_delta_x = delta.x() / cached_zoom;
    const double world_delta_y = delta.y() / cached_zoom;
    const double gkm_world_position_x = view_point_x + world_delta_x;
    const double gkm_world_position_y = view_point_y - world_delta_y;

    QString mouse_position_in_status = tr("Gkm-World Position: ");
    std::stringstream mouse_position;
    mouse_position << std::fixed << std::setprecision(2) << gkm_world_position_x << "; " << gkm_world_position_y;
    mouse_position_in_status += mouse_position.str().c_str();
    g_main_window->statusBar()->showMessage(mouse_position_in_status);
}

void BalancerMonitorWidget::mousePressEvent(QMouseEvent* event) {
    if (event->buttons() & Qt::LeftButton) {
        left_mouse_pressed = true;
        previous_point = event->localPos();
        previous_view_point_x = view_point_x;
        previous_view_point_y = view_point_y;
    }
}

void BalancerMonitorWidget::mouseReleaseEvent(QMouseEvent* event) {
    left_mouse_pressed = false;
    previous_point = event->localPos();
    previous_view_point_x = view_point_x;
    previous_view_point_y = view_point_y;
}

int BalancerMonitorWidget::to_screen_x(double x) const {
    return static_cast<int>((x - view_point_x) * cached_zoom + screen_center_x);
}

int BalancerMonitorWidget::to_screen_y(double y) const {
    return static_cast<int>((y - view_point_y) * -cached_zoom + screen_center_y);
}

int BalancerMonitorWidget::to_screen_w(double w) const {
    return static_cast<int>(w * cached_zoom);
}

int BalancerMonitorWidget::to_screen_h(double h) const {
    return static_cast<int>(h * -cached_zoom);
}

double BalancerMonitorWidget::zoom() const {
    const double scale = std::pow(ZOOM_BASE, INITIAL_ZOOM_STEP);
    return std::pow(ZOOM_BASE, zoom_step) / scale;
}


// Copyright 2018 Google LLC
//
// 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 <stdint.h>  // NOLINT
#include <algorithm>
#include <iomanip>

#include <Eigen/Dense>
#include "cpp/sensor_fusion/geometry_toolbox_mahony.h"
#include "cpp/sensor_fusion/low_pass_filter.h"
#include "cpp/sensor_fusion/orientation_filter.h"
#include "cpp/sensor_fusion/stationary_detector.h"

namespace {

// Minimum time step between sensor updates. This corresponds to 1000 Hz.
static const double kMinTimestepS = 0.001f;

// Maximum time step between sensor updates. This corresponds to 1 Hz.
static const double kMaxTimestepS = 1.0f;

// Gravity constant in m.s^(-2).
static const double kMagnitudeOfGravity = 9.81;

// Small threshold to check if close to zero.
static const double kEpsilon = 1e-9;

// Number of runs for bias correction to remain static.
// This is less than 1 sec.
static const int kMagBiasCorrectionStaticCount = 40;

// Small threshold used for mag initialization.
static const double kMagInitTolerance = 1e-6;

// Number of mag samples used for mag measurement initialization. 25 samples
// correspond to ~.4 seconds for mag at 60 Hz.
static const int kNumMagForInitialization = 25;

// Number of continuous invalid mag measurement that results in temporarily
// disabling mag fusion, unless new mag calibration is performed.
static const int kNumMagForFilterOutlierRejection = 10;

// Threshold of rejecting using one mag measurement. The value for this
// threshold is in rad, which corresponds to 5 degree.
static const double kMaxAllowedMagDeviationRadians = 0.0872665;

// Threshold of using mag measurements on its timestamp. If the timestamp
// between mag and gyro is larger than the given value, we simply do not use
// the current mag measurement. This behavior happens in this multi-threaded
// sensor fusion system, but not often.
static const double kMaxTimeDifferentInMagAndGyroInSeconds = 0.003f;

// Helper method to validate the time steps of sensor timestamps.
bool IsTimestampDeltaValid(double timestamp_delta_s) {
  if (timestamp_delta_s <= kMinTimestepS) {
    return false;
  }
  if (timestamp_delta_s > kMaxTimestepS) {
    return false;
  }
  return true;
}

mahony_filter::StationaryDetector::StationaryDetectorConfiguration
GetStationaryDetectorConfig(double stationary_bias_correction_gain) {
  mahony_filter::StationaryDetector::StationaryDetectorConfiguration
      stationary_config;
  stationary_config.stationary_bias_correction_gain =
      stationary_bias_correction_gain;
  return stationary_config;
}

}  // namespace

OrientationFilter::OrientationFilter(
    const OrientationFilterConfiguration& config)
    : config_(config),
      state_(Eigen::Matrix<double, 7, 1>::Zero()),
      next_state_(Eigen::Matrix<double, 7, 1>::Zero()),
      is_orientation_initialized_(false),
      first_accel_timestamp_s_(0.0),
      has_received_gyro_sample_(false),
      mag_is_available_(false),
      mag_low_pass_filter_(config.magnetometer_low_pass_cutoff_frequency),
      accel_aligned_R_yaw_mag_aligned_(Eigen::Matrix3d::Identity()),
      num_mag_measurements_for_yaw_initialization_(kNumMagForInitialization),
      mag_meas_for_init_index_(0),
      mag_bias_(Eigen::Vector3d::Zero()),
      new_mag_calibration_available_(false),
      mag_status_(MagStatus::kInitial),
      accumulated_num_of_outlier_mag_measurement_(0),
      current_mag_sample_fits_calibration_(false),
      stationary_detector_(
          GetStationaryDetectorConfig(config.stationary_bias_correction_gain)) {
  // Initialize quaternions to start with the identity rotation.
  state_(3) = 1.0;
  next_state_(3) = 1.0;

  // Initialize the data buffer for mag initialization.
  projected_mag_measurement_vector_.resize(
      Eigen::NoChange, num_mag_measurements_for_yaw_initialization_);

  // Set bias of magnetometer calibrator when it's provided in configuration.
  if (config_.init_mag_bias != Eigen::Vector3d::Zero()) {
    SetMagBias(config_.init_mag_bias);
  }
}

void OrientationFilter::Run() {
  // Initialization for the filter orientation.
  if (!is_orientation_initialized_ &&
      current_accel_measurement_.timestamp_s > 0.0 &&
      (config_.init_config ==
           OrientationFilterConfiguration::InitialOrientationConfiguration::
               kDontUseMagToInitOrientation ||
       current_mag_measurement_.timestamp_s > 0.0)) {
    is_orientation_initialized_ = OrientationFromAccelAndMag();
  }

  // Only start to propagate once the orientation has already been initialized
  // by the accel / mag.
  if (is_orientation_initialized_) {
    FilterPropagate();
  }
}

void OrientationFilter::RegisterOnBadMagnetometerCalibrationDetectedCallback(
    std::function<void()>* on_bad_mag_calibration_detected_callback) {
  on_bad_mag_calibration_detected_callbacks_.insert(
      on_bad_mag_calibration_detected_callback);
}

void OrientationFilter::UnRegisterOnBadMagnetometerCalibrationDetectedCallback(
    std::function<void()>* on_bad_mag_calibration_detected_callback) {
  on_bad_mag_calibration_detected_callbacks_.erase(
      on_bad_mag_calibration_detected_callback);
}

void OrientationFilter::BadMagnetometerCalibrationDetectedBroadcast() const {
  for (auto callback : on_bad_mag_calibration_detected_callbacks_) {
    if (callback) {
      (*callback)();
    }
  }
}

void OrientationFilter::AddAccelMeasurement(const Eigen::Vector3d& sample,
                                            double timestamp_s) {
  if (first_accel_timestamp_s_ == 0.0) {
    first_accel_timestamp_s_ = timestamp_s;
    if (is_orientation_initialized_) {
      // Check if the state is aligned with gravity and fix it if it is not.
      const Eigen::Vector3d g_from_acc = sample.normalized();
      const Eigen::Vector3d g_est = ComputeGravityEstimate();

      double dot_product = g_from_acc.dot(g_est);
      // Clamp dot_prodcut.
      if (std::abs(dot_product) > 1.0) {
        dot_product = (dot_product > 0.0 ? 1.0 : -1.0);
      }
      const double angular_error_deg = 180. / M_PI * acos(dot_product);
      // TODO: Add a better gravity / acceleration check here.
      if (std::abs(angular_error_deg) > 44) {
        // The error after SetPose is too large. Correct directly the aligment
        // with gravity.
        const Eigen::Vector4d correction_quat =
            geometry_toolbox::RotateInto(g_est, g_from_acc);

        state_.head<4>() = geometry_toolbox::QuaternionMultiplication(
            correction_quat, state_.head<4>());
      }
    }
  }

  current_accel_measurement_ = SensorSample(sample, timestamp_s);

  if (IsStationaryBiasCorrectionEnabled()) {
    stationary_detector_.AddAccelMeasurement(current_accel_measurement_.sample,
                                             timestamp_s);
  }

  if (!has_received_gyro_sample_) {
    // If we have not received a gyro sample supply fake gyro data to make use
    // of every accel sample.
    current_gyro_measurement_ =
        SensorSample(Eigen::Vector3d::Zero(), timestamp_s);
    Run();
  }
}

void OrientationFilter::AddGyroMeasurement(const Eigen::Vector3d& sample,
                                           double timestamp_s) {
  current_gyro_measurement_ = SensorSample(sample, timestamp_s);

  if (IsStationaryBiasCorrectionEnabled()) {
    stationary_detector_.AddGyroMeasurement(current_gyro_measurement_.sample,
                                            timestamp_s);
  }

  const double delta_t_s =
      (timestamp_s - previous_gyro_measurement_.timestamp_s);
  if (IsTimestampDeltaValid(delta_t_s)) {
    if (!has_received_gyro_sample_) {
      has_received_gyro_sample_ = true;
    }
    Run();
  }

  previous_gyro_measurement_ = current_gyro_measurement_;
}

void OrientationFilter::AddMagMeasurement(const Eigen::Vector3d& sample,
                                          double timestamp_s,
                                          bool fits_calibration) {
  // First sample is only used for setting up the timeline.
  if (current_mag_measurement_.timestamp_s == 0.0) {
    current_mag_measurement_ = SensorSample(sample, timestamp_s);
    current_mag_sample_fits_calibration_ = fits_calibration;
    return;
  }

  const double mag_delta_time_s =
      (timestamp_s - previous_mag_measurement_.timestamp_s);

  // Use uncalibrated mag correction only if stationary correction is not
  // enabled.
  if (!IsStationaryBiasCorrectionEnabled()) {
    // Now that the timestep is valid we can initialize the low_pass filter.
    mag_low_pass_filter_.AddSampleData(sample, mag_delta_time_s);
    if (mag_low_pass_filter_.IsInitialized()) {
      previous_mag_measurement_ =
          SensorSample(mag_low_pass_filter_.GetFilteredData(),
                       current_mag_measurement_.timestamp_s);
    }
  } else {
    previous_mag_measurement_ = current_mag_measurement_;
  }

  current_mag_measurement_ = SensorSample(sample, timestamp_s);
  current_mag_sample_fits_calibration_ = fits_calibration;

  // Only process valid mag sample.
  mag_is_available_ = IsTimestampDeltaValid(mag_delta_time_s);
}

bool OrientationFilter::ComputeIterativeSolution(
    const Eigen::VectorXd& mag_vector, const double initial_solution,
    Eigen::Matrix3d* accel_aligned_R_yaw_mag_aligned) {
  CHECK(accel_aligned_R_yaw_mag_aligned != nullptr);
  double current_solution = initial_solution;

  // Parameters for Gauss-Newton algorithm.
  constexpr int kMaxIteration = 25;
  constexpr double kNormCorrectionForConvergence = 1e-5;

  int current_iteration = 0;
  for (; current_iteration < kMaxIteration; ++current_iteration) {
    // Estimated measurement and Jacobian for Gauss-Newton.
    Eigen::Vector2d z_est(-std::sin(current_solution),
                          std::cos(current_solution));
    Eigen::Vector2d Jacobian(-std::cos(current_solution),
                             -std::sin(current_solution));

    double residual = 0;
    double hessian = 0;

    for (int i = 0; i < mag_vector.rows() / 2; ++i) {
      const Eigen::Vector2d residual_i = z_est - mag_vector.segment<2>(i * 2);
      residual += Jacobian.transpose() * residual_i;
      hessian += Jacobian.transpose() * Jacobian;
    }

    double correction = -residual / hessian;
    if (hessian < kMagInitTolerance) {
      // Invalid Hessian, mag initialization fails.
      return false;
    }

    current_solution += correction;

    if (std::abs(correction) < kNormCorrectionForConvergence) {
      break;
    }
  }

  if (current_iteration >= kMaxIteration) {
    // Maximum iteration reached, mag initialization fails.
    return false;
  }

  *accel_aligned_R_yaw_mag_aligned << std::cos(current_solution),
      -std::sin(current_solution), 0, std::sin(current_solution),
      std::cos(current_solution), 0, 0, 0, 1;

  return true;
}

bool OrientationFilter::ComputeYawAlignmentMatrix(
    const Eigen::Matrix3Xd& mag_proj,
    Eigen::Matrix3d* accel_aligned_R_yaw_mag_aligned) {
  CHECK(accel_aligned_R_yaw_mag_aligned != nullptr);
  const int num_rows = 2 * mag_proj.cols();
  Eigen::VectorXd mag_vector(num_rows, 1);
  for (int i = 0; i < mag_proj.cols(); ++i) {
    if (mag_proj.col(i).head<2>().squaredNorm() < kMagInitTolerance) {
      return false;
    }

    mag_vector.segment<2>(i * 2) = mag_proj.col(i).head<2>().normalized();
  }

  const double init_solution = -std::atan2(mag_vector(0), mag_vector(1));

  return ComputeIterativeSolution(mag_vector, init_solution,
                                  accel_aligned_R_yaw_mag_aligned);
}

bool OrientationFilter::OrientationFromAccelAndMag() {
  Eigen::Vector3d L_x;
  Eigen::Vector3d L_y;
  Eigen::Vector3d L_z;
  Eigen::Matrix<double, 3, 3> L_R_G;

  // East-North-Up frame of reference:
  //   - Gravity vector lies along +z axis
  //   - Horizontal component of Mag vector lies along +y axis
  //   - x-axis points east
  L_z = current_accel_measurement_.sample.normalized();

  switch (config_.init_config) {
    case OrientationFilterConfiguration::InitialOrientationConfiguration::
        kDontUseMagToInitOrientation: {
      // Depending on whether we are portrait or landscape, this will
      // be the direction we expect gravity to be pointing if the device
      // is right-side-up.
      Eigen::Vector3d canonical_down;
      // Is it more in landscape or in portrait mode?
      if (std::abs(L_z.dot(Eigen::Vector3d::UnitY())) <
          std::abs(L_z.dot(Eigen::Vector3d::UnitX()))) {
        // Landscape
        L_y = Eigen::Vector3d::UnitY();
        canonical_down = Eigen::Vector3d::UnitX();
      } else {
        // Portrait
        L_y = -Eigen::Vector3d::UnitX();
        canonical_down = Eigen::Vector3d::UnitY();
      }
      // Is it right-side-up or upside-down?
      if (L_z.dot(canonical_down) < 0) {
        L_y = -L_y;
      }
      break;
    }
    case OrientationFilterConfiguration::InitialOrientationConfiguration::
        kUseMagToInitOrientation:
      L_y = current_mag_measurement_.sample.normalized();
      L_y -= L_z * L_z.transpose() * L_y;
  }

  L_x = -L_z.cross(L_y);
  if (L_x.norm() == 0.0) {
    return false;
  }

  L_x = L_x / L_x.norm();
  L_y = L_z.cross(L_x);
  if (L_y.norm() == 0.0) {
    return false;
  }

  L_R_G.block<3, 1>(0, 0) = L_x;
  L_R_G.block<3, 1>(0, 1) = L_y;
  L_R_G.block<3, 1>(0, 2) = L_z;

  // Compute and assign the quaternion of orientation from the resulting
  // rotation matrix.
  state_.block<4, 1>(0, 0) =
      geometry_toolbox::RotationMatrixToQuaternion(L_R_G);

  state_from_previous_mag_ = state_;
  mag_is_available_ = false;
  return true;
}

void OrientationFilter::FilterPropagate() {
  // TODO: Orientation filter should be robust to different update
  // frequency.
  const double delta_t = current_gyro_measurement_.timestamp_s -
                         previous_gyro_measurement_.timestamp_s;
  if (!IsTimestampDeltaValid(delta_t)) {
    return;
  }

  // Ignore the current mag measurement if it is too old.
  if (mag_is_available_) {
    const double gyro_time_ahead = current_gyro_measurement_.timestamp_s -
                                   current_mag_measurement_.timestamp_s;

    if (gyro_time_ahead > kMaxTimeDifferentInMagAndGyroInSeconds) {
      // If this behavior happens very often, we should project previous mag
      // measurement onto current timestamp, to do not waste too much data.
      // In practice, in current experiments, this happens at a very low
      // frequency.
      mag_is_available_ = false;
    }
  }

  Eigen::Matrix<double, 6, 1> gyro_measurements;
  gyro_measurements.head<3>() = previous_gyro_measurement_.sample;
  gyro_measurements.tail<3>() = current_gyro_measurement_.sample;
  // Bias compensate the gyro data. Since imu_measurement format is
  // [prev_x, prev_y, prev_z, curr_x, curr_y, curr_z] this can be accomplished
  // by subtracting the bias_x, bias_y, bias_z from the appropriate elements.
  const Eigen::Vector3d rate_correction = ComputeAccelAndMagRateCorrection();

  gyro_measurements.head<3>() +=
      (-state_.tail<3>() +
       (IsInitializing()
            ? config_.attitude_correction_gain_during_initialization
            : config_.attitude_correction_gain) *
           rate_correction);
  gyro_measurements.tail<3>() +=
      (-state_.tail<3>() +
       (IsInitializing()
            ? config_.attitude_correction_gain_during_initialization
            : config_.attitude_correction_gain) *
           rate_correction);

  Eigen::Matrix<double, 4, 1> current_q, next_q;
  current_q = state_.head<4>();
  next_q = state_.head<4>();
  quaternion_integrator_.Integrate(current_q, gyro_measurements, delta_t,
                                   &next_q);
  next_state_.head<4>() = next_q;
  next_state_.tail<3>() =
      state_.tail<3>() -
      ((IsInitializing() ? 0.0 : config_.gyroscope_bias_correction_gain) *
       delta_t * rate_correction);

  // Bias correction if stationary.
  if (IsStationaryBiasCorrectionEnabled()) {
    next_state_.tail<3>() -=
        delta_t *
        stationary_detector_.GetGyroBiasCorrection(
            next_state_.tail<3>(), current_gyro_measurement_.timestamp_s);
  }
  state_ = next_state_;

  if (mag_is_available_) {
    const double mag_delta_t = (current_mag_measurement_.timestamp_s -
                                previous_mag_measurement_.timestamp_s);
    if (!IsStationaryBiasCorrectionEnabled()) {
      // Only apply this correction if the stationary bias correction is not
      // enabled.
      const Eigen::Vector3d mag_correction = EstimateBiasUpdateUsingMag();
      state_.tail<3>() -=
          mag_delta_t *
          config_.magnetometer_gain_for_gyroscope_bias_estimation *
          mag_correction;
    }

    state_from_previous_mag_ = state_;
    mag_is_available_ = false;
  }
}

Eigen::Vector3d OrientationFilter::EstimateBiasUpdateUsingMag() {
  if (!mag_low_pass_filter_.IsInitialized()) {
    return Eigen::Vector3d::Zero();
  }
  // Check that gyro norm is below threshold. This is to ensure we only estimate
  // drift when device is still and gyro signal is most likely due to drift.
  const bool is_gyro_static =
      current_gyro_measurement_.sample.norm() <
      config_.maximum_allowed_gyro_norm_changed_for_mag_bias_correction;

  // Check that mag norm is below threshold. This is to ensure we only estimate
  // drift when device is still and mag signal is similar to noise.
  const bool is_mag_static =
      (previous_mag_measurement_.sample -
       mag_low_pass_filter_.GetFilteredData())
          .norm() < config_.maximum_allowed_magnitude_magnetometer_change_mt;
  mag_low_pass_filter_.SetIsStatic(is_gyro_static && is_mag_static);

  if (!mag_low_pass_filter_.GetIsStaticForN(kMagBiasCorrectionStaticCount)) {
    return Eigen::Vector3d::Zero();
  }

  // Get down direction from both previous mag and current state.
  const Eigen::Vector3d previous_accel_est =
      geometry_toolbox::QuaternionToRotationMatrix(
          state_from_previous_mag_.head<4>())
          .col(2);
  const Eigen::Vector3d previous_mag_est =
      geometry_toolbox::QuaternionToRotationMatrix(
          state_from_previous_mag_.head<4>())
          .col(1);
  const Eigen::Vector3d current_accel_est =
      geometry_toolbox::QuaternionToRotationMatrix(state_.head<4>()).col(2);
  const Eigen::Vector3d current_mag_est =
      geometry_toolbox::QuaternionToRotationMatrix(state_.head<4>()).col(1);

  // Grab the previous mag, remove gravity and normalize.
  Eigen::Vector3d previous_mag_meas = previous_mag_measurement_.sample;
  previous_mag_meas -=
      previous_accel_est * previous_accel_est.transpose() * previous_mag_meas;
  previous_mag_meas.normalize();

  // Grab the current mag, remove gravity and normalize.
  Eigen::Vector3d mag_meas = mag_low_pass_filter_.GetFilteredData();
  mag_meas -= current_accel_est * current_accel_est.transpose() * mag_meas;
  mag_meas.normalize();

  return mag_meas.cross(previous_mag_meas) -
         current_mag_est.cross(previous_mag_est);
}

Eigen::Vector3d OrientationFilter::ComputeAccelAndMagRateCorrection() {
  Eigen::Vector3d accel_meas = current_accel_measurement_.sample;
  const double accel_magnitude = accel_meas.norm();
  if (accel_magnitude < 1e-6) {
    return Eigen::Vector3d::Zero();
  }

  accel_meas.normalize();
  Eigen::Matrix<double, 3, 3> L_R_G_accel_aligned =
      geometry_toolbox::QuaternionToRotationMatrix(state_.head<4>());
  Eigen::Vector3d accel_est = L_R_G_accel_aligned.col(2);

  // Dampen the effect of body acceleration. This is only applied when not
  // initializing because it would otherwise slow down convergence.
  double gain = 1.0;
  const double gyro_norm = current_gyro_measurement_.sample.norm();
  if (!IsInitializing()) {
    // Don't update filter while moving too fast.
    // TODO: Consider using something different than this, since most
    // accelerometer sensors don't report 1G at rest.
    gain /= (1.0 + std::abs(accel_magnitude - kMagnitudeOfGravity));

    // Don't update filter while moving to fast.
    // Use full gain between 0 and 0.1, dampen toward 0 between 0.1 to
    // 0.3 rad.s^(-1).
    // 1 __                  //
    //     \                 //
    //      \                //
    //       \______ 0.0     //
    //  ^  ^  ^              //
    //  0 .1  .3             //
    gain *= std::max(0.0, std::min(1.0, 1.5 - 5.0 * gyro_norm));
  } else {
    // Use full gain between 0 and 0.04, dampen toward 0 between 0.04 to
    // 0.1 rad.s^(-1).
    // 1 __                  //
    //     \                 //
    //      \                //
    //       \______ 0.0     //
    //  ^  ^  ^              //
    //  0 .04  .1            //
    gain *= std::max(0.0, std::min(1.0, 1.5 - 15.0 * gyro_norm));
  }

  Eigen::Vector3d rate_correction_vector = gain / 2 *
                                           config_.accel_yaw_correction_gain *
                                           accel_meas.cross(accel_est);

  // Check if there is a valid mag sample ready to be consumed.
  if (mag_is_available_ && current_mag_sample_fits_calibration_ &&
      config_.mag_yaw_correction_gain > 0 &&
      current_mag_measurement_.timestamp_s > 0) {
    // First check if received a new mag calibration.
    if (new_mag_calibration_available_) {
      // Invalidate the existing mag alignement and force recompute.
      mag_status_ = MagStatus::kAligning;
      mag_meas_for_init_index_ = 0;
      LOG(INFO) << "SensorFusion: Received new bias, estimating alignment.";
      new_mag_calibration_available_ = false;
    }

    const Eigen::Vector3d current_calibrated_mag_measurement =
        current_mag_measurement_.sample - mag_bias_;

    // Compute estimated mag value, projected on yaw and represented locally.
    Eigen::Vector3d mag_est_projection = current_calibrated_mag_measurement;
    mag_est_projection -=
        accel_est * accel_est.transpose() * mag_est_projection;
    mag_est_projection.normalize();

    // Estimate alignment between the filter orientation and magnetic north.
    if (mag_status_ == MagStatus::kAligning) {
      // Store the projected mag in a buffer that will be used by the iterative
      // solver.
      projected_mag_measurement_vector_.col(mag_meas_for_init_index_) =
          L_R_G_accel_aligned.transpose() * mag_est_projection;

      ++mag_meas_for_init_index_;

      // Check if we have enough sample to run the iterative solver.
      if (mag_meas_for_init_index_ ==
          num_mag_measurements_for_yaw_initialization_) {
        if (ComputeYawAlignmentMatrix(projected_mag_measurement_vector_,
                                      &accel_aligned_R_yaw_mag_aligned_)) {
          // In this case, mag is successfully initialized.
          mag_status_ = MagStatus::kAligned;
        } else {
          // Mag initialization fails. Clear mag measurements, and re-start data
          // collection.
          mag_meas_for_init_index_ = 0;
          LOG(INFO) << "SensorFusion: Mag alignment failed in orientation "
                       "tracker. Will retry.";
        }
      }
    }

    Eigen::Vector3d mag_meas = current_calibrated_mag_measurement;
    mag_meas -= accel_meas * accel_meas.transpose() * mag_meas;
    if (mag_meas.norm() < 1e-6) {
      return Eigen::Vector3d();
    }
    mag_meas.normalize();

    if (mag_status_ == MagStatus::kAligned) {
      // Magnetometer is calibrated and aligned with the filter now so we can
      // use it for yaw correction.
      mag_is_available_ = false;

      const Eigen::Vector3d mag_est =
          (L_R_G_accel_aligned * accel_aligned_R_yaw_mag_aligned_).col(1);

      // Test whether the current mag estimate and mag measurement diverge too
      // much.
      const double angle_between_est_and_meas =
          std::acos(mag_est_projection.dot(mag_est));

      if (angle_between_est_and_meas > kMaxAllowedMagDeviationRadians) {
        ++accumulated_num_of_outlier_mag_measurement_;
        if (accumulated_num_of_outlier_mag_measurement_ >
            kNumMagForFilterOutlierRejection) {
          // Invalidate the existing mag alignement and force recompute.
          mag_status_ = MagStatus::kAligning;
          state_from_previous_mag_ = state_;
          mag_meas_for_init_index_ = 0;
          LOG(INFO) << "Consistently received outlier measurements, resetting.";
          BadMagnetometerCalibrationDetectedBroadcast();
          accumulated_num_of_outlier_mag_measurement_ = 0;
        }
      } else {
        rate_correction_vector +=
            config_.mag_yaw_correction_gain * mag_meas.cross(mag_est);

        accumulated_num_of_outlier_mag_measurement_ = 0;
      }
    }
  }

  return rate_correction_vector;
}

void OrientationFilter::Recenter() {
  // This function enforces two constraints on the updated L_R_G_recenter
  // - The gravity direction is kept
  //    L_R_G_recenter * z = L_R_G * z
  // - yaw is equal to zero
  //    L_R_G_recenter^{-1} * -z  =[sin(pitch) 0 cos(pitch)]
  // Notation:
  // L_R_G^{-1} * -z =  [cos(yaw) * sin(pitch) sin(yaw) cos(yaw) * cos(pitch)]

  //
  // We can deduce that
  // L_R_G_recenter(:,3) == L_R_G(:,3) (1)
  // L_R_G_recenter(3,2) = 0.0 (2)
  //
  // As L_R_G is an orthonormal matrix
  // |L_R_G(:,2)| = 1 (3)
  //
  // Combining (2) and (3)
  // |L_R_G(1:2,2)| = 1 (4)
  // L_R_G(1:2,2) . L_R_G(1:2,3) = 0  (5) Orthogonality
  //
  // if L_R_G(1,3) >  Eps
  // L_R_G(2,2) =  +/- sqrt( 1 / ( 1 + (L_R_G(2,3) / L_R_G(1,3))^2)) (6)
  // L_R_G(1,2) = - L_R_G(2,2) * L_R_G(2,3) / L_R_G(1,3)
  //
  // Finally we use the orthogonality constraint for the first column
  // L_R_G(:,1) = L_R_G(:,2) x L_R_G(:,3)

  const Eigen::Matrix3d L_R_G =
      geometry_toolbox::QuaternionToRotationMatrix(state_.head<4>());

  Eigen::Matrix3d L_R_G_recentered;
  // Keep down direction the same (1).
  L_R_G_recentered.block<3, 1>(0, 2) = L_R_G.block<3, 1>(0, 2);

  if (std::abs(L_R_G(0, 2)) < kEpsilon) {
    // Arbitrary deciding to use X axis.
    L_R_G_recentered(0, 1) = 1.0;
    L_R_G_recentered(1, 1) = 0.0;
  } else {
    const double x_y_ratio = L_R_G(1, 2) / L_R_G(0, 2);  // Eq. (6)
    // The sign  is kept positive to enforce "forwardness".
    L_R_G_recentered(1, 1) = sqrt(1.0 / (1 + x_y_ratio * x_y_ratio));
    L_R_G_recentered(0, 1) = -L_R_G_recentered(1, 1) * x_y_ratio;
  }
  L_R_G_recentered(2, 1) = 0.0;  // Eq. (2)

  // Enforce orthogonality.
  L_R_G_recentered.block<3, 1>(0, 0) = L_R_G_recentered.block<3, 1>(0, 1).cross(
      L_R_G_recentered.block<3, 1>(0, 2));

  state_.head<4>() =
      geometry_toolbox::RotationMatrixToQuaternion(L_R_G_recentered);

  // Reset the mag based bias update by setting the previous state to current
  // state. This avoids using the change in pose due to recentering for bias
  // update.
  state_from_previous_mag_ = state_;
}


/****************************************************************************
 *
 * This is a part of CTPPS offline software.
 * Authors:
 *   Laurent Forthomme (laurent.forthomme@cern.ch)
 *   Nicola Minafra (nicola.minafra@cern.ch)
 *   Filip Dej
 *
 ****************************************************************************/

#include "RecoCTPPS/TotemRPLocal/interface/TotemTimingConversions.h"
#include "FWCore/Utilities/interface/Exception.h"

#include <boost/exception/exception.hpp>

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

const float TotemTimingConversions::SAMPIC_SAMPLING_PERIOD_NS = 1. / 7.695;
const float TotemTimingConversions::SAMPIC_ADC_V = 1. / 256;
const int TotemTimingConversions::SAMPIC_MAX_NUMBER_OF_SAMPLES = 64;
const int TotemTimingConversions::SAMPIC_DEFAULT_OFFSET = 30;
const int TotemTimingConversions::ACCEPTED_TIME_RADIUS = 4;
const unsigned long TotemTimingConversions::CELL0_MASK = 0xfffffff000;

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

TotemTimingConversions::TotemTimingConversions(bool mergeTimePeaks, const std::string& calibrationFile) :
  calibrationFileOpened_(false),
  mergeTimePeaks_(mergeTimePeaks)
{
  if (!calibrationFile.empty())
    try {
      parsedData_.parseFile(calibrationFile);
      calibrationFunction_ = std::make_unique<reco::FormulaEvaluator>(parsedData_.getFormula());
      calibrationFileOpened_ = true;
    } catch (const boost::exception& e) {
      throw cms::Exception("TotemTimingConversions")
        << "Failed to open calibration file \"" << calibrationFile << "\" for parsing!";
    }
}

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

float
TotemTimingConversions::getTimeOfFirstSample(const TotemTimingDigi& digi) const
{
  unsigned int offsetOfSamples = digi.getEventInfo().getOffsetOfSamples();
  if (offsetOfSamples == 0)
    offsetOfSamples = SAMPIC_DEFAULT_OFFSET; // FW 0 is not sending this, FW > 0 yes

  unsigned int timestamp = (digi.getCellInfo() <= SAMPIC_MAX_NUMBER_OF_SAMPLES/2)
    ? digi.getTimestampA()
    : digi.getTimestampB();

  int cell0TimeClock = timestamp +
    ((digi.getFPGATimestamp()-timestamp) & CELL0_MASK)
    - digi.getEventInfo().getL1ATimestamp()
    + digi.getEventInfo().getL1ALatency();

  // time of first cell
  float cell0TimeInstant = SAMPIC_MAX_NUMBER_OF_SAMPLES * SAMPIC_SAMPLING_PERIOD_NS * cell0TimeClock;

  // time of triggered cell
  float firstCellTimeInstant = (digi.getCellInfo() < offsetOfSamples)
    ? cell0TimeInstant + digi.getCellInfo() * SAMPIC_SAMPLING_PERIOD_NS
    : cell0TimeInstant - (SAMPIC_MAX_NUMBER_OF_SAMPLES - digi.getCellInfo())*SAMPIC_SAMPLING_PERIOD_NS;

  int db = digi.getHardwareBoardId();
  int sampic = digi.getHardwareSampicId();
  int channel = digi.getHardwareChannelId();
  float t = firstCellTimeInstant + parsedData_.getTimeOffset(db, sampic, channel);
  //NOTE: If no time offset is set, getTimeOffset returns 0

  if (mergeTimePeaks_) {
    if (t < -ACCEPTED_TIME_RADIUS)
      t += SAMPIC_MAX_NUMBER_OF_SAMPLES * SAMPIC_SAMPLING_PERIOD_NS;
    if (t > ACCEPTED_TIME_RADIUS)
      t -= SAMPIC_MAX_NUMBER_OF_SAMPLES * SAMPIC_SAMPLING_PERIOD_NS;
  }
  return t;
}

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

float
TotemTimingConversions::getTriggerTime(const TotemTimingDigi& digi) const
{
  unsigned int offsetOfSamples = digi.getEventInfo().getOffsetOfSamples();
  if (offsetOfSamples == 0)
    offsetOfSamples = 30; // FW 0 is not sending this, FW > 0 yes

  return getTimeOfFirstSample(digi) + (SAMPIC_MAX_NUMBER_OF_SAMPLES - offsetOfSamples) * SAMPIC_SAMPLING_PERIOD_NS;
}

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

float
TotemTimingConversions::getTimePrecision(const TotemTimingDigi& digi) const
{
  int db = digi.getHardwareBoardId();
  int sampic = digi.getHardwareSampicId();
  int channel = digi.getHardwareChannelId();
  return parsedData_.getTimePrecision(db, sampic, channel);
}

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

std::vector<float>
TotemTimingConversions::getTimeSamples(const TotemTimingDigi& digi) const
{
  std::vector<float> time(digi.getNumberOfSamples());
  for (unsigned int i = 0; i < time.size(); ++i)
    time.at(i) = getTimeOfFirstSample(digi) + i * SAMPIC_SAMPLING_PERIOD_NS;
  return time;
}

//----------------------------------------------------------------------------------------------------
// NOTE: If no proper file is specified, calibration is not applied

std::vector<float>
TotemTimingConversions::getVoltSamples(const TotemTimingDigi& digi) const
{
  std::vector<float> data;
  if (!calibrationFileOpened_ || !calibrationFunction_)
    for (const auto& sample : digi.getSamples())
      data.emplace_back(SAMPIC_ADC_V * sample);
  else {
    unsigned int db = digi.getHardwareBoardId();
    unsigned int sampic = digi.getHardwareSampicId();
    unsigned int channel = digi.getHardwareChannelId();
    unsigned int cell = digi.getCellInfo();
    for (const auto& sample : digi.getSamples()) {
      // ring buffer on Sampic, so accounting for samples register boundary
      const unsigned short sample_cell = (cell++) % SAMPIC_MAX_NUMBER_OF_SAMPLES;
      auto parameters = parsedData_.getParameters(db, sampic, channel, sample_cell);
      if (parameters.empty())
        throw cms::Exception("TotemTimingConversions:getVoltSamples")
          << "Invalid calibrations retrieved for Sampic digi"
          << " (" << db << ", " << sampic << ", " << channel << ", " << sample_cell << ")!";
      data.emplace_back(calibrationFunction_->evaluate(std::vector<double>{(double)sample}, parameters));
    }
  }
  return data;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE crypto3_marshalling_r1cs_gg_ppzksnark_verification_key_test

#include <boost/test/unit_test.hpp>
#include <boost/algorithm/string/case_conv.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <iostream>
#include <iomanip>

#include <nil/marshalling/status_type.hpp>
#include <nil/marshalling/field_type.hpp>
#include <nil/marshalling/endianness.hpp>

#include <nil/crypto3/multiprecision/cpp_int.hpp>
#include <nil/crypto3/multiprecision/number.hpp>

#include <nil/crypto3/algebra/random_element.hpp>
#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/curves/detail/marshalling.hpp>
#include <nil/crypto3/algebra/pairing/bls12.hpp>
#include <nil/crypto3/algebra/pairing/mnt4.hpp>
#include <nil/crypto3/algebra/pairing/mnt6.hpp>

#include <nil/crypto3/zk/snark/sparse_vector.hpp>
#include <nil/crypto3/zk/snark/accumulation_vector.hpp>

#include <nil/crypto3/marshalling/types/zk/r1cs_gg_ppzksnark/verification_key.hpp>

template<typename TIter>
void print_byteblob(TIter iter_begin, TIter iter_end) {
    for (TIter it = iter_begin; it != iter_end; it++) {
        std::cout << std::hex << int(*it) << std::endl;
    }
}

template<typename FpCurveGroupElement>
void print_fp_curve_group_element(FpCurveGroupElement e) {
    std::cout << e.X.data << " " << e.Y.data << " " << e.Z.data << std::endl;
}

template<typename Fp2CurveGroupElement>
void print_fp2_curve_group_element(Fp2CurveGroupElement e) {
    std::cout << "(" << e.X.data[0].data << " " << e.X.data[1].data << ") (" << e.Y.data[0].data << " "
              << e.Y.data[1].data << ") (" << e.Z.data[0].data << " " << e.Z.data[1].data << ")" << std::endl;
}

template<typename Endianness, typename CurveType>
void test_verification_key(zk::snark::r1cs_gg_ppzksnark_verification_key<CurveType> val) {

    using namespace nil::crypto3::marshalling;

    std::size_t units_bits = 8;
    using unit_type = unsigned char;
    using verification_key_type =
        types::r1cs_gg_ppzksnark_verification_key<nil::marshalling::field_type<Endianness>,
                                                  zk::snark::r1cs_gg_ppzksnark_verification_key<CurveType>>;

    verification_key_type filled_val =
        types::fill_r1cs_gg_ppzksnark_verification_key<zk::snark::r1cs_gg_ppzksnark_verification_key<CurveType>,
                                                       Endianness>(val);

    zk::snark::r1cs_gg_ppzksnark_verification_key<CurveType> constructed_val =
        types::make_r1cs_gg_ppzksnark_verification_key<zk::snark::r1cs_gg_ppzksnark_verification_key<CurveType>,
                                                       Endianness>(filled_val);
    BOOST_CHECK(val == constructed_val);

    std::size_t unitblob_size = filled_val.length();

    std::vector<unit_type> cv;
    cv.resize(unitblob_size, 0x00);

    auto write_iter = cv.begin();

    nil::marshalling::status_type status = filled_val.write(write_iter, cv.size());

    verification_key_type test_val_read;

    auto read_iter = cv.begin();
    status = test_val_read.read(read_iter, cv.size());

    zk::snark::r1cs_gg_ppzksnark_verification_key<CurveType> constructed_val_read =
        types::make_r1cs_gg_ppzksnark_verification_key<zk::snark::r1cs_gg_ppzksnark_verification_key<CurveType>,
                                                       Endianness>(test_val_read);

    BOOST_CHECK(val == constructed_val_read);
}

template<typename CurveType, typename Endianness, std::size_t TSize>
void test_verification_key() {
    using g1_type = typename CurveType::g1_type<>;
    using g2_type = typename CurveType::g2_type<>;
    using gt_type = typename CurveType::gt_type;

    std::cout << std::hex;
    std::cerr << std::hex;
    for (unsigned i = 0; i < 128; ++i) {
        if (!(i % 16) && i) {
            std::cout << std::dec << i << " tested" << std::endl;
        }
        typename g1_type::value_type first = nil::crypto3::algebra::random_element<g1_type>();
        std::vector<typename g1_type::value_type> rest;
        for (std::size_t i = 0; i < TSize; i++) {
            rest.push_back(nil::crypto3::algebra::random_element<g1_type>());
        }
        test_verification_key<Endianness, CurveType>(zk::snark::r1cs_gg_ppzksnark_verification_key<CurveType>(
            nil::crypto3::algebra::random_element<gt_type>(),
            nil::crypto3::algebra::random_element<g2_type>(),
            nil::crypto3::algebra::random_element<g2_type>(),
            std::move(zk::snark::accumulation_vector<g1_type>(std::move(first), std::move(rest)))));
    }
}

BOOST_AUTO_TEST_SUITE(sparse_vector_test_suite)

BOOST_AUTO_TEST_CASE(sparse_vector_bls12_381_be) {
    std::cout << "BLS12-381 r1cs_gg_ppzksnark verification key big-endian test started" << std::endl;
    test_verification_key<nil::crypto3::algebra::curves::bls12<381>, nil::marshalling::option::big_endian, 5>();
    std::cout << "BLS12-381 r1cs_gg_ppzksnark verification key big-endian test finished" << std::endl;
}

// BOOST_AUTO_TEST_CASE(sparse_vector_bls12_381_le) {
//     std::cout << "BLS12-381 r1cs_gg_ppzksnark verification key little-endian test started" << std::endl;
//     test_verification_key<nil::crypto3::algebra::curves::bls12<381>,
//         nil::marshalling::option::little_endian,
//         5>();
//     std::cout << "BLS12-381 r1cs_gg_ppzksnark verification key little-endian test finished" << std::endl;
// }

BOOST_AUTO_TEST_SUITE_END()


//#define GraphicalModelDecomposition DualDecompostionSubgradientInference_GraphicalModelDecomposition

#include <boost/python.hpp>
#include <string>
#include "inf_def_visitor.hxx"

#include <opengm/inference/dualdecomposition/dualdecomposition_subgradient.hxx>
#include <param/dual_decompostion_subgradient_param.hxx>

// Graph cut subinference
#include <opengm/inference/auxiliary/minstcutboost.hxx>
#ifdef WITH_MAXFLOW
# include <opengm/inference/auxiliary/minstcutkolmogorov.hxx>
#endif
#include <opengm/inference/graphcut.hxx>
#include <param/graph_cut_param.hxx>

// Cplex subinference
#ifdef WITH_CPLEX
#include <opengm/inference/lpcplex.hxx>
#include <param/lpcplex_param.hxx>
#endif

// dynamic programming subinference
#include <param/dynamic_programming_param.hxx>
#include <opengm/inference/dynamicprogramming.hxx>

#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>
#include <opengm/python/pythonfunction.hxx>

using namespace boost::python;

template<class GM,class ACC>
void export_dual_decomposition_subgradient(){

   using namespace boost::python;
   import_array();

   typedef typename GM::ValueType                                                   ValueType;
   typedef double                                                                   ViewValueType;
   typedef opengm::DDDualVariableBlock<marray::View<ViewValueType, false> >         DualBlockType;
   typedef typename opengm::DualDecompositionBase<GM,DualBlockType>::SubGmType      SubGmType;
   typedef typename SubGmType::ValueType                                            SubGmValueType;
   // export enums of DualDecompositionBaseParameter
   enum_<opengm::DualDecompositionBaseParameter::DecompositionId> ("DecompositionId")
   .value("manual",        opengm::DualDecompositionBaseParameter::MANUAL)
   .value("tree",          opengm::DualDecompositionBaseParameter::TREE)
   .value("spanningtrees", opengm::DualDecompositionBaseParameter::SPANNINGTREES)
   .value("blocks",        opengm::DualDecompositionBaseParameter::BLOCKS)
   ;
   enum_<opengm::DualDecompositionBaseParameter::DualUpdateId> ("DualUpdateId")
   .value("adaptive",      opengm::DualDecompositionBaseParameter::ADAPTIVE)
   .value("stepsize",      opengm::DualDecompositionBaseParameter::STEPSIZE)
   .value("steplength",    opengm::DualDecompositionBaseParameter::STEPLENGTH)
   .value("kiewil",        opengm::DualDecompositionBaseParameter::KIEWIL)
   ;

   append_subnamespace("solver");

   // documentation
   InfSetup setup;
   setup.cite       = "";
   setup.algType    = "dual-decomposition";
   setup.hyperParameterKeyWords        = StringVector(1,std::string("subInference"));
   setup.hyperParametersDoc            = StringVector(1,std::string("inference algorithms for the sub-blocks"));
   // parameter of inference will change if hyper parameter changes
   setup.hasInterchangeableParameter   = false;

   #ifdef WITH_MAXFLOW
   {
      // set up hyper parameter name for this template
      setup.isDefault=false;

      setup.hyperParameters= StringVector(1,std::string("graph-cut"));
      typedef opengm::external::MinSTCutKolmogorov<size_t,SubGmValueType>        MinStCutKolmogorov;
      typedef opengm::GraphCut<SubGmType, ACC, MinStCutKolmogorov>               SubInfernce;
      typedef opengm::DualDecompositionSubGradient<GM,SubInfernce,DualBlockType> PyDualDecomposition;
      // export parameter
      exportInfParam<SubInfernce>("_SubParameter_DualDecompositionSubgradient_GraphCutKolmogorov");
      exportInfParam<PyDualDecomposition>("_DualDecompositionSubgradient_GraphCutKolmogorov");
      // export inferences
      class_< PyDualDecomposition>("_DualDecompositionSubgradient_GraphCutKolmogorov",init<const GM & >())
      .def(InfSuite<PyDualDecomposition,false>(std::string("DualDecompositionSubgradient"),setup))
      ;
   }
   #else
   {
      // set up hyper parameter name for this template
      setup.isDefault=false;

      setup.hyperParameters= StringVector(1,std::string("graph-cut"));
      typedef opengm::MinSTCutBoost<size_t, ValueType, opengm::KOLMOGOROV>       MinStCutBoostKolmogorov;
      typedef opengm::GraphCut<SubGmType,ACC, MinStCutBoostKolmogorov>           SubInfernce;
      typedef opengm::DualDecompositionSubGradient<GM,SubInfernce,DualBlockType> PyDualDecomposition;
      // export parameter
      exportInfParam<SubInfernce>("_SubParameter_DualDecompositionSubgradient_GraphCutBoostKolmogorov");
      exportInfParam<PyDualDecomposition>("_DualDecompositionSubgradient_GraphCutBoostKolmogorov");
      // export inferences
      class_< PyDualDecomposition>("_DualDecompositionSubgradient_GraphCutBoostKolmogorov",init<const GM & >())
      .def(InfSuite<PyDualDecomposition,false>(std::string("DualDecompositionSubgradient"),setup))
      ;
   }
   #endif

   /*
   #ifdef WITH_CPLEX
   {
      // set up hyper parameter name for this template
      setup.isDefault=false;

      setup.hyperParameters= StringVector(1,std::string("cplex"));
      typedef opengm::LPCplex<SubGmType, ACC>                                    SubInfernce;
      typedef opengm::DualDecompositionSubGradient<GM,SubInfernce,DualBlockType> PyDualDecomposition;

      exportInfParam<SubInfernce>("_SubParameter_DualDecompositionSubgradient_Cplex");
      exportInfParam<PyDualDecomposition>("DualDecompositionSubgradient_Cplex");
      // export inferences
      class_< PyDualDecomposition>("_DualDecompositionSubgradient_Cplex",init<const GM & >())
      .def(InfSuite<PyDualDecomposition,false>(std::string("DualDecompositionSubgradient"),setup))
      ;

   }
   #endif
   */

   {

      // set up hyper parameter name for this template
      setup.isDefault = true;

      setup.hyperParameters= StringVector(1,std::string("dynamic-programming"));
      typedef opengm::DynamicProgramming<SubGmType, ACC>                         SubInfernce;
      typedef opengm::DualDecompositionSubGradient<GM,SubInfernce,DualBlockType> PyDualDecomposition;

      exportInfParam<SubInfernce>("_SubParameter_DualDecompositionSubgradient_DynamicProgramming");
      exportInfParam<PyDualDecomposition>("_DualDecompositionSubgradient_DynamicProgramming");
      // export inferences
      class_< PyDualDecomposition>("_DualDecompositionSubgradient_DynamicProgramming",init<const GM & >())
      .def(InfSuite<PyDualDecomposition,false>(std::string("DualDecompositionSubgradient"),setup))
      ;

   }

}

template void export_dual_decomposition_subgradient<opengm::python::GmAdder,opengm::Minimizer>();


#include <boost/lambda/lambda.hpp>
#include <iostream>
#include <iterator>
#include <algorithm>

int main()
{
	std::cout << "Hello World!";
	using namespace boost::lambda;
	typedef std::istream_iterator<int> in;

	std::for_each(
		in(std::cin), in(), std::cout << (_1 * 3) << " ");
	return 0;
}


//==-------------------------------------------------------------*- C++ -*--==//
//
// Copyright 2019 Contributors to the Gazer project
//
// 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 "ThetaCfaGenerator.h"
#include "gazer/Core/Expr/ExprWalker.h"
#include "gazer/ADT/StringUtils.h"

#include <llvm/Support/raw_ostream.h>

#include <boost/range/irange.hpp>

#include <functional>

using namespace gazer;

class ThetaExprPrinter : public ExprWalker<ThetaExprPrinter, std::string>
{
public:
    ThetaExprPrinter(std::function<std::string(Variable*)> replacedNames)
        : mReplacedNames(replacedNames)
    {}

    /// If there was an expression which could not be handled by
    /// this walker, returns it. Otherwise returns nullptr.
    ExprPtr getInvalidExpr() {
        return mUnhandledExpr;
    }

public:
    std::string visitExpr(const ExprPtr& expr) {
        mUnhandledExpr = expr;
        llvm::errs() << "Unhandled expr " << *expr << "\n";
        return "__UNHANDLED_EXPR__";
    }

    std::string visitLiteral(const ExprRef<LiteralExpr>& expr)
    {
        if (auto intLit = llvm::dyn_cast<IntLiteralExpr>(expr)) {
            auto val = intLit->getValue();
            return val < 0 ? "(" + std::to_string(val) + ")" : std::to_string(intLit->getValue());
        }

        if (auto boolLit = llvm::dyn_cast<BoolLiteralExpr>(expr)) {
            return boolLit->getValue() ? "true" : "false";
        }

        if (auto realLit = llvm::dyn_cast<RealLiteralExpr>(expr)) {
            auto val = realLit->getValue();
            return std::to_string(val.numerator()) + "%" + std::to_string(val.denominator());
        }

        return visitExpr(expr);
    }

    std::string visitVarRef(const ExprRef<VarRefExpr>& expr) {
        std::string newName = mReplacedNames(&expr->getVariable());
        if (!newName.empty()) {
            return newName;
        }

        return expr->getVariable().getName();
    }

    std::string visitNot(const ExprRef<NotExpr>& expr) {
        return "(not " + getOperand(0) + ")";
    }

    // Binary
    std::string visitAdd(const ExprRef<AddExpr>& expr) {
        return "(" + getOperand(0) + " + " + getOperand(1) + ")";
    }

    std::string visitSub(const ExprRef<SubExpr>& expr) {
        return "(" + getOperand(0) + " - " + getOperand(1) + ")";
    }

    std::string visitMul(const ExprRef<MulExpr>& expr) {
        return "(" + getOperand(0) + " * " + getOperand(1) + ")";
    }

    std::string visitDiv(const ExprRef<DivExpr>& expr) {
        return "(" + getOperand(0) + " / " + getOperand(1) + ")";
    }

    std::string visitMod(const ExprRef<ModExpr>& expr) {
        return "(" + getOperand(0) + " mod " + getOperand(1) + ")";
    }

    std::string visitAnd(const ExprRef<AndExpr>& expr)
    {
        std::string buffer;
        llvm::raw_string_ostream rso{buffer};

        rso << "(";
        auto r = boost::irange<size_t>(0, expr->getNumOperands());

        join_print_as(rso, r.begin(), r.end(), " and ", [this](auto& rso, size_t i) {
            rso << getOperand(i);
        });
        rso << ")";

        return rso.str();
    }

    std::string visitOr(const ExprRef<OrExpr>& expr)
    {
        std::string buffer;
        llvm::raw_string_ostream rso{buffer};

        rso << "(";
        auto r = boost::irange<size_t>(0, expr->getNumOperands());

        join_print_as(rso, r.begin(), r.end(), " or ", [this](auto& rso, size_t i) {
            rso << getOperand(i);
        });
        rso << ")";

        return rso.str();
    }

    std::string visitImply(const ExprRef<ImplyExpr>& expr) {
        return "(" + getOperand(0) + " imply " + getOperand(1) + ")";
    }

    std::string visitEq(const ExprRef<EqExpr>& expr) {
        return "(" + getOperand(0) + " = " + getOperand(1) + ")";
    }

    std::string visitNotEq(const ExprRef<NotEqExpr>& expr) {
        return "(" + getOperand(0) + " /= " + getOperand(1) + ")";
    }

    std::string visitLt(const ExprRef<LtExpr>& expr) {
        return "(" + getOperand(0) + " < " + getOperand(1) + ")";
    }

    std::string visitLtEq(const ExprRef<LtEqExpr>& expr) {
        return "(" + getOperand(0) + " <= " + getOperand(1) + ")";
    }

    std::string visitGt(const ExprRef<GtExpr>& expr) {
        return "(" + getOperand(0) + " > " + getOperand(1) + ")";
    }

    std::string visitGtEq(const ExprRef<GtEqExpr>& expr) {
        return "(" + getOperand(0) + " >= " + getOperand(1) + ")";
    }

    std::string visitSelect(const ExprRef<SelectExpr>& expr) {
        return "(if " + getOperand(0) + " then " + getOperand(1) + " else " + getOperand(2) + ")";
    }

    std::string visitArrayRead(const ExprRef<ArrayReadExpr>& expr) {
        return  "(" + getOperand(0) + ")[" + getOperand(1) + "]";
    }

    std::string visitArrayWrite(const ExprRef<ArrayWriteExpr>& expr) {
        return  "(" + getOperand(0) + ")[" + getOperand(1) + " <- " + getOperand(2) + "]";
    }

private:
    ExprPtr mUnhandledExpr = nullptr;
    std::function<std::string(Variable*)> mReplacedNames;
};

std::string gazer::theta::printThetaExpr(const ExprPtr& expr)
{
    auto getName = [](Variable* var) -> std::string { return var->getName(); };
    return printThetaExpr(expr, getName);
}

std::string gazer::theta::printThetaExpr(const ExprPtr& expr, std::function<std::string(Variable*)> variableNames)
{
    ThetaExprPrinter printer(variableNames);

    return printer.walk(expr);
}


#include <boost/date_time/gregorian/gregorian.hpp>
#include<iostream>
using namespace std;
using namespace boost::gregorian;
int main(){
date d1;                    // 无效日期
date d2(2010,1,1);
date d3(2000, Jan, 1);
date d4(d2);                // 拷贝构造
date d0 = from_string("1999-12-31");
date d5 ( from_string("2005/1/1") );
date d6 = from_undelimited_string("20001109");

cout << day_clock::local_day() << endl;
cout << day_clock::universal_day() << endl;

date neg(neg_infin);            //negative infinite time
date pos(pos_infin);            //positive infinite time
date notdate(not_a_date_time);  //not a date time
date maxdate(max_date_time);    //max date
date mindate(min_date_time);    //min date
cout << neg << endl << pos << endl << maxdate << endl << mindate <<endl;

}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-boolean
    Function object implementing seldec capabilities

    Decrements a value by @ref One if a predicate is @ref True.

    @par Semantic:

    For every parameters of types respectively C, T:

    @code
    T r = seldec(cond,t);
    @endcode

    is similar to:

    @code
    T r = cond : t-One<T>() : t;
    @endcode

    @par Alias:
    @c if_dec

  **/
  const boost::dispatch::functor<tag::seldec_> seldec = {};
} }
#endif

#include <boost/simd/function/scalar/seldec.hpp>
#include <boost/simd/function/simd/seldec.hpp>

#endif


#include <boost/assign/std/vector.hpp>
#include <cradle/geometry/common.hpp>

#define BOOST_TEST_MODULE bouding_box
#include <cradle/test.hpp>

using namespace cradle;
using namespace boost::assign;

BOOST_AUTO_TEST_CASE(vector_of_points_test)
{
    std::vector<vector2i> points;
    points +=
        make_vector( 0,  0),
        make_vector(-1, -1),
        make_vector(-3,  0),
        make_vector( 0,  7),
        make_vector( 3,  3),
        make_vector( 3,  2);
    BOOST_CHECK_EQUAL(bounding_box(points),
        box2i(make_vector(-3, -1), make_vector(6, 8)));
}

BOOST_AUTO_TEST_CASE(structure_geometry_test)
{
}


#ifndef TABLE_HPP
#define TABLE_HPP

// This is needed so that I can create a hash func for
// dynamic_bitset. It makes m_bits publically accessible.
// ref: http://stackoverflow.com/a/3897217/1558890
#define BOOST_DYNAMIC_BITSET_DONT_USE_FRIENDS

#include "LSHFunc_ITQ.hpp"
#include "utils.hpp"
#include <vector>
#include <unordered_map>
#include <unordered_set>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/unordered_map.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/unordered_set.hpp>
#include <boost/functional/hash.hpp>
#include <boost/dynamic_bitset.hpp>

struct dynbitset_hash {
  template <typename B, typename A>
    std::size_t operator()(const boost::dynamic_bitset<B, A>& bs) const {
      return boost::hash_value(bs.m_bits);
    }
};

class Table {
  friend class boost::serialization::access;
  LSHFunc_ITQ lshFunc;
  unordered_map<boost::dynamic_bitset<>, unordered_set<long long>, dynbitset_hash> index;
  vector<boost::dynamic_bitset<>> indexKeys; // maintain keys to above map, for fast hamming search
public:
  Table(int k) : lshFunc(k) {}
  Table() {} // used for serializing
  template <typename T>
  void train(const vector<vector<T>>& sampleData) {
    lshFunc.train(sampleData);
  }
  void insert(const vector<float>& feat, long long int label) {
    boost::dynamic_bitset<> hash = lshFunc.computeHash(feat);

    auto pos = index.find(hash);
    if (pos == index.end()) {
      unordered_set<long long int> lst;
      lst.insert(label);
      index[hash] = lst;
      indexKeys.push_back(hash);
    } else {
      pos->second.insert(label);
    }
  }
  bool search_exact(const vector<float>& feat, unordered_set<long long int>& output) const {
    boost::dynamic_bitset<> hash = lshFunc.computeHash(feat);
    auto pos = index.find(hash);
    if (pos != index.end()) {
      output = pos->second;
      return true;
    }
    return false;
  }
  /**
   * Hamming distance search
   */
  bool search(const vector<float>& feat, unordered_set<long long>& output, int nRerank) const {
    output.clear();
    vector<int> hamdists(indexKeys.size(), 0);
    boost::dynamic_bitset<> hash = lshFunc.computeHash(feat);
    #pragma omp parallel for
    for (size_t i = 0; i < indexKeys.size(); i++) {
      hamdists[i] = (indexKeys[i] ^ hash).count();
    }
    vector<size_t> order = argsort(hamdists);
    for (size_t i = 0; i < min(hamdists.size(), (size_t) nRerank); i++) {
      // ref: http://stackoverflow.com/a/262872
      unordered_set<long long> match = index.at(indexKeys[order[i]]);
      output.insert(match.begin(), match.end());
    }
  }
  template<class Archive>
  void serialize(Archive &ar, const unsigned int version) {
    ar & lshFunc;
    ar & index;
    ar & indexKeys;
  }
};

/**
 * The following code is to implement serialization for
 * boost::dynamic_bitset. Implementation directly copied from
 * http://stackoverflow.com/a/31014564
 */
namespace boost { namespace serialization {
  template <typename Ar, typename Block, typename Alloc>
    void save(Ar& ar, dynamic_bitset<Block, Alloc> const& bs, unsigned) {
      size_t num_bits = bs.size();
      std::vector<Block> blocks(bs.num_blocks());
      to_block_range(bs, blocks.begin());

      ar & num_bits & blocks;
    }

  template <typename Ar, typename Block, typename Alloc>
    void load(Ar& ar, dynamic_bitset<Block, Alloc>& bs, unsigned) {
      size_t num_bits;
      std::vector<Block> blocks;
      ar & num_bits & blocks;

      bs.resize(num_bits);
      from_block_range(blocks.begin(), blocks.end(), bs);
      bs.resize(num_bits);
    }

  template <typename Ar, typename Block, typename Alloc>
    void serialize(Ar& ar, dynamic_bitset<Block, Alloc>& bs, unsigned version) {
      split_free(ar, bs, version);
    }
} }

#endif