#include "OctreeSupport.hpp"
#include <cmath>

#include <newmat.h>
#include <newmatap.h>

//#include "Octree.hpp"

#include <iostream>
/* Definitions of Vector functions:
For class and project design, look at the top of Octree.hpp
*/

#ifdef use_namespace
using namespace NEWMAT;
#endif


/***************************************************/
double Vector::Norm() const {
	return sqrt(x * x + y * y + z * z);
}
/***************************************************/
Vector& Vector::operator*=(const double scaleFactor) {
	x *= scaleFactor;
	y *= scaleFactor;
	z *= scaleFactor;
	return *this;
}
Vector& Vector::operator/=(const double scaleFactor) {
	x /= scaleFactor;
	y /= scaleFactor;
	z /= scaleFactor;
	return *this;
}
/***************************************************/
Vector& Vector::operator-=(const Vector& V) {
	x -= V.x;
	y -= V.y;
	z -= V.z;
	return *this;
}
/***************************************************/
Vector& Vector::operator+=(const Vector& V) {
	x += V.x;
	y += V.y;
	z += V.z;
	return *this;
}
/***************************************************/
void Vector::Print(void) const {
	std::cout << "X: " << x << "\tY: " << y << "\tZ: " << z << std::endl;
}
/***************************************************/
/***************************************************/
Vector operator+ (Vector U, const Vector& V) {
	U += V;
	return U;
}
Vector operator- (Vector U, const Vector& V) {
	U -= V;
	return U;
}
/***************************************************/
void Path::Print(void) const {
	std::cout << "X: " << x << "\tY: " << y << "\tZ: " << z << std::endl;
}


/* local funciton performs specific task similar to Matlab's [value,index] = min(stuff)
where negative values have been replaced with NaN
*/
int Octree_PickMinPositiveRatio(const double Xratio, const double Yratio, const double Zratio) {
	// positive is to filter out the '-1' cases from having directionVector component == 0
	if(Xratio >= 0) {
		if((Xratio < Yratio) || (Yratio < 0)) {
			if((Xratio < Zratio) || (Zratio < 0)) {
				return 1;//X ratio is the one we want
			}
			return 3;//Z
		}
		if((Yratio < Zratio) || (Zratio < 0)) {
			return 2;//Y
		}
		return 3;//Z
	}//X ratio not valid
	if(Yratio >= 0) {
		if((Yratio < Zratio) || (Zratio < 0)) {
			return 2;//Y
		}
		return 3;//Z
	}//Y ratio not valid
	if(Zratio >= 0) {
		return 3;//Z
	}
	return -1;//error
}


// local funcion performs specific task similar to Matlab's [val,index] = max(stuff)
int Octree_PickMaxRatio(double& Xratio, const double Yratio, const double Zratio) {
	// returns the 'index' of the maximum value, and sets the first input to the corresponding value.
	if(Xratio < Yratio) {
		if(Yratio < Zratio)	{
			Xratio = Zratio;
			return 3;
		}
		Xratio = Yratio;
		return 2;
	}
	if(Xratio < Zratio) {
		Xratio = Zratio;
		return 3;
	}
	return 1;
}

// local functions:
void OctreeNode_PrintTabs(int num) {
	for(int ii = 0; ii < num; ii++) {
		std::cout << "  ";
	}
}


/* The serrious PlanarFitNode constructor
fits a plane  to minimize the cost function 
	J(unitNormal and offset) = sum_i( (unitNormal' * x_i - offset)^2 )
	such that: norm(unitNormal) == 1
	
	x_i = points[indicies[i]] - nodeLowerBounds;
*/
PlanarFitNode::
PlanarFitNode(const Vector * const points , const unsigned long * const indicies , int numPoints, 
			  const Vector& nodeLowerBounds, const Vector& outwardVector){
	
	double A[6] = {0,0,0,0,0,0};//a11 a12 a13 a22 a23 a33
	Vector b;

	Vector temp;
	for(int index = 0; index < numPoints; index++){
		temp = points[indicies[index]] - nodeLowerBounds;
		
		A[0] += temp.x * temp.x;
		A[1] += temp.x * temp.y;
		A[2] += temp.x * temp.z;
		A[3] += temp.y * temp.y;
		A[4] += temp.y * temp.z;
		A[5] += temp.z * temp.z;
		
		b += temp;
	}

	SymmetricMatrix C (3);
	C(1,1) = b.x * b.x / numPoints - A[0];
	C(1,2) = b.x * b.y / numPoints - A[1];
	C(1,3) = b.x * b.z / numPoints - A[2];
	C(2,2) = b.y * b.y / numPoints - A[3];
	C(2,3) = b.y * b.z / numPoints - A[4];
	C(3,3) = b.z * b.z / numPoints - A[5];

	DiagonalMatrix D ;
	Matrix Theta;
	
	/* I have proved that the optimal unit normal (THeta) is the eigenvector assiociated 
	with the least negative (last) eigenvalue of this C matrix.  The optimal offset is 
	just the average of the points dotted with the unitNormal. */
	Jacobi(C,D,Theta);

	unitNormal.SetValues(Theta(1,3), Theta(2,3), Theta(3,3));
	if(unitNormal.Dot(outwardVector) >= 0){
		unitNormal /= unitNormal.Norm();
	}else{
		unitNormal /= -unitNormal.Norm();
	}
	
	offset = b.Dot(unitNormal)/numPoints;
}



//for printing PlanarFitNodes
ostream& operator<< (ostream& out, const PlanarFitNode& value){
	out << "normal: <" << value.unitNormal.x << ", " << value.unitNormal.y << ", " << value.unitNormal.z << ">  offset: " <<  value.offset;
	return out;
}