417 lines
11 KiB
C
417 lines
11 KiB
C
//Copyright 2015 <>< Charles Lohr under the ColorChord License.
|
|
|
|
#include "embeddednf.h"
|
|
#include <stdio.h>
|
|
#include <string.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];
|
|
uint8_t note_jumped_to[MAXNOTES];
|
|
|
|
|
|
#ifndef PRECOMPUTE_FREQUENCY_TABLE
|
|
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;
|
|
}
|
|
*/
|
|
|
|
#endif
|
|
|
|
#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
|
|
|
|
|
|
void UpdateFreqs()
|
|
{
|
|
|
|
#ifndef PRECOMPUTE_FREQUENCY_TABLE
|
|
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 + 0.5;
|
|
}
|
|
#else
|
|
|
|
#define PCOMP( f ) (uint16_t)((65536.0)/(DFREQ) * (f * BASE_FREQ) * 16 + 0.5)
|
|
|
|
static const uint16_t fbins[FIXBPERO] = {
|
|
PCOMP( 1.000000 ), PCOMP( 1.029302 ), PCOMP( 1.059463 ), PCOMP( 1.090508 ), PCOMP( 1.122462 ), PCOMP( 1.155353 ),
|
|
PCOMP( 1.189207 ), PCOMP( 1.224054 ), PCOMP( 1.259921 ), PCOMP( 1.296840 ), PCOMP( 1.334840 ), PCOMP( 1.373954 ),
|
|
PCOMP( 1.414214 ), PCOMP( 1.455653 ), PCOMP( 1.498307 ), PCOMP( 1.542211 ), PCOMP( 1.587401 ), PCOMP( 1.633915 ),
|
|
PCOMP( 1.681793 ), PCOMP( 1.731073 ), PCOMP( 1.781797 ), PCOMP( 1.834008 ), PCOMP( 1.887749 ), PCOMP( 1.943064 ) };
|
|
#endif
|
|
|
|
#ifdef USE_32DFT
|
|
UpdateBins32( fbins );
|
|
#else
|
|
UpdateBinsForProgressiveIntegerSkippyInt( fbins );
|
|
#endif
|
|
}
|
|
|
|
void InitColorChord()
|
|
{
|
|
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;
|
|
}
|
|
|
|
memset( folded_bins, 0, sizeof( folded_bins ) );
|
|
memset( fuzzed_bins, 0, sizeof( fuzzed_bins ) );
|
|
|
|
//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;
|
|
uint8_t hitnotes[MAXNOTES];
|
|
memset( hitnotes, 0, sizeof( hitnotes ) );
|
|
|
|
#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.
|
|
for( j = 0; j < FILTER_BLUR_PASSES; j++ )
|
|
{
|
|
//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);
|
|
|
|
//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 we find a note closer to where we are than any of the
|
|
//others, we can mark it as our desired note.
|
|
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.
|
|
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 we found a note to attach to, we have to use the IIR to
|
|
//increase the strength of the note, but we can't exactly snap
|
|
//it to the new strength.
|
|
if( marked_note != -1 )
|
|
{
|
|
hitnotes[marked_note] = 1;
|
|
|
|
note_peak_amps[marked_note] =
|
|
note_peak_amps[marked_note] -
|
|
(note_peak_amps[marked_note]>>AMP_1_IIR_BITS) +
|
|
(this>>(AMP_1_IIR_BITS-3));
|
|
|
|
note_peak_amps2[marked_note] =
|
|
note_peak_amps2[marked_note] -
|
|
(note_peak_amps2[marked_note]>>AMP_2_IIR_BITS) +
|
|
((this<<3)>>(AMP_2_IIR_BITS));
|
|
}
|
|
}
|
|
}
|
|
|
|
#if 0
|
|
for( i = 0; i < MAXNOTES; i++ )
|
|
{
|
|
if( note_peak_freqs[i] == 255 ) continue;
|
|
printf( "%d / ", note_peak_amps[i] );
|
|
}
|
|
printf( "\n" );
|
|
#endif
|
|
|
|
//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 it wraps around above the halfway point, then we're closer to it
|
|
//on the other side.
|
|
if( distance > ((1<<(SEMIBITSPERBIN-1))*FIXBPERO) )
|
|
{
|
|
distance = ((1<<(SEMIBITSPERBIN))*FIXBPERO) - distance;
|
|
}
|
|
|
|
if( distance > MAX_COMBINE_DISTANCE )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
int into;
|
|
int from;
|
|
|
|
if( note_peak_amps[i] > note_peak_amps[j] )
|
|
{
|
|
into = i;
|
|
from = j;
|
|
}
|
|
else
|
|
{
|
|
into = j;
|
|
from = i;
|
|
}
|
|
|
|
//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[into];
|
|
int16_t amp2 = note_peak_amps[from];
|
|
|
|
//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;
|
|
|
|
//When combining notes, we have to use the stronger amplitude note.
|
|
//trying to average or combine the power of the notes looks awful.
|
|
note_peak_freqs[into] = newnote;
|
|
note_peak_amps[into] = (note_peak_amps[into]>note_peak_amps[from])?
|
|
note_peak_amps[into]:note_peak_amps[j];
|
|
note_peak_amps2[into] = (note_peak_amps2[into]>note_peak_amps2[from])?
|
|
note_peak_amps2[into]:note_peak_amps2[j];
|
|
|
|
note_peak_freqs[from] = 255;
|
|
note_peak_amps[from] = 0;
|
|
note_jumped_to[from] = i;
|
|
}
|
|
|
|
//For al lof the notes that have not been hit, we have to allow them to
|
|
//to decay. We only do this for notes that have not found a peak.
|
|
for( i = 0; i < MAXNOTES; i++ )
|
|
{
|
|
if( note_peak_freqs[i] == 255 || hitnotes[i] ) continue;
|
|
|
|
note_peak_amps[i] -= note_peak_amps[i]>>AMP_1_IIR_BITS;
|
|
note_peak_amps2[i] -= note_peak_amps2[i]>>AMP_2_IIR_BITS;
|
|
|
|
//In the event a note is not strong enough anymore, it is to be
|
|
//returned back into the great pool of unused notes.
|
|
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 0
|
|
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( "%4d ", folded_bins[i] );
|
|
}
|
|
printf( "\n" );
|
|
#endif
|
|
|
|
|
|
}
|
|
|
|
|