#include "plugin.h"

PLUGIN_HEADER

#define buffer_size 8820

#define D_NOTE          1.059463094359
#define LOG_D_NOTE      0.057762265047
#define D_NOTE_SQRT     1.029302236643
#define LOG_2           0.693147180559

static struct plugin_api* rb;

static unsigned int rate = 44100;
static int blockSize;
static signed short int schmittBuffer[4410];
static signed short int *schmittPointer = NULL;
static signed short audio_data[buffer_size];
static int recording=0;


static unsigned back_color, front_color;
static int xx,yy;


//static const char *englishNotes[12] = {"A","A#","B","C","C#","D","D#","E","F","F#", "G", "G#"};
//static const char *frenchNotes[12] = {"La","La#","Si","Do","Do#","Ré","Ré#","Mi","Fa","Fa#","Sol","Sol#"};
static const char *germanNotes[12] = {"A","A#","H","C","C#","D","D#","E","F","F#", "G", "G#"};
static const char **notes = germanNotes;
static double freqs[12];
static double lfreqs[12];


double log(double inp){
    int x;
    long t,y;
    
    x = (int) ( inp * ( 1 << 16 ) );
    
    y=0xa65af;
    if(x<0x00008000) x<<=16,              y-=0xb1721;
    if(x<0x00800000) x<<= 8,              y-=0x58b91;
    if(x<0x08000000) x<<= 4,              y-=0x2c5c8;
    if(x<0x20000000) x<<= 2,              y-=0x162e4;
    if(x<0x40000000) x<<= 1,              y-=0x0b172;
    t=x+(x>>1); if((t&0x80000000)==0) x=t,y-=0x067cd;
    t=x+(x>>2); if((t&0x80000000)==0) x=t,y-=0x03920;
    t=x+(x>>3); if((t&0x80000000)==0) x=t,y-=0x01e27;
    t=x+(x>>4); if((t&0x80000000)==0) x=t,y-=0x00f85;
    t=x+(x>>5); if((t&0x80000000)==0) x=t,y-=0x007e1;
    t=x+(x>>6); if((t&0x80000000)==0) x=t,y-=0x003f8;
    t=x+(x>>7); if((t&0x80000000)==0) x=t,y-=0x001fe;
    x=0x80000000-x;
    y-=x>>15;
    
    return ((double)y / ( 1 << 16 ));
}

void print_init(void){
	back_color  = rb->lcd_get_background();
	front_color = rb->lcd_get_foreground();
	rb->lcd_getstringsize("test", &xx, &yy);
}

void print_clear(void){
	rb->lcd_set_foreground(back_color);
	rb->lcd_fillrect(0,0,159,yy);
	rb->lcd_set_foreground(front_color);
	rb->lcd_update();
}

void print_int(int v){
	char temp[20];
	
	print_clear();
	rb->snprintf(temp,20,"%d",v);
	rb->lcd_puts(0,0,temp);
	rb->lcd_update();
}

void print_str(char* s){
	print_clear();
	rb->lcd_puts(0,0,s);
	rb->lcd_update();
}

double fabs(double x){
  return ((x>0) ? x : -x);
}

void displayFrequency (double freq){
	double ldf, mldf;
	double lfreq, nfreq;
	int i, note = 0;
	char str_buf[30];

	if (freq < 1E-15) freq = 1E-15;
	lfreq = log(freq);
	while (lfreq < lfreqs[0]-LOG_D_NOTE/2.) lfreq += LOG_2;
	while (lfreq >= lfreqs[0]+LOG_2-LOG_D_NOTE/2.) lfreq -= LOG_2;
	mldf = LOG_D_NOTE;
	for (i=0; i<12; i++) 
	{
	    ldf = fabs(lfreq-lfreqs[i]);
	    if (ldf < mldf) 
		{
		      mldf = ldf;
		      note = i;
	    }
	}
	nfreq = freqs[note];
	while (nfreq/freq > D_NOTE_SQRT) nfreq /= 2.0;
	while (freq/nfreq > D_NOTE_SQRT) nfreq *= 2.0;
	
	ldf=1200*(log(freq/nfreq)/LOG_2);
	
	rb->snprintf(str_buf,30, "%s : %d cents (%dHz)", notes[note], (int)ldf ,(int) freq);

	print_str(str_buf);
}

void algorithm_init(int size){
  blockSize = rate/size;    // = 44100/10 = 4410
  //schmittBuffer = (signed short int *)malloc(blockSize*sizeof(signed short int));
  schmittPointer = schmittBuffer;
}

static inline int Abs(int x){
  return ((x>0) ? x : -x);
}

void schmittS16LE (int nframes, signed short int *indata){
  int i, j;
  double trigfact = 0.6;

  for (i=0; i<nframes; i++) 
  {
	    *schmittPointer++ = indata[i];
	    //rb->splash(0,"indata[4]: %d", indata[4]);
	    if (schmittPointer-schmittBuffer >= blockSize ) 
		{
		      int endpoint, startpoint, t1, t2, A1, A2, tc, schmittTriggered;

		      schmittPointer = schmittBuffer;

		      for (j=0,A1=0,A2=0; j<blockSize; j++) 
			  {
					if (schmittBuffer[j]>0 && A1<schmittBuffer[j])  A1 = schmittBuffer[j];
					if (schmittBuffer[j]<0 && A2<-schmittBuffer[j]) A2 = -schmittBuffer[j];
		      }
		      t1 =   (int)( A1 * trigfact + 0.5);
		      t2 = - (int)( A2 * trigfact + 0.5);
		      startpoint=0;
			  
		      for (j=1; schmittBuffer[j]<=t1 && j<blockSize; j++);
			  
		      for (; !(schmittBuffer[j]  >=t2 && schmittBuffer[j+1]< t2) && j<blockSize; j++);
		      startpoint=j;
		      schmittTriggered=0;
		      endpoint=startpoint+1;
		      for(j=startpoint,tc=0; j<blockSize; j++) 
			  {
					if (!schmittTriggered) 
					{
						schmittTriggered = (schmittBuffer[j] >= t1);
					} 
					else if (schmittBuffer[j]>=t2 && schmittBuffer[j+1]<t2) 
					{
						  endpoint=j;
						  tc++;
						  schmittTriggered = 0;
					}
		      }
		      if (endpoint > startpoint) 
			  {                           
					displayFrequency((double)rate*(tc/(double)(endpoint-startpoint)));
		      }
	    }
  }
}

int recording_callback(int status){
	(void) status;
	
	rb->pcm_stop_recording();
	recording=0;
	return -1;	
}

void record_and_get_pitch(void){
	int quit=0;
	int button;
	
	int i=0;
	
	rb->audio_set_output_source(AUDIO_SRC_PLAYBACK);
    rb->audio_set_input_source(AUDIO_SRC_MIC, SRCF_RECORDING);
	rb->pcm_apply_settings();
	rb->pcm_init_recording();
	
	print_init();

	rb->splash(30,"recording started!");
	while(!quit) 
	{
		rb->pcm_record_data(recording_callback, (void *) audio_data, (size_t) buffer_size*sizeof(signed short));
		recording=1;	
		
		while (recording && !quit)	//wait for recording to stop
		{		
			i++;
			rb->backlight_on();
			rb->yield();
			//rb->sleep(100);
		    button=rb->button_get(false);
		    switch(button) 
			{
		        case BUTTON_POWER:
		        quit=true;
				rb->yield();
		        break;
				
		        default:
				rb->yield();
				
		        break;
		    }
		}
		
		schmittS16LE (buffer_size, audio_data);
	}
	rb->pcm_close_recording();
}

void audio_init(void){
	double aFreq = 440.0;
	
	/* Initialize tuning */

	int i;
	freqs[0]=aFreq;
	lfreqs[0]=log(freqs[0]);
	for (i=1; i<12; i++) 
	{
		freqs[i] = freqs[i-1] * D_NOTE;
		lfreqs[i] = lfreqs[i-1] + LOG_D_NOTE;
	}

}

enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
{
  int latency = 10;
  
  
  (void)parameter;
  rb=api;
  
  //rb->cpu_boost(true);
  
  audio_init();
  algorithm_init(latency);
  record_and_get_pitch();
  

  //rb->cpu_boost(false);

  return 0;
}