
#include "Filter.h"
#include "matrix/PrVector.h"

#ifndef M_PI
#define TWOPI	(2.0*3.14159265358979323846264338327950288419716939937510582)
#else
#define TWOPI	(2*M_PI)
#endif //M_PI


Filter::Filter(const Filter &src)
{
  Allocate(src.size_,src.order_);
  operator=(src);
}


Filter &
Filter::operator=(const Filter &src)
{
  (*a_) = *src.a_;
  (*b_) = *src.b_;
  (*w_) = *src.w_;
  for(int i=0; i<order_+1 ; i++ )
  { *f_[i] = *src.f_[i];
    *r_[i] = *src.r_[i];
  }
  return (*this);
}


void
Filter::Allocate( int size,  int order)
// Allocates memory for filter constants and aging the data
{
  size_ = size;
  order_ = order;

  a_ = new PrVector(order_+1);
  b_ = new PrVector(order_+1);
  w_ = new PrVector(order_);

  f_ = new PrVector * [order_+1];
  r_ = new PrVector * [order_+1];
  for(int i=0; i<order_+1 ; i++ )
  { f_[i] = new PrVector(size_);
    r_[i] = new PrVector(size_);
  }
}


Filter::~Filter()
// Destructor
{
  delete a_;
  delete b_;
  delete w_;
  for(int i=0; i<order_+1 ; i++ )
  { delete f_[i];
    delete r_[i];
  }
  delete [] f_;
  delete [] r_; 
}


void
Filter::filt(const PrVector &in, PrVector &out)
// Use this after a filter has been initialized to filter the data
{
  int i;

  out = in*(*b_)[0];

  for( i=1; i<order_+1 ; i++)
  { 
    out +=   *r_[i] * (*b_)[i] - *f_[i] * (*a_)[i];
  }
  // AGE THE VALUES
  for( i=2; i<order_+1 ; i++)
  { *f_[i] = *f_[i-1];
    *r_[i] = *r_[i-1];
  }
  *f_[1] = out;
  *r_[1] = in;
}

void
Filter::init_zNumDen(const PrVector &r0, const PrVector &z_num, const PrVector &z_den)
// general digital transfer function: z_num/z_den
{
  Allocate( r0.size(), z_den.size() );

  *b_ = z_num;
  *a_ = z_den;

  *r_[1] = r0; //Store initial value
}


void
Filter::init_LowPass(const PrVector &r0, Float sampFreq, Float cutOffFreq)
// w/(s+w)
{
  Allocate( r0.size(), 1 );
  period_ = 1.0/sampFreq;

  (*w_)[0] = TWOPI*cutOffFreq;

  (*b_)[0] =  (period_*(*w_)[0]/2)/((period_*(*w_)[0]/2)+1);
  (*b_)[1] =  (*b_)[0];
  (*a_)[0] =  1;
  (*a_)[1] =  ((period_*(*w_)[0]/2)-1)/((period_*(*w_)[0]/2)+1);

  *r_[1] = r0; //Store initial value
}


void
Filter::init_dLowPass(const PrVector &r0, Float sampFreq, Float cutOffFreq)
// (s*w)/(s+w)
{
  Allocate( r0.size(), 1 );
  period_ = 1.0/sampFreq;

  (*w_)[0] = TWOPI*cutOffFreq;

  (*b_)[0] =  2*(*w_)[0]/((*w_)[0]*period_+2);
  (*b_)[1] = -(*b_)[0];
  (*a_)[0] =  1;
  (*a_)[1] =  ((*w_)[0]*period_-2)/((*w_)[0]*period_+2);

  *r_[1] = r0; //Store initial value
}


void
Filter::init_d(const PrVector &r0, Float sampFreq)
//  s
{
  Allocate( r0.size(), 1 );
  period_ = 1.0/sampFreq;

  (*b_)[0] =  1.0/(period_);
  (*b_)[1] = -(*b_)[0];
  (*a_)[0] =  1;
  (*a_)[1] =  0;

  *r_[1] = r0; //Store initial value
}
