#ifndef C_3D_POINTS_SMOOTHER_H_

#define C_3D_POINT_SMOOTHER_H_

#include <iostream>
#include <list>

using namespace std;

#include "Vect3D.h"
#include "wiimote.h"

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;

typedef v3 Vect3D_t;

class C_3DPointSmoother
{
public:
  C_3DPointSmoother() : 
    m_Method(SimpMovingAvg), 
    m_iWindowSize(2), 
    m_iSampCount(0),
    m_dExponent(0.0),
    m_St() {}

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

  Vect3D_t Smooth(const Vect3D_t &f_Xt);

private:
  SmoothingMethod m_Method;
  int             m_iWindowSize, //window size for SimpMovingAvg
                  m_iSampCount;  //number of processed samples
  list<Vect3D_t>  m_SampWindow;
  double          m_dExponent;   //exponent for ExpMovingAvg
  Vect3D_t        m_St;          //last smoothed statistic S(t)

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

typedef struct rd
{
  int rx, ry;

  rd() { rx = ry = 0;}
  rd(const rd &d) { rx=d.rx; ry=d.ry; }
  inline const rd& operator=(const rd &rhs) { rx=rhs.rx; ry=rhs.ry; return *this;}
  inline const rd& operator-=(const rd &rhs) { rx-=rhs.rx; ry-=rhs.ry; return *this;}
  inline const rd operator-(const rd &rhs) { return rd(*this)-=rhs;}
} RawDot;

inline ostream& operator<<(ostream& os, const RawDot& r ) 
{
  stringstream ss;
  ss << setprecision(4)
     << "(" << r.rx << ", " << r.ry << ")";

  return os << ss.str();
}

bool FindSensorBarDots(wiimote_state::ir &f_ir, Vect3D_t f_Dots[2]);
bool FindSensorBarDots(RawDot *f_rd, int f_iNumdots,  Vect3D_t f_Dots[2]);

#endif //C_3D_POINT_SMOOTHER_H_