#pragma once
#include "includes.h"

typedef double type_float;

inline double square(double a) {
	return a*a;
}

template <typename _Ty> class RandomQueue {
	private:
		vector<_Ty>* _data;
	public:
		RandomQueue() {
			_data = new vector<_Ty>();
		}
		~RandomQueue() {
			delete _data;
		}
		inline bool empty(void) {
			return _data->empty();
		}
		_Ty pop(void) {
			int index = rand()%_data->size();
			_Ty object = _data->at(index);
			_data->erase(_data->begin()+index);
			return object;
		}
		void push(_Ty obj) {
			_data->push_back(obj);
		}
};

class PoissonPoint {
	public:
		type_float x,y;
		PoissonPoint(type_float x, type_float y) {
			this->x = x;
			this->y = y;
		}
};
inline bool compare_points_box(PoissonPoint* a, PoissonPoint* b) {
	/*return (
		a->x < b->x
		&&
		a->y < b->y
	) ? true : false;*/
	return max(a->x,a->y) < max(b->x,b->y);
}
inline bool compare_points_circle(PoissonPoint* a, PoissonPoint* b) {
	return square(a->x-0.5)+square(a->y-0.5) < square(b->x-0.5)+square(b->y-0.5); //comparse r^2
}

inline bool in_domain_box(PoissonPoint* p) {
	return (p->x>0.0&&p->y>0.0&&p->x<1.0&&p->y<1.0) ? true : false;
}
inline bool in_domain_circle(PoissonPoint* p) {
	return (square(p->x-0.5)+square(p->y-0.5)<0.25) ? true : false; //comparse r^2
}

void imageToGrid(PoissonPoint* point, int grid_dim, int* x, int* y) {
	*x = (int)(point->x * grid_dim);
	*y = (int)(point->y * grid_dim);
}
bool inNeighborhood(PoissonPoint*** grid, PoissonPoint* point, type_float mindist, type_float cell_size, int grid_dim) {
	int x,y;
	imageToGrid(point, grid_dim, &x, &y);

	//get the neighborhood of the point in the grid
	for (int i=x-2;i<x+3;++i) {
		if (i<0) continue;
		if (i>=grid_dim) break;
		for (int j=y-2;j<y+3;++j) {
			if (j<0) continue;
			if (j>=grid_dim) continue;
			if (grid[i][j]!=NULL) {
				type_float temp1[2] = {point->x,point->y};
				type_float temp2[2] = {grid[i][j]->x,grid[i][j]->y};
				type_float temp3[2];
				glLibMathVec::subtract2(temp1,temp2,temp3);
				if (glLibMathVec::length2(temp3)<mindist) return true;
			}
		}
	}
	return false;
}
PoissonPoint* generateRandomPointAround(PoissonPoint* point, type_float mindist) {
	//non-uniform, favours points closer to the inner ring, leads to denser packings
	type_float r1 = glLibMathRandom::get_uniform();
	type_float r2 = glLibMathRandom::get_uniform();
	//random radius between mindist and 2 * mindist
	type_float radius = mindist * (r1 + 1);
	//random angle
	type_float angle = 2 * glLibMathMisc::pi * r2;
	//the new point is generated around the point (x, y)
	type_float newX = point->x + radius * cos(angle);
	type_float newY = point->y + radius * sin(angle);
	return new PoissonPoint(newX,newY);
}

list<PoissonPoint*>* generate_poisson(type_float min_dist, int new_points_count, bool(*in_domain)(PoissonPoint*) ) {
	//Create the grid
	type_float cell_size = min_dist / glLibMathMisc::SQRT_2;

	int grid_dim = 1 / cell_size;
	PoissonPoint*** grid = new PoissonPoint**[grid_dim];
	for (int i=0;i<grid_dim;++i) {
		grid[i] = new PoissonPoint*[grid_dim];
		for (int j=0;j<grid_dim;++j) {
			grid[i][j] = NULL;
		}
	}

	//RandomQueue works like a queue, except that it
	//pops a random element from the queue instead of
	//the element at the head of the queue
	RandomQueue<PoissonPoint*> processList;
	list<PoissonPoint*>* samplePoints = new list<PoissonPoint*>();

	//generate the first point randomly
	//and updates

	PoissonPoint* firstPoint = new PoissonPoint(glLibMathRandom::get_uniform(),glLibMathRandom::get_uniform());

	//update containers
	processList.push(firstPoint);
	samplePoints->push_back(firstPoint);
	int x,y; imageToGrid(firstPoint, grid_dim, &x, &y);
	grid[x][y] = firstPoint;

	//generate other points from points in queue.
	while (!processList.empty()) {
		PoissonPoint* point = processList.pop();
		for (int i=0; i<new_points_count; ++i) {
			PoissonPoint* newPoint = generateRandomPointAround(point, min_dist);

			//check that the point is in the image region
			//and no points exist in the point's neighborhood
			if (!in_domain(newPoint)||
				inNeighborhood(grid,newPoint,min_dist,cell_size,grid_dim))
			{
				delete newPoint;
				continue;
			}

			//update containers
			processList.push(newPoint);
			samplePoints->push_back(newPoint);
			int x,y;
			imageToGrid(newPoint,grid_dim,&x,&y);
			grid[x][y] = newPoint;

			//printf("Generated %f,%f to %d,%d\n",newPoint->x,newPoint->y,x,y);
		}
	}

	for (int i=0;i<grid_dim;++i) {
		delete [] (grid[i]);
	}
	delete [] grid;

	return samplePoints;
}

list<PoissonPoint*>* generate_poisson_helper(
	double r,
	unsigned int num_points,
	int trial_points,
	bool(*comparator)(PoissonPoint*,PoissonPoint*),
	bool(*in_domain)(PoissonPoint*)
) {
	list<PoissonPoint*>* points;

	while (true) {
		points = generate_poisson(r,trial_points,in_domain);
		if (points->size()>=num_points) break;
		printf("Wanted at least %u, but could only generate %d\n",num_points,points->size());
		delete points;
	}

	printf("Wanted %u; got %d, waste of %2.3f%%\n",num_points,points->size(),100.0*(double)points->size()/(double)num_points-100.0);

	points->sort(comparator);
	while (points->size()>num_points) {
		delete points->back();
		points->pop_back();
	}

	if (points->size()==1) {
		points->front()->x = points->front()->y = 0.5;
		return points;
	}

	/*PoissonPoint point_min( 1.5, 1.5);
	PoissonPoint point_max(-0.5,-0.5);
	for (list<PoissonPoint*>::const_iterator iter=points->begin();iter!=points->end();++iter) {
		PoissonPoint* p = *iter;
		point_min.x = min(point_min.x,p->x);
		point_min.y = min(point_min.y,p->y);
		point_max.x = max(point_max.x,p->x);
		point_max.y = max(point_max.y,p->y);
	}
	type_float delta = r * 0.5;
	type_float half_delta = delta / 2;
	type_float scale_x = (1.0-delta) / (point_max.x-point_min.x);
	type_float scale_y = (1.0-delta) / (point_max.y-point_min.y);
	for (list<PoissonPoint*>::const_iterator iter=points->begin();iter!=points->end();++iter) {
		(*iter)->x = ((*iter)->x-point_min.x) * scale_x + half_delta;
		(*iter)->y = ((*iter)->y-point_min.y) * scale_y + half_delta;
	}*/

	return points;
}
inline list<PoissonPoint*>* generate_poisson_square(unsigned int num_points, int trial_points=30) {
	//Maximum density of grid should be 2/(r*r*sqrt(3)), where r is the distance between points (triangular grid)
	//Solve num_points=density*area for r

	//This should give us an estimate of the radius required to get the right number of sample points back.
	//In practice, we'll need more, because Poisson disk is not a triangular grid.  This fudge factor will
	//be added reciprocally.
	
	static type_float sqrt2_over_fourth3 = pow(2.0,0.5) / pow(3.0,0.25);

	type_float r = sqrt2_over_fourth3 / (1.25*pow((type_float)num_points,0.51));

	return generate_poisson_helper(r,num_points,trial_points,&compare_points_box,&in_domain_box);
}
inline list<PoissonPoint*>* generate_poisson_disk(unsigned int num_points, int trial_points=30) {
	//Maximum density of grid should be 2/(r*r*sqrt(3)), where r is the distance between points (triangular grid)
	//Solve num_points=density*area for r

	//This should give us an estimate of the radius required to get the right number of sample points back.
	//In practice, we'll need more, because Poisson disk is not a triangular grid.  This fudge factor will
	//be added reciprocally.
	
	static type_float sqrt2_over_fourth3 = pow(2.0,0.5) / pow(3.0,0.25);
	static type_float circle_factor = 1.0 / (0.25*glLibMathMisc::pi);

	type_float r = sqrt2_over_fourth3 / (1.3*pow((type_float)num_points*circle_factor,0.51));

	return generate_poisson_helper(r,num_points,trial_points,&compare_points_circle,&in_domain_circle);
}