//
// Created by bruno on 13. 7. 2026.
//

#include "goertzel.h"
#include <math.h>


void goertzel_init(Goertzel *g, float freq, float sampleRate, int samples)
{
    float omega = 2.0f * M_PI * freq / sampleRate;
    g->coeff = 2.0f*cosf(omega);
    g->s1=0;
    g->s2=0;
}

void goertzel_add(Goertzel *g, float sample)
{
    float s = sample + g->coeff*g->s1 - g->s2;
    g->s2=g->s1;
    g->s1=s;
}

float goertzel_energy(Goertzel *g)
{
    float power = g->s1*g->s1 + g->s2*g->s2 - g->coeff*g->s1*g->s2;
    g->s1=0;
    g->s2=0;
    return power;
}

int goertzel_bin_freq(int freq, int sampleRate, int samples)
{
    int bin = (freq * samples + sampleRate / 2) / sampleRate;
    return bin * sampleRate / samples;
}