/*
 *  GXML/Graphite: Geometry and Graphics Programming Library + Utilities
 *  Copyright (C) 2000 Bruno Levy
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *  If you modify this software, you should include a notice giving the
 *  name of the person performing the modification, the date of modification,
 *  and the reason for such modification.
 *
 *  Contact: Bruno Levy
 *
 *     levy@loria.fr
 *
 *     ISA Project
 *     LORIA, INRIA Lorraine, 
 *     Campus Scientifique, BP 239
 *     54506 VANDOEUVRE LES NANCY CEDEX 
 *     FRANCE
 *
 *  Note that the GNU General Public License does not permit incorporating
 *  the Software into proprietary programs. 
 */


#include "matrix_util.h"
#include <math.h>



namespace OGF {
    
    namespace MatrixUtil {

        static const double EPS = 0.00001 ;
        static int MAX_ITER = 100 ;
        
        void semi_definite_symmetric_eigen(
            const double *mat, int n, double *eigen_vec, double *eigen_val
        ) {
            double *a,*v;
            double a_norm,a_normEPS,thr,thr_nn;
            int nb_iter = 0;
            int jj;
            int i,j,k,ij,ik,l,m,lm,mq,lq,ll,mm,imv,im,iq,ilv,il,nn;
            int *index;
            double a_ij,a_lm,a_ll,a_mm,a_im,a_il;
            double a_lm_2;
            double v_ilv,v_imv;
            double x;
            double sinx,sinx_2,cosx,cosx_2,sincos;
            double delta;
        
            // Number of entries in mat
        
            nn = (n*(n+1))/2;
        
            // Step 1: Copy mat to a
        
            a = new double[nn];
        
            for( ij=0; ij<nn; ij++ ) {
                a[ij] = mat[ij];
            }
        
            // Ugly Fortran-porting trick: indices for a are between 1 and n
            a--;
        
            // Step 2 : Init diagonalization matrix as the unit matrix
            v = new double[n*n];
        
            ij = 0;
            for( i=0; i<n; i++ ) {
                for( j=0; j<n; j++ ) {
                    if( i==j ) {
                        v[ij++] = 1.0;
                    } else {
                        v[ij++] = 0.0;
                    }
                }
            }
        
            // Ugly Fortran-porting trick: indices for v are between 1 and n
            v--;
        
            // Step 3 : compute the weight of the non diagonal terms 
            ij     = 1  ;
            a_norm = 0.0;
            for( i=1; i<=n; i++ ) {
                for( j=1; j<=i; j++ ) {
                    if( i!=j ) {
                        a_ij    = a[ij];
                        a_norm += a_ij*a_ij;
                    }
                    ij++;
                }
            }
        
            if( a_norm != 0.0 ) {
        
                a_normEPS = a_norm*EPS;
                thr       = a_norm    ;
    
                // Step 4 : rotations
                while( thr > a_normEPS   &&   nb_iter < MAX_ITER ) {
        
                    nb_iter++;
                    thr_nn = thr / nn;
            
                    for( l=1  ; l< n; l++ ) {
                        for( m=l+1; m<=n; m++ ) {
                    
                            // compute sinx and cosx 
                    
                            lq   = (l*l-l)/2;
                            mq   = (m*m-m)/2;
                    
                            lm     = l+mq;
                            a_lm   = a[lm];
                            a_lm_2 = a_lm*a_lm;
                    
                            if( a_lm_2 < thr_nn ) {
                                continue ;
                            }
                    
                            ll   = l+lq;
                            mm   = m+mq;
                            a_ll = a[ll];
                            a_mm = a[mm];
                    
                            delta = a_ll - a_mm;
                    
                            if( delta == 0.0 ) {
                                x = - M_PI/4 ; 
                            } else {
                                x = - atan( (a_lm+a_lm) / delta ) / 2.0 ;
                            }

                            sinx    = sin(x)   ;
                            cosx    = cos(x)   ;
                            sinx_2  = sinx*sinx;
                            cosx_2  = cosx*cosx;
                            sincos  = sinx*cosx;
                    
                            // rotate L and M columns 
       
                            ilv = n*(l-1);
                            imv = n*(m-1);
                    
                            for( i=1; i<=n;i++ ) {
                                if( (i!=l) && (i!=m) ) {
                                    iq = (i*i-i)/2;
                            
                                    if( i<m )  { 
                                        im = i + mq; 
                                    } else {
                                        im = m + iq;
                                    }
                                    a_im = a[im];
                            
                                    if( i<l ) {
                                        il = i + lq; 
                                    } else {
                                        il = l + iq;
                                    }
                                    a_il = a[il];
                            
                                    a[il] =  a_il*cosx - a_im*sinx;
                                    a[im] =  a_il*sinx + a_im*cosx;
                                }
                        
                                ilv++;
                                imv++;
                        
                                v_ilv = v[ilv];
                                v_imv = v[imv];
                        
                                v[ilv] = cosx*v_ilv - sinx*v_imv;
                                v[imv] = sinx*v_ilv + cosx*v_imv;
                            } 
                    
                            x = a_lm*sincos; x+=x;
                    
                            a[ll] =  a_ll*cosx_2 + a_mm*sinx_2 - x;
                            a[mm] =  a_ll*sinx_2 + a_mm*cosx_2 + x;
                            a[lm] =  0.0;
                    
                            thr = fabs( thr - a_lm_2 );
                        }
                    }
                }         
            }
        
            // Step 5: index conversion and copy eigen values 
        
            // back from Fortran to C++
            a++;
        
            for( i=0; i<n; i++ ) {
                k = i + (i*(i+1))/2;
                eigen_val[i] = a[k];
            }
     
            delete[] a;
        
            // Step 6: sort the eigen values and eigen vectors 
        
            index = new int[n];
            for( i=0; i<n; i++ ) {
                index[i] = i;
            }
        
            for( i=0; i<(n-1); i++ ) {
                x = eigen_val[i];
                k = i;
            
                for( j=i+1; j<n; j++ ) {
                    if( x < eigen_val[j] ) {
                        k = j;
                        x = eigen_val[j];
                    }
                }
            
                eigen_val[k] = eigen_val[i];
                eigen_val[i] = x;
          
                jj       = index[k];
                index[k] = index[i];
                index[i] = jj;
            }


            // Step 7: save the eigen vectors 
    
            v++; // back from Fortran to to C++
        
            ij = 0;
            for( k=0; k<n; k++ ) {
                ik = index[k]*n;
                for( i=0; i<n; i++ ) {
                    eigen_vec[ij++] = v[ik++];
                }
            }
    
            delete[] v    ;
            delete[] index;
            return;
        }
    
//_________________________________________________________

    }
}