#include "mbed.h"
#include "WiflyInterface.h"
#include "Websocket.h"

AnalogIn Ain(p20);
#define NCoef 4                 // NCoef is equal to 4

/* wifly object where:            mbed              - RN-XV
*     - p9 and p10 are for the serial communication - p3, p2
*     - p11 is for the reset pin                    - p5
*     - p26 is for the connection status            - p15
*     - "mbed" is the ssid of the network
*     - "password" is the password
*     - WPA is the security
*/
WiflyInterface wifly(p9, p10, p11, p26, "mbed", "1234567890", WPA);

Ticker s2khz_tick;
 
void s2khz_task(void);          // Declare function
 

float iir(float NewSample) 
{
float ACoef[NCoef+1] = {
    0.98440741971685686000,         // coefficients
    -3.88963077749501010000,
    5.81103181235840700000,
    -3.88963077749501010000,
    0.98440741971685686000
                        };

float BCoef[NCoef+1] = {
    1.00000000000000000000,         // coefficients
    -3.90736054963595820000,
    5.77290552976015550000,
    -3.82150735005017150000,
    0.95654367650114913000
                        };

static float y[NCoef+1]; //output samples
static float x[NCoef+1]; //input samples
int n;

//shift the old samples
for(n=NCoef; n>0; n--) {
   x[n] = x[n-1];
   y[n] = y[n-1];
                        }

//Calculate the new output
x[0] = NewSample;
y[0] = ACoef[0] * x[0];
for(n=1; n<=NCoef; n++)
    y[0] += ACoef[n] * x[n] - BCoef[n] * y[n];

return y[0];            // return new filtered data
}

 

 
void s2khz_task(void)
{
float data_in, data_out, ecg;        // Declaring float variables    
char package[30];
    
data_in=Ain;           
data_out=iir(data_in);      // Retreiving new filtered data 
ecg=data_out;          
sprintf (package, "{\"ecg\":%f}", ecg);
ws.send(package);
}

int main() 
{
    wifly.init();               // use DHCP
    while (!wifly.connect());   // join the network
    
    Websocket ws("ws://sockets.mbed.org:443/ws/ECG/wo");
    ws.connect();
    ws.send("Connected");
    s2khz_tick.attach_us(&s2khz_task,500);      // uses function every 500ms
    while(1);
}