// =========================================================================
// @author Leonardo Florez Valencia
// @email florez-l@javeriana.edu.co
// =========================================================================

#include <fpa/Generic/PeakDetector.h>
#include <cmath>

// -------------------------------------------------------------------------
fpa::Generic::PeakDetector::
PeakDetector( )
  : m_K( 3 ),
    m_T( 3.5 ),
    m_I( 0.5 )
{
}

// -------------------------------------------------------------------------
fpa::Generic::PeakDetector::
~PeakDetector( )
{
}

// -------------------------------------------------------------------------
unsigned long fpa::Generic::PeakDetector::
GetKernelSize( ) const
{
  return( this->m_K );
}

// -------------------------------------------------------------------------
double fpa::Generic::PeakDetector::
GetThreshold( ) const
{
  return( this->m_T );
}

// -------------------------------------------------------------------------
double fpa::Generic::PeakDetector::
GetInfluence( ) const
{
  return( this->m_I );
}

// -------------------------------------------------------------------------
void fpa::Generic::PeakDetector::
SetKernelSize( unsigned long k )
{
  this->m_K = k;
  this->Clear( );
}

// -------------------------------------------------------------------------
void fpa::Generic::PeakDetector::
SetThreshold( double t )
{
  this->m_T = t;
  this->Clear( );
}

// -------------------------------------------------------------------------
void fpa::Generic::PeakDetector::
SetInfluence( double i )
{
  this->m_I = i;
  this->Clear( );
}

// -------------------------------------------------------------------------
void fpa::Generic::PeakDetector::
Clear( )
{
  this->m_X.clear( );
  this->m_Y.clear( );
  this->m_YF.clear( );
  this->m_Avg.clear( );
  this->m_STD.clear( );
  this->m_Peaks.clear( );
  this->m_Mean = 0;
  this->m_Vari = 0;
}

// -------------------------------------------------------------------------
unsigned long fpa::Generic::PeakDetector::
GetNumberOfSamples( ) const
{
  return( this->m_X.size( ) );
}

// -------------------------------------------------------------------------
fpa::Generic::PeakDetector::
TPeak fpa::Generic::PeakDetector::
AddValue( double x, double y )
{
  this->m_X.push_back( x );
  this->m_Y.push_back( y );

  if( this->m_YF.size( ) < this->m_K )
  {
    double n = double( this->m_YF.size( ) + 1 );
    this->m_YF.push_back( y );
    this->m_Avg.push_back( double( 0 ) );
    this->m_STD.push_back( double( 0 ) );
    this->m_Peaks.push_back( Self::NoPeak );
    if( n > double( 1 ) )
      this->m_Vari =
        ( ( ( n - 2.0 ) / ( n - 1.0 ) ) * this->m_Vari ) +
        ( ( ( y - this->m_Mean ) * ( y - this->m_Mean ) ) / n );
    this->m_Mean += ( y - this->m_Mean ) / n;
    if( this->m_YF.size( ) == this->m_K )
    {
      this->m_Avg.push_back( this->m_Mean );
      this->m_STD.push_back( std::sqrt( this->m_Vari ) );

    } // fi
  }
  else
  {
    unsigned long i = this->m_X.size( ) - 1;
    if(
      ( std::fabs( y - this->m_Avg[ i - 1 ] ) ) >
      ( this->m_T * this->m_STD[ i - 1 ] )
      )
    {
      this->m_Peaks.push_back(
        ( y > this->m_Avg[ i - 1 ] )? Self::PosPeak: Self::NegPeak
        );
      this->m_YF.push_back(
        ( this->m_I * y ) + ( ( 1.0 - this->m_I ) * this->m_YF[ i - 1 ] )
        );
    }
    else
    {
      this->m_Peaks.push_back( Self::NoPeak );
      this->m_YF.push_back( y );

    } // fi

    double avg = double( 0 );
    double var = double( 0 );
    unsigned long k = 0;
    for( unsigned long j = i - this->m_K; j <= i; ++j, ++k )
    {
      double v = this->m_YF[ j ];
      double n = double( k + 1 );
      if( k > 1 )
        var =
          ( ( ( n - 2.0 ) / ( n - 1.0 ) ) * var ) +
          ( ( ( v - avg ) * ( v - avg ) ) / n );
      avg += ( v - avg ) / n;

    } // rof
    this->m_Avg.push_back( avg );
    this->m_STD.push_back( std::sqrt( var ) );

  } // fi
  return( this->m_Peaks.back( ) );
}

// eof - $RCSfile$