/*
The MIT License

Copyright (c) 2015-2017 Albert Murienne

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include "edge_detect.h"

#include <boost/type_traits/is_same.hpp>
#include <boost/math/special_functions/pow.hpp>

#include <iostream>

using namespace cimg_library;

extern "C" {
	#include "ccv.h"
}

////////////////////////////////////// SOBEL CCV /////////////////////////////////////////////////

template<> void sobel_ccv::process<unsigned char>( const CImg<unsigned char>& image_in, CImg<unsigned char>& image_out )
{
	ccv_dense_matrix_t* ccv_image_in =
		ccv_dense_matrix_new( image_in.height(), image_in.width(), CCV_8U | CCV_C1 | CCV_NO_DATA_ALLOC, (void*)image_in.data(), 0);
	ccv_image_in->step = image_in.width() * sizeof(unsigned char);

	ccv_dense_matrix_t* ccv_image_out =
		ccv_dense_matrix_new( image_out.height(), image_out.width(), CCV_8U | CCV_C1 | CCV_NO_DATA_ALLOC, (void*)image_out.data(), 0);
	ccv_image_out->step = image_out.width() * sizeof(unsigned char);

	ccv_sobel( ccv_image_in, &ccv_image_out, CCV_8U, 0, 1 );

	//CImg<float> test( ccv_image_out->data.f32, 50, 50, 1, 1 );
	//test.display();
}

////////////////////////////////////// CANNY CCV /////////////////////////////////////////////////

template<> void canny_ccv::process<unsigned char>( const CImg<unsigned char>& image_in, CImg<unsigned char>& image_out )
{
	// http://www.kerrywong.com/2009/05/07/canny-edge-detection-auto-thresholding/
	unsigned char mean = image_in.mean();
	unsigned char low_thresh = 0.66 * mean;
	unsigned char high_thresh = 1.33 * mean;

	ccv_dense_matrix_t* ccv_image_in =
		ccv_dense_matrix_new( image_in.height(), image_in.width(), CCV_8U | CCV_C1 | CCV_NO_DATA_ALLOC, (void*)image_in.data(), 0);
	ccv_image_in->step = image_in.width() * sizeof(unsigned char);

	ccv_dense_matrix_t* ccv_image_out =
		ccv_dense_matrix_new( image_out.height(), image_out.width(), CCV_8U | CCV_C1 | CCV_NO_DATA_ALLOC, (void*)image_out.data(), 0);
	ccv_image_out->step = image_out.width() * sizeof(unsigned char);

	ccv_canny( ccv_image_in, &ccv_image_out, CCV_8U, 1, low_thresh, high_thresh );
}

////////////////////////////////////// SOBEL /////////////////////////////////////////////////

// use with normalized [0,1] floating point images
template<typename T>
void sobel::process( const CImg<T>& image_in, CImg<T>& image_out )
{
	//static_assert( boost::is_same<T,float>::value || boost::is_same<T,double>::value,
	//	"Template type should be floating point type!" );

	T upper_bound = 1;
	T lower_bound = 0;
	T sum;
	T sumX, sumY;

	T GX[3][3];
	T GY[3][3];

	//Sobel Matrices Horizontal
	GX[0][0] = 1; GX[0][1] = 0; GX[0][2] = -1;
	GX[1][0] = 2; GX[1][1] = 0; GX[1][2] = -2;
	GX[2][0] = 1; GX[2][1] = 0; GX[2][2] = -1;

	//Sobel Matrices Vertical
	GY[0][0] =  1; GY[0][1] =	 2; GY[0][2] =   1;
	GY[1][0] =  0; GY[1][1] =	 0; GY[1][2] =   0;
	GY[2][0] = -1; GY[2][1] =	-2;	GY[2][2] =  -1;

	/*Edge detection using Sobel Algorithm*/

	for( int y = 0; y < image_in.height() ; y++)
	{
		for( int x = 0; x < image_in.width() ; x++)
		{
			sumX	= 0;
			sumY	= 0;

			/*Image Boundaries*/
			if( y == 0 || y == image_in.height() - 1 )
				sum = 0;
			else if( x == 0 || x == image_in.width() - 1 )
				sum = 0;
			else
			{
				/*Convolution for X*/
				for( int i = -1; i < 2; i++ )
				{
					for( int j = -1; j < 2; j++ )
					{
						sumX = sumX + GX[j+1][i+1] * image_in(x+j,y+i);
					}
				}

				/*Convolution for Y*/
				for( int i = -1; i < 2; i++ )
				{
					for( int j = -1; j < 2; j++ )
					{
						sumY = sumY + GY[j+1][i+1] * image_in(x+j,y+i);
					}
				}

				/*Edge strength*/
				sum = std::sqrt( boost::math::pow<2>( sumX ) + boost::math::pow<2>( sumY ) );
			}

			if(sum > upper_bound) sum = upper_bound;
			if(sum < lower_bound) sum = lower_bound;

			image_out(x,y) = sum;//( upper_bound - sum );

			//std::cout << "x " << x << " y " << y << " SUM " << image_out(x,y) << std::endl;
		}
	}
}

template void sobel::process<float>( const CImg<float>& image_in, CImg<float>& image_out );

////////////////////////////////////// CANNY /////////////////////////////////////////////////

#define MAX_SIZE 5

//***************************
// helper function that returns true if a>b and c
//***************************
bool is_first_max( int a, int b, int c )
{
	return ( a>b && a>c );
}

//***************************
// convolve is a general helper funciton that applies a convolution
// to the image and then returns the weighted sum so that
// it can replace whatever pixel we were just analyzing
//**************************
template<typename T,const int dim>
T convolve( const CImg<T>& image_in, T con[][MAX_SIZE], T divisor, int i, int j )
{
    int midx = dim/2;
    int midy = dim/2;

	T weighted_sum = 0;
	for( int x = i-midx; x < i + dim-midx; x++ )
	{
		for( int y = j-midy; y < j + dim-midy; y++ )
		{
			weighted_sum += divisor * con[x-i+midx][y-j+midy] * image_in(x,y);
		}
	}
	return weighted_sum;
}

//*****************************
//helper function that says whether arg is between a-b or c-d
//*****************************
bool is_between( float arg, float a, float b, float c, float d )
{
	return ( ( arg >= a && arg <= b ) || ( arg >= c && arg <= d ) );
}

//****************************
// buckets the thetas into 0, 45, 90, 135
//****************************
int get_orientation( float angle )
{
	if( is_between( angle, -22.5, 22.5, -180, -157.5 ) || is_between( angle, 157.5, 180, -22.5, 0 ) )
		return 0;
	if( is_between( angle, 22.5, 67.5, -157.5, -112.5 ) )
		return 45;
	if( is_between( angle, 67.5, 112.5, -112.5, -67.5) )
		return 90;
	if( is_between( angle, 112.5, 157.5, -67.5, -22.5  ) )
		return 135;

	return -1;
}

template<typename T>
canny<T>::canny( const int& columns, const int& rows )
	: m_rows( rows ), m_columns( columns ),
	m_low_thresh( 0 ), m_high_thresh( 0 ),
	m_thetas( boost::extents[columns][rows] ), m_mag_array( boost::extents[columns][rows] )
{
	static_assert( boost::is_same<T,float>::value || boost::is_same<T,double>::value,
		"Template type should be floating point type!" );
}

template canny<float>::canny( const int& rows, const int& columns );

template<typename T>
canny<T>::~canny()
{
}

template canny<float>::~canny();

//*****************************
// gaussian blur
// applies a gaussian blur via a convolution of a gaussian
// matrix with sigma = 1.4. hard-coded in.
// future development could generate the gauss matrix on the fly
//*****************************
template<typename T>
void canny<T>::_gaussian_blur( const CImg<T>& image_in, CImg<T>& image_out )
{
	// define gauss matrix
	T gauss_array[5][5] = {	{2, 4, 5, 4, 2},
							{4, 9, 12,9, 4},
							{5, 12, 15, 12, 5},
							{4, 9, 12,9, 4},
							{2, 4, 5, 4, 2} };

	T gauss_divisor = 1.0/159.0;
	T sum = 0.0;

	for( auto j=2U; j < m_rows-2; j++ )
	{
		for( auto i=2U; i < m_columns-2; i++ )
		{
			sum = convolve<T,5>( image_in, gauss_array, gauss_divisor, i, j );
			image_out(i,j) = sum;
		}
	}
}

//****************************
// Applies a sobel filter to find the gradient direction
// and magnitude. those values are then stored in thetas and magArray
// so that info can be used later for further analysis
//****************************
template<typename T>
void canny<T>::_sobel( CImg<T>& image )
{
	T G_x, G_y, G;
	T sobel_y[5][5] = {	{-1, 0, 1,0,0},
						{-2, 0, 2,0,0},
						{-1, 0, 1,0,0},
						{0, 0, 0, 0, 0},
						{0, 0, 0, 0, 0} };

	T sobel_x[5][5] = {	{1, 2, 1, 0, 0},
						{0, 0, 0, 0, 0},
						{-1, -2, -1, 0, 0},
						{0, 0, 0, 0, 0},
						{0, 0, 0, 0, 0} };

	for ( auto j = 1U; j < m_rows-1; j++ )
	{
		for ( auto i = 1U; i < m_columns-1; i++ )
		{
			G_x = convolve<T,3>( image, sobel_x, 1, i, j );
			G_y = convolve<T,3>( image, sobel_y, 1, i, j );
			G = std::sqrt( G_x*G_x + G_y*G_y );

			m_thetas[i][j] = get_orientation( 180.0 * std::atan2( G_y, G_x ) / cimg::PI );

			m_mag_array[i][j] = G;
		}
	}
}

//*****************************
//non-maximum suppression
//depending on the orientation, pixels are either thrown away or accepted
//by checking it's neighbors
//*****************************
template<typename T>
void canny<T>::_no_max( CImg<T>& image )
{
	for( auto j=1U ; j < m_rows-1 ; j++ )
	{
	    for( auto i=1U ; i < m_columns-1 ; i++ )
		{
			 	//std::cout << m_thetas[i][j] << std::endl;

				switch( m_thetas[i][j] )
				{
				case 0:
					if( is_first_max( m_mag_array[i][j], m_mag_array[i+1][j], m_mag_array[i-1][j] ) )
					{
						image(i,j) = 1; // white
					}
					else
					{
						image(i,j) = 0; // black
					}
				break;

				case 45:
					if( is_first_max( m_mag_array[i][j], m_mag_array[i+1][j+1], m_mag_array[i-1][j-1] ) )
					{
						image(i,j) = 1; // white
					}
					else
					{
						image(i,j) = 0; // black
					}
				break;

				case 90:
					if( is_first_max( m_mag_array[i][j], m_mag_array[i][j+1], m_mag_array[i][j-1] ) )
					{
						image(i,j) = 1; // white
					}
					else
					{
						image(i,j) = 0; // black
					}
				break;

				case 135:
					if( is_first_max( m_mag_array[i][j], m_mag_array[i+1][j-1], m_mag_array[i-1][j+1] ) )
					{
						image(i,j) = 1; // white
					}
					else
					{
						image(i,j) = 0; // black
					}
				break;

				default:
				break;
			}
		}
	}
}

//*******************************
//hysteresis noise filter makes lines continuous and filters out the noise
// see the pdf that we used to understand this step in english (Step 5)
//*******************************
template<typename T>
void canny<T>::_hysteresis( CImg<T>& image )
{
	bool greater_found;
	bool between_found;

	for( auto j=2U ; j < m_rows-2 ; j++ )
	{
		for( auto i=2U ; i < m_columns-2 ; i++ )
		{
			if( m_mag_array[i][j] < m_low_thresh )
			{
				image(i,j) = 0; // black
			}

			if( m_mag_array[i][j] > m_high_thresh )
			{
				image(i,j) = 1; // white
			}

			/*If pixel (x, y) has gradient magnitude between tlow and thigh and
			any of its neighbors in a 3 × 3 region around
			it have gradient magnitudes greater than thigh, keep the edge*/

			if( m_mag_array[i][j] >= m_low_thresh && m_mag_array[i][j] <= m_high_thresh)
			{
				greater_found = false;
				between_found = false;
				for( int m = -1; m < 2; m++ )
				{
					for( int n = -1; n < 2; n++ )
					{
						if( m_mag_array[i+m][j+n] > m_high_thresh )
						{
							image(i,j) = 0;
							greater_found = true;
				 		}
				 		if( m_mag_array[i][j] > m_low_thresh && m_mag_array[i][j] < m_high_thresh )
							between_found = true;
					}
				}

				if( !greater_found && between_found )
				{
					for( int m = -2; m < 3; m++ )
					{
						for( int n = -2; n < 3; n++ )
						{
							if( m_mag_array[i+m][j+n] > m_high_thresh )
								greater_found = true;
						}
					}
				}

				if( greater_found )
					image(i,j) = 0;
				else
					image(i,j) = 1;
			}
		}
	}
}

/*If pixel (x, y) has gradient magnitude between tlow and thigh and any of its neighbors in a 3 × 3 region around
it have gradient magnitudes greater than thigh, keep the edge (write out white).
If none of pixel (x, y)s neighbors have high gradient magnitudes but at least one falls between tlow and thigh,
search the 5 × 5 region to see if any of these pixels have a magnitude greater than thigh. If so, keep the edge
(write out white).
*/

template<typename T>
void canny<T>::process( const CImg<T>& image_in, CImg<T>& image_out )
{
	// http://www.kerrywong.com/2009/05/07/canny-edge-detection-auto-thresholding/
	T mean = image_in.mean();
	m_low_thresh = 0.66 * mean;
	m_high_thresh = 1.33 * mean;

	_gaussian_blur( image_in, image_out );
	_sobel( image_out );
	_no_max( image_out );
	_hysteresis( image_out );

	// 2px border not managed for now
	cimg_for_borderXY( image_out, x, y, 3 ) { image_out( x, y ) = 0; }
}

template void canny<float>::process( const CImg<float>& image_in, CImg<float>& image_out );


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2014 Erik Erlandson
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//#include <boost/units/systems/information.hpp>

/**
\file

\brief information.cpp

\details
Demonstrate information unit system.

Output:
@verbatim
bytes= 1.25e+08 B
bits= 8e+06 b
nats= 4605.17 nat
1024 bytes in a kibi-byte
8.38861e+06 bits in a mebi-byte
0.000434294 hartleys in a milli-nat
entropy in bits= 1 b
entropy in nats= 0.693147 nat
entropy in hartleys= 0.30103 Hart
entropy in shannons= 1 Sh
entropy in bytes= 0.125 B
@endverbatim
**/

#include <cmath>
#include <iostream>
using std::cout;
using std::endl;

#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>
#include <boost/units/conversion.hpp>
namespace bu = boost::units;
using bu::quantity;
using bu::conversion_factor;

// SI prefixes
#include <boost/units/systems/si/prefixes.hpp>
namespace si = boost::units::si;

// information unit system
#include <boost/units/systems/information.hpp>
using namespace bu::information;

// Define a function for the entropy of a bernoulli trial.
// The formula is computed using natural log, so the units are in nats.
// The user provides the desired return unit, the only restriction being that it
// must be a unit of information.  Conversion to the requested return unit is
// accomplished automatically by the boost::units library.
template <typename Sys>
quantity<bu::unit<bu::information_dimension, Sys> >
bernoulli_entropy(double p, const bu::unit<bu::information_dimension, Sys>&) {
    typedef bu::unit<bu::information_dimension, Sys> requested_unit;
    return quantity<requested_unit>((-(p*log(p) + (1-p)*log(1-p)))*nats);
}

int main(int argc, char** argv) {
    // a quantity of information (default in units of bytes)
    quantity<info> nbytes(1 * si::giga * bit);
    cout << "bytes= " << nbytes << endl;

    // a quantity of information, stored as bits
    quantity<hu::bit::info> nbits(1 * si::mega * byte);
    cout << "bits= " << nbits << endl;

    // a quantity of information, stored as nats
    quantity<hu::nat::info> nnats(2 * si::kilo * hartleys);
    cout << "nats= " << nnats << endl;

    // how many bytes are in a kibi-byte?
    cout << conversion_factor(kibi * byte, byte) << " bytes in a kibi-byte" << endl;

    // how many bits are in a mebi-byte?
    cout << conversion_factor(mebi * byte, bit) << " bits in a mebi-byte" << endl;

    // how many hartleys are in a milli-nat?
    cout << conversion_factor(si::milli * nat, hartley) << " hartleys in a milli-nat" << endl;

    // compute the entropy of a fair coin flip, in various units of information:
    cout << "entropy in bits= " << bernoulli_entropy(0.5, bits) << endl;
    cout << "entropy in nats= " << bernoulli_entropy(0.5, nats) << endl;
    cout << "entropy in hartleys= " << bernoulli_entropy(0.5, hartleys) << endl;
    cout << "entropy in shannons= " << bernoulli_entropy(0.5, shannons) << endl;
    cout << "entropy in bytes= " << bernoulli_entropy(0.5, bytes) << endl;

    return 0;
}


// example_policy_handling.cpp

// Copyright Paul A. Bristow 2007, 2010.
// Copyright John Maddock 2007.

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

// See error_handling_example.cpp for use of
// macro definition to change policy for
// domain_error - negative degrees of freedom argument
// for student's t distribution CDF,
// and catching the exception.

// See error_handling_policies.cpp for more examples.

// Boost
#include <boost/math/distributions/students_t.hpp>
using boost::math::students_t_distribution;  // Probability of students_t(df, t).
using boost::math::students_t;  // Probability of students_t(df, t) convenience typedef for double.

using boost::math::policies::policy;
using boost::math::policies::domain_error;
using boost::math::policies::ignore_error;

// std
#include <iostream>
   using std::cout;
   using std::endl;

#include <stdexcept>

// Define a (bad?) policy to ignore domain errors ('bad' arguments):
typedef policy<
      domain_error<ignore_error>
      > my_policy;

// Define my_students_t distribution with this different domain error policy:
typedef students_t_distribution<double, my_policy> my_students_t;

int main()
{  // Example of error handling of bad argument(s) to a distribution.
  cout << "Example error handling using Student's t function. " << endl;

  double degrees_of_freedom = -1; double t = -1.; // Two 'bad' arguments!

  try
  {
    cout << "Probability of ignore_error Student's t is "
      << cdf(my_students_t(degrees_of_freedom), t) << endl;
    cout << "Probability of default error policy Student's t is " << endl;
    // By contrast the students_t distribution default domain error policy is to throw,
    cout << cdf(students_t(-1), -1) << endl;  // so this will throw.
/*`
    Message from thrown exception was:
   Error in function boost::math::students_t_distribution<double>::students_t_distribution:
   Degrees of freedom argument is -1, but must be > 0 !
*/

    // We could also define a 'custom' distribution
    // with an "ignore overflow error policy" in a single statement:
    using boost::math::policies::overflow_error;
    students_t_distribution<double, policy<overflow_error<ignore_error> > > students_t_no_throw(-1);

  }
  catch(const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }

  return 0;
} // int main()

/*

Output:

   error_policy_example.cpp
  Generating code
  Finished generating code
  error_policy_example.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Release\error_policy_example.exe
  Example error handling using Student's t function.
  Probability of ignore_error Student's t is 1.#QNAN
  Probability of default error policy Student's t is

  Message from thrown exception was:
     Error in function boost::math::students_t_distribution<double>::students_t_distribution: Degrees of freedom argument is -1, but must be > 0 !

*/


#include <iostream>
#include <cmath>
#include <chrono>
#include <cassert>
#include <vector>
#include <NTL/ZZ.h>
#include <NTL/vector.h>
#include "Element.hpp"

using namespace std;
using namespace std::chrono;
using namespace NTL;

void test_wilson(int bound);
void test_wolstenholme(int bound);
void test_kurepa(int bound);

template<typename T, typename M>
void remainder_tree(vector<Elt<T>> &C, vector<Elt<T>> &A, vector<Elt<T>> &m, Elt<T> &AProd, Elt<M> &mProd, Elt<T> const &root_value = Elt<T>(1), int start = 0, int end = -1);
// void remainder_tree_v1(vector<Elt<T>> &C, vector<Elt<T>> &A, vector<Elt<M>> &m, Elt<T> const &root_value = Elt<T>(1), const int k = 2);
template<typename T, typename M>
Elt<T> get_node(int index, vector<Elt<T>> &base, Elt<M> const &mod = Elt<M>(0));
template<typename T, typename M>
void remainder_tree_v2(vector<Elt<T>> &C, vector<Elt<T>> &A, vector<Elt<M>> &m, Elt<T> const &root_value = Elt<T>(1), const int k = 2);
template<typename T>
void print_tree(vector<Elt<T>> tree);
void complexity_graph(int N, int d);

/* Tags:
 *
 * //DEBUG// for debug statements
 * Optimization idea for optimizations that havent been impemented yet
 * TODO: add typename specifications to functions when they are called
 * TODO: add .t whenever need to access a value of Elt
 */

int main(){

	complexity_graph(1<<24, 10);
}

template<typename T>
void elt_to_base(vector<T> &b, vector<Elt<T>> &e){
	assert(b.size() == e.size());
	for(int i = 0; i < b.size(); i++){
		b[i] = e[i].t;
	}
}

template<typename T>
void base_to_elt(vector<Elt<T>> &e, vector<T> &b){
	assert(e.size() == b.size());
	for(int i = 0; i < e.size(); i++){
		e[i] = b[i];
	}
}

// Test for Wilson Primes
void test_wilson(int bound){

	vector<ZZ> A;
	A.resize(bound);
	vector<ZZ> m;
	m.resize(bound);

	vector<Elt<ZZ>> A_elt;
	A_elt.resize(bound);
	vector<Elt<ZZ>> m_elt;
	m_elt.resize(bound);

	base_to_elt(A_elt, A);
	base_to_elt(m_elt, m);

	// make sure implicit ints don't overflow
	for(int i = 1; i <= bound; i++){
		A[i-1] = ZZ(i);
		m[i-1] = ProbPrime(ZZ(i)) ? ZZ(i)*ZZ(i) : ZZ(1);
	}

	/*
	for(int i = 0; i < A.size(); i++){
		cout << A[i] << " ";
	}
	cout << endl;

	for(int i = 0; i < m.size(); i++){
		cout << m[i] << " ";
	}
	cout << endl;
	*/

	vector<ZZ> C;
	C.resize(bound);
	vector<Elt<ZZ>> C_elt;
	C_elt.resize(bound);

	base_to_elt(C_elt, C);

	remainder_tree_v2<ZZ, ZZ>(C_elt, A_elt, m_elt, ZZ(1), 4);

	elt_to_base(C, C_elt);

	/*
	for(int i = 0; i < C.size(); i++){
		cout << (i+1) << ": " << C[i] << endl;
	}
	cout << endl;
	*/
}

// Test for Wolstenholme Primes
void test_wolstenholme(int bound){

	vector<ZZ> Anum;
	Anum.resize(bound);
	vector<ZZ> m;
	m.resize(bound);

	vector<Elt<ZZ>> Anum_elt;
	Anum_elt.resize(bound);
	vector<Elt<ZZ>> m_elt;
	m_elt.resize(bound);

	for(int i = 1; i <= bound; i++){
		Anum[i-1] = 4*i+2;
		m[i-1] = ProbPrime(ZZ(i)) ? ZZ(i)*ZZ(i)*ZZ(i)*ZZ(i)*ZZ(i) : ZZ(1);

	}

	base_to_elt(Anum_elt, Anum);
	base_to_elt(m_elt, m);

	vector<ZZ> Cnum;
	Cnum.resize(bound);
	vector<Elt<ZZ>> Cnum_elt;
	Cnum_elt.resize(bound);

	remainder_tree_v2<ZZ, ZZ>(Cnum_elt, Anum_elt, m_elt, ZZ(1), 4);

	elt_to_base(Cnum, Cnum_elt);

	for(int i = 1; i <= bound; i++){
		Cnum[i-1] /= i; // i = prime at this index
	}

	vector<ZZ> Adem;
	Adem.resize(bound);
	vector<Elt<ZZ>> Adem_elt;
	Adem_elt.resize(bound);

	for(int i = 1; i <= bound; i++){
		Adem[i-1] = i+1;
	}

	base_to_elt(Adem_elt, Adem);

	vector<ZZ> Cdem;
	Cdem.resize(bound);
	vector<Elt<ZZ>> Cdem_elt;
	Cdem_elt.resize(bound);

	remainder_tree_v2<ZZ, ZZ>(Cdem_elt, Adem_elt, m_elt, ZZ(1), 4);

	elt_to_base(Cdem, Cdem_elt);

	for(int i = 1; i <= bound; i++){
		Cdem[i-1] /= i;
		ZZ d;
		ZZ k;
		XGCD(d, Cdem[i-1], k, Cdem[i-1], ZZ(i)*ZZ(i)*ZZ(i)*ZZ(i)); // changes Cdem[i-1] to its inverse mod p^4
		if (Cdem[i-1] < 0) Cdem[i-1] += ZZ(i)*ZZ(i)*ZZ(i)*ZZ(i); // make residue positive

		Cnum[i-1] = (Cnum[i-1] * Cdem[i-1]) % (ZZ(i)*ZZ(i)*ZZ(i)*ZZ(i));
	}

	for(int i = 1; i <= bound; i++){
		if (Cnum[i-1] == 1){
			cout << i << ": " << Cnum[i-1] << endl;
		}
	}

}

void test_kurepa(int bound){
	// need to first implement remainder tree for matrices
}

/*
 * Original Remainder Tree implementation
 */

template<typename T, typename M>
void remainder_tree(vector<Elt<T>> &C, vector<Elt<T>> &A, vector<Elt<M>> &m, Elt<T> &AProd, Elt<M> &mProd, Elt<T> const &root_value, int start, int end){

	//DEBUG// cout << "AProd: " << AProd << endl;
	//DEBUG// cout << "mProd: " << mProd << endl;
	//DEBUG// cout << "root_value: " << root_value << endl;
	// set default value for end
	if (end == -1) end = C.size();

	// Assert that interval [start, end] exists in C, A and m
	assert(end <= C.size());
	assert(end <= A.size());
	assert(end <= m.size());

	// Set N = length of interval
	int N = end - start;

	// Change nothing if N = 0
	if (N == 0) {
		return;
	}

	// Index of leaf at the bottom left
	int leftmost = 1 << ((int)ceil(log2(N)));

	// Declare trees (always of length 2N for any N)
	vector<Elt<T>> ATree;
	ATree.resize(2 * N);
	vector<Elt<M>> mTree;
	mTree.resize(2 * N);
	vector<Elt<T>> CTree;
	CTree.resize(2 * N);

	/*
	 * For example when N=11 the leaves are in this order:
	 *     / \       /\   /\    /\
	 *    /   \     /  7 8  9 10  11
	 *   /\   /\   /\
	 *  1  2 3  4 5  6
	 *
	 */

	// Initialize the leaves in ATree and mTree
	for (int i = leftmost; i < 2 * N; i++) { // leaves on lowest layer
		ATree[i] = A[i - leftmost + start];
		mTree[i] = m[i - leftmost + start];
	}
	for (int i = N; i < leftmost; i++) { // leaves on second lowest layer
		ATree[i] = A[i + N - leftmost + start];
		mTree[i] = m[i + N - leftmost + start];
	}

	// Calculate the rest of the product tree mTree
	for (int i = N - 1; i > 0; i--) {
		mTree[i] = mTree[2 * i] * mTree[2 * i + 1]; // parent is product of leaves
	}

	// Calculate the rest of the product tree aTree, taking mod mTree[1] = m[0]*...*m[N-1]
	for(int i = N - 1; i > 0; i--) {
		ATree[i] = (ATree[2 * i] * ATree[2 * i + 1]) % mProd; // parent is product of leaves mod mTree[1]
		ATree[2 * i] %= mTree[1];
		delete ATree[2 * i + 1];
	}

	mProd /= mTree[1]; // Get rid of this tree's moduli from mProd
	AProd = ATree[1]; // Set AProd as the product of A's mod new mProd

	// Calculate accumulating remainder tree
	CTree[1] = root_value % mTree[1];
	//DEBUG// cout << "CTree root: " << CTree[1] << endl;
	for (int i = 1; i < N; i++) {
		CTree[2 * i] = CTree[i] % mTree[2 * i]; // Left branch
		CTree[2 * i + 1] = (CTree[i] * ATree[2 * i]) % mTree[2 * i + 1]; // Right branch
		delete CTree[i];
	}

	//DEBUG// print_tree<ZZ>(ATree);
	//DEBUG// print_tree<ZZ>(mTree);
	//DEBUG// print_tree<ZZ>(CTree);

	for (int i = leftmost; i < 2 * N; i++) {
		C[i - leftmost + start] = CTree[i];
	}
	for (int i = N; i < leftmost; i++) {
		C[i + N - leftmost + start] = CTree[i];
	}

	return;
}

/*
 * Implements Sutherland's optimization
 * Doesn't do intervals yet
 * k = layer where we divide into subtrees
 */
template<typename T, typename M>
void remainder_tree_v2(vector<Elt<T>> &C, vector<Elt<T>> &A, vector<Elt<M>> &m, Elt<T> const &root_value, const int k){

	// Assert that lengths of A and m match
	assert(C.size() == A.size());
	assert(C.size() == m.size());

	// Set N = length of input arrays
	int N = C.size();

	// Change nothing if N = 0
	if (N == 0) {
		return;
	}

	// Ensure that there are at least k layers
	assert(N >= (1<<k));

	// Index of leaf at the bottom left
	int leftmost = 1 << ((int)ceil(log2(N)));

	// Declare Ctree (always of length 2N for any N)
	vector<Elt<T>> CTree;
	CTree.resize(2 * N);

	/*
	 * For example when N=11 the leaves are in this order:
	 *     / \       /\   /\    /\
	 *    /   \     /  7 8  9 10  11
	 *   /\   /\   /\
	 *  1  2 3  4 5  6
	 *
	 */

	// Calculate the product of all the mods to keep A's small
	// Elt<M> mProd = get_node<ZZ, ZZ>(1, m, Elt<M>(0));

	uint64_t start2 = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();

	// Step 2: Calculate the subproduct trees
	// Roots are CTree[2^k + i]: {CTree[2^k], ..., CTree[2^(k+1)-1]}

	// First find index of root in subtree with leaves in both layers
	int notfirstk = ((int)log2(2*N-1) - k); // (#bits in 2*N-1) minus k
	int special = (2*N-1 - leftmost) >> notfirstk; // firstk digits excluding the most significant digit

	Elt<T> AProd = Elt<T>(1);
	Elt<M> mProd = get_node<ZZ, ZZ>(1, m, Elt<M>(0));
	CTree[1<<k] = root_value % mProd;
	// Subtrees with leaves in first layer
	for(int i = 0; i < special; i++) {
		// Number of leaves: 2^notfirstk = leftmost/2^k
		//DEBUG// cout << "Calculating interval: [" << (i<<notfirstk) << ", " << ((i+1)<<notfirstk) << "]" << endl;
		remainder_tree<ZZ, ZZ>(C, A, m, AProd, mProd, CTree[(1<<k) + i], i<<notfirstk, (i+1)<<notfirstk);
		CTree[(1<<k) + i+1] = (CTree[(1<<k) + i] * AProd) % mProd;
	}

	// Subtree with leaves in both layers
	// First, calculate number of leaves in this subtree, stored in specialleaves
	int notfirstkdigits = (2*N-1) % (1<<notfirstk);
	int onenotfirstkdigits = (1<<notfirstk) + notfirstkdigits;
	int specialleaves = (onenotfirstkdigits+1)/2;
	//DEBUG// cout << "Calculating interval: [" << (special<<notfirstk) << ", " << ((special<<notfirstk) + specialleaves) << "]" << endl;
	remainder_tree<ZZ, ZZ>(C, A, m, AProd, mProd, CTree[(1<<k) + special], special<<notfirstk, (special<<notfirstk) + specialleaves);
	CTree[(1<<k) + special+1] = (CTree[(1<<k) + special] * AProd) % mProd;

	// Subtrees with leaves in second layer
	for(int i = special+1; i < 1<<k; i++){
		//DEBUG// cout << "Calculating interval: [" << ((special<<notfirstk) + specialleaves + ((i - special-1)<<(notfirstk-1))) << ", " << ((special<<notfirstk) + specialleaves + ((i - special)<<(notfirstk-1))) << "]" << endl;
		remainder_tree<ZZ, ZZ>(C, A, m, AProd, mProd, CTree[(1<<k) + i], (special<<notfirstk) + specialleaves + ((i - special-1)<<(notfirstk-1)), (special<<notfirstk) + specialleaves + ((i - special)<<(notfirstk-1)));
		if (i == (1<<k) - 1) continue; // Prevent index out of range for next operation
		CTree[(1<<k) + i+1] = (CTree[(1<<k) + i] * AProd) % mProd;
	}

	uint64_t end2 = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
	//DEBUG// cout << "Time taken for subtree step: " << (end2-start2) << endl;

	return;

}

/*
 * Returns the value of the node on the tree at index k with leaves having value base
 */
template<typename T, typename M>
Elt<T> get_node(int i, vector<Elt<T>> &base, Elt<M> const &mod) { // Optimization idea: pass in what you're taking a mod of as well so if the modulus ever gets bigger than the value, just return the value
	int N = base.size();
	int leftmost = 1 << ((int)ceil(log2(N)));
	if (mod == 0){
		if (i >= leftmost) return base[i - leftmost];
		else if (i >= N) return base[i + N - leftmost];

		return get_node<ZZ, ZZ>(2*i, base, Elt<M>(0))*get_node<ZZ, ZZ>(2*i+1, base, Elt<M>(0));
	}

	else {
		if (i >= leftmost) return base[i - leftmost] % mod;
		else if (i >= N) return base[i + N - leftmost] % mod;

		return (get_node<ZZ, ZZ>(2*i, base, mod)*get_node<ZZ, ZZ>(2*i+1, base, mod)) % mod;
	}
}

/*
 * Prints a tree given in vector<ZZ> form
 */
template<typename T>
void print_tree(vector<Elt<T>> tree){
	int top = 1;
	int counter = 0;
	for(int i = 1; i < tree.size(); i++){
		cout << tree[i] << " ";
		counter++;
		if (counter == top){
			cout << endl;
			counter = 0;
			top *= 2;
		}
	}
	cout << endl;
}

/*
 * Gives data points on size of input vs. computation time.
 * N = max size of data, d = number of data points
 */

void complexity_graph(int N, int d){
	vector<int> x;
	vector<int> y;
	vector<int> z;

	int interval = N/d;
	int B = 0;
	while(B <= N){
		cout << "Testing: " << B << endl;
		/*
		int testSize = B;
		int numSize = B;

		vector<ZZ> test_A;
		test_A.resize(testSize);
		vector<ZZ> test_m;
		test_m.resize(testSize);
		for (int i = 0; i < testSize; i++) {
			test_A[i] = rand() % numSize + 1;
			test_m[i] = rand() % numSize + 1;
		}

		vector<ZZ> test_C;
		test_C.resize(testSize);
		*/

		x.push_back(B);

		uint64_t start;
		uint64_t end;

		start = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
		//remainder_tree<ZZ, ZZ>(test_C, test_A, test_m);
		end = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();

		y.push_back(end-start);

		start = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
		test_wilson(B);
		end = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();

		z.push_back(end-start);

		B += interval;
	}

	for(int i = 0; i < x.size(); i++){
		cout << x[i] << ", ";
	}
	cout << endl;
	for(int i = 0; i < y.size(); i++){
		cout << y[i] << ", ";
	}
	cout << endl;
	for(int i = 0; i < z.size(); i++){
		cout << z[i] << ", ";
	}
	cout << endl;

}


// File:  main.cpp
// Date:  11/17/2019
// Auth:  K. Loux
// Desc:  Entry point for SplinePatchToFlatPattern application.

// optimization headers
#include "optimization/nelderMead.h"

// Eigen headers
#include <Eigen/Eigen>
#include <Eigen/StdVector>

// Standard C++ headers
#include <string>
#include <fstream>
#include <sstream>
#include <vector>
#include <iostream>
#include <cassert>
#include <algorithm>

typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> Vector2DVectors;
typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> Vector3DVectors;

bool ParseToken(const std::string& token, double& value)
{
	std::istringstream ss(token);
	return !(ss >> value).fail();
}

bool ParseLine(const std::string& line, Vector3DVectors& curve1, Vector3DVectors& curve2)
{
	std::istringstream ss(line);
	Eigen::Vector3d p1, p2;
	std::string token;
	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p1(0)))
		return false;

	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p1(1)))
		return false;
	p1(1) = fabs(p1(1));

	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p1(2)))
		return false;

	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p2(0)))
		return false;

	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p2(1)))
		return false;
	p2(1) = fabs(p2(1));

	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p2(2)))
		return false;

	curve1.push_back(p1);
	curve2.push_back(p2);

	return true;
}

bool ReadInputFile(const std::string& fileName, Vector3DVectors& curve1, Vector3DVectors& curve2)
{
	std::ifstream file(fileName);
	if (!file.is_open() || !file.good())
	{
		std::cerr << "Failed to open '" << fileName << "' for input\n";
		return false;
	}

	std::string line;
	unsigned int lineCount(0);
	while (std::getline(file, line))
	{
		++lineCount;
		if (!ParseLine(line, curve1, curve2))
		{
			std::cerr << "Failed to parse line " << line << '\n';
			return false;
		}
	}

	auto sortPredicate([](const Eigen::Vector3d&a, const Eigen::Vector3d& b)
	{
		return a(0) < b(0);
	});

	std::sort(curve1.begin(), curve1.end(), sortPredicate);
	std::sort(curve2.begin(), curve2.end(), sortPredicate);

	return true;
}

class Spline
{
public:
	void AddPoint(const Eigen::Vector3d& p, const Eigen::Vector3d& c)
	{
		intersectionPoints.push_back(p);
		controlVectors.push_back(c);
	}

	void SetControlVector(const unsigned int& i, const Eigen::Vector3d& v) { controlVectors[i] = v; }

	unsigned int GetSegmentCount() const { return intersectionPoints.size() - 1; }
	Eigen::Vector3d GetIntersectionPoint(const unsigned int& i) const { return intersectionPoints[i]; }
	Eigen::Vector3d GetControlVector(const unsigned int& i) const { return controlVectors[i]; }

	const Vector3DVectors& GetIntersectionPoints() const { return intersectionPoints; }
	const Vector3DVectors& GetControlVectors() const { return controlVectors; }

private:
	Vector3DVectors intersectionPoints;
	Vector3DVectors controlVectors;
};

Vector3DVectors ComputeSpline(const Spline& s, const unsigned int& segmentResolution)
{
	const auto segments(s.GetSegmentCount());
	Vector3DVectors points(segments * segmentResolution);
	for (unsigned int i = 0; i < segments; ++i)
	{
		double t(0.0);
		const double tStep(1.0 / segmentResolution);
		for (unsigned int j = 0; j < segmentResolution; ++j)
		{
			const Eigen::Vector3d p0(s.GetIntersectionPoint(i));
			const Eigen::Vector3d p1(s.GetIntersectionPoint(i) - s.GetControlVector(i));
			const Eigen::Vector3d p2(s.GetIntersectionPoint(i + 1) + s.GetControlVector(i + 1));
			const Eigen::Vector3d p3(s.GetIntersectionPoint(i + 1));

			points[i * segmentResolution + j] = pow(1.0 - t, 3) * p0 + 3.0 * pow(1.0 - t, 2) * t * p1 + 3.0 * (1. - t) * t * t * p2 + pow(t, 3) * p3;
			t += tStep;
		}
	}

	return points;
}

double ComputeError(const Spline& s, const Vector3DVectors& goalPoints)
{
	const unsigned int resolution(1000);
	const auto sPoints(ComputeSpline(s, resolution));
	double e(0.0);
	for (const auto& p : sPoints)
	{
		double minDistance(std::numeric_limits<double>::max());
		for (const auto& v : goalPoints)
		{
			const auto distance((p - v).norm());
			if (distance < minDistance)
				minDistance = distance;
		}
		e += minDistance;
	}

	return e;
}

Vector3DVectors BuildControlVectors(Eigen::VectorXd x, const Eigen::VectorXd* initialGuess = nullptr)
{
	Vector3DVectors controlVectors;
	controlVectors.push_back(Eigen::Vector3d(0.0, -fabs(x(0)), 0.0));

	if (initialGuess)
	{
		for (int i = 1; i < x.size() - 1; ++i)
		{
			if (x(i) * (*initialGuess)(i) < 0.0)
				x(i) *= -1.0;
		}
	}

	for (int i = 1; i < x.size() - 1; i += 3)
		controlVectors.push_back(Eigen::Vector3d(x(i), x(i + 1), x(i + 2)));

	controlVectors.push_back(Eigen::Vector3d(0.0, fabs(x(x.size() - 1)), 0.0));

	return controlVectors;
}

struct SplineFitArgs : public Optimizer::AdditionalArgs
{
	SplineFitArgs(const Vector3DVectors& goalPoints,
		const Vector3DVectors& intersectionPoints, const Eigen::VectorXd& initialGuess)
		: goalPoints(goalPoints), intersectionPoints(intersectionPoints), initialGuess(initialGuess) {}

	const Vector3DVectors& goalPoints;
	const Vector3DVectors& intersectionPoints;
	const Eigen::VectorXd initialGuess;
};

Eigen::VectorXd DoIteration(const Eigen::VectorXd& guess, const Optimizer::AdditionalArgs* args)
{
	const auto& arguments(*dynamic_cast<const SplineFitArgs*>(args));
	const auto controlVectors(BuildControlVectors(guess, &arguments.initialGuess));
	Spline s;
	for (unsigned int i = 0; i < controlVectors.size(); ++i)
		s.AddPoint(arguments.intersectionPoints[i], controlVectors[i]);

	return Eigen::VectorXd(guess.size()).setOnes() * ComputeError(s, arguments.goalPoints);// TODO:  Is this correct?  Can we return 1x1?
}

void FitSplineToPoints(const Vector3DVectors& points, Spline& spline)
{
	constexpr unsigned int splineSegmentCount(3);// Assume that we'll get a good fit if we choose three segments.
	assert(points.size() > splineSegmentCount);
	Eigen::VectorXd initialGuess((splineSegmentCount - 1) * 3 + 2, 1);
	initialGuess.setOnes();

	spline.AddPoint(points.front(), Eigen::Vector3d(0.0, -1.0, 0.0));

	const unsigned int i1(points.size() / splineSegmentCount);
	for (unsigned int a = 1; a < splineSegmentCount; ++a)
	{
		const double signAdjust(1.0);
		spline.AddPoint(points[i1 * a], (points[i1 * a - 1] - points[i1 * a + 1]) * signAdjust);
		initialGuess((a - 1) * 3 + 1) = spline.GetControlVector(a)(0);
		initialGuess((a - 1) * 3 + 2) = spline.GetControlVector(a)(1);
		initialGuess((a - 1) * 3 + 3) = spline.GetControlVector(a)(2);
	}

	spline.AddPoint(points.back(), Eigen::Vector3d(0.0, 1.0, 0.0));

	/*std::cout << "\nIntersection Points:\n";
	for (const auto& ip : spline.GetIntersectionPoints())
		std::cout << ip.transpose() << '\n';

	std::cout << "\nInitial Control Vectors:\n";
	for (const auto& cp : spline.GetControlVectors())
		std::cout << cp.transpose() << '\n';*/

	SplineFitArgs arguments(points, spline.GetIntersectionPoints(), initialGuess);
	const unsigned int iterationLimit(10000);
	NelderMead<(splineSegmentCount - 1) * 3 + 2> optimizer(DoIteration, iterationLimit, &arguments);
	optimizer.SetInitialGuess(initialGuess);
	const auto x(optimizer.Optimize());
	const auto newControlVectors(BuildControlVectors(x));

	for (unsigned int i = 0; i < newControlVectors.size(); ++i)
		spline.SetControlVector(i, newControlVectors[i]);

	/*std::cout << "\nFinal Control Vectors:\n";
	for (const auto& cp : spline.GetControlVectors())
		std::cout << cp.transpose() << '\n';*/
}

double ComputeLength(const Vector3DVectors& p)
{
	double length(0.0);
	for (unsigned int i = 1; i < p.size(); ++i)
		length += (p[i] - p[i - 1]).norm();
	return length;
}

bool FindIntersectionOfTwoCircles(const Eigen::Vector2d& c1, const double& r1,
	const Eigen::Vector2d& c2, const double& r2, Eigen::Vector2d& isect1, Eigen::Vector2d& isect2)
{
	const double distance((c1 - c2).norm());
	if (distance > r1 + r2 /*|| distance < fabs(r1 - r2)*/ || (distance == 0.0 && r1 == r2))// If there are no solutions, or infinite solutions, we cannot proceed
		return false;

	const double a((r1 * r1 - r2 * r2 + distance * distance) / (2.0 * distance));
	const double h(sqrt(r1 * r1 - a * a));
	const Eigen::Vector2d p(c1 + a * (c2 - c1) / distance);

	isect1(0) = p(0) + h * (c2(1) - c1(1)) / distance;
	isect1(1) = p(1) - h * (c2(0) - c1(0)) / distance;

	isect2(0) = p(0) - h * (c2(1) - c1(1)) / distance;
	isect2(1) = p(1) + h * (c2(0) - c1(0)) / distance;

	return true;
}

Eigen::Vector2d ChooseBestIntersection(const Eigen::Vector2d& isect1, const Eigen::Vector2d& isect2, const Vector2DVectors& c)
{
	// TODO:  Improve this.  Should also consider distance between intersection results as criteria?  And/or distance from previous point?
	/*if (c.size() < 2)
		return isect1;

	if ((c.back() - isect1).norm() > (c.back() - isect2).norm())
		return isect1;*/
	return isect2;
}

bool GenerateFlatPattern(const Spline& s1, const Spline& s2, const double& stepTarget, Vector2DVectors& flatPatternPoints, const unsigned int& targetOutputPointCount)
{
	const unsigned int resolution(1000);
	auto c1(ComputeSpline(s1, resolution));
	auto c2(ComputeSpline(s2, resolution));

	double s1Length(ComputeLength(c1));
	double s2Length(ComputeLength(c2));

	// Recalculate with resolution fine enough to give good distance resolution
	const double factor(100.0);
	c1 = ComputeSpline(s1, static_cast<unsigned int>(s1Length / stepTarget * factor));
	c2 = ComputeSpline(s2, static_cast<unsigned int>(s2Length / stepTarget * factor));

	s1Length = ComputeLength(c1);
	s2Length = ComputeLength(c2);

	const double step1(s1Length > s2Length ? stepTarget : stepTarget * s1Length / s2Length);
	const double step2(s2Length > s1Length ? stepTarget : stepTarget * s2Length / s1Length);

	Vector2DVectors curve1, curve2;
	double d((c1.front() - c2.front()).norm());
	curve1.push_back((Eigen::Vector2d() << 0.0, 0.0).finished());
	curve2.push_back((Eigen::Vector2d() << d, 0.0).finished());

	unsigned int i1(1), i2(1);
	unsigned int i1Last(0), i2Last(0);
	while (i1 < c1.size() - 1 && i2 < c2.size() - 1)
	{
		for (; i1 < c1.size() - 1; ++i1)
		{
			if ((c1[i1] - c1[i1Last]).norm() > step1)
				break;
		}

		for (; i2 < c2.size() - 1; ++i2)
		{
			if ((c2[i2] - c2[i2Last]).norm() > step2)
				break;
		}

		const double d1From1((c1[i1] - c1[i1Last]).norm());
		const double d1From2((c1[i1] - c2[i2Last]).norm());
		const double d2From1((c2[i2] - c1[i1Last]).norm());
		const double d2From2((c2[i2] - c2[i2Last]).norm());
		i1Last = i1;
		i2Last = i2;

		Eigen::Vector2d isect11, isect12, isect21, isect22;
		if (!FindIntersectionOfTwoCircles(curve1.back(), d1From1, curve2.back(), d1From2, isect11, isect12))
			return false;
		if (!FindIntersectionOfTwoCircles(curve1.back(), d2From1, curve2.back(), d2From2, isect21, isect22))
			return false;

		curve1.push_back(ChooseBestIntersection(isect11, isect12, curve1));
		curve2.push_back(ChooseBestIntersection(isect21, isect22, curve2));
	}

	auto decimate([](Vector2DVectors& curve, const unsigned int& increment)
	{
		unsigned int swapCount(0);
		for (unsigned int i = 0; i < curve.size(); ++i)
		{
			if (i % increment == 0 || i == curve.size() - 1)
				continue;
			auto it(curve.begin() + i - swapCount);
			std::rotate(it, it + 1, curve.end());
			++swapCount;
		}
		curve.erase(curve.begin() + curve.size() - swapCount, curve.end());
	});

	const unsigned int increment(std::max(static_cast<unsigned int>(curve1.size() / targetOutputPointCount), 1U));
	decimate(curve1, increment);
	decimate(curve2, increment);

	flatPatternPoints = curve1;
	flatPatternPoints.insert(flatPatternPoints.end(), curve2.rbegin(), curve2.rend());

	return true;
}

int main(int argc, char* argv[])
{
	if (argc != 2)
	{
		std::cout << "Usage:  " << argv[0] << " <input file>\n"
			<< "  Input file must be comma-delimited and must contain four columns.\n"
			<< "  The first three columns are (x,y,z) for a series of points\n"
			<< "  describing one spline, and columns 4-5 are (x,y,z) for a series of\n"
			<< "  points describing the second spline.  Points should only be included\n"
			<< "  for half of each curve (i.e. positive y-ordinates only).  It is\n"
			<< "  assumed that x-z plane symmetry is desired, and curves are\n"
			<< "  constrained to have slopes parallel to the y-axis where the curves\n"
			<< "  meet the x-z plane.\n" << std::endl;
	}

	Vector3DVectors curve1, curve2;
	if (!ReadInputFile(argv[1], curve1, curve2))
		return 1;

	Spline spline1, spline2;
	FitSplineToPoints(curve1, spline1);
	FitSplineToPoints(curve2, spline2);

	const unsigned int res(30);
	const auto c1(ComputeSpline(spline1, res));
	const auto c2(ComputeSpline(spline2, res));
	std::ofstream splinesOut("splinesOut.csv");
	for (unsigned int i = 0; i < c1.size(); ++i)
		splinesOut << c1[i](0) << ',' << c1[i](1) << ',' << c1[i](2) << ',' << c2[i](0) << ',' << c2[i](1) << ',' << c2[i](2) << '\n';//*/

	const double distanceResolution(0.01);
	const unsigned int targetOutputPointCount(100);
	Vector2DVectors flatPatternPoints;
	if (!GenerateFlatPattern(spline1, spline2, distanceResolution, flatPatternPoints, targetOutputPointCount))
		return 1;

	std::ofstream flatPatternOut("flatPattern.csv");
	flatPatternOut.precision(10);
	for (unsigned int i = 0; i < flatPatternPoints.size(); ++i)
		flatPatternOut << std::fixed << flatPatternPoints[i](0) << ',' << flatPatternPoints[i](1) << '\n';

	return 0;
}


#pragma once

#include <Eigen/Dense>

template <typename T>
void deg2rad(T &deg)
{
    deg *= M_PI / 180.;
}

template <typename T>
Eigen::Matrix<T, 4, 1> eulerToQuaternion(const Eigen::Matrix<T, 3, 1> &rpy)
{
    Eigen::Quaternion<T> q = Eigen::AngleAxis<T>(rpy.x(), Eigen::Matrix<T, 3, 1>::UnitX()) *
                             Eigen::AngleAxis<T>(rpy.y(), Eigen::Matrix<T, 3, 1>::UnitY()) *
                             Eigen::AngleAxis<T>(rpy.z(), Eigen::Matrix<T, 3, 1>::UnitZ());
    Eigen::Matrix<T, 4, 1> quat;
    quat << q.w(), q.vec();

    return quat;
}

template <typename T>
Eigen::Matrix<T, 4, 4> omegaMatrix(const Eigen::Matrix<T, 3, 1> &w)
{
    Eigen::Matrix<T, 4, 4> omegaMatrix;
    omegaMatrix << T(0.), -w(0), -w(1), -w(2),
        w(0), T(0.), w(2), -w(1),
        w(1), -w(2), T(0.), w(0),
        w(2), w(1), -w(0), T(0.);

    return omegaMatrix;
}

template <typename T>
Eigen::Matrix<T, 3, 3> omegaMatrixReduced(const Eigen::Matrix<T, 3, 1> &q)
{
    Eigen::Matrix<T, 3, 3> omegaMatrix;
    const T qw = sqrt(1. - q.squaredNorm());
    omegaMatrix << qw, -q(2), q(1),
        q(2), qw, -q(0),
        -q(1), q(0), qw;

    return omegaMatrix;
}

template <typename T>
Eigen::Matrix<T, 3, 3> EulerRotationMatrix(const Eigen::Matrix<T, 3, 1> &eta)
{
    const T phi = eta(0);
    const T theta = eta(1);
    const T psi = eta(2);

    Eigen::Matrix<T, 3, 3> R;
    R.row(0) << cos(theta) * cos(psi), -cos(theta) * sin(psi), sin(theta);
    R.row(1) << sin(phi) * sin(theta) * cos(psi) + cos(phi) * sin(psi),
        cos(phi) * cos(psi) - sin(phi) * sin(theta) * sin(psi), -sin(phi) * cos(theta);
    R.row(2) << -cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi),
        sin(phi) * cos(psi) + cos(phi) * sin(theta) * sin(psi), cos(phi) * cos(theta);

    return R;
}

template <typename T>
Eigen::Matrix<T, 3, 3> EulerRotationJacobian(const Eigen::Matrix<T, 3, 1> &eta)
{
    const T phi = eta(0);
    const T theta = eta(1);
    const T psi = eta(2);

    Eigen::Matrix<T, 3, 3> J;
    J.row(0) << cos(psi), -sin(psi), 0.;
    J.row(1) << cos(theta) * sin(psi), cos(theta) * cos(psi), 0;
    J.row(2) << -sin(theta) * cos(psi), sin(theta) * sin(psi), cos(theta);

    return 1. / cos(theta) * J;
}


// Eigen::Ref can be used to refer to a matrix *or* a block without copying.
// The referred matrix must have its memory fully allocated: cannot be resized.

#include <iostream>
#include <stdlib.h>
#include <string>
#include <vector>

#include <Eigen/Dense>

using namespace std;

Eigen::MatrixXd A = Eigen::MatrixXd::Random(1, 2);
Eigen::Ref<Eigen::MatrixXd> ref_from_address(Eigen::MatrixXd *address) {
  return *address;
}

int main() {
  std::cout << "A: " << std::endl << A << std::endl << std::endl;

  Eigen::MatrixXd *A_address = &A;
  std::cout << "A_address: " << A_address << std::endl;

  std::cout << "A = Eigen::MatrixXd::Random(2, 4);" << std::endl;
  A = Eigen::MatrixXd::Random(2, 4);
  std::cout << A << std::endl << std::endl;

  std::cout << "ref_from_address(A_address) =  Eigen::MatrixXd::Random(2, 4);"
            << " // Okay, since shape matches" << std::endl;
  ref_from_address(A_address) =  Eigen::MatrixXd::Random(2, 4);
  std::cout << A << std::endl << std::endl;

  std::cout << "ref_from_address(A_address).col(2) = "
            << "Eigen::MatrixXd::Zero(2, 1); // Also okay" << std::endl;
  ref_from_address(A_address).col(2) =  Eigen::MatrixXd::Zero(2, 1);
  std::cout << A << std::endl << std::endl;

  std::cout << "// ref_from_address(A_address) =  "
            << "Eigen::MatrixXd::Ones(2, 10); // ERROR! "
            << "\"DenseBase::resize() does not actually allow to resize.\""
            << std::endl << std::endl;
  // ref_from_address(A_address) =  Eigen::MatrixXd::Ones(2, 10);

  std::cout << "// First need to directly resize NOT USING REF. This destroys "
            << "coeffs if the number of coeffs is different." << std::endl;
  std::cout << "A_address->resize(2, 10);  " << std::endl;
  A_address->resize(2, 10);
  // ref_from_address(A_address).resize(2, 4);  // This gives error.
  std::cout << A << std::endl << std::endl;

  std::cout << "ref_from_address(A_address) =  "
            << "Eigen::MatrixXd::Ones(2, 10); // Now okay" << std::endl;
  ref_from_address(A_address) =  Eigen::MatrixXd::Ones(2, 10);
  std::cout << A << std::endl << std::endl;

  std::cout << "ref_from_address(A_address) << 1, 3, 5, 7, 9, 11, 13, 15, 17, "
            << "19, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20; // Also okay"
            << std::endl;
  ref_from_address(A_address) <<
      1, 3, 5, 7, 9, 11, 13, 15, 17, 19, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20;
  std::cout << A << std::endl << std::endl;

  std::cout << "// Aside: resizing is conservative when the number of coeffs "
            << "remains the same." << std::endl;
  std::cout << "A_address->resize(2, 10);  " << std::endl;
  A_address->resize(4, 5);
  std::cout << A << std::endl << std::endl;

  std::cout << "// Coefficient access" << std::endl;
  std::cout << "ref_from_address(A_address)(1, 2) = "
            << ref_from_address(A_address)(1, 2) << std::endl;
  std::cout << "ref_from_address(A_address).data()[9] = "
            << ref_from_address(A_address).data()[9] << std::endl;

  return 0;
}


//  (C) Copyright John Maddock 2005.
//  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 BOOST_MATH_EXPM1_INCLUDED
#define BOOST_MATH_EXPM1_INCLUDED

#include <cmath>
#include <math.h> // platform's ::expm1
#include <boost/limits.hpp>
#include <boost/math/special_functions/detail/series.hpp>

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#  include <boost/static_assert.hpp>
#else
#  include <boost/assert.hpp>
#endif

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::exp; using ::fabs; }
#endif

namespace boost{ namespace math{

namespace detail{
//
// Functor expm1_series returns the next term in the Taylor series
// x^k / k!
// each time that operator() is invoked.
//
template <class T>
struct expm1_series
{
   typedef T result_type;

   expm1_series(T x)
      : k(0), m_x(x), m_term(1) {}

   T operator()()
   {
      ++k;
      m_term *= m_x;
      m_term /= k;
      return m_term;
   }

   int count()const
   {
      return k;
   }

private:
   int k;
   const T m_x;
   T m_term;
   expm1_series(const expm1_series&);
   expm1_series& operator=(const expm1_series&);
};

} // namespace

//
// Algorithm expm1 is part of C99, but is not yet provided by many compilers.
//
// This version uses a Taylor series expansion for 0.5 > |x| > epsilon.
//
template <class T>
T expm1(T x)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT(::std::numeric_limits<T>::is_specialized);
#else
   BOOST_ASSERT(std::numeric_limits<T>::is_specialized);
#endif

   T a = std::fabs(x);
   if(a > T(0.5L))
      return std::exp(x) - T(1);
   if(a < std::numeric_limits<T>::epsilon())
      return x;
   detail::expm1_series<T> s(x);
   T result = detail::kahan_sum_series(s, std::numeric_limits<T>::digits + 2);
   return result;
}
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x564))
inline float expm1(float z)
{
   return expm1<float>(z);
}
inline double expm1(double z)
{
   return expm1<double>(z);
}
inline long double expm1(long double z)
{
   return expm1<long double>(z);
}
#endif

#ifdef expm1
#  ifndef BOOST_HAS_expm1
#     define BOOST_HAS_expm1
#  endif
#  undef expm1
#endif

#ifdef BOOST_HAS_EXPM1
#  if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901)
inline float expm1(float x){ return ::expm1f(x); }
inline long double expm1(long double x){ return ::expm1l(x); }
#else
inline float expm1(float x){ return ::expm1(x); }
#endif
inline double expm1(double x){ return ::expm1(x); }
#endif

} } // namespaces

#endif // BOOST_MATH_HYPOT_INCLUDED


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <filesystem>
#include <string>

using namespace std::string_literals;

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "miMaS/field.h"
#include "miMaS/complex_field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/rk.h"
#include "miMaS/config.h"
#include "miMaS/signal_handler.h"
#include "miMaS/iteration.h"

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*fh.step.dx+fh.range.x_min)
#define Vk(k) (k*fh.step.dv+fh.range.v_min)

auto
maxwellian ( double rho , double u , double T ) {
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

int
main ( int argc , char const * argv[] )
{
  std::filesystem::path p("config.init");
  if ( argc > 1 )
    { p = argv[1]; }
  auto c = config(p);
  c.name = "vhll";

  c.create_output_directory();
  std::ofstream ofconfig( c.output_dir / "config.init" );
  ofconfig << c << "\n";
  ofconfig.close();

/* ------------------------------------------------------------------------- */
  field<double,1> fh(boost::extents[c.Nv][c.Nx]);
  complex_field<double,1> hfh(boost::extents[c.Nv][c.Nx]);

  const double Kx = 0.5;
  fh.range.v_min = -8.; fh.range.v_max = 8.;
  fh.range.x_min =  0.; fh.range.x_max = 2./Kx*math::pi<double>();
  fh.compute_steps();

  ublas::vector<double> v(c.Nv,0.);
  std::generate( v.begin() , v.end() , [&,k=0]() mutable {return (k++)*fh.step.dv+fh.range.v_min;} );

  ublas::vector<double> kx(c.Nx); // beware, Nx need to be odd
  {
    double l = fh.range.len_x();
    for ( auto i=0 ; i<c.Nx/2 ; ++i ) { kx[i]      = 2.*math::pi<double>()*i/l; }
    for ( int i=-c.Nx/2 ; i<0 ; ++i ) { kx[c.Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  auto tb_M1 = maxwellian( 0.5*c.alpha , c.ui , 1. ) , tb_M2 = maxwellian( 0.5*c.alpha , -c.ui , 1. );
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
    fft::fft(fh[k].begin(),fh[k].end(),hfh[k].begin());
  }
  fh.write( c.output_dir / ("init_"+c.name+".dat") );

  iteration::iteration<double> iter;
  iter.iter = 0;
  iter.current_time = 0.;
  iter.dt = 0.5*fh.step.dv;

  ublas::vector<double> uc(c.Nx,0.);
  ublas::vector<double> E (c.Nx,0.);

  std::vector<double> ee;   ee.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<double> Emax; Emax.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<double> H;    H.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<iteration::iteration<double>> iterations; iterations.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<iteration::iteration<double>> success_iterations; success_iterations.reserve(int(std::ceil(c.Tf/iter.dt))+1);

  std::vector<double> times; times.reserve(int(std::ceil(c.Tf/iter.dt))+1);

  // lambda to save data, any time you want, where you want
  auto save_data = [&] ( std::string && suffix ) -> void {
    suffix = c.name + suffix;

    // save iterations informations
    auto writer_iter = [] ( auto const & it ) {
      std::stringstream ss; ss << it;
      return ss.str();
    };
    c << monitoring::data( "iterations_"+suffix+".dat"         , iterations         , writer_iter );
    c << monitoring::data( "success_iterations_"+suffix+".dat" , success_iterations , writer_iter );

    // save temporel data
    auto dt_y = [&,count=0] (auto const& y) mutable {
      std::stringstream ss; ss<<times[count++]<<" "<<y;
      return ss.str();
    };
    c << monitoring::data( "ee_"+suffix+".dat"   , ee   , dt_y );
    c << monitoring::data( "Emax_"+suffix+".dat" , Emax , dt_y );
    c << monitoring::data( "H_"+suffix+".dat"    , H    , dt_y );

    // save distribution function
    for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
    fh.write( c.output_dir / ("vp_"+suffix+".dat") );
  };

  // to stop simulation at any time (and save data)
  signal_handler::signal_handler<SIGINT,SIGILL>::handler( [&]( int signal ) -> void {
    std::cerr << "\n\033[41;97m ** End of execution after signal " << signal << " ** \033[0m\n";
    std::cerr << "\033[38;5;202msave data...\033[0m\n";
    save_data("_SIGINT");
  });

  const double rho_c = 1.-c.alpha;
  const double sqrt_rho_c = std::sqrt(rho_c);

  // init E with Poisson solver, init also ee, Emax, H and times
  {
    poisson<double> poisson_solver(c.Nx,fh.range.len_x());
    ublas::vector<double> rho(c.Nx,0.);
    rho = fh.density(); // compute density from init data
    for ( auto i=0 ; i<c.Nx ; ++i ) { rho[i] += (1.-c.alpha); } // add (1-alpha) for cold particules
    E = poisson_solver(rho);

    Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
    double electric_energy = std::sqrt(std::accumulate(
      E.begin() , E.end() , 0. ,
      [&] ( double partial_sum , double ei ) {
        return partial_sum + ei*ei*fh.step.dx;
      }
    ));
    ee.push_back( electric_energy );

    double total_energy = energy(fh,E);
    total_energy += 0.; // sum(rho_c*u_c*u_c) = 0 because u_c = 0 at time 0
    H.push_back( total_energy );
    times.push_back(0.);
  }

  // initialize memory for all temporary variables
  ublas::vector<double> J(c.Nx,0.);
  fft::spectrum_ d(c.Nx);
  field<double,1> Edvf(tools::array_view<const std::size_t>(fh.shape(),2));
  ublas::vector<double> uc1(c.Nx) , uc2(c.Nx) , uc3(c.Nx) , uc4(c.Nx) , uc5(c.Nx) , uc6(c.Nx) , uc7(c.Nx),
                        E1 (c.Nx) , E2(c.Nx)  , E3 (c.Nx) , E4 (c.Nx) , E5 (c.Nx) , E6 (c.Nx) , E7 (c.Nx);
  complex_field<double,1> hfh1(boost::extents[c.Nv][c.Nx]) , hfh2(boost::extents[c.Nv][c.Nx]) ,
                          hfh3(boost::extents[c.Nv][c.Nx]) , hfh4(boost::extents[c.Nv][c.Nx]) ,
                          hfh5(boost::extents[c.Nv][c.Nx]) , hfh6(boost::extents[c.Nv][c.Nx]) ,
                          hfh7(boost::extents[c.Nv][c.Nx]) ;

  while (  iter.current_time < c.Tf ) {
    std::cout << "\r" << iteration::time(iter) << std::flush;

///////////////////////////////////////////////////////////////////////////////
// DP4(3) /////////////////////////////////////////////////////////////////////

    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c05 = std::cos(0.5*iter.dt*sqrt_rho_c), s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc1[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c - 0.5*iter.dt*J[i]*s05/sqrt_rho_c;
        E1[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*iter.dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) - 0.5*iter.dt*d[i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c05 = std::cos(0.5*iter.dt*sqrt_rho_c), s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc2[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c;
        E2[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh2[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) - 0.5*iter.dt*d[i];
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc3[i] =  uc[i]*c1 + E[i]*s1/sqrt_rho_c - iter.dt*J[i]*s05/sqrt_rho_c;
        E3[i]  = -uc[i]*s1*sqrt_rho_c + E[i]*c1 - iter.dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh3[k][i] = hfh[k][i]*std::exp(-I*kx[i]*v[k]*iter.dt) - iter.dt*d[i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt);
        }
      }
    } // end stage 3

    // STAGE 4
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh3[k].begin(),hfh3[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E3);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc4[i] = -(1./3.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./3.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./3.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/3.;
        E4[i]  = -(1./3.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./3.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./3.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/3. - (1./6.)*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh4[k][i] = -(1./3.)*hfh[k][i]*std::exp(-I*kx[i]*v[k]*iter.dt) + (1./3.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) + (2./3.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) + hfh3[k][i]/3. - (1./6.)*iter.dt*d[i];
        }
      }
    } // end stage 4

    // STAGE 5
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh4[k].begin(),hfh4[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E4);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc5[i] = -(1./5.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./5.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./5.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/5. + (2./5.)*uc4[i];
        E5[i]  = -(1./5.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./5.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./5.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/5. + (2./5.)*E4[i] - (1./10.)*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh5[k][i] = -(1./5.)*hfh[k][i]*std::exp(-I*kx[i]*v[k]*iter.dt) + (1./5.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) + (2./5.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) + hfh3[k][i]/5. + (2./5.)*hfh4[k][i] - 0.1*iter.dt*d[i];
        }
      }
    } // end stage 5

///////////////////////////////////////////////////////////////////////////////
// MONITORING /////////////////////////////////////////////////////////////////

    // CHECH local error for compute next time step
    iter.E_error(E5,E4,fh.step.dx);
    iter.hfh_error(hfh5,hfh4,fh.step.dx*fh.step.dv);
    iter.success = std::abs(iter.error() - c.tol) <= c.tol;

    std::cout << " -- " << iteration::error(iter) << std::flush;
    if ( iter.success )
    {
      // SAVE TIME STEP
      std::copy( uc4.begin()  , uc4.end()  , uc.begin()  );
      std::copy( E4.begin()   , E4.end()   , E.begin()   );
      std::copy( hfh4.begin() , hfh4.end() , hfh.begin() );

      Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
      double electric_energy = std::sqrt(std::accumulate(
        E.begin() , E.end() , 0. ,
        [&] ( double partial_sum , double ei ) {
          return partial_sum + ei*ei*fh.step.dx;
        }
      ));
      ee.push_back( electric_energy );

      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      double total_energy = energy(fh,E);
      {
        auto rhoh = fh.density();
        fft::spectrum_ hrhoh(c.Nx); hrhoh.fft(rhoh.begin());
        fft::spectrum_ hE(c.Nx); hE.fft(E.begin());
        fft::spectrum_ hrhoc(c.Nx);
        hrhoc[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
        for ( auto i=1 ; i<c.Nx ; ++i ) {
          hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i];
        }
        ublas::vector<double> rhoc (c.Nx,0.); hrhoc.ifft(rhoc.begin());

        for ( auto i=0 ; i<c.Nx ; ++i ) {
          total_energy += rhoc[i]*uc[i]*uc[i];
        }
      }
      H.push_back( total_energy );

      // increment time
      iter.current_time += iter.dt;
      times.push_back( iter.current_time );
      success_iterations.push_back( iter );
    }
    iterations.push_back( iter );

    ++iter.iter;
    iter.dt = std::pow( c.tol/iter.Lhfh , 0.25 )*iter.dt;
    if ( iter.current_time+iter.dt > c.Tf ) { iter.dt = c.Tf - iter.current_time; }
  } // while (  iter.current_time < c.Tf ) // end of time loop
  std::cout << "\r" << time(iter) << std::endl;

  save_data("dp4");

  auto dx_y = [&,count=0](auto const& y) mutable {
    std::stringstream ss; ss<< fh.step.dx*(count++) <<" "<<y;
    return ss.str();
  };
  c << monitoring::data( "E_"+c.name+".dat" , E , dx_y );

  ublas::vector<double> rho (c.Nx,0.);
  ublas::vector<double> rhoc(c.Nx,0.);
  {
    ublas::vector<double> rhoh = fh.density();
    fft::spectrum_ hrhoh(c.Nx); hrhoh.fft(rhoh.begin());
    fft::spectrum_ hE(c.Nx);    hE.fft(E.begin());

    fft::spectrum_ hrho(c.Nx), hrhoc(c.Nx);

    hrho[0] = I*kx[0]*hE[0] + 1.;
    hrho[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
    for ( auto i=1 ; i<c.Nx ; ++i ) {
      hrho[i]  = I*kx[i]*hE[i];
      hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i];
    }
    hrho.ifft(rho.begin());
  }

  c << monitoring::data( "rho_"+c.name+".dat"  , rho  , dx_y );
  c << monitoring::data( "uc_"+c.name+".dat"   , uc   , dx_y );
  c << monitoring::data( "rhoc_"+c.name+".dat" , rhoc , dx_y );

  J = fh.courant();
  for ( auto i=0 ; i<c.Nx ; ++i ) {
    J[i] += rhoc[i]*uc[i];
  }
  c << monitoring::data( "J_"+c.name+".dat" , J , dx_y );

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2015 Kenjiro Sugimoto
// Released under the MIT license
// http://opensource.org/licenses/mit-license.php
////////////////////////////////////////////////////////////////////////////////

// This code implements the algorithm of the following paper. Please cite it in
// your paper if your research uses this code.
//   + K. Sugimoto and S. Kamata: "Compressive bilateral filtering", IEEE Trans.
//     Image Process., vol. 24, no. 11, pp. 3357-3369 (Nov. 2015).

#pragma once
#define _USE_MATH_DEFINES
#include <iostream>
#include <stdexcept>
#include <algorithm>
#include <string>
#include <vector>
#include <cmath>
#include <cassert>
#ifdef USE_OPENCV2
#include <opencv2/opencv.hpp>
#endif
#ifdef USE_BOOST
#include <boost/math/special_functions/erf.hpp> // for erfc_inv() only
#endif
#include "o1_spatial_gaussian_filter.hpp"

//==============================================================================

class compressive_bilateral_filter
{
private:
	int tone;

	// this parameter will provide sufficient accuracy.
	o1_spatial_gaussian_filter<2> gaussian;
	int K; // number of basis range kernels
	double T; // period length of periodic range kernel
	std::vector<double> sqrta;

public:
	compressive_bilateral_filter(double sigmaS,double sigmaR,double tol=0.10,int tone=256):tone(tone),gaussian(sigmaS)
	{
#ifdef USE_BOOST
		double xi=boost::math::erfc_inv(tol*tol);
#else
		// hard-coding for boost-less running
		double xi;
		     if(tol==0.05) xi=2.1378252338818511;
		else if(tol==0.10) xi=1.8213863677184496;
		else if(tol==0.20) xi=1.4522197815622468;
		else
			throw std::invalid_argument("Unsupported tolerance! ({0.05,0.10,0.20} only or use boost)");
#endif
		// estimating an optimal K
		double s=sigmaR/(tone-1.0); // normalized to dynamic range [0,1]
		K=static_cast<int>(std::ceil(xi*xi/(2.0*M_PI)+xi/(2.0*M_PI*s)-0.5));

		// estimating an optimal T
		derivative_estimated_gaussian_range_kernel_error df(s,K);
		double t1=s*xi+1.0;
		double t2=M_PI*(2*K+1)*s/xi;
		// It is better to slightly extend the original search domain D
		// because it might uncover the minimum of E(T) due to approximate error.
		const double MAGICNUM=0.03;
		T=(tone-1.0)*solve_by_bs(df,t1,t2+MAGICNUM);

		// precomputing the square root of spectrum
		double omega=2.0*M_PI/T;
		sqrta=std::vector<double>(K);
		for(int k=1;k<=K;++k)
			sqrta[k-1]=M_SQRT2*exp(-0.25*omega*omega*sigmaR*sigmaR*k*k);
	}

private:
	/// a scale-adjusted derivative of the estimated Gaussian range kernel error
	class derivative_estimated_gaussian_range_kernel_error
	{
	private:
		double sigma,kappa;
	public:
		derivative_estimated_gaussian_range_kernel_error(double sigma,int K)
			:sigma(sigma),kappa(M_PI*(2*K+1)){}
	public:
		double operator()(double T)
		{
			double phi=(T-1.0)/sigma;
			double psi=kappa*sigma/T;
			return kappa*exp(-phi*phi)-psi*psi*exp(-psi*psi);
		}
	};
	/// solve df(x)==0 by binary search
	template<class Functor>
	inline double solve_by_bs(Functor df,double x1,double x2,int loop=10)
	{
		for(int i=0;i<loop;++i)
		{
			double x=(x1+x2)/2.0;
			((0.0<=df(x))?x2:x1)=x;
		}
		return (x1+x2)/2.0;
	}

public:
#ifdef USE_OPENCV2
	/// O(1) cross/joint bilateral filtering
	/// "guide" has to have dynamic range [0,tone).
	void operator()(const cv::Mat_<double>& src,const cv::Mat_<double>& guide,cv::Mat_<double>& dst)
	{
		assert(src.size()==guide.size());
		assert(src.size()==dst.size());

		// lookup tables (discretized for fast computation)
		std::vector<double> tblC(tone);
		std::vector<double> tblS(tone);
		// component images
		cv::Mat_<cv::Vec4d> compsI(src.size());
		cv::Mat_<cv::Vec4d> compsO(src.size());

		// DC component
		const int winsz=gaussian.window_size();
		cv::Mat_<double> denom(src.size(),winsz*winsz);
		cv::Mat_<double> numer(src.size());
		gaussian.filter_xy(src,numer);

		// AC components
		double omega=2.0*M_PI/T;
		for(int k=1;k<=K;++k)
		{
			// preparing look-up tables
			double omegak=omega*k;
			for(int t=0;t<tone;++t)
			{
				double theta=omegak*t;
				tblC[t]=sqrta[k-1]*cos(theta);
				tblS[t]=sqrta[k-1]*sin(theta);
			}

			// generating k-th component images
			for(int y=0;y<src.rows;++y)
			for(int x=0;x<src.cols;++x)
			{
				int t=int(guide(y,x)); // from guide image
				double c=tblC[t];
				double s=tblS[t];
				double p=src(y,x);
				compsI(y,x)=cv::Vec4d(c*p,s*p,c,s);
			}
			gaussian.filter_xy(compsI,compsO);

			// decompressing k-th components
			for(int y=0;y<src.rows;++y)
			for(int x=0;x<src.cols;++x)
			{
				int t=int(guide(y,x)); // from guide image
				double c=tblC[t];
				double s=tblS[t];
				const cv::Vec4d& values=compsO(y,x);
				numer(y,x)+=c*values[0]+s*values[1];
				denom(y,x)+=c*values[2]+s*values[3];
			}
		}
		dst=numer/denom;
	}
	/// O(1) bilateral filtering
	/// "src" has to have dynamic range [0,tone).
	void operator()(const cv::Mat_<double>& src,cv::Mat_<double>& dst)
	{
		assert(src.size()==dst.size());

		// lookup tables (discretized for fast computation)
		std::vector<double> tblC(tone);
		std::vector<double> tblS(tone);
		// component images
		cv::Mat_<cv::Vec4d> compsI(src.size());
		cv::Mat_<cv::Vec4d> compsO(src.size());

		// DC component
		const int winsz=gaussian.window_size();
		cv::Mat_<double> denom(src.size(),winsz*winsz);
		cv::Mat_<double> numer(src.size());
		gaussian.filter_xy(src,numer);

		// AC components
		double omega=2.0*M_PI/T;
		for(int k=1;k<=K;++k)
		{
			// preparing look-up tables
			double omegak=omega*k;
			for(int t=0;t<tone;++t)
			{
				double theta=omegak*t;
				tblC[t]=sqrta[k-1]*cos(theta);
				tblS[t]=sqrta[k-1]*sin(theta);
			}

			// generating k-th component images
			for(int y=0;y<src.rows;++y)
			for(int x=0;x<src.cols;++x)
			{
				int t=int(src(y,x));
				double c=tblC[t];
				double s=tblS[t];
				double p=src(y,x);
				compsI(y,x)=cv::Vec4d(c*p,s*p,c,s);
			}
			gaussian.filter_xy(compsI,compsO);

			// decompressing k-th components
			for(int y=0;y<src.rows;++y)
			for(int x=0;x<src.cols;++x)
			{
				int t=int(src(y,x));
				double c=tblC[t];
				double s=tblS[t];
				const cv::Vec4d& values=compsO(y,x);
				numer(y,x)+=c*values[0]+s*values[1];
				denom(y,x)+=c*values[2]+s*values[3];
			}
		}
		dst=numer/denom;
	}
#endif
};

//==============================================================================


#include <CGAL/config.h>
#define CGAL_EIGEN3_ENABLED
#if defined(BOOST_GCC) && (__GNUC__ <= 4) && (__GNUC_MINOR__ < 4)
#include <iostream>
int main()
{
  std::cerr << "NOTICE: This test requires G++ >= 4.4, and will not be compiled." << std::endl;
}
#else
#include <CGAL/Epick_d.h>
#include <eigen3/Eigen/Core>
#include <CGAL/Delaunay_triangulation.h>
#include <CGAL/IO/Triangulation_off_ostream.h>
#include <CGAL/point_generators_d.h>
#include <CGAL/Timer.h>
#include <CGAL/algorithm.h>
#include <CGAL/Origin.h>

#include <vector>
#include <random>
#include <string>
#include <fstream>
#include <cstdlib>
#include <algorithm>
#include <cmath>
#include <boost/algorithm/string.hpp>
#include <chrono>

// uncomment this if want generate random points
// #define RANDOM_PTS

const double lower_bound = 0.0;
const double upper_bound = 1.0;
const double step_size = 0.01;
const double small_step_size = 0.001;
//TODO: Change the path_prefix before running the function!
const std::string path_prefix = "/Users/angelynaye/Desktop/Research/result/space-partition-adv";

/** Helper Functions Section  **/

//function to read in data from csv file
std::vector<std::vector<double> > read_data(std::string file_name, int dim,
std::string delimeter = " ")
{
	std::ifstream file(file_name);

	std::vector<std::vector<double> > data_list;

	std::string line = "";
	// Iterate through each line split the content using delimeter
  // convert it to double
	while (getline(file, line))
	{
		std::vector<std::string> vec;
		boost::algorithm::split(vec, line, boost::is_any_of(delimeter));
        std::vector<double> dd;
        dd.reserve(dim);
        for(std::vector<std::string>::iterator it = vec.begin();
        it != vec.end(); ++it) {
          dd.push_back(std::stod(*it));
        }

		data_list.push_back(dd);

	}
	// Close the File
	file.close();

	return data_list;
}

// return true if within [0,1]
template<int D>
bool check_boundary(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a) {
    for(int i = 0; i < D; i++) {
      if(a[i] < lower_bound || a[i] > upper_bound) {
        return false;
      }
    }
    return true;
}

// compute Euclidean distance of dimension D points
template<int D>
double distance(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d b) {
    double distance = 0;
    for(int i = 0; i < D; i++) {
      distance += pow(a[i] - b[i], 2);
    }
    return distance;
  }

// compute distance of all points in vector to another point v
template<int D>
std::vector<double> neighbors_distance(
  std::set<typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Vertex_handle> neighs,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d st_proj,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d cur_n) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;
    typedef typename DT::Vertex_handle Vertex_handle;
    typedef std::set<Vertex_handle> Vertex_set;
    std::vector<double> nei_dists;
    for(typename Vertex_set::iterator neighs_it = neighs.begin();
    neighs_it != neighs.end(); neighs_it++) {
      Point n = (*neighs_it)->point();
      // remove the n on the voronoi edge from the neighbors list
      if(n == cur_n) continue;
      nei_dists.push_back(distance<D>(n, st_proj));
    }
    return nei_dists;
  }

// return dot product of two points
template<int D>
double compute_dot_product(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d b) {
    double product = 0;
    for(int i = 0; i < D; i++) {
      product += a[i] * b[i];
    }
    return product;
  }

// assign the bounding value for edge
template<int D>
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d assign_vals(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;
    double new_a[D];

    for(int i = 0; i < D; i++) {
      if(a[i] < lower_bound ) {
        new_a[i] = lower_bound;
      } else if (a[i] > upper_bound) {
        new_a[i] = upper_bound;
      }
    }
    Point result(&new_a[0], &new_a[D]);
    return result;
}

// return addition of point
template<int D>
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d point_addition(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d b) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;

    // std::vector<double> sum;
    double sum[D];
    // sum.reserve(D);
    for(int i = 0; i < D; i++) {
      sum[i] = a[i] + b[i];
      // sum.push_back(a[i] + b[i]);
    }
    Point result(&sum[0], &sum[D]);
    // Point result(&sum.at(0), &sum.at(sum.size() - 1));
    return result;
  }

// return multiplication of a point and a constant double
template<int D>
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d point_mul(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a,
  double b) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;

    // std::vector<double> mul;
    // mul.reserve(D);
    double mul[D];
    for(int i = 0; i < D; i++) {
      mul[i] = a[i]*b;
      // mul.push_back(a[i]*b);
    }
    Point result(&mul[0], &mul[D]);
    // Point result(&mul.at(0), &mul.at(mul.size() - 1));
    return result;
  }

// return whether right point is larger than left point in a given direction
template<int D>
bool is_larger(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d right,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d left,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d direction) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;

    Point diff = point_addition<D>(right, point_mul<D>(left,  -1.0));
    // since right = left + direction * constant
    // constant is the same for all direction, we only need to check first poistion's c
    double diff_0 = diff[0];
    double direction_0 = direction[0];
    double c = diff_0 / direction_0;
    return c > 0;
  }

// return the projection of a point on half space
template<int D>
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d projection(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d v,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d w,
  double c) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;

    // v - v*unit_w*w + c*unit_w
    double w_norm = sqrt(compute_dot_product<D>(w,w));
    Point unit_w = point_mul<D>(w, 1/w_norm);
    double proj_normal_length = compute_dot_product<D>(v, unit_w);
    Point proj_normal = point_mul<D>(w, proj_normal_length);
    Point proj_v = point_addition<D>(v, proj_normal);
    Point offset = point_mul<D>(unit_w, c);
    return point_addition<D>(proj_v, offset);
  }

// binary search get start and end of the voronoi edge
template<int D>
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d binary_search(
  std::set<typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Vertex_handle> neighs,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d cur_n,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d start,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d direction,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d v,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d w,
  double c,  double cur_dist, double min_nei_dist, bool reverse) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;

    Point original_dir = direction;
    Point left = start;
    Point right = start;
    Point right_proj;
    double mul = 2.0;
    std::vector<double> nei_dists;

    // reverse = true if we want find end point in case 1/ start/end point in case 2:
    // first point closer to other neighs than v: distance(v, proj_pt) >= min_nei_dist
    if(reverse) {
      cur_dist *= -1.0;
      min_nei_dist *= -1.0;
    }

    // find the range of the point

    // find start in case 1: first point closer to v than to other neighs
    // i.e. distance(v, proj_pt) <= min_nei_dist if reverse -> ">="
    while(check_boundary<D>(right) && cur_dist > min_nei_dist) {
      left = right;
      direction = point_mul<D>(direction, mul);
      right = point_addition<D>(right, direction);
      right_proj = projection<D>(right, w, c);
      cur_dist = reverse? -1.0 * distance<D>(right_proj, v) : distance<D>(right_proj, v);
      nei_dists = neighbors_distance<D>(neighs, right_proj, cur_n);
      min_nei_dist = *std::min_element(nei_dists.begin(), nei_dists.end());
      min_nei_dist = reverse? -1.0 * min_nei_dist : min_nei_dist;
    }

    // get `right` inside the boundary if it's out
    // may because the step size is too large that we skip the voronoi edge
    if(!check_boundary<D>(right)) {
      if(cur_dist > min_nei_dist) {
        // case 1: cur_dist > min_nei_dist: redo it with small step size,
        right = start;
        direction = original_dir;
        while(check_boundary<D>(right) && cur_dist > min_nei_dist) {
          left = right;
          right = point_addition<D>(right, direction);
          right_proj = projection<D>(right, w, c);
          cur_dist = reverse? -1.0 * distance<D>(right_proj, v) : distance<D>(right_proj, v);
          nei_dists = neighbors_distance<D>(neighs, right_proj, cur_n);
          min_nei_dist = *std::min_element(nei_dists.begin(), nei_dists.end());
          min_nei_dist = reverse? -1.0 * min_nei_dist : min_nei_dist;
        }
      } else {
        // case 2: cur_dist <= min_nei_dist: get all the points within bound
        right = assign_vals<D>(right);
      }
    }

    Point mid;
    Point mid_proj;
    // (right - left)/ direction > 0
    while(is_larger<D>(right, left, original_dir)) {
      Point diff = point_mul<D>(point_addition<D>(right, point_mul<D>(left, -1.0)), 1/2.0);
      // get mid point
      mid = point_addition<D>(left, diff);
      mid_proj = projection<D>(mid, w, c);
      cur_dist = reverse? -1.0 * distance<D>(mid_proj, v) : distance<D>(mid_proj, v);
      nei_dists = neighbors_distance<D>(neighs, mid_proj, cur_n);
      min_nei_dist = *std::min_element(nei_dists.begin(), nei_dists.end());
      min_nei_dist = reverse? -1.0 * min_nei_dist : min_nei_dist;

      // if midpoint out of bound or midpoint is closer to v than all the other neighbors
      if(!check_boundary<D>(mid) || cur_dist <= min_nei_dist) {
        right = mid;
      } else {
        left = point_addition<D>(mid, original_dir);
      }
    }

    return right;
  }

template<int D>
std::vector<typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d>
find_voronoi_edge(
std::set<typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Vertex_handle> neighs,
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d cur_n,
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d lp,
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d v,
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d w, double c) {

  typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
  typedef CGAL::Delaunay_triangulation<K> DT;
  typedef typename DT::Point_d Point;
  typedef typename DT::Vertex_handle Vertex_handle;
  typedef std::set<Vertex_handle> Vertex_set;

  std::vector<double> dummy = std::vector<double>(D + 1, 0.5);
  int end = dummy.size() - 1;

  Point start(&dummy.at(0), &dummy.at(end));

  double sum = 0;
  // projection of point onto plane
  Point st_proj = projection<D>(start, w, c);
  // calculate v's neighbors' distance
  std::vector<double> nei_dists = neighbors_distance<D>(neighs, st_proj, cur_n);

  double cur_dist = distance<D>(st_proj, v);
  double min_nei_dist = *std::min_element(nei_dists.begin(), nei_dists.end());

  Point start_vor;
  Point end_vor;

  // generate random direction to walk [current random seed based on time]
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator (seed);
  std::uniform_int_distribution<int> dist(-1 , 1);
  double direct = dist(generator); // -1 or 1
  direct *= step_size;

  // case 1: st_proj isn’t included in the voronoi edge
  if(cur_dist > min_nei_dist) {
    Point point_direct = point_mul<D>(lp, direct);
    Point st2 = point_addition<D>(start, point_direct);
    Point st2_proj = projection<D>(st2, w, c);
    double temp_dist = distance<D>(st2_proj, v);
    //oof, we are moving the wrong direction:
    if(temp_dist > cur_dist) {
      point_direct = point_mul<D>(point_direct, -1.0); // flip the direction
    }

    start_vor =
    binary_search<D>(neighs, cur_n, start, point_direct, v, w, c, cur_dist, min_nei_dist, false);

    Point start_vor_proj = projection<D>(start, w, c);
    double dist = distance<D>(start_vor_proj, v);
    nei_dists = neighbors_distance<D>(neighs, start_vor_proj, cur_n);
    double temp_min_nei_dist = *std::min_element(nei_dists.begin(), nei_dists.end());
    end_vor =
    binary_search<D>(neighs, cur_n, start_vor, point_direct, v, w, c, dist, temp_min_nei_dist, true);

  } else {
    Point point_direct = point_mul<D>(w, direct);
    Point another_point_direct = point_mul<D>(point_direct, -1.0); // flip the direction
    start_vor =
    binary_search<D>(neighs, cur_n, start, point_direct, v, w, c, cur_dist, min_nei_dist, true);
    end_vor =
    binary_search<D>(neighs, cur_n, start, another_point_direct, v, w, c, cur_dist, min_nei_dist, true);
  }

  std::vector<Point> result;
  result.reserve(2);
  result.push_back(start_vor);
  result.push_back(end_vor);

  return result;
}

// build LSH
template<int D>
std::vector<std::vector<std::vector<double>>> compute_LSH(std::string file_name,
std::size_t N, std::size_t num_proj, double bucket_size)
{
  typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
  typedef CGAL::Delaunay_triangulation<K> DT;

  typedef typename DT::Vertex Vertex;
  typedef typename DT::Vertex_handle Vertex_handle;
  typedef typename DT::Full_cell Full_cell;
  typedef typename DT::Full_cell_handle Full_cell_handle;
  typedef typename DT::Facet Facet;
  typedef typename DT::Point_d Point;
  typedef typename DT::Geom_traits::RT RT;
  typedef typename DT::Finite_full_cell_const_iterator Finite_full_cell_const_iterator;
  typedef typename DT::Finite_vertex_iterator Finite_vertex_iterator;
  typedef typename DT::Vertex_iterator Vertex_iterator;
  typedef typename DT::Face Face;
  typedef std::set<Vertex_handle> Vertex_set;
  typedef std::vector<Face> Faces;
  typedef CGAL::Random_points_in_cube_d<Point> Random_points_iterator;

  CGAL::Timer cost;  // timer
  std::vector<Point> points;

  // Generate points
  #ifdef RANDOM_PTS
  CGAL::Random rng;
  Random_points_iterator rand_it(D, 1.0, rng); // generate point within the cube with length 1
  std::copy_n(rand_it, N, std::back_inserter(points));
  #endif

  std::string full_file_name = path_prefix + file_name;

  // CSVReader reader(full_file_name);
  std::cout <<"          Reading file: " << file_name << std::endl;

  // Get the data from CSV File
  std::vector<std::vector<double> > data_vec = read_data(full_file_name, D);

  // construct points
  for(int i = 0; i < data_vec.size(); i++) {
    std::vector<double> cur = data_vec.at(i);
    double temp[cur.size()];
    std::copy(cur.begin(), cur.end(), temp);
    Point p(&temp[0], &temp[cur.size()]);
    points.push_back(p);
  }

  #ifdef READ_PTS
  for(int i = 0; i < D; i++) {
    std::cout << "Points 0[ " << i << "]"<< points.at(0)[i] << std::endl;
  }
  #endif

  cost.reset();
  cost.start();

  N = data_vec.size();
  std::cout << "Delaunay triangulation of " << N <<
    " points in dim " << D << ":" << std::endl;

  // create delaunay triangulation with dimension D
  DT dt(D);

  dt.insert(points.begin(), points.end());

  // assert the delaunay triangle is valid
  CGAL_assertion(dt.is_valid());

  // generate random line passing through (0,0), stored it in vector<Point> w
  std::default_random_engine generator;
  double cur[D] = {};
  // randomly generate pts from standard normal distribution
  std::normal_distribution<double> distribution(0.0, 1.0);
  std::vector<Point> proj_lines;

  double sum_sqr;
  for(int i= 0; i< num_proj; ++i) {
    sum_sqr = 0.0;
    for(int j = 0; j < D; j++) {
        cur[j]= distribution(generator);
        sum_sqr += pow(cur[j], 2);
    }

    // check not divide by zero, add a really small constant
    // divide by norm + e^-10
    double norm = sqrt(sum_sqr);
    if(norm == 0) norm += 1e-9;
    for(int j = 0; j < D; j++) {
        cur[j] /= norm;
    }

    Point p(&cur[0], &cur[D]);
    proj_lines.push_back(p);
    std::cout << "\n" << std::endl;
  }

  int max_bucket = 2 * std::ceil(sqrt(D) / bucket_size);
  int bound = max_bucket / 2;
  int num_edges = 0;

  // generate all the neighs for each vertices
  // calculate the #edges for all
  Vertex_iterator fvit = dt.vertices_begin();
  for (;fvit != dt.vertices_end(); fvit++) {
    if(dt.is_infinite(fvit)) continue;
    Vertex_handle curr = fvit;
    // circulate through incident full cells to get all neighbors of current vertex
    std::vector<Full_cell_handle> neigh_full_cellss;
    dt.tds().incident_full_cells(curr, back_inserter(neigh_full_cellss));
    Vertex_handle vhh;
    for(typename std::vector<Full_cell_handle>::iterator it = neigh_full_cellss.begin();
    it != neigh_full_cellss.end(); ++it ) {
        for( int i = 0; i <= dt.current_dimension(); ++i )
        {
            vhh = (*it)->vertex(i);
            if( dt.is_infinite(vhh) || vhh ==  curr)
                continue;
            num_edges++;
        }
    }
  }

  std::vector<std::vector<std::vector<double>>> buks(num_proj,
  std::vector<std::vector<double>>(max_bucket, std::vector<double>(num_edges)));

  // double buks[num_proj][max_bucket][num_edges];

  // iterate through all vertices
  fvit = dt.vertices_begin();
  for (;fvit != dt.vertices_end(); fvit++) {
    if(dt.is_infinite(fvit)) continue;
    Vertex_handle cur = fvit;
    Point v = cur->point();
    // circulate through incident full cells to get all neighbors of current vertex
    std::vector<Full_cell_handle> neigh_full_cells;
    dt.tds().incident_full_cells(cur, back_inserter(neigh_full_cells));
    Vertex_set neighs;
    Vertex_handle vh;
    for(typename std::vector<Full_cell_handle>::iterator it = neigh_full_cells.begin();
    it != neigh_full_cells.end(); ++it ) {
        for( int i = 0; i <= dt.current_dimension(); ++i )
        {
            vh = (*it)->vertex(i);
            if( dt.is_infinite(vh) || vh ==  cur)
                continue;
            neighs.insert(vh);
        }
    }

    int idx = 0;
    Point n;
    // iterate through neighbors
    for(typename Vertex_set::iterator neighs_it = neighs.begin();
    neighs_it != neighs.end(); neighs_it++) {
      n = (*neighs_it)->point();

      // find the half space
      Point w = point_addition<D>(v, point_mul<D>(n, -1.0)); // v - n

      Point half_point = point_mul<D>(point_addition<D>(n, v), 1 /2.0); // (n + v) / 2

      double product = compute_dot_product<D>(w, half_point);

      // iterate through project line
      for(std::size_t i = 0; i != proj_lines.size(); i++) {
        Point lp = proj_lines[i];
        std::vector<Point> edge = find_voronoi_edge<D>(neighs, n, lp, v, w, product);

        Point start = edge.at(0);
        Point end = edge.at(1);
        // project the start and end point to the line
        double a = compute_dot_product<D>(w, start);
        double b = compute_dot_product<D>(w, end);
        int s_idx = std::max(ceil(std::min(a,b)), lower_bound);
        int e_idx = std::min(upper_bound, ceil(std::max(a,b)));

        std::cout<< "s_idx " << "line: "<<i<< " neigh: "<<idx << " is " << s_idx << std::endl;
        std::cout<< "e_idx " << "line: "<<i<< " neigh: "<<idx << " is " << e_idx << std::endl;

        for(int s = s_idx; s != e_idx + 1; s++) {
          // buks[i][s][idx] += 1;
          buks.at(i).at(s).at(idx) += 1;
        }
      }
      // std::cout << "I am here ------------" << std::endl;
      idx++;
    }
  }

  double timing = cost.time();

  std::cout<< "Total computation time is: " << timing << std::endl;
  return buks;

}

int main(int argc, char **argv)
{
    srand(static_cast<unsigned int>(time(NULL)));

    std::vector<std::vector<std::vector<double>>> lsh_buk =
    compute_LSH<2>("data.csv", 10, 5, 0.1);

    return 0;
}
#endif

// FIXME: low dimension, 3 , put 5 - 10 points, know exactly the edges
// fix the projection line [eliminate all randomness]
// get the neighbors, check correct
// get the voronoi edges, check
// test the projection
//