#ifndef COCTREE_H
#define COCTREE_H
#include "Math/vec3.h"
#include <intrin.h>
#include "cAABB.h"
#include <vector>

template<class T>class cNode{
	public:
	// i chose to put all nodes in an array so I dont need to store all of the children, I just hold the pointer to the beginning knowing that there are guaranteed 8 children
	cNode* Children;
	cNode* Parent;
	bool Visible;
	//objects can be what ever you want. Most likely, they will be some pointer type, or built in data type that is a renderable object. 
	std::vector<T> Objects;
	// this type QueryID is my own datatype for my engine. You should basically replace this with your own way of storing the query for this particular node
	// if a query is sent out so the results of the query can be accessed later on.
	QueryID Queryid;
	//I could have stored a static pointer to the root node and stored the x y and z indicies to this node, but that would only save me a little space
	//so instead I just precompute the aabb and store it here. The total memory requirements still arent that had for an octree with all of this in it
	cAABB AABB;
	uint32_t LastVisited; // this is the framenumber that this node was last checked on. a uint32_t can hold a number big enough that you dont need to worry of overflow
	cNode(): Parent(0), Children(0), Visible(false), LastVisited(0) { }
	bool insert(T object){ Objects.push_back(object); return true;}
	bool remove(T object){ 
		for(vector<T>::iterator beg(Objects.begin()); beg!= Objects.end(); beg++){
			if(*beg == object){
				Objects.erase(beg);
				return true;
			}
		}
		return false;
	}
	void PullUpVisability(){// set this node to visable and if this node is visable it means the parents MUST be visable
		cNode* temp = this;
		Visible = true;
		while(temp->Parent){// continue setting the parents to visable
			temp = Parent;
			temp->Visible =true;
		}
	}
	bool IsVisible(){ return Visible; }
	void SetVisible(bool v){ Visible=v;}
	bool hasChildren(){ return Children != 0; }
	bool HasChildren(vector<cNode<T>*>& nodes){ 
		if(HasChildren()){// if there are children, add the 8 to the list
			for(uint32_t i(0); i< 8; i++){
				nodes.push_back(Children + i);
			}
			return true;
		}
		return false;
	}
	void SetLastVisited(uint32_t id) { LastVisited = id; }
	uint32_t LastVisited(){ return LastVisited; }
	void reset(){
		Visible =false;
		Objects.clear();
		LastVisited=0;
	}
	void SetQueryID(QueryID& id){ Queryid = id; }
	void AddObjects(vector<T>& objs){
		for(vector<T>::iterator beg(Objects.begin()); beg!= Objects.end(); beg++){
			objs.push_back(*beg);
		}
	}
};

// this octree is built for speed, so I will make sure that there is always at least a valid root node so i can skip a check every insert 
template<class T>class cOcTree{
public:

	/* This ocTree can be passed a level of zero, it just means that there is only a root node and nothing else.
	the levels should be the number of splits, and octsize should be the TOTAL SIZE of the ROOT box. 
	 the Children should then always be half the parents size on each dimension so if the octsize is 256, then
	 the total octree should be 256 tall by 256 wide and 256 depth. Therefore, the first level of children should he half that at 128x128x128 blocks, the second,  should be 1/2 of level 1 parents size at 64x64x64
	 a good size for this is probably the farplane on your world. You should be able to pass that without worrying if its a power of two. The init function will enforce the power of two
	 */
cOcTree(uint32_t levels, uint32_t octsize): Root(0) {
	Init(levels, octsize); // octsize NEEDs TO BE a power of two to get some nice speedups
}// octsize is the total size of the ROOT, if zero is passed, just the root node is created
~cOcTree(){ delete [] Root; }

void Init(uint32_t levels, uint32_t octsize){// levels can be thought of as the number of splits or divisions of the octree. Remeber, these divisions are exponential. 5 levels means the fifth level has 8^5 nodes, which is 32768.. so, realisticly, 4 levels should be the max at 4096
	if(octsize < 128 ) {// the min size has to be bigger than the number of divisions for the entire octreee. Anything smaller doesnt make sense and also defeats the purpose of an octree
		octsize = 128; // enforce a min size here
	}
	if(!POWOFTWO(octsize) ){// ENFORCE A POWER OF TWO. If the octsize is not a power of two, get the highest bit set and use that as the size, ignoring the rest
		DWORD highest(0), mask(static_cast<DWORD>(octsize));
		_BitScanReverse(&highest, mask);
		octsize = 1<<highest;// make sure that octsize is only a power of two!!
	}
	if(levels > 4) levels = 4;// no more than 4 levels on this please.. memory requirements need to be keep in check and too fine of checking defeeats the purpose
	TotalNodes=0;
	for(uint32_t i(0); i< levels+1; i++){
		TotalNodes+= static_cast<uint32_t>(powf(8.0f, static_cast<float>(i)));// each level has 8 to the power of the current level nodes. Level 0 (Root), has 8^0 = 1; level 1 has 8^1 = 8 nodes, level 2 has 8^3 = 64 nodes.. etc
	}
	memset(NodeLevelPointerArray, 0, sizeof(cNode*)*8 );
	delete [] Root;// just in case
	Root = new cNode[TotalNodes];
	cNode* temp = Root;
	////////////////////////SETUP ALL THE PARENT CHILDREN RELATIONSHIPS so the octree can be traversed normally!!!!
	// this is setup differently than a normal tree because I am chosing to store all of the nodes in one large array!!!!
	for(uint32_t i(0); i< levels; i++){// the root node will be pow(8, 0), which is 1, so it will run correctly
		uint32_t nodesinthislevel = static_cast<uint32_t>(powf(8.0f, static_cast<float>(i)));// this is the number of children at this current level I am setting up
		NodeLevelPointerArray[i] = temp;// this is the beginning of the level to be used for quickly finding levels later on
		for(uint32_t j(0); j< nodesinthislevel; j++){// for the number of nodes in this level
			temp->Children = (nodesinthislevel + temp - j) + ( 8 * j); 
			for(uint32_t k (0); k< 8; k++){
				(temp->Children+k)->Parent = temp;
			}
			temp++;
		}
	}
	NodeLevelPointerArray[levels] = temp;// set the last level manually because the loops above do not assign it
	NumberOfTreeLevels = levels;// Remeber, zero is a valid level number
	RootBV.Max = vec3(static_cast<float>(octsize), static_cast<float>(octsize), static_cast<float>(octsize));
	RootBV.Min = vec3(0, 0, 0);
	DWORD highest(0), sz(octsize);
	_BitScanReverse(&highest, sz);
	BitsToShiftForOcSpace = highest - (NumberOfTreeLevels+1);// this is what I need to shift by to get the bounding volumes into the [0, 2^(levels+1)] range for using bit stuff
}
bool insert(T object, const cAABB& bv){// the AABB SHOULD BE IN WORLD SPACE
	cAABB temp(bv);
	temp.MoveByOffset(-RootBV.Min);// move the box into the OcTree's posotive space. now testing can be done inside the octree
	if(!RootBV.Insertsect(temp)) return false;// the object is not within the root bv.. reject insertion
	cNode* node = GetNodeContaining(temp);
	return node->insert(object);
}
bool erase(T object, const cAABB& bv){// the AABB SHOULD BE IN WORLD SPACE
	cAABB temp(bv);
	temp.MoveByOffset(-RootBV.Min);// move the box into the OcTree's posotive space. now testing can be done inside the octree
	if(!RootBV.Insertsect(temp)) return false;// the object is not within the root bv.. reject
	cNode* node = GetNodeContaining(temp);
	return node->erase(object);
}
void reset(){// pretty neat, huh? No need for any recursion for this 
	for(uint32_t i(0); i<TotalNodes; i++){
		Root[i].reset();
	}
}
void MoveTo(const vec3& newpos){ RootBV.MoveTo(newpos); }
void MoveByOffset(const vec3& offset){ RootBV.MoveByOffset(offset);}
private:

cNode* GetNodeContaining(const cAABB &bv){
	/* a little info about this function. The normal method of traversing a tree for this would be to test each aabb for each level and go down through till you cannot insert any more
	then you go up one node and place the object there. In an octree, that means at a min of 1 aabb test per level, to a max of 8, assuming a 50% rate, there will be 4 tests per level
	plus, the one extra level to check that the insertion cannot continue any further, so if most objects fit into level 3 that would be
	(Number of levels tested + 1)  *  4 tests per level. For level 3 it would be    (3 + 1) * 4 = 16 AABB tests per insert. Each aabb test is 6 if's, but assuming only 50% of the if's are processed because of early breaks
	for a total of 16 * 3 branches = 48 branches to insert an object.. Assuming most fit into level 2, this would be 3 *4 = 12 * 3 = 36 branches. Still not good at all
	So, below is the code which takes advantage of the fact that our octree is divded up into powers of two so we can use a bunch of binary functions which lead to no 
	branches so it should be much fasters for finding the correct node
	
	/////////////Untested code below using sse2 instructions. I may skip the float to int conversion and do a manual bsr using the floating point numbers, but this may be too much work for little payoff
	__m128i min = _mm_srai_epi32(_mm_cvttps_epi32(_mm_set_ps(0, bv.Min.z, bv.Min.y, bv.Min.x)), BitsToShiftForOcSpace);
	__m128i max = _mm_srai_epi32(_mm_cvttps_epi32(_mm_set_ps(0, bv.Max.z, bv.Max.y, bv.Max.x)), BitsToShiftForOcSpace);
	__m128i result = _mm_xor_si128(min, max);

	// THERE IS NO FUNCTION FOR A BITSCAN TO OPERATE IN PARALELLLLLL So i have to break it up like this. MAKE SURE TO USE THE 32 BiT VERSION, not the 64 bit scan ok???
	unsigned long xindex(0), yindex(0), zindex(0);
	_BitScanReverse(&xindex, *(reinterpret_cast<unsigned long*>(&result) + 3));// REMEMBER, the xmm registers store everything in little endian format here, so the x is at the end of the array!!!
	_BitScanReverse(&yindex, *(reinterpret_cast<unsigned long*>(&result) + 2));
	_BitScanReverse(&zindex, *(reinterpret_cast<unsigned long*>(&result) + 1));
	*/

	// the next 6 lines of code will
	//scale the box into a [0, 2 ^ level] scale of units, then xor the dimensions with each other to get the highest bit set
	DWORD   minx1 = static_cast<DWORD>(bv.Min.x)>>BitsToShiftForOcSpace;
	DWORD   miny1 = static_cast<DWORD>(bv.Min.y)>>BitsToShiftForOcSpace;
	DWORD   minz1 = static_cast<DWORD>(bv.Min.z)>>BitsToShiftForOcSpace;

	DWORD   maxx1 = static_cast<DWORD>(bv.Max.x)>>BitsToShiftForOcSpace;
	DWORD   maxy1 = static_cast<DWORD>(bv.Max.y)>>BitsToShiftForOcSpace;
	DWORD   maxz1 = static_cast<DWORD>(bv.Max.z)>>BitsToShiftForOcSpace;

	DWORD   xResult = minx1 ^ maxx1;
	DWORD   yResult = miny1 ^ maxy1;
	DWORD   zResult = minz1 ^ maxz1;

	DWORD xindex(0), yindex(0), zindex(0);
	DWORD nodelevel(NumberOfTreeLevels), shiftcount(0);
	while (xResult + yResult + zResult!= 0 ){//Count highest bit position
        xResult>>= 1;
        yResult>>= 1;
		zResult>>=1;
        nodelevel--;
        shiftcount++;
    }

	cNode* level = NodeLevelPointerArray[nodelevel];// get the pointer to the array where the octree is broke up to where I can insert this aabb.. now find the exact point of insertion

	minx1>>=shiftcount;// this is how many bytes I need to shift to get into array space for indexing the array nodex
	miny1>>=shiftcount;// this is how many bytes I need to shift to get into array space for indexing the array nodex
	minz1>>=shiftcount;// this is how many bytes I need to shift to get into array space for indexing the array nodex
	uint32_t jump = (nodelevel -1);

	return &(level[(minz1<<(jump*jump)) + (miny1<<jump) + minx1] );

}

	cOcTree();// dont allow the creation of an empty one
	cOcTree& operator=(cOcTree& other);// or this either
	cNode* Root;
	cNode* NodeLevelPointerArray[8];// this will hold points to each level of the octree so I can goto a level fast. There will never be more than 8 levels
	cAABB RootBV;
	uint32_t NumberOfTreeLevels, BitsToShiftForOcSpace, TotalNodes;// BitsToShift is the number of times the position needs to be shifted in order to be in the [0, 2^levels] space
};


#endif