#include "lmath.h"
#include "lmath_internal.h"
#include "lmath_goertzel.h"
#include <stdlib.h>

struct st_lmath_goertzel_ctx {
    int size;
    double wsin;
    double wcos2;
    double w0;
    double w1;
};


t_lmath_goertzel_ctx lmath_goertzel_ctx_create(int size, double dt, double f) {
    t_lmath_goertzel_ctx ctx = (t_lmath_goertzel_ctx)malloc(sizeof(*ctx));
    if (ctx != NULL) {
        const double m = f * dt * size;
        const double w = 2.*M_PI * m / size;
        ctx->size = size;
        ctx->wsin = sin(w);
        ctx->wcos2 = 2 * cos(w);
        ctx->w0 = ctx->w1 = 0;
    }
    return ctx;
}

void lmath_goertzel_ctx_destroy(t_lmath_goertzel_ctx ctx) {
    if (ctx != NULL) {
        free(ctx);
    }
}

void lmath_goertzel_process(t_lmath_goertzel_ctx ctx, const double *sig, int sig_size) {
    int i;
    const double wcos2 = ctx->wcos2;
    double w0 = ctx->w0;
    double w1 = ctx->w1;

    for (i = 0; i < sig_size; i++) {
        double x = *sig++;
        double w2 = wcos2 * w1 - w0 + x;
        w0 = w1;
        w1 = w2;
    }
    ctx->w0 = w0;
    ctx->w1 = w1;
}

t_lmath_errs lmath_goertzel_result(t_lmath_goertzel_ctx ctx, double *r, double *im) {
    if (r)
        *r = ctx->w1 * ctx->wcos2/2 - ctx->w0;
    if (im)
        *im = ctx->w1* ctx->wsin;
    return LMATH_ERR_OK;
}

t_lmath_errs lmath_goertzel_result_amp_pha(t_lmath_goertzel_ctx ctx, t_lmath_phase_mode pha_mode,
                                           double *amp, double *pha) {
    t_lmath_errs err;
    double r, im;
    err = lmath_goertzel_result(ctx, &r, &im);
    if (err == LMATH_ERR_OK) {
        if (amp)
            *amp = sqrt(r * r + im * im) * 2 / ctx->size;
        if (pha)
            *pha = lmath_phase_calc(r, im, pha_mode);
    }
    return err;
}
