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

#ifndef __CPEXTENSIONS__ALGORITHMS__BEZIERCURVEFUNCTION__HXX__
#define __CPEXTENSIONS__ALGORITHMS__BEZIERCURVEFUNCTION__HXX__

// -------------------------------------------------------------------------
template< class V >
void cpExtensions::Algorithms::BezierCurveFunction< V >::
AddPoint( const TVector& v )
{
  this->m_Vectors.push_back( v );
  this->m_DerivativeUpdated = false;
  this->Modified( );
}

// -------------------------------------------------------------------------
template< class V >
unsigned int cpExtensions::Algorithms::BezierCurveFunction< V >::
GetNumberOfPoints( ) const
{
  return( this->m_Vectors.size( ) );
}

// -------------------------------------------------------------------------
template< class V >
typename cpExtensions::Algorithms::BezierCurveFunction< V >::
TVector cpExtensions::Algorithms::BezierCurveFunction< V >::
Evaluate( const TScalar& u ) const
{
  TVectorsContainer Q = this->m_Vectors;
  unsigned int n = Q.size( );
  TScalar _1u = TScalar( 1 ) - u;

  for( unsigned int k = 1; k < n; k++ )
  {
    // CM Fixed a bug appearing under Windows : changed the stopping
    // condition from <= to <. Otherwise, on the last step, an element out
    // of the range of vector Q is accessed (Q[ i + 1 ])...
    for( unsigned int i = 0; i < n - k; i++ )
      Q[ i ] = ( Q[ i ] * _1u ) + ( Q[ i + 1 ] * u );

  } // rof
  return( Q[ 0 ] );
}

// -------------------------------------------------------------------------
template< class V >
typename cpExtensions::Algorithms::BezierCurveFunction< V >::
TFrame cpExtensions::Algorithms::BezierCurveFunction< V >::
EvaluateFrenetFrame( const TScalar& u ) const
{
  TFrame fr;
  fr.Fill( TScalar( 0 ) );
  if( TVector::Dimension == 2 )
  {
    this->_UpdateDerivative( );
    this->m_Derivative->_UpdateDerivative( );

    TVector vT = this->m_Derivative->Evaluate( u );
    TScalar nvT = vT.GetNorm( );
    if( TScalar( 0 ) < nvT )
      vT /= nvT;

    fr[ 0 ][ 0 ] = vT[ 0 ];
    fr[ 1 ][ 0 ] = vT[ 1 ];

    fr[ 0 ][ 1 ] = -vT[ 1 ];
    fr[ 1 ][ 1 ] =  vT[ 0 ];
  }
  else if( TVector::Dimension == 3 )
  {
    this->_UpdateDerivative( );
    this->m_Derivative->_UpdateDerivative( );
    TVector vT = this->m_Derivative->Evaluate( u );
    TScalar nvT = vT.GetNorm( );
    if( nvT > TScalar( 0 ) )
    {
      vT /= nvT;
      TVector vN = this->m_Derivative->m_Derivative->Evaluate( u );
      TScalar nvN = vN.GetNorm( );
      if( nvT > TScalar( 0 ) )
      {
        vN /= nvN;
        TVector vB;
        vB[ 0 ] = ( vT[ 1 ] * vN[ 2 ] ) - ( vT[ 2 ] * vN[ 1 ] );
        vB[ 1 ] = ( vT[ 2 ] * vN[ 0 ] ) - ( vT[ 0 ] * vN[ 2 ] );
        vB[ 2 ] = ( vT[ 0 ] * vN[ 1 ] ) - ( vT[ 1 ] * vN[ 0 ] );

        for( unsigned int d = 0; d < 3; d++ )
        {
          fr[ d ][ 0 ] = vT[ d ];
          fr[ d ][ 1 ] = vN[ d ];
          fr[ d ][ 2 ] = vB[ d ];

        } // rof
      }
      else
        std::cerr << "ERROR normal" << std::endl;
    }
    else
      std::cerr << "ERROR tangent" << std::endl;

  } // fi
  return( fr );
}

// -------------------------------------------------------------------------
template< class V >
typename cpExtensions::Algorithms::BezierCurveFunction< V >::
TScalar cpExtensions::Algorithms::BezierCurveFunction< V >::
EvaluateLength( ) const
{
  unsigned int n = this->GetNumberOfPoints( ) << 1;
  TScalar d = TScalar( 0 );
  TVector v0 = this->Evaluate( 0 );
  for( unsigned int i = 1; i < n; i++ )
  {
    TVector v1 = this->Evaluate( TScalar( i ) / TScalar( n - 1 ) );
    d += ( v1 - v0 ).GetNorm( );
    v0 = v1;

  } // rof
  return( d );
}

// -------------------------------------------------------------------------
template< class V >
cpExtensions::Algorithms::BezierCurveFunction< V >::
BezierCurveFunction( )
  : Superclass( ),
    m_DerivativeUpdated( false )
{
}

// -------------------------------------------------------------------------
template< class V >
void cpExtensions::Algorithms::BezierCurveFunction< V >::
_UpdateDerivative( ) const
{
  if( this->m_DerivativeUpdated )
    return;

  this->m_Derivative = Self::New( );
  unsigned int n = this->m_Vectors.size( ) - 1;
  for( unsigned int i = 0; i < n; i++ )
    this->m_Derivative->AddPoint(
      TScalar( n ) * ( this->m_Vectors[ i + 1 ] - this->m_Vectors[ i ] )
      );
}

#endif // __CPEXTENSIONS__ALGORITHMS__BEZIERCURVEFUNCTION__HXX__

// eof - $RCSfile$