/** ALOE headers
 */
#include <phal_sw_api.h>
#include <math.h>
#include <swapi_utils.h>

#include "inputs.h"
#include "outputs.h"
#include "stats.h"

#include "typetools.h"

#define PI 3.141592654

int input_i[INPUT_MAX_DATA];

#define MAX_FILTER_LEN	128

float square[MAX_FILTER_LEN];
float mem_i[MAX_FILTER_LEN],mem_q[MAX_FILTER_LEN];

double phase=0.0,error=0.0,error_int=0.0;

float y[2]={0.0,0.0};

int process_data(int len)
{
	int nsamples,i;

    nsamples=typeNsamplesArray(datatype,len);

    type2int(input_data,input_i,nsamples,datatype);

    for (i=0;i<nsamples;i++) {

        /* extract i/q */
        int_out_i[i]=(int) input_i[i]*cos(phase + (double) K*error_int);
        int_out_q[i]=(int) -input_i[i]*sin(phase + (double) K*error_int);

        error = (float) int_out_q[i]/att_error;

        /* filter error */
        if (alpha) {
        	/** 1-tap IIR */
        	y[0]=y[1];
        	y[1]=error*(1-alpha)+alpha*y[0];
        	error=y[1];
        }

        error_int+=error;

        phase+=2*PI*freq;

        while(phase>2*PI)
        	phase-=2*PI;
        while(phase<-2*PI)
        	phase+=2*PI;

        /* save error and error_int */
        if (i<ERROR_VECT_LEN) {
			error_vect[i]=error;
			error_int_vect[i]=error_int;
        }
    }

    int2type(int_out_i,output_data_i,nsamples,datatype);
    if (isItfConnected(1))
    	int2type(int_out_q,output_data_q,nsamples,datatype);

    SendItf(0,typeSizeArray(datatype,nsamples));
    if (isItfConnected(1))
        SendItf(1,typeSizeArray(datatype,nsamples));

    return 1;
}

int RunCustom()
{
	return 1;
}

int InitCustom()
{
	int i;

    memset(mem_i,0,MAX_FILTER_LEN*sizeof(float));
    memset(mem_q,0,MAX_FILTER_LEN*sizeof(float));

    for (i=0;i<MAX_FILTER_LEN;i++) {
    	square[i]=1.0;
    }
    return 1;
}

void ConfigInterfaces(){}