#include <iostream>
#include "biquad.h"
#include "bad_thing.h"
using namespace std;

// gory-detail constructor
biquad::biquad(vector<double> const _c, vector<double> const _d,
                vector<C> _zeros, vector<C> _poles)
: c(_c), d(_d), zeros(_zeros), poles(_poles),
  ws1(0), ws2(0)
{
    if (d[0] != 1.) throw bad_thing("biquad: d0 must be 1.");
}

// constructor in terms of zero and pole position,
// assuming conjugate pairs:
biquad::biquad(C const zero, C const pole)
///// nume = (z-zero)(z-zerobar)
/////      = z^2 - 2 zero.real + |zero|^2
: biquad({1., -2.*zero.real(), norm(zero)},
         {1., -2.*pole.real(), norm(pole)},
         {zero, conj(zero)},
         {pole, conj(pole)})
{}

C constexpr biquad::C0;
C constexpr biquad::C1;

vector<C> solve(vector<double> const poly) {
    double disc(1 - 4. * poly[0] * poly[2] / poly[1] / poly[1]);
    C inner(biquad::C1 + sqrt(C(disc, 0)));
    C root1 = C(-poly[1]/2/poly[0], 0) * inner;
    return { root1,
             C(poly[2]/poly[0], 0) / root1 };
}

// constructor in terms of coefficients:
biquad::biquad(vector<double> const _c, vector<double> const _d)
: biquad(_c, _d,
       {C0, C0}, {C0, C0})
{
  zeros = solve(c);
  poles = solve(d);
}

double biquad::step(double const Vin){
    double q1(ws1);
    double q2(ws2);
    double Vout = c[0]*Vin + q1;
    ws1 = c[1]*Vin - d[1]*Vout + q2;
    ws2 = c[2]*Vin - d[2]*Vout;
    return Vout;
}

// transfer function
C biquad::xfunc(C const z) const {
    C nume = (c[0]*z + c[1])*z + c[2];
    C denom =     (z + d[1])*z + d[2];
    return nume / denom;
}

void biquad::please_normalize(C const z) {
    double denom(abs(xfunc(z)));
//    cout << "denom: " << denom << endl;
//    cout << "c2: " << c[2] << endl;
//    cout << "z: " << z << endl;
    for (auto &coeff : c) {
      coeff /= denom;
    }
}

// same as above, but with some error checking:

void biquad::normalize(C const z) {
  if (abs(abs(z) - 1.) > 1e-10) {
//
    throw bad_thing("normalizing to z not on unit circle");
  }
  please_normalize(z);
}