#include "dft.h"
#include <math.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

void DoDFT( float * outbins, float * frequencies, int bins, float * databuffer, int place_in_data_buffer, int size_of_data_buffer, float q )
{
	int i, j;
	for( i = 0; i < bins; i++ )
	{
		float freq = frequencies[i];
		float phi = 0;
		int sampleplace = place_in_data_buffer;
		float advance = 3.14159*2.0/freq;

		float binqtys = 0;
		float binqtyc = 0;

		for( j = 0; j <= freq * q; j++ )
		{
			float sample = databuffer[sampleplace];
			sampleplace = (sampleplace-1+size_of_data_buffer)%size_of_data_buffer;
//printf( "%d\n", sampleplace );
			float sv = sin( phi ) * sample;
			float cv = cos( phi ) * sample;

			binqtys += sv;
			binqtyc += cv;

			phi += advance;
		}

		float amp = sqrtf( binqtys * binqtys + binqtyc * binqtyc );
		outbins[i] = amp / freq / q;
	}
}

void DoDFTQuick( float * outbins, float * frequencies, int bins, const float * databuffer, int place_in_data_buffer, int size_of_data_buffer, float q, float speedup )
{
	int i, j;

	for( i = 0; i < bins; i++ )
	{
		int flirts = 0;

		float freq = frequencies[i];
		float phi = 0;
		int ftq = freq * q; 
		int sampleplace = place_in_data_buffer;
		float advance = 3.14159*2.0/freq;

		float binqtys = 0;
		float binqtyc = 0;

		int skip = floor( ftq / speedup );
		if( skip == 0 ) skip = 1;
		advance *= skip;

		for( j = 0; j <= ftq; j += skip )
		{
			float sample = databuffer[sampleplace];
			sampleplace = (sampleplace-skip+size_of_data_buffer)%size_of_data_buffer;
//printf( "%d\n", sampleplace );
			float sv = sinf( phi ) * sample;
			float cv = cosf( phi ) * sample;

			binqtys += sv;
			binqtyc += cv;

			phi += advance;
			flirts++;
		}

		float amp = sqrtf( binqtys * binqtys + binqtyc * binqtyc );
		outbins[i] = amp / freq / q * skip;
	}
}



////////////////////////////DFT Progressive is for embedded systems, primarily.


static float * gbinqtys;
static float * gbinqtyc;
static float * phis;
static float * gfrequencies;
static float * goutbins;
static float * lastbins;
static float * advances;

static int     gbins;
static float   gq;
static float   gspeedup;

#define PROGIIR .005

void HandleProgressive( float sample )
{
	int i;

	for( i = 0; i < gbins; i++ )
	{
		float thiss = sinf( phis[i] ) * sample;
		float thisc = cosf( phis[i] ) * sample;

		float s = gbinqtys[i] = gbinqtys[i] * (1.-PROGIIR) + thiss * PROGIIR;
		float c = gbinqtyc[i] = gbinqtyc[i] * (1.-PROGIIR) + thisc * PROGIIR;

		phis[i] += advances[i];
		if( phis[i] > 6.283 ) phis[i]-=6.283;

		goutbins[i] = sqrtf( s * s + c * c );
	}
}


void DoDFTProgressive( float * outbins, float * frequencies, int bins, const float * databuffer, int place_in_data_buffer, int size_of_data_buffer, float q, float speedup )
{
	int i;
	static int last_place;

	if( gbins != bins )
	{
		if( gbinqtys ) free( gbinqtys );
		if( gbinqtyc ) free( gbinqtyc );
		if( phis ) free( phis );
		if( lastbins ) free( lastbins );
		if( advances ) free( advances );

		gbinqtys = malloc( sizeof(float)*bins );
		gbinqtyc = malloc( sizeof(float)*bins );
		phis = malloc(  sizeof(float)*bins );
		lastbins = malloc(  sizeof(float)*bins );
		advances = malloc(  sizeof(float)*bins );

		memset( gbinqtys, 0, sizeof(float)*bins );
		memset( gbinqtyc, 0, sizeof(float)*bins );
		memset( phis, 0, sizeof(float)*bins );
		memset( lastbins, 0, sizeof(float)*bins );

	}
	memcpy( outbins, lastbins, sizeof(float)*bins );

	for( i = 0; i < bins; i++ )
	{
		float freq = frequencies[i];
		advances[i] = 3.14159*2.0/freq;
	}

	gbins = bins;
	gfrequencies = frequencies;
	goutbins = outbins;
	gspeedup = speedup;
	gq = q;

	place_in_data_buffer = (place_in_data_buffer+1)%size_of_data_buffer;

	int didrun = 0;
	for( i = last_place; i != place_in_data_buffer; i = (i+1)%size_of_data_buffer )
	{
		float fin = ((float)((int)(databuffer[i] * 127))) / 127.0;  //simulate 8-bit input (it looks FINE!)
		HandleProgressive( fin );
		didrun = 1;
	}
	last_place = place_in_data_buffer;

	if( didrun )
	{
		memcpy( lastbins, outbins, sizeof(float)*bins );
	}

/*	for( i = 0; i < bins; i++ )
	{
		printf( "%0.2f ", outbins[i]*100 );
	}
	printf( "\n" );*/

}







/////////////////////////////INTEGER DFT




//NOTES to self:
//
// Let's say we want to try this on an AVR.
//  24 bins, 5 octaves = 120 bins.
// 20 MHz clock / 4.8k sps = 4096 IPS = 34 clocks per bin = :(
//  We can do two at the same time, this frees us up some 

static uint8_t donefirstrun;
static int8_t sintable[512]; //Actually [sin][cos] pairs.


//LDD instruction on AVR can read with constant offset.  We can set Y to be the place in the buffer, and read with offset.
static uint16_t * datspace;  //(advances,places,isses,icses)

void HandleProgressiveInt( int8_t sample1, int8_t sample2 )
{
	int i;
	int16_t ts, tc;
	int16_t tmp1;
	int8_t s1, c1;
	uint16_t ipl, localipl, adv;

	uint16_t * ds = &datspace[0];
	int8_t * st;
	//Clocks are listed for AVR.

	//Estimated 78 minimum instructions... So for two pairs each... just over 4ksps, theoretical.
	//Running overall at ~2kHz.  With GCC: YUCK! 102 cycles!!!
	for( i = 0; i < gbins; i++ )            //Loop, fixed size = 3 + 2 cycles                       N/A
	{
		//12 cycles MIN
		adv = *(ds++); //Read, indirect from RAM (and increment)  2+2 cycles			4
		ipl = *(ds++); //Read, indirect from RAM (and increment)  2+2 cycles			4

		//13 cycles MIN
		ipl += adv;   				         //Advance, 16bit += 16bit, 1 + 1 cycles                2
		localipl = (ipl>>8)<<1;				//Select upper 8 bits  1 cycles							1   *** AS/IS: 4

		st = &sintable[localipl];
		s1 = *(st++);						//Read s1 component out of table. 2+2    cycles			2
		c1 = *st;							//Read c1 component out of table. 2    cycles			2   *** AS/IS: 4

		ts = (s1 * sample1);				// 8 x 8 multiply signed + copy R1 out. zero MSB ts		2  ->Deferred
		tc = (c1 * sample1);				// 8 x 8 multiply signed + copy R1 out. zero MSB tc   	2  ->Deferred


		//15 cycles MIN
		ipl += adv;   				        //Advance, 16bit += 16bit, 1 + 1 cycles   				2
		localipl = (ipl>>8)<<1;				//Select upper 8 bits  1 cycles							1  *** AS/IS: 4

			// need to load Z with 'sintable' and add localipl										2
		st = &sintable[localipl];
		s1 = *(st++);						//Read s1 component out of table. 2   cycles			2	
		c1 = *st;							//Read c1 component out of table. 2    cycles			2 *** AS/IS: 4

		ts += (s1 * sample2);				// 8 x 8 multiply signed + add R1 out.					3  ->Deferred
		tc += (c1 * sample2);				// 8 x 8 multiply signed + add R1 out.					3  ->Deferred


		//Add TS and TC to the datspace stuff. (24 instructions)
		tmp1 = (*ds);			//Read out, sin component.								4  Accurate.
		tmp1 -= tmp1>>7;					//Subtract from the MSB (with carry)					2 -> 6  AS/IS: 7+7 = 14
		tmp1 += ts>>7;						//Add MSBs with carry									2 -> 6  AS/IS: 6

		*(ds++) = tmp1;			//Store values back										4

		tmp1 = *ds;			//Read out, sin component.								4
		tmp1 -= tmp1>>7;					//Subtract from the MSB (with carry)					2 -> 6 AS/IS: 7+7 = 14
		tmp1 += tc>>7;						//Add MSBs with carry									2 -> 6 AS/IS: 6

		*ds++ = tmp1;			//Store values back										4

		*(ds-3) = ipl;			//Store values back										4 AS/IS: 6

		//AS-IS: 8 loop overhead.
	}
}

void DoDFTProgressiveInteger( float * outbins, float * frequencies, int bins, const float * databuffer, int place_in_data_buffer, int size_of_data_buffer, float q, float speedup )
{
	int i;
	static int last_place;

	if( !donefirstrun )
	{
		donefirstrun = 1;
		for( i = 0; i < 256; i++ )
		{
			sintable[i*2+0] = (int8_t)((sinf( i / 256.0 * 6.283 ) * 127.0));
			sintable[i*2+1] = (int8_t)((cosf( i / 256.0 * 6.283 ) * 127.0));
		}
	}

	if( gbins != bins )
	{
		gbins = bins;
		if( datspace ) free( datspace );
		datspace = malloc( bins * 2 * 4 );
	}

	
	for( i = 0; i < bins; i++ )
	{
		float freq = frequencies[i];
		datspace[i*4] = 65536.0/freq;
	}


	for( i = last_place; i != ( place_in_data_buffer&0xffffe ); i = (i+2)%size_of_data_buffer )
	{
		int8_t ifr1 = (int8_t)( ((databuffer[i+0]) ) * 127 );
		int8_t ifr2 = (int8_t)( ((databuffer[i+1]) ) * 127 );
//		printf( "%d %d\n", i, place_in_data_buffer&0xffffe );
		HandleProgressiveInt( ifr1, ifr2 );
	}

	last_place = place_in_data_buffer&0xfffe;

	//Extract bins.
	for( i = 0; i < bins; i++ )
	{
		int16_t isps = datspace[i*4+2];
		int16_t ispc = datspace[i*4+3];
		int16_t mux = ( (isps/256) * (isps/256)) + ((ispc/256) * (ispc/256));
//		printf( "%d (%d %d)\n", mux, isps, ispc );
		outbins[i] = sqrt( mux )/100.0;
	}
//	printf( "\n");
}














////////////////////////SKIPPY DFT

//Skippy DFT is a very ood one. 



#define OCTAVES  5
#define FIXBPERO 24
#define FIXBINS  (FIXBPERO*OCTAVES)
#define BINCYCLE (1<<OCTAVES)

//NOTES to self:
//
// Let's say we want to try this on an AVR.
//  24 bins, 5 octaves = 120 bins.
// 20 MHz clock / 4.8k sps = 4096 IPS = 34 clocks per bin = :(
//  We can do two at the same time, this frees us up some 

static uint8_t Sdonefirstrun;
static int8_t Ssintable[512]; //Actually [sin][cos] pairs.
static uint16_t Sdatspace[FIXBINS*4];  //(advances,places,isses,icses)

//For 
static uint8_t Sdo_this_octave[BINCYCLE];
static int16_t Saccum_octavebins[OCTAVES];
static uint8_t Swhichoctaveplace;

void HandleProgressiveIntSkippy( int8_t sample1 )
{
	int i;
	int16_t ts, tc;
	int16_t tmp1;
	int8_t s1, c1;
	uint16_t ipl, localipl, adv;

	uint8_t oct = Sdo_this_octave[Swhichoctaveplace];
	Swhichoctaveplace ++;
	Swhichoctaveplace &= BINCYCLE-1;

	if( oct > 128 )
	{
		//Special: This is when we can update everything.

/*		if( (rand()%100) == 0 )
		{
			for( i = 0; i < FIXBINS; i++ )
//				printf( "%0.2f ",goutbins[i]*100 );
				printf( "(%d %d)",Sdatspace[i*4+2], Sdatspace[i*4+3] );
			printf( "\n" );
		} */

		for( i = 0; i < FIXBINS; i++ )
		{
			int16_t isps = Sdatspace[i*4+2];
			int16_t ispc = Sdatspace[i*4+3];
			int16_t mux = ( (isps/256) * (isps/256)) + ((ispc/256) * (ispc/256));
	//		printf( "%d (%d %d)\n", mux, isps, ispc );

			int octave = i / FIXBPERO;
//			mux >>= octave; 
			goutbins[i] = sqrt( mux );
//			goutbins[i]/=100.0;
			goutbins[i]/=100*(1<<octave);
			Sdatspace[i*4+2] -= isps>>5;
			Sdatspace[i*4+3] -= ispc>>5;
		}

	}

	for( i = 0; i < OCTAVES;i++ )
	{
		Saccum_octavebins[i] += sample1;
	}

	uint16_t * ds = &Sdatspace[oct*FIXBPERO*4];
	int8_t * st;

	sample1 = Saccum_octavebins[oct]>>(OCTAVES-oct);
	Saccum_octavebins[oct] = 0;

	for( i = 0; i < FIXBPERO; i++ )            //Loop, fixed size = 3 + 2 cycles                       N/A
	{
		//12 cycles MIN
		adv = *(ds++); //Read, indirect from RAM (and increment)  2+2 cycles			4
		ipl = *(ds++); //Read, indirect from RAM (and increment)  2+2 cycles			4

		//13 cycles MIN
		ipl += adv;   				         //Advance, 16bit += 16bit, 1 + 1 cycles                2
		localipl = (ipl>>8)<<1;				//Select upper 8 bits  1 cycles							1   *** AS/IS: 4

		st = &Ssintable[localipl];
		s1 = *(st++);						//Read s1 component out of table. 2+2    cycles			2
		c1 = *st;							//Read c1 component out of table. 2    cycles			2   *** AS/IS: 4

		ts = (s1 * sample1);				// 8 x 8 multiply signed + copy R1 out. zero MSB ts		2  ->Deferred
		tc = (c1 * sample1);				// 8 x 8 multiply signed + copy R1 out. zero MSB tc   	2  ->Deferred


		//Add TS and TC to the datspace stuff. (24 instructions)
		tmp1 = (*ds);			//Read out, sin component.								4  Accurate.
//		tmp1 -= tmp1>>4;					//Subtract from the MSB (with carry)					2 -> 6  AS/IS: 7+7 = 14
		tmp1 += ts>>3;						//Add MSBs with carry									2 -> 6  AS/IS: 6

		*(ds++) = tmp1;			//Store values back										4

		tmp1 = *ds;			//Read out, sin component.								4
//		tmp1 -= tmp1>>4;					//Subtract from the MSB (with carry)					2 -> 6 AS/IS: 7+7 = 14
		tmp1 += tc>>3;						//Add MSBs with carry									2 -> 6 AS/IS: 6

		*ds++ = tmp1;			//Store values back										4

		*(ds-3) = ipl;			//Store values back										4 AS/IS: 6

		//AS-IS: 8 loop overhead.
	}
}

void DoDFTProgressiveIntegerSkippy( float * outbins, float * frequencies, int bins, const float * databuffer, int place_in_data_buffer, int size_of_data_buffer, float q, float speedup )
{
	static float backupbins[FIXBINS];
	int i, j;
	static int last_place;

//printf( "SKIPPY\n" );

	if( !Sdonefirstrun )
	{
		memset( outbins, 0, bins * sizeof( float ) );
		goutbins = outbins;
		//Sdatspace = malloc(FIXBPERO*OCTAVES*8);
		//memset(Sdatspace,0,FIXBPERO*OCTAVES*8);
		//printf( "MS: %d\n", FIXBPERO*OCTAVES*8);
		Sdonefirstrun = 1;
		for( i = 0; i < 256; i++ )
		{
			Ssintable[i*2+0] = (int8_t)((sinf( i / 256.0 * 6.283 ) * 127.0));
			Ssintable[i*2+1] = (int8_t)((cosf( i / 256.0 * 6.283 ) * 127.0));
		}

		for( i = 0; i < BINCYCLE; i++ )
		{
			// Sdo_this_octave = 
			// 4 3 4 2 4 3 4 ...
			//search for "first" zero

			for( j = 0; j <= OCTAVES; j++ )
			{
				if( ((1<<j) & i) == 0 ) break;
			}
			if( j > OCTAVES )
			{
				fprintf( stderr, "Error: algorithm fault.\n" );
				exit( -1 );
			}
			Sdo_this_octave[i] = OCTAVES-j-1;
		}
	}

	memcpy( outbins, backupbins, FIXBINS*4 );

	if( FIXBINS != bins )
	{
		fprintf( stderr, "Error: Bins was reconfigured.  skippy requires a constant number of bins.\n" );
		return;
	}

	
	for( i = 0; i < bins; i++ )
	{
		float freq = frequencies[(i%FIXBPERO) + (FIXBPERO*(OCTAVES-1))];
		Sdatspace[i*4] = (65536.0/freq);// / oneoveroctave;
	}


	for( i = last_place; i != place_in_data_buffer; i = (i+1)%size_of_data_buffer )
	{
		int8_t ifr1 = (int8_t)( ((databuffer[i]) ) * 127 );
		HandleProgressiveIntSkippy( ifr1 );
		HandleProgressiveIntSkippy( ifr1 );
	}

	last_place = place_in_data_buffer;

	memcpy( backupbins, outbins, FIXBINS*4 );

	//Extract bins.
/*
	for( i = 0; i < bins; i++ )
	{
		int16_t isps = Sdatspace[i*4+2];
		int16_t ispc = Sdatspace[i*4+3];
		int16_t mux = ( (isps/256) * (isps/256)) + ((ispc/256) * (ispc/256));
//		printf( "%d (%d %d)\n", mux, isps, ispc );
		outbins[i] = sqrt( mux )/100.0;
	}
*/

//	printf( "\n");
}