/*
  #include <cmath>
  #include <limits>
  #include <map>
*/
#include <vnl/vnl_math.h>
#include <cpExtensions/DataStructures/FourierSeriesContour.h>

// -------------------------------------------------------------------------
template< class _TScalar >
cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
FourierSeriesContour( unsigned int q )
{
  this->resize( ( q << 1 ) + 1, TComplex( TScalar( 0 ), TScalar( 0 ) ) );
}

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

// -------------------------------------------------------------------------
template< class _TScalar >
_TScalar cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
GetArea( ) const
{
  S a = TScalar( 0 );
  int q = this->GetNumberOfHarmonics( );
  typename Self::const_iterator i = this->begin( );
  for( int l = -q; i != this->end( ); ++i, ++l )
    a += TScalar( l ) * std::norm( *i );
  return( TScalar( vnl_math::pi ) * a );
}

// -------------------------------------------------------------------------
template< class _TScalar >
_TScalar cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
GetCurvature( const _TScalar& w ) const
{
  TComplex d1 = this->_Z( w, 1 );
  TComplex d2 = this->_Z( w, 2 );
  TScalar d = std::abs( d1 );
  if( d > TScalar( 0 ) )
    return( std::imag( std::conj( d1 ) * d2 ) / ( d * d * d ) );
  else
    return( TScalar( 0 ) );
}

// -------------------------------------------------------------------------
template< class _TScalar >
void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
SetOrdering( bool cw )
{
  // Check if the area sign is different from the desired orientation
  bool ap = ( TScalar( 0 ) < this->GetArea( ) );
  if( ( !ap && cw ) || ( ap && !cw ) ) // XOR
    this->InvertOrdering( );
}

// -------------------------------------------------------------------------
template< class _TScalar >
void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
InvertOrdering( )
{
  int q = this->GetNumberOfHarmonics( );
  for( int l = 1; l <= q; l++ )
  {
    TComplex z = ( *this )[ l ];
    ( *this )[  l ] = ( *this )[ -l ];
    ( *this )[ -l ] = z;

  } // rof
}

// -------------------------------------------------------------------------
template< class _TScalar >
void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
SetOrderingToClockWise( )
{
  this->SetOrdering( false );
}

// -------------------------------------------------------------------------
template< class _TScalar >
void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
SetOrderingToCounterClockWise( )
{
  this->SetOrdering( true );
}

// -------------------------------------------------------------------------
template< class _TScalar >
int cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
GetNumberOfHarmonics( ) const
{
  return( ( this->size( ) - 1 ) >> 1 );
}

// -------------------------------------------------------------------------
template< class _TScalar >
void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
SetNumberOfHarmonics( const int& q )
{
  int diff_q = this->GetNumberOfHarmonics( ) - q;

  while( diff_q != 0 )
  {
    if( diff_q > 0 )
    {
      this->pop_front( );
      this->pop_back( );
      diff_q--;
    }
    else
    {
      this->push_front( TComplex( TScalar( 0 ), TScalar( 0 ) ) );
      this->push_back( TComplex( TScalar( 0 ), TScalar( 0 ) ) );
      diff_q++;

    } // fi

  } // elihw
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
TComplex& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator[]( const int& l )
{
  static TComplex _zero;
  int idx = ( ( this->size( ) - 1 ) >> 1 ) + l;
  if( idx < 0 || idx >= this->size( ) )
  {
    _zero = TComplex( TScalar( 0 ) );
    return( _zero );
  }
  else
    return( this->Superclass::operator[]( idx ) );
}

// -------------------------------------------------------------------------
template< class _TScalar >
const typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
TComplex& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator[]( const int& l ) const
{
  static const TComplex _zero( TScalar( 0 ) );
  int idx = ( ( this->size( ) - 1 ) >> 1 ) + l;
  if( idx < 0 || idx >= this->size( ) )
    return( _zero );
  else
    return( this->Superclass::operator[]( idx ) );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
TPoint cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator( )( const _TScalar& w ) const
{
  TComplex z = this->_Z( w, 0 );
  TPoint p;
  p[ 0 ] = z.real( );
  p[ 1 ] = z.imag( );
  return( p );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
TVector cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator( )( const _TScalar& w, const unsigned int& n ) const
{
  TComplex z = this->_Z( w, n );
  TVector v;
  v[ 0 ] = z.real( );
  v[ 1 ] = z.imag( );
  return( v );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator+( const Self& o ) const
{
  int q1 = o.GetNumberOfHarmonics( );
  int q2 = this->GetNumberOfHarmonics( );
  int q = ( q2 < q1 )? q1: q2;

  Self res;
  res.SetNumberOfHarmonics( q );
  for( int l = -q; l <= q; ++l )
    res[ l ] = ( *this )[ l ] + o[ l ];

  return( res );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator+=( const Self& o )
{
  int q1 = o.GetNumberOfHarmonics( );
  int q2 = this->GetNumberOfHarmonics( );
  int q = ( q2 < q1 )? q1: q2;

  this->SetNumberOfHarmonics( q );
  for( int l = -q; l <= q; ++l )
    ( *this )[ l ] += o[ l ];

  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator-( const Self& o ) const
{
  int q1 = o.GetNumberOfHarmonics( );
  int q2 = this->GetNumberOfHarmonics( );
  int q = ( q2 < q1 )? q1: q2;

  Self res;
  res.SetNumberOfHarmonics( q );
  for( int l = -q; l <= q; ++l )
    res[ l ] = ( *this )[ l ] - o[ l ];

  return( res );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator-=( const Self& o )
{
  int q1 = o.GetNumberOfHarmonics( );
  int q2 = this->GetNumberOfHarmonics( );
  int q = ( q2 < q1 )? q1: q2;

  this->SetNumberOfHarmonics( q );
  for( int l = -q; l <= q; ++l )
    ( *this )[ l ] -= o[ l ];

  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator+( const TComplex& translation ) const
{
  Self res = *this;
  res[ 0 ] += translation;
  return( res );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator+=( const TComplex& translation )
{
  ( *this )[ 0 ] += translation;
  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator-( const TComplex& translation ) const
{
  Self res = *this;
  res[ 0 ] -= translation;
  return( res );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator-=( const TComplex& translation )
{
  ( *this )[ 0 ] -= translation;
  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator+( const TVector& translation ) const
{
  return( ( *this ) + TComplex( translation[ 0 ], translation[ 1 ] ) );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator+=( const TVector& translation )
{
  ( *this ) += TComplex( translation[ 0 ], translation[ 1 ] );
  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator-( const TVector& translation ) const
{
  return( ( *this ) - TComplex( translation[ 0 ], translation[ 1 ] ) );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator-=( const TVector& translation )
{
  ( *this ) -= TComplex( translation[ 0 ], translation[ 1 ] );
  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
TPoint cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
GetCenter( ) const
{
  TComplex z = ( *this )[ 0 ];
  TPoint p;
  p[ 0 ] = z.real( );
  p[ 1 ] = z.imag( );
  return( p );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator*( const TComplex& scale_or_rotation ) const
{
  Self res;
  res.clear( );
  for( typename Self::const_iterator i = this->begin( ); i != this->end( ); ++i )
    res.push_back( *i * scale_or_rotation );
  return( res );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator*=( const TComplex& scale_or_rotation )
{
  for( typename Self::iterator i = this->begin( ); i != this->end( ); ++i )
    *i *= scale_or_rotation;
  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator/( const _TScalar& scale ) const
{
  Self res;
  res.clear( );
  for( typename Self::const_iterator i = this->begin( ); i != this->end( ); ++i )
    res.push_back( *i / scale );
  return( res );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator/=( const _TScalar& scale )
{
  for( typename Self::iterator i = this->begin( ); i != this->end( ); ++i )
    *i /= scale;
  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator&( const _TScalar& phase ) const
{
  Self res;
  res.clear( );
  int q = this->GetNumberOfHarmonics( );
  typename Self::const_iterator i = this->begin( );
  for( int l = -q; i != this->end( ); ++i, ++l )
    res.push_back( *i * std::polar( S( 1 ), S( l ) * phase ) );
  return( res );
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Self& cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
operator&=( const _TScalar& phase )
{
  int q = this->GetNumberOfHarmonics( );
  typename Self::iterator i = this->begin( );
  for( int l = -q; i != this->end( ); ++i, ++l )
    *i *= std::polar( S( 1 ), S( l ) * phase );
  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScalar >
_TScalar cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
GetPhase( const _TScalar& w ) const
{
  // Some constant values
  static const TScalar _0 = TScalar( 0 );
  static const TScalar _1 = TScalar( 1 );
  static const TScalar _2 = TScalar( 2 );
  static const TScalar _2pi = _2 * TScalar( vnl_math::pi );
  static const TScalar _epsilon = TScalar( 1e-14 );
  static const TScalar _tolerance = TScalar( 1e-10 );
  static const unsigned int _samples = 100;
  static const unsigned int _maxIterations = 1000;
  static const TScalar _angleOff = _2pi / TScalar( _samples );

  // Some other values
  int q = int( this->GetNumberOfHarmonics( ) );
  if( q == 0 )
    return( _0 );

  // Get a centered copy
  Self contour = *this;
  contour[ 0 ] = TComplex( _0, _0 );

  // Compute maximum coefficient norm
  TScalar max_norm = _0;
  for( int l = 1; l <= q; ++l )
  {
    TScalar np = std::abs( contour[  l ] );
    TScalar nn = std::abs( contour[ -l ] );
    TScalar n = ( np < nn )? nn: np;
    if( max_norm < n )
      max_norm = n;

  } // rof

  // Rotate to desired phase
  contour *= std::polar( _1, -w );

  // Try to normalize contour: if not, malformed contour!
  if( max_norm > _0 )
  {
    contour /= max_norm;

    // 1. Approximate initial guess by a simple dichotomy
    std::vector< std::pair< TScalar, TScalar > > function;
    TScalar minA = std::numeric_limits< TScalar >::max( );
    unsigned int minIdx = -1;

    // 1.1. Roughly sample phases
    for( unsigned int s = 0; s < _samples; ++s )
    {
      S w = TScalar( s ) * _angleOff;
      S a = std::arg( contour._Z( w, 0 ) );
      function.push_back( std::pair< TScalar, TScalar >( w, a ) );
      if( a < minA )
      {
        minA = a;
        minIdx = s;

      } // fi

    } // rof

    // 1.2. Get zero cuts by zero crossing analysis and keep the farthest
    //      point in the real axis (ie. the last data in "cuts")
    TScalar prevA = _1;
    std::map< TScalar, unsigned int > cuts;
    for( unsigned int s = 0; s < _samples; ++s )
    {
      unsigned int i = ( s + minIdx ) % _samples;
      if( function[ i ].second * prevA < _0 )
        cuts[ std::real( contour._Z( function[ i ].first, 0 ) ) ] = i;
      prevA = function[ i ].second;

    } // rof

    // 1.3. Get initial guess
    TScalar w0 = _0;
    if( cuts.size( ) > 0 )
    {
      unsigned int i = cuts.rbegin( )->second;
      if( i == 0 )
        i = function.size( );
      w0 = function[ i - 1 ].first;

    } // fi

    // 2. Refine by Newthon-Raphson
    bool stop = false;
    unsigned int i = 0;
    while( i < _maxIterations && !stop )
    {
      TComplex c = contour._Z( w0, 0 );
      TScalar d = std::imag( c * std::conj( contour._Z( w0, 1 ) ) );

      // Emergency stop!!!
      if( !( std::fabs( d ) < _epsilon ) )
      {
        TScalar w1 = w0 + ( std::arg( c ) * ( std::norm( c ) / d ) );
        if( ( std::fabs( w1 - w0 ) / std::fabs( w1 ) ) < _tolerance )
          stop = true;
        else
          w0 = w1;
        i++;
      }
      else
        stop = true;

    } // elihw
    return( w0 );
  }
  else
    return( _0 );
}

// -------------------------------------------------------------------------
template< class _TScalar >
void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
Sample( std::vector< TPoint >& p, const unsigned long& s ) const
{
  static const TScalar _2pi = TScalar( 2 ) * TScalar( vnl_math::pi );
  TScalar off = _2pi / TScalar( s - 1 );
  for( unsigned int w = 0; w < s; ++w )
    p.push_back( ( *this )( S( w ) * off ) );
}

// -------------------------------------------------------------------------
template< class _TScalar >
void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
GetEllipse( int l, _TScalar& a, _TScalar& b, _TScalar& t, _TScalar& p ) const
{
  a = b = t = p = TScalar( 0 );
  if( l == 0 || l > this->GetNumberOfHarmonics( ) )
    return;

  TComplex zp = ( *this )[  l ];
  TComplex zn = ( *this )[ -l ];
  TScalar np = std::abs( zp );
  TScalar nn = std::abs( zn );

  // Radii
  a = np + nn;
  b = np - nn;

  // Rotation and phase
  if( np > TScalar( 0 ) && nn > TScalar( 0 ) )
  {
    zp /= np;
    zn /= nn;
    t = std::real( std::log( zp * zn ) / TComplex( TScalar( 0 ), S( 2 ) ) );
    p = std::real( std::log( zp / zn ) / TComplex( TScalar( 0 ), S( 2 * l ) ) );
  }
  else
  {
    t = TScalar( 0 );
    p = ( np > TScalar( 0 ) )? std::arg( zp ): std::arg( zn );

  } // fi
}

// -------------------------------------------------------------------------
template< class _TScalar >
void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
SetEllipse( int l, _TScalar& a, _TScalar& b, _TScalar& t, _TScalar& p )
{
  TScalar nzp = ( a + b ) / TScalar( 2 );
  TScalar nzn = ( a - b ) / TScalar( 2 );
  TComplex et = std::polar( TScalar( 1 ), t );
  TComplex ep = std::polar( TScalar( 1 ), TScalar( l ) * p );
  ( *this )[  l ] = et * ep * nzp;
  ( *this )[ -l ] = et * std::conj( ep ) * nzn;
}

// -------------------------------------------------------------------------
template< class _TScalar >
typename cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
TComplex cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
_Z( const _TScalar& w, const unsigned int& n ) const
{
  static const TScalar _0 = TScalar( 0 );
  static const TScalar _1 = TScalar( 1 );
  TScalar _n = TScalar( n );

  TComplex z( _0, _0 );
  int q = this->GetNumberOfHarmonics( );
  for( int l = -q; l <= q; ++l )
  {
    TComplex v = ( *this )[ l ] * std::polar( _1, w * TScalar( l ) );
    if( n > 0 )
      v *= std::pow( TComplex( _0, TScalar( l ) ), _n );
    z += v;

  } // rof
  return( z );
}

// -------------------------------------------------------------------------
template< class _TScalar >
void cpExtensions::DataStructures::FourierSeriesContour< _TScalar >::
_DFT( const std::vector< TComplex >& p, unsigned int q )
{
  static const S _2pi = TScalar( 2 ) * TScalar( vnl_math::pi );

  this->SetNumberOfHarmonics( q );
  *this *= TScalar( 0 );
  if( p.size( ) == 0 )
    return;

  std::vector< TComplex > dft;
  for( long m = 0; m < p.size( ); ++m )
  {
    TComplex z( TScalar( 0 ), TScalar( 0 ) );
    for( long k = 0; k < p.size( ); ++k )
      z +=
        p[ k ] *
        std::polar(
          TScalar( 1 ),
          -( _2pi * TScalar( m * k ) ) / TScalar( p.size( ) )
          );
    z /= TScalar( p.size( ) );
    dft.push_back( z );

  } // rof

  ( *this )[ 0 ] = dft[ 0 ];
  for( int l = 1; l <= q; ++l )
  {
    ( *this )[  l ] = dft[ l ];
    ( *this )[ -l ] = dft[ p.size( ) - l ];

  } // rof
}

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

// eof - $RCSfile$