#pragma once
#include "VimCommon.h"
#pragma warning (disable:4756)

#include <iostream>
#include <queue>

#ifdef _OPENMP
#include <omp.h>
#endif // _OPENMP

#ifndef _OPENMP
int omp_get_num_procs(void) { return 1; }
int omp_get_thread_num(void) { return 0; }
#endif // _OPENMP

#define _f3_(x, y, z) (float)(x), (float)(y), (float)(z)
#define _i3_(x, y, z) (int)(x), (int)(y), (int)(z)
#define _fp_ float*

#define __v3minmax(__INOUT, __IN, __OP) {\
	__INOUT.x = __OP(__INOUT.x, __IN.x);\
	__INOUT.y = __OP(__INOUT.y, __IN.y);\
	__INOUT.z = __OP(__INOUT.z, __IN.z);\
};

#define EXB 2

using namespace vmobjects;
using namespace vmmath;
using namespace std;

template <typename T>
inline T __ReadVoxel(const vmint3& idx, const int width_slice, const int exb, T** vol_slices)
{
	return vol_slices[idx.z + exb][idx.x + exb + (idx.y + exb) * width_slice];
};

template <typename T>
inline void __WriteVoxel(T v, const vmint3& idx, const int width_slice, const int exb, T** vol_slices)
{
	vol_slices[idx.z + exb][idx.x + exb + (idx.y + exb) * width_slice] = v;
};

inline vmint3 __MultInt3(const vmint3* pi3_0, const vmint3* pi3_1)
{
	return vmint3(pi3_0->x * pi3_1->x, pi3_0->y * pi3_1->y, pi3_0->z * pi3_1->z);
}

inline bool __SafeCheck(const vmint3& idx, const vmint3& vol_size)
{
	vmint3 max_dir = idx - (vol_size - vmint3(1, 1, 1));
	vmint3 mult_dot = __MultInt3(&max_dir, &idx);
	return !(mult_dot.x > 0 || mult_dot.y > 0 || mult_dot.z > 0);
}

template <typename T>
inline T __Safe__ReadVoxel(const vmint3& idx, const vmint3& vol_size, const int width_slice, const int exb, T** vol_slices, const T bnd_v = 0)
{
	return __SafeCheck(idx, vol_size) ? __ReadVoxel<T>(idx, width_slice, exb, vol_slices) : (T)bnd_v;
};

inline float __TrilinearInterpolation(float v_0, float v_1, float v_2, float v_3, float v_4, float v_5, float v_6, float v_7,
	const vmfloat3& ratio)
{
	float v01 = v_0 * (1.f - ratio.x) + v_1 * ratio.x;
	float v23 = v_2 * (1.f - ratio.x) + v_3 * ratio.x;
	float v0123 = v01 * (1.f - ratio.y) + v23 * ratio.y;
	float v45 = v_4 * (1.f - ratio.x) + v_5 * ratio.x;
	float v67 = v_6 * (1.f - ratio.x) + v_7 * ratio.x;
	float v4567 = v45 * (1.f - ratio.y) + v67 * ratio.y;
	return v0123 * (1.f - ratio.z) + v4567 * ratio.z;
}

template <typename T>
inline float __Safe_TrilinearSample(const vmfloat3& pos_sample, const vmint3& vol_size, const int width_slice, const int exb, T** vol_slices, const T bnd_v = 0)
{
	vmfloat3 __pos_sample = pos_sample + vmfloat3(1000.f, 1000.f, 1000.f); // SAFE //
	vmint3 idx_sample = vmint3((int)__pos_sample.x, (int)__pos_sample.y, (int)__pos_sample.z);
	vmfloat3 ratio = vmfloat3((__pos_sample.x) - (float)(idx_sample.x), (__pos_sample.y) - (float)(idx_sample.y), (__pos_sample.z) - (float)(idx_sample.z));
	idx_sample -= vmint3(1000, 1000, 1000);

	float v0, v1, v2, v3, v4, v5, v6, v7;
	v0 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(0, 0, 0), vol_size, width_slice, exb, vol_slices, bnd_v);
	v1 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(1, 0, 0), vol_size, width_slice, exb, vol_slices, bnd_v);
	v2 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(0, 1, 0), vol_size, width_slice, exb, vol_slices, bnd_v);
	v3 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(1, 1, 0), vol_size, width_slice, exb, vol_slices, bnd_v);
	v4 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(0, 0, 1), vol_size, width_slice, exb, vol_slices, bnd_v);
	v5 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(1, 0, 1), vol_size, width_slice, exb, vol_slices, bnd_v);
	v6 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(0, 1, 1), vol_size, width_slice, exb, vol_slices, bnd_v);
	v7 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(1, 1, 1), vol_size, width_slice, exb, vol_slices, bnd_v);
	return __TrilinearInterpolation(v0, v1, v2, v3, v4, v5, v6, v7, ratio);
};

template <typename T>
inline vmfloat3 __Safe_Gradient_by_Samples(const vmfloat3& pos_sample, const vmint3& vol_size, const vmfloat3& dir_x, const vmfloat3& dir_y, const vmfloat3& dir_z, const int width_slice, const int exb, T** vol_slices)
{
	float v_XL = __Safe_TrilinearSample<T>(pos_sample - dir_x, vol_size, width_slice, exb, vol_slices);
	float v_XR = __Safe_TrilinearSample<T>(pos_sample + dir_x, vol_size, width_slice, exb, vol_slices);
	float v_YL = __Safe_TrilinearSample<T>(pos_sample - dir_y, vol_size, width_slice, exb, vol_slices);
	float v_YR = __Safe_TrilinearSample<T>(pos_sample + dir_y, vol_size, width_slice, exb, vol_slices);
	float v_ZL = __Safe_TrilinearSample<T>(pos_sample - dir_z, vol_size, width_slice, exb, vol_slices);
	float v_ZR = __Safe_TrilinearSample<T>(pos_sample + dir_z, vol_size, width_slice, exb, vol_slices);
	float g_x = v_XR - v_XL;
	float g_y = v_YR - v_YL;
	float g_z = v_ZR - v_ZL;
	return vmfloat3(g_x, g_y, g_z) * 0.5f;
}

#define SGRAD(p, vinfo) __Safe_Gradient_by_Samples(p, vinfo.vol_size, \
	vinfo.vec_grad_dirs[0], vinfo.vec_grad_dirs[1], vinfo.vec_grad_dirs[2], \
		vinfo.width_slice, EXB, vinfo.vol_slices)

template <typename T>
void ___debugout(std::string str, T v)
{
	std::cout << str.c_str() << std::to_string(v).c_str() << std::endl;
}

template <typename T>
struct __VolSampleInfo
{
	//ushort** vol_slices_filtered;
	T** vol_slices;
	vmint3 vol_size;
	int width_slice;
	vmmat44f mat_ws2vs;
	vmmat44f mat_vs2ws;
	vmfloat3 vec_grad_dirs[3];
	float min_sample_dist;

	__VolSampleInfo() : vol_slices(NULL), //vol_slices_origin(NULL),
		vol_size(vmint3()), width_slice(0), mat_ws2vs(NULL), mat_vs2ws(NULL) {};

	__VolSampleInfo(T** _vol_slices, vmint3 _vol_size,
		int _width_slice, vmmat44f _mat_ws2vs, vmmat44f _mat_vs2ws,
		vmfloat3 _vec_grad_dirs[3], float _min_sample_dist) : vol_slices(_vol_slices),
		vol_size(_vol_size), width_slice(_width_slice), mat_ws2vs(_mat_ws2vs), mat_vs2ws(_mat_vs2ws), min_sample_dist(_min_sample_dist)
	{
		//vol_slices_filtered = _vol_slices;
		vec_grad_dirs[0] = _vec_grad_dirs[0];
		vec_grad_dirs[1] = _vec_grad_dirs[1];
		vec_grad_dirs[2] = _vec_grad_dirs[2];
	}
};

template <typename T>
__VolSampleInfo<T> Get_volsample_info(VmVObjectVolume* pCVolume, const double sample_dist_scale)
{
	if (pCVolume == NULL) return __VolSampleInfo<T>();

	vmfloat3 vecVoxelGradDirs[3];// = { vmfloat3(1, 0, 0), vmfloat3(0, 1, 0), vmfloat3(0, 0, 1) };
	VolumeData* volArchive = pCVolume->GetVolumeData();
	float min_dist_sample = (float)min(min(volArchive->vox_pitch.x, volArchive->vox_pitch.y), volArchive->vox_pitch.z);
	fTransformVector(&vecVoxelGradDirs[0], &vmfloat3(min_dist_sample, 0, 0), &pCVolume->GetMatrixWS2OSf());
	fTransformVector(&vecVoxelGradDirs[1], &vmfloat3(0, min_dist_sample, 0), &pCVolume->GetMatrixWS2OSf());
	fTransformVector(&vecVoxelGradDirs[2], &vmfloat3(0, 0, min_dist_sample), &pCVolume->GetMatrixWS2OSf());

	vmfloat3 vecVoxelGradDirs_unit[3] = {
		vecVoxelGradDirs[0] * (float)sample_dist_scale,
		vecVoxelGradDirs[1] * (float)sample_dist_scale,
		vecVoxelGradDirs[2] * (float)sample_dist_scale };

	VolumeData *volArchiveSample = pCVolume->GetVolumeData();
	vmint3 volSize = volArchiveSample->vol_size;
	vmint3 volSizeEx = volArchiveSample->bnd_size;
	int widthSamplePitch = volSize.x + volSizeEx.x * 2;

	return __VolSampleInfo<T>((T**)volArchiveSample->vol_slices,
		volSize, widthSamplePitch, pCVolume->GetMatrixWS2OSf(), pCVolume->GetMatrixOS2WSf(), vecVoxelGradDirs_unit, min_dist_sample);
};

#include "../nanoflann.hpp"
template <typename T, typename TT>
struct PointCloud
{
	//public:
	const TT* pts;
	const size_t num_pts;
	PointCloud(const TT* _pts, const size_t _num_pts) : pts(_pts), num_pts(_num_pts) { }
	PointCloud(const vector<TT>& vtr_pts) : pts(&vtr_pts[0]), num_pts(vtr_pts.size()) { }

	// Must return the number of data points
	inline size_t kdtree_get_point_count() const { return num_pts; }

	// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
	inline T kdtree_distance(const T *p1, const size_t idx_p2, size_t) const
	{
		const T d0 = p1[0] - pts[idx_p2].x;
		const T d1 = p1[1] - pts[idx_p2].y;
		const T d2 = p1[2] - pts[idx_p2].z;
		return d0 * d0 + d1 * d1 + d2 * d2;
	}

	// Returns the dim'th component of the idx'th point in the class:
	// Since this is inlined and the "dim" argument is typically an immediate value, the
	//  "if/else's" are actually solved at compile time.
	inline T kdtree_get_pt(const size_t idx, int dim) const
	{
		if (dim == 0) return pts[idx].x;
		else if (dim == 1) return pts[idx].y;
		else return pts[idx].z;
	}

	// Optional bounding-box computation: return false to default to a standard bbox computation loop.
	//   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
	//   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
	template <class BBOX>
	bool kdtree_get_bbox(BBOX&) const { return false; }
};

typedef nanoflann::KDTreeSingleIndexAdaptor<
	nanoflann::L2_Simple_Adaptor<float, PointCloud<float, vmfloat3> >,
	PointCloud<float, vmfloat3>,
	3 // dim
> kd_tree_t;

void make_band(__VolSampleInfo<char>& band_info, const int v_low, const int v_high, const __VolSampleInfo<ushort>& vol_info_outside, const __VolSampleInfo<ushort>& vol_info_inside)
{
	const int clip_bnd = 2;
#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int z = 0; z < band_info.vol_size.z; z++)
		for (int y = 0; y < band_info.vol_size.y; y++)
			for (int x = 0; x < band_info.vol_size.x; x++)
			{
				if (x < clip_bnd || y < clip_bnd || z < clip_bnd
					|| x >= band_info.vol_size.x - clip_bnd
					|| y >= band_info.vol_size.y - clip_bnd
					|| z >= band_info.vol_size.z - clip_bnd)
					continue;
				int v_out = __ReadVoxel(vmint3(x, y, z), vol_info_outside.width_slice, EXB, vol_info_outside.vol_slices);
				int v_in = __ReadVoxel(vmint3(x, y, z), vol_info_inside.width_slice, EXB, vol_info_inside.vol_slices);
				if (v_in > v_high) __WriteVoxel((char)-1, vmint3(x, y, z), band_info.width_slice, EXB, band_info.vol_slices);
				else if (v_out < v_low) __WriteVoxel((char)1, vmint3(x, y, z), band_info.width_slice, EXB, band_info.vol_slices);
			}
}

template <typename T>
void get_isosurface_vtx(set<tuple<float, float, float>>& voxel_pts_set,
	const float isovalue, const uint sampleoffset, const __VolSampleInfo<T>& vol_info)
{
	auto __read_v = [&](vmint3& idx)
	{
		idx.x = min(idx.x, vol_info.vol_size.x - 1);
		idx.y = min(idx.y, vol_info.vol_size.y - 1);
		idx.z = min(idx.z, vol_info.vol_size.z - 1);
		return (float)__ReadVoxel(idx, vol_info.width_slice, EXB, vol_info.vol_slices);
	};

	const float offset = (float)sampleoffset;
	for (int z = 0; z <= vol_info.vol_size.z; z += sampleoffset)
	{
		for (int y = 0; y <= vol_info.vol_size.y; y += sampleoffset)
		{
			for (int x = 0; x <= vol_info.vol_size.x; x += sampleoffset)
			{
				// sample at 8 nodes of a cell
				float samplevalues[4] = {
					__read_v(vmint3(x + 0, y + 0, z + 0)),
					__read_v(vmint3(x + sampleoffset, y + 0, z + 0)),
					__read_v(vmint3(x + 0, y + sampleoffset, z + 0)),
					__read_v(vmint3(x + 0, y + 0, z + sampleoffset)),
				};

				bool isOriSmall = samplevalues[0] < isovalue;
				bool isEdgeXSmall = samplevalues[1] < isovalue;
				bool isEdgeYSmall = samplevalues[2] < isovalue;
				bool isEdgeZSmall = samplevalues[3] < isovalue;

				if (isOriSmall != isEdgeXSmall)
				{
					float ratio = (float)(isovalue - samplevalues[0]) / (float)(samplevalues[1] - samplevalues[0]);
					vmfloat3 pos = vmfloat3((float)x + ratio * offset, (float)y, (float)z);
					voxel_pts_set.insert(tuple<float, float, float>(pos.x, pos.y, pos.z));
				}
				if (isOriSmall != isEdgeYSmall)
				{
					float ratio = (float)(isovalue - samplevalues[0]) / (float)(samplevalues[2] - samplevalues[0]);
					vmfloat3 pos = vmfloat3((float)x, (float)y + ratio * offset, (float)z);
					voxel_pts_set.insert(tuple<float, float, float>(pos.x, pos.y, pos.z));
				}
				if (isOriSmall != isEdgeZSmall)
				{
					float ratio = (float)(isovalue - samplevalues[0]) / (float)(samplevalues[3] - samplevalues[0]);
					vmfloat3 pos = vmfloat3((float)x, (float)y, (float)z + ratio * offset);
					voxel_pts_set.insert(tuple<float, float, float>(pos.x, pos.y, pos.z));
				}
			} // for x
		} // for y
	} //for z
}

void simplify_points_ugrid(vector<vmfloat3>& pos_simplified_pts, vmfloat3& aabb_diff, const vector<vmfloat3>& pos_pts, const float grid_length)
{
	uint num_pts = (uint)pos_pts.size();
	vmfloat3 aabb_min(FLT_MAX), aabb_max(-FLT_MAX);
	for (uint i = 0; i < num_pts; i++)
	{
		const vmfloat3& pos_pt = pos_pts[i];
		__v3minmax(aabb_min, pos_pt, min);
		__v3minmax(aabb_max, pos_pt, max);
	}
	aabb_diff = aabb_max - aabb_min;
	vmint3 aabb_size = vmint3(aabb_diff / grid_length) + vmint3(1);
	uint* index_map = new uint[aabb_size.x * aabb_size.y * aabb_size.z];
	memset(index_map, 0, sizeof(uint) * aabb_size.x * aabb_size.y * aabb_size.z);

	uint count = 0;
	for (uint i = 0; i < num_pts; i++)
	{
		const vmfloat3& pos_pt = pos_pts[i];
		vmfloat3 pos_cell = (pos_pt - aabb_min) / grid_length;
		vmint3 idx_cell = pos_cell;
		uint addr = (uint)idx_cell.x + (uint)(idx_cell.y * aabb_size.x) + (uint)idx_cell.z * (uint)(aabb_size.x * aabb_size.y);
		uint prev_idx = index_map[addr];
		if (prev_idx == 0)
		{
			index_map[addr] = i + 1;
			count++;
		}
		else
		{
			const vmfloat3& pos_prev_pt = pos_pts[prev_idx];
			vmfloat3 pos_prev_cell = (pos_prev_pt - aabb_min) / grid_length;
			vmint3 idx_prev_cell = pos_prev_cell;

			vmfloat3 cell_pos = pos_cell - (vmfloat3)idx_cell - vmfloat3(0.5f);
			vmfloat3 prev_cell_pos = pos_prev_cell - (vmfloat3)idx_prev_cell - vmfloat3(0.5f);
			if (fLengthVectorSq(&cell_pos) < fLengthVectorSq(&prev_cell_pos))
				index_map[addr] = i + 1;
		}
	}

	pos_simplified_pts.assign(count, vmfloat3());
	uint count_idx = 0;
	for (uint i = 0; i < (uint)aabb_size.x * (uint)aabb_size.y * (uint)aabb_size.z; i++)
	{
		uint idx = index_map[i];
		if (idx > 0)
			pos_simplified_pts[count_idx++] = pos_pts[idx - 1];
	}

	VMSAFE_DELETEARRAY(index_map);
}

int OtsuThresholdValue(ullong* pullHistogramValues, int histo_size, int begin_idx, int end_idx)
{
	double total_elements = 0;
	double sum1 = 0;

	//for (int i = begin_idx; i <= end_idx; i++)
	for (int i = 0; i < histo_size; i++)
	{
		total_elements += (double)pullHistogramValues[i];
		sum1 += (double)i * (double)pullHistogramValues[i];
	}

	double sumB = 0;
	double wB = 0;
	double _maximum = 0;
	begin_idx = min(begin_idx, histo_size - 1);
	end_idx = min(end_idx, histo_size - 1);

	int otsuThreshold = 0;
	for (int i = begin_idx; i <= end_idx; i++)
	{
		wB += (double)pullHistogramValues[i];
		double wF = total_elements - wB;
		if (wB == 0 || wF == 0)
			continue;

		sumB += (double)i * (double)pullHistogramValues[i];
		double mF = (sum1 - sumB) / wF;
		double btn = wB * wF * ((sumB / wB) - mF) * ((sumB / wB) - mF);
		if (btn >= _maximum)
		{
			otsuThreshold = i;
			_maximum = btn;
		}
	}

	return otsuThreshold;
}

void raytraversal_gradmax(std::vector<vmfloat3>& lmax_pts,
	const std::vector<vmfloat3>& pos_pts, const std::vector<vmfloat3>& dir_pts,
	const int num_maxrc_steps,
	const float dir_scale, /*const bool use_quadinterpolation, const bool use_consistent_dir,*/
	const float min_v, const float max_v,
	const __VolSampleInfo<char>& band_info,
	const __VolSampleInfo<ushort>& vol_info)
{
	auto __gradient = [&](const vmfloat3& pos_vs)
	{
		return SGRAD(pos_vs, vol_info);
		//return -__Safe_TrilinearSample_T(pos_vs, mask_Info.vol_size, mask_Info.width_slice, 0, mask_Info.grad_slices, vmfloat3());
	};

	int num_pts = (int)pos_pts.size();
	lmax_pts.assign(num_pts, vmfloat3(0, 0, 0));
	//std::vector<__float3> tmp_lmax_pts(num_pts, __float3(FLT_MAX, FLT_MAX, FLT_MAX));
	vmfloat3 vol_size_f(_f3_(vol_info.vol_size.x, vol_info.vol_size.y, vol_info.vol_size.z));
	const float safe_bnd = 2.f;

#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int i = 0; i < num_pts; i++)
	{
		const vmfloat3& dir = dir_pts[i];
		if (fLengthVectorSq(&dir) < FLT_EPSILON)
		{
			lmax_pts[i] = pos_pts[i];
			continue;
		}

		float max_gmsq = -1;
		const vmfloat3& pos_start = pos_pts[i];
		vmfloat3 pos_lmax = pos_start;

		for (int j = 0; j < num_maxrc_steps; j++)
		{
			vmfloat3 pos_cur = pos_start + dir * dir_scale * (float)j, pos_cur_vs;
			fTransformPoint(&pos_cur_vs, &pos_cur, &vol_info.mat_ws2vs);

			if (pos_cur_vs.x < safe_bnd || pos_cur_vs.y < safe_bnd || pos_cur_vs.z < safe_bnd
				|| pos_cur_vs.x >= vol_size_f.x - safe_bnd
				|| pos_cur_vs.y >= vol_size_f.y - safe_bnd
				|| pos_cur_vs.z >= vol_size_f.z - safe_bnd)
				continue;

			if (j > 1 && band_info.vol_slices) // j > 1 is for safe zone
			{
				float mask_value = __Safe_TrilinearSample<char>(pos_cur_vs, band_info.vol_size, band_info.width_slice, EXB, band_info.vol_slices, -1);
				if (mask_value == 1.f || mask_value == -1.f)
					break;
			}

			vmfloat3 grad = SGRAD(pos_cur_vs, vol_info);
			float sample_v = __Safe_TrilinearSample<ushort>(pos_cur_vs, vol_info.vol_size, vol_info.width_slice, EXB, vol_info.vol_slices, -1);
			if (fDotVector(&dir, &grad) >= 0 || sample_v  < min_v || sample_v > max_v)
				continue;

			float gmsq = fLengthVectorSq(&grad);
			if (max_gmsq < gmsq)
			{
				max_gmsq = gmsq;
				pos_lmax = pos_cur;
			}
		}

//#define QUADRATIC
#ifdef QUADRATIC
		auto quadratic_maxsampler = [](const vmfloat3& pos_a, const vmfloat3& pos_c, const float gm_a, const float gm_b, const float gm_c)
		{
			vmfloat3 pos_max;
			float _div_ = (gm_a + gm_c) - 2.f * gm_b;
			if (_div_ == 0)
			{
				pos_max = pos_a;
			}
			else
			{
				float t = (3.f * gm_a + gm_c - 4.f * gm_b) / (4.f * _div_);
				pos_max = pos_a + min(max(t, 0.f), 1.f) * (pos_c - pos_a);
			}
			return pos_max;
		};
		if (max_gmsq > 0)
		{
			vmfloat3 pos_a = pos_lmax - dir * dir_scale, pos_a_vs;
			vmfloat3 pos_c = pos_lmax + dir * dir_scale, pos_c_vs;
			fTransformPoint(&pos_a_vs, &pos_a, &vol_info.mat_ws2vs);
			fTransformPoint(&pos_c_vs, &pos_c, &vol_info.mat_ws2vs);
			float edge_a = fLengthVector(&SGRAD(pos_a_vs, vol_info));
			float edge_c = fLengthVector(&SGRAD(pos_c_vs, vol_info));
			lmax_pts[i] = quadratic_maxsampler(pos_a, pos_c, edge_a, sqrt(max_gmsq), edge_c);
		}
		else
			lmax_pts[i] = pos_lmax;
#else
		lmax_pts[i] = pos_lmax;
#endif
	}
}

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/Cholesky>
#include <Eigen/Householder>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#define _DIM_ 3
auto __construct_covariance_matrix = [](const Eigen::VectorXd& cov) -> Eigen::MatrixXd
{
	Eigen::MatrixXd m(_DIM_, _DIM_);

	for (std::size_t i = 0; i < _DIM_; ++i)
	{
		for (std::size_t j = i; j < _DIM_; ++j)
		{
			m(i, j) = static_cast<float>(cov[(_DIM_ * i) + j - ((i * (i + 1)) / 2)]);

			if (i != j)
				m(j, i) = m(i, j);
		}
	}

	return m;
};
auto __diagonalize_selfadjoint_matrix = [](Eigen::MatrixXd& m, Eigen::MatrixXd& eigenvectors, Eigen::VectorXd& eigenvalues) -> bool
{
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver;

	//eigensolver.computeDirect(m);
	eigensolver.compute(m);

	if (eigensolver.info() != Eigen::Success)
		return false;

	eigenvalues = eigensolver.eigenvalues();
	eigenvectors = eigensolver.eigenvectors();

	return true;
};
auto __diagonalize_selfadjoint_covariance_matrix = [&]
(const Eigen::VectorXd& cov, float* eigenvalues, vmfloat3* eigenvectors)
{
	Eigen::MatrixXd m = __construct_covariance_matrix(cov);

	// Diagonalizing the matrix
	Eigen::VectorXd eigenvalues_;
	Eigen::MatrixXd eigenvectors_;
	bool res = __diagonalize_selfadjoint_matrix(m, eigenvectors_, eigenvalues_);

	if (res)
	{
		for (std::size_t i = 0; i < _DIM_; ++i)
		{
			eigenvalues[i] = static_cast<float>(eigenvalues_[i]);

			for (std::size_t j = 0; j < _DIM_; ++j)
				((float*)eigenvectors)[_DIM_*i + j] = static_cast<float>(eigenvectors_(j, i));
		}
	}

	return res;
};

float estimate_shape_variation(const vmfloat3& pos_src, const vmfloat3& nrl_src, const vmfloat3* pos_pts, const vmfloat3* nrl_pts, const kd_tree_t& kdt_index, const float kernel_radius)
{
	float sv = 1.f;
	const float r_sq = kernel_radius * kernel_radius;

	static nanoflann::SearchParams params;
	params.sorted = false;

	std::vector<std::pair<size_t, float>> ret_matches;
	const int nMatches = (int)kdt_index.radiusSearch((float*)&pos_src, r_sq, ret_matches, params);

	std::vector<int> valid_idx_pts;
	for (int k = 0; k < nMatches; k++)
	{
		int idx_neighbor = (int)ret_matches[k].first;
		vmfloat3 nrl_nb = nrl_pts[idx_neighbor];
		if (nrl_src.x * nrl_nb.x + nrl_src.y * nrl_nb.y + nrl_src.z * nrl_nb.z >= 0)
			valid_idx_pts.push_back(idx_neighbor);
	}

	const int num_valid_nb_pts = (int)valid_idx_pts.size();
	if (num_valid_nb_pts < 3)
		return 1; // treated as an outlier

	vmdouble3 pos_centroid = vmdouble3();
	double ft_num_ptns = (double)nMatches;
	for (int k = 0; k < nMatches; k++)
	{
		int idx_nb = (int)ret_matches[k].first;
		vmdouble3 pos_nb = pos_pts[idx_nb];
		pos_centroid.x += ((double)pos_nb.x / ft_num_ptns);
		pos_centroid.y += ((double)pos_nb.y / ft_num_ptns);
		pos_centroid.z += ((double)pos_nb.z / ft_num_ptns);
	}

	vmfloat3 pos_centroid_f = vmfloat3((float)pos_centroid.x, (float)pos_centroid.y, (float)pos_centroid.z);
	//Eigen::VectorXd evecs(6); evecs << 0, 0, 0, 0, 0, 0;
	Eigen::VectorXd evecs = Eigen::VectorXd::Zero(6);
	for (int k = 0; k < nMatches; k++)
	{
		int idx_nb = (int)ret_matches[k].first;
		vmfloat3 pos_nb = pos_pts[idx_nb];
		vmfloat3 diff = vmfloat3(pos_centroid_f.x - pos_nb.x, pos_centroid_f.y - pos_nb.y, pos_centroid_f.z - pos_nb.z);
		evecs(0) += diff.x * diff.x;
		evecs(1) += diff.x * diff.y;
		evecs(2) += diff.x * diff.z;
		evecs(3) += diff.y * diff.y;
		evecs(4) += diff.y * diff.z;
		evecs(5) += diff.z * diff.z;
	}

	float eigenvalues[3];
	vmfloat3 eigenvectors[3];
	if (__diagonalize_selfadjoint_covariance_matrix(evecs, eigenvalues, eigenvectors))
	{
		// eigenvalues[0] is smallest
		float sum_egv = eigenvalues[0] + eigenvalues[1] + eigenvalues[2];
		sv = eigenvalues[0] / sum_egv * 3.f;
	}

	return sv;
}

float estimate_orient_variation(const vmfloat3& pos_src, const __VolSampleInfo<ushort>& vol_info, const float kernel_radius)
{
	auto __sample_v = [&](const vmfloat3& pos_vs)
	{
		return __Safe_TrilinearSample(pos_vs, vol_info.vol_size, vol_info.width_slice, EXB, vol_info.vol_slices);
	};

	float ov = 1.f;
	float voxel_kernel = kernel_radius / vol_info.min_sample_dist;
	vmfloat3 dirs[3] = { vol_info.vec_grad_dirs[0] * 0.5f * voxel_kernel, vol_info.vec_grad_dirs[1] * 0.5f * voxel_kernel, vol_info.vec_grad_dirs[2] * 0.5f * voxel_kernel };

	vmfloat3 pos_sample_vs;
	fTransformPoint(&pos_sample_vs, &pos_src, &vol_info.mat_ws2vs);

	float v = __sample_v(pos_sample_vs);
	float v_XXR = __sample_v(pos_sample_vs + 2.f * dirs[0]);
	float v_XXL = __sample_v(pos_sample_vs - 2.f * dirs[0]);
	float v_YYR = __sample_v(pos_sample_vs + 2.f * dirs[1]);
	float v_YYL = __sample_v(pos_sample_vs - 2.f * dirs[1]);
	float v_ZZR = __sample_v(pos_sample_vs + 2.f * dirs[2]);
	float v_ZZL = __sample_v(pos_sample_vs - 2.f * dirs[2]);
	float v_XR = __sample_v(pos_sample_vs + dirs[0]);
	float v_XL = __sample_v(pos_sample_vs - dirs[0]);
	float v_YR = __sample_v(pos_sample_vs + dirs[1]);
	float v_YL = __sample_v(pos_sample_vs - dirs[1]);
	float v_ZR = __sample_v(pos_sample_vs + dirs[2]);
	float v_ZL = __sample_v(pos_sample_vs - dirs[2]);
	float v_XRYR = __sample_v(pos_sample_vs + dirs[0] + dirs[1]);
	float v_XRYL = __sample_v(pos_sample_vs + dirs[0] - dirs[1]);
	float v_XLYR = __sample_v(pos_sample_vs - dirs[0] + dirs[1]);
	float v_XLYL = __sample_v(pos_sample_vs - dirs[0] - dirs[1]);
	float v_YRZR = __sample_v(pos_sample_vs + dirs[1] + dirs[2]);
	float v_YRZL = __sample_v(pos_sample_vs + dirs[1] - dirs[2]);
	float v_YLZR = __sample_v(pos_sample_vs - dirs[1] + dirs[2]);
	float v_YLZL = __sample_v(pos_sample_vs - dirs[1] - dirs[2]);
	float v_XRZR = __sample_v(pos_sample_vs + dirs[0] + dirs[2]);
	float v_XRZL = __sample_v(pos_sample_vs + dirs[0] - dirs[2]);
	float v_XLZR = __sample_v(pos_sample_vs - dirs[0] + dirs[2]);
	float v_XLZL = __sample_v(pos_sample_vs - dirs[0] - dirs[2]);

	vmmat44f H;

	vmfloat3 g = vmfloat3(v_XR - v_XL, v_YR - v_YL, v_ZR - v_ZL);
	H[0][0] = v_XXR - 2.f * v + v_XXL; // f_xx
	H[0][1] = (v_XRYR - v_XLYR - v_XRYL + v_XLYL);	//f_xy
	H[0][2] = (v_XRZR - v_XLZR - v_XRZL + v_XLZL);	//f_xz
	H[1][0] = H[0][1];
	H[1][1] = v_YYR - 2.f * v + v_YYL;								//f_yy
	H[1][2] = (v_YRZR - v_YLZR - v_YRZL + v_YLZL);	//f_yz
	H[2][0] = H[0][2];
	H[2][1] = H[1][2];
	H[2][2] = v_ZZR - 2.f * v + v_ZZL;								//f_zz
	H[3][3] = 1;
	//
	float gm = fLengthVector(&g);
	if (gm > FLT_EPSILON)
	{
		vmfloat3 n = -g / gm;
		vmmat44f nnT, P;
		nnT[0][0] = n.x*n.x;
		nnT[0][1] = n.x*n.y;
		nnT[0][2] = n.x*n.z;
		nnT[1][0] = n.y*n.x;
		nnT[1][1] = n.y*n.y;
		nnT[1][2] = n.y*n.z;
		nnT[2][0] = n.z*n.x;
		nnT[2][1] = n.z*n.y;
		nnT[2][2] = n.z*n.z;
		nnT[3][3] = 0;
		P[0][0] = 1.0f;
		P[1][1] = 1.0f;
		P[2][2] = 1.0f;
		P[3][3] = 1.0f;
		P = P - nnT;
		/////////////////////////
		vmmat44f FF = (-P * H) / (float)gm * nnT;
		float fFlowCurv = sqrt(FF[0][0] * FF[0][0] + FF[0][1] * FF[0][1] + FF[0][2] * FF[0][2]
			+ FF[1][0] * FF[1][0] + FF[1][1] * FF[1][1] + FF[1][2] * FF[1][2]
			+ FF[2][0] * FF[2][0] + FF[2][1] * FF[2][1] + FF[2][2] * FF[2][2]);
		ov = min(fFlowCurv, 1.0f);
	}
	return ov;
}

void compute_geometry_info(vector<vmfloat3>& nrl_pts, vector<vmfloat2>& gm_gc_pts, vector<vmfloat3>& pos_pts, const float kernel_radius, const __VolSampleInfo<ushort>& vol_info)
{
	int num_pts = (int)pos_pts.size();
	nrl_pts.assign(num_pts, vmfloat3());
	gm_gc_pts.assign(num_pts, vmfloat2());

#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int i = 0; i < num_pts; i++)
	{
		vmfloat3 pos_src = pos_pts[i], pos_vs;
		fTransformPoint(&pos_vs, &pos_src, &vol_info.mat_ws2vs);
		nrl_pts[i] = -SGRAD(pos_vs, vol_info);
		float leng = fLengthVector(&nrl_pts[i]);
		nrl_pts[i] = leng > FLT_EPSILON ? nrl_pts[i] / leng : vmfloat3();
		gm_gc_pts[i].x = leng;
	}

	PointCloud<float, vmfloat3> pc_kdt(pos_pts);
	kd_tree_t kdt(3, pc_kdt, nanoflann::KDTreeSingleIndexAdaptorParams(10));
	kdt.buildIndex();

#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int i = 0; i < num_pts; i++)
	{
		vmfloat3 pos_src = pos_pts[i];
		float sv = min(estimate_shape_variation(pos_src, nrl_pts[i], &pos_pts[0], &nrl_pts[0], kdt, kernel_radius), 1.f);
		float ov = min(estimate_orient_variation(pos_src, vol_info, kernel_radius), 1.f);
		gm_gc_pts[i].y = max(sv, ov);
	}
}

void clustering(std::map<int, std::vector<int>>& map_clusters, const std::vector<vmfloat3>& pos_pts, const kd_tree_t& kdt, const std::vector<vmfloat3>& nrl_pts, const float e_c)
{
	int num_pts = (int)pos_pts.size();
	if (num_pts == 0) return;

	nanoflann::SearchParams params;
	params.sorted = false;
	const float r_sq = e_c * e_c;

	auto is_side_angle = [](const vmfloat3& p, const vmfloat3& q, const float eps)
	{
		float angle = std::acos(max(min(fDotVector(&p, &q), 1.f), -1.f)); // 0 to PI
		return (angle > VM_PI / 2.f - eps) && (angle < VM_PI / 2.f + eps);
	};
	auto subs_norm = [](const vmfloat3& p, const vmfloat3& q)
	{
		vmfloat3 v = vmfloat3(p.x - q.x, p.y - q.y, p.z - q.z);
		fNormalizeVector(&v, &v);
		return v;
	};

	std::vector<int> cluster_map_pts;
	cluster_map_pts.assign(num_pts, 0);

	int count_id_cluster = 1;
	for (int i = 0; i < num_pts; i++)
	{
		if (cluster_map_pts[i] != 0) continue;

		std::vector<int> index_cluster_pts;
		std::queue<int> que_for_process;

		que_for_process.push(i);
		index_cluster_pts.push_back(i);
		cluster_map_pts[i] = count_id_cluster;

		int error_prop_count = 0;
		while (!que_for_process.empty())
		{
			if (error_prop_count++ > 50000000)
			{
				cout << "ERROR, num conf. clusters : " << map_clusters.size() << endl;
				return;
			}

			int source_idx = que_for_process.front();
			que_for_process.pop();

			const vmfloat3& pos_src = pos_pts[source_idx];
			const vmfloat3& nrl_src = nrl_pts[source_idx];

			std::vector<std::pair<size_t, float> >   ret_matches;
			const int nMatches = (int)kdt.radiusSearch((float*)&pos_src, r_sq, ret_matches, params);

			for (int j = 0; j < nMatches; j++)
			{
				int idx_nb = (int)ret_matches[j].first;
				vmfloat3 pos_nb = pos_pts[idx_nb];
				if (cluster_map_pts[idx_nb] == 0
					&& is_side_angle(nrl_src, subs_norm(pos_nb, pos_src), VM_fPI / 6.f)
					)
				{
					que_for_process.push(idx_nb);
					index_cluster_pts.push_back(idx_nb);
					cluster_map_pts[idx_nb] = count_id_cluster;
				}
			}
		}

		if (index_cluster_pts.size() > 0)
			map_clusters[count_id_cluster++] = index_cluster_pts;
	}
}

void estimating_hole(bool& is_boundary, bool& is_src_vector_set, vmfloat3& vector_o,
	double(&btw_angles_max)[4], double(&btw_angles_min)[4],
	const std::vector<std::pair<size_t, float> > ret_matches,
	const std::vector<vmfloat3>& pos_pts, const vmfloat3& pos_src, const vmfloat3& nrl_src,
	const float thres_susceptibility_sq)
{
	const int nMatches = (int)ret_matches.size();
	for (int j = 0; j < nMatches; j++)
	{
		int idx_nb = (int)ret_matches[j].first;

		const vmfloat3& pos_nb = pos_pts[idx_nb];

		// plane is defined by pos_src and nrl_src
		vmfloat3 _v = pos_nb - pos_src;
		vmfloat3 _tv = nrl_src * fDotVector(&_v, &nrl_src);
		vmfloat3 pos_nb_on_plane = pos_nb - _tv;
		vmfloat3 vec_on_plane = pos_nb_on_plane - pos_src;
		double length_sq = fLengthVectorSq(&vec_on_plane);

		if (length_sq > thres_susceptibility_sq)
		{
			vec_on_plane /= (float)std::sqrt(length_sq);

			if (!is_src_vector_set)
			{
				is_src_vector_set = true;
				vector_o = vec_on_plane;
			}
			else
			{
				double btw_angle = std::acos(max(min(fDotVector(&vector_o, &vec_on_plane), 1.f), -1.f)); // 0 to PI
				vmfloat3 cross_v;
				fCrossDotVector(&cross_v, &vec_on_plane, &vector_o);
				if (fDotVector(&nrl_src, &cross_v) < 0)
					btw_angle = 2 * VM_fPI - btw_angle;
				//ordered_angles.insert((float)btw_angle);

				if (btw_angle < VM_fPI * 0.5)
				{
					btw_angles_max[0] = max(btw_angle, btw_angles_max[0]);
					//btw_angles_min[0] = min(btw_angle, btw_angles_min[0]);//
				}
				else if (btw_angle < VM_fPI)
				{
					btw_angles_max[1] = max(btw_angle, btw_angles_max[1]);
					btw_angles_min[1] = min(btw_angle, btw_angles_min[1]);
				}
				else if (btw_angle < VM_fPI * 1.5)
				{
					btw_angles_max[2] = max(btw_angle, btw_angles_max[2]);
					btw_angles_min[2] = min(btw_angle, btw_angles_min[2]);
				}
				else
				{
					//btw_angles_max[3] = max(btw_angle, btw_angles_max[3]); //
					btw_angles_min[3] = min(btw_angle, btw_angles_min[3]);
				}

				if (btw_angles_min[1] - btw_angles_max[0] < VM_fPI / 2.
					&& btw_angles_min[2] - btw_angles_max[1] < VM_fPI / 2.
					&& btw_angles_min[3] - btw_angles_max[2] < VM_fPI / 2.)
				{
					is_boundary = false;
					break; // for (int j = 0; j < nMatches; j++)
				}
			}
		}
	}
}

void detecting_boundary(std::vector<int>& bnd_id_pts, const std::vector<vmfloat3>& pos_pts, const std::vector<vmfloat3>& nrl_pts,
	const kd_tree_t& kdt_index, const float e_b, const bool consistent_dir_neighbors/*projecting except opposite directional points of the target poin*/)
{
	float thres_susceptibility_sq = e_b * 0.01f;
	thres_susceptibility_sq *= thres_susceptibility_sq;
	int num_pts = (int)pos_pts.size();

	nanoflann::SearchParams params;
	params.sorted = false;
	const float r_sq = e_b * e_b;
	// params.eps

	bool* hole_flags = new bool[num_pts];
	ZeroMemory(hole_flags, sizeof(bool) * num_pts);

#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int i = 0; i < num_pts; i++)
	{
		const vmfloat3& pos_src = pos_pts[i];
		const vmfloat3& nrl_src = nrl_pts[i];

		std::vector<std::pair<size_t, float> >   ret_matches;
		const int nMatches = (int)kdt_index.radiusSearch((float*)&pos_src, r_sq, ret_matches, params);

		//std::set<float> ordered_angles;
		bool is_src_vector_set = false;
		vmfloat3 vector_o;

		double btw_angles_max[4] = { -100000., -100000., -100000., -100000. };
		double btw_angles_min[4] = { 100000., 100000., 100000., 100000. };
		bool is_boundary = true;
		// criterion 2 angle criterion
		std::vector<std::pair<size_t, float> >   dir_matches;
		{
			for (int j = 0; j < nMatches; j++)
			{
				vmfloat3 nrl_nb = nrl_pts[ret_matches[j].first];
				if (fDotVector((vmfloat3*)&nrl_src, (vmfloat3*)&nrl_nb) >= 0 || !consistent_dir_neighbors)
					dir_matches.push_back(ret_matches[j]);
			}
		}

		estimating_hole(is_boundary, is_src_vector_set, vector_o,
			btw_angles_max, btw_angles_min, dir_matches, pos_pts, pos_src, nrl_src, thres_susceptibility_sq);

		// store boundary result
		if (is_boundary) hole_flags[i] = true;
	}

	for (int i = 0; i < num_pts; i++)
		if (hole_flags[i]) bnd_id_pts.push_back(i);

	delete[] hole_flags;
}

#define RANGE_SURF_STEP_MAX_NUM 10
#define SURFACE_REFINEMENT_NUM 5
void relocate_to_target_densities(vmfloat3* pos_relocated_pts,
	const vmfloat3* pos_pts, const vmfloat3* nrl_pts, const float* densities_pts, const int num_pts,
	const float sample_dist, const __VolSampleInfo<ushort>& vol_info)
{
	auto __sample_v = [&](const vmfloat3& pos_vs)
	{
		return __Safe_TrilinearSample(pos_vs, vol_info.vol_size, vol_info.width_slice, EXB, vol_info.vol_slices);
	};

	auto __traverse_serach_dstv = [&__sample_v, &vol_info](vmfloat3& pos_dst, const vmfloat3& pos_start, const vmfloat3& vec_sample, const float dst_v, const bool from_lower)
	{
		for (int j = 1; j < RANGE_SURF_STEP_MAX_NUM; j++)
		{
			vmfloat3 pos_sample = pos_start + vec_sample * (float)j, pos_sample_vs;
			fTransformPoint(&pos_sample_vs, &pos_sample, &vol_info.mat_ws2vs);
			float sample_v = __sample_v(pos_sample_vs);

			if (
				(sample_v > dst_v && from_lower)
				|| (sample_v < dst_v && !from_lower)
				)
			{
				// requires boundary-fitting
				vmfloat3 pos_S = pos_sample - vec_sample;
				vmfloat3 pos_E = pos_sample;
				vmfloat3 pos_O = pos_S;
				for (uint k = 0; k < SURFACE_REFINEMENT_NUM; k++)
				{
					vmfloat3 pos_bis = (pos_S + pos_E) * 0.5f, pos_bis_vs;
					fTransformPoint(&pos_bis_vs, &pos_bis, &vol_info.mat_ws2vs);
					sample_v = __sample_v(pos_bis_vs);

					if (
						(sample_v > dst_v && from_lower)
						|| (sample_v < dst_v && !from_lower))
						pos_E = pos_bis;
					else
						pos_S = pos_bis;
				}

				pos_dst = pos_S;
				return true;
			}
		} // for (uint j = 1; j < RANGE_SURF_STEP_MAX_NUM; j++)
		return false;
	};

	//pos_relocated_pts.clear();
	//pos_relocated_pts.assign(num_pts, vmfloat3());

#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int i = 0; i < num_pts; i++)
	{
		const vmfloat3& pos_sample = pos_pts[i];
		const vmfloat3& nrl_sample = nrl_pts[i];
		vmfloat3 pos_sample_vs;
		float dst_v = densities_pts[i];
		if (dst_v == 7777777)
		{
			pos_relocated_pts[i] = pos_sample;
			continue; // magic number exception
		}
		fTransformPoint(&pos_sample_vs, &pos_sample, &vol_info.mat_ws2vs);
		float sample_v = __sample_v(pos_sample_vs);

		vmfloat3 pos_dst_0, pos_dst_1;
		bool ret_0 = __traverse_serach_dstv(pos_dst_0, pos_sample, nrl_sample * sample_dist, dst_v, sample_v < dst_v);
		bool ret_1 = __traverse_serach_dstv(pos_dst_1, pos_sample, -nrl_sample * sample_dist, dst_v, sample_v < dst_v);
		if (ret_0 && ret_1)
		{
			pos_relocated_pts[i] = fLengthVectorSq(&(pos_sample - pos_dst_0)) < fLengthVectorSq(&(pos_sample - pos_dst_1)) ? pos_dst_0 : pos_dst_1;
		}
		else if (ret_0)
		{
			pos_relocated_pts[i] = pos_dst_0;
		}
		else if (ret_1)
		{
			pos_relocated_pts[i] = pos_dst_1;
		}
		else
		{
			pos_relocated_pts[i] = pos_sample;
		}
	} // for (uint i = 0; i < numVoxels; i++)
}

//#include <opencv2/highgui.hpp>
// ui work
void register_pobj(VmVObjectPrimitive& pobj, const vector<vmfloat3>& pos_pts, const vector<vmfloat3>& nrl_pts, const vector<vmfloat3>* ptr_clr_pts)
{
	PrimitiveData vtx_data;
	vtx_data.is_ccw = true; // NA //
	vtx_data.is_stripe = false; // NA//
	vtx_data.check_redundancy = true; // NA //
	vtx_data.ptype = PrimitiveTypePOINT;
	vtx_data.idx_stride = 1;
	vtx_data.num_vtx = (uint)pos_pts.size();
	vtx_data.num_prims = vtx_data.num_vtx;

	vmfloat3* buf_vtx_pos = new vmfloat3[vtx_data.num_vtx];
	vmfloat3* buf_nrl_pos = new vmfloat3[vtx_data.num_vtx];
	vmfloat3* buf_clr_pos = new vmfloat3[vtx_data.num_vtx];
	memcpy(buf_vtx_pos, &pos_pts[0], sizeof(vmfloat3) * vtx_data.num_vtx);
	memcpy(buf_nrl_pos, &nrl_pts[0], sizeof(vmfloat3) * vtx_data.num_vtx);
	if (ptr_clr_pts)
	{
		memcpy(buf_clr_pos, &ptr_clr_pts->at(0), sizeof(vmfloat3) * vtx_data.num_vtx);
	}
	else
	{
		for (uint i = 0; i < vtx_data.num_vtx; i++)
			buf_clr_pos[i] = vmfloat3(1);
	}

	vtx_data.ReplaceOrAddVerticeDefinition("POSITION", buf_vtx_pos);
	vtx_data.ReplaceOrAddVerticeDefinition("NORMAL", buf_nrl_pos);
	vtx_data.ReplaceOrAddVerticeDefinition("TEXCOORD0", buf_clr_pos); // special case //
	vtx_data.ComputeOrthoBoundingBoxWithCurrentValues();

	pobj.RegisterPrimitiveData(vtx_data);
	pobj.UpdateKDTree();
	pobj.RegisterCustomParameter("_bool_ApplyShadingFactors", true);
	pobj.RegisterCustomParameter("_bool_CtModelerStep", true);
}

#define COLORMAP_SIZE 1024
void fill_jet_colormap(int* colorarray, int ary_size)
{
	int prev_idx = 0;

	float gap = (float)ary_size / 4.f;
	vmfloat3 redf = vmfloat3(1.f, 0, 0);
	vmfloat3 greenf = vmfloat3(0, 1.f, 0);
	vmfloat3 bluef = vmfloat3(0, 0, 1.f);

	int index_1 = (int)(gap / 2.f + 0.5f);
	int index_2 = (int)(gap / 2.f + gap + 0.5f);
	int index_3 = (int)(gap / 2.f + 2 * gap + 0.5f);
	int index_4 = (int)(gap / 2.f + 3 * gap + 0.5f);
	int index_5 = ary_size;

	auto float3_2_int = [&](vmfloat3& c)
	{
		byte r = (byte)__min(c.x * 255.f, 255.f);
		byte g = (byte)__min(c.y * 255.f, 255.f);
		byte b = (byte)__min(c.z * 255.f, 255.f);
		return (r << 16) | (g << 8) | (b);
	};

	for (int i = 0; i <= index_1; i++)
	{
		vmfloat3 clr = bluef * (0.5f + 0.5f * (float)i / (float)index_1);
		colorarray[i] = float3_2_int(clr);
	}
	for (int i = index_1; i <= index_2; i++)
	{
		float ratio = (float)(i - index_1) / (float)(index_2 - index_1);
		vmfloat3 clr = bluef + greenf * ratio;
		colorarray[i] = float3_2_int(clr);
	}
	for (int i = index_2; i <= index_3; i++)
	{
		float ratio = (float)(i - index_2) / (float)(index_3 - index_2);
		vmfloat3 clr = greenf + bluef * (1.f - ratio) + redf * ratio;
		colorarray[i] = float3_2_int(clr);
	}
	for (int i = index_3; i <= index_4; i++)
	{
		float ratio = (float)(i - index_3) / (float)(index_4 - index_3);
		vmfloat3 clr = redf + greenf * (1.f - ratio);
		colorarray[i] = float3_2_int(clr);
	}
	for (int i = index_4; i < index_5; i++)
	{
		vmfloat3 clr = redf * (1.f - 0.5f * (float)(i - index_4) / (float)(index_5 - index_4));
		colorarray[i] = float3_2_int(clr);
	}
}

void fill_cool_colormap(int* colorarray, int ary_size)
{
	int prev_idx = 0;

	float gap = (float)ary_size / 4.f;
	vmfloat3 pinkf = vmfloat3(1.f, 0, 1.f);
	vmfloat3 skyf = vmfloat3(0, 1.f, 1.f);

	auto float3_2_int = [&](vmfloat3& c)
	{
		byte r = (byte)__min(c.x * 255.f, 255.f);
		byte g = (byte)__min(c.y * 255.f, 255.f);
		byte b = (byte)__min(c.z * 255.f, 255.f);
		return (r << 16) | (g << 8) | (b);
	};

	for (int i = 0; i < ary_size; i++)
	{
		float ratio = (float)(i) / (float)(ary_size);
		vmfloat3 clr = skyf * (1 - ratio) + pinkf * ratio;
		colorarray[i] = float3_2_int(clr);
	}
}

inline void convert_int_to_float3(const int _color, vmfloat3& rgb)
{
	rgb.r = ((_color >> 16) & 0xFF) / 255.f;
	rgb.g = ((_color >> 8) & 0xFF) / 255.f;
	rgb.b = ((_color >> 0) & 0xFF) / 255.f;
}

//auto is_inside_box = [](vmfloat3& p, vmfloat3& bp, float bs)
//{
//	vmfloat3 pos_min = bp - vmfloat3(bs);
//	vmfloat3 pos_max = bp + vmfloat3(bs);
//	return p.x >= pos_min.x && p.x <= pos_max.x
//		&& p.y >= pos_min.y && p.y <= pos_max.y
//		&& p.z >= pos_min.z && p.z <= pos_max.z;
//};
//auto cout_f3 = [](string prefix, vmfloat3& p)
//{
//	cout << prefix << p.x << ", " << p.y << ", " << p.z << endl;
//};


#include<iostream>
//#define EIGEN_USE_MKL_ALL
#include"basis.hpp"
#include"operators.hpp"
#include"diag.h"
#include"tpoperators.hpp"
#include"files.hpp"
#include"timeev.hpp"
#include"ETH.hpp"
#include<iomanip>
#include <boost/program_options.hpp>

using namespace boost::program_options;
int main(int argc, char *argv[])
{
  using namespace Eigen;
using namespace std;
using namespace Many_Body;
  using Fermi_HubbardBasis= TensorProduct<ElectronBasis, ElectronBasis>;
   using HolsteinBasis= TensorProduct<ElectronBasis, PhononBasis>;
  using Mat= Operators::Mat;
   using boost::program_options::value;
   int M{};
  int L{};
  int i{};
  int j{};
    double T{};
      double tot{};
        double dt{};
  double t0{};
  double omega{};
  double gamma{};
  double cutoff{};
  bool PB{};

  std::string sM{};
  std::string sL{};
  std::string si{};
  std::string sj{};
  std::string star{};
  std::string sT{};
    std::string stot{};
      std::string sdt{};
  std::string st0{};
  std::string somega{};
  std::string sgamma{};
  std::string starget{};
  std::string scutoff{};
  std::string sPB{};
  std::string filename="SPexT";
  try
  {
    boost::program_options::options_description desc{"Options"};
    desc.add_options()
      ("help,h", "Help screen")
      ("L", value(&L)->default_value(4), "L")
      ("i", value(&i)->default_value(0), "i")
      ("j", value(&j)->default_value(1), "j")
      ("T", value(&T)->default_value(0.1), "T")
      ("M,m", value(&M)->default_value(2), "M")
      ("tot", value(&tot)->default_value(1.), "tot")
      ("dt", value(&dt)->default_value(0.1), "dt")
      ("t0", value(&t0)->default_value(1.), "t0")
      ("gam", value(&gamma)->default_value(1.), "gamma")
      ("omg", value(&omega)->default_value(1.), "omega")
      ("pb", value(&PB)->default_value(0), "pb");
  boost::program_options::variables_map vm;
  boost::program_options::store(parse_command_line(argc, argv, desc), vm);
  boost::program_options::notify(vm);

  if (vm.count("help"))
      {std::cout << desc << '\n'; return 0;}
    else{
      if (vm.count("L"))
      {      std::cout << "L: " << vm["L"].as<int>() << '\n';
      	sL="L"+std::to_string(vm["L"].as<int>());
	filename+=sL;
      }
         if (vm.count("M"))
      {      std::cout << "M: " << vm["M"].as<int>() << '\n';
      	sM="M"+std::to_string(vm["M"].as<int>());
	filename+=sM;
      }
	   if (vm.count("i"))
      {      std::cout << "i: " << i << '\n';
      	si="i"+std::to_string(i);
	filename+=si;
      }
	   if (vm.count("j"))
      {      std::cout << "j: " << j << '\n';
      	sj="j"+std::to_string(j);
	filename+=sj;
      }
      	 if (vm.count("t0"))
      {      std::cout << "t0: " << vm["t0"].as<double>() << '\n';
      	st0="t0"+std::to_string(vm["t0"].as<double>()).substr(0, 3);
      	filename+=st0;
      }
      	 	 if (vm.count("omg"))
      {      std::cout << "omega: " << vm["omg"].as<double>() << '\n';
      	somega="omg"+std::to_string(vm["omg"].as<double>()).substr(0, 3);
      		filename+=somega;
      }

      		 if (vm.count("gam"))
      {      std::cout << "gamma: " << vm["gam"].as<double>() << '\n';
      	sgamma="gam"+std::to_string(vm["gam"].as<double>()).substr(0, 3);
      		filename+=sgamma;
      }
		      		 if (vm.count("tot"))
      {      std::cout << "tot: " << vm["tot"].as<double>() << '\n';
      	stot="tot"+std::to_string(vm["tot"].as<double>()).substr(0, 3);
      		filename+=stot;
      }
				 if (vm.count("dt"))
      {      std::cout << "dt: " << vm["dt"].as<double>() << '\n';
      	sdt="dt"+std::to_string(vm["dt"].as<double>()).substr(0, 3);
      		filename+=sdt;
      }

      		 if (vm.count("T"))
      {      std::cout << "T: " << T << '\n';
      	sT="T"+std::to_string(vm["T"].as<double>()).substr(0, 3);
      	filename+=sT;
      }
		 if (vm.count("pb"))
      {      std::cout << "PB: " << vm["pb"].as<bool>() << '\n';
      	sPB="PB"+std::to_string(vm["pb"].as<bool>());
      		filename+=sPB;
      }
      }
    }
  catch (const boost::program_options::error &ex)
  {
    std::cerr << ex.what() << '\n';
    return 0;
  }
  filename+=".bin";
  double mean=0.5*L*M*omega;

  // declaring the tensor products with 0 e, 1, e, 2 e
    ElectronBasis e0( L, 0);
  ElectronBasis e1( L, 1);
  ElectronBasis e2( L, 2);
  std::cout<< e2<<std::endl;
  PhononBasis ph(L, M);
HolsteinBasis TP0(e0, ph);
      HolsteinBasis TP1(e1, ph);
        HolsteinBasis TP2(e2, ph);

   		  std::cout<< TP1.dim << std::endl;
		  //std::cout<< ph << std::endl;
		  //    std::cout<< e1 << std::endl;
      Eigen::VectorXd eigenVals1(TP1.dim);
      Eigen::VectorXd eigenVals2(TP2.dim);
      Eigen::VectorXd eigenVals0(TP0.dim);
      Mat H0=Operators::HolsteinHam(TP0, t0, omega, gamma, PB);
      Mat H1=Operators::HolsteinHam(TP1, t0, omega, gamma, PB);
      Mat H2=Operators::HolsteinHam(TP2, t0, omega, gamma, PB);
    Eigen::MatrixXd HH0=Eigen::MatrixXd(H0);
    Eigen::MatrixXd HH1=Eigen::MatrixXd(H1);
    Eigen::MatrixXd HH2=Eigen::MatrixXd(H2);
    //         std::cout<< HH<<std::endl;
    //  Eigen::MatrixXd N=Eigen::MatrixXd(Eph);
    Many_Body::diagMat(HH0, eigenVals0);
	  Many_Body::diagMat(HH1, eigenVals1);
       Many_Body::diagMat(HH2, eigenVals2);
       //
       auto C2=Operators::CdagOperator(TP1, TP2, j,  PB);
       auto C1=Operators::CdagOperator(TP1, TP0,i,  PB);
       std::cout<<"MIN E0 "<<std::setprecision(15)<< eigenVals0(0)<< std::endl;
     std::cout<<"MIN E1 "<<std::setprecision(15)<< eigenVals1(0)<< std::endl;
     std::cout<<"MIN E2 "<<std::setprecision(15)<< eigenVals2(0)<< std::endl;
     std::cout<< " dim T0 "<< TP0.dim<<std::endl;
          std::cout<< " dim T1 "<< TP1.dim<<std::endl;
	       std::cout<< " dim T2 "<< TP2.dim<<std::endl;

     // std::cout<< Eigen::MatrixXd(Cdag) << std::endl;
     // std::cout<< std::endl;
     // std::cout<<TP0<<std::endl;
     //      std::cout<<TP1<<std::endl;
	       std::cout<< "mat "<< std::endl;
	       std::cout<< Eigen::MatrixXd(C1)*Eigen::MatrixXd(C1).transpose()<< std::endl;
      std::cout<< "endl "<<std::endl;
      std::cout<< Eigen::MatrixXd(C2) << std::endl;
     // std::cout<< "\n";
     //  std::cout<< Eigen::MatrixXd(Operators::CdagOperator(TP1, TP0,1,  PB)) << std::endl;
       int n=0;
       Eigen::MatrixXcd evExpbeta=Eigen::MatrixXcd::Zero(eigenVals1.rows(), eigenVals1.rows());
       double Z{0};
       for(int k=0; k<eigenVals1.rows(); k++)
	 {
	   double ex=std::exp(-(eigenVals1(k)-eigenVals1(0))/T);
	   evExpbeta(k, k)=std::complex<double>{std::exp(-(eigenVals1(k)-eigenVals1(0))/T), 0};
	Z+=ex;
	//	sum+=ex*en;
	 }

       //=TimeEv::EigenvalExponent(eigenVals1, -Many_Body::im*1./T);

     std::vector<double> time;
     std::vector<std::complex<double>> cdagc;
     std::vector<std::complex<double>> ccdag;
     auto H1T=HH1.transpose();
     auto H2T=HH2.transpose();
       auto H0T=HH0.transpose();
  //       while(n*dt<tot)
  //       	{

  //    	    Eigen::MatrixXcd evExp1m=TimeEv::EigenvalExponent(eigenVals1, n*dt);
  //      Eigen::MatrixXcd evExp1p=TimeEv::EigenvalExponent(eigenVals1, -n*dt);
  //       Eigen::MatrixXcd evExp2m=TimeEv::EigenvalExponent(eigenVals2, n*dt);
  //        Eigen::MatrixXcd evExp0p=TimeEv::EigenvalExponent(eigenVals0, -n*dt);

  // 	 auto CDAGC=(evExpbeta*H1T*C1*HH0*evExp0p*H0T*C1.adjoint()*HH1*evExp1m).trace()/Z;

  // 	 //.trace()/Z;
  // 		     //*evExp1m).trace()/Z;
  // 	 	 auto CCDAG= ((evExpbeta*evExp1p*H1T*C2.adjoint()*HH2*evExp2m*H2T*C2*HH1)).trace()/Z;
  // 	 // std::cout<< "shape2 "<< CCDAG.rows() << " x "  << CCDAG.cols() << " tr "<< CCDAG.trace()<<std::endl;
  // 		 std::cout<<n*dt << "\t"<< CCDAG<< " and  "<<CDAGC<< "diff "<< CCDAG+CDAGC<< std::endl;
  // 		      //	std::cout<<n*dt << "\t"<<  " and  "<<CCDAG<< "  "<< "  sum  " <<Z<<std::endl;
  //      				    time.push_back(n*dt);
  //    				    ccdag.push_back(CCDAG);
  //    			    cdagc.push_back(CDAGC);
  // n++;

  // 	 	}
  // 	std::cout<< ccdag.size() << "  "<< cdagc.size()<< std::endl;
  //     bin_write("time"+filename, time);
  //     bin_write("CCDAG"+filename, ccdag);
  //     bin_write("CDAGC"+filename, cdagc);

    // 			  int pb=PB;
    // 		  	  std::string Hs="H";
    // 			  std::string Ts="T";
    // 			  std::string phds="PHD";

    // 		      bin_write("E"+filename, Evec);
    // 		      bin_write("Nph"+filename, Ovec);
    // 		      bin_write("EK"+filename, Ovec2);
    // 		      bin_write("nX"+filename, Ovec3);
    // 		      bin_write("temp"+filename, Tr);
  return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef ALIGNED_EIGEN_TYPES_HPP
#define ALIGNED_EIGEN_TYPES_HPP

#include <Eigen/Geometry>

namespace Eigen {

/**
 * \defgroup Aligned4Vector3_Module Aligned vector3 module
 *
 * \code
 * #include <unsupported/Eigen/Aligned4Vector3>
 * \endcode
 */
//@{

/** \class Aligned4Vector3
 *
 * \brief A vectorization friendly 3D vector
 *
 * This class represents a 3D vector internally using a 4D vector
 * such that vectorization can be seamlessly enabled. Of course,
 * the same result can be achieved by directly using a 4D vector.
 * This class makes this process simpler.
 *
 */
// TODO specialize Cwise
template <typename _Scalar>
class Aligned4Vector3;

namespace internal {
template <typename _Scalar>
struct traits<Aligned4Vector3<_Scalar> > : traits<Matrix<_Scalar, 3, 1, 0, 3, 1> > {};
}  // namespace internal

template <typename _Scalar>
class Aligned4Vector3 : public MatrixBase<Aligned4Vector3<_Scalar> > {
  typedef Matrix<_Scalar, 4, 1> CoeffType;
  CoeffType m_coeffs;

 public:
  typedef MatrixBase<Aligned4Vector3<_Scalar> > Base;
  EIGEN_DENSE_PUBLIC_INTERFACE(Aligned4Vector3)
  using Base::operator*;

  inline Index rows() const { return 3; }
  inline Index cols() const { return 1; }

  Scalar* data() { return m_coeffs.data(); }
  const Scalar* data() const { return m_coeffs.data(); }
  Index innerStride() const { return 1; }
  Index outerStride() const { return m_coeffs.outerStride(); }

  inline const Scalar& coeff(Index row, Index col) const { return m_coeffs.coeff(row, col); }

  inline Scalar& coeffRef(Index row, Index col) { return m_coeffs.coeffRef(row, col); }

  inline const Scalar& coeff(Index index) const { return m_coeffs.coeff(index); }

  inline Scalar& coeffRef(Index index) { return m_coeffs.coeffRef(index); }

  inline Aligned4Vector3(const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, Scalar(1)) {}

  inline Aligned4Vector3() : m_coeffs(Scalar(0), Scalar(0), Scalar(0), Scalar(1)) {}

  inline Aligned4Vector3(const Aligned4Vector3& other) : Base(), m_coeffs(other.m_coeffs) {}

  template <typename XprType, int Size = XprType::SizeAtCompileTime>
  struct generic_assign_selector {};

  template <typename XprType>
  struct generic_assign_selector<XprType, 4> {
    inline static void run(Aligned4Vector3& dest, const XprType& src) {
      dest.m_coeffs = src;
      dest.m_coeffs.w() = Scalar(1);
    }
  };

  template <typename XprType>
  struct generic_assign_selector<XprType, 3> {
    inline static void run(Aligned4Vector3& dest, const XprType& src) {
      dest.m_coeffs.template head<3>() = src;
      dest.m_coeffs.w() = Scalar(1);
    }
  };

  template <typename Derived>
  inline Aligned4Vector3(const MatrixBase<Derived>& other) {
    generic_assign_selector<Derived>::run(*this, other.derived());
  }

  inline Aligned4Vector3& operator=(const Aligned4Vector3& other) {
    m_coeffs = other.m_coeffs;
    return *this;
  }

  template <typename Derived>
  inline Aligned4Vector3& operator=(const MatrixBase<Derived>& other) {
    generic_assign_selector<Derived>::run(*this, other.derived());
    return *this;
  }

  inline Aligned4Vector3 operator+(const Aligned4Vector3& other) const {
    return Aligned4Vector3(m_coeffs + other.m_coeffs);
  }

  inline Aligned4Vector3& operator+=(const Aligned4Vector3& other) {
    m_coeffs += other.m_coeffs;
    return *this;
  }

  inline Aligned4Vector3 operator-(const Aligned4Vector3& other) const {
    return Aligned4Vector3(m_coeffs - other.m_coeffs);
  }

  inline Aligned4Vector3 operator-=(const Aligned4Vector3& other) {
    m_coeffs -= other.m_coeffs;
    return *this;
  }

  inline Aligned4Vector3 operator*(const Scalar& s) const { return Aligned4Vector3(m_coeffs * s); }

  inline friend Aligned4Vector3 operator*(const Scalar& s, const Aligned4Vector3& vec) {
    return Aligned4Vector3(s * vec.m_coeffs);
  }

  inline Aligned4Vector3& operator*=(const Scalar& s) {
    m_coeffs *= s;
    return *this;
  }

  inline Aligned4Vector3 operator/(const Scalar& s) const { return Aligned4Vector3(m_coeffs / s); }

  inline Aligned4Vector3& operator/=(const Scalar& s) {
    m_coeffs /= s;
    return *this;
  }

  inline Scalar dot(const Aligned4Vector3& other) const {
    eigen_assert(m_coeffs.w() == Scalar(1));
    eigen_assert(other.m_coeffs.w() == Scalar(1));
    return m_coeffs.dot(other.m_coeffs) - Scalar(1);
  }

  inline void normalize() { m_coeffs /= norm(); }

  inline Aligned4Vector3 normalized() const { return Aligned4Vector3(m_coeffs / norm()); }

  inline Scalar sum() const {
    eigen_assert(m_coeffs.w() == Scalar(1));
    return m_coeffs.sum() - Scalar(1);
  }

  inline Scalar squaredNorm() const {
    eigen_assert(m_coeffs.w() == Scalar(1));
    return m_coeffs.squaredNorm() - Scalar(1);
  }

  inline Scalar norm() const {
    using std::sqrt;
    return sqrt(squaredNorm());
  }

  inline Aligned4Vector3 cross(const Aligned4Vector3& other) const {
    return Aligned4Vector3(m_coeffs.cross3(other.m_coeffs));
  }

  template <typename Derived>
  inline bool isApprox(const MatrixBase<Derived>& other, RealScalar eps = NumTraits<Scalar>::dummy_precision()) const {
    return m_coeffs.template head<3>().isApprox(other, eps);
  }

  CoeffType& coeffs() { return m_coeffs; }
  const CoeffType& coeffs() const { return m_coeffs; }
};

namespace internal {

template <typename _Scalar>
struct eval<Aligned4Vector3<_Scalar>, Dense> {
  typedef const Aligned4Vector3<_Scalar>& type;
};

template <typename Scalar>
struct evaluator<Aligned4Vector3<Scalar> > : evaluator<Matrix<Scalar, 4, 1> > {
  typedef Aligned4Vector3<Scalar> XprType;
  typedef evaluator<Matrix<Scalar, 4, 1> > Base;

  evaluator(const XprType& m) : Base(m.coeffs()) {}
};

}  // namespace internal

//@}

}  // namespace Eigen

#endif  // ALIGNED_EIGEN_TYPES


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_TWO_SPLIT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_TWO_SPLIT_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/two_split.hpp>
#include <boost/simd/include/constants/splitfactor.hpp>
#include <boost/fusion/tuple.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_split_, tag::cpu_,
                          (A0),
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                         )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a,
                              A0 & r0,A0 & r1) const
    {
      A0 c  = Splitfactor<A0>()*a;
      r0 = c-(c-a);
      r1 = a-r0;
      return 0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_split_, tag::cpu_,
                          (A0),
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                         )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A0 const& a2) const
    {
      A0 a1;
      two_split(a0,a1,a2);
      return a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_split_, tag::cpu_,
                            (A0),
                            ((scalar_<floating_<A0> >))
                           )
  {
    typedef typename boost::fusion::tuple<A0, A0> result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type res;
      two_split(a0,boost::fusion::at_c<0>(res),boost::fusion::at_c<1>(res));
      return res;
    }

    private :
    inline void eval(A0 const& a, A0& r0, A0& r1)const
    {
      A0 c;
      c  = Splitfactor<A0>()*a;
      r0 = c-(c-a);
      r1 = a-r0;
    }
  };
} } }

#endif


#define SDL_MAIN_HANDLED // for some reason SDL main is broken on my computer
#include <SDL2/SDL.h>
#include <imgui/imgui.h>
#include <imgui/imgui_impl_sdl.h>
#include <imgui/imgui_impl_sdlrenderer.h>
#include <stdio.h>
#include <fstream>
#include <sstream>
#include <Eigen/Dense>
#define AINI_IMPLEMENTATION
#include "aini.hpp"
#include "KerrBlackHole.hpp"
#include "physicalConstants.hpp"
#include "drawUtility.hpp"

using vec = Eigen::Vector2<double>;

static std::string readText(std::ifstream const& settingsFile)
{
	std::ostringstream sstream;
	sstream << settingsFile.rdbuf();
	return sstream.str();
}

int main(int, char**)
{
	if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_TIMER | SDL_INIT_GAMECONTROLLER) != 0)
	{
		printf("Error: %s\n", SDL_GetError());
		return -1;
	}

	// Setup window
	SDL_WindowFlags const window_flags = (SDL_WindowFlags)(SDL_WINDOW_RESIZABLE | SDL_WINDOW_ALLOW_HIGHDPI);
	SDL_Window* window = SDL_CreateWindow("Interstellar-sandbox-2d", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, 1280, 720, window_flags);

	// Setup SDL_Renderer instance
	SDL_Renderer* renderer = SDL_CreateRenderer(window, -1, SDL_RENDERER_PRESENTVSYNC | SDL_RENDERER_ACCELERATED);
	if (renderer == nullptr)
	{
		printf("Error: %s\n", SDL_GetError());
		return -1;
	}

	// Setup Dear ImGui context
	IMGUI_CHECKVERSION();
	ImGui::CreateContext();
	ImGuiIO& io = ImGui::GetIO(); (void)io;
	io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard;     // Enable Keyboard Controls

	// Setup Dear ImGui style
	ImGui::StyleColorsDark();

	// Setup Platform/Renderer backends
	ImGui_ImplSDL2_InitForSDLRenderer(window, renderer);
	ImGui_ImplSDLRenderer_Init(renderer);

	// Our state
	ImVec4 clear_color = ImVec4(0.0f, 0.0f, 0.0f, 1.00f);
	ImVec4 grid_color = ImVec4(0.0f, 1.0f, 0.0f, 1.00f);

	vec initialPos{};

	int grid_size = 20;
	int n_rays = 360;
	double pixel_scale = 1;
	double constexpr G = 1;
	double constexpr C = 1;
	float time = 0.0;
	float animation_speed = 1.0f;
	double M = 1000;
	SchwarzschildBlackHole blackHole;
	{
		std::ifstream saveFile("save.ini");
		if (saveFile.is_open())
		{
			aini::Reader reader(readText(saveFile));
			blackHole.deserialize(reader);
		}
	}

	// Main loop
	bool done = false;
	int steps = 400;
	while (!done)
	{
		// Poll and handle events (inputs, window resize, etc.)
		// You can read the io.WantCaptureMouse, io.WantCaptureKeyboard flags to tell if dear imgui wants to use your inputs.
		// - When io.WantCaptureMouse is true, do not dispatch mouse input data to your main application, or clear/overwrite your copy of the mouse data.
		// - When io.WantCaptureKeyboard is true, do not dispatch keyboard input data to your main application, or clear/overwrite your copy of the keyboard data.
		// Generally you may always pass all inputs to dear imgui, and hide them from your application based on those two flags.
		SDL_Event event;
		while (SDL_PollEvent(&event))
		{
			ImGui_ImplSDL2_ProcessEvent(&event);
			if (event.type == SDL_QUIT)
				done = true;
			if (event.type == SDL_WINDOWEVENT && event.window.event == SDL_WINDOWEVENT_CLOSE && event.window.windowID == SDL_GetWindowID(window))
				done = true;
		}

		// Start the Dear ImGui frame
		ImGui_ImplSDLRenderer_NewFrame();
		ImGui_ImplSDL2_NewFrame();
		ImGui::NewFrame();

		// 2. Show a simple window that we create ourselves. We use a Begin/End pair to created a named window.
		{
			ImGui::Begin("Settings");
			ImGui::ColorEdit3("clear color", (float*)&clear_color);
			ImGui::ColorEdit3("grid color", (float*)&grid_color);
			ImGui::SliderInt("grid size", &grid_size, 10, 100);

			ImGui::DragInt("steps", &steps, 100, 1, 0);
			ImGui::DragInt("n rays", &n_rays, 1, 1, 0);
			ImGui::DragDouble("pixel scale", &pixel_scale);
			ImGui::DragDouble("M", &M);
			ImGui::SliderFloat("Time", &time, 0, 1.0f);
			ImGui::DragFloat("animation speed", &animation_speed);
			static bool animate = false;
			ImGui::Checkbox("Animate", &animate);
			if (animate)
			{
				time += animation_speed * ImGui::GetIO().DeltaTime;
				if (time > 1.0f)
					time = 0.0f;
				if (time < 0.0f)
					time = 1.0f;
			}

			ImGui::DragDouble("initial pos x", &initialPos[0]);
			ImGui::DragDouble("initial pos y", &initialPos[1]);

			blackHole.showEditor();

			if (ImGui::Button("save"))
			{
				aini::Writer writer;
				blackHole.serialize(writer);
				std::ofstream saveFile("save.ini");
				saveFile << writer.write();
				saveFile.flush();
			}

			ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate);
			ImGui::End();
		}

		SDL_RenderClear(renderer);

		SDL_SetRenderDrawColor(renderer, (Uint8)(grid_color.x * 255), (Uint8)(grid_color.y * 255), (Uint8)(grid_color.z * 255), (Uint8)(grid_color.w * 255));
		int width, height;
		SDL_GetWindowSize(window, &width, &height);

		// newton approximation
		// see: https://fr.wikipedia.org/wiki/Tests_exp%C3%A9rimentaux_de_la_relativit%C3%A9_g%C3%A9n%C3%A9rale
		// https://fr.wikipedia.org/wiki/Loi_universelle_de_la_gravitation

		auto draw_ray = [&](vec pos, double angle)
		{
			vec dir{ cos(angle), sin(angle) };
			for (int i = 0; i < (int)(steps * time); i++)
			{
				vec const d = blackHole.pos - pos;
				auto const F = (G * M) / (d.squaredNorm());
				auto const gravity = d.normalized() * F;
				dir += gravity;
				pos += dir * C;

				SDL_RenderDrawPoint(renderer, (int)(pos[0] / pixel_scale), (int)(pos[1] / pixel_scale));
			}
		};

		double const angleStep = (M_PI * 2.0) / n_rays;
		double angle = 0;
		for (int n = 0; n < n_rays; n++)
		{
			draw_ray(initialPos, angle);
			angle += angleStep;
		}

		// draw black hole center
		SDL_SetRenderDrawColor(renderer, 255, 0, 0, 255);
		SDL_Rect brect{ (int)(blackHole.pos[0] / pixel_scale), (int)(blackHole.pos[1] / pixel_scale), 4, 4};
		SDL_RenderFillRect(renderer, &brect);
		//draw_circle(renderer, blackHole.pos[0] / pixel_scale, blackHole.pos[1] / pixel_scale, blackHole.SchwarzschildRadius() / pixel_scale);

		ImGui::Render();
		SDL_SetRenderDrawColor(renderer, (Uint8)(clear_color.x * 255), (Uint8)(clear_color.y * 255), (Uint8)(clear_color.z * 255), (Uint8)(clear_color.w * 255));
		ImGui_ImplSDLRenderer_RenderDrawData(ImGui::GetDrawData());
		SDL_RenderPresent(renderer);
	}

	// Cleanup
	ImGui_ImplSDLRenderer_Shutdown();
	ImGui_ImplSDL2_Shutdown();
	ImGui::DestroyContext();

	SDL_DestroyRenderer(renderer);
	SDL_DestroyWindow(window);
	SDL_Quit();

	return 0;
}


// Demonstration of estimating an RC network using MST
// to accompany "Analyzing On-Chip Interconnect with Modern C++"
// Author: Jeff Trull <edaskel@att.net>

/*
Copyright (c) 2014 Jeffrey E. Trull

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 <iostream>
#include <vector>

#include <boost/iterator/counting_iterator.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/property_map/function_property_map.hpp>

#include "ckt_graph.h"

// Define an implicit graph class
struct pin_distance_graph {
    // For Prim we must model "Vertex List Graph" and "Incidence Graph"
    // Graph requirements
    typedef size_t vertex_descriptor;
    typedef std::pair<size_t, size_t> edge_descriptor;
    typedef boost::undirected_tag directed_category;
    typedef boost::disallow_parallel_edge_tag edge_parallel_category;
    // inherit traversal from both concepts
    struct traversal_category : virtual public boost::vertex_list_graph_tag,
                                virtual public boost::incidence_graph_tag {};

    // Vertex List Graph requirements
    typedef boost::counting_iterator<size_t> vertex_iterator;
    typedef size_t vertices_size_type;

    // Incidence Graph requirements
    // The most complicated part is generating new edges
    // We need to make a standard conforming edge iterator
    // The Boost.Iterator library can help us make a "forward iterator"
    struct  edge_iterator_t : boost::iterator_facade<edge_iterator_t,
                                                     edge_descriptor,
                                                     boost::forward_traversal_tag,
                                                     edge_descriptor const&> {
        // Implementation constrained by two things:
        // 1) We need to provide a default constructor, so sentinel cannot reference graph
        // 2) We need to provide a non-const dereference operator for Prim, so must keep
        //    a true edge descriptor around

        edge_iterator_t()
            : g_(nullptr) {}  // initialize to invalid
        edge_iterator_t(pin_distance_graph const* g, vertex_descriptor u)
            : current_edge_(std::make_pair(u, 0)), g_(g) {
            advance_to_legal();
        }
    private:
        friend class boost::iterator_core_access;
        // iterator_facade requirements:
        void increment() {
            current_edge_.second++;
            advance_to_legal();
        }
        bool equal(edge_iterator_t const& other) const {
            // true if both invalid, or they match source/dest
            if (invalid()) {
                return other.invalid();
            } else {
                return !other.invalid() && (current_edge_ == other.current_edge_);
            }
        }
        edge_descriptor const& dereference() const {
            return current_edge_;
        }
        // implementation details for above:
        bool invalid() const {
            return ((g_ == nullptr) || (current_edge_.second >= num_vertices(*g_)));
        }
        void advance_to_legal() {
            // ensure target and source are not the same
            if (current_edge_.first == current_edge_.second) {
                current_edge_.second++;
            }
        }
        edge_descriptor current_edge_;
        pin_distance_graph const* g_;
    };

    typedef edge_iterator_t out_edge_iterator;
    typedef size_t degree_size_type;

    // Vertex List Graph free functions
    friend std::pair<vertex_iterator, vertex_iterator>
    vertices(pin_distance_graph const& g) {
        return std::make_pair(vertex_iterator(0), vertex_iterator(g.points_.size()));
    }

    friend vertices_size_type num_vertices(pin_distance_graph const& g) {
        return g.points_.size();
    }

    // Incidence Graph free functions
    friend vertex_descriptor source(edge_descriptor const& e, pin_distance_graph const&) {
        return e.first;
    }

    friend vertex_descriptor target(edge_descriptor const& e, pin_distance_graph const&) {
        return e.second;
    }

    friend std::pair<out_edge_iterator, out_edge_iterator>
    out_edges(vertex_descriptor u, pin_distance_graph const& g) {
        return std::make_pair(edge_iterator_t(&g, u), edge_iterator_t());
    }

    friend degree_size_type
    out_degree(vertex_descriptor, pin_distance_graph const& g) {
        return g.points_.size() - 1;  // "complete" graph, every node connected to every other
    }

    // Main functionality
    typedef std::pair<int, int> point_t;
    template<typename PtIter>
    pin_distance_graph(PtIter beg, PtIter end) : points_(beg, end) {}

    // Provide bracket operator a la adjacency_list
    point_t operator[](vertex_descriptor u) const {
        return points_[u];
    }

private:
    std::vector<point_t> points_;

};

// Run Prim on it
// Prim seems vertex-focused, as opposed to the edge-focused Kruskal
// since our edges are implicit I favored the former

int main() {
    using namespace boost;
    using namespace std;

    // Data for implicit graph - a list of points
    vector<pair<int, int>> pinlocs =
        {{-100, -100}, {-100, 100}, {0, 0}, {100, 100}, {100, -100},
         {-50, 0}, {103, 100}, {100, 90}};

    pin_distance_graph pdg(pinlocs.begin(), pinlocs.end());

    // Create Prim requirements (temporary data structures used in algorithm)

    // vertex index map.  Since we use size_t for vertex descriptors, they
    // are immediately usable as indices:
    auto vindex_map = typed_identity_property_map<size_t>();

    // Predecessor map
    vector<pin_distance_graph::vertex_descriptor> predvec(num_vertices(pdg));   // underlying storage
    auto predpmap = make_iterator_property_map(predvec.begin(), vindex_map);

    // Weight Map
    // Another case where we can be "implicit" - the weight of an edge is
    // the Manhattan distance between the vertices
    typedef pin_distance_graph::edge_descriptor edge_t;
    auto weightpmap = make_function_property_map<edge_t>(
        [&pdg](edge_t e) -> int {
            auto coord1 = pdg[source(e, pdg)];
            auto coord2 = pdg[target(e, pdg)];
            return abs(coord1.first - coord2.first) + abs(coord1.second - coord2.second);
        });

    // call Prim
    prim_minimum_spanning_tree(pdg, predpmap,
                               weight_map(weightpmap).
                               vertex_index_map(vindex_map));

    // produce output as SVG
    cout << "<svg xmlns=\"http://www.w3.org/2000/svg\"" << endl;
    cout << "     xmlns:xlink=\"http://www.w3.org/1999/xlink\">" << endl;
    auto vitpair = vertices(pdg);
    for (auto v : make_iterator_range(vitpair.first, vitpair.second)) {
        // SVG wants positive numbers, and the Y coordinate is canvas style (reversed from Cartesian)
        // so we will transform the data en route to the display for viewability:
        // scale by 2X, mirror Y axis, and add 250 to both
        int x2 = 250 + 2*pdg[v].first;
        int y2 = 250 - 2*pdg[v].second;
        if (predvec[v] == v) {
            // Root.  Make a red circle
            cout << "    <circle cx=\"" << x2 << "\" cy=\"" << y2 << "\" r=\"10\" style=\"fill:#cc0000\"/>" << endl;
        } else {
            // Not Root: a gray one
            cout << "    <circle cx=\"" << x2 << "\" cy=\"" << y2 << "\" r=\"10\" style=\"fill:#cccccc; stroke:#222222\"/>" << endl;
        }
    }
    // Go back and add lines on top of the circles (for visibility)
    for (auto v : make_iterator_range(vitpair.first, vitpair.second)) {
        int x2 = 250 + 2*pdg[v].first;
        int y2 = 250 - 2*pdg[v].second;
        if (predvec[v] != v) {
            int x1 = 250 + 2*pdg[predvec[v]].first;
            int y1 = 250 - 2*pdg[predvec[v]].second;
            // Make a line to the successor node
            cout << "    <line x1=\"" << x1 << "\" y1=\"" << y1 << "\" x2=\"" << x2 << "\" y2=\"" << y2 << "\" style=\"stroke:#666666; stroke-width:3px\"/>" << endl;
        }
    }
    cout << "</svg>" << endl;

    // Future work: turn resulting tree into an implicit graph compatible with ckt_graph_t,
    // with estimated RC values on edges

}


// STL includes
#include <iostream>
#include <fstream>
#include <set>
#include <cstdlib> // error codes
#include <memory>
#include <cmath>
#include <chrono>
#include <random>
// Local includes
#include "models.h"
#include "types.h"
#include "config.h"

#if HAVE_LIBBOOST_PROGRAM_OPTIONS == 1
  // Boost
  #include <boost/program_options.hpp>
  namespace po = boost::program_options;
#endif

std::string model_desc_help_string(std::set<std::string> models,
                                   std::map<std::string, std::string> models_desc,
                                   std::string base_string)
{
  std::string help_string = base_string;
  for (auto m: models)
    help_string += " \"" + m + "\"" + " : " + models_desc.at(m) + "\n";
  return help_string.substr(0, help_string.size() - 1);
}

int main(int argc, char const *argv[])
{
  // models
  std::map<std::string, std::string> models_description;
  std::map<std::string, std::string> models_parameters;
  std::map<std::string, unsigned int> models_parameters_count;
  std::set<std::string> implemented_models = {"delayed", "gn", "bianconi_barabasi", "generalized_gn"};
  // unstructured models
  models_description["delayed"] = "L. H.-Dufresne. et al., PRE 92, (2015).";
  models_parameters["delayed"] = "a, alpha, b, mu, tau.";
  models_parameters_count["delayed"] = 5;
  // structured models
  models_description["gn"] = "Krapivsky-Redner, PRE 63, (2001).";
  models_parameters["gn"] = "gamma, m.";
  models_parameters_count["gn"] = 2;
  models_description["bianconi_barabasi"] = "Bianconie-Barabasi, PRL 86, (2001).";
  models_parameters["bianconi_barabasi"] = "gamma, nu, avg, std, m.";
  models_parameters_count["bianconi_barabasi"] = 5;
  models_description["generalized_gn"] = "General PA with densification.";
  models_parameters["generalized_gn"] = "a, alpha, b, m1, m2, gamma, tau, negative_kernel.";
  models_parameters_count["generalized_gn"] = 8;

  /* ~~~~~ Program options ~~~~~~~*/
  unsigned int T;
  double_vec_t parameters;
  std::string model_name;
  unsigned int seed = 0;
  std::string state_file_path;

  // ================
  // BOOST INTERFACE
  // ================
  #if HAVE_LIBBOOST_PROGRAM_OPTIONS == 1
    // boost::po call
    po::options_description description("Options");
    description.add_options()
    ("model,m", po::value<std::string>(&model_name)->default_value("gn"),
      // automatically generated help message
      model_desc_help_string(implemented_models,
                             models_description,
                             "Name of the growth model. The implemented models are:\n").c_str())
    ("parameters,p", po::value<double_vec_t>(&parameters)->multitoken(),
      // automatically generated help message
      model_desc_help_string(implemented_models,
                             models_parameters,
                             "Parameters of the model, provided as a list of doubles.\n").c_str())
    ("T,t", po::value<unsigned int>(&T)->default_value(1000), "Number of growth events.")
    ("seed,d", po::value<unsigned int>(&seed),
        "Seed of the pseudo random number generator (Mersenne-twister 19937)."\
        "Seeded with current time if seed is not specified or equal to 0.")
    ("state_file,f", po::value<std::string>(&state_file_path),
      "Save state variables in a file upon completion (degree, fitness, etc.).")
    ("verbose,v", "Output parameters to stdlog.")
    ("help,h", "Produce this help message.")
    ;
    po::variables_map var_map;
    try
    {
      po::store(po::parse_command_line(argc,argv,description), var_map);
      po::notify(var_map);
    }
    catch (po::validation_error& e)
    {
      std::clog << "Boost program option error:\n";
      std::clog << e.what();
      std::clog << "\n";
      return EXIT_FAILURE;
    }

    // Input validation and actions
    if (var_map.count("help") > 0 || argc == 1)
    {
      std::clog << "Usage:\n"
                << "  "+std::string(argv[0])+" [--option_1=value] [--option_s2=value] ...\n";
      std::clog << description;
      return EXIT_SUCCESS;
    }
    if (var_map.count("seed") == 0)
    {
      seed = (unsigned int) std::chrono::high_resolution_clock::now().time_since_epoch().count();
    }
    if (models_parameters_count[model_name] != parameters.size())
    {
      std::clog << "Incorrect number of parameters for the \"" + model_name + "\" growth model.\n";
      return EXIT_FAILURE;
    }
    if (implemented_models.find(model_name) == implemented_models.end())
    {
      std::clog << "Model \"" + model_name + "\" not implemented.\n";
      return EXIT_FAILURE;
    }
    if (T <= 1)
    {
      std::clog << "Number of event T is too small (T=" << T << ")\n";
      return EXIT_FAILURE;
    }

    // Logger
    if (var_map.count("verbose") > 0)
    {
      std::clog << "Model: " << model_name << "\n";
      std::clog << "Parameters (" + models_parameters[model_name] + "): ";
      for (auto p: parameters)
        std::clog << p << " ";
      std::clog << "\n";
      std::clog << "T: " << T << "\n";
      std::clog << "Seed: " << seed << "\n";
    }
  // ================
  // end of boost interface
  // ================
  #else
    model_name = std::string(argv[1]);
    if (model_name=="-h")
    {
      std::clog << "This is the limited interface of this program. boost::program_options could not be found and linked.\n";
      std::clog << "Usage: " << argv[0] << "  model_name T seed param1 param2 ...\n\n";
      std::clog << model_desc_help_string(implemented_models,
                                          models_description,
                                          "Name of the growth model. The implemented models are:\n");
      std::clog << "\n\n";
      std::clog << model_desc_help_string(implemented_models,
                                          models_parameters,
                                          "Parameters of the model, provided as a list of doubles.\n");
      std::clog << "\n";
      return EXIT_SUCCESS;
    }
    T = std::atoi(argv[2]);
    seed = std::atoi(argv[3]);
    parameters.resize(models_parameters_count[model_name], 0);
    for (unsigned int i = 0; i < models_parameters_count[model_name]; ++i)
    {
      parameters[i] = std::atof(argv[4 + i]);
    }
  #endif

  /*~~~~~~~Initialize model~~~~~~~~~~~~*/
  std::shared_ptr<growth_model> model;

  // unstructured models
  if (model_name == "delayed")
  {
    double a = parameters[0];
    double alpha = parameters[1];
    double b = parameters[2];
    double gamma = parameters[3];
    double tau = parameters[4];
    model = std::make_shared<delayed_model>(a, alpha, b, gamma, tau);
  }
  // structured models
  if (model_name == "gn")
  {
    double gamma = parameters[0];
    unsigned int m = (unsigned int) parameters[1];
    model = std::make_shared<gn_model>(gamma, m);
  }
  if (model_name == "generalized_gn")
  {
    double a = parameters[0];
    double alpha = parameters[1];
    double b = parameters[2];
    unsigned int m1 = (unsigned int) parameters[3];
    unsigned int m2 = (unsigned int) parameters[4];
    double gamma = parameters[5];
    double tau = parameters[6];
    if (parameters[7]) gamma = -gamma;
    model = std::make_shared<generalized_gn_model>(a, alpha, b, m1, m2, gamma, tau);
    --T; // we start with an edge
  }
  if (model_name == "bianconi_barabasi")
  {
    double gamma = parameters[0];
    double nu = parameters[1];
    double avg = parameters[2];
    double std = parameters[3];
    unsigned int m = (unsigned int) parameters[4];
    model = std::make_shared<bianconi_barabasi_model>(gamma, nu, avg, std, m);
  }

  /*~~~~~~~~~~~Run~~~~~~~~~~~~~~~~*/
  std::mt19937 engine(seed);
  model->run(T, engine);
  // std::clog << "Ran\n";

  /*~~~Dump history to stream~~~~*/
  model->print_history(std::cout);
  #if HAVE_LIBBOOST_PROGRAM_OPTIONS == 1
      if (var_map.count("state_file") > 0)
      {
        std::ofstream sf(state_file_path.c_str(), std::ios::out);
        model->print_states(sf);
        sf.close();
      }
  #endif

  return 0;
}


#include <cassert>
#include <exception>
#include <iostream>

#include <boost/safe_numerics/safe_integer.hpp>

int main(int, const char *[]){
    std::cout << "example 2:";
    std::cout << "undetected overflow in data type" << std::endl;
    // problem: undetected overflow
    std::cout << "Not using safe numerics" << std::endl;
    try{
        int x = INT_MAX;
        // the following silently produces an incorrect result
        ++x;
        std::cout << x << " != " << INT_MAX << " + 1" << std::endl;
        std::cout << "error NOT detected!" << std::endl;
    }
    catch(std::exception){
        std::cout << "error detected!" << std::endl;
    }
    // solution: replace int with safe<int>
    std::cout << "Using safe numerics" << std::endl;
    try{
        using namespace boost::safe_numerics;
        safe<int> x = INT_MAX;
        // throws exception when result is past maximum possible
        ++x;
        assert(false); // never arrive here
    }
    catch(std::exception & e){
        std::cout << e.what() << std::endl;
        std::cout << "error detected!" << std::endl;
    }
    return 0;
}


// Licensed to the Apache Software Foundation (ASF) under one
// or more contributor license agreements.  See the NOTICE file
// distributed with this work for additional information
// regarding copyright ownership.  The ASF licenses this file
// to you under the Apache License, Version 2.0 (the
// "License"); you may not use this file except in compliance
// with the License.  You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing,
// software distributed under the License is distributed on an
// "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied.  See the License for the
// specific language governing permissions and limitations
// under the License.

#include <iostream>
#include <random>
#include <vector>

#include "cutil.hpp"
#include "util.hpp"
#include "zipf_dist.hpp"

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

namespace po = boost::program_options;
namespace fs = boost::filesystem;

po::variables_map parse_cmdargs(int argc, char const* argv[])
{
    po::variables_map vm;
    po::options_description desc("Allowed options");
    // clang-format off
    desc.add_options()
        ("help,h", "produce help message")
        ("text,t", "text (default is uint32_t binary)")
        ("num,n",po::value<size_t>()->required(), "number of integers per file")
        ("output,o",po::value<std::string>()->required(), "output path");
    // clang-format on
    try {
        po::store(po::parse_command_line(argc, argv, desc), vm);
        if (vm.count("help")) {
            std::cout << desc << "\n";
            exit(EXIT_SUCCESS);
        }
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc;
        std::cerr << "Missing required option: " << e.what() << std::endl;
        exit(EXIT_FAILURE);
    } catch (po::error& e) {
        std::cout << desc;
        std::cerr << "Error parsing cmdargs: " << e.what() << std::endl;
        exit(EXIT_FAILURE);
    }

    return vm;
}

template <class t_dist>
void generate_data(
    const t_dist& dist, size_t n, std::string out_file, bool write_text)
{
    std::string file_name = out_file;
    std::cout << "generating file " << file_name << std::endl;
    std::vector<uint32_t> nums;
    std::mt19937 gen(0);
    t_dist dst = dist;
    for (size_t i = 0; i < n; i++) {
        nums.push_back(dst(gen));
    }
    if (write_text)
        write_file_text(nums, file_name + ".txt");
    else
        write_file_u32(nums, file_name + ".u32");
}

int main(int argc, char const* argv[])
{
    auto cmdargs = parse_cmdargs(argc, argv);
    auto output_path = cmdargs["output"].as<std::string>();
    auto n = cmdargs["num"].as<size_t>();
    auto write_text = cmdargs.count("text") != 0;

    generate_data(std::uniform_int_distribution<uint32_t>(0, (1 << 8) - 1), n,
        output_path + "/uniform08", write_text);
    generate_data(std::uniform_int_distribution<uint32_t>(0, (1 << 12) - 1), n,
        output_path + "/uniform12", write_text);
    generate_data(std::uniform_int_distribution<uint32_t>(0, (1 << 16) - 1), n,
        output_path + "/uniform16", write_text);
    generate_data(std::uniform_int_distribution<uint32_t>(0, (1 << 20) - 1), n,
        output_path + "/uniform20", write_text);

    generate_data(std::geometric_distribution<uint32_t>(0.01), n,
        output_path + "/geom0.01", write_text);
    generate_data(std::geometric_distribution<uint32_t>(0.1), n,
        output_path + "/geom0.1", write_text);
    generate_data(std::geometric_distribution<uint32_t>(0.2), n,
        output_path + "/geom0.2", write_text);
    generate_data(std::geometric_distribution<uint32_t>(0.4), n,
        output_path + "/geom0.4", write_text);
    generate_data(std::geometric_distribution<uint32_t>(0.6), n,
        output_path + "/geom0.6", write_text);
    generate_data(std::geometric_distribution<uint32_t>(0.8), n,
        output_path + "/geom0.8", write_text);
    generate_data(std::geometric_distribution<uint32_t>(0.9), n,
        output_path + "/geom0.9", write_text);
    generate_data(std::geometric_distribution<uint32_t>(0.99), n,
        output_path + "/geom0.99", write_text);

    generate_data(zipf_distribution<uint32_t>(1 << 12), n,
        output_path + "/zipf12", write_text);
    generate_data(zipf_distribution<uint32_t>(1 << 20), n,
        output_path + "/zipf20", write_text);
}


#pragma once

#include <deal.II/base/exceptions.h>
#include <deal.II/base/index_set.h>
#include <deal.II/base/utilities.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/lac/trilinos_vector.h>

#include "collision_tensor.hpp"
#include "collision_tensor_galerkin.hpp"
#include "dense/collision_tensor_zlastAM.hpp"
#include "dense/collision_tensor_zlastAM_eigen.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"

namespace boltzmann {
// -------------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------
class CollisionTensorOperatorBase
{
 protected:
  typedef typename SpectralBasisFactoryKS::basis_type spectral_basis_t;
  typedef dealii::TrilinosWrappers::MPI::Vector trilinos_vector_t;

 protected:
  CollisionTensorOperatorBase(MPI_Comm& communicator)
      : comm(communicator)
  { /* empty */
  }

 public:
  /// explicit Euler step
  virtual void apply(trilinos_vector_t& out, double dt) const = 0;
  /// load tensor from HDF5-file
  virtual void load_tensor(std::string fname) = 0;
  void set_truncation_threshold(double tre);

 protected:
  MPI_Comm comm;
  double truncate_treshold_ = 0;
  bool use_treshold_ = false;

};

// -------------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------
class CollisionTensorOperatorPG : public CollisionTensorOperatorBase
{
 public:
  CollisionTensorOperatorPG(const dealii::DoFHandler<2>& dh,
                            const spectral_basis_t& basis,
                            const dealii::IndexSet& parallel_partitioning,
                            MPI_Comm communicator = MPI_COMM_WORLD)
      : CollisionTensorOperatorBase(communicator)
      , n_phys_dofs(dh.n_dofs())
      , n_velo_dofs(basis.n_dofs())
      , Q(basis.n_dofs())
      , vtmp(parallel_partitioning, communicator)
      , local_buffer(parallel_partitioning.n_elements())
  { /* empty */ }

  virtual void apply(trilinos_vector_t& out, double dt) const;
  virtual void load_tensor(std::string fname);

 private:
  const unsigned int n_phys_dofs;
  const unsigned int n_velo_dofs;
  CollisionTensor Q;
  mutable trilinos_vector_t vtmp;
  mutable std::vector<double> local_buffer;
};

// -------------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------
class CollisionTensorOperatorG : public CollisionTensorOperatorBase
{
 public:
  CollisionTensorOperatorG(const dealii::DoFHandler<2>& dh,
                           const spectral_basis_t& basis,
                           const dealii::IndexSet& parallel_partitioning,
                           MPI_Comm communicator = MPI_COMM_WORLD)
      : CollisionTensorOperatorBase(communicator)
      , n_phys_dofs(dh.n_dofs())
      , n_velo_dofs(basis.n_dofs())
      , n_local_phys_dofs_(parallel_partitioning.n_elements() / basis.n_dofs())
      , Q(basis)
      , vtmp(parallel_partitioning, communicator)
  {
    lambda.resize(4, n_local_phys_dofs_);
    lambda_prev.resize(4, n_local_phys_dofs_);
  }

  virtual void apply(trilinos_vector_t& out, double dt) const;
  virtual void load_tensor(std::string fname);

 private:
  const unsigned int n_phys_dofs;
  const unsigned int n_velo_dofs;
  const unsigned int n_local_phys_dofs_;
  CollisionTensorGalerkin Q;
  mutable trilinos_vector_t vtmp;
  /// conservation of momentum
  typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> carray_t;
  mutable carray_t lambda;
  mutable carray_t lambda_prev;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

};

#ifdef USE_MPI
// -------------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------
template <typename CT_DENSE = ct_dense::CollisionTensorZLastAM>
class CollisionTensorOperatorDense : public CollisionTensorOperatorBase
{
 public:
  CollisionTensorOperatorDense(const dealii::DoFHandler<2>& dh,
                               const spectral_basis_t& basis,
                               const dealii::IndexSet& parallel_partitioning,
                               int vblksize = 1,
                               MPI_Comm communicator = MPI_COMM_WORLD)
      : CollisionTensorOperatorBase(communicator)
      , basis_(basis)
      , n_phys_dofs_(dh.n_dofs())
      , n_velo_dofs_(basis.n_dofs())
      , vblksize_(vblksize)
      , n_local_phys_dofs_(parallel_partitioning.n_elements() / basis.n_dofs())
      , Q_(basis, parallel_partitioning.n_elements() / basis.n_dofs())
      , vtmp(parallel_partitioning, communicator)
  {
    /* empty */
  }

 private:
  const spectral_basis_t& basis_;
  const unsigned int n_phys_dofs_;
  const unsigned int n_velo_dofs_;
  const int vblksize_;
  const int n_local_phys_dofs_;
  CT_DENSE Q_;

 private:
  mutable trilinos_vector_t vtmp;
  typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> in_array_t;
  /// padded input array
  mutable in_array_t padded_in;
  /// conservation of momentum
  mutable in_array_t lambda;
  mutable in_array_t lambda_prev;

 public:
  virtual void apply(trilinos_vector_t& out, double dt) const override;
  virtual void load_tensor(std::string fname) override;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <typename CT_DENSE>
void
CollisionTensorOperatorDense<CT_DENSE>::apply(trilinos_vector_t& out, double dt) const
{
  BOOST_ASSERT(out.local_size()/n_velo_dofs_ == n_local_phys_dofs_);
  BOOST_ASSERT(vtmp.local_size()/n_velo_dofs_ == n_local_phys_dofs_);

  const double* data = out.begin();

  Eigen::Map<const in_array_t> vin(data, n_velo_dofs_, n_local_phys_dofs_);
  int imax = -1;
  if (use_treshold_) {
    for (int i = n_velo_dofs_ -1; i >= 0; --i) {
      if ((vin.row(i).abs() > truncate_treshold_).any()) {
        imax = i;
        break;
      }
    }
  }
  // pad & copy to padded array
  Q_.pad(padded_in, vin);
  Eigen::Map<in_array_t> vout(out.begin(), n_velo_dofs_, n_local_phys_dofs_);
  // get lambda (moment conservation)
  Q_.get_lambda(lambda_prev, vin);
  // apply collision tensor write directly to output array
  Eigen::Map<in_array_t> vtmp_eigen(vtmp.begin(), n_velo_dofs_, n_local_phys_dofs_);
  Q_.apply(vtmp_eigen, padded_in, use_treshold_ ? imax : -1);
  // explicit Euler timestep
  out.sadd(1.0, dt, vtmp);
  // get contribution to lambda from next timestep
  Q_.get_lambda(lambda, vout);
  lambda -= lambda_prev;
  // conserve moments
  Q_.project_lambda(vout, lambda);
}

template <typename CT_DENSE>
void
CollisionTensorOperatorDense<CT_DENSE>::load_tensor(std::string fname)
{
  Q_.import_entries_mpishmem(fname, vblksize_);

  // initialize buffer arrays used during ::apply
  int npadded = Q_.padded_vector_length();
  padded_in.resize(npadded, n_local_phys_dofs_);
  // this needs to be done only once, the parts used to
  // store the input vector are overwritten, and the elements in between
  // are multiplied by zero during Q_.apply, just make sure they are not NaN's by chance.
  padded_in.setZero();

  // temporary arrays for conservation of momentum
  lambda_prev.resize(4, n_local_phys_dofs_);
  lambda.resize(4, n_local_phys_dofs_);
}

#endif

}  // namespace boltzmann