/**
 * \ file LinkwitzRileyFilter.cpp
 */

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

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

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

constexpr gsl::index PROCESSSIZE = 1024*64;

BOOST_AUTO_TEST_CASE( IIRFilter_LinkwitzRileyLowPassCoefficients_frequency_test )
{
  ATK::IIRFilter<ATK::LinkwitzRileyLowPassCoefficients<double> > filter;
  filter.set_cut_frequency(20);
  BOOST_CHECK_EQUAL(filter.get_cut_frequency(), 20);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LinkwitzRiley4LowPassCoefficients_frequency_test )
{
  ATK::IIRFilter<ATK::LinkwitzRiley4LowPassCoefficients<double> > filter;
  filter.set_cut_frequency(20);
  BOOST_CHECK_EQUAL(filter.get_cut_frequency(), 20);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LinkwitzRiley4LowPassCoefficients_frequency_range_test )
{
  ATK::IIRFilter<ATK::LinkwitzRiley4LowPassCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_cut_frequency(0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LinkwitzRileyLowPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::LinkwitzRileyLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LinkwitzRileyLowPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::LinkwitzRileyLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LinkwitzRileyLowPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::LinkwitzRileyLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LinkwitzRileyLowPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::LinkwitzRileyLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LinkwitzRileyHighPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::LinkwitzRileyHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LinkwitzRileyHighPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::LinkwitzRileyHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LinkwitzRileyHighPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::LinkwitzRileyHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_LinkwitzRileyHighPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::LinkwitzRileyHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE(IIRFilter_LinkwitzRiley4LowPassCoefficients_1k_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::LinkwitzRiley4LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024 * 64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE(IIRFilter_LinkwitzRiley4LowPassCoefficients_100_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::LinkwitzRiley4LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024 * 64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE(IIRFilter_LinkwitzRiley4LowPassCoefficients_2k_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::LinkwitzRiley4LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024 * 64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE(IIRFilter_LinkwitzRiley4LowPassCoefficients_200_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::LinkwitzRiley4LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024 * 64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE(IIRFilter_LinkwitzRiley4HighPassCoefficients_1k_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::LinkwitzRiley4HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024 * 64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE(IIRFilter_LinkwitzRiley4HighPassCoefficients_100_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::LinkwitzRiley4HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024 * 64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE(IIRFilter_LinkwitzRiley4HighPassCoefficients_2k_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::LinkwitzRiley4HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024 * 64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE(IIRFilter_LinkwitzRiley4HighPassCoefficients_200_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::LinkwitzRiley4HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_cut_frequency(100);

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

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

  filter.process(1024 * 64);

  checker.process(PROCESSSIZE);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SDK_MEMORY_META_IS_ALIGNED_HPP_INCLUDED
#define BOOST_SIMD_SDK_MEMORY_META_IS_ALIGNED_HPP_INCLUDED

#include <cstddef>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/simd/sdk/memory/parameters.hpp>
#include <boost/simd/sdk/memory/meta/is_power_of_2.hpp>

namespace boost { namespace simd {  namespace meta
{
  //////////////////////////////////////////////////////////////////////////////
  // Boolean meta-function checking if a integral constant is aligned on a given
  // power of 2 boundary.
  // Documentation: reference/sdk/aligned/meta/is_aligned_c.rst
  //////////////////////////////////////////////////////////////////////////////
  template<std::size_t V, std::size_t N = BOOST_SIMD_CONFIG_ALIGNMENT>
  struct is_aligned_c : boost::mpl::bool_<!(V & (N-1) )>
  {
    //==========================================================================
    /*
     * Alignment done on a non-power of two boundary
     */
    //==========================================================================
    BOOST_MPL_ASSERT_MSG
    ( (meta::is_power_of_2_c<N>::value)
    , BOOST_SIMD_INVALID_ALIGNMENT_VALUE
    , (boost::mpl::int_<N>)
    );
  };

  //////////////////////////////////////////////////////////////////////////////
  // Boolean meta-function checking if a Integral Constant is aligned on a given
  // power of 2 boundary.
  // Documentation: reference/sdk/aligned/meta/is_aligned.rst
  //////////////////////////////////////////////////////////////////////////////
  template<class V, class N = boost::mpl::size_t<BOOST_SIMD_CONFIG_ALIGNMENT> >
  struct is_aligned : is_aligned_c<V::value,N::value> {};

} } }

#endif


#include <cmath>

#include <chrono>

#include <boost/program_options.hpp>

#include "flpmc_yche.h"
#include "simrank.h"
#include "../playground/pretty_print.h"

using namespace std;
using namespace boost::program_options;

#ifdef VARYING_RMAX
FLPMC::FLPMC(string g_name_, GraphYche &g_, double c_, double epsilon_, double delta_, size_t Q_, double r_max_) :
        g_name(g_name_), g(&g_), c(c_), epsilon(epsilon_), delta(delta_), Q(Q_) {
    // 1st: init r_max
    r_max = r_max_;

    // 2nd: compute or load index
    size_t n = static_cast<size_t>(g->n);
    cout << format("r_max: %s") % get_rmax() << endl;
    lp = new Full_LocalPush(*g, g_name, c, get_lp_epsilon(), n);
    if (!lp_file_exists(g_name, c, get_lp_epsilon(), n, true)) { // test wether the local push index exists
        cout << "local push offline index doesn't exists.. " << endl;
        auto start_time = std::chrono::high_resolution_clock::now();
        lp->local_push(*g);
        auto end_time = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double> elapsed = end_time - start_time;
        cout << format("total indexing cost: %s s") % elapsed.count() << endl; // record the pre-processing time
        cout << format("building compete, saving to %s ") % lp->get_file_path_base() << endl;
        lp->save();
        cout << "saved." << endl;
    } else {
        cout << "offline index exists..loading " << endl;
        lp->load();
    }
}
#else

FLPMC::FLPMC(string g_name_, GraphYche &g_, double c_, double epsilon_, double delta_, size_t Q_) :
        g_name(g_name_), g(&g_), c(c_), epsilon(epsilon_), delta(delta_), Q(Q_) {
    // init local push
    size_t n = static_cast<size_t>(g->n);
    cout << format("r_max: %s") % get_rmax() << endl;
    lp = new Full_LocalPush(*g, g_name, c, get_lp_epsilon(), n);

//    {
//        // directly compute the index, instead of reading and parsing
//        auto clock_start = clock();
//
//        auto start_time = std::chrono::high_resolution_clock::now();
//
//        auto plp = Full_LocalPush(*g, g_name, c, get_lp_epsilon(), n);
//        plp.local_push(*g);
//        swap(lp->P, plp.P);
//        swap(lp->R, plp.R);
//        auto end_time = std::chrono::high_resolution_clock::now();
//        auto clock_end = clock();
//        cout << "total indexing cpu time:" << static_cast<double>(clock_end - clock_start) / CLOCKS_PER_SEC << "s"
//             << endl;
//        std::chrono::duration<double> elapsed = end_time - start_time;
//        cout << format("total indexing cost: %s s") % elapsed.count() << endl; // record the pre-processing time
//    }

    if (!lp_file_exists(g_name, c, get_lp_epsilon(), n, true)) { // test wether the local push index exists
        cout << "local push offline index doesn't exists.. " << endl;
        auto start_time = std::chrono::high_resolution_clock::now();
        lp->local_push(*g);
        auto end_time = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double> elapsed = end_time - start_time;
        cout << format("total indexing cost: %s s") % elapsed.count() << endl; // record the pre-processing time
        cout << format("building compete, saving to %s ") % lp->get_file_path_base() << endl;
        lp->save();
        cout << "saved." << endl;
    } else {
        cout << "offline index exists..loading " << endl;
        lp->load();
    }
}

#endif

FLPMC::FLPMC(const FLPMC &other_obj) {
//    cout << "flpmc..." << endl;
    rand_gen = SFMTRand();

    g_name = other_obj.g_name;
    Q = other_obj.Q;
    epsilon = other_obj.epsilon; // the error bound required by query
    delta = other_obj.delta;
    c = other_obj.c;
    g = other_obj.g; // the pointer to the graph
    lp = other_obj.lp; // the pointer to the local push index

    // rmax
#ifdef VARYING_RMAX
    r_max = other_obj.r_max;
#endif
}

double FLPMC::get_rmax() {
//    double r = sqrt(epsilon);
#ifdef VARYING_RMAX
    return r_max;
#else
    double r = pow(epsilon, 1.0 / 1.5);
    return r;
#endif
//    double r = pow(pow(epsilon, 2) / (pow(c, 3) / log(2.0 / delta)), 1.0 / 3);
//    cout << format("r_max of local push: %s") % r << endl;
}

double FLPMC::get_lp_epsilon() {
//    double lp_epsilon = get_rmax() * (1 - c);
    double lp_epsilon = get_rmax() / (1 - c);
//    cout << format("local push error: %s") % lp_epsilon << endl;
    return lp_epsilon;
}

double FLPMC::query_one2one(NodePair np) {
//    cout << np << endl;
    if (np.first == np.second) {
        return 1;
    } else if (np.first > np.second) { // make sure np.first < np.second
        np = NodePair({np.second, np.first});
    }
    /* FLP result */
    double p_i = lp->query_P(np.first, np.second);
    double r_i = lp->query_R(np.first, np.second);
//    cout << format("pi: %s, ri:%s") % p_i % r_i << endl;

    // set up the random number generator
#ifndef SFMT
    std::random_device rd;  //Will be used to obtain a seed for the random number engine
    std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
    std::uniform_real_distribution<double> distribution(0.0, 1.0);
#endif

    /* MC sampling phase */
    int N = get_N();
    double E_residual = 0;
    for (int i = 0; i < N; i++) {
        int a = np.first;
        int b = np.second;
        int step = 0;
        bool is_dead_node = false;

#ifndef SFMT
        while(( distribution(gen) < c ) && (a != b)){
            a = sample_in_neighbor(a, *g);
            b = sample_in_neighbor(b, *g);
#else
        while ((step == 0 || rand_gen.double_rand() < c) && (a != b)) {
            a = sample_in_neighbor(a, *g, rand_gen);
            b = sample_in_neighbor(b, *g, rand_gen);
#endif
            step++;
            if (a == -1 || b == -1) {
                is_dead_node = true;
                break;
            }
        }
        if (is_dead_node) {
            E_residual += 0;
        } else {
            E_residual += lp->query_R(a, b);
        }
    }

//    cout << format("p_i: %s") % p_i << endl;
//    cout << format("number of samples: %s") % get_N() << endl;
    return p_i + r_i + (c / (1 - c)) * (E_residual / N);
}

double FLPMC::get_N() {
    // compute the number of needed samples
    double tmp = (pow(c, 2.) * pow(get_rmax(), 2.) * log(2. / delta)) / (2 * pow(epsilon, 2.) * pow((1 - c), 2.));
    int n = ceil(tmp);
    return n;
}


#include <Eigen/Core>

#include <map-manager/map-manager.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <vi-map/vi-map.h>
#include <vi-mapping-test-app/vi-mapping-test-app.h>

#include "landmark-triangulation/landmark-triangulation.h"

DECLARE_double(vi_map_landmark_quality_min_observation_angle_deg);
DECLARE_uint64(vi_map_landmark_quality_min_observers);
DECLARE_double(vi_map_landmark_quality_max_distance_from_closest_observer);
DECLARE_double(vi_map_landmark_quality_min_distance_from_closest_observer);

namespace landmark_triangulation {

class ViMappingTest : public ::testing::Test {
 protected:
  virtual void SetUp() {
    test_app_.loadDataset("./test_maps/vi_app_test");
    CHECK_NOTNULL(test_app_.getMapMutable());
  }

  virtual void corruptLandmarks();
  visual_inertial_mapping::VIMappingTestApp test_app_;
};

void ViMappingTest::corruptLandmarks() {
  constexpr double kLandmarkPositionStdDev = 5.0;
  constexpr int kEveryNthToCorrupt = 1;
  test_app_.corruptLandmarkPositions(
      kLandmarkPositionStdDev, kEveryNthToCorrupt);
}

TEST_F(ViMappingTest, TestLandmarkTriangulation) {
  corruptLandmarks();

  EXPECT_TRUE(retriangulateLandmarks(test_app_.getMapMutable()));
  constexpr double kPrecision = 0.1;
  constexpr double kMinPassingLandmarkFraction = 0.99;
  test_app_.testIfLandmarksMatchReference(
      kPrecision, kMinPassingLandmarkFraction);
}

void checkLandmarkQualityInView(
    const vi_map::VIMap& map, int expected_num_unknown_quality,
    int expected_num_good_quality, int expected_num_bad_quality) {
  pose_graph::VertexIdList all_vertices;
  map.getAllVertexIds(&all_vertices);

  int num_unknown_quality = 0;
  int num_good_quality = 0;
  int num_bad_quality = 0;
  for (const pose_graph::VertexId& vertex_id : all_vertices) {
    const vi_map::Vertex& vertex = map.getVertex(vertex_id);
    for (const vi_map::Landmark& landmark : vertex.getLandmarks()) {
      switch (landmark.getQuality()) {
        case vi_map::Landmark::Quality::kUnknown: {
          ++num_unknown_quality;
          break;
        }
        case vi_map::Landmark::Quality::kBad: {
          ++num_bad_quality;
          break;
        }
        case vi_map::Landmark::Quality::kGood: {
          ++num_good_quality;
          break;
        }
        default: {
          // Fall through intended.
        }
      }
    }
  }

  EXPECT_EQ(num_unknown_quality, expected_num_unknown_quality);
  EXPECT_EQ(num_good_quality, expected_num_good_quality);
  EXPECT_EQ(num_bad_quality, expected_num_bad_quality);
}

TEST_F(ViMappingTest, TestLandmarkQualityMetrics) {
  corruptLandmarks();

  vi_map::VIMap* map = test_app_.getMapMutable();
  checkLandmarkQualityInView(*map, 8359, 0, 0);

  FLAGS_vi_map_landmark_quality_min_observation_angle_deg = 5;
  FLAGS_vi_map_landmark_quality_min_observers = 4;
  FLAGS_vi_map_landmark_quality_max_distance_from_closest_observer = 40;
  FLAGS_vi_map_landmark_quality_min_distance_from_closest_observer = 0.05;

  EXPECT_TRUE(retriangulateLandmarks(map));
  checkLandmarkQualityInView(*map, 0, 6138, 2221);
}

}  // namespace landmark_triangulation

MAPLAB_UNITTEST_ENTRYPOINT


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <fstream>
#include <vector>
#include <string>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/graphviz.hpp>

int
main()
{
  using namespace boost;
  GraphvizGraph g;
  read_graphviz("figs/cc-internet.dot", g);

  std::vector<int> component(num_vertices(g));

  connected_components
    (g, make_iterator_property_map(component.begin(),
                                   get(vertex_index, g), component[0]));

  property_map < GraphvizGraph, vertex_attribute_t >::type
    vertex_attr_map = get(vertex_attribute, g);
  std::string color[] = {
  "white", "gray", "black", "lightgray"};
  graph_traits < GraphvizGraph >::vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
    vertex_attr_map[*vi]["color"] = color[component[*vi]];
    vertex_attr_map[*vi]["style"] = "filled";
    if (vertex_attr_map[*vi]["color"] == "black")
      vertex_attr_map[*vi]["fontcolor"] = "white";
  }
  write_graphviz("figs/cc-internet-out.dot", g);

}