#include <cpExtensions/DataStructures/Simple3DCurve.h>

// -------------------------------------------------------------------------
template< class _TScalar >
void cpExtensions::DataStructures::Simple3DCurve< _TScalar >::
Modified( ) const
{
  this->Superclass::Modified( );
  this->m_FramesUpdated = false;
}

// -------------------------------------------------------------------------
template< class _TScalar >
void cpExtensions::DataStructures::Simple3DCurve< _TScalar >::
Clear( )
{
  this->m_Points.clear( );
  this->m_Frames.clear( );
  this->Modified( );
}

// -------------------------------------------------------------------------
template< class _TScalar >
unsigned long cpExtensions::DataStructures::Simple3DCurve< _TScalar >::
GetNumberOfPoints( ) const
{
  return( this->m_Points.size( ) );
}

// -------------------------------------------------------------------------
template< class _TScalar >
const typename cpExtensions::DataStructures::Simple3DCurve< _TScalar >::
TMatrix& cpExtensions::DataStructures::Simple3DCurve< _TScalar >::
GetFrame( unsigned int id ) const
{
  if( !( this->m_FramesUpdated ) )
  {
    unsigned long N = this->m_Points.size( );

    std::vector< TVector > tg( N );
    tg[ 0 ] = this->m_Points[ 1 ] - this->m_Points[ 0 ];
    tg[ N - 1 ] = this->m_Points[ N - 1 ] - this->m_Points[ N - 2 ];
    for( unsigned int i = 1; i < N - 1; ++i )
      tg[ i ] = this->m_Points[ i + 1 ] - this->m_Points[ i - 1 ];

    std::vector< TVector > no( N );
    std::vector< TVector > bn( N );
    TVector prev_tg( _TScalar( 0 ) ), prev_no( _TScalar( 0 ) );
    prev_tg[ 0 ] = _TScalar( 1 );
    prev_no[ 1 ] = _TScalar( 1 );
    for( unsigned int i = 0; i < N; ++i )
    {
      auto ntg = tg[ i ].GetNorm( );
      if( double( ntg ) > double( 0 ) )
        tg[ i ] /= ntg;

      _TScalar ct = prev_tg * tg[ i ];
      TVector a = itk::CrossProduct( prev_tg, tg[ i ] );
      _TScalar st = a.GetNorm( );
      if( st > _TScalar( 0 ) )
        a /= st;

      no[ i ] =
        ( prev_no * ct ) +
        ( itk::CrossProduct( a, prev_no ) * st ) +
        ( a * ( ( a * prev_no ) * ( _TScalar( 1 ) - ct ) ) );
      auto nno = no[ i ].GetNorm( );
      if( double( nno ) > double( 0 ) )
        no[ i ] /= nno;

      bn[ i ] = itk::CrossProduct( tg[ i ], no[ i ] );

      prev_tg = tg[ i ];
      prev_no = no[ i ];

    } // rof

    this->m_Frames.clear( );
    for( unsigned int i = 0; i < N; ++i )
    {
      TMatrix m;
      for( unsigned int d = 0; d < 3; ++d )
      {
        m[ d ][ 0 ] = tg[ i ][ d ];
        m[ d ][ 1 ] = no[ i ][ d ];
        m[ d ][ 2 ] = bn[ i ][ d ];

      } // rof
      this->m_Frames.push_back( m );

    } // rof

    this->m_FramesUpdated = true;

  } // fi
  return( this->m_Frames[ id ] );
}

// -------------------------------------------------------------------------
template< class _TScalar >
const typename cpExtensions::DataStructures::Simple3DCurve< _TScalar >::
TPoint& cpExtensions::DataStructures::Simple3DCurve< _TScalar >::
GetPoint( unsigned int id ) const
{
  return( this->m_Points[ id ] );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::Simple3DCurve< _TScalar >::
TVector cpExtensions::DataStructures::Simple3DCurve< _TScalar >::
GetVector( unsigned int id ) const
{
  return( this->m_Points[ id ].GetVectorFromOrigin( ) );
}

// -------------------------------------------------------------------------
template< class _TScalar >
cpExtensions::DataStructures::Simple3DCurve< _TScalar >::
Simple3DCurve( )
  : Superclass( ),
    m_FramesUpdated( false )
{
}

// -------------------------------------------------------------------------
template< class _TScalar >
cpExtensions::DataStructures::Simple3DCurve< _TScalar >::
~Simple3DCurve( )
{
}

// -------------------------------------------------------------------------
template class cpExtensions::DataStructures::Simple3DCurve< float >;
template class cpExtensions::DataStructures::Simple3DCurve< double >;

// eof - $RCSfile$