#include <dsplib.h>


int32_t handleSize = 0;
DSPLIB_kernelHandle handle;
DSPLIB_bufParams2D_t bufParamsIn, bufParamsOut, bufParamsFilterVar;
DSPLIB_bufParams2D_t bufParamsFilterCoeff;
 DSPLIB_bufParams2D_t bufParamsFilterCoeffCn;
DSPLIB_cascadeBiquad_InitArgs kerInitArgs;
bool is_init= false;
int32_t coeffBuffSize = 0;
DSPLIB_STATUS status_coefs = DSPLIB_SUCCESS;
DSPLIB_STATUS status_init = DSPLIB_SUCCESS;
DSPLIB_STATUS status_opt = DSPLIB_SUCCESS;
DSPLIB_STATUS status_init_opt = DSPLIB_SUCCESS;
DSPLIB_STATUS status_exec_opt = DSPLIB_SUCCESS;
float cascadeBiquad_coeff_buff[MAX_EQ_BAND*DSPLIB_NUM_BIQUAD_COEFS] = {0};
float cascadeBiquad_state_buff[MAX_EQ_BAND*DSPLIB_NUM_STATE_VARS_DF1] = {0};



void cascadeBiquadInit()
{
    int eq_band = 15;
    int ch = 1;
        
    handleSize = DSPLIB_cascadeBiquad_getHandleSize(&kerInitArgs);
    handle   = malloc(handleSize);
    kerInitArgs.dataSize    = 64;
	kerInitArgs.numChannels = ch;
	kerInitArgs.numStages   = eq_band;
    kerInitArgs.enableNChCoeffs = 0;
    kerInitArgs.biquadForm        = DSPLIB_BIQUAD_DF1;

    
	//int32_t coeffBuffSize = DSPLIB_cascadeBiquad_get_coefficientsBufferSize(kerInitArgs.numStages);

	

	bufParamsIn.data_type = DSPLIB_FLOAT32;
	bufParamsIn.dim_x     = ch;
    bufParamsIn.dim_y     = 64;
    bufParamsIn.stride_y  = ch*4;

	bufParamsOut.data_type = DSPLIB_FLOAT32;
	bufParamsOut.dim_x     = ch;
    bufParamsOut.dim_y     = 64;
    bufParamsOut.stride_y  = ch*4;


	bufParamsFilterVar.data_type = DSPLIB_FLOAT32;
    bufParamsFilterVar.dim_x      = ch;
	bufParamsFilterVar.dim_y      = eq_band*DSPLIB_NUM_STATE_VARS_DF1; // Five coefficients per stage
    bufParamsFilterVar.stride_y   = ch*4;

    // bufParamsFilterCoeffCn.data_type = DSPLIB_FLOAT32;
    // bufParamsFilterCoeffCn.dim_x     = eq_band * DSPLIB_NUM_BIQUAD_COEFS;
    // bufParamsFilterCoeffCn.dim_y     = 1;
    // bufParamsFilterCoeffCn.stride_y  = bufParamsFilterCoeffCn.dim_x * DSPLIB_sizeof(bufParamsFilterCoeffCn.data_type);

    bufParamsFilterCoeff.data_type = DSPLIB_FLOAT32;
    bufParamsFilterCoeff.dim_x     = eq_band * DSPLIB_NUM_BIQUAD_COEFS;
    bufParamsFilterCoeff.dim_y     = 1;
    bufParamsFilterCoeff.stride_y  = bufParamsFilterCoeff.dim_x * DSPLIB_sizeof(bufParamsFilterCoeff.data_type);


    // Get optimized coefficient buffer descriptor
    status_coefs = DSPLIB_cascadeBiquad_get_coefficientsBufferDesc(DSPLIB_FLOAT32, 
    eq_band, ch, DSPLIB_BIQUAD_DF1, 0, &bufParamsFilterCoeff);
    if (status_coefs != DSPLIB_SUCCESS) {
 
    }
 
}


void filterInit()
{
	
	cascadeBiquadInit();
	for(i=0; i<15; i++)
	{
		for (j=0; j<5; j++) {
			//cascadeBiquad_coeff_buff[i*5+j] = eq->pool.filters[i].coeffs[j];
			cascadeBiquad_coeff_buff[i*5+j] = 0;
		}
		
	}
	kerInitArgs.funcStyle = DSPLIB_FUNCTION_OPTIMIZED;
	status_init = DSPLIB_cascadeBiquad_init_checkParams(handle, &bufParamsIn, &bufParamsFilterCoeff,&bufParamsFilterVar, &bufParamsOut, &kerInitArgs);
	if (status_init_opt == DSPLIB_SUCCESS) {
		status_init_opt = DSPLIB_cascadeBiquad_init(handle, &bufParamsIn, &bufParamsFilterCoeff, &bufParamsFilterVar,
														&bufParamsOut, &kerInitArgs);
	}
}

void filter_process( float * in_data, float *restrict out_data,)
{
	if (status_init_opt == DSPLIB_SUCCESS) 
    {
        status_exec_opt = DSPLIB_cascadeBiquad_exec_checkParams(handle, in_data, cascadeBiquad_coeff_buff, cascadeBiquad_state_buff, out_data);
        if (status_exec_opt == DSPLIB_SUCCESS) {
            status_exec_opt = DSPLIB_cascadeBiquad_exec(handle, in_data, cascadeBiquad_coeff_buff, cascadeBiquad_state_buff, out_data);
            if (status_exec_opt != DSPLIB_SUCCESS) {
                return;
            }
        
        }
    }
}