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

#include "rcos.h"

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

#define MAX_TAPS    128

#define FIRST_PEAK


int barker13_rev[13]={1,-1,1,-1,1,1,-1,-1,1,1,1,1,1};
float filter[MAX_TAPS];
int filter_i[MAX_TAPS];
int mem[MAX_SEQ_LEN];

int seq_len;

int input_i[INPUT_MAX_DATA];

int long_in_block=0;
int long_out_block=0;
int max_;

int process_input(int len)
{
    int i;
    int corr,corr_;
    int max,maxpos;
    int started;

    long_in_block=typeNsamplesArray(datatype,len);

    type2int(input_data,input_i,long_in_block,datatype);

    /* copy previous buffer */
    if (long_out_block<frame_size) {
        if (max_<0) {
            for (i=0;i<frame_size-long_out_block;i++) {
                input_i[i]*=-1;
            }
            int2type(input_i,&output_data[typeSizeArray(datatype,long_out_block)],
                     frame_size-long_out_block,datatype);
        } else {
            memcpy(&output_data[typeSizeArray(datatype,long_out_block)],
                   input_data,
                   typeSizeArray(datatype,frame_size-long_out_block));
        }
    }

    type2int(output_data,output_i,MAX_SEQ_LEN,datatype);

    /* send */
    if (start_sample) {
        Log("Found correlation value %d at position %d%s\n",max_,start_sample,
        		(long_out_block==frame_size)?"":" (delayed)");

        SendItf(0,typeSizeArray(datatype,frame_size));
    }

    /* correlate and find maximum */
    start_sample=0;
    max=0;
    maxpos=-1;
    memset(mem,0,seq_len*sizeof(int));

    started=0;

    for (i=0;i<long_in_block;i++) {
    	if (abs(input_i[i])<start_th) {
			started++;
		}
		if (started>start_len || !start_len) {
			corr_=convii(sequence,mem,seq_len,(input_i[i]>0)?1:-1);
			corr=abs(corr_);
			if (i<MAX_SEQ_LEN) {
				correlation[i]=corr_;
			}
#ifdef FIRST_PEAK
            if (corr>max && corr>det_threshold) {
                max=corr;
                max_=corr_;
                maxpos=i;
            }
            if (maxpos>-1 && corr<max) {
                start_sample=maxpos-offset;
                break;
            }
#else
			if (corr>max) {
				max=corr;
				max_=corr_;
				maxpos=i;
				start_sample=maxpos-offset;
			}
#endif
		}
    }

    if (start_sample<0)
        start_sample=0;

    long_out_block=frame_size;

    if (long_out_block+start_sample>long_in_block) {
        long_out_block = long_in_block-start_sample;
    }

    if (max_<0) {
        for (i=start_sample;i<start_sample+long_out_block;i++) {
            input_i[i]*=-1;
        }
        int2type(&input_i[start_sample],output_data,long_out_block,datatype);
    } else {
        memcpy(output_data,&input_data[typeSizeArray(datatype,start_sample)],typeSizeArray(datatype,long_out_block));
    }

    return 1;
}

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

int InitCustom()
{
    int i,j;

    seq_len=13;
    memset(mem,0,sizeof(int)*MAX_SEQ_LEN);
    memset(sequence,0,sizeof(int)*MAX_SEQ_LEN);
    memset(correlation,0,sizeof(int)*MAX_SEQ_LEN);

    /* create sequence */
    for (i=0;i<13;i++) {
        sequence[i]=barker13_rev[i];
    }
    memset(mem,0,sizeof(int)*MAX_SEQ_LEN);
    start_sample=0;
    long_out_block=frame_size;
    return 1;
}


void ConfigInterfaces(){}