/* ==================================================================== *\
 *       Input driver for capturing data from waveform audio device     *
 * 
\* ==================================================================== */

#include <v_lib.h>

/*
 * These two structs exposed to client
 */

typedef struct databuf_s
{
	vec_t	*left_data;			/* left-channel in time domain */
	vec_t	*right_data;		/* right-channel in time domain */
	vec_t	*left_spectrum;		/* left-channel in frequency domain */
	vec_t	*right_spectrum;	/* right-channel in frequency domain */
}	databuf_t, *databuf_p;

typedef struct minmaxinfo_s
{
	vec2_t	ld_minmax;			/* output range for 'left_data' */
	vec2_t	rd_minmax;			/* output range for 'right_data' */
	vec2_t	ls_minmax;			/* output range for 'left_spectrum' */
	vec2_t	rs_minmax;			/* output range for 'right_spectrum' */
}	minmaxinfo_t, *minmaxinfo_p;

/*
 * The rest is driver-specific.
 */

/*
 * Same as 'databuf_t', with two extra arrays to hold imaginary part
 * of FFT - transformed data.
 */
typedef struct tmpbuf_s
{
	vec_t	*left_data;
	vec_t	*right_data;
	vec_t	*left_spectrum[2];  /* both real and imaginary */
	vec_t	*right_spectrum[2]; /* both real and imaginary */
}	tmpbuf_t, *tmpbuf_p;

typedef struct xforms_s
{
	vec2_t	ld_st;	/* scale and translate to apply to 'left_data' */
	vec2_t	rd_st;	/* scale and translate to apply to 'right_data' */
	vec2_t	ls_st;	/* scale and translate to apply to 'left_spectrum' */
	vec2_t	rs_st;	/* scale and translate to apply to 'right_spectrum' */
}	xforms_t, *xforms_p;

/*
 * Global data -----------------------------------------------------------
 */
vboolean	processing;			/* == true after drvBeginProcessing() */
vhandle_t	cmd_scanner;
vhandle_t	timer;				/* timer for timedomain ranges */
vec_t		block_float_timediff;	/* time per block */
vec_t		curtime;			/* current time in seconds */
vec_t		lastblock_float_time;	/* time of last block arrival */
u32_t		numchannels;		/* mono / stereo */
u32_t		frequency;			/* sampling frequency */
u32_t		blocksize;			/* number of samples in block */
u32_t		size_power;			/* log2(blocksize) */
vec_t		*zero_array;		/* for fft; GET RID OF IT */
u32_t		rdata_size;			/* size of block in bytes */
s16_t		*raw_data;			/* raw data from dsp */
/*
 * 'tmpbuf' holds data being processed
 */
tmpbuf_t	tmpbuf = {NULL,NULL,{NULL,NULL},{NULL,NULL}};
/*
 * client-visible 'outbuf' holds copy of procesecced data
 */
databuf_t	outbuf = {NULL,NULL,NULL,NULL};
/*
 * Command - related data
 */
vec_t	SignalScale = 1;

/*
 * End of global data ----------------------------------------------------
 */

/*
 * VERY unoptimized power spectrum estimator.
 * Instead these, we have to use nspsRealFftl().
 */

#define	HYP2(x,y)	((x)*(x) + (y)*(y))

V_LOCAL vec_t
nearest_less_power_2(vec_t in)
{
	return (vec_t)pow(2,floor(log(in) / log(2)));
}

V_LOCAL void
fftPower(vec_t *re_in,vec_t *im_in,vec_t *out)
{
	vec_t scale = (vec_t)1.0 / (vec_t)blocksize;
	u32_t i, n = blocksize >> 1;
	scale *= scale; /* 1/blocksize^2 */
	out[0] = scale*HYP2(re_in[0],im_in[0]);
	out[n] = scale*HYP2(re_in[n],im_in[n]);
	scale *= 2;
	for(i=1;i<n;i++)
		out[i]=scale*(HYP2(re_in[i],im_in[i]) + 
			HYP2(re_in[blocksize-i],im_in[blocksize-i]));
}

V_LOCAL void
autoscale_spectrum(vec_t *spectrum)
{
	u32_t i, len = blocksize >> 1;
	vec_t min = 0, max = 0, scale;
	for(i=0;i<len;i++)
	{
		if(spectrum[i] < min)
			min = spectrum[i];
		if(spectrum[i] > max)
			max = spectrum[i];
	}
	if(IsZero(max - min))
		return;
	scale = 1 / (max - min);
	scale = nearest_less_power_2(scale);
	for(i=0;i<len;i++)
		spectrum[i] *= scale;
}

V_LOCAL	void
calc_spectrum(void)
{
	fftCalcNip(tmpbuf.left_spectrum[0],tmpbuf.left_spectrum[1],
		tmpbuf.left_data,zero_array,size_power,1);
	fftPower(tmpbuf.left_spectrum[0],tmpbuf.left_spectrum[1],
		tmpbuf.left_spectrum[0]);
	autoscale_spectrum(tmpbuf.left_spectrum[0]);
	if(numchannels == 2)
	{
		fftCalcNip(tmpbuf.right_spectrum[0],tmpbuf.right_spectrum[1],
			tmpbuf.right_data,zero_array,size_power,1);
		fftPower(tmpbuf.right_spectrum[0],tmpbuf.right_spectrum[1],
			tmpbuf.right_spectrum[0]);
		autoscale_spectrum(tmpbuf.right_spectrum[0]);
	}
}

V_LOCAL void xform_from_range(vec2_t st,vec_t in_min,vec_t in_max,vec2_t r_out)
{
	st[0] = (r_out[1]-r_out[0]) / (in_max-in_min);
	st[1] = r_out[0] - st[0] * in_min;
}

V_LOCAL void xforms_from_ranges(xforms_t *xfs,minmaxinfo_t *mmi)
{
	xform_from_range(xfs->ld_st,1,-1,mmi->ld_minmax);
	xform_from_range(xfs->ls_st,1,0,mmi->ls_minmax);
	if(numchannels==2)
	{
		xform_from_range(xfs->rd_st,1,-1,mmi->rd_minmax);
		xform_from_range(xfs->rs_st,1,0,mmi->rs_minmax);
	}
}

V_LOCAL void xform_tmpbuf_to_outbuf(xforms_t *xfs)
{
	u32_t i, slim = blocksize >> 1;
	vec_t *ld_st = &xfs->ld_st[0];
	vec_t *ls_st = &xfs->ls_st[0];
	vec_t *rd_st = &xfs->rd_st[0];
	vec_t *rs_st = &xfs->rs_st[0];
	for(i=0;i<blocksize;i++)
		outbuf.left_data[i] = 
			tmpbuf.left_data[i] * ld_st[0] + ld_st[1];
	for(i=0;i<slim;i++)
		outbuf.left_spectrum[i] = 
			tmpbuf.left_spectrum[0][i] * ls_st[0] + ls_st[1];
	if(numchannels==2)
	{
		for(i=0;i<blocksize;i++)
			outbuf.right_data[i] = 
				tmpbuf.right_data[i] * rd_st[0] + rd_st[1];
		for(i=0;i<slim;i++)
			outbuf.right_spectrum[i] = 
				tmpbuf.right_spectrum[0][i] * rs_st[0] + rs_st[1];
	}
}

V_LOCAL void
drvHandleCommand(char_p cmd_string)
{
	char_p cmd;
	tokRestart(cmd_scanner,cmd_string,NULL);
	cmd=tokNext(cmd_scanner);
	if(!cmd)
		return;
	if(!strcmp(cmd,"set_sigscale"))
	{
		vec_t scale;
		/* one argument: float scale */
		if(!tokNext(cmd_scanner))
		{
			conlPuts(LOG_ERROR,"DRVCMD 'set_scale' requires one float argument.\n");
			return;
		}
		scale = (vec_t)atof(tokLast(cmd_scanner));
		if(scale < 1 || scale > 1e6)
		{
			conlPrintf(LOG_ERROR,"DRVCMD 'set_scale': scale '%g' out of range.\n",scale);
			return;
		}
		SignalScale = scale;
		conlPrintf(LOG_TRACE,"DRVCMD set_scale %g\n",SignalScale);
	} else
		conlPrintf(LOG_WARNING,"drvHandleCommand(): unknown command '%s'.\n",cmd);
}

vboolean V_GLOBAL
drvBeginProcessing(u32_t bsize,u32_t nchannels,u32_t freq,
  void (V_CALL *on_progress)(void *),void *arg)
{
	if(processing)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"drvBeginProcessing(): already started.\n");
#endif	/* CHECKED_BUILD */
		return false;
	}
	if(!IS_POWER_TWO(blocksize))
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"drvBeginProcessing(): "
			"expected power of 2, got '%u'.\n",blocksize);
#endif	/* CHECKED_BUILD */
		return false;
	}
	blocksize = bsize;
	numchannels = nchannels;
	frequency = freq;
	if(!waveCaptureInit(numchannels,freq,16,on_progress,arg))
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"drvBeginProcessing(): "
			"unable to waveCaptureInit().\n");
#endif	/* CHECKED_BUILD */
		return false;
	}
	for(size_power=0,bsize=1;bsize < blocksize;size_power++,bsize <<= 1);
	/* allocate memory for output buffers */
	rdata_size = bsize * nchannels * sizeof(s16_t);
	raw_data = (s16_t*)memAlloc(rdata_size);
	outbuf.left_data = (vec_t*)memAlloc(blocksize * sizeof(vec_t));
	outbuf.left_spectrum = (vec_t*)memAlloc(blocksize * sizeof(vec_t));
	outbuf.right_data = (vec_t*)memAlloc(blocksize * sizeof(vec_t));
	outbuf.right_spectrum = (vec_t*)memAlloc(blocksize * sizeof(vec_t));
	tmpbuf.left_data = (vec_t*)memAlloc(blocksize * sizeof(vec_t));
	tmpbuf.left_spectrum[0] = (vec_t*)memAlloc(blocksize * sizeof(vec_t));
	tmpbuf.left_spectrum[1] = (vec_t*)memAlloc(blocksize * sizeof(vec_t));
	tmpbuf.right_data = (vec_t*)memAlloc(blocksize * sizeof(vec_t));
	tmpbuf.right_spectrum[0] = (vec_t*)memAlloc(blocksize * sizeof(vec_t));
	tmpbuf.right_spectrum[1] = (vec_t*)memAlloc(blocksize * sizeof(vec_t));
	zero_array = (vec_t*)memAlloc(blocksize * sizeof(vec_t));
	memZero(zero_array,blocksize * sizeof(vec_t));
	block_float_timediff = (vec_t)blocksize / (vec_t)freq;
	curtime = lastblock_float_time = 0;
	cmd_scanner = tokParseString("dummy","");
	timer=timerCreate();
	waveCaptureResume();
	processing=true;
	return true;
}

void V_GLOBAL
drvEndProcessing(void)
{
	if(!processing)
		return;
	processing = false;
	waveCapturePause();
	waveCaptureShutdown();
	memFree(raw_data);
	memFree(outbuf.left_data);
	memFree(outbuf.left_spectrum);
	memFree(outbuf.right_data);
	memFree(outbuf.right_spectrum);
	memFree(tmpbuf.left_data);
	memFree(tmpbuf.left_spectrum[0]);
	memFree(tmpbuf.left_spectrum[1]);
	memFree(tmpbuf.right_data);
	memFree(tmpbuf.right_spectrum[0]);
	memFree(tmpbuf.right_spectrum[1]);
	memFree(zero_array);
	timerDestroy(timer);
	tokDestroy(cmd_scanner);
#ifdef	CHECKED_BUILD
	memPrintStats();
#endif /* CHECKED_BUILD */
	return;
}

#if 1

databuf_p V_GLOBAL
drvGetData(minmaxinfo_t *mmi)
{
	u32_t i,len;
	s16_t *ptr;
	xforms_t xfs;
	vec_t sample, scale = SignalScale / (vec_t)MAXSHORT;
	if(!processing)
		return NULL;
	/* retrieve data, set input time */
	len = waveCaptureLength();
	if(len < rdata_size)
		return NULL;
	curtime += timerDiffTime(timer);
	lastblock_float_time = 
		curtime - block_float_timediff * (vec_t)len / (vec_t)rdata_size;
	if(waveCaptureGet(raw_data,rdata_size) != rdata_size)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"drvGetData(): waveCaptureGet() failed.\n");
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	/* first pass: process data clamping it to -1 .. 1 */
	ptr=raw_data;
	if(numchannels==1)
	{
		for(i=0;i<blocksize;i++)
		{
			sample = *(ptr++) * scale;
			if(sample > 1)
				sample = 1;
			else
				if(sample<-1)
					sample = -1;
			tmpbuf.left_data[i] = sample;
		}
	}
	else
	{
		for(i=0;i<blocksize;i++)
		{
			sample = *(ptr++) * scale;
			if(sample > 1)
				sample = 1;
			else
				if(sample<-1)
					sample = -1;
			tmpbuf.left_data[i] = sample;
			sample = *(ptr++) * scale;
			if(sample > 1)
				sample = 1;
			else
				if(sample<-1)
					sample = -1;
			tmpbuf.right_data[i] = sample;
		}
	}
	calc_spectrum();
	/* second pass: translate to desired range */
	xforms_from_ranges(&xfs,mmi);
	xform_tmpbuf_to_outbuf(&xfs);
	/* pass pointer to client */
	return &outbuf;
}

#else

/*
 * TODO1: move to 'in_dummy.dll'
 * TODO2: use NSP for tone generation, if available
 * TODO3: tabulate sine, if NSP not available
 */

#define	NUM_TONES	3
vec_t	tones[NUM_TONES] = {100,500,5000};
vec_t	phases[NUM_TONES] = {0,0,0};

V_LOCAL	void
generate_tones(
	vec_t *outbuf,		/* output buffer */
	u32_t outbuf_size,	/* number of samples in ouput buffer */
	u32_t num_tones,	/* numver of sine waves to sum */
	vec_t tones[],		/* frequencies of tones */
	vec_t phases[])		/* current tone phases */
{
#define	MAX_TONES	1024
	u32_t s,c;
	vec_t out;
	vec_t phase[MAX_TONES];
	vec_t dph[MAX_TONES];
	V_ASSERT(MAX_TONES >= NUM_TONES);
	for(c=0;c<num_tones;c++)
	{
		phase[c] = (vec_t)fmod(phases[c],M_2PI);
		dph[c] = M_2PI * tones[c] / (vec_t)frequency; /* global */
	}
	for(s=0;s<outbuf_size;s++)
	{
		out = 0;
		for(c=0;c<num_tones;c++)
		{
			out += (vec_t)sin(phase[c]);
			phase[c] += dph[c];
		}
		/* scale and clamp output signal */
		if(out < -1)
			out = -1;
		else
			if(out > 1)
				out = 1;
		outbuf[s]=out;
	}
	memCopy(phases,phase,num_tones * sizeof(vec_t));
#undef	MAX_TONES
}

databuf_p V_GLOBAL
drvGetData(minmaxinfo_t *mmi)
{
	u32_t len;
	xforms_t xfs;
	if(!processing)
		return NULL;
	drvProcessCommands();
	/* retrieve data, and waste it  */
	len = waveCaptureLength();
	if(len < rdata_size)
		return NULL;
	curtime += timerDiffTime(timer);
	lastblock_float_time = 
		curtime - block_float_timediff * (vec_t)len / (vec_t)rdata_size;
	if(waveCaptureGet(raw_data,rdata_size) != rdata_size)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"drvGetData(): waveCaptureGet() failed.\n");
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	/* first pass: generate tones */
	generate_tones(tmpbuf.left_data,blocksize,NUM_TONES,tones,phases);
	if(numchannels==2)
		generate_tones(tmpbuf.right_data,blocksize,NUM_TONES,tones,phases);
	calc_spectrum();
	/* second pass: translate to desired range */
	xforms_from_ranges(&xfs,mmi);
	xform_tmpbuf_to_outbuf(&xfs);
	/* pass pointer to client */
	return &outbuf;
}

#endif

vboolean V_GLOBAL
drvGetRanges(vec2_t timedomain_ranges,vec2_t freqdomain_ranges)
{
	if(!processing)
		return false;
	vec2Set(timedomain_ranges,lastblock_float_time,
		lastblock_float_time+block_float_timediff);
	vec2Set(freqdomain_ranges,0,(vec_t)(frequency >> 1));
	return true;
}

void V_GLOBAL
drvCommand(char_p cmd)
{
	drvHandleCommand(cmd);
}

#ifndef	__IN_WAVE_DLL__

/*
 * Debug section
 */

int main()
{
	vec2_t trange, frange;
	int count=0;
	databuf_t *result;
	minmaxinfo_t mmi;
	logStartConsoleLog();
	logUpperLevel(LOG_TRACE);
	vec2Set(mmi.ld_minmax,10,90);
	vec2Set(mmi.rd_minmax,10,90);
	vec2Set(mmi.ls_minmax,10,90);
	vec2Set(mmi.rs_minmax,10,90);
	if(!drvBeginProcessing(16 * 4096,2,44100,NULL,NULL))
	{
		conPuts("drvBeginProcessing() failed.\n");
		return -1;
	}
	while(count < 50)
	{
		result = drvGetData(&mmi);
		if(result == NULL)
		{
			timeDelay(10);
			continue;
		}
		drvGetRanges(trange,frange);
		conPrintf("Got block %u: timedomain(%f .. %f)\n",count,trange[0],trange[1]);
		count++;
	}
	drvEndProcessing();
	return 0;
}

#endif	/* __IN_WAVE_DLL__ */
