/** 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 data_i[INPUT_MAX_DATA],data_q[INPUT_MAX_DATA];

int rcv_i,rcv_q;

int process_input_i(int len)
{
    rcv_i=len;
    return 1;
}

int process_input_q(int len)
{
    rcv_q=len;
    return 1;
}

double phase=0;

int err_cnt1=0,err_cnt2=0;

float lerr_freq,lerr_phase;

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

    int len;
    int i,j;
    int s;
    float f;

    len=typeNsamplesArray(datatype,rcv_i);

    type2int(input_data_i,data_i,len,datatype);

    if (rcv_q)
        type2int(input_data_q,data_q,len,datatype);
    else if (!rcv_i)
        return 1;

    f=2*PI*fsin+err_freq;

    for (i=0;i<len;i++) {
        phase+=f;
        
        while (phase>2.0*PI)
            phase-=2.0*PI;
        while (phase<-2.0*PI)
            phase+=2.0*PI;

        output[i] = (int) data_i[i]*cos(phase + err_phase);
        if (rcv_q)
            output[i] += (int) data_q[i]*sin(phase);
    }

    int2type(output,output_data,len,datatype);

    SendItf(0,rcv_i);

    rcv_i=0;
    rcv_q=0;

    return 1;
}

int InitCustom()
{
    rcv_i=0;
    rcv_q=0;

    return 1;
}

void ConfigInterfaces(){}