/** 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.1415

int input_i[INPUT_MAX_DATA];

float square[8]={1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
float mem_vco[8];

float vco=0;
float error_int=0;

int process_input(int len)
{
    int i;
    int nsamples;
    float error;
    float phase;

    nsamples=typeNsamplesArray(datatype,len);

    type2int(input_data,input_i,nsamples,datatype);

    for (i=0;i<nsamples;i++) {
        /* get phase error */
        error = (float) Kp*input_i[i]*vco;

        /* filter error */
        error = convff(square,mem_vco,8,error);

        error_phase[i]=error;

        /* integrate */
        error_int += error;

        /* get phase */
        phase=(float) 2*PI*i*fsin/freq+Kv*error_int;

        output_phase[i]=phase;

        /* close loop */
        vco = cos(phase);

        vco_v[i]=10000*vco;
    }

    SendItf(0,len);
    SendItf(1,nsamples);

    return 1;
}

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

int InitCustom()
{
    memset(mem_vco,0,8*sizeof(float));
    return 1;
}

void ConfigInterfaces(){}