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

  @copyright 2016 NumScale SAS

  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_CONSTANT_INVSQRTEPS_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_INVSQRTEPS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate  value \f$\1/sqrt(Eps<T>()\f$

    @return The Invsqrteps constant for the proper type
  **/
  template<typename T> T invsqrteps();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant Invsqrteps.

      @return The invsqrteps constant for the proper type
    **/
    const boost::dispatch::functor<tag::invsqrteps_> invsqrteps = {};
  }
} }
#endif

#include <boost/simd/constant/scalar/invsqrteps.hpp>
#include <boost/simd/constant/simd/invsqrteps.hpp>

#endif


/*  The compile and run commands are:

	Compile:- $ g++-7 mr-pr-cpp.cpp /usr/lib/x86_64-linux-gnu/libboost_system.a /usr/lib/x86_64-linux-gnu/libboost_iostreams.a /usr/lib/x86_64-linux-gnu/libboost_filesystem.a -pthread -o mr-pr-cpp.o
	Run:- $ ./mr-pr-cpp.o <input_file>.txt -o <output_file>.txt

	* Compile command assumes the presence of the Mapreduce include files in the cwd
	* Run command assumes the existence of a file <input_file>.txt in a directory named "test" in the cwd of the code
	* Run command also assumes the existence of a directory named "output" where it writes the outputs of the pagerank algorithm execution.

*/

// Including the Boost configurations
#include <boost/config.hpp>
#if defined(BOOST_MSVC)
#   pragma warning(disable: 4127)

// turn off checked iterators to avoid performance hit
#   if !defined(__SGI_STL_PORT)  &&  !defined(_DEBUG)
#       define _SECURE_SCL 0
#       define _HAS_ITERATOR_DEBUGGING 0
#   endif
#endif

#include <bits/stdc++.h>
#include "mapreduce.hpp"
using namespace std;

// Functions for the proper usage of the code
const char *OUTPUT_ARG = "-o";

void usage() {
    cerr << "Use the below format " << endl
    	 << "pagerank <graph_file> -o <output_file>" << endl
         << " -o enable output " << endl;
}

// Global variables for the pagerank algorithm
vector<vector<double>> intermediate_page(100000);
vector<vector<int>> outgoing_links(100000);
vector<double> pageranks(100000);
vector<double> pageranks_calc(100000);

// Parameters of alpha and convergence rate
double alpha = 0.85;
double conv = 0.00001;
double dangling_pointer =0.0f;
int total_webpages=0;

namespace page_rank {

template<typename MapTask>
class number_source : mapreduce::detail::noncopyable
{
  public:
    number_source()
      : sequence_(0)
    {
    }

    bool const setup_key(typename MapTask::key_type &key)
    {
        key = sequence_++;
        return (key <= total_webpages);
    }

    bool const get_data(typename MapTask::key_type const &key, typename MapTask::value_type &value)
    {
        value = pageranks[key];
        return true;
    }

  private:
    unsigned  sequence_;
};

struct map_task : public mapreduce::map_task<unsigned, double>
{
    template<typename Runtime>
    void operator()(Runtime &runtime, key_type const &key, value_type const &value) const
    {
		int chilren = outgoing_links[key].size();

		if(chilren!=0){
			for(int i =0; i<chilren; i++){
		        typename Runtime::reduce_task_type::key_type const emit_temp = outgoing_links[key][i];
		        double temp = double(value/chilren);
		        runtime.emit_intermediate(emit_temp, temp);
			}
		}

		// Debugging
		runtime.emit_intermediate(key,0);

    }
};

struct reduce_task : public mapreduce::reduce_task<unsigned, double>
{
    template<typename Runtime, typename It>
    void operator()(Runtime &runtime, key_type const &key, It it, It ite) const
    {
        value_type results(*it);
		double pagerank_temp = 0.0f;
        for (It it1=it; it1!=ite; it1++)
        {
			pagerank_temp += (double)(*it1);
        }

		// Applying the pagerank update rule
		double pageranks_calc_temp = pagerank_temp*alpha + (1-alpha)/total_webpages + alpha*dangling_pointer/total_webpages;

		// Emitting the (Key,Value) pair for the pageranks_calc
		results = pageranks_calc_temp;
		runtime.emit(key,results);
    }
};

typedef
mapreduce::job<page_rank::map_task,
               page_rank::reduce_task,
               mapreduce::null_combiner,
               page_rank::number_source<page_rank::map_task>
> job;

} // namespace page_rank

// Function to check the convergence
bool converging(vector<double> pageranks, vector<double> pageranks_calc, int total_webpages){
	bool isConverged= true;
	for (int i = 0; i < total_webpages; i++){
		if (abs(pageranks[i] - pageranks_calc[i])>conv){
			isConverged = false;
			break;
		}
	}
	return isConverged;
}

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

	// File names
	string input_file;
	string output_file;

	// Reading the command line inputs
	if (argc == 4){
		if (!strcmp(argv[2],OUTPUT_ARG)){
			input_file=argv[1];
			output_file=argv[3];
		}

		else{
			usage();
			exit(1);
		}
	}

	else{
		usage();
		exit(1);
	}

	int max_page_id = 0;
	int min_page_id = 0;

	// Reading inputs from the specified file in the test folder of the standard pagerank repo
	ifstream fopen;
	fopen.open("test/" + input_file);
	while(!fopen.eof()){
		int a,b;
		fopen>>a>>b;
		outgoing_links[a].push_back(b);

		int max_temp = std::max(a,b);
		if (max_temp > max_page_id)
			max_page_id = max_temp;

		int min_temp = std::min(a,b);
		if (min_temp < min_page_id)
			min_page_id = min_temp;
	}

	// Total number of web-pages as: max_page_id - min_page_id + 1
	fopen.close();
	total_webpages = max_page_id - min_page_id + 1;

	// Initializing the pageranks;
	double pgr = double(1.0f/total_webpages);
	for(int i=0; i<total_webpages; i++){
		pageranks[i] = pgr;
	}

	// Initializing the Mapreduce specifications
    mapreduce::specification spec;
    spec.reduce_tasks = std::max(1U, std::thread::hardware_concurrency());

	while(true){

		cout << pageranks[0] << endl;

		// Handling dangling pointers
		dangling_pointer=0.0f;
		for(int i=0; i<total_webpages; i++){
			if (outgoing_links[i].size()==0){
				dangling_pointer+=pageranks[i];
			}
		}

		// Scheduling the map-reduce for the pagerank calculation
        page_rank::job::datasource_type number_source;
        page_rank::job job(number_source, spec);
        mapreduce::results result;

		#ifdef _DEBUG
			job.run<mapreduce::schedule_policy::sequential<page_rank::job> >(result);
		#else
			job.run<mapreduce::schedule_policy::cpu_parallel<page_rank::job> >(result);
		#endif

		// Storing the reults of the calculated pagerank
        for (auto it=job.begin_results(); it!=job.end_results(); it++){
            pageranks_calc[it->first] = it->second;
        }

		// Checking for convergence
		if(converging(pageranks, pageranks_calc, total_webpages)){
			break;
		}
		else{
			for(int i=0; i<total_webpages; i++){
				pageranks[i] = pageranks_calc[i];
			}
		}
	}

	// Writing to the file
	ofstream fout;
	fout.open("output/" + output_file);

	string str;
	double sumVal = 0.0;
	for(int i=0; i<total_webpages; i++){
		fout<<i<<" = "<<pageranks[i]<<endl;
		sumVal += pageranks[i];
	}

	fout << "sum " << sumVal << endl;
	fout.close();

	return 0;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 4> test_int_6_4(const Eigen::Matrix<int, 6, 4> & M)
{
	return M;
}
void export_int_6_4()
{
	boost::python::def("test_int_6_4",test_int_6_4);
}


#define BOOST_TEST_MODULE LCMTest

#include "LCM.hpp"
#include <vector>
#include <cstdint>

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

BOOST_AUTO_TEST_SUITE(LCMSuite)

using namespace com::github::nimelo;
using namespace std;

BOOST_AUTO_TEST_CASE(ShouldCorrectlyReturnLeastCommonMultiple)
{
    // Arrange
    vector<int> numbers = { 1, 7, 9 };
    int expected_lcm = 1 * 7 * 9;

    LCM<int> lcm(numbers);

    // Act
    auto actual_lcm  = lcm.get();

    // Assert
    BOOST_CHECK_EQUAL(expected_lcm, actual_lcm);
}

BOOST_AUTO_TEST_CASE(ShouldCorrectlyReturnLeastCommonMultiple_2)
{
    // Arrange
    vector<int> numbers = { 48, 180 };
    int expected_lcm = 720;

    LCM<int> lcm(numbers);

    // Act
    auto actual_lcm  = lcm.get();

    // Assert
    BOOST_CHECK_EQUAL(expected_lcm, actual_lcm);
}

BOOST_AUTO_TEST_CASE(ShouldCorrectlyReturnLeastCommonMultiple_3)
{
    // Arrange
    vector<int> numbers = { 21, 6 };
    int expected_lcm = 42;

    LCM<int> lcm(numbers);

    // Act
    auto actual_lcm  = lcm.get();

    // Assert
    BOOST_CHECK_EQUAL(expected_lcm, actual_lcm);
}

BOOST_AUTO_TEST_CASE(ShouldCorrectlyReturnLeastCommonMultiple_4)
{
    // Arrange
    vector<int> numbers = { 8, 9, 21 };
    int expected_lcm = 504;

    LCM<int> lcm(numbers);

    // Act
    auto actual_lcm  = lcm.get();

    // Assert
    BOOST_CHECK_EQUAL(expected_lcm, actual_lcm);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 * Created on January 12, 2021, x:xx AM
 */

#include <gtest/gtest.h>

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

#include "MaterialLib/MPL/Medium.h"
#include "MaterialLib/MPL/Properties/CreateGasPressureDependentPermeability.h"
#include "MaterialLib/MPL/Properties/GasPressureDependentPermeability.h"
#include "MaterialLib/MPL/Utils/FormEigenTensor.h"
#include "ParameterLib/ConstantParameter.h"
#include "TestMPL.h"
#include "Tests/TestTools.h"

TEST(MaterialPropertyLib, GasPressureDependentPermeability)
{
    ParameterLib::ConstantParameter<double> const k0("k0", 1.e-20);
    double const a1 = 0.125;
    double const a2 = 152.0;
    double const pressure_threshold = 3.2e6;
    double const min_permeability = 1.e-22;
    double const max_permeability = 1.e-10;

    auto const k_model = MPL::GasPressureDependentPermeability<3>(
        "k_gas", k0, a1, a2, pressure_threshold, min_permeability,
        max_permeability, nullptr);

    ParameterLib::SpatialPosition const pos;
    double const t = std::numeric_limits<double>::quiet_NaN();
    double const dt = std::numeric_limits<double>::quiet_NaN();
    MPL::VariableArray vars;

    /// For gas pressure smaller than threshold value.
    {
        double const p_gas = 2.5e6;

        vars[static_cast<int>(MPL::Variable::phase_pressure)] = p_gas;
        auto const k = MPL::formEigenTensor<3>(k_model.value(vars, pos, t, dt));

        double const k_expected = 1.312500000000000000e-20;

        ASSERT_LE(std::fabs(k_expected - k(0, 0)) / k_expected, 1e-10)
            << "for expected permeability with gas pressure below threshold"
            << k_expected
            << " and for computed permeability with gas pressure below "
               "threshold "
            << k(0, 0);
    }
    /// For gas pressure bigger than threshold value.
    {
        double const p_gas = 4.5e6;

        vars[static_cast<int>(MPL::Variable::phase_pressure)] = p_gas;
        auto const k = MPL::formEigenTensor<3>(k_model.value(vars, pos, t, dt));

        double const k_expected = 1.990000000000000000000e-18;

        ASSERT_LE(std::fabs(k_expected - k(0, 0)) / k_expected, 1e-10)
            << "for expected permeability with gas pressure above threshold "
            << k_expected
            << " and for computed permeability with gas pressure above "
               "threshold "
            << k(0, 0);
    }
}


#include <boost/random/exponential_distribution.hpp>


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/test/impl/execution_monitor.ipp>

#include <algorithm>
#include <vector>
#include <map>

#include <boost/geometry/index/detail/minmax_heap.hpp>
#include <boost/geometry/index/detail/maxmin_heap.hpp>

using namespace boost::geometry::index::detail;

struct noncopyable
{
    noncopyable(int i_) : i(i_) {}
    noncopyable(noncopyable const&) = delete;
    noncopyable& operator=(noncopyable const&) = delete;
    noncopyable(noncopyable&&) = default;
    noncopyable& operator=(noncopyable&&) = default;
    bool operator<(noncopyable const& other) const { return i < other.i; }
    operator int() const { return i; }
    int i;
};

template <typename T>
struct minmax_default
{
    minmax_default() = default;
    minmax_default(std::vector<int> const& vec)
        : heap(vec.begin(), vec.end())
    {
        make_minmax_heap(heap.begin(), heap.end());
    }
    T const& top() const
    {
        return heap[0];
    }
    T const& bottom() const
    {
        return bottom_minmax_heap(heap.begin(), heap.end());
    }
    void push(int i)
    {
        heap.push_back(T(i));
        push_minmax_heap(heap.begin(), heap.end());
    }
    void pop_top()
    {
        pop_top_minmax_heap(heap.begin(), heap.end());
        heap.pop_back();
    }
    void pop_bottom()
    {
        pop_bottom_minmax_heap(heap.begin(), heap.end());
        heap.pop_back();
    }
    bool is_heap() const
    {
        return is_minmax_heap(heap.begin(), heap.end());
    }
    bool empty() const
    {
        return heap.empty();
    }
    std::vector<T> heap;
};

template <typename T>
struct minmax_less
{
    minmax_less() = default;
    minmax_less(std::vector<int> const& vec)
        : heap(vec.begin(), vec.end())
    {
        make_minmax_heap(heap.begin(), heap.end(), std::less<>());
    }
    T const& top() const
    {
        return heap[0];
    }
    T const& bottom() const
    {
        return bottom_minmax_heap(heap.begin(), heap.end(), std::less<>());
    }
    void push(int i)
    {
        heap.push_back(T(i));
        push_minmax_heap(heap.begin(), heap.end(), std::less<>());
    }
    void pop_top()
    {
        pop_top_minmax_heap(heap.begin(), heap.end(), std::less<>());
        heap.pop_back();
    }
    void pop_bottom()
    {
        pop_bottom_minmax_heap(heap.begin(), heap.end(), std::less<>());
        heap.pop_back();
    }
    bool is_heap() const
    {
        return is_minmax_heap(heap.begin(), heap.end(), std::less<>());
    }
    bool empty() const
    {
        return heap.empty();
    }
    std::vector<T> heap;
};

template <typename T>
struct maxmin_greater
{
    maxmin_greater() = default;
    maxmin_greater(std::vector<int> const& vec)
        : heap(vec.begin(), vec.end())
    {
        make_maxmin_heap(heap.begin(), heap.end(), std::greater<>());
    }
    T const& top() const
    {
        return heap[0];
    }
    T const& bottom() const
    {
        return bottom_maxmin_heap(heap.begin(), heap.end(), std::greater<>());
    }
    void push(int i)
    {
        heap.push_back(T(i));
        push_maxmin_heap(heap.begin(), heap.end(), std::greater<>());
    }
    void pop_top()
    {
        pop_top_maxmin_heap(heap.begin(), heap.end(), std::greater<>());
        heap.pop_back();
    }
    void pop_bottom()
    {
        pop_bottom_maxmin_heap(heap.begin(), heap.end(), std::greater<>());
        heap.pop_back();
    }
    bool is_heap() const
    {
        return is_maxmin_heap(heap.begin(), heap.end(), std::greater<>());
    }
    bool empty() const
    {
        return heap.empty();
    }
    std::vector<T> heap;
};

template <typename T>
struct maxmin_default_switch
{
    maxmin_default_switch() = default;
    maxmin_default_switch(std::vector<int> const& vec)
        : heap(vec.begin(), vec.end())
    {
        make_maxmin_heap(heap.begin(), heap.end());
    }
    T const& bottom() const
    {
        return heap[0];
    }
    T const& top() const
    {
        return bottom_maxmin_heap(heap.begin(), heap.end());
    }
    void push(int i)
    {
        heap.push_back(T(i));
        push_maxmin_heap(heap.begin(), heap.end());
    }
    void pop_top()
    {
        pop_bottom_maxmin_heap(heap.begin(), heap.end());
        heap.pop_back();
    }
    void pop_bottom()
    {
        pop_top_maxmin_heap(heap.begin(), heap.end());
        heap.pop_back();
    }
    bool is_heap() const
    {
        return is_maxmin_heap(heap.begin(), heap.end());
    }
    bool empty() const
    {
        return heap.empty();
    }
    std::vector<T> heap;
};

template <typename Heap>
void test()
{
    std::vector<int> vec;
    int const n = 20;
    for (int i = 0; i < n; ++i)
    {
        vec.push_back(rand() % n);
    }

    {
        std::map<int, int> map;
        Heap heap;
        for (int i : vec)
        {
            heap.push(i);
            BOOST_CHECK(heap.is_heap());

            map[i]++;
            BOOST_CHECK_EQUAL(heap.top(), map.begin()->first);
            BOOST_CHECK_EQUAL(heap.bottom(), (--map.end())->first);
        }

        while (! heap.empty())
        {
            int i = heap.top();
            BOOST_CHECK_EQUAL(i, map.begin()->first);
            BOOST_CHECK_EQUAL(heap.bottom(), (--map.end())->first);
            BOOST_CHECK(map[i] > 0);
            map[i]--;
            if (map[i] <= 0)
                map.erase(i);

            heap.pop_top();
            BOOST_CHECK(heap.is_heap());
        }

        BOOST_CHECK(map.empty());
    }

    {
        Heap heap(vec);
        BOOST_CHECK(heap.is_heap());

        std::map<int, int> map;
        for (int i : vec)
            map[i]++;
        BOOST_CHECK_EQUAL(heap.top(), map.begin()->first);
        BOOST_CHECK_EQUAL(heap.bottom(), (--map.end())->first);

        while (! heap.empty())
        {
            int i = heap.bottom();
            BOOST_CHECK_EQUAL(heap.top(), map.begin()->first);
            BOOST_CHECK_EQUAL(i, (--map.end())->first);
            BOOST_CHECK(map[i] > 0);
            map[i]--;
            if (map[i] <= 0)
                map.erase(i);

            heap.pop_bottom();
            BOOST_CHECK(heap.is_heap());
        }

        BOOST_CHECK(map.empty());
    }
}

int test_main(int, char* [])
{
    test<minmax_default<int>>();
    test<minmax_default<noncopyable>>();
    test<minmax_less<int>>();
    test<maxmin_greater<int>>();
    test<maxmin_default_switch<int>>();

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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 NT2_HYPERBOLIC_FUNCTIONS_SCALAR_SINHC_HPP_INCLUDED
#define NT2_HYPERBOLIC_FUNCTIONS_SCALAR_SINHC_HPP_INCLUDED
#include <nt2/hyperbolic/functions/sinhc.hpp>
#include <nt2/hyperbolic/functions/details/sinhc_kernel.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/log_2.hpp>
#include <nt2/include/constants/maxlog.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/average.hpp>
#include <nt2/include/functions/scalar/exp.hpp>
#include <nt2/include/functions/scalar/if_else.hpp>
#include <nt2/include/functions/scalar/rec.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( sinhc_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 1 sinhc is computed using a polynomial(float)
      // respectively rational(double) approx inspired from cephes sinh approx.
      // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
      // computed
      // * in the first case sinh is ((e-rec(e))/2)/x
      // * in the second     sinh is (e/2/x)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2 defined in Maxshlog
      //////////////////////////////////////////////////////////////////////////////
      result_type x = nt2::abs(a0);
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (x == Inf<A0>()) return x;
      #endif
      if( x < One<A0>())
      {
        return details::sinhc_kernel<A0>::compute(sqr(x));
      }
      else
      {
        bool test1 = (x >  Maxlog<A0>()-Log_2<A0>());
        A0 fac = if_else(test1, Half<A0>(), One<A0>());
        A0 tmp = exp(x*fac);
        A0 tmp1 = (Half<A0>()*tmp)/x;
        return if_else(test1, tmp1*tmp, average(tmp, -rec(tmp))/x);
      }
     }
  };

} }
#endif


#pragma once

#include <opencv2/core/core.hpp>
#include <Eigen/Core>

#include "../../Colour/colour.hh"
#include "../../DistributionTracker/distributiontracker.hh"
#include "../../geometry/Line.hh"
#include "../../geometry/LineSegment/LineSegment2/LineSegment2i/linesegment2i.hh"
#include "../linefinder.hh"

namespace bold
{
  class RandomPairLineFinder : public LineFinder
  {
    // TODO initialise controls

  public:
    struct LineHypothesis
    {
      LineHypothesis(Line const& line, Eigen::Vector2i const& dot1, Eigen::Vector2i const& dot2)
      : d_lengthDistribution(),
        d_theta(line.theta()),
        d_radius(line.radius())
      {
        auto diff = dot2 - dot1;

        d_lengthDistribution.add(length(diff));

        // Determines whether we consider x or y for min/max
        d_isHorizontal = abs(diff.x()) > abs(diff.y());

        // Set min/max values
        int elementIndex = d_isHorizontal ? 0 : 1;
        if (dot1[elementIndex] < dot2[elementIndex])
        {
          d_min = dot1;
          d_max = dot2;
        }
        else
        {
          d_min = dot2;
          d_max = dot1;
        }

        d_lines.push_back(line);
      }

      bool tryMerge(Line const& line, Eigen::Vector2i const& dot1, Eigen::Vector2i const& dot2)
      {
        double dt = line.theta() - d_theta;
        double dr = line.radius() - d_radius;

        const double dtThreshold = Math::degToRad(10);
        const double drThreshold = 15;

        if (fabs(dt) < dtThreshold && fabs(dr) < drThreshold)
        {
          d_lines.push_back(line);

          // Update min/max values if breached
          int elementIndex = d_isHorizontal ? 0 : 1;
          if (dot1[elementIndex] < dot2[elementIndex])
          {
            if (d_min[elementIndex] < dot1[elementIndex])
              d_min = dot1;
            if (d_max[elementIndex] > dot2[elementIndex])
              d_max = dot2;
          }
          else
          {
            if (d_min[elementIndex] < dot2[elementIndex])
              d_min = dot2;
            if (d_max[elementIndex] > dot1[elementIndex])
              d_max = dot1;
          }

          d_lengthDistribution.add(length(dot2 - dot1));
          return true;
        }

        return false;
      }

      Line toLine() const
      {
        double t = d_theta;
        double r = d_radius;

        while (t < 0)
        {
          t += M_PI;
          r = -r;
        }

        while (t > M_PI)
        {
          t -= M_PI;
          r = -r;
        }

        return Line(r, t, d_lines.size());
      }

      int count() const
      {
        return d_lines.size();
      }

      Eigen::Vector2i min() const { return d_min; }
      Eigen::Vector2i max() const { return d_max; }
      DistributionTracker lengthDistribution() const { return d_lengthDistribution; }

      friend std::ostream& operator<<(std::ostream& stream, LineHypothesis const& hypothesis)
      {
        auto line = hypothesis.toLine();
        return stream
          << "theta=" << line.theta() << " (" << (line.thetaDegrees()) << " degs)"
          << " radius=" << line.radius() << " votes=" << hypothesis.count()
          << " length=" << (hypothesis.max().cast<double>() - hypothesis.min().cast<double>()).norm()
          << " lengthAvg=" << hypothesis.lengthDistribution().average()
          << " lengthStdDev=" << hypothesis.lengthDistribution().stdDev()
          << " lengthAvg/lengthStdDev=" << hypothesis.lengthDistribution().average()/hypothesis.lengthDistribution().stdDev();
      }

    private:
      static double length(Eigen::Vector2i v)
      {
        return sqrt(v.x()*v.x() + v.y()*v.y());
      }

      DistributionTracker d_lengthDistribution;
      Eigen::Vector2i d_min;
      Eigen::Vector2i d_max;
      double d_theta;
      double d_radius;
      std::vector<Line> d_lines;
      bool d_isHorizontal;
    };

    RandomPairLineFinder(int imageWidth, int imageHeight)
    : d_minDotManhattanDistance(3)
    {}

    std::vector<LineSegment2i> findLineSegments(std::vector<Eigen::Vector2i>& lineDots) override;

    void setMinDotManhattanDistance(unsigned minDotManhattanDistance) { d_minDotManhattanDistance = minDotManhattanDistance; }

  private:
    unsigned d_minDotManhattanDistance;
  };
}


/**********************
规定 type:
type 0: left bottom
type 1: right bottom
type 2: left top
type 3: right top
**********************/

#include "ros/ros.h"
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>

#include <vector>
#include <string.h>
#include <math.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <Eigen/SVD>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include </usr/local/include/g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <cmath>
#include <chrono>

#define start_region_boundary_distance 2.1

std::vector<int> laser_blacklist;

double angle_max;
double angle_min;
double angle_increment;

int corner_type = 0;
//ranges_boundary和angles_boundary是按逆时针顺序排列的边缘点信息
std::vector<double> ranges_boundary;
std::vector<double> angles_boundary;

int corner = 0;

//g2o图优化顶点：位姿
class BoundaryPoseVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    // 重置
    virtual void setToOriginImpl()
    {
        _estimate << 0,0,0;
    }
     // 更新
    virtual void oplusImpl( const double* update )
    {
        _estimate += Eigen::Vector3d(update);
    }
    // 存盘和读盘：留空
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
//_measurement是角度和距离的二维向量
//场地下边的点所构成的图优化边
class BoundaryEdge_Bottom: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, BoundaryPoseVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    BoundaryEdge_Bottom(double point_theta, double point_range):
    BaseUnaryEdge(), _point_theta(point_theta), _point_range(point_range) {}
    // 计算曲线模型误差
    void computeError()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();
        _error(0,0) = pose_estimation(1) + _measurement(1) * sin( _measurement(0) + pose_estimation(2) ) ;
    }
    virtual void linearizeOplus()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = 1;
        _jacobianOplusXi(0,2) = _point_range * cos(_point_theta + pose_estimation(2));
    }
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}

public:
	  double _point_range;
		double _point_theta;
		//measurement(0)其实和point_theta是一样的，measurement(1)其实和point_range是一样的
};
//场地左边的点所构成的图优化边
class BoundaryEdge_Left: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, BoundaryPoseVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    BoundaryEdge_Left(double point_theta, double point_range):
    BaseUnaryEdge(), _point_theta(point_theta), _point_range(point_range) {}
    // 计算曲线模型误差
    void computeError()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();
        _error(0,0) = pose_estimation(0) + _measurement(1) * cos( _measurement(0) + pose_estimation(2) ) ;
    }
    virtual void linearizeOplus()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();

        _jacobianOplusXi(0,0) = 1;
        _jacobianOplusXi(0,1) = 0;
        _jacobianOplusXi(0,2) = -_point_range * sin(_point_theta + pose_estimation(2));
    }
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}

public:
	  double _point_range;
		double _point_theta;
		//measurement(0)其实和point_theta是一样的，measurement(1)其实和point_range是一样的
};
class BoundaryEdge_Top: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, BoundaryPoseVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    BoundaryEdge_Top(double point_theta, double point_range):
    BaseUnaryEdge(), _point_theta(point_theta), _point_range(point_range) {}
    // 计算曲线模型误差
    void computeError()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();
        _error(0,0) = 5.0 - pose_estimation(1) - _measurement(1) * sin( _measurement(0) + pose_estimation(2) ) ;
    }
    virtual void linearizeOplus()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -1;
        _jacobianOplusXi(0,2) = -_point_range * cos(_point_theta + pose_estimation(2));
    }
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}

public:
	  double _point_range;
		double _point_theta;
		//measurement(0)其实和point_theta是一样的，measurement(1)其实和point_range是一样的
};
class BoundaryEdge_Right: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, BoundaryPoseVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    BoundaryEdge_Right(double point_theta, double point_range):
    BaseUnaryEdge(), _point_theta(point_theta), _point_range(point_range) {}
    // 计算曲线模型误差
    void computeError()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();
        _error(0,0) = 8.0 - pose_estimation(0) - _measurement(1) * cos( _measurement(0) + pose_estimation(2) ) ;
    }
    virtual void linearizeOplus()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -1;
        _jacobianOplusXi(0,2) = _point_range * sin(_point_theta + pose_estimation(2));
    }
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}

public:
	  double _point_range;
		double _point_theta;
		//measurement(0)其实和point_theta是一样的，measurement(1)其实和point_range是一样的
};

//计算提取出来的边界点的曲率的平方的25倍，由于是为了比较大小，不在开方以及乘以倍数，注意不能提取最靠两边的四个点，否则会出现错误
double curve_smoothness_square(int number) {
	double delta_x = 0, delta_y = 0;

	for (int i = -9 ; i < 10; i++) {
			if (i == 0)
				continue;
			double delta_angle = angles_boundary[number + i] - angles_boundary[number];
			delta_x += ranges_boundary[number] - ranges_boundary[number + i] * cos(delta_angle);
			delta_y += ranges_boundary[number + i] * sin(delta_angle);
	}
	double smoothness_square = (delta_x * delta_x + delta_y * delta_y) / (ranges_boundary[number] * ranges_boundary[number]);
	return smoothness_square;
}
//将角度限制在-π到π之间
void angle_standard(double& angle) {
	while (angle > M_PI)
		angle -= 2 * M_PI;
	while (angle < - M_PI)
		angle += 2 * M_PI;
}

//提取边缘点信息到ranges_boundary和angles_boundary
void extract_boundary_information(const std::vector<double> &ranges_raw, const std::vector<int> &increment_raw) {
	int available_number = ranges_raw.size();
	std::vector<bool> distance(available_number, false);

	//找到第一个不符合距离要求的点
	int first_negative = -1;
	for (int i = 0; i < available_number; i++) {
		if (ranges_raw[i] < start_region_boundary_distance)
			distance[i] = true;
		else if (first_negative == -1)
			first_negative = i;
	}

	int continuous_max_last_location = 0;
	int continuous_max_number = 0;
	int continuous_max_first_location = 0;
	int continuous_first_location_temp = 0;
	int continuous_number_now = 0;
	//找到满足距离要求的最大连续点集,以判断两个直角边位置,从第一个不满足距离要求的点开始，循环一圈，防止重复遗漏，以及vector起始终止处连续点集判断错误的情况
	for (int i = first_negative + 1; i < available_number; i++) {
		if (distance[i]) {
			//如果上一个点远，这一个点近，则取当前点作为当前连续点集起始边界，由于是从first_negative+1开始，到available结束的，所以不会判断初始集外的位置
			if (!distance[i - 1])
				continuous_first_location_temp = i;

			continuous_number_now++;
			if (continuous_number_now > continuous_max_number) {
				continuous_max_number = continuous_number_now;
				continuous_max_last_location = i;
				continuous_max_first_location = continuous_first_location_temp;
			}
		} else
			continuous_number_now = 0;
	}
	if (distance[0]) {
		if (!distance[available_number - 1])
			continuous_first_location_temp = 0;

		continuous_number_now++;
		if (continuous_number_now > continuous_max_number) {
			continuous_max_number = continuous_number_now;
			continuous_max_last_location = 0;
			continuous_max_first_location = continuous_first_location_temp;
		}
	} else
		continuous_number_now = 0;
	for (int i = 1; i < first_negative + 1; i++) {
		if (distance[i]) {
			//如果上一个点远，这一个点近，则取当前点作为当前连续点集起始边界，由于是从first_negative+1开始，到available结束的，所以不会判断初始集外的位置
			if (!distance[i - 1])
				continuous_first_location_temp = i;

			continuous_number_now++;
			if (continuous_number_now > continuous_max_number) {
				continuous_max_number = continuous_number_now;
				continuous_max_last_location = i;
				continuous_max_first_location = continuous_first_location_temp;
			}
		} else
			continuous_number_now = 0;
	}

	ranges_boundary.clear();
	angles_boundary.clear();

	if (continuous_max_first_location < continuous_max_last_location) {
		for (int i = continuous_max_first_location; i < continuous_max_last_location + 1; i++) {
			ranges_boundary.push_back(ranges_raw[i]);
			angles_boundary.push_back(angle_min + angle_increment * increment_raw[i]);
		}
	} else {
		for (int i = continuous_max_first_location; i < available_number; i++) {
			ranges_boundary.push_back(ranges_raw[i]);
			angles_boundary.push_back(angle_min + angle_increment * increment_raw[i]);
		}
		for (int i = 0; i <= continuous_max_last_location; i++) {
			ranges_boundary.push_back(ranges_raw[i]);
			angles_boundary.push_back(angle_min + angle_increment * increment_raw[i]);
		}
	}
}

//根据距离算坐标，根据曲率算角点，corner_number是曲率最大点
void find_roughpose_and_corner(Eigen::Vector3d &pose, int &corner_number, const int type) {

	double smoothness_max = 0;
	corner_number = 0;
	for (int i = 9; i < ranges_boundary.size() - 9; i++) {
		double smoothness = curve_smoothness_square(i);
		if (smoothness > smoothness_max) {
			smoothness_max = smoothness;
			corner_number = i;
		}
	}
	std::cout << "Corner: " << corner_number << "\tSmoothness_max1: " << smoothness_max << std::endl;

	double min_range1 = 100, min_range2 = 100;

	for (int i = 0; i < corner_number - 9; i++) {
		if (ranges_boundary[i] < min_range1)
			min_range1 = ranges_boundary[i];
	}
	for (int i = corner_number + 9; i < ranges_boundary.size(); i++) {
		if (ranges_boundary[i] < min_range2) {
			min_range2 = ranges_boundary[i];
		}
	}

	//距离边缘最近的距离分别是lidar位置x,y，alpha是x=0线到lidar的angle_min的基准线的角度 theta是中间变量
	double x, y, alpha, theta;

	if (min_range1 > ranges_boundary[corner_number] && min_range2 > ranges_boundary[corner_number]) {
		std::cout << "Ranges of all boundary points are bigger than range of corner points!" << std::endl
							<< "Fatal error happened! Program is shuting down..." << std::endl;

		exit(0);
	}

	switch(type) {
		case 0:
			alpha = angles_boundary[corner_number];
			x = min_range1;
			y = min_range2;
			if (min_range1 < min_range2)
				theta = acos(min_range1 / ranges_boundary[corner_number]);
			else
				theta = asin(min_range2 / ranges_boundary[corner_number]);
			alpha = M_PI - alpha + theta;
			break;
		case 1:
			alpha = angles_boundary[corner_number];
			x = 8 - min_range2;
			y = min_range1;
			if (min_range1 > min_range2)
				theta = acos(min_range2 / ranges_boundary[corner_number]);
			else
				theta = asin(min_range1 / ranges_boundary[corner_number]);
			alpha = - alpha - theta;
			break;
		case 2:
			alpha = angles_boundary[corner_number];
			x = min_range2;
			y = 5 - min_range1;
			if (min_range1 > min_range2)
				theta = acos(min_range2 / ranges_boundary[corner_number]);
			else
				theta = asin(min_range1 / ranges_boundary[corner_number]);
			alpha = M_PI - theta - alpha;
			break;
		case 3:
			alpha = angles_boundary[corner_number];
			x = 8 - min_range1;
			y = 5 - min_range2;
			if (min_range1 < min_range2)
				theta = acos(min_range1 / ranges_boundary[corner_number]);
			else
				theta = asin(min_range2 / ranges_boundary[corner_number]);
			alpha = -alpha + theta;
			break;
	}

	angle_standard(alpha);
	pose = Eigen::Vector3d(x, y, alpha);
}

Eigen::Vector3d find_pose_precise(const Eigen::Vector3d &initial_pose, const int& corner, const int type) {
	Eigen::Vector3d pose;
	// pose 维度为 3, landmark 维度为 1
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<3, 1> > Block;
	// 线性方程求解器
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
	// 矩阵块求解器
	Block* solver_ptr = new Block ( linearSolver );
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
	g2o::SparseOptimizer optimizer;
	optimizer.setAlgorithm ( solver );

  // vertex
	BoundaryPoseVertex* pose_vertex = new BoundaryPoseVertex(); // camera pose
	pose_vertex->setEstimate(initial_pose);
	pose_vertex->setId(0);
	optimizer.addVertex(pose_vertex);

	int index = 1;

	Eigen::Matrix2d Information_Matrix = Eigen::Matrix2d::Identity() * 1e-4;

	switch(type) {
		case 0:
			for (int i = 0; i < corner - 9; i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Left* edge = new BoundaryEdge_Left(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			for (int i = corner + 9; i < ranges_boundary.size(); i++) {
					Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
					BoundaryEdge_Bottom* edge = new BoundaryEdge_Bottom(angles_boundary[i], ranges_boundary[i]);
					edge->setId(index);
					edge->setVertex(0, pose_vertex);
					// 设置连接的顶点
					edge->setMeasurement(point);
					// 观测数值
					edge->setInformation(Information_Matrix);
					// 信息矩阵：协方差矩阵之逆
					optimizer.addEdge(edge);
					index++;
				}
			break;
		case 1:
			for (int i = 0; i < corner - 9; i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Bottom* edge = new BoundaryEdge_Bottom(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			for (int i = corner + 9; i < ranges_boundary.size(); i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Right* edge = new BoundaryEdge_Right(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			break;
		case 2:
			for (int i = 0; i < corner - 9; i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Top* edge = new BoundaryEdge_Top(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			for (int i = corner + 9; i < ranges_boundary.size(); i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Left* edge = new BoundaryEdge_Left(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			break;
		case 3:
			for (int i = 0; i < corner - 9; i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Right* edge = new BoundaryEdge_Right(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			for (int i = corner + 9; i < ranges_boundary.size(); i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Top* edge = new BoundaryEdge_Top(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			break;
	}
	std::cout << "start optimization" << std::endl;
	optimizer.initializeOptimization();
	optimizer.optimize(100);

	return pose_vertex->estimate();
}

void scanCallback(const sensor_msgs::LaserScanConstPtr msg) {
	angle_max = msg->angle_max;
	angle_min = msg->angle_min;
	angle_increment = msg->angle_increment;
	int angle_number = (int)( (angle_max - angle_min)/angle_increment ) + 1;

	std::vector<double> ranges_raw;
	std::vector<int> increment_raw;

	//去除遮挡点，保留有效雷达点信息，ranges_raw是range信息，increment是有效点顺序信息（用于后续生成角度信息）
	for (int i = 0; i < angle_number; i++) {
		if (msg->ranges[i] != std::numeric_limits<float>::infinity()) {
			ranges_raw.push_back(msg->ranges[i]);
			increment_raw.push_back(i);
		}
	}

	extract_boundary_information(ranges_raw, increment_raw);
	//std::cout << "boundary_size: " << ranges_boundary.size() << std::endl;
	Eigen::Vector3d rough_pose;

	find_roughpose_and_corner(rough_pose, corner, corner_type);

	std::cout << "rough_pose: " << rough_pose.transpose() << std::endl;

	Eigen::Vector3d pose;
	pose = find_pose_precise(rough_pose, corner, corner_type);
	angle_standard(pose(2));

	std::cout << "precise_pose: " << pose.transpose() << std::endl << std::endl;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "initial_pose");
  ros::NodeHandle n;

  ros::Subscriber scan_sub = n.subscribe("/scan", 1, scanCallback);
	sensor_msgs::LaserScan scan;

	ros::Publisher boundary_pub = n.advertise<sensor_msgs::PointCloud>("boundary", 50);
	ros::Publisher corner_pub = n.advertise<sensor_msgs::PointCloud>("corner", 50);

	ros::Rate loop_rate(10);
	while (ros::ok()) {
		unsigned int num_points = ranges_boundary.size();
		sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "laser";

    cloud.points.resize(num_points);

    //we'll also add an intensity channel to the cloud
    cloud.channels.resize(1);
    cloud.channels[0].name = "intensities";
    cloud.channels[0].values.resize(num_points);

    //generate some fake data for our point cloud
    for(unsigned int i = 0; i < num_points; ++i){
      cloud.points[i].x = ranges_boundary[i] * cos(angles_boundary[i]);
      cloud.points[i].y = ranges_boundary[i] * sin(angles_boundary[i]);
      cloud.points[i].z = 0;
      cloud.channels[0].values[i] = 100;
    }

    sensor_msgs::PointCloud corner_cloud;
		corner_cloud.header.stamp = ros::Time::now();
		corner_cloud.header.frame_id = "laser";
		corner_cloud.points.resize(2);
		corner_cloud.channels.resize(1);
		corner_cloud.channels[0].name = "intensities";
		corner_cloud.channels[0].values.resize(2);
		if (corner != 0) {
			corner_cloud.points[0].x = (ranges_boundary[corner] + 1) * cos(angles_boundary[corner]);
			corner_cloud.points[0].y = (ranges_boundary[corner] + 1) * sin(angles_boundary[corner]);
			corner_cloud.points[0].z = 0;
			corner_cloud.channels[0].values[0] = 200;
		}
		corner_cloud.points[1].x = 1;
		corner_cloud.points[1].y = 0;
		corner_cloud.points[1].z = 0;
		corner_cloud.channels[0].values[1] = 10;
    boundary_pub.publish(cloud);
		corner_pub.publish(corner_cloud);
		ros::spinOnce();
		loop_rate.sleep();
	}
  return 0;
}


#ifndef FINDINTRA_CPP
#define FINDINTRA_CPP

#include <cstdlib>
#include <ctime>
#include <fstream>
#include <iostream>
#include <map>
#include <math.h>
#include <vector>

#include <gsl/gsl_cdf.h>

#include <scythestat/rng/mersenne.h>
#include <scythestat/distributions.h>
#include <scythestat/ide.h>
#include <scythestat/la.h>
#include <scythestat/matrix.h>
#include <scythestat/rng.h>
#include <scythestat/smath.h>
#include <scythestat/stat.h>
#include <scythestat/optimize.h>

#include <IRLS_glm/IRLS.h>

//#include <RInside.h>

#include <mlpack/methods/lars/lars.hpp>
#include <mlpack/methods/linear_regression/linear_regression.hpp>
#include <boost/test/unit_test.hpp>

#define INTTAG 0
#define BOOLTAG 1
#define DIETAG 2

#define TESTFREQTHRES 1
#define NEIGHBDIS 5
//#define MORANI 0.001
#define PCUTOFF 0.005
#define PRECISION 10e48
#define READBLOCK 409600
#define OPTDIST 1000

//using namespace std;
using std::abs;
using std::basic_string;
using std::cerr;
using std::cin;
using std::cout;
using std::endl;
using std::exception;
using std::flush;
using std::ifstream;
using std::istringstream;
using std::make_pair;
using std::map;
using std::ofstream;
using std::ostringstream;
using std::pair;
using std::pow;
using std::sort;
using std::string;
using std::vector;

int reader_dif(const string&, vector< vector< vector<int> > >&);
int reader_dsm(const string&, vector< vector<int> >&);
int reader_coef(const string&, map<int, double>&);
void reader_ictm(const string&, map<int, int>&);
int findIntraDomainInteraction(map<int, int>&, string, vector< vector<int> >&, vector< vector< vector<int> > >&, map<int, int>&, map<int, double>&, map< int, map< pair<int, int>, double > >&, double, double, double);
int rmDependentCol(vector< vector<double> >&, vector<double>&, vector< pair<int, int> >&, double, int&, vector< vector<double> >&);
int calStanScores(vector< vector<double> >&);
int calPearsonCorr(vector< vector<double> >&, vector< pair<int, int> >&, const double&, int, map<int, int>&);
double calBgFrags(map<int, double>&, int);
int writer_corrFile(map< pair<int, int>, double >&, string&);

void usage();

inline void usage()
{
    cout <<"Usage: findIntraDomainInteraction empericalDistributionFile interChrFreqFile domainSitesFile domainInteractionFreqFile chrNum MORANI CorrelationThresholdForScreen"<<endl;
}

//convtIndex is now 0 based
inline int convtIndex(int row_fun, int col_fun, int siteNum_fun)
{
    if (row_fun==col_fun || row_fun>=siteNum_fun || col_fun>=siteNum_fun || row_fun<0 || col_fun<0)
    {//cout<<"row or column exceed bound."<<endl;
        return -1;
    }
    else
    {
        int max_fun = 0, min_fun = 0;
        if (row_fun<col_fun)
        {
            //switch to 1 based
            max_fun = col_fun+1;
            min_fun = row_fun+1;
        }
        else
        {
            max_fun = row_fun+1;
            min_fun = col_fun+1;
        }
        return((2*siteNum_fun-min_fun)*(min_fun-1)/2+max_fun-min_fun) - 1;//-1 put the output to be 0 based
    }
}

//l=sigma(yi(theta*xi+offseti)-exp(theta*xi+offseti))
class PoissonModel {
    public:
    double operator() (const scythe::Matrix<double> beta){
        const int n = y_.rows();
        const int p = X_.cols();

        scythe::Matrix<double> eta = X_ * beta + offset_;
        scythe::Matrix<double> m = exp(eta);
        double loglike = 0.0;
        for (int i=0; i<n; ++i)
        loglike += y_(i) * log(m(i)) - m(i);
        return -1.0 * loglike;
    }
    scythe::Matrix<double> y_;
    scythe::Matrix<double> X_;
    scythe::Matrix<double> offset_;
};

int main(int argc, char* argv[])
{
    if (argc < 8)
    {
        usage();
        exit(1);
    }

    //string empDisFile = argv[1];
    string ictmFile = argv[1];
    string dsmFile = argv[2];
    string difFile = argv[3];
    string chrNum = argv[4];
    //double MORANI = atof(argv[6]);
    double corrThres = 0.0;
    string disCoefFile = argv[5];
    double mu_eff = atof(argv[6]);
    double beta_eff = atof(argv[7]);

    map<int, int> empDis_map;
    map<int, int> ictm_map;
    vector< vector<int> > dsm_map;
    vector< vector< vector<int> > > dif_map;
    map<int, double> disPolyCoef;

    //reader_ictm(empDisFile, empDis_map);
    //cout<<"\nReading empDis file done."<<endl;
    reader_ictm(ictmFile, ictm_map);
    cout<<"\nRead ictm file done."<<endl;
    reader_dsm(dsmFile, dsm_map);
    cout<<"\nRead dsm file done."<<endl;
    reader_dif(difFile, dif_map);
    cout<<"\nRead dif file done."<<endl;
    reader_coef(disCoefFile, disPolyCoef);
    cout<<"\nRead distance polynomial fit coefficients done."<<endl;

    map< int, map< pair<int, int>, double > > pVal_map;

    findIntraDomainInteraction(empDis_map, chrNum, dsm_map, dif_map, ictm_map, disPolyCoef, pVal_map, corrThres, mu_eff, beta_eff);

    return 0;
}

int reader_dif(const string& fileToread, vector< vector< vector<int> > >& dif_local)
{
    ifstream inputFile(fileToread.c_str());
    if (!inputFile)
    {
        //char a;
        //cin >>a;
        cout <<"\n"<< "Error opening " << fileToread << "." << endl;
        exit(1);
    }

    char lineStr[READBLOCK];
    char freq_str[15];
    int freq_int = 0;
    int domainNum = 0;

    int column = 0;

    char* it_lineStr;
    char* it_token;

    int peakNum = 0;

    //copy data to a map, which chrNo is the key, the value is a vector of ints(read's starting point)
    vector< vector<int> > tempDomn;
    while(inputFile.getline(lineStr,READBLOCK))
    {
        it_lineStr = lineStr;

        column = 0;

        if (lineStr != NULL)
        {
            vector<int> tempFreq;

            //pass head white space
            if(*it_lineStr == 'd')
            {
                if (tempDomn.begin() != tempDomn.end())
                {
                    ++domainNum;
                    dif_local.push_back(tempDomn);
                    tempDomn.clear();
                }
                continue;
            }

            while(*it_lineStr != '\0' && *it_lineStr != '\n')
            {
                if (*it_lineStr == ' ' || *it_lineStr == '\t')
                {
                    //remove white space
                    while(*it_lineStr == ' ' || *it_lineStr == '\t')
                    {
                        ++it_lineStr;
                    }

                    ++column;
                }
                else
                {
                    it_token = freq_str;
                    while(*it_lineStr != ' ' && *it_lineStr != '\t' && *it_lineStr != '\0' && *it_lineStr != '\n')
                    {
                        *it_token = *it_lineStr;
                        ++it_token;
                        ++it_lineStr;
                    }
                    *it_token = '\0';
                    freq_int = atoi(freq_str);
                    tempFreq.push_back(freq_int);
                }
            }

            tempDomn.push_back(tempFreq);
            ++peakNum;
        }
    }

    if(tempDomn.begin() != tempDomn.end())
    {
        dif_local.push_back(tempDomn);
    }

    inputFile.close();

    return peakNum;
}

int reader_dsm(const string& fileToread, vector< vector<int> >& dsm_local)
{
    ifstream inputFile(fileToread.c_str());
    if (!inputFile)
    {
        //char a;
        //cin >>a;
        cout <<"\n"<< "Error opening " << fileToread << "." << endl;
        exit(1);
    }

    char lineStr[READBLOCK];
    char pos_str[15];
    int pos_int = 0;

    int column = 0;

    char* it_lineStr;
    char* it_token;

    int peakNum = 0;

    //copy data to a map, which chrNo is the key, the value is a vector of ints(read's starting point)
    //ofstream testBadFile("test");
    while(inputFile.getline(lineStr,READBLOCK))
    {
        it_lineStr = lineStr;

        column = 0;

        if (lineStr != NULL)
        {
            vector<int> tempPos;
            //testBadFile<<lineStr<<endl;

            //pass head white space
            while(*it_lineStr == ' '|| *it_lineStr == '\t')
            {
                ++it_lineStr;
            }

            while(*it_lineStr != '\0' && *it_lineStr != '\n')
            {
                if (*it_lineStr == ' ' || *it_lineStr == '\t')
                {
                    //remove white space
                    while(*it_lineStr == ' ' || *it_lineStr == '\t')
                    {
                        ++it_lineStr;
                    }

                    ++column;
                }
                else
                {
                    it_token = pos_str;
                    while(*it_lineStr != ' ' && *it_lineStr != '\t' && *it_lineStr != '\0' && *it_lineStr != '\n')
                    {
                        *it_token = *it_lineStr;
                        ++it_token;
                        ++it_lineStr;
                    }
                    *it_token = '\0';
                    pos_int = atoi(pos_str);
                    tempPos.push_back(pos_int);
                }
            }

            dsm_local.push_back(tempPos);
            ++peakNum;
        }
    }

    inputFile.close();

    return peakNum;
}

int reader_coef(const string& fileToread, map<int, double>& disPolyCoef_local)
{
    ifstream inputFile(fileToread.c_str());
    if (!inputFile)
    {
        cout <<"\n"<< "Error opening " << fileToread << "." << endl;
        exit(1);
    }

    char degree[15];
    int degree_int = 0;
    char coef[50];
    double coef_dbl = 0.0;

    char* it_lineStr;
    char* it_token;

    char lineStr[512];
    while(inputFile.getline(lineStr,512))
    {
        if (lineStr != NULL)
        {
            it_lineStr = lineStr;

            it_token = degree;
            while(*it_lineStr != '\t' && *it_lineStr != ' ' && *it_lineStr != '\n' && *it_lineStr != '\0')
            {
                *it_token = *it_lineStr;
                ++it_token;
                ++it_lineStr;
            }
            *it_token = '\0';
            degree_int = atoi(degree);
	    ++it_lineStr;
	    while(*it_lineStr == ' ' || *it_lineStr == '\t')
	    {
		++it_lineStr;
	    }

            it_token = coef;
            while(*it_lineStr != '\t' && *it_lineStr != ' ' && *it_lineStr != '\n' && *it_lineStr != '\0')
            {
                *it_token = *it_lineStr;
                ++it_token;
                ++it_lineStr;
            }
            *it_token = '\0';
            coef_dbl = atof(coef);

            disPolyCoef_local[degree_int] = coef_dbl;
        }
    }
    return 0;
}

void reader_ictm(const string& fileToread, map<int, int>& ictm_local)
{
    ifstream inputFile(fileToread.c_str());
    if (!inputFile)
    {
        cout <<"\n"<< "Error opening " << fileToread << "." << endl;
        exit(1);
    }

    char pos[15];
    int pos_int = 0;
    char freq[15];
    int freq_int = 0;

    char* it_lineStr;
    char* it_token;

    char lineStr[512];
    while(inputFile.getline(lineStr,512))
    {
        if (lineStr != NULL)
        {
            it_lineStr = lineStr;

            it_token = pos;
            while(*it_lineStr != '\t')
            {
                *it_token = *it_lineStr;
                ++it_token;
                ++it_lineStr;
            }
            *it_token = '\0';
            ++it_lineStr;
            pos_int = atoi(pos);

            it_token = freq;
            while(*it_lineStr != '\t' && *it_lineStr != '\n' && *it_lineStr != '\0')
            {
                *it_token = *it_lineStr;
                ++it_token;
                ++it_lineStr;
            }
            *it_token = '\0';
            freq_int = atoi(freq);

            ictm_local[pos_int] = freq_int;
        }
    }
}

int findIntraDomainInteraction(map<int, int>& empDisMap_fun, string jobChr_fun, vector< vector<int> >& domainSitesMap_fun, vector< vector< vector<int> > >& domainCSinterFreq_fun, map<int, int>& csInterChromTotalMap_fun, map<int, double>& disPolyCoef_local, map< int, map< pair<int, int>, double > >& pVal_fun, double corrThres_fun, double mu_eff_fun, double beta_eff_fun)
{
    string file_debug =  jobChr_fun + "_debug";
    ofstream outDebug(file_debug.c_str());

    int domainNum = domainSitesMap_fun.size();

    //#pragma omp parallel for
    for (int domainIt = 0; domainIt < domainNum; ++domainIt)
    {
	//if(domainIt==1)
	//{exit(0);}
        map< pair<int, int>, double>& domainPval = pVal_fun[domainIt];
        outDebug<<"domain "<<domainIt<<endl;
        vector<int>& sitesMap = domainSitesMap_fun[domainIt];
        vector< vector<int> >& csInterFreqMap = domainCSinterFreq_fun[domainIt];
        int siteNum = sitesMap.size();

        int size_convert = convtIndex(siteNum-2,siteNum-1,siteNum) + 1;//the convtIndex function's first two input is 0 based and the output is 0 based too
	vector<double> freq_convert;
        vector<double> offset;//distance effect
        vector<double> effVec;//efficiency of cutting site and mappability, measured by the total inter chromosome hybrid frags

	ostringstream domainIt_str;
	domainIt_str<<domainIt;
	string domainFile = "domainData_" + domainIt_str.str();
	//ofstream outDomainData(domainFile);

        for (int rowIt=0; rowIt<siteNum; ++rowIt)
        {
            for (int colIt=rowIt+1; colIt<siteNum; ++colIt)
            {
                int index_convert=convtIndex(rowIt,colIt,siteNum);

		//if(abs(sitesMap[rowIt]-sitesMap[colIt])>OPTDIST)//the index of freq_convert need to be changed because of this
		{
		    //log(y/ydist)=mu+betaeff*effvec
		    freq_convert.push_back(csInterFreqMap[rowIt][colIt]);
		    offset.push_back(calBgFrags(disPolyCoef_local, abs(sitesMap[rowIt]-sitesMap[colIt])));
		    //the digestion and ligation efficient of a cutting site should take log because, its log should proportional to log(freq)
		    effVec.push_back(log(sqrt(csInterChromTotalMap_fun[sitesMap[rowIt]])*sqrt(csInterChromTotalMap_fun[sitesMap[colIt]])+1));
		    //outDomainData<<rowIt<<"\t"<<colIt<<"\t"<<csInterFreqMap[rowIt][colIt]<<"\t"<<csInterChromTotalMap_fun[sitesMap[rowIt]]<<"\t"<<csInterChromTotalMap_fun[sitesMap[colIt]]<<"\t"<<log(sqrt(csInterChromTotalMap_fun[sitesMap[rowIt]])*sqrt(csInterChromTotalMap_fun[sitesMap[colIt]])+1)<<"\t"<<sitesMap[rowIt]<<"\t"<<sitesMap[colIt]<<"\t"<<calBgFrags(disPolyCoef_local, abs(sitesMap[rowIt]-sitesMap[colIt])+OPTDIST)<<endl;
		}
            }
        }
	//outDomainData.close();

	vector<double> constTerms(size_convert, 0.0);
        vector<double> effConsts(size_convert, 0.0);//efficiency constand mu+beta_eff_fun*effVec[i]
	for (int i = 0; i < size_convert; ++i)
	{
	    //constTerms[i] = mu+dist_ij[i]*alpha+effVec[i]*beta;
	    effConsts[i] = mu_eff_fun+effVec[i]*beta_eff_fun;
	    constTerms[i] = exp(effConsts[i]+offset[i]);
	    //constTerms[i] = mu+effVec[i]*beta+offset[i];
	}

        int counter_fit=0;
        int counter_skip=0;

	int regionIt=0;
	int testIt=0;

	string regionFile = "regionData_" + domainIt_str.str();
	ofstream outRegionData(regionFile);
	bool flag_test = false;

	string distMatrixFile = "distMatrix_" + domainIt_str.str();
	ofstream outDistMatrix(distMatrixFile);

        //#pragma omp parallel for
        for (int rowIt=0; rowIt<siteNum; ++rowIt)
        {
            for (int colIt=rowIt+1; colIt<siteNum; ++colIt)
            {
                if(csInterFreqMap[rowIt][colIt]>TESTFREQTHRES && abs(sitesMap[rowIt]-sitesMap[colIt])>=20000)
                {
                    vector<double> neighbFreq;
                    vector<double> neighbConstTerms;
                    vector<double> fullExp;

                    int sideLen = 2*NEIGHBDIS + 1;
                    int colNum_mat = 0;
                    int rowNum_mat = 0;

		    //ostringstream regionIt_str;
		    //regionIt_str<<regionIt;
		    //string regionFile = "regionData_" + domainIt_str.str() + "_" + regionIt_str.str();
		    //ofstream outRegionData(regionFile);
                    vector< pair<int, int> > oriCoor;
                    for (int neighbRowIt=(rowIt-NEIGHBDIS), squareRowIt = 0; squareRowIt < sideLen; ++neighbRowIt, ++squareRowIt)
                    {
                        for (int neighbColIt=colIt-NEIGHBDIS, squareColIt = 0; squareColIt < sideLen; ++neighbColIt, ++squareColIt)
                        {
                            int distMatrix_colIt=convtIndex(neighbRowIt, neighbColIt, siteNum);
                            //if (distMatrix_colIt==-1 || neighbColIt<=neighbRowIt || colIt<=neighbRowIt || neighbColIt<=rowIt || (neighbRowIt==rowIt && neighbColIt==colIt) )
                            if (distMatrix_colIt==-1 || neighbColIt<=neighbRowIt || colIt<=neighbRowIt || neighbColIt<=rowIt)
                            {continue;}

                            //for future get back the original coordinates
                            if(freq_convert[distMatrix_colIt] > TESTFREQTHRES)
                            {
                                oriCoor.push_back(make_pair(neighbRowIt, neighbColIt));
                                ++colNum_mat;
                            }

                            neighbFreq.push_back(freq_convert[distMatrix_colIt]-constTerms[distMatrix_colIt]);
			    //outRegionData<<neighbRowIt<<"\t"<<neighbColIt<<"\t"<<distMatrix_colIt<<"\t"<<freq_convert[distMatrix_colIt]<<"\t"<<constTerms[distMatrix_colIt]<<"\t"<<mu_eff_fun<<"\t"<<beta_eff_fun<<"\t"<<effVec[distMatrix_colIt]<<"\t"<<sitesMap[rowIt]<<"\t"<<sitesMap[neighbRowIt]<<"\t"<<sitesMap[colIt]<<"\t"<<sitesMap[neighbColIt]<<"\t"<<mu_eff_fun<<"\t"<<beta_eff_fun<<"\t"<<calBgFrags(disPolyCoef_local, abs(sitesMap[rowIt]-sitesMap[neighbRowIt])+abs(sitesMap[colIt]-sitesMap[neighbColIt])+OPTDIST)<<endl;
                            //neighbConstTerms.push_back(constTerms[distMatrix_colIt]);
			    //
                            fullExp.push_back(exp(effConsts[distMatrix_colIt]+calBgFrags(disPolyCoef_local, abs(sitesMap[rowIt]-sitesMap[neighbRowIt])+abs(sitesMap[colIt]-sitesMap[neighbColIt])+OPTDIST)));
                            ++rowNum_mat;
                        }
                    }
		    //exit(0);

                    //int neighbFreq_size = neighbFreq.size();
		    arma::vec neighbFreq_lm(neighbFreq);

		    int fullExpMat_size = fullExp.size();

		    arma::mat fullExpMat_lm(1,fullExpMat_size);

		    for(int i=0; i<fullExpMat_size; ++i)
		    {
			//fullExpMat_lm(i,0)=1;
			fullExpMat_lm(0,i)=fullExp[i];
		    }

		    mlpack::regression::LinearRegression lm(fullExpMat_lm, neighbFreq_lm,0.0,true);
		    //cout<<"construct region model done."<<endl;

		    //set start values for theta
		    arma::vec& beta_lm = lm.Parameters();
		    //cout << "The region MLEs are: " << endl;
		    //std::cout <<"regionMLEs: "<<beta_lm(0)<<"\t"<<beta_lm(1)<<endl;

		    if(beta_lm(1)>0.01)
		    {
			//arma::vec temp = neighbFreq_lm - arma::trans( (arma::trans(beta_lm.subvec(1, beta_lm.n_elem - 1)) * fullExpMat_lm) + beta_lm(0));
			arma::vec temp = neighbFreq_lm - arma::trans((beta_lm(1) * fullExpMat_lm) + beta_lm(0));
			double sigmaSq = arma::dot(temp, temp) / (fullExpMat_size-2);//denominator: n-number of betas(parameters)

			//double var_err = lm.ComputeError(fullExpMat_lm, neighbFreq_lm);
			//cout<<"sigma squared: "<<sigmaSq<<endl;

			double sumX=0.0;
			double sumXsq=0.0;
			for(int i=0; i<fullExpMat_size; ++i)
			{
			    sumX+=fullExp[i];
			    sumXsq+=fullExp[i]*fullExp[i];
			}

			double beta_stderr = sqrt(fullExpMat_size*sigmaSq/(fullExpMat_size*sumXsq-sumX*sumX));
			//cout<<"beta standard error: "<<beta_stderr<<endl;
			double fitPval=2 * gsl_cdf_gaussian_P(-fabs(beta_lm(1)/beta_stderr), 1.0);
			std::cout <<"regionMLEs: "<<beta_lm(0)<<"\t"<<beta_lm(1)<<"\t"<<fitPval<<"\t"<<colNum_mat<<endl;

			if (fitPval < PCUTOFF && colNum_mat > 1)
			{
			    int distNum = colNum_mat;
			    vector<double> distVec_lasso(colNum_mat, 0.0);
			    vector< vector<double> > distMatrix_lasso;
			    for (int i=0; i<rowNum_mat; ++i)
			    {
				distMatrix_lasso.push_back(distVec_lasso);
			    }

			    int testIndex = 0;
			    int matColIndex = 0;
			    for (int neighbRowIt=(rowIt-NEIGHBDIS), squareRowIt = 0; squareRowIt < sideLen; ++neighbRowIt, ++squareRowIt)
			    {
				for (int neighbColIt=colIt-NEIGHBDIS, squareColIt = 0; squareColIt < sideLen; ++neighbColIt, ++squareColIt)
				{
				    int distMatrix_colIt=convtIndex(neighbRowIt, neighbColIt, siteNum);
				    //if (distMatrix_colIt==-1 || neighbColIt<=neighbRowIt || colIt<=neighbRowIt || neighbColIt<=rowIt || (neighbRowIt==rowIt && neighbColIt==colIt) )
				    if (distMatrix_colIt==-1 || neighbColIt<=neighbRowIt || colIt<=neighbRowIt || neighbColIt<=rowIt || freq_convert[distMatrix_colIt] <= TESTFREQTHRES)
				    //if (distMatrix_colIt==-1 || neighbColIt<=neighbRowIt || colIt<=neighbRowIt || neighbColIt<=rowIt || freq_convert[distMatrix_colIt] <= TESTFREQTHRES || abs(sitesMap[neighbRowIt]-sitesMap[neighbColIt])<20000)
				    {continue;}

				    int matRowIndex = 0;//inside loop is actually the row index for the final matrix
				    for (int neighbNeighbRowIt=(rowIt-NEIGHBDIS), squareNeighbRowIt = 0; squareNeighbRowIt < sideLen; ++neighbNeighbRowIt, ++squareNeighbRowIt)
				    {
					for (int neighbNeighbColIt=colIt-NEIGHBDIS, squareNeighbColIt = 0; squareNeighbColIt < sideLen; ++neighbNeighbColIt, ++squareNeighbColIt)
					{
					    int distMatrix_rowIt=convtIndex(neighbNeighbRowIt, neighbNeighbColIt, siteNum);
					    if (distMatrix_rowIt==-1 || neighbNeighbColIt<=neighbNeighbRowIt || colIt<=neighbNeighbRowIt || neighbNeighbColIt<=rowIt)
					    {continue;}
					    //distMatrix_lasso[matRowIndex][matColIndex]=exp(-MORANI*(abs(sitesMap[neighbNeighbRowIt]-sitesMap[neighbRowIt])+abs(sitesMap[neighbNeighbColIt]-sitesMap[neighbColIt])));
					    distMatrix_lasso[matRowIndex][matColIndex]=exp(effConsts[distMatrix_rowIt]+calBgFrags(disPolyCoef_local, abs(sitesMap[neighbNeighbRowIt]-sitesMap[neighbRowIt])+abs(sitesMap[neighbNeighbColIt]-sitesMap[neighbColIt])+OPTDIST));
					    ++matRowIndex;
					}
				    }
				    if (neighbRowIt==rowIt && neighbColIt==colIt)
				    {
					testIndex=matColIndex;
				    }
				    ++matColIndex;
				    //distMatrix[make_pair(distMatrix_rowIt, distMatrix_colIt)]=exp(-MORANI*(abs(sitesMap[rowIt]-sitesMap[neighbRowIt])+abs(sitesMap[colIt]-sitesMap[neighbColIt])));
				}
			    }

			    int testFlag = 1;
			    bool flag_rmDep = false;

			    if (testFlag == 1)
			    {
				flag_test=true;
				//#pragma omp critical
				{
				//cout<<rowIt<<"\t"<<colIt<<"\ttest column number: "<<testIndex<<endl;

				    const int rowNum_r = distMatrix_lasso.size();
				    const int colNum_r = (distMatrix_lasso.begin())->size();
				    //cout<<rowNum_r<<"\t"<<colNum_r<<endl;
			//!!!!!!!!!!! if remove dependents is true, this need to change
			//
				    arma::mat fullExpMat_lars(colNum_r,rowNum_r);

				    //ostringstream testIt_str;
				    //testIt_str<<testIt;
				    //string distMatrixFile = "distMatrix_" + domainIt_str.str() + "_" + regionIt_str.str() + "_" + testIt_str.str();

				    //ofstream outDistMatrix(distMatrixFile);
				    outDistMatrix<<domainIt<<"\t"<<regionIt<<"\t"<<testIt<<"\n";
				    for(int i=0; i<colNum_r; ++i)
				    {
					int j=0;
					fullExpMat_lars(i,j)=distMatrix_lasso[j][i];
					outDistMatrix<<distMatrix_lasso[j][i];
					for(j=1; j<rowNum_r; ++j)
					{
					    fullExpMat_lars(i,j)=distMatrix_lasso[j][i];
					    outDistMatrix<<"\t"<<distMatrix_lasso[j][i];
					}
					outDistMatrix<<"\n";
				    }

				    outRegionData<<domainIt<<"\t"<<regionIt<<"\n";
				    vector<double>::const_iterator neighbFreqIt = neighbFreq.begin(), neighbFreqIt_end = neighbFreq.end();
				    outRegionData<<*neighbFreqIt;
				    for(++neighbFreqIt; neighbFreqIt != neighbFreqIt_end; ++neighbFreqIt)
				    {
					outRegionData<<"\t"<<*neighbFreqIt;
				    }
				    outRegionData<<"\n";

				    regionIt++;
				    testIt++;

				    distMatrix_lasso.clear();
				    ++counter_fit;
				}
				//#pragma omp end critical

				//if (fitPval < PCUTOFF)
				//{
				//
				//}
			    }
			}
			else if(colNum_mat==1)
			{
			    //only 1 box has signal (freq>1), no need to use lasso to choose which one is real,or maybe could remove the ones due to the const terms, however glm should have taken care of that already
			}
		    }
                }
                else
                {
                    //#pragma omp critical
                    {++counter_skip;}
                    //#pragma omp end critical
                }
            }
        }
	outRegionData.close();
	outDistMatrix.close();
    }
    outDebug.close();

    return 0;
}

int rmDependentCol(vector< vector<double> >& distMatrix_local, vector<double>& freqVec_local, vector< pair<int, int> >& oriCoor_local, double thres_local, int& testIndex_local, vector< vector<double> >& distMatRmDep_local)
{
    int rowNum_fun = distMatrix_local.size();
    if (rowNum_fun != freqVec_local.size())
    {
        cerr<<"Error: row number of the dist matrix is different from the size of the frequency vector."<<endl;
        exit(1);
    }
    int colNum_fun = (distMatrix_local.begin())->size();
    vector< vector<double> > distMatrix_std;
    for (int i = 0; i < colNum_fun; ++i)
    {
        vector<double> tempVec;
        for (int j = 0; j < rowNum_fun; ++j)
        {
            tempVec.push_back(distMatrix_local[j][i]);
        }
        distMatrix_std.push_back(tempVec);
    }
    distMatrix_std.push_back(freqVec_local);
    calStanScores(distMatrix_std);

    map< pair<int, int>, double > corrMap;
    map<int, int> rmIndex;
    int testFlag_local = calPearsonCorr(distMatrix_std, oriCoor_local, thres_local, testIndex_local, rmIndex);
    cout<<"testFlag: "<<testFlag_local<<endl;

    int oriTestIndex = testIndex_local;
    for (int m = 0; m < oriTestIndex; ++m)
    {
        if (rmIndex[m]==1)
        {
            testIndex_local--;
        }
    }

    for (int i = 0; i < rowNum_fun; ++i)
    {
        vector<double> tempVec;
        for (int j = 0; j < colNum_fun; ++j)
        {
            if (rmIndex[j]!=1)
            {
                tempVec.push_back(distMatrix_local[i][j]);
            }
        }
        distMatRmDep_local.push_back(tempVec);
    }

    return testFlag_local;
}

int calStanScores(vector< vector<double> >& distMatrix_std_fun)
{
    for (vector< vector<double> >::iterator colIt = distMatrix_std_fun.begin(), colIt_end = distMatrix_std_fun.end(); colIt != colIt_end; ++colIt) //colomn as in original matrix before transpose
    {
        int rowNum_fun = colIt->size();
        double sum = 0.0;
        for (vector<double>::const_iterator rowIt = colIt->begin(), rowIt_end = colIt->end(); rowIt != rowIt_end; ++rowIt)
        {
            sum += *rowIt;
        }
        double mean = sum / rowNum_fun;

        double var = 0.0;
        for (vector<double>::const_iterator rowIt = colIt->begin(), rowIt_end = colIt->end(); rowIt != rowIt_end; ++rowIt)
        {
            double diff = *rowIt - mean;
            var += diff * diff;
        }
        var = var / (rowNum_fun - 1);
        double std = sqrt(var);

        for (vector<double>::iterator rowIt = colIt->begin(), rowIt_end = colIt->end(); rowIt != rowIt_end; ++rowIt)
        {
            *rowIt = (*rowIt - mean) / std;
        }
    }

    return 0;
}

int calPearsonCorr(vector< vector<double> >& distMatrix_std_fun, vector< pair<int, int> >& oriCoor_fun, const double& thres_fun, int testIndex_fun, map<int, int>& rmIndex_fun)
{
    return 1;
}

double calBgFrags(map<int, double>& coef_fun, int dist_fun)
{
    double logDist=log(dist_fun);
    double bgFrags=0.0;
    map<int, double>::const_iterator coefIt=coef_fun.begin(), coefIt_end=coef_fun.end();
    for( ; coefIt != coefIt_end; ++coefIt)
    {
	bgFrags+=pow(logDist, coefIt->first)*coefIt->second;
    }
    return bgFrags;
}

int writer_corrFile(map< pair<int, int>, double >& corrMap_local, string& fileName)
{
    ofstream outputFile(fileName.c_str());

    for (map< pair<int, int>, double>::const_iterator corrIt = corrMap_local.begin(), corrIt_end = corrMap_local.end(); corrIt != corrIt_end; ++corrIt)
    {
        outputFile<<corrIt->first.first<<"\t"<<corrIt->first.second<<"\t"<<corrIt->second<<endl;
    }
    outputFile.close();

    return 0;
}

/*
nnlasso.normal<-function(x,y,lambda=NULL,intercept=TRUE,normalize=TRUE,tau=1,tol=1e-6,maxiter=1e5,nstep=100,min.lambda=1e-4,eps=1e-6,path=TRUE,SE=FALSE)
{
        np=dim(x)
        n=np[1]
        p=np[2]
        if (intercept)
        {
                meanx = colMeans(x)
                x = scale(x, meanx, FALSE)
                meany = sum(y)/n
                y = y - meany
        } else {
                meanx = rep(0, p)
                meany = 0
                }
        if (normalize)
        {
                normx = sqrt(colSums(x^2))
                x = scale(x, FALSE, normx)
        } else normx = rep(1, p)
        tx=t(x)
        xpy=tx%*%y
        max.lambda=max(abs(xpy))
        if (path)
        {
                stepsize=exp((log(min.lambda)-log(max.lambda))/nstep)
                lambdas=max.lambda*stepsize^((1:nstep)-1)
        } else {
                        nstep=2
                        lambdas=c(max.lambda,lambda)
                }
        coef=matrix(0,nstep,p)
        of.value=rep(0,nstep)
        coef[1,]=1e-2
        xbeta.old=x%*%coef[1,]
        if (n>p)
        {
                xpx=tx%*%x
                xpxbetaold=xpx%*%coef[1,]
        } else {
                        xpx=NULL
                        xpxbetaold=tx%*%xbeta.old
                }
        of.value[1]=-sum((y-xbeta.old)^2)/2-max.lambda*(tau*sum(coef[1,])+(1-tau)*sum(coef[1,]^2))
        lambda.iter=rep(0,nstep)
        g1=xpy
        for(iter in 2:nstep)
        {
                kkt=FALSE
                while(kkt==FALSE)
                {
                        res=nnlasso.normal.lambda(n,p,x,y,xpx,xpy,beta.old=coef[iter-1,],tau,lambda1=lambdas[iter],tol,maxiter,xbeta.old,eps,SE)
                        if (res$conv=="yes")
                        {
                                coef[iter,]=res$beta.new
                                xbeta=res$xbeta.new
                                if (n>p)
                                {
                                        xpxbetaold=xpx%*%res$beta.new
                                } else xpxbetaold=tx%*%xbeta
                                g1=xpy-xpxbetaold-lambdas[iter]*2*(1-tau)*coef[iter,]
                                indices=NULL
                                if (length(indices)==0)
                                {
                                        xbeta.old=res$xbeta.new
                                        lambda.iter[iter]=res$iter
                                        of.value[iter]=res$ofv.new
                                        kkt=TRUE
                                }
                        } else stop("The algorithm did not converge")
                }
        }
        coef=scale(coef,center=FALSE,scale=normx)
        if(SE)
        {
                vcov=res$vcov
                vcov=vcov/normx
                vcov=t(vcov)/normx
                se=sqrt(diag(vcov))
                if(intercept)
                {
                        se0=sqrt(t(meanx)%*%vcov%*%meanx)
                        se=c(se0,se)
                }
        } else se=NULL
        if (intercept) beta0=rep(meany,nstep)-coef%*%meanx else beta0=rep(0,nstep)
        L1norm=rowSums(abs(coef))
        norm.frac=L1norm/max(L1norm)
        obj=list(beta0=beta0,coef=coef,lambdas=lambdas,L1norm=L1norm,norm.frac=norm.frac,lambda.iter=lambda.iter,of.value=of.value,normx=normx,se=se)
        class(obj)='nnlasso'
        return(obj)
}

nnlasso.normal.lambda<-function(n,p,x,y,xpx,xpy,beta.old,tau,lambda1,tol,maxiter,xbeta.old,eps,SE=FALSE)
{
        epp=0.001
        if (n<=p) tx=t(x)
        ofv.old=-sum((y-xbeta.old)^2)/2-lambda1*(tau*sum(beta.old)+(1-tau)*sum(beta.old^2))
        for (iter in 1:maxiter)
        {
                if (n>p)
                {
                        xpxbetaold=xpx%*%beta.old
                } else xpxbetaold=tx%*%xbeta.old
                g1minus<-g1<-xpy-xpxbetaold
                g1minus[which(g1>0)]=0
                b=beta.old*(g1-lambda1*tau-2*lambda1*(1-tau)*beta.old)/(lambda1*tau+2*lambda1*(1-tau)*beta.old-g1minus+1e-16)
                beta.new=beta.old+b
                xb=x%*%b
                xbeta.new=xbeta.old+xb
                ofv.new=-sum((y-xbeta.new)^2)/2-lambda1*(tau*sum(beta.new)+(1-tau)*sum(beta.new^2))
                delta=1
                t1=epp*(sum(g1*b))
                while (ofv.new-delta*t1<ofv.old & delta>1e-5)
                {
                        delta=delta/2
                        beta.new=beta.old+delta*b
                        xbeta.new=xbeta.old+delta*xb
                        ofv.new=-sum((y-xbeta.new)^2)/2-lambda1*(tau*sum(beta.new)+(1-tau)*sum(beta.new^2))
                }
                if (ofv.new-delta*t1<ofv.old & delta<=1e-5)
                {
                        beta.new=beta.old
                        ofv.new=ofv.old
                        xbeta.new=xbeta.old
                        break
                }
                if(abs(ofv.old-ofv.new)<=tol) break
                beta.old=beta.new
                xbeta.old=xbeta.new
                ofv.old=ofv.new
        }
        if (iter<maxiter) conv="yes" else conv="no"
        if (SE==TRUE & conv=="yes")
        {
                grad=xpy-t(x)%*%xbeta.new-lambda1*tau-2*lambda1*(1-tau)*beta.new
                Finv<-F<-xpx+2*lambda1*(1-tau)
                index=which(beta.new<=eps & grad< -1e-2)
                if (length(index)>0)
                {
                        temp=F[-index,-index]
                        tempinv=solve(temp)
                        indexc=setdiff(1:p,index)
                        if (length(indexc)>0) Finv[indexc,indexc]=tempinv
                        Finv[index,index]=0
                } else Finv=solve(F)
                vc=Finv%*%F
                vcov=vc%*%t(Finv)
        } else vcov=NULL
        res=list(beta.new=beta.new,conv=conv,iter=iter,ofv.new=ofv.new,xbeta.new=xbeta.new,vcov=vcov)
        return(res)
}
*/

#endif


#include "ModuleSetList.h"

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

using namespace boost::lambda;

namespace hydla {
namespace hierarchy {

ModuleSetList::ModuleSetList()
{}

ModuleSetList::ModuleSetList(ModuleSet m) :
  ModuleSetContainer(m)
{}

ModuleSetList::~ModuleSetList()
{}

void ModuleSetList::add_parallel(ModuleSetList& parallel_module_set_list)
{

  // parallel(X, Y) = X ∪ Y ∪ {x ∪ y | x∈X, y∈Y}
  // Y
  module_set_set_t new_list(full_module_set_set_);

  // X
  for (auto p_it : parallel_module_set_list.full_module_set_set_)
  {
    new_list.insert(p_it);
  }

  // {x ∪ y | x∈X, y∈Y}
  for (auto p_it : parallel_module_set_list.full_module_set_set_)
  {
    for (auto this_it : full_module_set_set_)
    {
      ModuleSet ms(this_it, p_it);
      new_list.insert(ms);
    }
  }

  full_module_set_set_.swap(new_list);
  maximal_module_set_ = *full_module_set_set_.rbegin();
}

void ModuleSetList::add_required_parallel(ModuleSetList& parallel_module_set_list)
{
  // 空のモジュール集合の集合を用意
  module_set_set_t new_list;

  // {x ∪ y | x∈X, y∈Y}
  for (auto p_it : parallel_module_set_list.full_module_set_set_)
  {
    for (auto this_it : full_module_set_set_)
    {
      ModuleSet ms(this_it, p_it);
      new_list.insert(ms);
    }
  }

  full_module_set_set_.swap(new_list);
  maximal_module_set_ = *full_module_set_set_.rbegin();
}

void ModuleSetList::add_weak(ModuleSetList& weak_module_set_list)
{
  // ordered(X, Y) = Y ∪ {x ∪ y | x∈X, y∈Y}

  // Y
  module_set_set_t new_list(full_module_set_set_);

  ModuleSet y = *full_module_set_set_.rbegin();
  // {x ∪ y | x∈X, y∈Y}
  for (auto p_it : weak_module_set_list.full_module_set_set_)
  {
    ModuleSet ms(y, p_it);
    new_list.insert(ms);
  }

  full_module_set_set_.swap(new_list);
  maximal_module_set_ = *full_module_set_set_.rbegin();
}

void ModuleSetList::remove_included_ms_by_current_ms()
{
  ms_to_visit_.clear();
}

std::ostream& ModuleSetList::dump(std::ostream& s) const
{
  dump_node_names(s);
  s << "\n";
  dump_node_trees(s);

  return s;
}

std::ostream& ModuleSetList::dump_node_names(std::ostream& s) const
{
  module_set_set_t::const_iterator it  = full_module_set_set_.begin();
  module_set_set_t::const_iterator end = full_module_set_set_.end();

  s << "{";
  if (it != end) s << (*(it++)).get_name();
  while (it != end)
  {
    s << ", " << (*(it++)).get_name();
  }
  s << "}";

  return s;
}

std::ostream& ModuleSetList::dump_node_trees(std::ostream& s) const
{
  module_set_set_t::const_iterator it  = full_module_set_set_.begin();
  module_set_set_t::const_iterator end = full_module_set_set_.end();

  s << "{";
  if (it != end) s << *(it++);
  while (it != end)
  {
    s << ", " << *(it++);
  }
  s << "}";

  return s;
}

} // namespace hierarchy
} // namespace hydla


#include <Eigen/StdVector>
#include <kdtreepp.hpp>
#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>

using Vector2 = Eigen::Vector2d;
using Vector3 = Eigen::Vector3d;
using AlignedBox2 = Eigen::AlignedBox2d;
using AlignedBox3 = Eigen::AlignedBox3d;

static void CHECK_VEC3_EQ(const Vector3& a, const Vector3& b) {
  CHECK(a[0] == Approx(b[0]));
  CHECK(a[1] == Approx(b[1]));
  CHECK(a[2] == Approx(b[2]));
}

TEST_CASE("VERSION_STRING") { REQUIRE(KDTREEPP_VERSION_STRING == std::string("1.0.0")); }

TEST_CASE("PointTreeTest") {
  // Build a k-d tree from a list of points

  std::vector<Vector3, Eigen::aligned_allocator<Vector3>> points;
  std::mt19937_64 randGen{size_t(42)};
  std::uniform_real_distribution<double> dist{-1000.0, 1000.0};

  // Make random points
  constexpr size_t count = 1500;
  points.resize(count);
  for (auto& point : points) {
    point << dist(randGen), dist(randGen), dist(randGen);
  }

  const auto node = kdtreepp::MakeEigenKdTreeNode<double, 3>(
      points.begin(), points.end(), [](const Vector3& p) { return p; },
      [](const Vector3& p) { return p; });

  const Vector3 checkPoint{dist(randGen), dist(randGen), dist(randGen)};

  // Find closest point via brute force search
  double bruteMinDistSq = std::numeric_limits<double>::max();
  Vector3 bruteClosestPoint;
  for (const auto& point : points) {
    const double rSq = (point - checkPoint).squaredNorm();
    if (rSq < bruteMinDistSq) {
      bruteMinDistSq = rSq;
      bruteClosestPoint = point;
    }
  }

  // Find closest point using kdtree
  double minDistSq = std::numeric_limits<double>::max();
  Vector3 closestPoint;
  size_t numBoundsChecks = 0;
  size_t numPointChecks = 0;
  node.visit(
      [&minDistSq, checkPoint, &numBoundsChecks](const AlignedBox3& bounds) {
        ++numBoundsChecks;
        return bounds.squaredExteriorDistance(checkPoint) < minDistSq;
      },

      [&minDistSq, &closestPoint, checkPoint, &numPointChecks](const Vector3& point) {
        ++numPointChecks;
        const double rSq = (point - checkPoint).squaredNorm();
        if (rSq < minDistSq) {
          minDistSq = rSq;
          closestPoint = point;
        }
      });

  CHECK_VEC3_EQ(bruteClosestPoint, closestPoint);
  CHECK(numBoundsChecks < count);
  CHECK(numPointChecks < count);

  // Count points within a bounding box
  AlignedBox3 searchBounds{Vector3{-500.0, -500.0, -500.0}, Vector3{500.0, 500.0, 500.0}};
  size_t foundPoints = 0;
  node.visit([&searchBounds](const AlignedBox3& bounds) { return searchBounds.intersects(bounds); },
             [&searchBounds, &foundPoints](const Vector3& point) {
               if (searchBounds.contains(point)) {
                 ++foundPoints;
               }
             });

  CHECK(foundPoints > count / 10);
  CHECK(foundPoints < count / 2);
}

TEST_CASE("ModifyInPlace") {
  // Build a k-d tree from a list of points

  std::vector<Vector2, Eigen::aligned_allocator<Vector2>> points;
  std::mt19937_64 randGen{size_t(42)};
  std::uniform_real_distribution<double> dist{0.1, 1.0};

  // Make random points
  constexpr size_t count = 100;
  points.resize(count);
  for (auto& point : points) {
    point << dist(randGen), dist(randGen);
  }

  auto node = kdtreepp::MakeEigenKdTreeNode<double, 2>(
      points.begin(), points.end(), [](const Vector2& p) { return p; },
      [](const Vector2& p) { return p; });

  size_t numBoundsChecks = 0;
  size_t numPointChecks = 0;
  node.visit(
      [&numBoundsChecks](const AlignedBox2& bounds) {
        (void)bounds;
        ++numBoundsChecks;
        return true;
      },

      [&numPointChecks](Vector2& point) {
        ++numPointChecks;
        point *= -1.0;
      });

  CHECK(numBoundsChecks < count);
  CHECK(numPointChecks == count);

  for (const auto& point : points) {
    CHECK(point.x() < 0.0);
    CHECK(point.y() < 0.0);
  }
}


// Copyright 2017 Shunji Lin. All rights reserved.
// Use of this source code is governed by an MIT-style
// license that can be found in the LICENSE file.
#include "pointer_table.hpp"
#include <vector>
#include <utility>
#include <cstddef>
#include <limits>
#include <cmath>
#include <climits>
#include <stdexcept>
#include <iostream>
#include "../utils/wall_timer.hpp"

// for primality testing
#include <boost/multiprecision/miller_rabin.hpp>

using namespace std;
using namespace boost::multiprecision;

constexpr size_t size_t_bits = sizeof(size_t) * CHAR_BIT;

// Note: bools in vectors take up 1 bit each
// (max_entries * ptr_bits) == bits.size()

PointerTable::PointerTable(size_t ptr_table_size_limit_in_bytes)
{
    utils::WallTimer timer;
    size_t big_ptr_size_in_bits = get_ptr_size_in_bits(ptr_table_size_limit_in_bytes);

    // choose pointer that gives max table size in entries
    size_t small_ptr_size_in_bits =
        big_ptr_size_in_bits > 0 ? big_ptr_size_in_bits - 1 : 0;

    size_t big_ptr_entries = ptr_table_size_limit_in_bytes * 8 / big_ptr_size_in_bits;
    size_t small_ptr_entries = pow(2, small_ptr_size_in_bits);

    for (; big_ptr_entries > 0; --big_ptr_entries) {
        if (miller_rabin_test(big_ptr_entries, 25)) break;
    }
    for (; small_ptr_entries > 0; --small_ptr_entries) {
        if (miller_rabin_test(small_ptr_entries, 25)) break;
    }

    size_t max_entries = 0;
    if (big_ptr_entries > small_ptr_entries) {
        ptr_size_in_bits = big_ptr_size_in_bits;
        max_entries = big_ptr_entries;
    } else {
        ptr_size_in_bits = small_ptr_size_in_bits;
        max_entries = small_ptr_entries;
    }

    bit_vector.resize(ptr_size_in_bits * max_entries, true);

    // invalid pointer representation: pointer with all bools set to true
    invalid_ptr = numeric_limits<size_t>::max() >> (size_t_bits - ptr_size_in_bits);

    // For logging purposes.
    // Due to primality tests, initialization may take some time.
    cout << "Time taken to initialize pointer table: " << timer << "\n"
         << "Size of pointer in pointer table: " << get_ptr_size_in_bits() << " bits\n"
         << "Size of pointer table: " << get_max_size_in_bytes() << " bytes\n"
         << "Max entries of pointer table: " << get_max_entries() << endl;
}

// Returns the biggest pointer size, may not be optimal in terms of size of
// pointer table.
size_t PointerTable::get_ptr_size_in_bits(size_t ptr_table_size_limit_in_bytes) const {
    size_t ptr_size_in_bits = 0;
    auto max_ptr_bits = size_t_bits;
    for (size_t ptr_sz = 0; ptr_sz < max_ptr_bits; ++ptr_sz) {
	size_t total_ptr_table_bits = ptr_sz * pow(2, ptr_sz);
	if (total_ptr_table_bits >= (ptr_table_size_limit_in_bytes * CHAR_BIT)) {
	    ptr_size_in_bits = ptr_sz;
	    break;
	}
    }
    return ptr_size_in_bits;
}

size_t PointerTable::get_ptr_at_index(size_t index) const {
    size_t pointer = 0;
    size_t bit_index = index * ptr_size_in_bits; // actual offset
    for (size_t i = 0; i < ptr_size_in_bits; ++i) {
	pointer <<= 1;
	if (bit_vector[bit_index]) pointer |= 1;
	++bit_index;
    }
    return pointer;
}

void PointerTable::insert_ptr_at_index(size_t pointer, size_t index) {
    if (get_n_entries() == get_max_entries())
        throw runtime_error("Attempting to insert in full pointer table");
    // go to last bit of the entry at index
    size_t bit_index = ((index + 1) * ptr_size_in_bits) - 1;
    // insert from last bit to first bit
    for (size_t i = 0; i < ptr_size_in_bits; ++i) {
	if (!(pointer & 1)) bit_vector[bit_index] = false;
	pointer >>= 1;
	--bit_index;
    }
    ++n_entries;
}

bool PointerTable::ptr_is_invalid(size_t ptr) const {
    return ptr == invalid_ptr;
}

void PointerTable::insert_ptr_with_hash(size_t pointer,
                               size_t hash_value,
                               size_t probe_value) {
    auto max_entries = get_max_entries();
    size_t probe_index = hash_value % max_entries;
    while (!ptr_is_invalid(get_ptr_at_index(probe_index))) {
	probe_index =
             (probe_index + (probe_value % max_entries)) % max_entries;
    }
    insert_ptr_at_index(pointer, probe_index);
}

size_t PointerTable::get_ptr_with_hash(size_t hash_value,
                               size_t probe_value,
                               bool first_probe) const {
    auto max_entries = get_max_entries();
    if (first_probe) {
	current_probe_index = hash_value % max_entries;
    } else {
        current_probe_index =
            (current_probe_index + (probe_value % max_entries)) % max_entries;
    }
    return get_ptr_at_index(current_probe_index);
}

size_t PointerTable::get_n_entries() const {
    return n_entries;
}

size_t PointerTable::get_max_entries() const {
    return bit_vector.size() / ptr_size_in_bits;
}

size_t PointerTable::get_max_size_in_bytes() const {
    return bit_vector.size() / 8;
}

size_t PointerTable::get_ptr_size_in_bits() const {
    return ptr_size_in_bits;
}

double PointerTable::get_load_factor() const {
    return static_cast<double>(get_n_entries()) /
        static_cast<double>(get_max_entries());
}


//
// Created by Hao Wu on 11/7/16.
//

#include <iostream>
#include "ROWPlus/ROWPlus.h"
#include "lorenz96.h"
#include <boost/numeric/odeint.hpp>

using namespace Eigen;
using namespace ROWPlus;
using namespace boost::numeric::odeint;
using namespace std;

template<typename vec>
void initX(vec& x, size_t N) {
  for (int i = 0; i < N; ++i)
    x[i] = 0.5 * ((double) i - 0.5 * (double) N);
}

int main() {
  const double t1 = 3.0;

  lorenz96 fun;

  std::cout << "Lorenz-96 (N=40)" << std::endl;

  lorenz96::state_type x_ref(40);
  initX(x_ref, 40);
  typedef runge_kutta_cash_karp54<lorenz96::state_type> error_stepper_type;
  integrate_adaptive(make_controlled<error_stepper_type>(1.0e-12, 1.0e-12),
                     fun, x_ref, 0.0, t1, 1e-8 );

  // initialize solution vectors
  lorenz96::state_type x_t1(40);
  VectorXd x_t2(40);

  // creat solver: runge_kutta4_classic
  runge_kutta4_classic< lorenz96::state_type > stepper_rk4;
  // create solver: ROWPlus::rosenbrock4
  ROWPlus::rosenbrock4<lorenz96, double> stepper_grk4t;
  stepper_grk4t.makeConstantStepper(&fun);
  // creat solver: rosenbrock_krylov4 ROK4A
  rosenbrock_krylov4<lorenz96, double> stepper_rok4a(4);
  ODEOptions<double> &_opts = stepper_rok4a.getOptions();
  _opts.TypeScheme = ROK4A;
  stepper_rok4a.makeConstantStepper(&fun);
  // creat solver: rosenbrock_krylov4 ROK4E
  rosenbrock_krylov4<lorenz96, double> stepper_rok4e(4);
  stepper_rok4e.makeConstantStepper(&fun);

  //
  double dt = 0.05;
  while (dt >= 5e-5) {
    ROWPlusSolverSpace::Status ret;
    cout << dt << " ";
    // stepper_rk54
    initX(x_t1, 40);
    integrate_const( stepper_rk4 , fun , x_t1 , 0.0 , t1 , dt );
    Map<VectorXd>(x_t1.data(), 40) -= Map<VectorXd>(x_ref.data(), 40);
    cout << Map<VectorXd>(x_t1.data(), 40).stableNorm() /
            Map<VectorXd>(x_ref.data(), 40).stableNorm() << " ";
    // stepper_grk4t
    initX(x_t2, 40);
    ret = stepper_grk4t.step(x_t2, 0.0, t1, dt);
    if (ret != ROWPlusSolverSpace::ComputeSucessful) {
      cout << "STAT = " << ret << endl;
      return 1;
    }
    x_t2 -= Map<VectorXd>(x_ref.data(), 40);
    cout << x_t2.stableNorm() /
        Map<VectorXd>(x_ref.data(), 40).stableNorm() << " ";
    // stepper_rok4a
    initX(x_t2, 40);
    ret = stepper_rok4a.step(x_t2, 0.0, t1, dt);
    if (ret != ROWPlusSolverSpace::ComputeSucessful) {
      cout << "STAT = " << ret << endl;
      return 1;
    }
    x_t2 -= Map<VectorXd>(x_ref.data(), 40);
    cout << x_t2.stableNorm() /
        Map<VectorXd>(x_ref.data(), 40).stableNorm() << " ";
    // stepper_rok4e
    initX(x_t2, 40);
    ret = stepper_rok4e.step(x_t2, 0.0, t1, dt);
    if (ret != ROWPlusSolverSpace::ComputeSucessful) {
      cout << "STAT = " << ret << endl;
      return 1;
    }
    x_t2 -= Map<VectorXd>(x_ref.data(), 40);
    cout << x_t2.stableNorm() /
        Map<VectorXd>(x_ref.data(), 40).stableNorm() << " ";

    cout << endl;
    dt /= 2.0;
  }
  return 0;
}


/* test_geometric_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/geometric_distribution.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::geometric_distribution<>
#define BOOST_RANDOM_ARG1 p
#define BOOST_RANDOM_ARG1_DEFAULT 0.5
#define BOOST_RANDOM_ARG1_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<int>::max)()
#define BOOST_RANDOM_DIST1_MIN 0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<int>::max)()

#define BOOST_RANDOM_TEST1_PARAMS (0.9999)
#define BOOST_RANDOM_TEST1_MIN 0
#define BOOST_RANDOM_TEST1_MAX 0

#define BOOST_RANDOM_TEST2_PARAMS (0.0001)
#define BOOST_RANDOM_TEST2_MIN 1

#include "test_distribution.ipp"