///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015 - 2016.
// 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)
//

// fixed_point_detail_cstdfloat.hpp implements a subset of
// floating-point typedefs having specified widths,
// as described in N3626 (proposed for C++14).
// See: http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2013/n3626.pdf
// This file has been adapted from <boost/cstdfloat.hpp>.
// Support for float128_t has been removed.

#ifndef FIXED_POINT_DETAIL_CSTDFLOAT_2015_08_14_HPP_
  #define FIXED_POINT_DETAIL_CSTDFLOAT_2015_08_14_HPP_

  #include <limits>

  #include <boost/fixed_point/detail/fixed_point_detail_stdfloat.hpp>

  // Here, we define floating-point typedefs having specified widths
  // within the namespace boost::fixed_point::detail.

  // In addition, a clear assessment of IEEE-754 comformance is
  // carried out using compile-time assertion.

  namespace boost { namespace fixed_point { namespace detail {

  #if(BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT32_NATIVE_TYPE == 1)
    static_assert(   (std::numeric_limits<float32_t>::is_iec559    == true)
                  && (std::numeric_limits<float32_t>::radix        ==    2)
                  && (std::numeric_limits<float32_t>::digits       ==   24)
                  && (std::numeric_limits<float32_t>::max_exponent ==  128),
                  "The 32-bit floating-point type in <cstdfloat> is not IEEE-754 conformant.");
  #endif

  #if(BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT64_NATIVE_TYPE == 1)
    static_assert(   (std::numeric_limits<float64_t>::is_iec559    == true)
                  && (std::numeric_limits<float64_t>::radix        ==    2)
                  && (std::numeric_limits<float64_t>::digits       ==   53)
                  && (std::numeric_limits<float64_t>::max_exponent == 1024),
                  "The 64-bit floating-point type in <cstdfloat> is not IEEE-754 conformant.");
  #endif

  #if(BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT80_NATIVE_TYPE == 1)
    static_assert(   (std::numeric_limits<float80_t>::is_iec559    ==  true)
                  && (std::numeric_limits<float80_t>::radix        ==     2)
                  && (std::numeric_limits<float80_t>::digits       ==    64)
                  && (std::numeric_limits<float80_t>::max_exponent == 16384),
                  "The 80-bit floating-point type in <cstdfloat> is not IEEE-754 conformant.");
  #endif

  #if(BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT128_NATIVE_TYPE == 1)
    static_assert(   (std::numeric_limits<float128_t>::is_iec559    ==  true)
                  && (std::numeric_limits<float128_t>::radix        ==     2)
                  && (std::numeric_limits<float128_t>::digits       ==   113)
                  && (std::numeric_limits<float128_t>::max_exponent == 16384),
                  "The 128-bit floating-point type in <cstdfloat> is not IEEE-754 conformant.");
  #endif

  #if(   (BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT32_NATIVE_TYPE  == 1) \
      || (BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT64_NATIVE_TYPE  == 1) \
      || (BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT80_NATIVE_TYPE  == 1) \
      || (BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT128_NATIVE_TYPE == 1))
  #else
    #error The maximum available floating-point width for <boost/fixed_point/detail/fixed_point_detailc_stdfloat.hpp> is undefined.
  #endif

  #undef BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT32_NATIVE_TYPE
  #undef BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT64_NATIVE_TYPE
  #undef BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT80_NATIVE_TYPE
  #undef BOOST_FIXED_POINT_STDFLOAT_HAS_FLOAT128_NATIVE_TYPE

  } } } // namespace boost::fixed_point::detail

#endif // FIXED_POINT_DETAIL_CSTDFLOAT_2015_08_14_HPP_


#include "solarsystem.h"
#include "planet.h"
#include <iostream>
#include <armadillo>
#include <stdio.h>
#include <iomanip>
using namespace arma;
using namespace std;

solarsystem::solarsystem()
{
}
void solarsystem::add(planet n){
     number_planets++;
     all_planets.push_back(n);
 }
void solarsystem::print_position(ofstream &output, ofstream &output2, vector<planet> vec){
    print_position(output, output2, vec, 3);
}

void solarsystem::print_position(ofstream &output, ofstream &output2, vector<planet> vec, int n){
    if(n>3 || n<=0) n=3;
    for(int i=0; i<vec.size(); i++){
        planet &this = vec[i];
        std::cout << std::scientific;
        for(int j=0; j<n;j++){
        std::cout << this.position[j] << "   ";
        output << std::scientific << this.position[j] << "   ";
        output2 << std::scientific << this.velocity[j] << "   ";
        }
        std::cout << "         ";
        output  << "         ";
        output2  << "         ";
       }
    std::cout << std::endl;
    output << endl;
    output2 << endl;
}

void solarsystem::insert_data(vector<planet> vec, mat &ma){
    for(int i=0; i<vec.size(); i++){
        planet &this = vec[i];
        ma(i,6)=this.mass;

        for(int k=0; k<3;k++){
            ma(i,k)=this.position[k];
            ma(i,k+3)=this.velocity[k];
        }
    }
}

void solarsystem::synctroniz(vector<planet> vec, mat &ma){
    int n = vec.size();

    for(int j=0; j<n;j++){
        planet &this = vec[j];
    for (int i = 0; i < 3; ++i){
       this.position[i] =  ma(j,i);
       this.velocity[i] = ma(j,i+3);
}
}

}

void solarsystem::solverRK4(vector<planet> vec, double h, double tmax){

    mat y_i(number_planets,7);
    mat y_i_temp(number_planets,7);
    mat k1(number_planets,7);
    mat k2(number_planets,7);
    mat k3(number_planets,7);
    mat k4(number_planets,7);

    insert_data(vec , y_i);
    double t=0;

    //for print file_________
    char *filename = new char[1000];
    char *filename2 = new char[1000];
        sprintf(filename, "Planet_position_RK4_%f.dat", h);
        sprintf(filename2, "Planet_velocity_RK4_%f.dat", h);

        ofstream output (filename);
        ofstream output2 (filename2);

        if (output.is_open()){
            output.precision(5);
            output2.precision(5);
    while(t<tmax){

        derivative(y_i, k1, number_planets);

        sum_matrix(y_i_temp, 1, y_i, 0.5*h, k1, number_planets);
        derivative(y_i_temp, k2, number_planets);

        sum_matrix( y_i_temp, 1,  y_i, 0.5*h,  k2, number_planets);

        derivative( y_i_temp,  k3, number_planets);

        sum_matrix( y_i_temp, 1,  y_i, h,  k3, number_planets);

        derivative( y_i_temp,  k4, number_planets);

        for(int j=1; j<number_planets; j++){

             for(int i=0; i<6; i++){
                 y_i(j,i) = y_i(j,i) + h*(k1(j,i) + 2*k2(j,i) + 2*k3(j,i) + k4(j,i))/6;
             }

             //Syncroniz position and velocity with the classes
             planet &this = vec[j];
             for(int i=0; i<3; i++){
             this.position[i] = y_i(j,i);
             this.velocity[i] = y_i(j,i+3);
             }

        }

print_position(output, output2, vec, 3);

t+=h;

}

output.close();
}
}

void solarsystem::solverVERLET(vector<planet> vec, double h, double tmax){

    mat y_i(number_planets,7);
    mat r_i_dt(number_planets,7);
    mat a_dt(number_planets,7);
    mat v_dt(number_planets,7);
    mat v_dt_2(number_planets,7);

    insert_data(vec , y_i);

    double t=0,zz =1;

    //print file___________________
    char *filename = new char[1000];
    char *filename2 = new char[1000];
        sprintf(filename, "Planet_position_Verlet_%f.dat", h);
        sprintf(filename2, "Planet_velocity_Verlet_%f.dat", h);

            ofstream output (filename);
            ofstream output2 (filename2);

            if (output.is_open()){
            output.precision(5);
            output2.precision(5);
    // end for print

    while(t<tmax){

        derivative(y_i, a_dt, number_planets);

        for(int j=0; j<number_planets; j++){

             for(int i=0; i<3; i++){
                 y_i(j,i) = y_i(j,i) + h*y_i(j,i+3) + 0.5*h*h*a_dt(j,i+3);
                 v_dt_2(j,i+3) = y_i(j,i+3) + 0.5*h*a_dt(j,i+3);
             }
        }

        derivative(y_i, a_dt, number_planets);

        for(int j=0; j<number_planets; j++){

             for(int i=3; i<6; i++){

                 y_i(j,i) = v_dt_2(j,i) + 0.5*h*a_dt(j,i);

             }

             planet &this = vec[j];
             for(int i=0; i<3; i++){
             this.position[i] = y_i(j,i);
             this.velocity[i] = y_i(j,i+3);
             }
        }
print_position(output, output2, vec,3);

t+=h;

}
output.close();
}

}
void solarsystem::sum_matrix(mat &result, double coeff_one, mat &first,double coeff_two, mat &second, int n){
    for(int j=0; j<n; j++){
         for(int i=0; i<6; i++){
            result(j,i) = coeff_one*first(j,i) + coeff_two*second(j,i);
         }
         result(j,6) = first(j,6);

    }
}
void solarsystem::printmat(mat &ma, int n){
    cout << endl;
    for(int i=0; i<7; i++){

        for(int k=0; k<n;k++){
            cout <<  ma(k,i)<<" " ;
        } cout << endl;}
}
double solarsystem::force(double x, double y, double z, double Mothers){
    double G=  4*M_PI*M_PI;
    double force=0;
    double distance=0;

    distance = x*x + y*y + z*z;

    force = G*Mothers/pow(distance, 1.5);

    return force;
}
void solarsystem::derivative(mat &dat, mat &de, int n){

    double accelleration_x=0,accelleration_y=0,accelleration_z=0, mod_force;
    for(int i=0; i<n; i++){

        accelleration_x=0,accelleration_y=0,accelleration_z=0;
        for(int j=0; j<n; j++){
            if(i!=j){

mod_force = force(dat(j,0)-dat(i,0),dat(j,1)-dat(i,1) ,dat(j,2)-dat(i,2),  dat(j,6));

accelleration_x += mod_force*(dat(j,0)-dat(i,0));
accelleration_y += mod_force*(dat(j,1)-dat(i,1));
accelleration_z += mod_force*(dat(j,2)-dat(i,2));
}
}
        de(i,3) = accelleration_x;
        de(i,4) = accelleration_y;
        de(i,5) = accelleration_z;

    }

    for(int i=0; i<n; i++){
        de(i,0) = dat(i,3); //velx
        de(i,1) = dat(i,4); //vely
        de(i,2) = dat(i,5); //velz
    }
}


// ---------------------------------------------------------------------------
//
//  This file is part of PermLib.
//
// Copyright (c) 2009-2011 Thomas Rehn <thomas@carmen76.de>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. The name of the author may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
// IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
// NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// ---------------------------------------------------------------------------

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE orbit lex min
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <fstream>
#include <algorithm>

#include <permlib/permutation.h>
#include <permlib/bsgs.h>
#include <permlib/symmetric_group.h>
#include <permlib/transversal/explicit_transversal.h>
#include <permlib/construct/schreier_sims_construction.h>

#include "test-common.h"
#include <permlib/transversal/orbit_set.h>
#include <permlib/search/orbit_lex_min_search.h>
#include <permlib/search/classic/lex_smaller_image_search.h>

using namespace permlib;
using namespace permlib::test;
using namespace std;

template<class BSGSIN>
void testOneSet(const BSGSIN& bsgs, const unsigned long n, const dset& testSet, bool withStabilizer = false) {
	typedef typename BSGSIN::PERMtype PERM;

	OrbitSet<PERM,dset> orbit;
	orbit.orbit(testSet, bsgs.S, DSetAction<PERM>(n));
	typedef OrbitLexMinSearch<BSGSIN>  OrbitLexMinSearchClass;

	OrbitLexMinSearchClass orbLexMin(bsgs);
	const dset lexMin = orbLexMin.lexMin(testSet);
	//cout << "lexMin = " << lexMin << endl;

	BOOST_REQUIRE(orbit.contains(lexMin));

	BOOST_FOREACH(const dset& el, make_pair(orbit.begin(), orbit.end())) {
		//cout << el << " - " << lexMin << " : " << OrbitLexMinSearch<int>::isLexSmaller(el, lexMin) << endl;
		BOOST_CHECK(!OrbitLexMinSearchClass::isLexSmaller(el, lexMin));
	}
}

void testGroup(const GroupInformation& info, const unsigned long numberOfTestRuns, const long perCent, bool withStabilizer = false) {
	typedef Permutation PERM;
	typedef ExplicitTransversal<PERM> TRANS;

	const BSGS<PERM,TRANS> bsgs = construct<PERM,TRANS>(info);
	const unsigned long n = info.n;

	for (unsigned int i = 0; i < numberOfTestRuns; ++i) {
		dset test(n);
		for (unsigned int j = 0; j < n; ++j) {
			if (rand() % 100 > perCent)
				test.set(j, 1);
		}
		testOneSet(bsgs, n, test, withStabilizer);
	}
}
BOOST_AUTO_TEST_CASE( lex_min_symmetric )
{
	const unsigned long numberOfTestRuns = 10;
	typedef Permutation PERM;

	const unsigned long n = 10;
	const SymmetricGroup<PERM> s_n(n);

	for (unsigned int i = 0; i < numberOfTestRuns; ++i) {
		dset test(n);
		for (unsigned int j = 0; j < n; ++j) {
			if (rand() % 100 > 60)
				test.set(j, 1);
		}
		testOneSet(s_n, n, test);
	}
}

BOOST_AUTO_TEST_CASE( lex_min_leon1997 )
{
	testGroup(info_1997, 30, 60);
}

BOOST_AUTO_TEST_CASE( lex_min_e6 )
{
	testGroup(info_e6, 5, 60);
}

BOOST_AUTO_TEST_CASE( lex_min_cyclic500 )
{
	testGroup(info_cyclic500, 30, 60);
}

void testGroup(const GroupInformation& info, bool isTwoTransitive = true) {
	typedef Permutation PERM;
	typedef ExplicitTransversal<PERM> TRANS;
	const unsigned int baseSize = 5;
	const unsigned int baseOffset = 2;
	BOOST_ASSERT(info.n > baseSize + baseOffset);

	BSGS<PERM,TRANS> bsgs = construct<PERM,TRANS>(info);

	std::vector<unsigned int> base(baseSize);
	for (unsigned int i = 0; i < base.size(); ++i) {
		base[i] = i + baseOffset;
	}

	ConjugatingBaseChange<PERM, TRANS, RandomBaseTranspose<PERM,TRANS> > cbc(bsgs);
	cbc.change(bsgs, base.begin(), base.end());

	std::list<unsigned int> zeros, ones;
	for (unsigned int i = 0; i < baseSize; ++i) {
		if (i == 0)
			zeros.push_back(i + baseOffset);
		else
			ones.push_back(i + baseOffset);
	}
	classic::LexSmallerImageSearch<BSGS<PERM,TRANS>, TRANS> backtrackSearch(bsgs, 0);
	backtrackSearch.construct(zeros.begin(), zeros.end(), ones.begin(), ones.end());
	PERM::ptr g = backtrackSearch.searchCosetRepresentative();
	BOOST_CHECK( !g );

	if (isTwoTransitive) {
		zeros.clear();
		ones.clear();
		for (unsigned int i = 0; i < baseSize; ++i) {
			if (i == 2 || i == 0)
				zeros.push_back(i + baseOffset);
			else
				ones.push_back(i + baseOffset);
		}
		backtrackSearch.construct(zeros.begin(), zeros.end(), ones.begin(), ones.end());
		g = backtrackSearch.searchCosetRepresentative();
		BOOST_CHECK( g );
	}
	zeros.clear();
	ones.clear();
	for (unsigned int i = 0; i < baseSize; ++i) {
		if (i == baseSize - 1)
			zeros.push_back(i + baseOffset);
		else
			ones.push_back(i + baseOffset);
	}
	backtrackSearch.construct(zeros.begin(), zeros.end(), ones.begin(), ones.end());
	g = backtrackSearch.searchCosetRepresentative();
	BOOST_CHECK( g );

	zeros.clear();
	ones.clear();
	for (unsigned int i = 0; i < baseSize; ++i) {
		if (i == baseSize/2)
			zeros.push_back(i + baseOffset);
		else
			ones.push_back(i + baseOffset);
	}
	backtrackSearch.construct(zeros.begin(), zeros.end(), ones.begin(), ones.end());
	g = backtrackSearch.searchCosetRepresentative();
	BOOST_CHECK( g );
}

BOOST_AUTO_TEST_CASE( not_lex_min )
{
	typedef Permutation PERM;
	typedef ExplicitTransversal<PERM> TRANS;

	testGroup(info_cyclic10, false);
	testGroup(info_e6);
	testGroup(info_1997);
}


#include "io_utils.h"

#include <Eigen/Dense>
#include <fstream>

Eigen::MatrixXd bayesmix::read_eigen_matrix(const std::string &filename) {
  // Initialize objects
  unsigned int cols = 0, rows = 0;
  double buffer[MAXBUFSIZE];
  std::ifstream istr(filename);
  if (!istr.is_open()) {
    std::string err = "File " + filename + " does not exist";
    throw std::invalid_argument(err);
  }

  // Loop over file lines
  while (!istr.eof()) {
    std::string line;
    getline(istr, line);

    unsigned int temp = 0;
    std::stringstream stream(line);
    while (!stream.eof()) {
      // Place read values into the buffer array
      stream >> buffer[cols * rows + temp++];
    }
    if (temp == 0) {
      continue;
    }
    if (cols == 0) {
      cols = temp;
    }
    rows++;
  }

  istr.close();
  rows--;

  // Fill an Eigen Matrix with values from the buffer array
  Eigen::MatrixXd mat(rows, cols-1);
  for (size_t i = 0; i < rows; i++) {
    for (size_t j = 0; j < cols-1; j++) {
      mat(i, j) = buffer[cols * i + j];
    }
  }
  return mat;
};

void bayesmix::write_matrix_to_file(const Eigen::MatrixXd &mat,
                                    std::string filename) {
  using namespace Eigen;
  const static IOFormat CSVFormat(StreamPrecision, DontAlignCols, ",", "\n");
  std::ofstream file(filename.c_str());
  file << mat.format(CSVFormat);
}


/*
 [auto_generated]
 libs/numeric/odeint/test/integrate_overflow.cpp

 [begin_description]
 This file tests the overflow exception of the integrate functions.
 [end_description]

 Copyright 2015 Mario Mulansky

 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)
 */

#define BOOST_TEST_MODULE odeint_integrate_overflow

#include <boost/test/unit_test.hpp>

#include <utility>
#include <iostream>
#include <vector>

#include <vector>
#include <cmath>
#include <iostream>

#include <boost/iterator/counting_iterator.hpp>

#include <boost/numeric/odeint.hpp>

using namespace boost::numeric::odeint;

typedef double value_type;
typedef std::vector< value_type > state_type;

// the famous lorenz system as usual
void lorenz( const state_type &x , state_type &dxdt , const value_type t )
{
    //const value_type sigma( 10.0 );
    const value_type R( 28.0 );
    const value_type b( value_type( 8.0 ) / value_type( 3.0 ) );

    // first component trivial
    dxdt[0] = 1.0; //sigma * ( x[1] - x[0] );
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

struct push_back_time
{
    std::vector< double >& m_times;

    push_back_time( std::vector< double > &times )
            :  m_times( times ) { }

    void operator()( const state_type &x , double t )
    {
        m_times.push_back( t );
    }
};

typedef runge_kutta_dopri5<state_type> stepper_type;
typedef runge_kutta_cash_karp54<state_type> cash_karp_type;

BOOST_AUTO_TEST_SUITE( integrate_overflow )

BOOST_AUTO_TEST_CASE( test_integrate_const )
{
    state_type x(3, 5.0);
    std::vector<double> times;

    // check the function signatures with normal stepper
    integrate_const(stepper_type(), lorenz, x, 0.0, 10.0, 1.0);
    integrate_const(stepper_type(), lorenz, x, 0.0, 10.0, 1.0, null_observer());
    // no exceptions expected for normal steppers
    integrate_const(stepper_type(), lorenz, x, 0.0, 10.0, 1.0, null_observer(), max_step_checker(10));

    // check exceptions for controlled stepper
    integrate_const(make_controlled<stepper_type>(1E-5, 1E-5), lorenz, x, 0.0, 10.0, 1.0);
    // very small error terms -> standard overflow threshold of 500 should fire an exception
    BOOST_CHECK_THROW(integrate_const(make_controlled<stepper_type>(1E-15, 1E-15), lorenz, x,
                                      0.0, 10.0, 1.0, push_back_time(times), max_step_checker()),
                      std::runtime_error);
    // small threshold of 10 -> larger error still gives an exception
    BOOST_CHECK_THROW(integrate_const(make_controlled<stepper_type>(1E-5, 1E-5), lorenz, x,
                                      0.0, 10.0, 1.0, push_back_time(times), max_step_checker(10)),
                      std::runtime_error);

    // check exceptions for dense output stepper
    integrate_const(make_dense_output<stepper_type>(1E-5, 1E-5), lorenz, x, 0.0, 10.0, 1.0);
    integrate_const(make_dense_output<stepper_type>(1E-5, 1E-5), lorenz, x, 0.0, 10.0, 1.0);
    // very small error terms -> standard overflow threshold of 500 should fire an exception
    BOOST_CHECK_THROW(integrate_const(make_dense_output<stepper_type>(1E-15, 1E-15), lorenz, x,
                                      0.0, 10.0, 1.0, push_back_time(times), max_step_checker()),
                      std::runtime_error);
    // small threshold of 10 -> larger error still gives an exception
    BOOST_CHECK_THROW(integrate_const(make_dense_output<stepper_type>(1E-5, 1E-5), lorenz, x,
                                      0.0, 10.0, 1.0, push_back_time(times), max_step_checker(10)),
                      std::runtime_error);
}

BOOST_AUTO_TEST_CASE( test_integrate_n_steps )
{
    state_type x(3, 5.0);
    std::vector<double> times;

    // check the function signatures with normal stepper
    integrate_n_steps(stepper_type(), lorenz, x, 0.0, 1.0, 10);
    integrate_n_steps(stepper_type(), lorenz, x, 0.0, 1.0, 10, null_observer());
    // no exceptions expected for normal steppers
    integrate_n_steps(stepper_type(), lorenz, x, 0.0, 1.0, 10, null_observer(), max_step_checker(10));

    // check exceptions for controlled stepper
    integrate_n_steps(make_controlled<stepper_type>(1E-5, 1E-5), lorenz, x, 0.0, 1.0, 10);
    // very small error terms -> standard overflow threshold of 500 should fire an exception
    BOOST_CHECK_THROW(integrate_n_steps(make_controlled<stepper_type>(1E-15, 1E-15), lorenz, x,
                                        0.0, 1.0, 10, push_back_time(times), max_step_checker()),
                      std::runtime_error);
    // small threshold of 10 -> larger error still gives an exception
    BOOST_CHECK_THROW(integrate_n_steps(make_controlled<stepper_type>(1E-5, 1E-5), lorenz, x,
                                        0.0, 1.0, 10, push_back_time(times), max_step_checker(10)),
                      std::runtime_error);

    // check exceptions for dense output stepper
    integrate_n_steps(make_dense_output<stepper_type>(1E-5, 1E-5), lorenz, x, 0.0, 1.0, 10);
    integrate_n_steps(make_dense_output<stepper_type>(1E-5, 1E-5), lorenz, x, 0.0, 1.0, 10, push_back_time(times));
    // very small error terms -> standard overflow threshold of 500 should fire an exception
    BOOST_CHECK_THROW(integrate_n_steps(make_dense_output<stepper_type>(1E-15, 1E-15), lorenz, x,
                                        0.0, 1.0, 10, push_back_time(times), max_step_checker()),
                      std::runtime_error);
    // small threshold of 10 -> larger error still gives an exception
    BOOST_CHECK_THROW(integrate_n_steps(make_dense_output<stepper_type>(1E-5, 1E-5), lorenz, x,
                                        0.0, 1.0, 10, push_back_time(times), max_step_checker(10)),
                      std::runtime_error);
}

BOOST_AUTO_TEST_CASE( test_integrate_times )
{
    state_type x(3, 5.0);
    std::vector<double> times;
    boost::counting_iterator<int> t0(0);
    boost::counting_iterator<int> t1(10);

    // check the function signatures with normal stepper
    integrate_times(stepper_type(), lorenz, x, t0, t1, 1.0 , push_back_time(times));
    // no exceptions expected for big enough step size
    integrate_times(stepper_type(), lorenz, x, t0, t1, 1.0 , push_back_time(times), max_step_checker(10));
    // if dt*max_steps < observer time difference we expect an exception
    BOOST_CHECK_THROW(integrate_times(stepper_type(), lorenz, x, t0, t1, 0.01, push_back_time(times),
                                      max_step_checker(10)),
                      std::runtime_error);

    // check exceptions for controlled stepper
    // no exception if no checker is provided
    integrate_times(make_controlled<stepper_type>(1E-5, 1E-5), lorenz, x, t0, t1, 1.0 , push_back_time(times));
    // very small error terms -> standard overflow threshold of 500 should fire an exception
    BOOST_CHECK_THROW(integrate_times(make_controlled<stepper_type>(1E-15, 1E-15), lorenz, x,
                                      t0, t1, 1.0 , push_back_time(times), max_step_checker()),
                      std::runtime_error);

    // small threshold of 10 -> larger error still gives an exception
    BOOST_CHECK_THROW(integrate_times(make_controlled<stepper_type>(1E-5, 1E-5), lorenz, x,
                                      t0, t1, 1.0 , push_back_time(times), max_step_checker(10)),
                      std::runtime_error);

    // check cash karp controlled stepper
    // no exception if no checker is provided
    integrate_times(make_controlled<cash_karp_type>(1E-5, 1E-5), lorenz, x, t0, t1, 1.0 , push_back_time(times));
    // very small error terms -> standard overflow threshold of 500 should fire an exception
    BOOST_CHECK_THROW(integrate_times(make_controlled<cash_karp_type>(1E-15, 1E-15), lorenz, x,
                      t0, t1, 1.0 , push_back_time(times), max_step_checker()),
    std::runtime_error);

    // small threshold of 10 -> larger error still gives an exception
    BOOST_CHECK_THROW(integrate_times(make_controlled<cash_karp_type>(1E-5, 1E-5), lorenz, x,
                      t0, t1, 1.0 , push_back_time(times), max_step_checker(10)),
    std::runtime_error);

    // check exceptions for dense output stepper
    integrate_times(make_dense_output<stepper_type>(1E-5, 1E-5), lorenz, x, t0, t1, 1.0 ,
                    push_back_time(times));
    // very small error terms -> standard overflow threshold of 500 should fire an exception
    BOOST_CHECK_THROW(integrate_times(make_dense_output<stepper_type>(1E-15, 1E-15), lorenz, x,
                                      t0, t1, 1.0 , push_back_time(times), max_step_checker()),
                      std::runtime_error);
    // small threshold of 10 -> larger error still gives an exception
    BOOST_CHECK_THROW(integrate_times(make_dense_output<stepper_type>(1E-5, 1E-5), lorenz, x,
                                      t0, t1, 1.0 , push_back_time(times), max_step_checker(10)),
                      std::runtime_error);
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2010-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <string>

// If defined, tests are run without rescaling-to-integer or robustness policy
// This multi_union currently contains no tests for double which then fail
// #define BOOST_GEOMETRY_NO_ROBUSTNESS

#include "test_union.hpp"
#include <algorithms/test_overlay.hpp>
#include <algorithms/overlay/multi_overlay_cases.hpp>

#include <boost/geometry/multi/algorithms/correct.hpp>
#include <boost/geometry/multi/algorithms/intersection.hpp>
#include <boost/geometry/multi/algorithms/within.hpp>

#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/multi/geometries/multi_linestring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

#include <boost/geometry/multi/io/wkt/read.hpp>

template <typename Ring, typename Polygon, typename MultiPolygon>
void test_areal()
{
    test_one<Polygon, MultiPolygon, MultiPolygon>("simplex_multi",
        case_multi_simplex[0], case_multi_simplex[1],
        1, 0, 20, 14.58);

    test_one<Polygon, Polygon, MultiPolygon>("simplex_multi_p_mp",
        case_single_simplex, case_multi_simplex[0],
        1, 0, 20, 14.58);
    test_one<Polygon, MultiPolygon, Polygon>("simplex_multi_mp_p",
        case_multi_simplex[0], case_single_simplex,
        1, 0, 20, 14.58);

    test_one<Polygon, Ring, MultiPolygon>("simplex_multi_r_mp",
        case_single_simplex, case_multi_simplex[0],
        1, 0, 20, 14.58);
    test_one<Ring, MultiPolygon, Polygon>("simplex_multi_mp_r",
        case_multi_simplex[0], case_single_simplex,
        1, 0, 20, 14.58);

    // Normal test cases
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_multi_no_ip",
        case_multi_no_ip[0], case_multi_no_ip[1],
        4, 0, 16, 66.5);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_multi_2",
        case_multi_2[0], case_multi_2[1],
        3, 0, 16, 59.1);

    // Constructed cases for multi/touch/equal/etc
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_61_multi",
        case_61_multi[0], case_61_multi[1],
        1, 0, 11, 4.0);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_62_multi",
        case_62_multi[0], case_62_multi[1],
        2, 0, 10, 2.0);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_63_multi",
        case_63_multi[0], case_63_multi[1],
        2, 0, 10, 2.0);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_64_multi",
        case_64_multi[0], case_64_multi[1],
        1, 0, 9, 3.0);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_65_multi",
        case_65_multi[0], case_65_multi[1],
        3, 0, 15, 4.0);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_66_multi",
        case_66_multi[0], case_66_multi[1],
        3, 0, 23, 7.0);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_72_multi",
        case_72_multi[0], case_72_multi[1],
        1, 0, 13, 10.65);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_75_multi",
        case_75_multi[0], case_75_multi[1],
        5, 0, 25, 5.0);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_76_multi",
        case_76_multi[0], case_76_multi[1],
        5, 0, 31, 8.0);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_89_multi",
        case_89_multi[0], case_89_multi[1],
        1, 0, 13, 6);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_101_multi",
        case_101_multi[0], case_101_multi[1],
        1, 0, 32, 22.25);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_103_multi",
        case_103_multi[0], case_103_multi[1],
        1, 0, 7, 25);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_104_multi",
        case_104_multi[0], case_104_multi[1],
        1, 0, 8, 25);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_105_multi",
        case_105_multi[0], case_105_multi[1],
        1, 0, 5, 25);

    test_one<Polygon, MultiPolygon, MultiPolygon>("case_recursive_boxes_1",
        case_recursive_boxes_1[0], case_recursive_boxes_1[1],
        1, 1, 36, 97.0);
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_recursive_boxes_2",
        case_recursive_boxes_2[0], case_recursive_boxes_2[1],
        1, 0, 14, 100.0); // Area from SQL Server
    test_one<Polygon, MultiPolygon, MultiPolygon>("case_recursive_boxes_3",
        case_recursive_boxes_3[0], case_recursive_boxes_3[1],
        17, 0, 159, 56.5); // Area from SQL Server

    test_one<Polygon, MultiPolygon, MultiPolygon>("ggl_list_20120915_h2_a",
         ggl_list_20120915_h2[0], ggl_list_20120915_h2[1],
         1, 0, 12, 23.0); // Area from SQL Server
    test_one<Polygon, MultiPolygon, MultiPolygon>("ggl_list_20120915_h2_b",
         ggl_list_20120915_h2[0], ggl_list_20120915_h2[2],
         1, 0, 12, 23.0); // Area from SQL Server

    test_one<Polygon, MultiPolygon, MultiPolygon>("ggl_list_20140212_sybren",
         ggl_list_20140212_sybren[0], ggl_list_20140212_sybren[1],
         2, 0, 16, 0.002471626);

    test_one<Polygon, MultiPolygon, MultiPolygon>("ticket_9081",
        ticket_9081[0], ticket_9081[1],
        3, 0, 31, 0.2187385);
}

template <typename P>
void test_all()
{

    {
        typedef bg::model::ring<P> ring;
        typedef bg::model::polygon<P> polygon;
        typedef bg::model::multi_polygon<polygon> multi_polygon;
        test_areal<ring, polygon, multi_polygon>();
    }

    {
        typedef bg::model::ring<P, false> ring_ccw;
        typedef bg::model::polygon<P, false> polygon_ccw;
        typedef bg::model::multi_polygon<polygon_ccw> multi_polygon_ccw;
        test_areal<ring_ccw, polygon_ccw, multi_polygon_ccw>();
    }

}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<double> >();

#if ! defined(BOOST_GEOMETRY_TEST_ONLY_ONE_TYPE)
    test_all<bg::model::d2::point_xy<float> >();

#if defined(HAVE_TTMATH)
    std::cout << "Testing TTMATH" << std::endl;
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

#endif

    return 0;
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/tuple.hpp>
using namespace boost::hana;

int main() {

{

//! [comparing]
BOOST_HANA_CONSTEXPR_LAMBDA auto grouped = group_by(comparing(length), make<Tuple>(
    make<Tuple>(1, 2, 3),
    make<Tuple>('x', 'y', 'z'),
    range_c<long, 0, 1>,
    tuple_t<char, int>,
    range_c<int, 0, 2>,
    make<Tuple>(123.4, nullptr)
));

BOOST_HANA_CONSTEXPR_CHECK(grouped == make<Tuple>(
    make<Tuple>(
        make<Tuple>(1, 2, 3),
        make<Tuple>('x', 'y', 'z')
    ),
    make<Tuple>(
        range_c<long, 0, 1>
    ),
    make<Tuple>(
        tuple_t<char, int>,
        range_c<int, 0, 2>,
        make<Tuple>(123.4, nullptr)
    )
));
//! [comparing]

}{

//! [equal]
BOOST_HANA_CONSTEXPR_CHECK(equal(make<Tuple>(1, 2), make<Tuple>(1, 2)));
BOOST_HANA_CONSTEXPR_CHECK(!equal('x', 'y'));
BOOST_HANA_CONSTANT_CHECK(!equal(make<Tuple>(1, 2), 'y'));

BOOST_HANA_CONSTEXPR_CHECK(any_of(make<Tuple>(1, 2, 3), equal.to(2)));
//! [equal]

}{

//! [not_equal]
BOOST_HANA_CONSTEXPR_CHECK(not_equal(make<Tuple>(1, 2), make<Tuple>(3)));
BOOST_HANA_CONSTEXPR_CHECK(not_equal('x', 'y'));
BOOST_HANA_CONSTANT_CHECK(not_equal(make<Tuple>(1, 2), 'y'));

BOOST_HANA_CONSTEXPR_CHECK(all_of(make<Tuple>(1, 2, 3), not_equal.to(5)));
//! [not_equal]

}

}


/*
 * SearchGrid.hpp
 *
 *  Created on: 13. 9. 2013
 *      Author: Petr
 */
#pragma once
#ifndef SEARCHGRID_HPP_
#define SEARCHGRID_HPP_

#include <vector>
#include <list>
#include <functional>
#include <tuple>
#include <memory>
#include <cmath>
#include <Eigen/Dense>
#include "TriangleElement.hpp"
#include "BoundingBox.hpp"

template<int dim>
class SearchGrid {

public:

	std::list<TriangleElement<double> > dataFromPosition(const Eigen::Array3d& coord);

	void insertElement(const TriangleElement<double>& element);

	SearchGrid(const Eigen::Array3d& x_lo, const Eigen::Array3d& x_hi, const Eigen::Array3i& m);

	std::list<TriangleElement<double> > operator()(int i, int j, int k) const;

private:

	std::vector< std::list< TriangleElement<double> > > data; // stores data inside the grid

	Eigen::Array3d x_lo;
	Eigen::Array3d x_hi;
	Eigen::Array3d dx;
	Eigen::Array3i m;

	long int toLinear(const Eigen::Array3i& inds) const;

	Eigen::Array3i toIndices(const Eigen::Array3d& coord) const;

	Box<double, dim> cellBox(const Eigen::Array3i& inds);

	std::tuple<Eigen::Array3i, Eigen::Array3i> listOfSpannedCells(const Box<double, dim>& boundingBox);

};

template<int dim>
SearchGrid<dim>::SearchGrid(const Eigen::Array3d& x_lo, const Eigen::Array3d& x_hi, const Eigen::Array3i& m) {

	this->x_lo = x_lo;
	this->x_hi = x_hi;
	this->m    = m;

	this->dx   = (x_hi - x_lo) / (m - 1).cast<double>();

	this->data.resize( m.prod() );

}

template<int dim>
inline std::list<TriangleElement<double> > SearchGrid<dim>::operator()(int i, int j, int k) const {

	int ii, jj, kk;

	ii = (i >= m[0]) ? m[0]-1 : (i < 0) ? 0 : i;
	jj = (j >= m[1]) ? m[1]-1 : (j < 0) ? 0 : j;
	kk = (k >= m[2]) ? m[2]-1 : (k < 0) ? 0 : k;

	return data[toLinear(Eigen::Array3i(ii,jj,kk))];
}

template <int dim>
long int SearchGrid<dim>::toLinear(const Eigen::Array3i& inds) const {
	return inds[0] + inds[1]*m[0] + inds[2]*m[0]*m[1];
}

template <int dim>
Eigen::Array3i SearchGrid<dim>::toIndices(const Eigen::Array3d& coord) const{
	return ((coord - x_lo) / dx).cast<int>();
}

template <int dim>
std::list<TriangleElement<double> > SearchGrid<dim>::dataFromPosition(const Eigen::Array3d& coord) {
	using namespace Eigen;

	Eigen::Array3i inds = toIndices(coord);
	std::list<TriangleElement<double> > ret;
	int pad = 1;
	while (ret.size() == 0) {
		for (int i = inds[0]-pad; i <= inds[0]+pad; ++i) {
			for (int j = inds[1]-pad; j <= inds[1]+pad; ++j) {
				for (int k = inds[2]-pad; k <= inds[2]+pad; ++k) {
					std::list<TriangleElement<double> > tmp = this->operator ()(i,j,k);
					ret.insert(ret.begin(), tmp.begin(), tmp.end());
				}
			}
		}
		pad++;
	}

	return ret;
}

template <int dim>
Box<double, dim> SearchGrid<dim>::cellBox(const Eigen::Array3i& inds) {

	double cx[dim] = { (x_lo[0]+ 0.5*dx[0]) + inds[0]*dx[1],
					   (x_lo[1]+ 0.5*dx[1]) + inds[1]*dx[1],
					   (x_lo[2]+ 0.5*dx[2]) + inds[2]*dx[2]};
	double ex[dim] = { 0.5*dx[0], 0.5*dx[1], 0.5*dx[2] };

	return Box<double, dim>(cx, ex);

}

template <int dim>
std::tuple<Eigen::Array3i, Eigen::Array3i> SearchGrid<dim>::listOfSpannedCells(const Box<double, dim>& boundingBox) {

	using namespace Eigen;

	Eigen::Array3i bli = toIndices(boundingBox.bl());
	Eigen::Array3i tri = toIndices(boundingBox.tr());

	return std::forward_as_tuple(bli, tri);

}

template <int dim>
void SearchGrid<dim>::insertElement(const TriangleElement<double>& element) {

	auto cellSpan = listOfSpannedCells( element.getBoundingBox() );

	auto bli = std::get<0>(cellSpan);
	auto uri = std::get<1>(cellSpan);

	for (int i = bli[0]; i <= uri[0]; ++i) {
		for (int j = bli[1]; j <= uri[1]; ++j) {
			for (int k = bli[2]; k <= uri[2]; ++k) {
				if ( element.isPartlyInRegion(cellBox(Eigen::Array3i(i,j,k))) ) {
					data[toLinear(Eigen::Array3i(i,j,k))].push_back(element);
				}
			}
		}
	}

}

//==========================================
// ostream operators used for debug printing
//==========================================

template <int d>
std::ostream& operator<<(std::ostream& os, const SearchGrid<d>& sg) {

	std::cout << sg.m << std::endl;

    return os;
}

#endif


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//               2014 Piotr Godlewski
//
// 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)
//=======================================================================
/**
 * @file ir_components.hpp
 * @brief
 * @author Piotr Wygocki, Piotr Godlewski
 * @version 1.0
 * @date 2013-05-10
 */
#ifndef PAAL_IR_COMPONENTS_HPP
#define PAAL_IR_COMPONENTS_HPP

#include "paal/data_structures/components/components.hpp"
#include "paal/lp/ids.hpp"
#include "paal/lp/lp_base.hpp"
#include "paal/lp/problem_type.hpp"
#include "paal/utils/floating.hpp"
#include "paal/utils/functors.hpp"

#include <boost/optional.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/tuple/tuple.hpp>

#include <cmath>

namespace paal {
namespace ir {

/**
 * @brief Default column rounding condition component.
 */
class default_round_condition {
  public:
    /**
     * @brief constructor takes epsilon used in double comparison.
     */
    default_round_condition(double epsilon = utils::compare<double>::default_epsilon()): m_compare(epsilon) { }

    /**
     * @brief Rounds the column if its value is integral.
     */
    template <typename Problem, typename LP>
    boost::optional<double> operator()(Problem &, const LP &lp,
                                       lp::col_id col) {
        double x = lp.get_col_value(col);
        double r = std::round(x);
        if (m_compare.e(x,r)) {
            return r;
        }
        return boost::none;
    }

  protected:
    /// Double comparison object.
    const utils::compare<double> m_compare;
};

/**
 * @brief Column rounding component.
 *        Rounds a column if its value is equal to one of the template parameter
 * values.
 */
template <int...> class round_condition_equals {
    round_condition_equals() = delete;
};

/**
 * @brief Column rounding component.
 *        Rounds a column if its value is equal to one of the template parameter
 * values.
 */
template <int arg, int... args>
class round_condition_equals<arg,
                             args...> : public round_condition_equals<args...> {
  public:
    /**
     * @brief constructor takes epsilon used in double comparison.
     */
    round_condition_equals(double epsilon = utils::compare<double>::default_epsilon()): round_condition_equals<args...>(epsilon) { }

    /// Rounds a column if its value is equal to one of the template parameter
    /// values.
    template <typename Problem, typename LP>
    boost::optional<double> operator()(Problem &, const LP &lp,
                                       lp::col_id col) {
        return get(lp, lp.get_col_value(col));
    }

  protected:
    /// Checks if the value can be rounded to the first template parameter.
    template <typename LP>
    boost::optional<double> get(const LP & lp, double x) {
        if (this->m_compare.e(x, arg)) {
            return double(arg);
        } else {
            return round_condition_equals<args...>::get(lp, x);
        }
    }
};

/**
 * @brief Column rounding component.
 *        Rounds a column if its value is equal to one of the template parameter
 * values.
 *        Edge case (no template parameter values).
 */
template <> class round_condition_equals<> {
  public:
    /**
     * @brief constructor takes epsilon used in double comparison.
     */
    round_condition_equals(double epsilon = utils::compare<double>::default_epsilon()): m_compare(epsilon) { }

  protected:
    /// Edge case: return false.
    template <typename LP> boost::optional<double> get(const LP &lp, double x) {
        return boost::none;
    }

    /// Double comparison object.
    const utils::compare<double> m_compare;
};

/**
 * @brief Column rounding component.
 *        Rounds a column if its value satisfies a fixed condition.
 *        The column is rounded to a value defined by a fixed function.
 */
template <typename Cond, typename F> class round_condition_to_fun {
  public:
    /**
     * @brief Constructor. Takes the rounding condition and the rounding
     * function.
     */
    round_condition_to_fun(Cond c = Cond(), F f = F()) : m_cond(c), m_f(f) {}

    /// Rounds a column if its value satisfies a fixed condition.
    template <typename Problem, typename LP>
    boost::optional<double> operator()(Problem &, const LP &lp,
                                       lp::col_id col) {
        double x = lp.get_col_value(col);
        if (m_cond(x)) {
            return m_f(x);
        }
        return boost::none;
    }

  private:
    Cond m_cond;
    F m_f;
};

/**
 * @brief Checks if a variable is greater or equal than a fixed bound.
 */
class cond_bigger_equal_than {
  public:
    /**
     * @brief constructor takes epsilon used in double comparison.
     */
    cond_bigger_equal_than(double b, double epsilon = utils::compare<double>::default_epsilon())
        : m_bound(b), m_compare(epsilon) {}

    /// Checks if a variable is greater or equal than a fixed bound.
    bool operator()(double x) { return m_compare.ge(x, m_bound); }

  private:
    double m_bound;
    const utils::compare<double> m_compare;
};

/**
 * @brief Column rounding component.
 *        A variable is rounded up to 1, if it has value at least half in the
 * solution.
 */
struct round_condition_greater_than_half  :
    public round_condition_to_fun<cond_bigger_equal_than, utils::return_one_functor> {
        /**
         * @brief constructor takes epsilon used in double comparison.
         */
        round_condition_greater_than_half(double epsilon = utils::compare<double>::default_epsilon()) :
            round_condition_to_fun(cond_bigger_equal_than(0.5, epsilon)) {}
};

/**
 * @brief Finds an extreme point solution to the LP.
 */
struct default_solve_lp_to_extreme_point {
    /// Finds an extreme point solution to the LP.
    template <typename Problem, typename LP>
    lp::problem_type operator()(Problem &, LP &lp) {
        return lp.solve_simplex(lp::PRIMAL);
    }
};

/**
 * @brief Finds an extreme point solution to the LP.
 */
struct default_resolve_lp_to_extreme_point {
    /// Finds an extreme point solution to the LP.
    template <typename Problem, typename LP>
    lp::problem_type operator()(Problem &, LP &lp) {
        return lp.resolve_simplex(lp::PRIMAL);
    }
};

/**
 * @brief Default stop condition component.
 */
class default_stop_condition {
  public:
    /**
     * @brief Constructor. Takes epsilon used in double comparison.
     */
    default_stop_condition(double epsilon = utils::compare<double>::default_epsilon()): m_compare(epsilon) { }

    /**
     * @brief Checks if the current LP solution has got only integer values.
     */
    template <typename Problem, typename LP>
    bool operator()(Problem &, const LP &lp) {
        for (lp::col_id col : lp.get_columns()) {
            double col_val = lp.get_col_value(col);
            if (!m_compare.e(col_val, std::round(col_val))) {
                return false;
            }
        }

        return true;
    }

  protected:
    /// Double comparison object.
    const utils::compare<double> m_compare;
};

/**
 * @brief Checks if the relaxations limit was reached.
 */
class relaxations_limit_condition {
  public:
    /// Constructor.
    relaxations_limit_condition(int limit = 1) : m_limit(limit) {}

    /**
     * @brief Checks if the relaxations limit was reached.
     */
    bool operator()(int relaxed) { return relaxed >= m_limit; }

  private:
    int m_limit;
};

class Init;
class RoundCondition;
class RelaxCondition;
class SetSolution;
class SolveLP;
class ResolveLP;
class StopCondition;
class RelaxationsLimit;

using components = data_structures::components<
    Init,
    data_structures::NameWithDefault<RoundCondition, default_round_condition>,
    data_structures::NameWithDefault<RelaxCondition, utils::always_false>,
    data_structures::NameWithDefault<SetSolution, utils::skip_functor>,
    data_structures::NameWithDefault<SolveLP, default_solve_lp_to_extreme_point>,
    data_structures::NameWithDefault<ResolveLP, default_resolve_lp_to_extreme_point>,
    data_structures::NameWithDefault<StopCondition, default_stop_condition>,
    data_structures::NameWithDefault<RelaxationsLimit, utils::always_false>>;

/**
 * @brief Iterative rounding components.
 */
template <typename... Args>
using IRcomponents = typename components::type<Args...>;

/**
 * @brief Returns iterative rounding components.
 */
template <typename... Args>
auto make_IRcomponents(Args &&... args)
    ->decltype(components::make_components(std::forward<Args>(args)...)) {
    return components::make_components(std::forward<Args>(args)...);
}

} // ir
} // paal

#endif // PAAL_IR_COMPONENTS_HPP


#include <tradelayer/parse_string.h>

#include <algorithm>
#include <stdint.h>
#include <string>

#include <boost/lexical_cast.hpp>

namespace mastercore
{
int64_t StrToInt64(const std::string& str, bool divisible)
{
    // copy original, so it remains unchanged
    std::string strAmount (str);
    int64_t nAmount = 0;

    // check for a negative (minus sign) and invalidate if present
    size_t negSignPos = strAmount.find("-");
    if (negSignPos != std::string::npos) return 0;

    // convert the string into a usable int64
    if (divisible) {
        // check for existance of decimal point
        size_t pos = strAmount.find(".");
        if (pos == std::string::npos) {
            // no decimal point but divisible so pad 8 zeros on right
            strAmount += "00000000";
        } else {
            // check for existence of second decimal point, if so invalidate amount
            size_t posSecond = strAmount.find(".", pos + 1);
            if (posSecond != std::string::npos) return 0;

            if ((strAmount.size() - pos) < 9) {
                // there are decimals either exact or not enough, pad as needed
                std::string strRightOfDecimal = strAmount.substr(pos + 1);
                unsigned int zerosToPad = 8 - strRightOfDecimal.size();

                // do we need to pad?
                if (zerosToPad > 0)
                {
                    for (unsigned int it = 0; it != zerosToPad; it++) {
                        strAmount += "0";
                    }
                }
            } else {
                // there are too many decimals, truncate after 8
                strAmount = strAmount.substr(0, pos + 9);
            }
        }
        strAmount.erase(std::remove(strAmount.begin(), strAmount.end(), '.'), strAmount.end());
        try {
            nAmount = boost::lexical_cast<int64_t>(strAmount);
        } catch (const boost::bad_lexical_cast &e) {}
    } else {
        size_t pos = strAmount.find(".");
        std::string newStrAmount = strAmount.substr(0, pos);
        try {
            nAmount = boost::lexical_cast<int64_t>(newStrAmount);
        } catch (const boost::bad_lexical_cast &e) {}
    }

    return nAmount;
}

} // namespace mastercore


// Copyright (C) 2008-2018 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0 (see accompanying
// file LICENSE_1_0.txt or a copy at http://www.boost.org/LICENSE_1_0.txt).
// See: http://www.boost.org/doc/libs/release/libs/contract/doc/html/index.html

//[n1962_factorial
#include <boost/contract.hpp>
#include <cassert>

int factorial(int n) {
    int result;
    boost::contract::check c = boost::contract::function()
        .precondition([&] {
            BOOST_CONTRACT_ASSERT(n >= 0); // Non-negative natural number.
            BOOST_CONTRACT_ASSERT(n <= 12); // Max function input.
        })
        .postcondition([&] {
            BOOST_CONTRACT_ASSERT(result >= 1);
            if(n < 2) { // Select assertion.
                BOOST_CONTRACT_ASSERT(result == 1);
            } else {
                // Assertions automatically disabled in other assertions.
                // Therefore, this postcondition can recursively call the
                // function without causing infinite recursion.
                BOOST_CONTRACT_ASSERT_AUDIT(n * factorial(n - 1));
            }
        })
    ;

    return n < 2 ? (result = 1) : (result = n * factorial(n - 1));
}

int main() {
    assert(factorial(4) == 24);
    return 0;
}
//]


// Example is based on num_list1.cpp from the boost.spirit repo:
// https://github.com/boostorg/spirit/blob/develop/example/x3/num_list/num_list1.cpp

#include <boost/spirit/home/x3.hpp>
#include <string>

namespace x3 = boost::spirit::x3;

int main()
{
    std::string csl("1,2,3,4");

    auto first = csl.begin();
    auto last = csl.end();

    auto res = x3::phrase_parse(first, last, x3::double_ >> *(',' >> x3::double_), x3::ascii::space);

    return res && (first == last) ? 0 : 1;
}


#include <boost/range/adaptor/transformed.hpp>
#include <cassert>

#include "host_tensor.hpp"

void HostTensorDescriptor::CalculateStrides()
{
    mStrides.clear();
    mStrides.resize(mLens.size(), 0);
    if(mStrides.empty())
        return;

    mStrides.back() = 1;
    std::partial_sum(
        mLens.rbegin(), mLens.rend() - 1, mStrides.rbegin() + 1, std::multiplies<std::size_t>());
}

std::size_t HostTensorDescriptor::GetNumOfDimension() const { return mLens.size(); }

std::size_t HostTensorDescriptor::GetElementSize() const
{
    assert(mLens.size() == mStrides.size());
    return std::accumulate(
        mLens.begin(), mLens.end(), std::size_t{1}, std::multiplies<std::size_t>());
}

std::size_t HostTensorDescriptor::GetElementSpace() const
{
    auto ls = mLens | boost::adaptors::transformed([](std::size_t v) { return v - 1; });
    return std::inner_product(ls.begin(), ls.end(), mStrides.begin(), std::size_t{0}) + 1;
}

const std::vector<std::size_t>& HostTensorDescriptor::GetLengths() const { return mLens; }

const std::vector<std::size_t>& HostTensorDescriptor::GetStrides() const { return mStrides; }

void ostream_HostTensorDescriptor(const HostTensorDescriptor& desc, std::ostream& os)
{
    os << "dim " << desc.GetNumOfDimension() << ", ";

    os << "lengths {";
    LogRange(os, desc.GetLengths(), ", ");
    os << "}, ";

    os << "strides {";
    LogRange(os, desc.GetStrides(), ", ");
    os << "}" << std::endl;
}


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>

#include <vector>

using namespace Eigen;

template<typename dtype>
using MatrixX = Matrix<dtype, Dynamic, Dynamic>;

template<typename dtype>
using MatrixXR = Matrix<dtype, Dynamic, Dynamic, RowMajor>;

template<typename dtype>
using VectorX = Matrix<dtype, Dynamic, 1>;

template<typename mat>
mat get_sub_matrix(const mat &M,
                   const std::vector<uint> &ind_rows,
                   const std::vector<uint> &ind_cols) {

    mat sub_M(ind_rows.size(), ind_cols.size());
    for (uint j = 0; j < ind_cols.size(); j++) {
        for (uint i = 0; i < ind_rows.size(); i++) {
            sub_M(i, j) = M(ind_rows[i], ind_cols[j]);
        }
    }

    return sub_M;
}

template<typename mat>
mat get_sub_matrix(const mat &M,
                   const std::vector<uint> &indices) {
    return get_sub_matrix(M, indices, indices);
}

template<typename mat1, typename mat2>
void set_sub_matrix(mat1 &M,
                    const std::vector<uint> &ind_rows,
                    const std::vector<uint> &ind_cols,
                    const mat2 &sub_M) {

    for (int j = 0; j < ind_cols.size(); j++) {
        for (int i = 0; i < ind_rows.size(); i++) {
            M(ind_rows[i], ind_cols[j]) = sub_M(i, j);
        }
    }
}

template<typename mat1, typename mat2>
void set_sub_matrix(mat1 &M,
                    const std::vector<uint> &indices,
                    const mat2 &sub_M) {

    return set_sub_matrix(M, indices, indices, sub_M);
}

template<typename vec>
vec get_sub_vector(const vec &V,
                   const std::vector<uint> &indices) {

    vec sub_V(indices.size());
    for (uint i = 0; i < indices.size(); i++) {
        sub_V(i) = V(indices[i]);
    }

    return sub_V;
}

template<typename vec1, typename vec2>
void set_sub_vector(vec1 &V,
                    const std::vector<uint> &indices,
                    const vec2 &sub_V) {

    for (uint i = 0; i < indices.size(); i++) {
        V(indices[i]) = sub_V(i);
    }
}

template<typename mat1, typename mat2>
void add_sub_matrix(mat1 &M,
                    const std::vector<uint> &ind_rows,
                    const std::vector<uint> &ind_cols,
                    const mat2 &sub_M) {

    for (uint j = 0; j < ind_cols.size(); j++) {
        for (uint i = 0; i < ind_rows.size(); i++) {
            M(ind_rows[i], ind_cols[j]) += sub_M(i, j);
        }
    }
}

template<typename mat1, typename mat2>
void add_sub_matrix(mat1 &M,
                    const std::vector<uint> &indices,
                    const mat2 &sub_M) {

    return add_sub_matrix(M, indices, indices, sub_M);
}

// Taken from https://gist.github.com/redpony/fc8a0db6b20f7b1a3f23
// set use_cholesky if M is symmetric - it's faster and more stable
// for dep paring it won't be
template <typename MatrixType>
inline typename MatrixType::Scalar logdet(const MatrixType& M, bool use_cholesky = false) {
    using namespace Eigen;
    typedef typename MatrixType::Scalar Scalar;
    Scalar ld = 0;
    LLT<Matrix<Scalar,Dynamic,Dynamic>> chol(M);
    return 2 * chol.matrixL().toDenseMatrix().diagonal().array().log().sum();
}


/* boost random/bernoulli_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_BERNOULLI_DISTRIBUTION_HPP
#define BOOST_RANDOM_BERNOULLI_DISTRIBUTION_HPP

#include <iosfwd>
#include <boost/assert.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>

namespace boost {
namespace random {

/**
 * Instantiations of class template \bernoulli_distribution model a
 * \random_distribution. Such a random distribution produces bool values
 * distributed with probabilities P(true) = p and P(false) = 1-p. p is
 * the parameter of the distribution.
 */
template<class RealType = double>
class bernoulli_distribution
{
public:
    // In principle, this could work with both integer and floating-point
    // types.  Generating floating-point random numbers in the first
    // place is probably more expensive, so use integer as input.
    typedef int input_type;
    typedef bool result_type;

    class param_type
    {
    public:

        typedef bernoulli_distribution distribution_type;

        /**
         * Constructs the parameters of the distribution.
         *
         * Requires: 0 <= p <= 1
         */
        explicit param_type(RealType p_arg = RealType(0.5))
          : _p(p_arg)
        {
            BOOST_ASSERT(_p >= 0);
            BOOST_ASSERT(_p <= 1);
        }

        /** Returns the p parameter of the distribution. */
        RealType p() const { return _p; }

        /** Writes the parameters to a std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        {
            os << parm._p;
            return os;
        }

        /** Reads the parameters from a std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        {
            is >> parm._p;
            return is;
        }

        /** Returns true if the two sets of parameters are equal. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._p == rhs._p; }

        /** Returns true if the two sets of parameters are different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _p;
    };

    /**
     * Constructs a \bernoulli_distribution object.
     * p is the parameter of the distribution.
     *
     * Requires: 0 <= p <= 1
     */
    explicit bernoulli_distribution(const RealType& p_arg = RealType(0.5))
      : _p(p_arg)
    {
        BOOST_ASSERT(_p >= 0);
        BOOST_ASSERT(_p <= 1);
    }
    /**
     * Constructs \bernoulli_distribution from its parameters
     */
    explicit bernoulli_distribution(const param_type& parm)
      : _p(parm.p()) {}

    // compiler-generated copy ctor and assignment operator are fine

    /**
     * Returns: The "p" parameter of the distribution.
     */
    RealType p() const { return _p; }

    /** Returns the smallest value that the distribution can produce. */
    bool min BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return false; }
    /** Returns the largest value that the distribution can produce. */
    bool max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return true; }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_p); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm) { _p = parm.p(); }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

    /**
     * Returns: a random variate distributed according to the
     * \bernoulli_distribution.
     */
    template<class Engine>
    bool operator()(Engine& eng) const
    {
        if(_p == RealType(0))
            return false;
        else
            return RealType(eng() - (eng.min)()) <= _p * RealType((eng.max)()-(eng.min)());
    }

    /**
     * Returns: a random variate distributed according to the
     * \bernoulli_distribution with parameters specified by param.
     */
    template<class Engine>
    bool operator()(Engine& eng, const param_type& parm) const
    {
        return bernoulli_distribution(parm)(eng);
    }

    /**
     * Writes the parameters of the distribution to a @c std::ostream.
     */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, bernoulli_distribution, bd)
    {
        os << bd._p;
        return os;
    }

    /**
     * Reads the parameters of the distribution from a @c std::istream.
     */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, bernoulli_distribution, bd)
    {
        is >> bd._p;
        return is;
    }

    /**
     * Returns true iff the two distributions will produce identical
     * sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(bernoulli_distribution, lhs, rhs)
    { return lhs._p == rhs._p; }

    /**
     * Returns true iff the two distributions will produce different
     * sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(bernoulli_distribution)

private:
    RealType _p;
};

} // namespace random

using random::bernoulli_distribution;

} // namespace boost

#endif // BOOST_RANDOM_BERNOULLI_DISTRIBUTION_HPP


//
// Created by felix on 22.03.19.
//

#include <sensor_model_parameter_estimator_em.h>
#include <boost/thread/thread.hpp>
#include <boost/chrono.hpp>

using namespace tuw;

SensorModelParameterEstimatorEM::SensorModelParameterEstimatorEM() : params_( nullptr )
{
}

void SensorModelParameterEstimatorEM::clear()
{
  z_m_e_.clear();
}

void SensorModelParameterEstimatorEM::setParams( ParametersEstimated &params )
{
  params_ = std::make_shared<ParametersEstimated>( params );
}

void SensorModelParameterEstimatorEM::add( double measured, double expected )
{
  z_m_e_.push_back( std::make_pair( measured, expected ));
}

std::shared_ptr<SensorModelParameterEstimatorEM::ParametersEstimated> SensorModelParameterEstimatorEM::compute()
{
  if ( !params_ )
  {
    return nullptr;
  }

  printf( "EM on %ld measurements\n", z_m_e_.size());

  //dry run
  for ( auto p_zme : z_m_e_ )
  {
    double z_kt = p_zme.first;
    double z_kt_star = p_zme.second;

    //printf("(%lf, %lf)\n", z_kt, z_kt_star);
  }

  bool do_estimate = true;
  int iter_n = 0;
  do
  {
    ParametersEstimated cached_ = ParametersEstimated( *params_ );
    std::vector<double> e_hit;
    std::vector<double> e_short;
    std::vector<double> e_max;
    std::vector<double> e_rand;
    double hit_normalized_total = 0;
    double short_normalized_total = 0;

    for ( auto p_zme : z_m_e_ )
    {
      double z_kt = p_zme.first;
      double z_kt_star = p_zme.second;

      double p_hit = pHit( z_kt, z_kt_star, params_->sigma_hit );
      double p_short = pShort( z_kt, z_kt_star, params_->lambda_short );
      double p_max = pZmax( z_kt, params_->z_max );
      double p_rand = pRand( z_kt, params_->z_max );

      //printf( "(%lf, %lf)\n", z_kt, z_kt_star );
      //printf( "(%lf, %lf, %lf, %lf)\n", z_hit, z_short, z_max, z_rand );
      //printf( "zhit: %lf\n", params_->z_hit );
      //printf( "zmax: %lf\n", params_->z_max );
      //printf( "zshort: %lf\n", params_->z_short );
      //printf( "zrand: %lf\n", params_->z_rand );
      //printf( "sigma_hit: %lf\n", params_->sigma_hit );
      //printf( "lambda_short: %lf\n", params_->lambda_short );

      //boost::this_thread::sleep_for( boost::chrono::milliseconds( 500 ));

      double nu = 1.0 / (p_hit + p_short + p_max + p_rand);

      e_hit.push_back( nu * p_hit );
      e_short.push_back( nu * p_short );
      e_max.push_back( nu * p_max );
      e_rand.push_back( nu * p_rand );

      hit_normalized_total += (nu * p_hit * pow( z_kt - z_kt_star, 2 ));
      short_normalized_total += (nu * p_short * z_kt);
    }

    //TODO: make
    double hit_total = 0;
    double short_total = 0;
    double max_total = 0;
    double rand_total = 0;
    for ( int k = 0; k < e_hit.size(); ++k )
    {
      hit_total += e_hit[k];
      short_total += e_short[k];
      max_total += e_max[k];
      rand_total += e_rand[k];
    }

    params_->w_hit = hit_total / e_hit.size();
    params_->w_short = short_total / e_short.size();
    params_->w_max = max_total / e_max.size();
    params_->w_rand = rand_total / e_rand.size();

    params_->sigma_hit = std::sqrt(( 1.0 / hit_total ) * hit_normalized_total);
    params_->lambda_short = short_total / short_normalized_total;

    double energy = cached_.dotMinus( *params_ );
    printf( "Iteration: %d, ", iter_n++ );
    printf( "Energy: %lf\n", energy );

    do_estimate = energy > 0.001;
  } while ( do_estimate );

  //printf( "hit_total: %lf\n", hit_total );
  //printf( "short_total: %lf\n", short_total );
  //printf( "max_total: %lf\n", max_total );
  //printf( "rand_total: %lf\n", rand_total );
  printf("Estimated parameters\n");
  printf( "whit: %lf\n", params_->w_hit );
  printf( "wmax: %lf\n", params_->w_max );
  printf( "wshort: %lf\n", params_->w_short );
  printf( "wrand: %lf\n", params_->w_rand );
  printf( "sigma_hit: %lf\n", params_->sigma_hit );
  printf( "lambda_short: %lf\n", params_->lambda_short );

  return params_;
}


#include <cmath>
#include <iostream>
#include <vector>
#include <string>
#include <fstream>

#include "boost/filesystem.hpp"
#include "gflags/gflags.h"
#include "picojson.h"
#include <Eigen/Dense>

#include "bakuage/utils.h"
#include "bakuage/sound_quality.h"

DECLARE_string(analysis_data_dir);

void TestSoundQuality() {
    using boost::filesystem::recursive_directory_iterator;

    bakuage::SoundQualityCalculator calculator;

    recursive_directory_iterator last;
    for (recursive_directory_iterator itr(FLAGS_analysis_data_dir); itr != last; ++itr) {
        const std::string path = itr->path().string();
        if (!bakuage::StrEndsWith(path, ".json")) continue;

        std::ifstream ifs(path.c_str());
        std::string json_str((std::istreambuf_iterator<char>(ifs)),
                             std::istreambuf_iterator<char>());
        Eigen::VectorXd mean;
        Eigen::MatrixXd covariance;
        bakuage::SoundQualityCalculator::ParseReference(json_str.c_str(), &mean, &covariance);
        calculator.AddReference(json_str.c_str());
    }

    calculator.Prepare();

    for (recursive_directory_iterator itr(FLAGS_analysis_data_dir); itr != last; ++itr) {
        const std::string path = itr->path().string();
        if (!bakuage::StrEndsWith(path, ".json")) continue;

        std::ifstream ifs(path.c_str());
        std::string json_str((std::istreambuf_iterator<char>(ifs)),
                             std::istreambuf_iterator<char>());
        Eigen::VectorXd mean;
        Eigen::MatrixXd covariance;
        bakuage::SoundQualityCalculator::ParseReference(json_str.c_str(), &mean, &covariance);
        double sound_quality, lof;
        calculator.CalculateSoundQuality(mean, covariance, &sound_quality, &lof);
        std::cerr << path << " " << sound_quality << " " << lof << std::endl;
    }

    std::cerr << "border_lof " << calculator.border_lof() << std::endl;

}


#include <blitz/array.h>

using namespace blitz;

int main()
{
    Array<float,2> A(4,4), B(4,4);
    Array<int,2> C(4,4);

    prettyPrintFormat f1;
    prettyPrintFormat f2(true);

    string s1, s2, s3, s4;

    (A+B).prettyPrint(s1,f1);
    cout << s1 << endl;
    (A+B).prettyPrint(s2,f2);
    cout << s2 << endl;
    (A+B+C).prettyPrint(s3,f1);
    cout << s3 << endl;
    (A+5*C).prettyPrint(s4,f1);
    cout << s4 << endl;
    {
    prettyPrintFormat f2(true);
    string s5;
    (A+5*C+cos(B)*A+A+pow2(B)+C).prettyPrint(s5,f2);
    cout << s5 << endl;
    }

    return 0;
}


/*  Stream inserter for static_poly, with helpers.
 *  (C) Copyright Nick Matteo 2016.
 *  Use, modification and distribution are subject to 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)
 */
#ifndef NAM_STATIC_POLYNOMIAL_IO_HPP
#define NAM_STATIC_POLYNOMIAL_IO_HPP

#include <ostream>
#include <cmath> //isnormal, fabs
#include <boost/range/algorithm/find_if.hpp>
#include <boost/range/algorithm/count_if.hpp>
#include <boost/math/special_functions/relative_difference.hpp>
#include "static_poly.hpp"

/** Forward declarations for ostream inserter helpers **/
namespace smath {
   template <typename T>
   struct complex;
}

namespace boost { namespace math {
   template <class T>
   class quaternion;

   template <class T>
   class octonion;
}}

/** Helpers for the stream inserter **/
namespace detail {
   struct xpow {
      int i;
   };

   inline std::ostream& operator << (std::ostream& os, xpow x) {
      if (x.i == 1)
         os << 'x';
      else if (x.i > 1)
         os << "x^" << x.i;
      return os;
   }

   template <typename T>
   std::enable_if_t<std::is_floating_point<T>::value, bool>
   is_zero(T n) {
       if (!std::isnormal(n))
           return true; // zero or denorm, or NaN or ∞
       return std::fabs(n) < 1e-11;
   }

   template <typename T>
   std::enable_if_t<!std::is_floating_point<T>::value, bool>
   is_zero(T n) {
       return n == T{0};
   }

   template <typename T>
   bool is_zero(smath::complex<T> ct) {
       return is_zero(ct.real) && is_zero(ct.imag.value);
   }

   template <typename T>
   struct notone {
       T val;
   };

   template <typename T>
   notone<T> ifnotone(T val) {
       return {val};
   }

   template <typename T>
   std::enable_if_t<std::is_floating_point<T>::value, bool>
   isone(T n) {
      using boost::math::relative_difference;
      return relative_difference(1.0, n) < 1e-11;
   }

   template <typename T>
   std::enable_if_t<!std::is_floating_point<T>::value, bool>
   isone(T n) {
      return n == T{1};
   }

   template <typename T>
   bool isone(smath::complex<T> n) {
      return isone(n.real) && is_zero(n.imag.value);
   }

   template <typename T>
   std::ostream& operator << (std::ostream& os, notone<T> io) {
      if (isone(-io.val))
         return os << '-';
      if (isone(io.val))
         return os;
      return os << io.val;
   }

   template <typename T>
   bool is_negative(T t) {
       return t < T{0} && !is_zero(t);
   }

   template <typename T>
   bool is_negative(smath::complex<T> ct) {
      return (ct.real < T{0} && !is_zero(ct.real)) ||
            (is_zero(ct.real) && ct.imag.value < T{0} && !is_zero(ct.imag.value));
   }

   template <typename T>
   bool is_negative(boost::math::quaternion<T> q) {
      // only if the first nonzero element is negative, and the majority of
      // nonzero terms are negative.
      using boost::range::find_if;
      using boost::range::count_if;
      T vals[] = {q.R_component_1(), q.R_component_2(),
                  q.R_component_3(), q.R_component_4()};
      auto nonzero = find_if(vals, [](T val){ return !is_zero(val); });
      if (nonzero == boost::end(vals)) return false;
      return *nonzero < T{0} &&
             count_if(vals, [](T val){ return val < T{0} && !is_zero(val); }) >=
             count_if(vals, [](T val){ return val > T{0} && !is_zero(val); });
   }

   template <typename T>
   bool is_negative(boost::math::octonion<T> q) {
      // only if the first nonzero element is negative, and the majority of
      // nonzero terms are negative.
      using boost::range::find_if;
      using boost::range::count_if;
      T vals[] = {q.R_component_1(), q.R_component_2(),
                  q.R_component_3(), q.R_component_4(),
                  q.R_component_5(), q.R_component_6(),
                  q.R_component_7(), q.R_component_8()};
      auto nonzero = find_if(vals, [](T val){ return !is_zero(val); });
      if (nonzero == boost::end(vals)) return false;
      return *nonzero < T{0} &&
             count_if(vals, [](T val){ return val < T{0} && !is_zero(val); }) >=
             count_if(vals, [](T val){ return val > T{0} && !is_zero(val); });
   }
} // close namespace detail

/** Output for smath::complex **/

namespace smath { // so it can be found
    template <typename T>
    std::ostream& operator << (std::ostream& os, const smath::complex<T>& cd) {
        using ::detail::is_zero;
        using ::detail::ifnotone;
        if (is_zero(cd.real)) {// real part is zero or denorm (or ∞, NaN)
            if (is_zero(cd.imag.value))
                return os << '0';
            return os << ifnotone(cd.imag.value) << 'i';
        }
        if (is_zero(cd.imag.value)) // imag part is zero or denorm (or ∞, NaN)
            return os << cd.real;
        os << '(' << cd.real;
        if (cd.imag.value < 0.)
            os << " - " << ifnotone(-cd.imag.value);
        else
            os << " + " << ifnotone(cd.imag.value);
        return os << "i)";
    }
}

template <class T, int N>
inline std::ostream& operator << (std::ostream& os, const static_poly<T, N>& poly) {
   using namespace detail;

   int i = poly.degree();
   if (i == -1)
      return os << '0';
   if (i == 0)
      return os << poly[0];

   os << ifnotone(poly[i]) << xpow{i};

   for (--i; i > 0; --i) {
      if (is_negative(poly[i]))
         os << " - " << ifnotone(-poly[i]) << xpow{i};
      else if (!is_zero(poly[i]))
         os << " + " << ifnotone(poly[i]) << xpow{i};
   }

   if (is_negative(poly[0]))
      os << " - " << -poly[0];
   else if (poly[0] != T{0})
      os << " + " << poly[0];
   return os;
}

#endif // NAM_STATIC_POLYNOMIAL_IO_HPP


// run example options : -b "-5,5,-5,5,0,0.1" -r "20,20,20" -o out.toto ./small_cylindre.cdf
//
#include <Discregrid/All>
#include <Eigen/Dense>
#include <cxxopts/cxxopts.hpp>

#include <string>
#include <iostream>
#include <array>
#include "cell.h"
#include "grid3D.h"
#include "mesh.h"
#include "constants.h"

using namespace Eigen;

std::istream& operator>>(std::istream& is, std::array<unsigned int, 3>& data)
{
  is >> data[0] >> data[1] >> data[2];
  return is;
}

std::istream& operator>>(std::istream& is, std::array<double, 3>& data)
{
  is >> data[0] >> data[1] >> data[2] ;
  return is;
}

std::istream& operator>>(std::istream& is, std::array<double, 6>& data)
{
  is >> data[0] >> data[1] >> data[2] >> data[3] >> data[4] >> data[5] >> data[6];
  return is;
}

//resolution and sample is redondant, however i need it in order to check the sampling method of "integration"
// in order to determine if a cell is a fluid cell or a solid cell
void sdfToVTK(std::string filename, std::unique_ptr<Discregrid::DiscreteGrid> &sdf, unsigned int field_id,
              std::vector<unsigned int> resolution, std::vector<int> &sample, std::vector<double> bbox);

int main(int argc, char* argv[])
{
	cxxopts::Options options(argv[0], "Transforms a discrete SDF to a symetric voxel file.");
	options.positional_help("[input SDF file]");

	options.add_options()
	("h,help", "Prints this help text")
	("f,field_id", "ID in which the SDF to export is stored.", cxxopts::value<unsigned int>()->default_value("0"))
	("r,resolution", "Grid resolution", cxxopts::value<std::vector<unsigned int>>()->default_value("10,10,10"))
	("s,psymetry", "Symetry point", cxxopts::value<std::array<double, 3>>()->default_value("0. 0. 0."))
	("b,bbox", "bounding box", cxxopts::value<std::vector<double>>()->default_value("0.,0.,0.,0.,0.,0."))
	("n,nSample", "Number of samples per cell", cxxopts::value<std::vector<unsigned int>>()->default_value("10,10,10"))
	("o,output", "Output file", cxxopts::value<std::string>()->default_value(""))
	("input", "SDF file", cxxopts::value<std::vector<std::string>>())
	;

	try
	{
		options.parse_positional("input");
		auto result = options.parse(argc, argv);

		if (result.count("help"))
		{
			std::cout << options.help() << std::endl;
			std::cout << std::endl << std::endl << "Example: SDFToBitmap -p xz file.sdf" << std::endl;
			exit(0);
		}
		if (!result.count("input"))
		{
			std::cout << "ERROR: No input file given." << std::endl;
			std::cout << options.help() << std::endl;
			std::cout << std::endl << std::endl << "Example: SDFToBitmap -p xz file.sdf" << std::endl;
			exit(1);
		}

		auto sdf = std::unique_ptr<Discregrid::DiscreteGrid>{};

		auto filename = result["input"].as<std::vector<std::string>>().front();
		auto lastindex = filename.find_last_of(".");
		auto extension = filename.substr(lastindex + 1, filename.length() - lastindex);

		std::cout << "Load SDF...";
		if (extension == "cdf" || extension == "cdm")
		{
			sdf = std::unique_ptr<Discregrid::CubicLagrangeDiscreteGrid>(
				new Discregrid::CubicLagrangeDiscreteGrid(filename));
		}
		std::cout << "DONE" << std::endl;

		auto const& domain = sdf->domain();
    auto resolution = result["r"].as<std::vector<unsigned int>>();
    auto psym = result["s"].as<std::array<double, 3>>();
		auto field_id = result["f"].as<unsigned int>();
    auto bbox = result["b"].as<std::vector<double>>();
    auto nbSamples = result["n"].as<std::vector<unsigned int>>();
    auto sample = std::vector<int>{(int)nbSamples[0],(int)nbSamples[1],(int)nbSamples[2]};

    if (!result.count("bbox")) {
      bbox= {domain.min()(0),domain.max()(0),
             domain.min()(1),domain.max()(1),
             domain.min()(2),domain.max()(2)};
    }

    //Creation of the mesh, considering the bounding box and the resolution
    mesh dm(bbox,resolution);

    dm.printResolution();
    dm.printBbox();
    dm.printGridSize();

    std::cout << "symetry point : ";
    std::copy(std::begin(psym), std::end(psym), std::ostream_iterator<double>(std::cout, " "));
    std::cout << std::endl;

    std::cout << "Nb samples per cell : ";
    std::copy(std::begin(sample), std::end(sample), std::ostream_iterator<int>(std::cout, " "));
    std::cout << std::endl;

    // At first, we will not take into account the symetry point
    // Thus, the total number of cells known.
    grid3D<cell> grid (resolution[0],resolution[1],resolution[2]);

    std::cout << "Domaine total size : " << grid.size() << std::endl;
    dm.initCorrdsConstantStep();
    {
      std::vector<double> dims(6,0);

      for (unsigned int i=0; i < grid.getXdim(); ++i) {
        for (unsigned int j=0; j < grid.getYdim(); ++j) {
          for (unsigned int k=0; k < grid.getZdim(); ++k) {
            dims={dm.getXcoord(i),dm.getXcoord(i+1),
                  dm.getYcoord(j),dm.getYcoord(j+1),
                  dm.getZcoord(k),dm.getZcoord(k+1)};
            grid(i,j,k).setDims(dims);
          }
        }
      }
    }

    std::cout << "dims of (0,0,0) : " << std::endl;
	  grid(0,0,0).printDims();
    std::cout << std::endl;

    // init the fields of all the cells
    #pragma omp parallel for default(none) shared(resolution,field_id,sample,grid,sdf)
    for (unsigned int i=0; i < grid.getXdim(); ++i) {
      for (unsigned int j = 0; j < grid.getYdim(); ++j) {
        for (unsigned int k = 0; k < grid.getZdim(); ++k) {
          grid(i,j,k).initSdfMeanValue(sdf,field_id,sample);
        }
      }
    }

    std::cout << "Statistics of the first cell (0,0,0) " << std::endl;
    std::cout << "MeanValue : " << grid(0,0,0).getSdfMeanValue() << std::endl;
    std::cout << "ratioInsideOverTotal : " << grid(0,0,0).getRatioInsideOverTotal() << std::endl;

    std::cout << "Statistics of the first cell (1,1,0) " << std::endl;
    std::cout << "MeanValue : " << grid(1,1,0).getSdfMeanValue() << std::endl;
    std::cout << "ratioInsideOverTotal : " << grid(1,1,0).getRatioInsideOverTotal() << std::endl;

    auto ycellwidth = (bbox[YMAX]-bbox[YMIN])/resolution[1];
    auto zcellwidth = (bbox[ZMAX]-bbox[ZMIN])/resolution[2];
    double posX=0;
    double posZ=2*zcellwidth/sample[2];
    auto Pt = Vector3d{};
    Pt={posX,0.5-4*ycellwidth/9,posZ};
    std::cout << "value of point (" << Pt[0] << "," << Pt[1] << ","<< Pt[2] <<") : " << sdf->interpolate(field_id,Pt) << std::endl;
    Pt={posX,0.5-3*ycellwidth/9,posZ};
    std::cout << "value of point (" << Pt[0] << "," << Pt[1] << ","<< Pt[2] <<") : " << sdf->interpolate(field_id,Pt) << std::endl;
    Pt={posX,0.5-2*ycellwidth/9,posZ};
    std::cout << "value of point (" << Pt[0] << "," << Pt[1] << ","<< Pt[2] <<") : " << sdf->interpolate(field_id,Pt) << std::endl;
    Pt={posX,0.5-ycellwidth/9,posZ};
    std::cout << "value of point (" << Pt[0] << "," << Pt[1] << ","<< Pt[2] <<") : " << sdf->interpolate(field_id,Pt) << std::endl;
    Pt={posX,0.5,posZ};
    std::cout << "value of point (" << Pt[0] << "," << Pt[1] << ","<< Pt[2] <<") : " << sdf->interpolate(field_id,Pt) << std::endl;
    Pt={posX,0.5+ycellwidth/9,posZ};
    std::cout << "value of point (" << Pt[0] << "," << Pt[1] << ","<< Pt[2] <<") : " << sdf->interpolate(field_id,Pt) << std::endl;
    Pt={posX,0.5+2*ycellwidth/9,posZ};
    std::cout << "value of point (" << Pt[0] << "," << Pt[1] << ","<< Pt[2] <<") : " << sdf->interpolate(field_id,Pt) << std::endl;
    Pt={posX,0.5+3*ycellwidth/9,posZ};
    std::cout << "value of point (" << Pt[0] << "," << Pt[1] << ","<< Pt[2] <<") : " << sdf->interpolate(field_id,Pt) << std::endl;
    Pt={posX,0.5+4*ycellwidth/9,posZ};
    std::cout << "value of point (" << Pt[0] << "," << Pt[1] << ","<< Pt[2] <<") : " << sdf->interpolate(field_id,Pt) << std::endl;
    std::cout << std::endl;

    for (unsigned int i=0; i<sample[2]; ++i) {
      Pt={posX,0.5,bbox[ZMIN]+i*zcellwidth/((double)sample[2]-1)};
      std::cout << "value of point (" << Pt[0] << "," << Pt[1] << ","<< Pt[2] <<") : " << sdf->interpolate(field_id,Pt) << std::endl;
    }
    std::cout << std::endl;

    sdfToVTK("sdf.vtk", sdf, field_id, resolution, sample, bbox);
    grid.saveGrid("toto.txt");

	}
	catch (cxxopts::OptionException const& e)
	{
		std::cout << "error parsing options: " << e.what() << std::endl;
		exit(1);
	}

	return 0;
}

//resolution and sample is redondant, however i need it in order to check the sampling method of "integration"
// in order to determine if a cell is a fluid cell or a solid cell
void sdfToVTK(std::string filename, std::unique_ptr<Discregrid::DiscreteGrid> &sdf, unsigned int field_id,
              std::vector<unsigned int> resolution, std::vector<int> &sample, std::vector<double> bbox) {

  //save the SDF in a vtk file
  std::cout << "Save vtk for sdf" << std::endl;

  //seek the total number of point given the resolution and the sample size.
  //the resolution gives the number of cells, the sample size the number of samples in the cell.
  int sizeX =  resolution[0]*(sample[0]-1);
  int sizeY =  resolution[1]*(sample[1]-1);
  int sizeZ =  resolution[2]*(sample[2]-1);
  double xwidth=(bbox[XMAX]-bbox[XMIN])/sizeX;
  double ywidth=(bbox[YMAX]-bbox[YMIN])/sizeY;
  double zwidth=(bbox[ZMAX]-bbox[ZMIN])/sizeZ;
  sizeX++;sizeY++;sizeZ++;

  //we construct the points of the structure grid
  auto x=new double [sizeX];
  auto y=new double [sizeY];
  auto z=new double [sizeZ];
  for (unsigned int i=0; i<sizeX; ++i) {
    x[i] = bbox[XMIN] + i*xwidth;
  }
  for (unsigned int i=0; i<sizeY; ++i) {
    y[i] = bbox[YMIN] + i*ywidth;
  }
  for (unsigned int i=0; i<sizeZ; ++i) {
    z[i] = bbox[ZMIN] + i*zwidth;
  }

  auto evaluatedPoint = Vector3d{};
  grid3D<double> forVTK(sizeX,sizeY,sizeZ);
  #pragma omp parallel for default(none) shared(sizeX,sizeY,sizeZ,field_id,forVTK,sdf,x,y,z) private(evaluatedPoint)
  for (unsigned int k=0; k<sizeZ; ++k) {
    for (unsigned int j = 0; j < sizeY; ++j) {
      for (unsigned int i = 0; i < sizeX; ++i) {
        evaluatedPoint={x[i],y[j],z[k]};
        forVTK(i,j,k) = sdf->interpolate(field_id,evaluatedPoint);
      }
    }
  }

  std::cout << "sizeX " << sizeX << " sizeY " << sizeY << " sizeZ " << sizeZ << std::endl;
  std::cout << "xwidth " << xwidth << " ywidth " << ywidth << " zwidth " << zwidth << std::endl;

  //we write the file
  std::ofstream outfile;
  outfile.open(filename);
  outfile << "# vtk DataFile Version 2.0" << std::endl;
  outfile << "Value of the signed distance function" << std::endl;
  outfile << "ASCII" << std::endl;
  outfile << "DATASET STRUCTURED_POINTS" << std::endl;
  outfile << "DIMENSIONS " << sizeX << " " << sizeY << " " << sizeZ << std::endl;
  outfile << "SPACING " << xwidth << " " << ywidth << " " << zwidth << std::endl;
  outfile << "ORIGIN " << bbox[XMIN] << " " << bbox[YMIN] << " " << bbox[ZMIN] << std::endl;
  outfile << "POINT_DATA " << sizeX*sizeY*sizeZ << std::endl;
  outfile << "SCALARS sdf double 1" << std::endl;
  outfile << "LOOKUP_TABLE default" << std::endl;
  for (unsigned int k=0; k<sizeZ; ++k) {
    for (unsigned int j = 0; j < sizeY; ++j) {
      for (unsigned int i = 0; i < sizeX; ++i) {
        outfile << forVTK(i,j,k) << " ";
      }
      outfile << std::endl;
    }
  }
  outfile.close();

  std::cout << "End of saving vtk" << std::endl;

}


#include <cassert>
#include <iostream>
#include <iomanip>
#include <vector>
#include <sstream>
#include <boost/algorithm/string.hpp>

const std::string PUZZLE_INPUT = "187,254,0,81,169,219,1,190,19,102,255,56,46,32,2,216";

std::vector<size_t> to_vector(const std::string &str)
{
    std::vector<std::string> parts;
    boost::split(parts, str, boost::is_any_of(","));

    std::vector<size_t> result;
    result.reserve(parts.size());

    for (const auto &part : parts)
        result.push_back(std::atoi(part.c_str()));

    return result;
}

void twist(std::vector<size_t> &loop, size_t cur_pos, size_t twist_size)
{
    for (size_t i = 0; i < (twist_size / 2); ++i)
    {
        size_t ia = (cur_pos + i) % loop.size();
        size_t ib = (cur_pos + twist_size - 1 - i) % loop.size();

        std::swap(loop[ia], loop[ib]);
    }
}

std::vector<size_t> hash_rounds(size_t size, size_t num_rounds, const std::vector<size_t> &lengths)
{
    std::vector<size_t> loop;

    loop.reserve(size);
    for (size_t i = 0; i < size; ++i)
        loop.push_back(i);

    size_t cur_pos = 0;
    size_t skip_size = 0;

    for (size_t round = 0; round < num_rounds; ++round)
    {
        for (size_t twist_size : lengths)
        {
            twist(loop, cur_pos, twist_size);
            cur_pos = (cur_pos + twist_size + skip_size) % size;
            skip_size += 1;
        }
    }

    return loop;
}

int hash1(size_t size, const std::vector<size_t> &lengths)
{
    auto loop = hash_rounds(size, 1, lengths);

    return loop[0] * loop[1];
}

std::string hash2(const std::string &str)
{
    std::vector<size_t> lengths;
    lengths.reserve(str.size() + 5);

    for (char ch : str)
        lengths.push_back(ch);

    lengths.push_back(17);
    lengths.push_back(31);
    lengths.push_back(73);
    lengths.push_back(47);
    lengths.push_back(23);

    std::vector<size_t> loop = hash_rounds(256, 64, lengths);

    assert(loop.size() == 256);

    std::stringstream stream;

    for (size_t i = 0; i < 16; ++i)
    {
        size_t dense_entry = 0;

        for (size_t j = 0; j < 16; ++j)
        {
            dense_entry ^= loop[16*i + j];
        }

        stream << std::hex << std::setfill('0') << std::setw(2) << dense_entry;
    }

    return stream.str();
}

int main(int /*argc*/, const char */*argv*/[])
{
    assert(hash1(5, {3, 4, 1, 5}) == 12);

    assert(hash2("") == "a2582a3a0e66e6e86e3812dcb672a272");
    assert(hash2("AoC 2017") == "33efeb34ea91902bb2f59c9920caa6cd");
    assert(hash2("1,2,3") == "3efbe78a8d82f29979031a4aa0b16a9d");
    assert(hash2("1,2,4") == "63960835bcdc130f0b66d7ff4f6a5a8e");

    std::cout << "Answer #1: " << hash1(256, to_vector(PUZZLE_INPUT)) << std::endl;
    std::cout << "Answer #2: " << hash2(PUZZLE_INPUT) << std::endl;

    return 0;
}


/*
 * StokesM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <iomanip>
#include <iostream>

#define DIRECTLAYER 2

namespace Stokes1D3D {

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */
template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

inline void Gkernel(const Eigen::Vector3d &target,
                    const Eigen::Vector3d &source, Eigen::Matrix3d &answer) {
    auto rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < 1e-13) {
        answer.setZero();
        return;
    }

    auto part2 = rst * rst.transpose() / (rnorm * rnorm * rnorm);
    auto part1 = Eigen::Matrix3d::Identity() / rnorm;
    answer = part1 + part2;
}

// calculate the M2L matrix of images from 2 to 1000
int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);

    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {
        -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {
        -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};
    auto pointMEquiv = surface(
        pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(
        pCheck, (double *)&(pCenterCheck[0]), scaleCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    auto pointLEquiv = surface(
        pEquiv, (double *)&(pCenterCheck[0]), scaleCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointLCheck = surface(
        pCheck, (double *)&(pCenterEquiv[0]), scaleEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    //	for (int i = 0; i < pointLEquiv.size() / 3; i++) {
    //		std::cout << pointLEquiv[3 * i] << " " << pointLEquiv[3 * i + 1]
    //<< " " << pointLEquiv[3 * i + 2] << " "
    //				<< std::endl;
    //	}
    //
    //	for (int i = 0; i < pointLCheck.size() / 3; i++) {
    //		std::cout << pointLCheck[3 * i] << " " << pointLCheck[3 * i + 1]
    //<< " " << pointLCheck[3 * i + 2] << " "
    //				<< std::endl;
    //	}

    const int imageN = 500000; // images to sum
    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd A(3 * checkN, 3 * equivN);
#pragma omp parallel for
    for (int k = 0; k < checkN; k++) {
        Eigen::Matrix3d G = Eigen::Matrix3d::Zero();
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                               pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l],
                                         pointLEquiv[3 * l + 1],
                                         pointLEquiv[3 * l + 2]);
            Gkernel(Cpoint, Lpoint, G);
            A.block<3, 3>(3 * k, 3 * l) = G;
        }
    }
    Eigen::MatrixXd ApinvU(A.cols(), A.rows());
    Eigen::MatrixXd ApinvVT(A.cols(), A.rows());
    pinv(A, ApinvU, ApinvVT);

    Eigen::MatrixXd M2L(3 * equivN, 3 * equivN);
#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1],
                                     pointMEquiv[3 * i + 2]);
        Eigen::MatrixXd f(3 * checkN, 3);
        //		std::cout<<"debug:"<<Mpoint<<std::endl;
        for (int k = 0; k < checkN; k++) {
            Eigen::Matrix3d temp = Eigen::Matrix3d::Zero();
            Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                                   pointLCheck[3 * k + 2]);
            //			std::cout<<"debug:"<<k<<std::endl;
            // sum the images
            for (int per = DIRECTLAYER + 1; per < imageN; per++) {
                Eigen::Vector3d perVec(1.0 * per, 0, 0);
                Eigen::Matrix3d G1 = Eigen::Matrix3d::Zero();
                Eigen::Matrix3d G2 = Eigen::Matrix3d::Zero();
                Gkernel(Cpoint, Mpoint + perVec, G1);
                Gkernel(Cpoint, Mpoint - perVec, G2);
                temp = temp + (G1 + G2);
            }
            //			std::cout << temp << std::endl;
            f.block<3, 3>(3 * k, 0) = temp;
        }
        M2L.block(0, 3 * i, 3 * equivN, 3) =
            (ApinvU.transpose() * (ApinvVT.transpose() * f));
    }

    // dump M2L
    for (int i = 0; i < 3 * equivN; i++) {
        for (int j = 0; j < 3 * equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific
                      << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forcePoint(3);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forceValue(3);
    forcePoint[0] = Eigen::Vector3d(0.5, 0.55, 0.2);
    forceValue[0] = Eigen::Vector3d(-0.2, 0, 0);
    forcePoint[1] = Eigen::Vector3d(0.5, 0.5, 0.5);
    forceValue[1] = Eigen::Vector3d(1, 0, 0);
    forcePoint[2] = Eigen::Vector3d(0.7, 0.7, 0.7);
    forceValue[2] = Eigen::Vector3d(-0.8, 0, 0);

    // solve M
    A.resize(3 * checkN, 3 * equivN);
    ApinvU.resize(A.cols(), A.rows());
    ApinvVT.resize(A.cols(), A.rows());
    Eigen::VectorXd f(3 * checkN);
#pragma omp parallel for
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d temp = Eigen::Vector3d::Zero();
        Eigen::Matrix3d G = Eigen::Matrix3d::Zero();
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1],
                               pointMCheck[3 * k + 2]);
        for (int p = 0; p < 3; p++) {
            Gkernel(Cpoint, forcePoint[p], G);
            temp = temp + G * (forceValue[p]);
        }
        f.block<3, 1>(3 * k, 0) = temp;
        for (int l = 0; l < equivN; l++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1],
                                   pointMEquiv[3 * l + 2]);
            Gkernel(Cpoint, Mpoint, G);
            A.block<3, 3>(3 * k, 3 * l) = G;
        }
    }
    pinv(A, ApinvU, ApinvVT);
    Eigen::VectorXd Msource = (ApinvU.transpose() * (ApinvVT.transpose() * f));
    // impose zero sum
    double fx = 0, fy = 0, fz = 0;
    for (int i = 0; i < equivN; i++) {
        fx += Msource[3 * i];
        fy += Msource[3 * i + 1];
        fz += Msource[3 * i + 2];
    }
    std::cout << "fx svd before correction: " << fx << std::endl;
    std::cout << "fy svd before correction: " << fy << std::endl;
    std::cout << "fz svd before correction: " << fz << std::endl;

    fx /= equivN;
    fy /= equivN;
    fz /= equivN;
    for (int i = 0; i < equivN; i++) {
        Msource[3 * i] -= fx;
        Msource[3 * i + 1] -= fy;
        Msource[3 * i + 2] -= fz;
    }

    std::cout << "Msource: " << Msource << std::endl;

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forcePointExt(0);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forceValueExt(0);
    for (size_t p = 0; p < forcePoint.size(); p++) {
        for (int k = -DIRECTLAYER; k < DIRECTLAYER + 1; k++) {
            forcePointExt.push_back(Eigen::Vector3d(k, 0, 0) + forcePoint[p]);
            forceValueExt.push_back(forceValue[p]);
        }
    }

    Eigen::VectorXd M2Lsource = M2L * (Msource);
    Eigen::Vector3d samplePoint(0.6, 0.5, 0.5);
    Eigen::Vector3d Usample(0, 0, 0);
    Eigen::Vector3d UsampleSP(0, 0, 0);
    Eigen::Matrix3d G;
    for (size_t p = 0; p < forcePointExt.size(); p++) {
        Gkernel(samplePoint, forcePointExt[p], G);
        Usample += G * (forceValueExt[p]);
    }
    std::cout << "Usample Direct:" << Usample << std::endl;

    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1],
                               pointLEquiv[3 * p + 2]);
        Eigen::Vector3d Fpoint(M2Lsource[3 * p], M2Lsource[3 * p + 1],
                               M2Lsource[3 * p + 2]);
        Gkernel(samplePoint, Lpoint, G);
        UsampleSP = UsampleSP + G * (Fpoint);
    }

    std::cout << "Usample M2L:" << UsampleSP << std::endl;
    std::cout << "Usample M2L total:" << UsampleSP + Usample << std::endl;

    Eigen::Vector3d UsampleN(0, 0, 0);
    for (size_t p = 0; p < forcePoint.size(); p++) {
        Eigen::Matrix3d G1 = Eigen::Matrix3d::Zero();
        Eigen::Matrix3d G2 = Eigen::Matrix3d::Zero();
        for (int per = DIRECTLAYER + 1; per < imageN; per++) {
            Eigen::Vector3d perVec(1.0 * per, 0, 0);
            Gkernel(samplePoint, forcePoint[p] + perVec, G1);
            Gkernel(samplePoint, forcePoint[p] - perVec, G2);
            UsampleN += (G1 + G2) * (forceValue[p]);
        }
        Gkernel(samplePoint, forcePoint[p], G2);
    }

    std::cout << "Usample N Images:" << UsampleN + Usample << std::endl;
    std::cout << "error" << UsampleSP - UsampleN << std::endl;

    return 0;
}

} // namespace Stokes1D3D

#undef DIRECTLAYER