#ifndef C_3D_POINT_SMOOTHER_H_

#define C_3D_POINT_SMOOTHER_H_

#include <iostream>
#include <list>
#include <iostream>
#include <iomanip>

using namespace std;

typedef enum
{                 //           k-1
  SimpMovingAvg,  //S(t) = (1/k)*sum(X(t-n))  =  S(t-1) + (X(t) - X(t-k))/k
                  //           n=0

  ExpMovingAvg    //S(0) = X(0)
                  //S(t) = a*X(t) + (1 - a)*S(t-1) = S(t-1) + a*(X(t) - S(t-1))

} SmoothingMethod;


template<typename T>
class C_Smoother
{
public:
  C_Smoother(T f_ZeroVal) : 
    m_Method(SimpMovingAvg), 
    m_iWindowSize(2), 
    m_iSampCount(0),
    m_ZeroVal(f_ZeroVal),
    m_dExponent(0.0) {}

  void SetSimpleMovingAverage(int f_iWindowSize);
  void SetExponentialMovingAverage(double f_dExponent);

  T Smooth(const T &f_Xt);

private:
  SmoothingMethod m_Method;
  int             m_iWindowSize, //window size for SimpMovingAvg
                  m_iSampCount;  //number of processed samples
  list<T>         m_SampWindow;
  double          m_dExponent;   //exponent for ExpMovingAvg
  T               m_ZeroVal,     //the value that is supposed to be zero
                  m_St;          //last smoothed statistic S(t)

  T SmoothSample_SimpMovingAvg(const T &f_Xt);
  T SmoothSample_ExpMovingAvg(const T &f_Xt);
};

template <typename T> 
void C_Smoother<T>::SetSimpleMovingAverage(int f_iWindowSize)
{
  //reset smoothing stuff for Simple moving average smoothing
  m_Method = SimpMovingAvg;
  m_iWindowSize = f_iWindowSize;
  m_iSampCount = 0;
  m_St = m_ZeroVal; 
  m_SampWindow.clear();
}

template <typename T> 
void C_Smoother<T>::SetExponentialMovingAverage(double f_dExponent)
{
  m_Method = ExpMovingAvg;
  m_dExponent = f_dExponent;
  m_iSampCount = 0;
  m_St = m_ZeroVal;
}

template <typename T> 
T C_Smoother<T>::SmoothSample_SimpMovingAvg(const T &f_Xt)
{
  //add sample to the window
  m_SampWindow.push_front(f_Xt);

  if (m_iSampCount < m_iWindowSize)
  {
    //we don't have enough cmopute the average of the points currently in the window
    T l_sum = m_ZeroVal;
    for (std::list<T>::iterator it = m_SampWindow.begin(); it != m_SampWindow.end(); it++)
      l_sum += *it;
    return l_sum / (double)m_SampWindow.size();
  }
  else
  {
    //there are mote than m_iWindowSize samples, incrementally update smoothed sample
    T l_Xtk = m_SampWindow.back(); //pop the last point from the (window) list
    m_SampWindow.pop_back(); 
    return m_St + ((f_Xt - l_Xtk)/(double)m_iWindowSize);
  }
}

template <typename T> 
T C_Smoother<T>::SmoothSample_ExpMovingAvg(const T &f_Xt)
{
  if (m_iSampCount == 0)
    return f_Xt;
  else
    return m_St + m_dExponent * (f_Xt - m_St);
}

template <typename T>
T C_Smoother<T>::Smooth(const T &f_Xt)
{
  switch(m_Method)
  {
  case SimpMovingAvg:
      m_St = SmoothSample_SimpMovingAvg(f_Xt);
    break;

  case ExpMovingAvg:
      m_St = SmoothSample_ExpMovingAvg(f_Xt);
    break;
  }

  m_iSampCount++;
  return m_St;
}

#endif //C_3D_POINT_SMOOTHER_H_