#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "jitter.h"
#include "./drv/hmem.h"
/*
*/
struct tag_jitter_mem {
	int size;
	int elem_count;
	int w_offset;
	int r_offset;
	float dataf[4];
};

#define _UPPER_LIMIT 0.75
#define _LOWER_LIMIT 0.25

jitter_mem* jitter_create(int size)
{
	jitter_mem* pjitter = mymalloc(SRAM_AUTO, size+16);
	if (pjitter == NULL) return NULL;

	memset(pjitter, 0 ,sizeof(jitter_mem));
	pjitter->size = size;
	return pjitter;
}

int jitter_write(jitter_mem* pjitter, const float* data, int num)
{
	if (pjitter == NULL) return 0;
	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;
	return num;
}

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;

	if (pjitter == NULL) return 0;

	if (percent >= _UPPER_LIMIT) {
		//x2
		size = num * 2;
		flag = 1;
	}
	else if (percent <= _LOWER_LIMIT) {
		//1/2
		size = num / 2;
		flag = 2;
	}
	else {
		size = num ;
		flag = 0;
	}

	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 == 1) {
		for (i = 0; i < size/2; i++) {//128
			data[i] = pcm[2*i];
		}
	}
	else if (flag == 2) {
		for (i = 0; i < size; i++) {//32
			data[2 * i] = pcm[i];
			data[2 * i+1] = pcm[i];
		}
	}
	else {
		memcpy(data, pcm, size);
	}

	return num;
}