#include <stdio.h>
#include <config.h>
#include <stdlib.h>
#include <string.h>
#ifdef __ADSP_21489
#include <sysreg.h>
#endif
#include <config.h>
#include "utility/biquad.h"
#include "jitter.h"
#include "eq.h"
#include "alg.h"

/*
*/

struct tag_jitter_mem {
	int size;
	int elem_count;
	int w_offset;
	int r_offset;
	float coeffs[5];
	float state[3];
	float dataf[4];
};


          
jitter_mem* jitter_create(int size)
{
	float b[3],a[3];
	jitter_mem* pjitter = malloc(size+16);
	if (pjitter == NULL) return NULL;

	memset(pjitter, 0 ,size+16);
	pjitter->size = size;
	
	callEQcoeffs(eq_lpf, 18000,0, 0.717, 48000, b,a);
	pjitter->coeffs[0] = b[0]/a[0];
	pjitter->coeffs[1] = b[1]/a[0];
	pjitter->coeffs[2] =  b[2]/a[0];
	pjitter->coeffs[3] = a[1]/a[0];
	pjitter->coeffs[4] =  a[2]/a[0];
	
	return pjitter;
}


int jitter_write(jitter_mem* pjitter, const float* data, int num)
{
#ifdef __ADSP_21489
	int enabled = sysreg_bit_tst(sysreg_MODE1, IRPTEN);
#endif
	if (pjitter == NULL) return 0;

#ifdef __ADSP_21489	
	if (enabled){
		sysreg_bit_clr(sysreg_MODE1, IRPTEN); // Disable
	}
#endif
	memcpy(pjitter->dataf+ pjitter->w_offset, data, num);
	pjitter->w_offset += num;
	if (pjitter->w_offset >= pjitter->size) {
		pjitter->w_offset = 0;
	}
	pjitter->elem_count += num;
	if(pjitter->elem_count >= pjitter->size){
		pjitter->elem_count = pjitter->size;	
	}
#ifdef __ADSP_21489
	if (enabled){
		sysreg_bit_set(sysreg_MODE1, IRPTEN);// Re-enable interrupts
	}
#endif
	return num;
}
//p2 ,p1, x,  r1
static float est_value(float p2,float p1, float r1)
{
	float est=0;
	float d1 = p1-p2;
	float d2 = r1-p1;
	float d3 = r1-p2;
	
	if(d2 > 0 && d1 > 0){
		est = p1 + (r1 - p1)*0.5;		
	}
	else if(d2 < 0 && d1 < 0){
		est = p1 + (r1 - p1)*0.5;	
	}
	else if(d2 > 0 && d1 < 0){
		if(d3 > 0){
			est = p1 + (r1 - p1)*0.5;
		}
		else{
			est = p1 + (p1 - p2);
		}	
	}
	else if(d2 < 0 && d1 > 0){
		if(d3 > 0){
			est = p1 + (p1 - p2);
		}
		else{
			est =  p1 + (r1 - p1) * 0.5;
		}	
	}
	
	if(est >1.0) est =1.0;
	else if(est <-1.0) est =-1.0;
	
	return est;
}

int jitter_read(jitter_mem* pjitter,  float* data, int num)
{
	float percent = pjitter->elem_count*1.0 / pjitter->size;
	float pcm[128];
	int i, size =0;
	int flag = 0;
	int delta;
#ifdef __ADSP_21489
	int enabled = sysreg_bit_tst(sysreg_MODE1, IRPTEN);
#endif	
	
	if (pjitter == NULL || pjitter->elem_count <= 0) {
		memset(data, 0 ,num);	
		return 0;
	}
#ifdef __ADSP_21489	
	if (enabled){
		sysreg_bit_clr(sysreg_MODE1, IRPTEN); // Disable
	}
#endif	
	if (percent >= 0.875) {
		//¶Á2¸öµã¶ª1¸öµã,2/3
		size = (int)(num * 1.5);
		flag = 1;
	}
	else if (percent>=0.75 && percent < 0.875) {
		//¶Á1¸öµã¶ª1¸öµã
		size = num *2 ;
		flag = 2;
	}
	else if (percent>0.125 && percent <= 0.25) {
		//¶Á2¸öµã²åÈë1¸öµã
		size = (int)(num * 2.0 / 3 + 0.5);
		flag = 3;
	}
	else if(percent <= 0.125 ){
		//¶Á1¸öµã²å1¸öµã
		size = num / 2;
		flag = 4;
	}
	else {
		size = num ;
		flag = 0;
	}
	
	memset(data, 0 ,num);
	

	delta = pjitter->size - pjitter->r_offset;
	if (delta >= size) {
		memcpy(pcm, pjitter->dataf + pjitter->r_offset, size);
		pjitter->r_offset += size;
		if (pjitter->r_offset >= pjitter->size) {
			pjitter->r_offset = 0;
		}
	}
	else {
		memcpy(pcm, pjitter->dataf + pjitter->r_offset, delta);
		memcpy(pcm + delta, pjitter->dataf, size - delta);
		pjitter->r_offset = size - delta;
	}
	pjitter->elem_count -= size;

	if (flag == 2) {
		int j=0;
		for (i = 0; i < size; i+=2) {//128
			data[j++] = pcm[i];
		}
	}
	else if (flag == 4) {
		int j=0;
		for (i = 0; i < size; i++) {//32
			data[j++] = pcm[i];
			data[j++] = pcm[i];
		}
	}
	else if (flag == 1) 
	{
		int j=0;
		for (i = 0; i < size; i+=3) {//96
			data[j++] = pcm[i];
			data[j++] = pcm[i+1];
		}
	}
	else if (flag == 3){
		int j=0;
		int len = size/2*2;
		
		for (i = 0; i < len; i+=2) {//42
			data[j++] = pcm[i];
			data[j++] = pcm[i+1];
			data[j++] = est_value(pcm[i],pcm[i+1],pcm[i+2]);
		}
		data[j] = pcm[len];
	}
	else {
		memcpy(data, pcm, size);
	}

	biquadd_vec(data , data ,pjitter->coeffs,pjitter->state, num, 1);

#ifdef __ADSP_21489	
	if (enabled){
		sysreg_bit_set(sysreg_MODE1, IRPTEN);// Re-enable interrupts
	}
#endif
	return num;
}