// -------------------------------------------------------------------------
// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
// -------------------------------------------------------------------------

#ifndef __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
#define __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__

#include <itkObject.h>

#include <itkObject.h>
#include <itkObjectFactory.h>
#include <itkMatrix.h>
#include <itkPoint.h>


#include <cmath>
#include <itkSymmetricEigenAnalysis.h>


namespace cpExtensions
{
  namespace Algorithms
  {
    /**
     */
    template< class S, unsigned int D >
    class InertiaTensorFunction
      : public itk::Object
    {
    public:
      // Standard itk types
      typedef InertiaTensorFunction           Self;
      typedef itk::Object                     Superclass;
      typedef itk::SmartPointer< Self >       Pointer;
      typedef itk::SmartPointer< const Self > ConstPointer;

      typedef S                           TScalar;
      typedef itk::Matrix< S, D, D >      TMatrix;
      typedef itk::Point< S, D >          TPoint;
      typedef typename TPoint::VectorType TVector;

    protected:
      typedef TMatrix _TInternalMatrix;
      typedef itk::Matrix< S, D, 1 > _TInternalVector;

    public:
      itkNewMacro( Self );
      itkTypeMacro( InertiaTensorFunction, itkObject );

    public:
      inline void Reset( )
        {
          this->m_M = S( 0 );
          this->m_MPP = S( 0 );
          this->m_MP.Fill( S( 0 ) );
          this->m_MPPT.Fill( S( 0 ) );
          this->Modified( );
        }
      inline void AddMass( const TPoint& pnt, const S& mass = S( 1 ) )
        {
          this->AddMass( pnt.GetVectorFromOrigin( ), mass );
        }
      inline void AddMass( const S* data, const S& mass = S( 1 ) )
        {
          this->AddMass( TVector( data ), mass );
        }
      inline void AddMass( const TVector& vec, const S& mass = S( 1 ) )
        {
          this->m_M += mass;
          this->m_MPP += mass * ( vec * vec );

          _TInternalVector mp;
          for( unsigned int d = 0; d < D; ++d )
          {
            this->m_MP[ d ][ 0 ] += mass * vec[ d ];
            mp[ d ][ 0 ] = vec[ d ];

          } // rof
          this->m_MPPT +=
            _TInternalMatrix( mp.GetVnlMatrix( ) * mp.GetTranspose( ) ) *
            mass;
        }

      inline S GetMass( ) const
        {
          return( this->m_M );
        }

      inline TMatrix GetInertia( ) const
        {
          TMatrix I;
          if( S( 0 ) < this->m_M )
          {
            I.SetIdentity( );
            I *= this->m_MPP - ( ( this->m_MP.GetTranspose( ) * this->m_MP.GetVnlMatrix( ) )[ 0 ][ 0 ] / this->m_M );
            I -= this->m_MPPT;
            I += TMatrix( this->m_MP.GetVnlMatrix( ) * this->m_MP.GetTranspose( ) ) / this->m_M;
          }
          else
            I.Fill( S( 0 ) );
          return( I );
        }

      inline TVector GetCenterOfGravity( ) const
        {
          TVector cog;
          if( S( 0 ) < this->m_M )
          {
            for( unsigned int d = 0; d < D; ++d )
              cog[ d ] = this->m_MP[ d ][ 0 ] / this->m_M;
          }
          else
            cog.Fill( S( 0 ) );
          return( cog );
        }

      inline void GetEigenAnalysis( TMatrix& pm, TVector& pv, TVector& r ) const
        {
          TMatrix I = this->GetInertia( );

          itk::SymmetricEigenAnalysis< TMatrix, TVector, TMatrix > eigen;
          eigen.SetDimension( D );
          eigen.SetOrderEigenMagnitudes( true );
          eigen.SetOrderEigenValues( 1 );
          eigen.ComputeEigenValuesAndVectors( I, pv, pm );
          pm = TMatrix( pm.GetTranspose( ) );
          S det = vnl_determinant( pm.GetVnlMatrix( ) );
          for( unsigned int d = 0; d < D; ++d )
            pm[ d ][ D - 1 ] *= det;

          if( D == 2 )
          {
            S coeff = S( 4 ) / this->m_M;
            r[ 0 ] = std::sqrt( std::fabs( coeff * pv[ 1 ] ) );
            r[ 1 ] = std::sqrt( std::fabs( coeff * pv[ 0 ] ) );
          }
          else if( D == 3 )
          {
            S coeff = S( 2.5 ) / this->m_M;
            r[ 0 ] = std::sqrt( std::fabs( coeff * ( pv[ 1 ] + pv[ 2 ] - pv[ 0 ] ) ) );
            r[ 1 ] = std::sqrt( std::fabs( coeff * ( pv[ 0 ] + pv[ 2 ] - pv[ 1 ] ) ) );
            r[ 2 ] = std::sqrt( std::fabs( coeff * ( pv[ 0 ] + pv[ 1 ] - pv[ 2 ] ) ) );
          }
          else
            r.Fill( S( 0 ) );
        }

    protected:
      InertiaTensorFunction( )
        : Superclass( )
        {
          this->Reset( );
        }
      virtual ~InertiaTensorFunction( )
        {
        }

    private:
      // Purposely not implemented.
      InertiaTensorFunction( const Self& );
      void operator=( const Self& );

    protected:
      S m_M;
      S m_MPP;
      _TInternalVector m_MP;
      _TInternalMatrix m_MPPT;
    };

  } // ecapseman

} // ecapseman

#ifndef ITK_MANUAL_INSTANTIATION
// #include <cpExtensions/Algorithms/InertiaTensorFunction.hxx>
#endif // ITK_MANUAL_INSTANTIATION

#endif // __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__

// eof - $RCSfile$