#ifndef __cpExtensions__DataStructures__PolyLineParametricPath__hxx__
#define __cpExtensions__DataStructures__PolyLineParametricPath__hxx__

#include <itkMath.h>
#include <itkNumericTraits.h>

// -------------------------------------------------------------------------
template< unsigned int _VDim >
void cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
AddVertex( const TContinuousIndex& vertex )
{
  this->Superclass::AddVertex( vertex );
  this->m_Bezier->AddPoint(
    this->GetPoint( this->GetSize( ) - 1 ).GetVectorFromOrigin( )
    );
  this->Modified( );
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
unsigned long cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
GetSize( ) const
{
  return( this->GetVertexList( )->Size( ) );
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
typename cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
TContinuousIndex
cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
GetContinuousVertex( unsigned long i ) const
{
  return( this->GetVertexList( )->GetElement( i ) );
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
typename cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
TIndex cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
GetVertex( unsigned long i ) const
{
  TContinuousIndex cidx = this->GetContinuousVertex( i );
  TIndex idx;
  for( unsigned int d = 0; d < _VDim; ++d )
    idx[ d ] = cidx[ d ];
  return( idx );
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
typename cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
TPoint cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
GetPoint( unsigned long i ) const
{
  typedef typename TPoint::CoordRepType _TCoordRep;
  TPoint pnt;
  TContinuousIndex idx = this->GetVertex( i );
  for( unsigned int r = 0; r < _VDim; ++r )
  {
    _TCoordRep sum = itk::NumericTraits< _TCoordRep >::ZeroValue( );
    for( unsigned int c = 0; c < _VDim; ++c )
      sum += this->m_IndexToPhysicalPoint( r, c ) * idx[ c ];
    pnt[ r ] = sum + this->m_Origin[ r ];

  } // rof
  return( pnt );
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
typename cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
TPoint cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
GetSmoothPoint( double u ) const
{
  TPoint p;
  p.Fill( 0 );
  p += this->m_Bezier->Evaluate( u );
  return( p );
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
void cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
SetSpacing( const TSpacing& spac )
{
  if( this->m_Spacing != spac )
  {
    this->m_Spacing = spac;
    this->_ComputeIndexToPhysicalPointMatrices( );
    this->Modified( );

  } // fi
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
void cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
SetSpacing( const double spac[ _VDim ] )
{
  this->SetSpacing( TSpacing( spac ) );
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
void cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
SetSpacing( const float spac[ _VDim ] )
{
  TSpacing s;
  for( unsigned int d = 0; d < _VDim; ++d )
    s[ d ] = spac[ d ];
  this->SetSpacing( s );
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
void cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
SetOrigin( const double ori[ _VDim ] )
{
  this->SetOrigin( TPoint( ori ) );
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
void cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
SetOrigin( const float ori[ _VDim ] )
{
  this->SetOrigin( TPoint( ori ) );
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
void cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
SetDirection( const TDirection& dir )
{
  bool modified = false;
  for( unsigned int r = 0; r < _VDim; r++ )
  {
    for( unsigned int c = 0; c < _VDim; c++ )
    {
      if(
        itk::Math::NotExactlyEquals(
          this->m_Direction[ r ][ c ], dir[ r ][ c ]
          )
        )
      {
        this->m_Direction[ r ][ c ] = dir[ r ][ c ];
        modified = true;
      } // fi

    } // rof

  } // rof
  if( modified )
  {
    this->_ComputeIndexToPhysicalPointMatrices( );
    this->m_InverseDirection = this->m_Direction.GetInverse( );
    this->Modified( );

  } // fi
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
PolyLineParametricPath( )
  : Superclass( )
{
  this->m_Bezier = TBezier::New( );

  this->m_Spacing.Fill( 1.0 );
  this->m_Origin.Fill( 0.0 );
  this->m_Direction.SetIdentity( );
  this->m_InverseDirection.SetIdentity( );
  this->m_IndexToPhysicalPoint.SetIdentity( );
  this->m_PhysicalPointToIndex.SetIdentity( );
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
~PolyLineParametricPath( )
{
}

// -------------------------------------------------------------------------
template< unsigned int _VDim >
void cpExtensions::DataStructures::PolyLineParametricPath< _VDim >::
_ComputeIndexToPhysicalPointMatrices( )
{
  TDirection scale;
  scale.Fill( 0.0 );
  for( unsigned int i = 0; i < _VDim; i++ )
  {
    if( this->m_Spacing[ i ] == 0.0 )
      itkExceptionMacro(
        "A spacing of 0 is not allowed: Spacing is " << this->m_Spacing
        );
    scale[ i ][ i ] = this->m_Spacing[ i ];

  } // rof

  if( vnl_determinant( this->m_Direction.GetVnlMatrix( ) ) == 0.0 )
    itkExceptionMacro(
      << "Bad direction, determinant is 0. Direction is "
      << this->m_Direction
      );
  this->m_IndexToPhysicalPoint = this->m_Direction * scale;
  this->m_PhysicalPointToIndex = this->m_IndexToPhysicalPoint.GetInverse( );
  this->Modified( );
}

#endif // __cpExtensions__DataStructures__PolyLineParametricPath__hxx__

// eof - $RCSfile$