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

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

/* Public Variables */
int8_t window[WINDOW_SIZE];				// sliding window
float coeff0 = 0, coeff1 = 0;			// goerztel coefficients
float lpf_coef0[5], lpf_coef1[5];		// lpf coefficients
float lpf_delay0[2], lpf_delay1[2];		// lpf delay line
int raw_bit = 0;						// afsk demodulator result


/* Private Functions */
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 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);
}

void dsps_biquad_gen_lpf_f32(float *coeffs, float f, float qFactor)
{
    if (qFactor <= 0.0001) {
        qFactor = 0.0001;
    }
    float Fs = 1;

    float w0 = 2 * M_PI * f / Fs;
    float c = cosf(w0);
    float s = sinf(w0);
    float alpha = s / (2 * qFactor);

    float b0 = (1 - c) / 2;
    float b1 = 1 - c;
    float b2 = b0;
    float a0 = 1 + alpha;
    float a1 = -2 * c;
    float a2 = 1 - alpha;

    coeffs[0] = b0 / a0;
    coeffs[1] = b1 / a0;
    coeffs[2] = b2 / a0;
    coeffs[3] = a1 / a0;
    coeffs[4] = a2 / a0;
    return;
}

void dsps_biquad_f32_ansi(const float *input, float *output, int len, float *coef, float *w)
{
    for (int i = 0 ; i < len ; i++) {
        float d0 = input[i] - coef[3] * w[0] - coef[4] * w[1];
        output[i] = coef[0] * d0 +  coef[1] * w[0] + coef[2] * w[1];
        w[1] = w[0];
        w[0] = d0;
    }
    return;
}

void lpf_init(void)
{
	// generate filter coef
	dsps_biquad_gen_lpf_f32(lpf_coef0, 0.10, 0.54119610);
	dsps_biquad_gen_lpf_f32(lpf_coef1, 0.10, 1.3065630);


	// setup delay line and lpf_input window
	int i;
	for(i=0;i<2;i++)
	{
		lpf_delay0[i] = 0;
		lpf_delay1[i] = 0;
	}
}

float lpf_baseband(float input)
{
	float output, temp;
	dsps_biquad_f32_ansi(&input, &temp, 1, lpf_coef0, lpf_delay0);
	dsps_biquad_f32_ansi(&temp, &output, 1, lpf_coef1, lpf_delay1);

	return output;
}

/* Public Functions */
void afsk_demod_init(uint32_t sample_rate, uint32_t symbol0_freq, uint32_t symbol1_freq)
{
	goertzel_init(symbol0_freq, symbol1_freq, &coeff0, &coeff1);
	window_init();
	lpf_init();
}

#define THRESHOLD 5

uint8_t afsk_demod_add_sample(int8_t sample)
{
	float E_s0, E_s1, lpf_output;
	window_add(sample);
	E_s0 = goertzel_filter(window, coeff0, WINDOW_SIZE);
	E_s1 = goertzel_filter(window, coeff1, WINDOW_SIZE);
	lpf_output = lpf_baseband((E_s0 - E_s1));

	if (lpf_output > THRESHOLD)
	{
		raw_bit = 1;
		//enable_debug_IO(DEBUG_2);

	}
	else if (lpf_output < THRESHOLD)
	{
		raw_bit = 0;
		//disable_debug_IO(DEBUG_2);
	}
	return raw_bit;						// if not above threshold keep old state.
}