//----------------------------------------------------------------------------
//  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"
#define EIGEN_NO_IO
#include "/EgtDev/Extern/Eigen/Dense"


//----------------------------------------------------------------------------
PointsPCA::PointsPCA( void)
{
  // inizializzo il rank ad un valore assurdo
   m_nRank = - 1 ;
  // inizializzo baricentro
   m_ptCen.Set( 0, 0, 0) ;
  // inizializzo il peso totale
   m_dTotW = 0 ;
  // riservo memoria per la matrice di punti e pesi
   m_vPntW.reserve( 128) ;
}

//----------------------------------------------------------------------------
void
PointsPCA::AddPoint( const Point3d& ptP, double dW)
{
  // salvo i dati
   m_vPntW.emplace_back( ptP, dW) ;
}

//----------------------------------------------------------------------------
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_vPntW.empty())
      return false ;

  // calcolo del peso totale
   for ( const auto& PntW : m_vPntW)
      m_dTotW += PntW.second ;
   if ( m_dTotW < EPS_ZERO)
      return false ;
  // fattore di scala
   double dScale = 1 / m_dTotW ;

  // calcolo del baricentro
   for ( const auto& PntW : m_vPntW)
      m_ptCen += PntW.second * PntW.first ;
   m_ptCen *= dScale ;

  // matrice di covarianza
   Eigen::Matrix3d CovMat ;
   CovMat.setZero() ;
   for ( const auto& PntW : m_vPntW) {
      Point3d ptP( PntW.first.x - m_ptCen.x, PntW.first.y - m_ptCen.y, PntW.first.z - m_ptCen.z) ;
      CovMat(0,0) += PntW.second * ( ptP.x * ptP.x) ;
      CovMat(1,1) += PntW.second * ( ptP.y * ptP.y) ;
      CovMat(2,2) += PntW.second * ( ptP.z * ptP.z) ;
      CovMat(0,1) += PntW.second * ( ptP.x * ptP.y) ;
      CovMat(0,2) += PntW.second * ( ptP.x * ptP.z) ;
      CovMat(1,2) += PntW.second * ( ptP.y * ptP.z) ;
   }
   CovMat(0,0) = CovMat(0,0) * dScale ;
   CovMat(1,1) = CovMat(1,1) * dScale ;
   CovMat(2,2) = CovMat(2,2) * dScale ;
   CovMat(0,1) = CovMat(0,1) * dScale ;
   CovMat(0,2) = CovMat(0,2) * dScale ;
   CovMat(1,2) = CovMat(1,2) * dScale ;
   CovMat(1,0) = CovMat(0,1) ;
   CovMat(2,0) = CovMat(0,2) ;
   CovMat(2,1) = CovMat(1,2) ;

  // calcolo gli autovalori e autovettori
   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es ;
   es.compute( 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 ;
}