#pragma once
#include "../includes.h"
#include "scene_elements.h"
#include "../render/trace.h"

template <typename type_float> void vec_min(type_float vec[3], type_float bound_min[3]) {
	for (char i=0;i<3;++i) bound_min[i] = min(vec[i],bound_min[i]);
}
template <typename type_float> void vec_max(type_float vec[3], type_float bound_max[3]) {
	for (char i=0;i<3;++i) bound_max[i] = max(vec[i],bound_max[i]);
}
template <typename type_float> void vec_min_max(type_float vec[3], type_float bound_min[3], type_float bound_max[3]) {
	vec_min(vec,bound_min);
	vec_max(vec,bound_max);
}
template <typename type_float> void reset_bounds(type_float bound_min[3], type_float bound_max[3]) {
	bound_min[0] = bound_min[1] = bound_min[2] = MAX_FP;
	bound_max[0] = bound_max[1] = bound_max[2] = MIN_FP;
}
template <typename type_float> void determine_bounds(type_float bound_min[3], type_float bound_max[3], Object* obj, bool reset=true) {
	if (reset) reset_bounds(bound_min,bound_max);
	if (obj==NULL) return;
	switch (obj->type) {
		case (SCENE_OBJECT_TYPE_POINT):
			vec_min_max(obj->position,bound_min,bound_max);
			break;
		case (SCENE_OBJECT_TYPE_SPHERE):
			bound_min[0] = min( bound_min[0], obj->position[0]-((Sphere*)(obj))->radius );
			bound_min[1] = min( bound_min[1], obj->position[1]-((Sphere*)(obj))->radius );
			bound_min[2] = min( bound_min[2], obj->position[2]-((Sphere*)(obj))->radius );
			bound_max[0] = max( bound_max[0], obj->position[0]+((Sphere*)(obj))->radius );
			bound_max[1] = max( bound_max[1], obj->position[1]+((Sphere*)(obj))->radius );
			bound_max[2] = max( bound_max[2], obj->position[2]+((Sphere*)(obj))->radius );
			break;
		case (SCENE_OBJECT_TYPE_PLANE):
			//Do nothing because we're going to test all planes anyway
			break;
		case (SCENE_OBJECT_TYPE_TRIANGLE):
			vec_min_max(             obj  ->position, bound_min,bound_max);
			vec_min_max(((Triangle*)(obj))->position2,bound_min,bound_max);
			vec_min_max(((Triangle*)(obj))->position3,bound_min,bound_max);
			break;
		default:
			throw glLib::error("Unrecognized type!");
	}
}
template <typename type_float> void determine_bounds(type_float bound_min[3], type_float bound_max[3], list<Object*>* objs, bool reset=true) {
	if (reset) reset_bounds(bound_min,bound_max);
	for (list<Object*>::const_iterator iter=objs->begin();iter!=objs->end();++iter) {
		Object* obj = *iter;
		determine_bounds(bound_min,bound_max,obj,false);
	}
}

template <typename type_float> class BoundingVolume {
	template <typename type_float2> friend bool compare_coords_x(BoundingVolume<type_float2>* a, BoundingVolume<type_float2>* b);
	template <typename type_float2> friend bool compare_coords_y(BoundingVolume<type_float2>* a, BoundingVolume<type_float2>* b);
	template <typename type_float2> friend bool compare_coords_z(BoundingVolume<type_float2>* a, BoundingVolume<type_float2>* b);
	template <class type_float2> friend class NodeBVH;
	private:
		type_float _min[3];
		type_float _max[3];
		type_float _center[3];
		Object* object;
		bool _is_infinite;
	public:
		BoundingVolume() {
			reset_bounds(_min,_max); _is_infinite = true;
			object = NULL;
			recalculate_average();
		}
		BoundingVolume(Object* obj) {
			determine_bounds(_min,_max,obj); //initializes to max/min int
			if (obj==NULL) _is_infinite = true;
			else _is_infinite = false;
			object = obj;
			recalculate_average();
		}
		BoundingVolume(list<Object*>* bvs) {
			reset_bounds(_min,_max);
			object = NULL;
			_is_infinite = true;
			for (list<Object*>::const_iterator iter=bvs->begin();iter!=bvs->end();++iter) {
				BoundingVolume temp(*iter);
				add_min(temp._min);
				add_max(temp._max);
				_is_infinite = false;
			}
			//recalculate_average(); //add_min and add_max do this
		}
		BoundingVolume(list<BoundingVolume<type_float>*>* bvs) {
			reset_bounds(_min,_max);
			object = NULL;
			_is_infinite = true;
			for (typename list<BoundingVolume<type_float>*>::const_iterator iter=bvs->begin();iter!=bvs->end();++iter) {
				add_min((*iter)->_min);
				add_max((*iter)->_max);
				_is_infinite = false;
			}
			//recalculate_average(); //add_min and add_max do this
		}
		BoundingVolume(BoundingVolume<type_float>* a, BoundingVolume<type_float>* b) {
			reset_bounds(_min,_max);
			object = NULL;
			add_min(a->_min); add_max(a->_max);
			add_min(b->_min); add_max(b->_max);
			_is_infinite = a->_is_infinite||b->_is_infinite;
			//recalculate_average(); //add_min and add_max do this
		}

		inline type_float get_volume(void) {
			return _is_infinite ? MAX_DOUBLE : (_max[0]-_min[0])*(_max[1]-_min[1])*(_max[2]-_min[2]);
		}
		void add_min(type_float _min[3]) {
			for (char i=0;i<3;++i) this->_min[i] = min(this->_min[i],_min[i]);
			_is_infinite = false;
			recalculate_average();
		}
		void add_max(type_float _max[3]) {
			for (char i=0;i<3;++i) this->_max[i] = max(this->_max[i],_max[i]);
			_is_infinite = false;
			recalculate_average();
		}
		static type_float get_overlap(BoundingVolume<type_float>* a, BoundingVolume<type_float>* b) {
			BoundingVolume<type_float> overlap = BoundingVolume<type_float>();
			overlap.add_min(a->_min);
			overlap.add_min(b->_min);
			overlap.add_max(a->_max);
			overlap.add_max(b->_max);
			return overlap.get_volume();
		}
		void print_float(type_float val) {
			if (val>=(type_float)(10.0)) printf("+INF");
			else if (val<=(type_float)(-10.0)) printf("-INF");
			else printf("%+1.1f",val);
		}
		void print_bounds(void) {
			printf("(");print_float(_min[0]);printf(",");print_float(_max[0]);printf(") ");
			printf("(");print_float(_min[1]);printf(",");print_float(_max[1]);printf(") ");
			printf("(");print_float(_min[2]);printf(",");print_float(_max[2]);printf(")");
		}
	private:
		bool intersect(type_float origin[3], type_float recip_direction[3], type_float* distance) {
			//Check for degenerate case, origin inside box
			if (_min[0]<=origin[0]&&origin[0]<=_max[0] &&
				_min[1]<=origin[1]&&origin[1]<=_max[1] &&
				_min[2]<=origin[2]&&origin[2]<=_max[2]) {
				*distance = (type_float)(0.0);
				return true;
			}

			//http://www.cs.utah.edu/~awilliam/box/box.pdf
			type_float tmin, tmax, tymin, tymax, tzmin, tzmax;

			if (recip_direction[0]<(type_float)(0.0)) {
				tmin  = (_max[0] - origin[0]) * recip_direction[0];
				tmax  = (_min[0] - origin[0]) * recip_direction[0];
			}
			else {
				tmin  = (_min[0] - origin[0]) * recip_direction[0];
				tmax  = (_max[0] - origin[0]) * recip_direction[0];
			}
			if (recip_direction[1]<(type_float)(0.0)) {
				tymin = (_max[1] - origin[1]) * recip_direction[1];
				tymax = (_min[1] - origin[1]) * recip_direction[1];
			}
			else {
				tymin = (_min[1] - origin[1]) * recip_direction[1];
				tymax = (_max[1] - origin[1]) * recip_direction[1];
			}

			if (tmin>tymax||tymin>tmax) return false;
			if (tymin>tmin) tmin = tymin;
			if (tymax<tmax) tmax = tymax;

			if (recip_direction[2]<(type_float)(0.0)) {
				tzmin = (_max[2] - origin[2]) * recip_direction[2];
				tzmax = (_min[2] - origin[2]) * recip_direction[2];
			}
			else {
				tzmin = (_min[2] - origin[2]) * recip_direction[2];
				tzmax = (_max[2] - origin[2]) * recip_direction[2];
			}

			if ( (tmin > tzmax) || (tzmin > tmax) ) return false;
			if (tzmin > tmin) tmin = tzmin;
			if (tzmax < tmax) tmax = tzmax;

			*distance = (tmin<(type_float)(0.0)) ? tmax : tmin; //tmin<t1 && tmax>t0;
			return true;
		}
		void recalculate_average(void) {
			//Average position
			glLib::Math::Vec::add3(_min,_max,_center);
			glLib::Math::Vec::scale3((type_float)(0.5),_center);
		}
};

template <typename type_float> bool compare_coords_x(BoundingVolume<type_float>* a, BoundingVolume<type_float>* b) {
	return (a->_center[0]<b->_center[0]);
}
template <typename type_float> bool compare_coords_y(BoundingVolume<type_float>* a, BoundingVolume<type_float>* b) {
	return (a->_center[1]<b->_center[1]);
}
template <typename type_float> bool compare_coords_z(BoundingVolume<type_float>* a, BoundingVolume<type_float>* b) {
	return (a->_center[2]<b->_center[2]);
}

bool intersect_object(list<Object*>::const_iterator iter, Ray ray, RayInfo* ray_info);

template <typename type_float> class NodeBVH {
	public:
		int depth;
		NodeBVH *child1, *child2;
		list<Object*>* elements;
		BoundingVolume<type_float>* bv;
		bool is_leaf;
	public:
		NodeBVH(list<BoundingVolume<type_float>*>* bounding_volumes, int depth, int max_depth) {
			this->depth = depth;
			//print_start(); printf("Adding node at depth %d with %d to store!\n",depth,bounding_volumes->size());
			//print_start(); printf("| \n");
			if (bounding_volumes->size()==1 || depth==max_depth) {
				is_leaf = true;
				//print_start(); printf("| This is a leaf node.\n");
				child1 = child2 = NULL;
				elements = new list<Object*>();
				for (typename list<BoundingVolume<type_float>*>::const_iterator iter=bounding_volumes->begin();iter!=bounding_volumes->end();++iter) {
					//print_start(); printf("| Adding bounding volume "); (*iter)->print_bounds(); printf(".\n");
					elements->push_back((*iter)->object);
				}
				//print_start(); printf("| \n");
				bv = new BoundingVolume<type_float>(elements);
			}
			else {
				is_leaf = false;
				//print_start(); printf("| This is an ordinary node.\n");
				//print_start(); printf("| Attempting to find the two smallest BVs.\n");

				elements = NULL;
				list<BoundingVolume<type_float>*> list_x_1, list_x_2, list_y_1, list_y_2, list_z_1, list_z_2;

				bounding_volumes->sort(compare_coords_x<type_float>);
				double best_overlap_x = optimize(bounding_volumes, &list_x_1,&list_x_2);
				/*print_start(); printf("| In x, the best overlap is %f, corresponding to:\n",best_overlap_x);
				print_start(); printf("| | List 1  ( left): "); BoundingVolume(&list_x_1).print_bounds(); printf("\n");
				for (list<BoundingVolume*>::const_iterator iter=list_x_1.begin();iter!=list_x_1.end();++iter) {
					print_start(); printf("| | | "); (*iter)->print_bounds(); printf(".\n");
				}
				print_start(); printf("| | List 2  (right): "); BoundingVolume(&list_x_2).print_bounds(); printf("\n");
				for (list<BoundingVolume*>::const_iterator iter=list_x_2.begin();iter!=list_x_2.end();++iter) {
					print_start(); printf("| | | "); (*iter)->print_bounds(); printf(".\n");
				}*/

				bounding_volumes->sort(compare_coords_y<type_float>);
				double best_overlap_y = optimize(bounding_volumes, &list_y_1,&list_y_2);
				/*print_start(); printf("| In y, the best overlap is %f, corresponding to:\n",best_overlap_x);
				print_start(); printf("| | List 1 (bottom): "); BoundingVolume(&list_y_1).print_bounds(); printf("\n");
				for (list<BoundingVolume*>::const_iterator iter=list_y_1.begin();iter!=list_y_1.end();++iter) {
					print_start(); printf("| | | "); (*iter)->print_bounds(); printf(".\n");
				}
				print_start(); printf("| | List 2 (   top): "); BoundingVolume(&list_y_2).print_bounds(); printf("\n");
				for (list<BoundingVolume*>::const_iterator iter=list_y_2.begin();iter!=list_y_2.end();++iter) {
					print_start(); printf("| | | "); (*iter)->print_bounds(); printf(".\n");
				}*/

				bounding_volumes->sort(compare_coords_z<type_float>);
				double best_overlap_z = optimize(bounding_volumes, &list_z_1,&list_z_2);
				/*print_start(); printf("| In z, the best overlap is %f, corresponding to:\n",best_overlap_x);
				print_start(); printf("| | List 1 (front?): "); BoundingVolume(&list_z_1).print_bounds(); printf("\n");
				for (list<BoundingVolume*>::const_iterator iter=list_z_1.begin();iter!=list_z_1.end();++iter) {
					print_start(); printf("| | | "); (*iter)->print_bounds(); printf(".\n");
				}
				print_start(); printf("| | List 2 ( back?): "); BoundingVolume(&list_z_2).print_bounds(); printf("\n");
				for (list<BoundingVolume*>::const_iterator iter=list_z_2.begin();iter!=list_z_2.end();++iter) {
					print_start(); printf("| | | "); (*iter)->print_bounds(); printf(".\n");
				}*/

				if (best_overlap_x<best_overlap_y&&best_overlap_x<best_overlap_z) { //best = x
					//print_start(); printf("| x is the best!\n");
					child1 = new NodeBVH<type_float>(&list_x_1,depth+1,max_depth);
					child2 = new NodeBVH<type_float>(&list_x_2,depth+1,max_depth);
				}
				else if (best_overlap_y<best_overlap_x&&best_overlap_y<best_overlap_z) { //best = y
					//print_start(); printf("| y is the best!\n");
					child1 = new NodeBVH<type_float>(&list_y_1,depth+1,max_depth);
					child2 = new NodeBVH<type_float>(&list_y_2,depth+1,max_depth);
				}
				else { //best = z
					//print_start(); printf("| z is the best!\n");
					child1 = new NodeBVH<type_float>(&list_z_1,depth+1,max_depth);
					child2 = new NodeBVH<type_float>(&list_z_2,depth+1,max_depth);
				}

				bv = new BoundingVolume<type_float>(child1->bv,child2->bv);
			}
			//print_start(); printf("This BV is at "); bv->print_bounds(); printf(".\n");
			//print_start(); printf("\n");
		}
		~NodeBVH() {
			if (child1!=NULL) delete child1;
			if (child2!=NULL) delete child2;
			if (elements!=NULL) delete elements;
			delete bv;
		}

		unsigned int intersect(Ray ray, RayInfo* ray_info) {
			type_float distance;
			if (!bv->intersect(ray.origin,ray.get_direction_reciprocal(),&distance)) return 1;

			unsigned int total;
			if (elements==NULL) {
				total = 0;
				total += child1->intersect(ray,ray_info);
				total += child2->intersect(ray,ray_info);
			}
			else {
				for (list<Object*>::const_iterator iter=elements->begin(); iter!=elements->end(); ++iter) {
					RayInfo temp = RayInfo();
					bool hit = intersect_object(iter,ray,&temp);
					if (hit) {
						if (temp.hit_distance<=0.0) continue;
						if (temp.hit_distance<ray_info->hit_distance) {
							ray_info->hit_object = *iter;
							ray_info->set_hit(*iter,temp.hit_distance);
						}
					}
				}
				total = elements->size();
			}
			return total;
		}

		double get_avg_in_leaf(void) {
			return (is_leaf) ? (double)(elements->size()) : (child1->get_avg_in_leaf()+child2->get_avg_in_leaf())/2.0;
		}
		unsigned int get_max_in_leaf(void) {
			return (is_leaf) ? elements->size() : max(child1->get_max_in_leaf(),child2->get_max_in_leaf());
		}
		double get_avg_depth(void) {
			return (is_leaf) ? (double)(depth) : (child1->get_avg_depth()+child2->get_avg_depth())/2.0;
		}
		unsigned int get_max_depth(void) {
			return (is_leaf) ? depth : max(child1->get_max_depth(),child2->get_max_depth());
		}

		void print(void) {
			print_start(); printf("Begin node:\n");
			if (elements==NULL) {
				print_start(); printf("Child 1:\n");
				child1->print();
				print_start(); printf("End child 1:\n");
				print_start(); printf("Child 2:\n");
				child2->print();
				print_start(); printf("End child 2:\n");
			}
			else {
				for (list<Object*>::const_iterator iter=elements->begin();iter!=elements->end();++iter) {
					Object* obj = *iter;
					print_start(); printf("  Object \"%s\" at ",obj->name->c_str());
					glLib::Math::Vec::print3(obj->position);
				}
			}
			print_start(); printf("End node\n");
		}
	private:
		inline void print_start(void) {
			for (int i=0;i<depth;++i) printf("|   ");
		}
		double optimize(list<BoundingVolume<type_float>*>* data, list<BoundingVolume<type_float>*>* l1, list<BoundingVolume<type_float>*>* l2) {
			typename list<BoundingVolume<type_float>*>::const_iterator iter = data->begin();
			unsigned int i = 0;
			for (;i<data->size()/2;++i) {
				l1->push_back(*iter);
				++iter;
			}
			for (;i<data->size();++i) {
				l2->push_back(*iter);
				++iter;
			}
			return BoundingVolume<type_float>(l1).get_volume() + BoundingVolume<type_float>(l2).get_volume();

			/*//Optimize to find the two AABB that have the smallest summed volume

			for (list<BoundingVolume*>::const_iterator iter=data->begin();iter!=data->end();++iter) {
				BoundingVolume* bv = *iter;
				l2->push_back(bv);
			}

			BoundingVolume bv1,bv2(l2);
			print_start(); printf("Beginning with "); bv2.print_bounds(); printf("\n");

			double total_previous = bv2.get_volume();

			while (l2->size()>0) {
				print_start(); printf("Starting iteration\n");
				print_start(); printf("  Previous sum: %f\n",total_previous);

				BoundingVolume* pivot = l2->front();
				l1->push_back(pivot);
				l2->pop_front();
				bv1.add_min(pivot->_min);
				bv1.add_max(pivot->_max);
				bv2 = BoundingVolume(l2);

				print_start(); printf("  List 1: "); BoundingVolume(l1).print_bounds(); printf("\n");
				for (list<BoundingVolume*>::const_iterator iter=l1->begin();iter!=l1->end();++iter) {
					print_start(); printf("    "); (*iter)->print_bounds(); printf(".\n");
				}
				print_start(); printf("  List 2: "); BoundingVolume(l2).print_bounds(); printf("\n");
				for (list<BoundingVolume*>::const_iterator iter=l2->begin();iter!=l2->end();++iter) {
					print_start(); printf("    "); (*iter)->print_bounds(); printf(".\n");
				}

				double total = bv1.get_volume() + bv2.get_volume();

				print_start(); printf("  Sum: %f = %f + %f\n",total,bv1.get_volume(),bv2.get_volume());

				if (total>total_previous) {
					//passed optimization point or used all elements; go back and then return
					BoundingVolume* pivot = l1->back();
					l2->push_front(pivot);
					l1->pop_back();
					print_start(); printf("Passed optimization point; reverting and returning.\n");
					return total_previous;
				}
				total_previous = total;
				print_start(); printf("  Ending iteration\n");
			}
			print_start(); printf("Surpassed all elements.\n");
			return bv1.get_volume(); //took all the elements out of l2 and put them into l1*/
		}
};
template <typename type_float> class WeightedBVH {
	private:
		type_float _min[3];
		type_float _max[3];
		list<Object*>* _objects;
		list<Plane*>* _planes;
		list<BoundingVolume<type_float>*>* _bounding_volumes;
		NodeBVH<type_float>* tree;
		unsigned int _max_allowed_depth;
		unsigned int _num_objects_triangles;
		unsigned int _num_objects_spheres;
		unsigned int _num_objects_planes;
	public:
		WeightedBVH(list<Object*>* objects, unsigned int max_allowed_depth) {
			_max_allowed_depth = max_allowed_depth;
			_objects = objects;
			_planes = new list<Plane*>();
			determine_bounds(_min,_max,objects);

			_num_objects_triangles = 0;
			_num_objects_spheres = 0;
			_num_objects_planes = 0;
			for (list<Object*>::const_iterator iter=objects->begin(); iter!=objects->end(); ++iter) {
				switch ((*iter)->type) {
					case SCENE_OBJECT_TYPE_TRIANGLE:
						++_num_objects_triangles;
						break;
					case SCENE_OBJECT_TYPE_SPHERE:
						++_num_objects_spheres;
						break;
					case SCENE_OBJECT_TYPE_PLANE:
						++_num_objects_planes;
						_planes->push_back((Plane*)(*iter)); //can hit, but can't be optimized by BVH
						break;
					case SCENE_OBJECT_TYPE_POINT: //ignore; can't hit it
						break;
					default:
						throw glLib::error("Unrecognized object type!");
				}
			}

			//printf("Got bounding volumes\n");
			_bounding_volumes = new list<BoundingVolume<type_float>*>();
			for (list<Object*>::const_iterator iter=objects->begin();iter!=objects->end();++iter) {
				_bounding_volumes->push_back(new BoundingVolume<type_float>(*iter));
				//printf("  ");_bounding_volumes->back()->print_bounds(); printf("\n");
			}

			tree = new NodeBVH<type_float>(_bounding_volumes,0,_max_allowed_depth);
		}
		~WeightedBVH() {
			delete _planes;

			for (typename list<BoundingVolume<type_float>*>::iterator iter=_bounding_volumes->begin();iter!=_bounding_volumes->end();++iter) {
				delete (*iter);
			}
			delete _bounding_volumes;

			delete tree;
		}

		unsigned int intersect(Ray ray, RayInfo* ray_info) {
			unsigned int tests = tree->intersect(ray,ray_info);

			for (list<Plane*>::const_iterator iter=_planes->begin(); iter!=_planes->end(); ++iter) {
				Plane* plane = *iter;
				type_float obj_dist;
				if (!glLib::Math::Intersect::ray_plane(
					ray.origin,ray.get_direction(),plane->position,plane->normal,&obj_dist)
				) continue;
				if (obj_dist<ray_info->hit_distance) {
					ray_info->set_hit(*iter,obj_dist);
				}
				++tests;
			}

			return tests;
		}

		inline double get_avg_in_leaf(void) {
			return tree->get_avg_in_leaf();
		}
		inline unsigned int get_max_in_leaf(void) {
			return tree->get_max_in_leaf();
		}
		inline double get_avg_depth(void) {
			return tree->get_avg_depth();
		}
		inline unsigned int get_max_depth(void) {
			return tree->get_max_depth();
		}
		inline unsigned int get_num_objects(void) {
			return _objects->size();
		}
		inline unsigned int get_max_possible_depth(void) {
			return _max_allowed_depth;
		}
		inline unsigned int get_num_objects_triangles(void) {
			return _num_objects_triangles;
		}
		inline unsigned int get_num_objects_spheres(void) {
			return _num_objects_spheres;
		}
		inline unsigned int get_num_objects_planes(void) {
			return _num_objects_planes;
		}

		inline void print(void) {
			tree->print();
		}
	private:
};