#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <complex.h>

typedef struct {
  float a1;
  float a2;
  float a3;
  float b0;
  float b1;
  float b2;
  float b3;
} BiquadCoeffs;


/* print array of three complex numbers */
void print_array_len3(char prefix[], float complex ary[3]) {
    printf("%s: array([ %f %fj, %f %fj, %f %fj ])\n",
           prefix,
           creal(ary[0]), cimag(ary[0]),
           creal(ary[1]), cimag(ary[1]),
           creal(ary[2]), cimag(ary[2]));
}


/* return 3rd order butterworth filter coefficients,
    Wn is the cutoff frequency in rad,
    btype can either be "low" or "high"
*/
BiquadCoeffs butter_3rd(float Wn, char* btype) {
    BiquadCoeffs coeffs = { 0, 0, 0, 0, 0, 0, 0 };
    int idx;
    float complex p_proto[3];    // array of the three analog prototype poles
    float warped;                // warped cutoff frequency
    float complex z_analog[3];   // array of the three analog zeroes
    float complex z_digital[3];  // array of the three digital zeroes
    float complex p_analog[3];   // array of the three analog poles
    float complex p_digital[3];  // array of the three digital poles
    float k_analog, k_digital;   // analog and digital gain
    float complex tmp1, tmp2;    // temporary variables


    // calculate poles of the analog prototype filter
    /* python code:
        p_proto = []
        for m in (-2, 0, 2):
            p_proto.append(-numpy.exp(1j * pi * m / 6))
    */
    for (int m = -2; m <= 2; m += 2){
        idx = (m + 2) / 2;
        p_proto[idx] = -1.0 * cexp(0+1i * M_PI * m / 6.0);
    }
    print_array_len3("p_proto", p_proto);


    // pre-warping factor
    /* python code:
        warped = 4 * tan(pi * Wn / 2)
    */
    warped = 4.0 * tan(M_PI * Wn / 2.0);
    printf("warped: %f\n", warped);


    // construct high/lowpass zeros and poles according btype
    if (strcmp(btype, "low") == 0) {
        /* python code:
            z_analog = numpy.array([])
            z_digital = numpy.array(-numpy.ones(3))
            p_analog = warped * p
            k_analog = warped ** 3
        */
        // z_analog is not needed later
        for (idx=0; idx<3; idx++)
            z_digital[idx] = -1.0;
        for (idx=0; idx<3; idx++)
            p_analog[idx] = warped * p_proto[idx];
        k_analog = warped * warped * warped;
    } else if (strcmp(btype, "high") == 0) {
        /* python code:
            z_analog = numpy.zeros(3)
            z_digital = numpy.ones(3)
            p_analog = warped / p
            k_analog = 1
        */
        for (idx=0; idx<3; idx++)
            z_analog[idx] = 0.0;
        for (idx=0; idx<3; idx++)
            z_digital[idx] = 1.0;
        for (idx=0; idx<3; idx++)
            p_analog[idx] = warped / p_proto[idx];
        k_analog = 1;
    }
    print_array_len3("z_analog", z_analog);
    print_array_len3("z_digital", z_digital);
    print_array_len3("p_analog", p_analog);
    printf("k_analog: %f\n", k_analog);


    /* python code:
        p_digital = (4 + p_analog) / (4 - p_analog)
    */
    for (idx=0; idx<3; idx++)
        p_digital[idx] = (4 + p_analog[idx]) / (4 - p_analog[idx]);
    print_array_len3("p_digital", p_digital);

    /* python code:
        k_digital = k_analog * numpy.real(numpy.prod(4 - z_analog) / numpy.prod(4 - p_analog))
    */
    if (strcmp(btype, "low") == 0) {
        tmp1 = 1.0; // special case, z_analog is empty in case of lowpass filter
    } else if (strcmp(btype, "high") == 0) {
        tmp1 = 4.0 - z_analog[0];
        for (idx=1; idx<3; idx++)
            tmp1 = tmp1 * (4.0 - z_analog[idx]);
    }
    tmp2 = 4.0 - p_analog[0];
    for (idx=1; idx<3; idx++)
        tmp2 = tmp2 * (4.0 - p_analog[idx]);
    k_digital = k_analog * creal(tmp1 / tmp2);
    printf("k_digital: %f\n", k_digital);


    // transform to ba form
    /* python code:
        b = k_digital * numpy.poly(z_digital)
        a = numpy.poly(p_digital)
    */
    // NEED HELP!!! :)

    return coeffs;
}


int main() {
    BiquadCoeffs coeffs;
    float Wn = 0.2;

    printf("\nlow\n===\n");
    coeffs = butter_3rd(Wn, "low");
    printf("coeffs b: %0.8f %0.8f %0.8f %0.8f\n", coeffs.b0, coeffs.b1, coeffs.b2, coeffs.b3);
    printf("coeffs a: %0.8f %0.8f %0.8f %0.8f\n", 1.0000000, coeffs.a1, coeffs.a2, coeffs.a3);

    printf("\nhigh\n====\n");
    coeffs = butter_3rd(Wn, "high");
    printf("coeffs b: %0.8f %0.8f %0.8f %0.8f\n", coeffs.b0, coeffs.b1, coeffs.b2, coeffs.b3);
    printf("coeffs a: %0.8f %0.8f %0.8f %0.8f\n", 1.0000000, coeffs.a1, coeffs.a2, coeffs.a3);
}