#include "iir_bp.h"

////////////////////////////////////////
// negative of a vector, component-by-component:
//
std::vector<C> neg(const std::vector<C>& foo) {
  using namespace std;
  int siz = foo.size();
  vector<C> rslt(siz);
  for (int ii = 0; ii < siz; ii++) {
    rslt[ii] = -foo[ii];
  }
  return rslt;
}

////////////////////////////////////////
//  expand_poly - multiplies a set of binomials together and returns
//  the coefficients of the resulting polynomial.
//
//  All polynomials (input and output) are represented using the
//  /reduced/ representation, meaning the coefficient of the highest
//  power of x is assumed to be 1, and is not included in the
//  representation.
//
//  The multiplication has the following form:
//
//  (x+c[0]) * (x+c[1]) *...* (x+c[n-1])
//
//  On input, the c[i] is the coefficient of x^0 in the ith binomial.
//  Each c[i] is a complex number.
//
//  The resulting polynomial has the following form:
//
//  x^n + a[0]*x^n-1 + a[1]*x^n-2 +...+ a[n-2]*x + a[n-1]
//
//  The a[i] coefficients can in general be complex but in typical
//  digital-filter applications should turn out to be real.
//
std::vector<C> expand_poly(const std::vector<C>& cvec) {
    int n = cvec.size();

    std::vector<C> a(n);
    for (int ii = 0; ii < n; ii++) a[ii] = 0.;

    for (int i = 0; i < n; ++i) {
      for (int j = i; j > 0; --j) a[j] += cvec[i] * a[j-1];
      a[0] += cvec[i];
    }
    return a;
}

////////////////////////////////////////
// Calculate poles for Butterworth low pass filter.
//
std::vector<C> dpole_bwlp (int n, double fcf, double extraPole)
{
    using namespace std;

    int needed(n);
    if (extraPole) needed += 2;
    vector<C> dpole(needed);

    double theta = 2. * M_PI * fcf;
    double st = sin(theta);
    double ct = cos(theta);

    for (int k = 0; k < n; ++k) {
        // angle, as seen from (1,0)
        // as we go around the small circlet of poles:
        double polang = M_PI * (double)(2*k+1)/(double)(2*n);
        double denom = 1.0 + st*sin(polang);
        dpole[k] = C(ct/denom, st*cos(polang)/denom);
    }
    if (extraPole) {
      double thetaZero = 2. * M_PI * extraPole;
      dpole[n]   = C(cos(thetaZero),  sin(thetaZero));
      dpole[n+1] = C(cos(thetaZero), -sin(thetaZero));
    }
    return dpole;
}

////////////////////////////////////////
// return the real part of a vector of complex numbers
// also convert from reduced to non-reduced representation,
// by inserting rslt[0] = 1 and shifting everything one place
std::vector<double> rxpoly(const std::vector<C>& foo){
    std::vector<double> rslt(1+foo.size());

    rslt[0] = 1.;
    for (unsigned int k = 0; k < foo.size(); ++k)
       rslt[1+k] = foo[k].real();
    return (rslt);
}


////////////////////////////////////////
//  calculate the d coefficients for a butterworth lowpass  filter.
//
//  Returns result in the non-reduced representation,
//  i.e. a vector of n+1 doubles,
//  where dcof[0] is the coefficient in front of x^n
//  However, dcof[0] will always be 1.0.
//
std::vector<double> dcof_bwlp (int n, double fcf, double extraPole)  {
    return rxpoly(expand_poly(neg(dpole_bwlp(n, fcf, extraPole))));
}

////////////////////////////////////////
//  calculate the c coefficients for a butterworth lowpass filter.
//  details same as above.
//
std::vector<double> ccof_bwlp(const int n, const double extraZero) {
    return rxpoly(expand_poly(neg(croot_bwlp(n, extraZero))));
}

////////////////////////////////////////
std::vector<C> croot_bwlp (int n, double extraZero) {
    using namespace std;

    int needed(n);
    if (extraZero) needed += 2;
    vector<C> dpole(needed);

    for (int k = 0; k < n; ++k)  {
        dpole[k] = -1;
    }
    if (extraZero) {
      double thetaZero = 2. * M_PI * extraZero;
      dpole[n]   = C(cos(thetaZero),  sin(thetaZero));
      dpole[n+1] = C(cos(thetaZero), -sin(thetaZero));
    }
    return dpole;
}