//#ifndef Octree_H_Inside_Header
//#error Do not include OctreeNode.tcc directly, instead include Octree.hpp
//#endif
#include "Octree.hpp"

#include <fstream>
#include <iostream>
#include <newmatap.h>
#include <cmath>

#include "OctreeSupport.hpp"



// Point adding for each OctreeType:
/*! Binary Occupancy
Sets the value of the MaxDepth leaf containing the point to true.  If a node at any level is found
which has a value of true, the function returns without needing to make any changes.  If the tree
is not yet split down to MaxDepth at the input point and the value is not true at that location,
AddPoint will split nodes until it is MaxDepth.
*/
template <class ValueType>
void
Octree<ValueType>::OctreeNode::
AddPointBinaryOccupancy(const Octree<ValueType>& OT, const Vector& point, const int depth) {
	// if at depth, add the point, otherwise figure out which child it goes with and call the child's addPoint
	if(depth < OT.MaxDepth) {
		//if value is true, we are done, otherwise we need to split
		if((children == NULL) && !this->value) {
			children = new OctreeNode*[8];
			for(int index = 0; index < 8; index++) {
				children[index] = new OctreeNode;
			}
		}
		//add the point one layer down
		children[OT.GetPointChildNumber(point, depth)]->AddPointBinaryOccupancy(OT, point, depth + 1);
	} else {
		//at depth
		value = true;
	}
}

/*! AddData
Takes a point and a value, and assignes the value to the leaf at MaxDepth containing that point.
If the tree is not yet split down to MaxDepth at the input point, AddData will split nodes until it is.
Assignment is always done, so the last entry written to a given leaf wins.  Collapsed values are
perserved in all of the new children except the MaxDepth leaf in which the new data value is located.
*/
template <class ValueType>
void
Octree<ValueType>::OctreeNode::
AddData(const Octree<ValueType>& OT, const Vector& point, const ValueType data, const int depth) {
	// if at depth, add the point, otherwise figure out which child it goes with and call the child's addData
	if(depth < OT.MaxDepth) {
		//if we don't have children, we need to split
		if(children == NULL) {
			children = new OctreeNode*[8];
			for(int index = 0; index < 8; index++) {
				children[index] = new OctreeNode(this->value);
			}
			this->value = OT.EmptyValue;
		}
		//add the data one layer down
		children[OT.GetPointChildNumber(point, depth)]->AddData(OT, point, data, depth + 1);
	} else {
		//at depth
		value = data;
	}
}

//Exists to satisfy c++, do not ever call it.
template <class ValueType>
void 
Octree<ValueType>::OctreeNode::
AddPointsPlanarFitFromDEM(const Octree<ValueType>& OT, const Vector* const points, const unsigned long* const indicies, 
						  const long numPoints, const int depth, const Vector& nodeLowerBounds, const Vector& nodeUpperBounds, 
						  const Vector& surfaceNormal, double MaximumAllowedFittingError){
	//Do nothing line to avoid Warning Unused parameter
	if(numPoints > depth && nodeLowerBounds.x < nodeUpperBounds.y){OT.ContainsPoint(points[indicies[0]]);}
	
	fprintf(stderr, "\nPlanarFit AddPoints called with ValueType not PlanarFitNode\n");
	return;
}

/*! AddPointsPlanarFitFromDEM
This constructor requires the normal vector passed in from Octree.cpp AddPoints has the correct 
direction (should point up).  Also, the tolerance parameter MaximumAllowedFittingError is set there.

When at less than max depth, this funciton confirme some coverage requirement in the node, fits a plane, 
tests the tolerance, and decides if that plane is a good fit to the points.  If it is, we're done; 
otherwise, the node splits.  

When at max depth, if there are three or more points, we fit a plane and stick with it... nothing else 
to do.  If there are two points, a the normal vector is chosen so that there is no slope across the 
line of the points.  If there is only one point, a horizontal plane at that height is fit.  
*/

template <>
void 
Octree<PlanarFitNode>::OctreeNode::
AddPointsPlanarFitFromDEM(const Octree<PlanarFitNode>& OT, const Vector* const points, const unsigned long* const indicies, 
						  const long numPoints, const int depth, const Vector& nodeLowerBounds, const Vector& nodeUpperBounds, 
						  const Vector& surfaceNormal, double MaximumAllowedFittingError){
	
	if(numPoints == 0){//no points: don't even try
		value = PlanarFitNode();
		return;
	}
	
	if(depth < OT.MaxDepth){//depth less than max: we might split this node
		
		//make sure there is a point in each quadrant (looking vertically) to ensure good coverage
		bool NorthWest = false;
		bool NorthEast = false;
		bool SouthWest = false;
		bool SouthEast = false;
		
		Vector midPoint = nodeUpperBounds + nodeLowerBounds;
		midPoint /= 2.0;
		
		for(int index = 0; index < numPoints; index++){
			NorthWest |= ((points[indicies[index]].x < midPoint.x) && (points[indicies[index]].y > midPoint.y));
			NorthEast |= ((points[indicies[index]].x > midPoint.x) && (points[indicies[index]].y > midPoint.y));
			SouthWest |= ((points[indicies[index]].x < midPoint.x) && (points[indicies[index]].y < midPoint.y));
			SouthEast |= ((points[indicies[index]].x > midPoint.x) && (points[indicies[index]].y < midPoint.y));
		}
		if(NorthEast && NorthWest && SouthEast && SouthWest){
			/* now we know that there is good coverage in this node (and at least 4 points)
			We'll fit a plane and see how good it is.*/
			value = PlanarFitNode(points, indicies, numPoints, nodeLowerBounds, surfaceNormal);
			bool goodFit = true;
			for(int index = 0; index < numPoints; index++){
				//test the fit: if any point is too far away from the plane, split the node
				goodFit &= (std::abs((value.unitNormal.Dot(points[indicies[index]] - nodeLowerBounds) - value.offset)) < MaximumAllowedFittingError);
			}
			if(goodFit){
				return;
			}
		}
		
		//either coverage was bad or the planar fit was bad, so we will split the node
		value = PlanarFitNode(0);
		if(children == NULL){ //it had better be NULL or you might not be doing what you think you are doing
			children = new OctreeNode*[8];
			for(int index = 0; index < 8; index ++){
				children[index] = new OctreeNode;
			}
		}
		
		// divide up the points: first get the childNumber for each point
		unsigned short* pointChildNumbers;
		pointChildNumbers = new unsigned short[numPoints];
		unsigned long numPointsPerChild[8] = {0,0,0,0,0,0,0,0};
		unsigned short childNumber;
		for(int index = 0; index <numPoints; index++){
			childNumber = OT.GetPointChildNumber(points[indicies[index]], depth);
			pointChildNumbers[index] = childNumber;
			numPointsPerChild[childNumber] ++;
		}
		
		//Then convert the pointChildNumbers into a list of indicies to pass down to the child's constructor
		for(childNumber = 0; childNumber < 8; childNumber++){
			unsigned long* childIndicies = new unsigned long [numPointsPerChild[childNumber]];
			unsigned long count = 0;
			for(int index = 0; index < numPoints; index ++){
				if(pointChildNumbers[index] == childNumber){
					childIndicies[count] = indicies[index];
					count ++;
				}
			}//count == numPointsPerChild[childNumber]
			
			Vector childUpperBounds, childLowerBounds;
			if(numPointsPerChild[childNumber] >= 1){
				OT.CalculateBoundsFromPath(childLowerBounds, childUpperBounds, OT.FindPathToPoint(points[childIndicies[0]]), depth + 1);
			}//if the child has no points, the bounds don't matter as teh numPoints == 0 test at the very start will trigger
			children[childNumber]->AddPointsPlanarFitFromDEM(OT, points, childIndicies, numPointsPerChild[childNumber], depth + 1, childLowerBounds, childUpperBounds, surfaceNormal, MaximumAllowedFittingError);
			delete[] childIndicies;//don't leak memory
		}
		delete[] pointChildNumbers;
	}else{//we are at full depth, and there is no splitting to consider
		switch(numPoints){
			case 1: {//horizontal plane at the height of the point
				value = PlanarFitNode(surfaceNormal, (points[indicies[0]].z - nodeLowerBounds.z));
				value.offset *= value.unitNormal.z;
			}
			break;
			case 2: {//Fit a plane to the two points so that it's normal vector is as vertical as possible
				Vector rVector01 = points[indicies[0]] - points[indicies[1]];
				Vector rVector12 (rVector01.y, -rVector01.x, 0);
				value.unitNormal = Vector(rVector01.y * rVector12.z - rVector01.z * rVector12.y, 
										  rVector01.z * rVector12.x - rVector01.x * rVector12.z, 
										  rVector01.x * rVector12.y - rVector01.y * rVector12.x);
				if(value.unitNormal.Dot(surfaceNormal) >= 0){
					value.unitNormal /= value.unitNormal.Norm();
				}else{
					value.unitNormal /= -value.unitNormal.Norm();
				}
				value.offset = value.unitNormal.Dot(points[indicies[0]] - nodeLowerBounds);
			}
			break;
			default: {//at least three points: planarFitNode can work with this
				value = PlanarFitNode(points, indicies, numPoints, nodeLowerBounds, surfaceNormal);
			}
		}
	}
}


/* Collapse function
Collapsing a node compresses the tree.  It only collapses nodes where the whole volume 
is uniform.
*/
template <class ValueType>
void
Octree<ValueType>::OctreeNode::
Collapse(void) {
	/* first run each child's collapse method.  Then test to see if they are all the
	same, and this node can represent all of them.
	Criteria for collapsing this node:
		all children have the same values
		all children have no children of their own
	*/
	
	if(children != NULL) {
	
		children[0]->Collapse();
		ValueType testValue = children[0]->value;
		bool collapseThisNode = (children[0]->children == NULL);
		for(int index = 1; index < 8; index++) {
			children[index]->Collapse();
			
			collapseThisNode &= (children[index]->children == NULL);
			collapseThisNode &= (testValue == children[index]->value);
			
		}
		if(collapseThisNode) {
			value = testValue;
			for(int index = 0; index < 8; index++) {
				delete children[index];
			}
			delete[] children;
			children = NULL;
		}
	}
}

// Save, Load, and Print functions
/* Save
For use by Octree SaveToFile.
*/
template <class ValueType>
bool
Octree<ValueType>::OctreeNode::
SaveToFile(std::FILE* saveFile) const {
	/* Depth first ordering of nodes.  Each node saves its value, then a boolean
	for if it has children or not.
	*/
	//value
	std::fwrite(&value, sizeof(value), 1, saveFile);
	
	//children
	bool hasChildren = (children != NULL);
	std::fwrite(&hasChildren, sizeof(hasChildren), 1, saveFile);
	
	//set up for children to write
	if(hasChildren) {
		for(int index = 0; index < 8; index++) {
			children[index]->SaveToFile(saveFile);
		}
	}
	return !std::ferror(saveFile);
}

/* Load
For use by Octree LoadFromFile.
*/
template <class ValueType>
bool
Octree<ValueType>::OctreeNode::
LoadFromFile(std::FILE* loadFile) {
	//first the value
	if(0 == std::fread(&value, sizeof(ValueType), 1, loadFile)) {
		return false;
	}
	//then the children
	bool hasChildren;
	if(0 == std::fread(&hasChildren, sizeof(bool), 1, loadFile)) {
		return false;
	}
	
	//clean up any old stuff in this structure regardless of hasChildren
	if(children != NULL) {
		for(int index = 0; index < 8; index++) {
			delete children[index];
		}
		delete[] children;
	}
	children = NULL;
	
	//and allocate the children if needed
	bool returnValue = true;
	if(hasChildren) {
		children = new OctreeNode*[8];
		for(int index = 0; index < 8; index++) {
			children[index] = new OctreeNode;
			returnValue &= children[index]->LoadFromFile(loadFile);
		}
	}
	return returnValue;
}

/* Print
Bad idea for large Octrees.  Used mostly when debugging on small testcase Octrees.
*/
template <class ValueType>
void
Octree<ValueType>::OctreeNode::
Print(int num) const {
	//Depth first order
	//PrintTabs indents so that the tree is actualy readable by a human.
	OctreeNode_PrintTabs(num);
	std::cout << "value:    " << value << std::endl;
	OctreeNode_PrintTabs(num);
	std::cout << "children: " << (children != NULL) << std::endl;
	OctreeNode_PrintTabs(num);
	std::cout << "---------------\n";
	
	if(children != NULL) {
		for(int index = 0; index < 8; index ++) {
			children[index]->Print(num + 1);
		}
	}
}

/* Constructors and such:
Some more are defined in the OctreeNode classdef at the bottom of class Octree in Octree.hpp
*/
/* Copy Constructor
Defined because children is dynamicaly allocated.
*/
template <class ValueType>
Octree<ValueType>::OctreeNode::
OctreeNode(const OctreeNode& nodeToCopy) {
	value = nodeToCopy.value;
	if(nodeToCopy.children) {
		int index;
		children = new OctreeNode*[8];
		for(index = 0; index < 8; index++) {
			children[index] = new OctreeNode(*(nodeToCopy.children[index]));
		}
	} else {
		children = NULL;
	}
}

/* copy assignment operator
Defined because children is dynamicaly allocated.
*/
template <class ValueType>
typename Octree<ValueType>::OctreeNode& //typename means Octree<ValueType>::OctreeNode is a type
Octree<ValueType>::OctreeNode::
operator=(OctreeNode rightHandSide) {
	//copy and swap
	this->Swap(rightHandSide);
	return *this;
}

/* Swap
For copy and swap idiom.
*/
template <class ValueType>
void
Octree<ValueType>::OctreeNode::
Swap(OctreeNode& nodeToSwap) {
	std::swap(nodeToSwap.value, value);
	
	OctreeNode** tempPointer = children;
	children = nodeToSwap.children;
	nodeToSwap.children = tempPointer;
}

/* Destructor
Defined because children is dynamicaly allocated.
*/
template <class ValueType>
Octree<ValueType>::OctreeNode::
~OctreeNode() {
	if(children) {
		for(int index = 0; index < 8; index ++) {
			delete children[index];
		}
	}
	delete[] children;
}


template class Octree<bool>::OctreeNode;
template class Octree<PlanarFitNode>::OctreeNode;