#include "phasefilter.h"
#include "math.h"
#undef M_PI
#ifndef M_PI
    #define M_PI 3.1415926535897932384626433832795
#endif

double phasefilter_calc_c(double r1, double r2, double phi, double f0, double fd) {
    const double w = (2. * M_PI * f0) / fd;
    const double c_etalon_min = (1e-6);
    const double c_etalon_max = (100e-6);
    double a, b, c, d, x1, x2, x;

    phi = phi*M_PI/180;
    a = tan(phi) * (2 - 2 * cos(w));
    b = tan(phi) * ((1. / r1 + 2. / r2) * (1 - cos(w))) - (sin(w) / r1);
    c = tan(phi) * (1. / r1 + 1. / r2) / r2;

    d = b * b - (4 * a * c);
    x1 = ((-b + sqrt(d)) / (2 * a)) / fd;
    x2 = ((-b - sqrt(d)) / (2 * a)) / fd;

    x = 0;
    if (x1 > c_etalon_min && x1 < c_etalon_max) {
        x = x1;
    } else if (x2 > c_etalon_min && x2 < c_etalon_max) {
        x = x2;
    }
    return x;
}

double phasefilter_calc_phi(double r1, double r2, double c, double f0, double fd) {
    double cd = c * fd;
    const double w = (2. * M_PI * f0) / fd;

    double tg = (cd * sin(w)/r1 )/(cd * cd * (2 - 2 * cos(w)) + cd * ((1. / r1 + 2. / r2) * (1 - cos(w))) +  (1. / r1 + 1. / r2) / r2);

    return  atan(tg) * 180. / M_PI;
}

void phasefilter_init(t_phasefilter_state *state, double r1, double r2,  double phi, double f0, double fd) {
    state->is_valid = (f0 > 0) && (fd > 0);
    if (state->is_valid) {
        double c = phasefilter_calc_c(r1, r2, phi, f0, 1e6);
        /* коэффициенты рассчитываются из передаточной функции аналогового фильтра
         * путем билинейного преобразования (рассчет во временной области
         * прямым переходом от диф. уравнений к разностным дает большую ошибку
         * на малых частотах дискретизации */

        double w1 = 1./(r1 * c * fd);
        double w2 = 1./(r2 * c * fd);
        double a0 = w2/2 + 1;

        state->a = (w2/2 - 1)/a0;
        state->b0 = ((w1 + w2)/2 - 1)/a0;
        state->b1 = ((w1 + w2)/2 + 1)/a0;
    }
    phasefilter_reset(state);
}

void phasefilter_reset(t_phasefilter_state *state) {
    state->pev_valid = 0;
}

double phasefilter_process_point(t_phasefilter_state *state, double x) {
    double y;    
    if (state->is_valid) {
        if (!state->pev_valid) {
            y = x;
            state->pev_valid = 1;
        } else {
            y = -state->a * state->prev_y + state->b1 * x + state->b0 * state->prev_x;
        }
        state->prev_x = x;
        state->prev_y = y;
    } else {
        y = x;
    }
    return y;
}
