#ifndef __CPM__ALGORITHMS__SIMPLEX__DEFORMATIONFILTER__HXX__
#define __CPM__ALGORITHMS__SIMPLEX__DEFORMATIONFILTER__HXX__

// -------------------------------------------------------------------------
template< class M >
void cpm::Algorithms::Simplex::DeformationFilter< M >::
AddForce( TForceFunction* f, const TScalar& w )
{
  this->m_Forces.push_back( TFunctionPair( f, w ) );
  this->Modified( );
}

// -------------------------------------------------------------------------
template< class M >
cpm::Algorithms::Simplex::DeformationFilter< M >::
DeformationFilter( )
  : Superclass( ),
    m_Damping( TScalar( 1 ) )
{
}

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

// -------------------------------------------------------------------------
template< class M >
void cpm::Algorithms::Simplex::DeformationFilter< M >::
GenerateData( )
{
  typedef typename M::VectorType TVector;

  const M* in = this->GetInput( );
  M* out = this->GetOutput( );
  unsigned int nPoints = in->GetNumberOfPoints( );
  std::vector< TVector > final_forces[ 2 ];
  unsigned int aForces = 0;
  final_forces[ 0 ].resize( nPoints, TVector( TScalar( 0 ) ) );
  final_forces[ 1 ].resize( nPoints, TVector( TScalar( 0 ) ) );

  // Configure forces
  typename TFunctionContainer::iterator fIt = this->m_Forces.begin( );
  for( ; fIt != this->m_Forces.end( ); ++fIt )
    fIt->first->SetMesh( in );

  unsigned int leo = 0;
  while( leo < 20000 ) // TODO: !!!!
  {
    leo++;
    TScalar damping = TScalar( 1 ) - this->m_Damping;
    unsigned int pForces = ( aForces + 1 ) % 2;

    // Compute forces
    for( TPointId pId = 0; pId < nPoints; ++pId )
    {
      if( in->FindEdge( pId )->IsOriginInternal( ) )
      {
        fIt = this->m_Forces.begin( );
        TVector f = fIt->first->Evaluate( pId ) * fIt->second;
        for( ++fIt; fIt != this->m_Forces.end( ); ++fIt )
          f += fIt->first->Evaluate( pId ) * fIt->second;
        f += final_forces[ pForces ][ pId ] * damping;
        final_forces[ aForces ][ pId ] = f;
      }
      else
        final_forces[ aForces ][ pId ].Fill( TScalar( 0 ) );

    } // rof

    // Move points
    for( TPointId pId = 0; pId < nPoints; ++pId )
    {
      typename M::PointType pnt = in->GetPoint( pId );
      pnt += final_forces[ aForces ][ pId ];
      out->SetPoint( pId, pnt );
      this->InvokeEvent( PointUpdatedEvent( pId ) );

    } // rof
    aForces = pForces;

    this->InvokeEvent( AllMeshUpdatedEvent( ) );

  } //elihw
}

#endif // __CPM__ALGORITHMS__SIMPLEX__DEFORMATIONFILTER__HXX__

// eof - $RCSfile$