#include <xc.h>
#include "stdint.h"
#include "stdio.h"
#include "mcc_generated_files/mcc.h"

#define FILTERTAPS 15
float test = 1;
uint16_t CH1 = 0;
uint16_t CH2 = 0;
uint16_t CH2int = 0;
float CH2fir = 0;
float CH2float = 0;
float values[FILTERTAPS] = {0, 0, 0, 0, 0,0,0,0,0,0,0,0,0,0,0};
float input = 1; // without a real input, looking at the step respons (input at unity, 1) would be nice to see
float output = 0; // output as a 0, but that doesn't really matter
uint8_t n = 0;
float fir(float in){
     static uint8_t k;
     uint8_t i = 0;
     float out = 0;
     values[k] = in;

     // declare variables for coefficients
    const float coef[FILTERTAPS] = { 0.00469777274253568,  0.0101883383139945,  0.0260326830879377,  0.0536287086268619,  0.0894202133696346,
        0.125555180991128, 0.152477534229966, 0.162440811786503, 0.152477534229966, 0.125555180991128, 0.0894202133696346,
        0.0536287086268619,  0.0260326830879377,  0.0101883383139945,  0.00469777274253568};


     //wzmocnienie
     float gain = 0.1; // set to 1 and input unity to see what this needs to be


     for (i=0; i<FILTERTAPS; i++) {
           out += coef[i] * values[(i + k) % FILTERTAPS];

       }
     out /= gain;

     k = (k+1) % FILTERTAPS;

       return out;

}
void main(void)
{
    // initialize the device
    SYSTEM_Initialize();

    // When using interrupts, you need to set the Global and Peripheral Interrupt Enable bits
    // Use the following macros to:

    // Enable the Global Interrupts
    //INTERRUPT_GlobalInterruptEnable();

    // Enable the Peripheral Interrupts
    //INTERRUPT_PeripheralInterruptEnable();

    // Disable the Global Interrupts
    //INTERRUPT_GlobalInterruptDisable();

    // Disable the Peripheral Interrupts
    //INTERRUPT_PeripheralInterruptDisable();

    while (1)
    {
        ADC_Initialize();
        // Add your application code
        CH1 = ADC_GetConversion(IN2); //sygnal po diodzie
        CH2 = ADC_GetConversion(IN); //wyjscie ukladu
        CH2float = (float)CH2;
        CH2fir = fir(CH2float);
        CH2int = (uint16_t) CH2fir;
        printf("%d %d\r\n",CH1,CH2int);
        __delay_ms(5); //200Hz

}
}