#include "fir_filter.h"

void init_fir_buffer(FIR_XN_BUFFER *input_buffer, float *coefficents_buffer) // Inicilaizace kruhového buffer pro FIR filtr
{
    uint16_t i;
    input_buffer->offset = 0; // Ukazatel
    input_buffer->offset_read_delay = (NUM_OF_TAPS - 1) / 2 + 1; // Ukazatel na zpožděné vzorky
    input_buffer->fir_buffer_address = coefficents_buffer; // Adresa na pole koeficentů FIR filtru
    for (i = 0; i < NUM_OF_TAPS; i++) // Vynulování buffer pro uložené vzorky
    {
        input_buffer->xn[i] = 0;
    }
}

float fir_filter_float(FIR_XN_BUFFER *xn_buffer, float *xn_new)
{
	// Incialiazce lokálních proměných
    float yn = 0; // Výstupní data z FIR filtru
    float *fir_coeff = xn_buffer->fir_buffer_address; // Uložení adresy koeficientů FIR filtru
    float *xn_buffer_offset = xn_buffer->xn + xn_buffer->offset; // Posunutí se na pozici ofesetu (místo pro zápis nového vzroku)
    float *xn_buffer_end;
    float *buffer = xn_buffer->xn; // Ukazatel na buffer s uloženými vzorky

    //Zapsání nového vzorku na novou pozici, na kterou ukazuje ukazatel offset
    *xn_buffer_offset = *xn_new;

    while (xn_buffer_offset >= buffer)
    {
        yn += *(xn_buffer_offset--) * *(fir_coeff++); // Výpočet konvoluce probíhá se vzorky, které leží před offsetem
    }

    xn_buffer_offset = xn_buffer->xn + NUM_OF_TAPS - 1; // Nastavení poslední adresy
    xn_buffer_end = xn_buffer->xn + xn_buffer->offset; // Nastavení místa kde je offset

    while (xn_buffer_end < xn_buffer_offset) // Výpočet konvoluce probíhá se vzorky, které leží za offsetem
    {
        yn += *(xn_buffer_offset--) * *(fir_coeff++);
    }

    if (++xn_buffer->offset == NUM_OF_TAPS) // Pokud je offset až nakoci pole vrací se na začátek, tedy na 0-tý prvek
    {
        xn_buffer->offset = 0;
    }

    return yn;
}

void fir_Hilbert_filter_float(FIR_XN_BUFFER *xn_buffer, float *xn_new, float *y_hilb_output, float *y_delay_out) // Implement FIR filter type III
{
	// Incialiazce lokálních proměných
    float yn = 0;
    float *fir_coeff = xn_buffer->fir_buffer_address; // Uložení adresy koeficientů FIR filtru
    float *xn_buffer_offset = xn_buffer->xn + xn_buffer->offset; // Posunutí se na pozici ofesetu (místo pro zápis nového vzroku)
    float *xn_buffer_end;
    float *xn_buffer_offset_read_xn = xn_buffer->xn + xn_buffer->offset_read_delay; //Buffer se požděními vzorky (zpoždění je dáno FIR filtrem) (N-1)/2

    //Zapsání nového vzorku na novou pozici, na kterou ukazuje ukazatel offset
    *xn_buffer_offset = *xn_new;

    while (xn_buffer_offset >= xn_buffer->xn)
    {
        yn += *(xn_buffer_offset--) * *(fir_coeff++); // Výpočet konvoluce probíhá se vzorky, které leží před offsetem
        xn_buffer_offset--;
        fir_coeff++;
    }

    xn_buffer_offset = xn_buffer->xn + NUM_OF_TAPS - 1; // Nastavení poslední adresy
    xn_buffer_end = xn_buffer->xn + xn_buffer->offset; // Nastavení místa kde je offset

    if (xn_buffer->offset % 2 == 0) // Pokud je offset lichý dojde k jeho dekrementaci
    {
        xn_buffer_offset--;
    }

    while (xn_buffer_end < xn_buffer_offset) // Výpočet konvoluce probíhá se vzorky, které leží za offsetem
    {
        yn += *(xn_buffer_offset--) * *(fir_coeff++);
        xn_buffer_offset--;
        fir_coeff++;
    }

    if (++xn_buffer->offset == NUM_OF_TAPS)
    {
        xn_buffer->offset = 0;
    }

    if (++xn_buffer->offset_read_delay == NUM_OF_TAPS)
    {
        xn_buffer->offset_read_delay = 0;
    }

    *y_hilb_output = yn; // Výstup Hilbertovy transforamce
    *y_delay_out = *xn_buffer_offset_read_xn; // Výstup zpožděných vzorků
}

void line_Hilbert_interpolation(LINE_HILB_APPROX *line, float *yn_new, float *y_hilb_new, float* output_buffer_y, float* output_buffer_hilb, uint16_t length)
{
    uint16_t i = 1;

    float t_y, t_hilb;

    line->new_y = *yn_new; // nový koncový bod
    line->new_hilb = *y_hilb_new;

    *output_buffer_y = line->old_y; // První bod přímky je předcházející vzorek
    *output_buffer_hilb = line->old_hilb;
    t_y = ((line->new_y - line->old_y) / length); // Sklon přímky mezi počátečním a koncovým bodem
    t_hilb = ((line->new_hilb - line->old_hilb) / length); // Sklon přímky mezi počátečním a koncovým bodem


    while (i<length) // Výpočet Lineární interpolace
    {
        *(++output_buffer_y) = line->old_y + i * t_y;
        *(++output_buffer_hilb) = line->old_hilb + i * t_hilb;
        i++;
    }

    line->old_y = line->new_y; // Nový vzorek se stavá prvním počátečním bodem přímky následujcí interpolaci
    line->old_hilb = line->new_hilb;

}



void line_interpolation(LINE_APPROX *line, float *yn_new, float* output_buffer, uint16_t length)
{
    uint16_t i = 1;

    float t_y;

    line->new_y = *yn_new; // nový koncový bod

    *output_buffer = line->old_y; // První bod přímky je předcházející vzorek

    t_y = ((line->new_y - line->old_y) / length); // Sklon přímky mezi počátečním a koncovým bodem

    while (i<length)
    {
        *(++output_buffer) = line->old_y + i * t_y;
        i++;
    }

    line->old_y = line->new_y; // Nový vzorek se stavá prvním počátečním bodem přímky následujcí interpolaci
}

