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

#include "typetools.h"

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

#define MAX_FILT_LEN	128

float mem[MAX_FILT_LEN];
float square[MAX_FILT_LEN];

int rssi_level(int *pint, int length)
{
	int i,r;
	int a,b;
	int acc_lsb, acc_msb;
	long int pow;

	acc_lsb=acc_msb=0;

	for (i=0; i<length; i++)
	{
		pow = pint[i]*pint[i];
		a=pow/100000000;
		b=pow%100000000;

		acc_lsb+=b;
		if (acc_lsb>100000000)
		{
			acc_lsb-=100000000;
			a++;
		}
		acc_msb+=a;
	}

	r=acc_lsb/length+(100000000/length)*acc_msb;


	return(r);
}

int process_input(int len)
{
    int i;
    int nsamples;

    if (enable) {
        nsamples=typeNsamplesArray(datatype,len);

        type2int(input_data,input_i,nsamples,datatype);

        rssi=rssi_level(input_i,nsamples);

        if (rssi>threshold) {
        	gain=sqrtf((float) amplitude*amplitude/rssi);
        	gain=convff(square,mem,avg_len,gain);
        } else {
        	gain=1.0;
        }

        for (i=0;i<nsamples;i++) {
        	input_i[i]=(int) (input_i[i]*gain);
        }

        int2type(input_i,output_data,nsamples,datatype);
    } else {
    	memcpy(output_data,input_data,len);
    }

    /* send throught output */
    SendItf(0,len);

    return 1;
}

/** Run function.
 * @return 1 if ok, 0 if error
 */
int RunCustom()
{
    return 1;
}

int InitCustom()
{
	int i;
	memset(mem,0,sizeof(float)*MAX_FILT_LEN);

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


void ConfigInterfaces()
{
    
}