/*
 * afsk_demodulator.c
 *
 *  Created on: Sep 10, 2019
 *      Author: curiousmuch
 */

#include "stdio.h"
#include "stdint.h"
#include "math.h"
#include "afsk_demodulator.h"

int8_t window[WINDOW_SIZE];
//int32_t lpf_window[WINDOW_SIZE];

void goertzel_init(float freq0, float freq1, float *coeff0, float *coeff1)
{
	float normalizedfreq;

	// calculate coeff0
	normalizedfreq = freq0 / SAMPLEFREQUENCY;
	*coeff0 = (float) 2*cos(2*M_PI*normalizedfreq);

	// calculate coeff1
	normalizedfreq = freq1 / SAMPLEFREQUENCY;
	*coeff1 = (float) 2*cos(2*M_PI*normalizedfreq);
}

float goertzelFilter(int samples[], float freq, int N) {
    float s_prev = 0.0;
    float s_prev2 = 0.0;
    float coeff,normalizedfreq,power,s;
    int i;
    normalizedfreq = freq / SAMPLEFREQUENCY;
    coeff = 2*cos(2*M_PI*normalizedfreq);
    for (i=0; i<N; i++) {
        s = samples[i] + coeff * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev = s;
    }
    power = s_prev2*s_prev2+s_prev*s_prev-coeff*s_prev*s_prev2;
    return power;
}

float goertzel_filter(int8_t samples[], float coeff, unsigned int N)
{
    float s_prev = 0.0;
    float s_prev2 = 0.0;
    float power, s;
    unsigned int i;
    for (i=0; i<N; i++) {
        s = samples[i] + coeff * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev = s;
    }
    power = s_prev2*s_prev2+s_prev*s_prev-coeff*s_prev*s_prev2;
    return power;
}

void window_init(void)
{
	// TODO: setup dynamic allocation

	for(unsigned int i=0;i<WINDOW_SIZE;i++)
	{
		window[i] = 0;
	}
}

void window_add(int8_t sample)
{
	for(uint32_t i=0;i<(WINDOW_SIZE-1);i++)
	{
		window[(WINDOW_SIZE-1)-i] = window[(WINDOW_SIZE-2)-i];
	}
	window[0] = sample;
}

int8_t* window_get(void)
{
	return window;
}

unsigned int window_get_size(void)
{
	return sizeof(window);
}