#include <string.h>
#include <float.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "../hrtf/mysofa.h"
#include "../hrtf/tools.h"
#include "tests.h"
#include "json.h"

#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif

/* #define VDEBUG */

void test_easy() {
	struct MYSOFA_EASY *easy;
	int err = 0;
	int filterlength;
	int filters=0;
	float theta,r;
	float *coordinates;
	float *ir;
	float *delays;
	int count=0;
	FILE *file;
	float c[3];
	float l1,l2;
	float sdiff1, sdiff2, diff1, diff2;

	easy = mysofa_open("share/MIT_KEMAR_normal_pinna.sofa", 8000., &filterlength, &err);
	if (!easy) {
		CU_FAIL_FATAL("Error reading file.");
		return;
	}

	mysofa_close(easy);

	easy = mysofa_open("tests/tester.sofa", 48000, &filterlength, &err);
	if (!easy) {
		CU_FAIL_FATAL("Error reading file.");
		return;
	}
	mysofa_minphase(easy->hrtf, 0.01);
	
	for(filters=0;filters<easy->hrtf->M;filters++) {
		c[0]=easy->hrtf->SourcePosition.values[filters*3];
		c[1]=easy->hrtf->SourcePosition.values[filters*3+1];
		c[2]=easy->hrtf->SourcePosition.values[filters*3+2];
		mysofa_c2s(c);
        
#ifdef VDEBUG        
   		printf("in %d %f %f %f # %f %f %f\n",filters,c[0],c[1],c[2],easy->hrtf->SourcePosition.values[filters*3],easy->hrtf->SourcePosition.values[filters*3+1],easy->hrtf->SourcePosition.values[filters*3+2]);
#endif   		
		c[0] = fmod(round(c[0]+360),360);
		c[1] = fmod(round(c[1]+361),360);        
		l1 = round(easy->hrtf->DataDelay.values[filters*2]*48000*2);
		l2 = round(easy->hrtf->DataDelay.values[filters*2+1]*48000*2);

#ifdef VDEBUG        
/*   		printf("compare %d %f %f %f %f %f\n",filters,c[0],c[1],c[2],l1,l2); */
#endif   		
		CU_ASSERT(!((fabs(c[0]-l1)>2 || fabs(c[1]-l2)>2) && !fequals(l2,90)));
	}
    
    
	filters=0; 
	for(theta=-90.;theta<=90.;theta+=5.) {
		r = round(cos(theta*M_PI/180.) * 120.);
		if(r==0.) r=1;
		filters+=r;
	}
#ifdef VDEBUG
	printf("Filters %d\n",filters);
#endif

	coordinates = malloc(filters*sizeof(float)*3);
	ir = malloc(filters*easy->hrtf->N*sizeof(float)*2);
	delays = malloc(filters*2*sizeof(float));

	sdiff1 = sdiff2 = 0;
	err = 0;
	for(theta=-90.;theta<=90.;theta+=5.) {
		int r = round(cos(theta*M_PI/180.) * 120.);
		int phi;
		if(r==0) r=1;
		for(phi=0;phi<r;phi++) {
			coordinates[count*3+0] = phi * (360. / r);
			coordinates[count*3+1] = theta;
			coordinates[count*3+2] = 1;
			mysofa_s2c(coordinates+count*3);
#ifdef VDEBUG
			printf("req %f %f %d %f %f %f\n",phi* (360. / r),theta,count,coordinates[count*3+0],coordinates[count*3+1],coordinates[count*3+2]);
#endif
			mysofa_getfilter_float(easy,
					       coordinates[count*3+0],
					       coordinates[count*3+1],
					       coordinates[count*3+2],
					       ir + 2*count * easy->hrtf->N,
					       ir + (2*count+1) * easy->hrtf->N,
					       &delays[2*count], &delays[2*count+1]);
			diff1 = fabs(phi * (360. / r) - delays[2*count] * 48000 * 2);
			diff2 = fabs(fmod(theta + 360,360) - delays[2*count+1] * 48000 * 2);
			if(diff1 > 5 || diff2 > 5)
				err++;
			else {
				sdiff1 += diff1;
				sdiff2 += diff2;
			}
                
#ifdef VDEBUG
			printf("diff %f %f\t", diff1, diff2);
			printf("delays %f %f %f %f\n", phi * (360. / r), fmod(theta + 360,360), delays[2*count] * 48000 * 2, delays[2*count+1] * 48000 * 2);
#endif
			count++;
			
		}
	}
	err = err*100./count;
	sdiff1 = sdiff1 / (count-err);
	sdiff2 = sdiff2 / (count-err);
	
#ifdef VDEBUG
	printf("errors %f%% diffs %f %f\n", err*100./count, sdiff1 / (count-err), sdiff2 / (count-err));
#endif
	CU_ASSERT(err < 31.7);
	CU_ASSERT(sdiff1 < 1.67);
	CU_ASSERT(sdiff2 < 1.43);

	free(easy->hrtf->DataDelay.values);
	free(easy->hrtf->DataIR.values);
	free(easy->hrtf->SourcePosition.values);
	easy->hrtf->DataDelay.elements=filters*2;
	easy->hrtf->DataDelay.values=delays;
	easy->hrtf->DataIR.elements=filters*2*easy->hrtf->N;
	easy->hrtf->DataIR.values=ir;
	easy->hrtf->SourcePosition.elements=filters*3;
	easy->hrtf->SourcePosition.values=coordinates;
	easy->hrtf->M = filters;

	file = fopen("/tmp/easy.tmp.json","w");
	CU_ASSERT(file!=NULL);
	printJson(file,easy->hrtf);
	fclose(file);
	/* TODO verify correctness of the easy.json file */

	mysofa_close(easy);
}

