#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <sys/time.h>

#include <resample.h>

#define AMP 16000
#define I_RATE 48000
#define O_RATE 44100
//#define O_RATE 24000

//#define test_func(x) 1
//#define test_func(x) sin(2*M_PI*(x)*10)
//#define test_func(x) sin(2*M_PI*(x)*(x)*1000)
#define test_func(x) sin(2*M_PI*(x)*(x)*12000)

short i_buf[I_RATE*2*2];
short o_buf[O_RATE*2*2];

static int i_offset;
static int o_offset;

FILE *out;

void test_res1(void);
void test_res2(void);
void test_res3(void);
void test_res4(void);
void test_res5(void);
void test_res6(void);
void test_res7(void);

int main(int argc,char *argv[])
{
	out = fopen("out","w");

	test_res7();

	return 0;
}

void *get_buffer(void *priv, unsigned int size)
{
	void *ret;
	ret = ((void *)o_buf) + o_offset;
	o_offset += size;
	return ret;
}

struct timeval start_time;
void start_timer(void)
{
	gettimeofday(&start_time,NULL);
	//printf("start %ld.%06ld\n",start_time.tv_sec,start_time.tv_usec);
}

void end_timer(void)
{
	struct timeval end_time;
	double diff;

	gettimeofday(&end_time,NULL);
	//printf("end %ld.%06ld\n",end_time.tv_sec,end_time.tv_usec);
	diff = (end_time.tv_sec - start_time.tv_sec) +
		1e-6*(end_time.tv_usec - start_time.tv_usec);

	printf("time %g\n",diff);

}

void test_res1(void)
{
	resample_t *r;
	int i;
	double sum10k,sum22k;
	double f;
	int n10k,n22k;
	double x;

	for(i=0;i<I_RATE;i++){
		i_buf[i*2+0] = rint(AMP * test_func((double)i/I_RATE));
		//i_buf[i*2+1] = rint(AMP * test_func((double)i/I_RATE));
		i_buf[i*2+1] = (i<1000)?AMP:0;
	}

	r = malloc(sizeof(resample_t));
	memset(r,0,sizeof(resample_t));

	r->i_rate = I_RATE;
	r->o_rate = O_RATE;
	//r->method = RESAMPLE_SINC_SLOW;
	r->method = RESAMPLE_SINC;
	r->channels = 2;
	//r->verbose = 1;
	r->filter_length = 64;
	r->get_buffer = get_buffer;

	resample_init(r);

	start_timer();
#define blocked
#ifdef blocked
	for(i=0;i+256<I_RATE;i+=256){
		resample_scale(r,i_buf+i*2,256*2*2);
	}
	if(I_RATE-i){
		resample_scale(r,i_buf+i*2,(I_RATE-i)*2*2);
	}
#else
	resample_scale(r,i_buf,I_RATE*2*2);
#endif
	end_timer();

	for(i=0;i<O_RATE;i++){
		f = AMP*test_func((double)i/O_RATE);
		//f = rint(AMP*test_func((double)i/O_RATE));
		fprintf(out,"%d %d %d %g %g\n",i,
			o_buf[2*i+0],o_buf[2*i+1],
			f,o_buf[2*i+0]-f);
	}

	sum10k=0;
	sum22k=0;
	n10k=0;
	n22k=0;
	for(i=0;i<O_RATE;i++){
		f = AMP*test_func((double)i/O_RATE);
		//f = rint(AMP*test_func((double)i/O_RATE));
		x = o_buf[2*i+0]-f;
		if(((0.5*i)/O_RATE*I_RATE)<10000){
			sum10k += x*x;
			n10k++;
		}
		if(((0.5*i)/O_RATE*I_RATE)<22050){
			sum22k += x*x;
			n22k++;
		}
	}
	printf("average error 10k=%g 22k=%g\n",
		sqrt(sum10k/n10k),
		sqrt(sum22k/n22k));
}


void test_res2(void)
{
	functable_t *t;
	int i;
	double x;
	double f1,f2;

	t = malloc(sizeof(*t));
	memset(t,0,sizeof(*t));

	t->start = -50.0;
	t->offset = 1;
	t->len = 100;

	t->func_x = functable_sinc;
	t->func_dx = functable_dsinc;

	functable_init(t);

	for(i=0;i<1000;i++){
		x = -50.0 + 0.1 * i;
		f1 = functable_sinc(NULL,x);
		f2 = functable_eval(t,x);
		fprintf(out,"%d %g %g %g\n",i,f1,f2,f1-f2);
	}
}

void test_res3(void)
{
	functable_t *t;
	int i;
	double x;
	double f1,f2;
	int n = 1;

	t = malloc(sizeof(*t));
	memset(t,0,sizeof(*t));

	t->start = -50.0;
	t->offset = 1.0 / n;
	t->len = 100 * n;

	t->func_x = functable_sinc;
	t->func_dx = functable_dsinc;

	t->func2_x = functable_window_std;
	t->func2_dx = functable_window_dstd;

	t->scale = 1.0;
	t->scale2 = 1.0 / (M_PI * 16);

	functable_init(t);

	for(i=0;i<1000 * n;i++){
		x = -50.0 + 0.1/n * i;
		f1 = functable_sinc(NULL,t->scale * x) *
			functable_window_std(NULL,t->scale2 * x);
		f2 = functable_eval(t,x);
		fprintf(out,"%d %g %g %g\n",i,f1,f2,f2-f1);
	}
}

double sinc_poly(double x)
{
#define INV3FAC 1.66666666666666666e-1
#define INV5FAC 8.33333333333333333e-3
#define INV7FAC 1.984126984e-4
#define INV9FAC 2.755731922e-6
#define INV11FAC 2.505210839e-8
	double x2 = x * x;

	return 1
		- x2 * INV3FAC
		+ x2 * x2 * INV5FAC
		- x2 * x2 * x2 * INV7FAC;
		//+ x2 * x2 * x2 * x2 * INV9FAC
		//- x2 * x2 * x2 * x2 * x2 * INV11FAC;
}

void test_res4(void)
{
	int i;
	double x,f1,f2;

	for(i=1;i<100;i++){
		x = 0.01 * i;
		f1 = 1 - sin(x)/x;
		f2 = 1 - sinc_poly(x);
	
		fprintf(out,"%g %.20g %.20g %.20g\n",x,f1,f2,f2-f1);
	}
}


void test_res5(void)
{
	int i;
	double sum;

	start_timer();
	sum = 0;
	for(i=0;i<I_RATE;i++){
		sum += i_buf[i*2];
	}
	end_timer();
	i_buf[0] = sum;
}


void short_to_double(double *d,short *x) { *d = *x; }
void short_to_float(float *f,short *x) { *f = *x; }
void float_to_double(double *f,float *x) { *f = *x; }
void double_to_short(short *f,double *x) { *f = *x; }

double res6_tmp[1000];

void test_res6(void)
{
	int i;

	for(i=0;i<I_RATE;i++){
		i_buf[i] = rint(AMP * test_func((double)i/I_RATE));
	}

	conv_double_short_ref(res6_tmp,i_buf,1000);
	for(i=0;i<1000;i++){
		res6_tmp[i] *= 3.0;
	}
	conv_short_double_ppcasm(o_buf,res6_tmp,1000);

	for(i=0;i<1000;i++){
		fprintf(out,"%d %d %g %d\n",i,i_buf[i],res6_tmp[i],o_buf[i]);
	}
}

void test_res7(void)
{
	resample_t *r;
	int i;
	double sum10k,sum22k;
	double f;
	int n10k,n22k;
	double x;

	for(i=0;i<I_RATE;i++){
		i_buf[i] = rint(AMP * test_func((double)i/I_RATE));
	}

	r = malloc(sizeof(resample_t));
	memset(r,0,sizeof(resample_t));

	r->i_rate = I_RATE;
	r->o_rate = O_RATE;
	//r->method = RESAMPLE_SINC_SLOW;
	r->method = RESAMPLE_SINC;
	r->channels = 1;
	//r->verbose = 1;
	r->filter_length = 64;
	r->get_buffer = get_buffer;

	resample_init(r);

	start_timer();
#define blocked
#ifdef blocked
	for(i=0;i+256<I_RATE;i+=256){
		resample_scale(r,i_buf+i,256*2);
	}
	if(I_RATE-i){
		resample_scale(r,i_buf+i,(I_RATE-i)*2);
	}
#else
	resample_scale(r,i_buf,I_RATE*2);
#endif
	end_timer();

	for(i=0;i<O_RATE;i++){
		f = AMP*test_func((double)i/O_RATE);
		//f = rint(AMP*test_func((double)i/O_RATE));
		fprintf(out,"%d %d %d %g %g\n",i,
			o_buf[i],0,
			f,o_buf[i]-f);
	}

	sum10k=0;
	sum22k=0;
	n10k=0;
	n22k=0;
	for(i=0;i<O_RATE;i++){
		f = AMP*test_func((double)i/O_RATE);
		//f = rint(AMP*test_func((double)i/O_RATE));
		x = o_buf[i]-f;
		if(((0.5*i)/O_RATE*I_RATE)<10000){
			sum10k += x*x;
			n10k++;
		}
		if(((0.5*i)/O_RATE*I_RATE)<22050){
			sum22k += x*x;
			n22k++;
		}
	}
	printf("average error 10k=%g 22k=%g\n",
		sqrt(sum10k/n10k),
		sqrt(sum22k/n22k));
}