#include "embeddednf.h"
#include <stdio.h>

uint16_t folded_bins[FIXBPERO];
uint16_t fuzzed_bins[FIXBINS];
uint8_t  note_peak_freqs[MAXNOTES];
uint16_t note_peak_amps[MAXNOTES];
uint16_t note_peak_amps2[MAXNOTES];


static const float bf_table[24] = {
        1.000000, 1.029302, 1.059463, 1.090508, 1.122462, 1.155353, 
        1.189207, 1.224054, 1.259921, 1.296840, 1.334840, 1.373954, 
        1.414214, 1.455653, 1.498307, 1.542211, 1.587401, 1.633915, 
        1.681793, 1.731073, 1.781797, 1.834008, 1.887749, 1.943064 };

/* The above table was generated using the following code:

#include <stdio.h>
#include <math.h>

int main()
{
	int i;
	#define FIXBPERO 24
	printf( "const float bf_table[%d] = {", FIXBPERO );
	for( i = 0; i < FIXBPERO; i++ )
	{
		if( ( i % 6 ) == 0 )
			printf( "\n\t" );
		printf( "%f, ", pow( 2, (float)i / (float)FIXBPERO ) );
	}
	printf( "};\n" );
	return 0;
}
*/


#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))


void UpdateFreqs()
{
	uint16_t fbins[FIXBPERO];
	int i;

	BUILD_BUG_ON( sizeof(bf_table) != FIXBPERO*4 );

	//Warning: This does floating point.  Avoid doing this frequently.  If you
	//absolutely cannot have floating point on your system, you may precompute
	//this and store it as a table.  It does preclude you from changing
	//BASE_FREQ in runtime.

	for( i = 0; i < FIXBPERO; i++ )
	{
		float frq =  ( bf_table[i] * BASE_FREQ );
		fbins[i] = ( 65536.0 ) / ( DFREQ ) * frq * 16;
	}

#ifdef USE_32DFT
	UpdateBins32( fbins );
#else
	UpdateBinsForProgressiveIntegerSkippyInt( fbins );
#endif
}

void Init()
{
	int i;
	//Set up and initialize arrays.
	for( i = 0; i < MAXNOTES; i++ )
	{
		note_peak_freqs[i] = 255;
		note_peak_amps[i] = 0;
		note_peak_amps2[i] = 0;
	}

	for( i = 0; i < FIXBPERO; i++ )
	{
		folded_bins[i] = 0;
	}

	for( i = 0; i < FIXBINS; i++ )
	{
		fuzzed_bins[i] = 0;
	}

	//Step 1: Initialize the Integer DFT.
#ifdef USE_32DFT
	SetupDFTProgressive32();
#else
	SetupDFTProgressiveIntegerSkippy();
#endif

	//Step 2: Set up the frequency list.  You could do this multiple times
	//if you want to change the loadout of the frequencies.
	UpdateFreqs();
}

void HandleFrameInfo()
{
	int i, j, k;

#ifdef USE_32DFT
	uint16_t * strens;
	UpdateOutputBins32();
	strens = embeddedbins32;
#else
	uint16_t * strens = embeddedbins;
#endif

	//Copy out the bins from the DFT to our fuzzed bins.
	for( i = 0; i < FIXBINS; i++ )
	{
		fuzzed_bins[i] = (fuzzed_bins[i] + (strens[i]>>FUZZ_IIR_BITS) -
			(fuzzed_bins[i]>>FUZZ_IIR_BITS));
	}

	//Taper first octave
	for( i = 0; i < FIXBPERO; i++ )
	{
		uint32_t taperamt = (65536 / FIXBPERO) * i;
		fuzzed_bins[i] = (taperamt * fuzzed_bins[i]) >> 16;
	}

	//Taper last octave
	for( i = 0; i < FIXBPERO; i++ )
	{
		int newi = FIXBINS - i - 1;
		uint32_t taperamt = (65536 / FIXBPERO) * i;
		fuzzed_bins[newi] = (taperamt * fuzzed_bins[newi]) >> 16;
	}

	//Fold the bins from fuzzedbins into one octave.
	for( i = 0; i < FIXBPERO; i++ )
		folded_bins[i] = 0;

	k = 0;
	for( j = 0; j < OCTAVES; j++ )
	{
		for( i = 0; i < FIXBPERO; i++ )
		{
			folded_bins[i] += fuzzed_bins[k++];
		}
	}

	//Now, we must blur the folded bins to get a good result.
	//Sometimes you may notice every other bin being out-of
	//line, and this fixes that.  We may consider running this
	//more than once, but in my experience, once is enough.
	{
		//Extra scoping because this is a large on-stack buffer.
		uint16_t folded_out[FIXBPERO];
		uint8_t adjLeft = FIXBPERO-1;
		uint8_t adjRight = 1;
		for( i = 0; i < FIXBPERO; i++ )
		{
			uint16_t lbin = folded_bins[adjLeft]>>2;
			uint16_t rbin = folded_bins[adjRight]>>2;
			uint16_t tbin = folded_bins[i]>>1;
			folded_out[i] = lbin + rbin + tbin;

			//We do this funny dance to avoid a modulus operation.  On some
			//processors, a modulus operation is slow.  This is cheap.
			adjLeft++; if( adjLeft == FIXBPERO ) adjLeft = 0;
			adjRight++; if( adjRight == FIXBPERO ) adjRight = 0;
		}

		for( i = 0; i < FIXBPERO; i++ )
		{
			folded_bins[i] = folded_out[i];
		}
	}

	//Next, we have to find the peaks, this is what "decompose" does in our
	//normal tool.  As a warning, it expects that the values in foolded_bins
	//do NOT exceed 32767.
	{
		uint8_t adjLeft = FIXBPERO-1;
		uint8_t adjRight = 1;
		for( i = 0; i < FIXBPERO; i++ )
		{
			int16_t prev = folded_bins[adjLeft];
			int16_t next = folded_bins[adjRight];
			int16_t this = folded_bins[i];
			uint8_t thisfreq = i<<SEMIBITSPERBIN;
			int16_t offset;
			adjLeft++; if( adjLeft == FIXBPERO ) adjLeft = 0;
			adjRight++; if( adjRight == FIXBPERO ) adjRight = 0;
			if( this < MIN_AMP_FOR_NOTE ) continue;
			if( prev > this || next > this ) continue;
			if( prev == this && next == this ) continue;

			//i is at a peak... 
			int32_t totaldiff = (( this - prev ) + ( this - next ));
			int32_t porpdiffP = ((this-prev)<<16) / totaldiff; //close to 0 =
					//closer to this side, 32768 = in the middle, 65535 away.
			int32_t porpdiffN = ((this-next)<<16) / totaldiff;

			if( porpdiffP < porpdiffN )
			{
				//Closer to prev.
				offset = -(32768 - porpdiffP);
			}
			else
			{
				//Closer to next
				offset = (32768 - porpdiffN);
			}

			//Need to round.  That's what that extra +(15.. is in the center.
			thisfreq += (offset+(1<<(15-SEMIBITSPERBIN)))>>(16-SEMIBITSPERBIN);

			//In the event we went 'below zero' need to wrap to the top.
			if( thisfreq > 255-(1<<SEMIBITSPERBIN) )
				thisfreq = (1<<SEMIBITSPERBIN)*FIXBPERO - (256-thisfreq);

			//printf( "Peak At: %3d /(%2d) %4d/%4d/%4d\n", thisfreq, i,prev, this, next );

			//Okay, we have a peak, and a frequency. Now, we need to search
			//through the existing notes to see if we have any matches.
			//If we have a note that's close enough, we will try to pull it
			//closer to us and boost it.

			int8_t lowest_found_free_note = -1;
			int8_t closest_note_id = -1;
			int16_t closest_note_distance = 32767;
			
			for( j = 0; j < MAXNOTES; j++ )
			{
				uint8_t nf = note_peak_freqs[j];

				if( nf == 255 )
				{
					if( lowest_found_free_note == -1 )
						lowest_found_free_note = j;
					continue;
				}

				int16_t distance = thisfreq - nf;

				if( distance < 0 ) distance = -distance;

				//Make sure that if we've wrapped around the right side of the
				//array, we can detect it and loop it back.
				if( distance > ((1<<(SEMIBITSPERBIN-1))*FIXBPERO) )
				{
					distance = ((1<<(SEMIBITSPERBIN))*FIXBPERO) - distance;
				}

				if( distance < closest_note_distance )
				{	
					closest_note_id = j;
					closest_note_distance = distance;
				}
			}

			int8_t marked_note = -1;

			if( closest_note_distance <= MAX_JUMP_DISTANCE )
			{
				//We found the note we need to augment.
				//XXX: TODO: Should we be IIRing this?

				note_peak_freqs[closest_note_id] = thisfreq;
				marked_note = closest_note_id;
			}
			//The note was not found.
			else if( lowest_found_free_note != -1 )
			{
				note_peak_freqs[lowest_found_free_note] = thisfreq;
				marked_note = lowest_found_free_note;
			}

			if( marked_note != -1 )
			{
				if( note_peak_amps[marked_note] <= this )
					note_peak_amps[marked_note] = this;
				if( note_peak_amps2[marked_note] <= this )
					note_peak_amps2[marked_note] = this;
			}
		}
	}

	//Now we need to handle combining notes.
	for( i = 0; i < MAXNOTES; i++ )
	for( j = 0; j < i; j++ )
	{
		//We'd be combining nf2 (j) into nf1 (i) if they're close enough.
		uint8_t nf1 = note_peak_freqs[i];
		uint8_t nf2 = note_peak_freqs[j];
		int16_t distance = nf1 - nf2;

		if( nf1 == 255 || nf2 == 255 ) continue;

		if( distance < 0 ) distance = -distance;

		if( distance > ((1<<(SEMIBITSPERBIN-1))*FIXBPERO) )
		{
			distance = ((1<<(SEMIBITSPERBIN))*FIXBPERO) - distance;
		}

		if( distance > MAX_JUMP_DISTANCE * 2 )
		{
			continue;
		}

		//We need to combine the notes.  We need to move the new note freq
		//towards the stronger of the two notes.  
		int16_t amp1 = note_peak_amps[i];
		int16_t amp2 = note_peak_amps[j];

		//0 to 32768 porportional to how much of amp1 we want.
		uint32_t porp = (amp1<<15) / (amp1+amp2);  
		uint16_t newnote = (nf1 * porp + nf2 * (32768-porp))>>15;

		note_peak_amps[i] = amp1 + amp2;
		note_peak_amps[j] = 0;
		note_peak_freqs[i] = newnote;
		note_peak_freqs[j] = 255;
		note_peak_amps2[i] += note_peak_amps2[j];
	}


	for( i = 0; i < MAXNOTES; i++ )
	{
		if( note_peak_freqs[i] == 255 ) continue;

		note_peak_amps[i] -= note_peak_amps[i]>>AMP_1_NERFING_BITS;
		note_peak_amps2[i] -= note_peak_amps2[i]>>AMP_2_NERFING_BITS;

		if( note_peak_amps[i] < MINIMUM_AMP_FOR_NOTE_TO_DISAPPEAR )
		{
			note_peak_freqs[i] = 255;
			note_peak_amps[i] = 0;
			note_peak_amps2[i] = 0;
		}
	}

	//We now have notes!!!
#if 1
	for( i = 0; i < MAXNOTES; i++ )
	{
		if( note_peak_freqs[i] == 255 ) continue;
		printf( "(%3d %4d %4d) ", note_peak_freqs[i], note_peak_amps[i], note_peak_amps2[i] );
	}
	printf( "\n") ;
#endif

#if 0
	for( i = 0; i < FIXBPERO; i++ )
	{
		printf( "%5d ", folded_bins[i] );
	}
	printf( "\n" );
#endif
}