#ifndef __CPM__ALGORITHMS__SIMPLEX__INTERNALFORCEFUNCTION__HXX__
#define __CPM__ALGORITHMS__SIMPLEX__INTERNALFORCEFUNCTION__HXX__

#include <cmath>
#include <vnl/vnl_math.h>

// -------------------------------------------------------------------------
template< class M >
typename cpm::Algorithms::Simplex::InternalForceFunction< M >::
TVector cpm::Algorithms::Simplex::InternalForceFunction< M >::
Evaluate( const TPointId& pId ) const
{
  typedef typename M::BarycenterVector TBarycenter;

  static const TScalar _1_3 = TScalar( 1 ) / TScalar( 3 );

  TVector f( TScalar( 0 ) );
  if( this->m_Mesh.IsNotNull( ) )
  {
    if( pId < this->m_Mesh->GetNumberOfPoints( ) )
    {
      TVector A, B, C, N;
      TBarycenter b;
      TScalar r, d, dp, phi;
      if(
        this->m_Mesh->
        ComputeSimplexParameters( pId, A, B, C, N, b, r, d, dp, phi )
        )
      {
        // Parallel force
        f  = A * ( _1_3 - b[ 0 ] );
        f += B * ( _1_3 - b[ 1 ] );
        f += C * ( _1_3 - b[ 2 ] );

        // Desired phi
        TScalar dphi;
        switch( this->m_PhiConstraint )
        {
        case Self::C1:
          dphi = TScalar( 0 );
          break;
        case Self::C2:
          dphi = this->_ComputeMeanPhi( pId );
          break;
        case Self::C3:
          dphi = this->m_InitialPhiAngles[ pId ];
          break;
        case Self::C0:
        default:
          dphi = phi;
          break;
        } // hctiws

        // Regularizing force
        f += N * ( Self::_L( r, dp, dphi ) - Self::_L( r, d, phi ) );

      } // fi

    } // fi

  } // fi
  return( f );
}

// -------------------------------------------------------------------------
template< class M >
void cpm::Algorithms::Simplex::InternalForceFunction< M >::
SetMesh( const M* m )
{
  typedef typename M::BarycenterVector TBarycenter;

  if( this->m_Mesh != m && m != NULL )
  {
    this->m_InitialPhiAngles.clear( );
    for( TPointId i = 0; i < m->GetNumberOfPoints( ); ++i )
    {
      TVector A, B, C, N;
      TBarycenter e;
      TScalar a, b, c, phi = TScalar( 0 );
      m->ComputeSimplexParameters( i, A, B, C, N, e, a, b, c, phi );
      this->m_InitialPhiAngles.push_back( phi );

    } // rof

  } // fi
  this->Superclass::SetMesh( m );
}

// -------------------------------------------------------------------------
template< class M >
void cpm::Algorithms::Simplex::InternalForceFunction< M >::
SetPhiConstraintToC0( )
{
  if( this->m_PhiConstraint != Self::C0 )
  {
    this->m_PhiConstraint = Self::C0;
    this->Modified( );

  } // fi
}

// -------------------------------------------------------------------------
template< class M >
void cpm::Algorithms::Simplex::InternalForceFunction< M >::
SetPhiConstraintToC1( )
{
  if( this->m_PhiConstraint != Self::C1 )
  {
    this->m_PhiConstraint = Self::C1;
    this->Modified( );

  } // fi
}

// -------------------------------------------------------------------------
template< class M >
void cpm::Algorithms::Simplex::InternalForceFunction< M >::
SetPhiConstraintToC2( const unsigned int& neigh_order )
{
  if( this->m_PhiConstraint != Self::C2 )
  {
    this->m_PhiConstraint = Self::C2;
    this->m_NeighborhoodOrder = neigh_order;
    this->Modified( );

  } // fi
}

// -------------------------------------------------------------------------
template< class M >
void cpm::Algorithms::Simplex::InternalForceFunction< M >::
SetPhiConstraintToC3( )
{
  if( this->m_PhiConstraint != Self::C3 )
  {
    this->m_PhiConstraint = Self::C3;
    this->Modified( );

  } // fi
}

// -------------------------------------------------------------------------
template< class M >
cpm::Algorithms::Simplex::InternalForceFunction< M >::
InternalForceFunction( )
  : Superclass( ),
    m_PhiConstraint( Self::C0 ),
    m_NeighborhoodOrder( 1 )
{
}

// -------------------------------------------------------------------------
template< class M >
cpm::Algorithms::Simplex::InternalForceFunction< M >::
~InternalForceFunction( )
{
}

// -------------------------------------------------------------------------
template< class M >
typename cpm::Algorithms::Simplex::InternalForceFunction< M >::
TScalar cpm::Algorithms::Simplex::InternalForceFunction< M >::
_ComputeMeanPhi( const TPointId& pId ) const
{
  // TODO: use this->m_NeighborhoodOrder
  return( TScalar( 0 ) );
}

// -------------------------------------------------------------------------
template< class M >
typename cpm::Algorithms::Simplex::InternalForceFunction< M >::
TScalar cpm::Algorithms::Simplex::InternalForceFunction< M >::
_L( const TScalar& delta, const TScalar& d, const TScalar& phi )
{
  static const double _pi2 = double( vnl_math::pi ) / double( 2 );

  TScalar eps = TScalar( 1 );
  if( _pi2 < std::fabs( double( phi ) ) )
    eps = TScalar( -1 );

  TScalar sphi = TScalar( std::sin( double( phi ) ) );
  TScalar cphi = TScalar( std::fabs( std::cos( double( phi ) ) ) );

  TScalar den = delta * cphi;
  TScalar s = ( delta + ( d * sphi ) ) * ( delta - ( d * sphi ) );
  if( s < TScalar( 0 ) )
    return( TScalar( 0 ) );

  den += eps * TScalar( std::sqrt( double( s ) ) );
  if( !( TScalar( 0 ) < den ) )
    return( TScalar( 0 ) );

  TScalar num = eps * ( delta + d ) * ( delta - d ) * sphi;
  return( num / den );
}

#endif // __CPM__ALGORITHMS__SIMPLEX__INTERNALFORCEFUNCTION__HXX__

// eof - $RCSfile$