#include "principal_axes.h"
#include "matrix_util.h"
#include "../geometry/geometry_global.h"

//_________________________________________________________

PrincipalAxes3d::PrincipalAxes3d() {

}

void PrincipalAxes3d::begin_points() {
	nb_points_ = 0 ;
	sum_weights_ = 0 ;
	center_[0] = center_[1] = center_[2] = 0 ;
	M_[0] = M_[1] = M_[2] = M_[3] = M_[4] = M_[5] = 0 ;
}

void PrincipalAxes3d::end_points() {
	center_[0] /= sum_weights_ ;
	center_[1] /= sum_weights_ ;
	center_[2] /= sum_weights_ ;

	// If the system is under-determined, 
	//   return the trivial basis.
	if(nb_points_ < 4) {
		axis_[0] = Vector3f(1.0f, 0.0f, 0.0f) ;
		axis_[1] = Vector3f(0.0f, 1.0f, 0.0f) ;
		axis_[2] = Vector3f(0.0f, 0.0f, 1.0f) ;
		eigen_value_[0] = 1.0f ;
		eigen_value_[1] = 1.0f ;
		eigen_value_[2] = 1.0f ;
	} else {
		double x = center_[0] ;
		double y = center_[1] ;
		double z = center_[2] ;

		M_[0] = M_[0]/sum_weights_ - x*x ;
		M_[1] = M_[1]/sum_weights_ - x*y ;
		M_[2] = M_[2]/sum_weights_ - y*y ;
		M_[3] = M_[3]/sum_weights_ - x*z ;
		M_[4] = M_[4]/sum_weights_ - y*z ;
		M_[5] = M_[5]/sum_weights_ - z*z ;

		if( M_[0] <= 0 ) {
			M_[0] = 1.e-30f ; 
		}
		if( M_[2] <= 0 ) {
			M_[2] = 1.e-30f ; 
		}
		if( M_[5] <= 0 ) {
			M_[5] = 1.e-30f ; 
		}

		double eigen_vectors[9] ;
		eigen_symmetric(M_, 3, eigen_vectors, eigen_value_) ;

		axis_[0] = Vector3f(eigen_vectors[0], eigen_vectors[1], eigen_vectors[2]) ;
		axis_[1] = Vector3f(eigen_vectors[3], eigen_vectors[4], eigen_vectors[5]) ;
		axis_[2] = Vector3f(eigen_vectors[6], eigen_vectors[7], eigen_vectors[8]) ;

		// Normalize the eigen vectors
		for(int i=0; i<3; i++) {
			Global::normalize(axis_[i]);
		}
	}

}

void PrincipalAxes3d::add_point(const Point3f& p, double weight) {
	double x = p.x() ;
	double y = p.y() ; 
	double z = p.z() ;

	center_[0] += x * weight ;
	center_[1] += y * weight ;
	center_[2] += z * weight ;

	M_[0] += weight * x*x ;
	M_[1] += weight * x*y ;
	M_[2] += weight * y*y ;
	M_[3] += weight * x*z ;
	M_[4] += weight * y*z ;
	M_[5] += weight * z*z ;

	nb_points_++ ;
	sum_weights_ += weight ;
}

//_________________________________________________________

PrincipalAxes2d::PrincipalAxes2d() {

}

void PrincipalAxes2d::begin_points() {
	nb_points_ = 0 ;
	sum_weights_ = 0 ;
	center_[0] = center_[1] = 0 ;
	M_[0] = M_[1] = M_[2] = 0 ;
}

void PrincipalAxes2d::end_points() {

	center_[0] /= sum_weights_ ;
	center_[1] /= sum_weights_ ;

	// If the system is under-determined, 
	//  return the trivial basis.
	if(nb_points_ < 3) {
		axis_[0] = Vector2f(1.0f, 0.0f) ;
		axis_[1] = Vector2f(0.0f, 1.0f) ;
		eigen_value_[0] = 1.0f ;
		eigen_value_[1] = 1.0f ;
	} else {
		double x = center_[0] ;
		double y = center_[1] ;

		M_[0] = M_[0]/sum_weights_ - x*x ;
		M_[1] = M_[1]/sum_weights_ - x*y ;
		M_[2] = M_[2]/sum_weights_ - y*y ;

		if( M_[0] <= 0 ) {
			M_[0] = 1.e-30f ; 
		}

		if( M_[2] <= 0 ) {
			M_[2] = 1.e-30f ; 
		}

		double eigen_vectors[4] ;
		eigen_symmetric(M_, 2, eigen_vectors, eigen_value_) ;

		axis_[0] = Vector2f(eigen_vectors[0], eigen_vectors[1]) ;
		axis_[1] = Vector2f(eigen_vectors[2], eigen_vectors[3]) ;

		// Normalize the eigen vectors

		for(int i=0; i<2; i++) {
			Global::normalize(axis_[i]) ;
		}
	}

}

void PrincipalAxes2d::add_point(const Point2f& p, double weight)
{
	double x = p.x() ;
	double y = p.y() ; 

	center_[0] += x * weight ;
	center_[1] += y * weight ;

	M_[0] += weight * x*x ;
	M_[1] += weight * x*y ;
	M_[2] += weight * y*y ;

	nb_points_++ ;
	sum_weights_ += weight ;
}

