/*
 *  Copyright (c) 2000-2022 Inria
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *  this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *  this list of conditions and the following disclaimer in the documentation
 *  and/or other materials provided with the distribution.
 *  * Neither the name of the ALICE Project-Team nor the names of its
 *  contributors may be used to endorse or promote products derived from this
 *  software without specific prior written permission.
 * 
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Contact: Bruno Levy
 *
 *     https://www.inria.fr/fr/bruno-levy
 *
 *     Inria,
 *     Domaine de Voluceau,
 *     78150 Le Chesnay - Rocquencourt
 *     FRANCE
 *
 */
#include <geogram/points/principal_axes.h>

namespace GEO {

    PrincipalAxes3d::PrincipalAxes3d() {
    }

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

    void PrincipalAxes3d::end() {
	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] = vec3(1,0,0) ;
	    axis_[1] = vec3(0,1,0) ;
	    axis_[2] = vec3(0,0,1) ;
	    eigen_value_[0] = 1.0 ;
	    eigen_value_[1] = 1.0 ;
	    eigen_value_[2] = 1.0 ;
	} 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-30 ; 
	    }
	    if( M_[2] <= 0 ) {
		M_[2] = 1.e-30 ; 
	    }
	    if( M_[5] <= 0 ) {
		M_[5] = 1.e-30 ; 
	    }
		
	    double eigen_vectors[9] ;
	    MatrixUtil::semi_definite_symmetric_eigen(
		M_, 3, eigen_vectors, eigen_value_
	    ) ;
		
	    axis_[0] = vec3(
		eigen_vectors[0], eigen_vectors[1], eigen_vectors[2]
	    );
		
	    axis_[1] = vec3(
		eigen_vectors[3], eigen_vectors[4], eigen_vectors[5]
	    );
        
	    axis_[2] = vec3(
		eigen_vectors[6], eigen_vectors[7], eigen_vectors[8]
	    );
        
	    // Normalize the eigen vectors
		
	    for(int i=0; i<3; i++) {
		axis_[i] = normalize(axis_[i]) ;
	    }
	}
    }

    void PrincipalAxes3d::add_point(const vec3& p, double weight) {
	center_[0] += p.x * weight ;
	center_[1] += p.y * weight ;
	center_[2] += p.z * weight ;
	    
	double x = p.x ;
	double y = p.y ; 
	double z = p.z ;
	    
	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 ;
    }
    
    
}




