//----------------------------------------------------------------------------
//  EgalTech 2014-2014
//----------------------------------------------------------------------------
// File : PointsPCA.cpp          Data : 12.08.14          Versione : 1.5h3
// Contenuto : Implementazione della classe PointsPCA,
//             Principal Components Analysis di un insieme di punti.
//
//
// Modifiche : 12.08.14 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "PointsPCA.h"


//----------------------------------------------------------------------------
PointsPCA::PointsPCA( void)
{
  // azzero numero punti
   m_dTotW = 0 ;
  // azzero il baricentro
   m_ptCen.Set( 0, 0, 0) ;
  // azzero matrice di covarianza
   m_CovMat.setZero() ;
  // inizializzo il rank ad un valore assurdo
   m_nRank = - 1 ;
}

//----------------------------------------------------------------------------
void
PointsPCA::AddPoint( const Point3d& ptP, double dW)
{
  // incremento numero punti
   m_dTotW += dW ;
  // aggiorno il baricentro
   m_ptCen += dW * ptP ;
  // aggiorno la matrice di covarianza (solo triangolo superiore perchè simmetrica)
   m_CovMat(0,0) += dW * ( ptP.x * ptP.x) ;
   m_CovMat(1,1) += dW * ( ptP.y * ptP.y) ;
   m_CovMat(2,2) += dW * ( ptP.z * ptP.z) ;
   m_CovMat(0,1) += dW * ( ptP.x * ptP.y) ;
   m_CovMat(0,2) += dW * ( ptP.x * ptP.z) ;
   m_CovMat(1,2) += dW * ( ptP.y * ptP.z) ;
}

//----------------------------------------------------------------------------
bool
PointsPCA::Finalize( void)
{
  // se già eseguito il calcolo finale, esco
   if ( m_nRank >= 0)
      return true ;

  // il rank viene annullato ( non sono ancora note direzioni principali)
   m_nRank = 0 ;

  // se non sono stati assegnati punti, esco
   if ( m_dTotW < EPS_ZERO)
      return false ;

  // fattore di scala per numero di punti
   double dScale = 1 / m_dTotW ;

  // calcolo del baricentro
   m_ptCen *= dScale ;

  // completo la matrice di covarianza
   m_CovMat(0,0) = m_CovMat(0,0) * dScale - m_ptCen.x * m_ptCen.x ;
   m_CovMat(1,1) = m_CovMat(1,1) * dScale - m_ptCen.y * m_ptCen.y ;
   m_CovMat(2,2) = m_CovMat(2,2) * dScale - m_ptCen.z * m_ptCen.z ;
   m_CovMat(0,1) = m_CovMat(0,1) * dScale - m_ptCen.x * m_ptCen.y ;
   m_CovMat(0,2) = m_CovMat(0,2) * dScale - m_ptCen.x * m_ptCen.z ;
   m_CovMat(1,2) = m_CovMat(1,2) * dScale - m_ptCen.y * m_ptCen.z ;
   m_CovMat(1,0) = m_CovMat(0,1) ;
   m_CovMat(2,0) = m_CovMat(0,2) ;
   m_CovMat(2,1) = m_CovMat(1,2) ;

  // calcolo gli autovalori e autovettori (essendo matrice 3x3 uso metodo diretto)
   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es ;
   es.compute( m_CovMat) ;        // non usare computeDirect : errore nell'ordine degli autovettori
   if ( es.info() == Eigen::NoConvergence)
      return false ;

  // recupero gli autovettori corrispondenti agli autovalori non nulli
   for ( int i = 0 ; i < 3 ; ++ i) {
      int j = ( i + 1) % 3 ;
      int k = ( i + 2) % 3 ;
     // se l'autovalore corrente è il più grande in modulo
      if ( abs( es.eigenvalues()(i)) >= abs( es.eigenvalues()(j))  &&
           abs( es.eigenvalues()(i)) >= abs( es.eigenvalues()(k))  &&
           abs( es.eigenvalues()(i)) > SQ_EPS_SMALL) {
        // il suo autovettore è il primo
         ++ m_nRank ;
         m_vtPC[0].Set( es.eigenvectors()(0,i), es.eigenvectors()(1,i), es.eigenvectors()(2,i)) ;
        // se il successivo autovalore è maggiore del rimanente in modulo
         if ( abs( es.eigenvalues()(j)) >= abs(es.eigenvalues()(k))  &&
              abs( es.eigenvalues()(j)) > SQ_EPS_SMALL) {
           // il suo autovettore è il secondo
            ++ m_nRank ;
            m_vtPC[1].Set( es.eigenvectors()(0,j), es.eigenvectors()(1,j), es.eigenvectors()(2,j)) ;
           // se il rimanente autovalore è non nullo
            if ( abs( es.eigenvalues()(k)) > SQ_EPS_SMALL) {
              // il suo autovettore è il terzo
               ++ m_nRank ;
               m_vtPC[2].Set( es.eigenvectors()(0,k), es.eigenvectors()(1,k), es.eigenvectors()(2,k)) ;
            }
         }
        // altrimenti, se il rimanente autovalore è maggiore del successivo in modulo
         else if ( abs( es.eigenvalues()(k)) >= abs( es.eigenvalues()(j))  &&
                   abs( es.eigenvalues()(k)) > SQ_EPS_SMALL) {
           // il suo autovettore è il secondo
            ++ m_nRank ;
            m_vtPC[1].Set( es.eigenvectors()(0,k), es.eigenvectors()(1,k), es.eigenvectors()(2,k)) ;
           // se il rimanente autovalore è non nullo
            if ( abs( es.eigenvalues()(j)) > SQ_EPS_SMALL) {
              // il suo autovettore è il terzo
               ++ m_nRank ;
               m_vtPC[2].Set( es.eigenvectors()(0,j), es.eigenvectors()(1,j), es.eigenvectors()(2,j)) ;
            }
         }
         break ;
      }
   }

   return true ;
}

//----------------------------------------------------------------------------
int
PointsPCA::GetRank( void)
{
  // verifico completamento conti
   Finalize() ;

   return m_nRank ;
}

//----------------------------------------------------------------------------
bool
PointsPCA::GetCenter( Point3d& ptCen)
{
  // verifico completamento conti
   Finalize() ;
  // assegno i dati
   ptCen = m_ptCen ;
   return ( m_dTotW > EPS_ZERO) ;
}

//----------------------------------------------------------------------------
bool
PointsPCA::GetPrincipalComponent( int nId, Vector3d& vtPC)
{
  // verifico completamento conti
   Finalize() ;
  // verifico esistenza componente (indice 0-based)
   if ( nId < 0  ||  nId >= m_nRank)
      return false ;
  // assegno dati
   vtPC = m_vtPC[nId] ;
   return true ;
}