#include "testApp.h"

#define FINAL
		
//--------------------------------------------------------------
void testApp::setup(){	 
	ard.connect("COM3", 115200);
	srand((unsigned int)time((time_t *)NULL));
	
	// 0 output channels, 
	// 2 input channels
	// 44100 samples per second
	// BUFFER_SIZE samples per buffer
	// 4 num buffers (latency)
	
	ofSoundStreamSetup(0,2,this, 44100,BUFFER_SIZE, 4);	
	left = new float[BUFFER_SIZE];
	right = new float[BUFFER_SIZE];

	/*for (int i = 0; i < NUM_WINDOWS; i++)
	{
		for (int j = 0; j < BUFFER_SIZE/2; j++)
		{
			freq[i][j] = 0;	
		}
	}*/
	
	ofSetColor(0x666666);
	setupArduino();

	show=0;
	delta=0;
	avg=0;
	avg_delta=0;

	avg_power = 0.0f;

#ifdef FINAL
	myfont.loadFont("franklinGothic.otf", 10);
	cout << myfont.bLoadedOk;
#endif
	
	height = 0.0f;
	width = 1600.0f/(BUFFER_SIZE/2);

	ofSetFrameRate(50);
}


//--------------------------------------------------------------
void testApp::update(){
		ofBackground(80,80,20);
		//updateArduino();
}

//--------------------------------------------------------------
void testApp::draw(){
		
	/* start from 1 because mag[0] = DC component */
	/* and discard the upper half of the buffer */
	/*for(int j=1; j < BUFFER_SIZE/2; j++) {
		freq[index][j] = magnitude[j];		
	}*/

	//ofLine(3,3,3,avg_power);
	

	/* draw the FFT */
	if(show){
		for (int i = 1; i < (int)(BUFFER_SIZE/2); i++){
			//ofLine(i,480,i,480-max(log(magnitude[i]*10.0f)*30.0f,0));
			height = max(log(magnitude[i]*10.0f)*50.0f,1); //a bit performance
			ofRect((i*width),480-height,width,height);
		}
		ofRect(10,10,min(max((int)magnitude[getBand(80)]*1,0),255)*2,10); 
		ofRect(10,20,min(max((int)dmagnitude[getBand(80)]*1,0),255)*2,10);
		//ofRect(10,20,min(max(log(magnitude[getBand(80)])*30.0f,0),255)*2,10);
		//ofRect(10,30,min(max(log(magnitude[getBand(80)]*100.0f)*30.0f,0),255)*2,10);

		/*ofRect(10,30,min(max((int)(
									(
									 (int)magnitude[getBand(80)]+
									 (int)magnitude[getBand(50)]+
									 (int)magnitude[getBand(60)]+
									 (int)magnitude[getBand(70)]
									)
									/4
								   ),0),255)*2,10);*/
		ofRect(10,30,min(max(davg_power,0),255)*2,10);

		ofRect(10,40,min(max(avg_power/2,0),255)*2,10); 
		
#ifdef FINAL
if(delta) ofSetColor(0xAA0000);
myfont.drawString("delta 80Hz (d)", 520,30);
if(delta) ofSetColor(0x666666);

if(avg_delta) ofSetColor(0xAA0000);
myfont.drawString("delta Avg (q)", 520,40);
if(avg_delta) ofSetColor(0x666666);

if(avg) ofSetColor(0xAA0000);
myfont.drawString("Avg (a)", 520,50);
if(avg) ofSetColor(0x666666);

if(!delta && !avg && !avg_delta) ofSetColor(0xAA0000);
myfont.drawString("80Hz (n)", 520,20);
ofSetColor(0x666666);
#endif

		ofLine(520,10,520,50);
		ofLine(110,10,110,50);
		ofLine(210,10,210,50);
		ofLine(310,10,310,50);
		ofLine(410,10,410,50);
		ofLine(510,10,510,50);
	}

	
}

//--------------------------------------------------------------
int testApp::getBand(int freq){
	return (int)((freq*BUFFER_SIZE)/((float)44100));
}

//--------------------------------------------------------------
void testApp::keyPressed  (int key){ 
	
}

//--------------------------------------------------------------
void testApp::keyReleased  (int key){ 
	if(key == 's')
		show ^= 1;
	if(key == 'd') {
		delta = 1;
		avg = avg_delta = 0; }
	if(key == 'a') {
		avg = 1;
		delta = avg_delta = 0; }
	if(key == 'n') {
		delta = avg = avg_delta = 0; }
	if(key == 'q') {
		avg = delta = 0;
		avg_delta = 1; }
}

//--------------------------------------------------------------
void testApp::mouseMoved(int x, int y ){
	
}

//--------------------------------------------------------------
void testApp::mouseDragged(int x, int y, int button){
	
}

//--------------------------------------------------------------
void testApp::mousePressed(int x, int y, int button){
	
}

//--------------------------------------------------------------
void testApp::mouseReleased(){

}

//--------------------------------------------------------------
void testApp::audioReceived 	(float * input, int bufferSize, int nChannels){	
	// samples are "interleaved"
	for (int i = 0; i < bufferSize; i++){
		left[i] = input[i*2];
		right[i] = input[i*2+1];
	}
	//bufferCounter++;
	/* do the FFT	*/
	myfft.powerSpectrum(0,(int)BUFFER_SIZE/2, left,BUFFER_SIZE,&magnitude[0],&phase[0],&power[0],&avg_power);

	/* delta magnitude calculations */
	if(delta || show){
		for(int j = 0; j < BUFFER_SIZE/2; j++){ //s.o.
			dmagnitude[j] = abs(magnitude[j]-pmagnitude[j]);
		}
	}

	if(avg_delta || show) davg_power = abs(pavg_power - avg_power);
//
	if(delta || show){
		for(int j = 0; j < BUFFER_SIZE/2; j++){ //halbe buffersize wegn s.o.
			pmagnitude[j] = magnitude[j];
		}
	}
	if(avg_delta || show) pavg_power = avg_power;

	updateArduino();
}

//--------------------------------------------------------------
void testApp::setupArduino(){
	ard.sendDigitalPinMode(11,ARD_PWM);
}

//--------------------------------------------------------------
void testApp::updateArduino(){
	ard.update();
	//ard.sendPwm(11,min(max(log(magnitude[getBand(80)]*10.0f)*20.0f,0),255));
	/**ard.sendPwm(11,min(max((int)(
								 (
								  (int)magnitude[getBand(80)]+
								  (int)magnitude[getBand(50)]+
								  (int)magnitude[getBand(60)]+
								  (int)magnitude[getBand(70)]
								 )
								 /4
								),0),255));**/
	
	if(delta) {
		ard.sendPwm(11,min(max((int)dmagnitude[getBand(80)]*1,0),255)); //delta ausgabe
		//cout << "delta" << endl;
	}
	else if(avg) {
		ard.sendPwm(11,min(max(avg_power/2,0),255));
		//cout << "avg" << endl;
	}
	else if(avg_delta) {
		ard.sendPwm(11,min(max(davg_power,0),255));
	}
	else {
		ard.sendPwm(11,min(max((int)(magnitude[getBand(80)]),0),255));
		//cout << "!delta" << endl;
	}
	//cout << "d: " << delta << "\t a: " << avg << endl;
	//ard.sendPwm(11,min(max(log(magnitude[getBand(80)])*30.0f,0),255));
	//cout << min(max((int)magnitude[getBand(80)]*1,0),255) << endl;
}

 
#ifndef _TEST_APP
#define _TEST_APP
#include "ofMain.h"
#include "fft.h"
#include "math.h"

#define BUFFER_SIZE 2048
#define NUM_WINDOWS 80



class testApp : public ofSimpleApp{
	
	public:
		
		void setup();
		void update();
		void draw();
		
		void keyPressed  (int key);
		void keyReleased (int key);
		void mouseMoved(int x, int y );
		void mouseDragged(int x, int y, int button);
		void mousePressed(int x, int y, int button);
		void mouseReleased();

		void setupArduino();
		void updateArduino();

		int getBand(int frequency);
		
		void audioReceived 	(float * input, int bufferSize, int nChannels); 

	private:	
		float * left;
		float * right;
		int 	bufferCounter;
		fft		myfft;

		float pmagnitude[BUFFER_SIZE]; //magnitude from previous sample
		float dmagnitude[BUFFER_SIZE]; //delta magnitude between two samples

		int show;
		int delta;
		int avg;
		int avg_delta;

		float avg_power;
		float pavg_power,davg_power;

		ofTrueTypeFont myfont;

		float height, width;
		
		float magnitude[BUFFER_SIZE];
		float phase[BUFFER_SIZE];
		float power[BUFFER_SIZE];
		
		//float freq[NUM_WINDOWS][BUFFER_SIZE/2];
		//float freq_phase[NUM_WINDOWS][BUFFER_SIZE/2];

		ofArduino ard;
};

#endif	

 
#include <Firmata.h>

#define LED_N_SIDE 2
#define LED_P_SIDE 3

const int led = 13;

void setup()
{
  Serial.begin(115200);
  
  pinMode(led,OUTPUT);
  pinMode(9,OUTPUT);
  pinMode(10,OUTPUT);
  pinMode(11,OUTPUT);
  pinMode(5,INPUT);
  
  digitalWrite(5,HIGH); //turn on internal pull-up resistor
  
  
  Firmata.setFirmwareVersion(0,1);
  Firmata.attach(ANALOG_MESSAGE, analogMessageCallback);
  Firmata.begin();
  
}

unsigned int wid = 10000; //length of one PWM width in us
long previousMicros = 0;

unsigned int vel = 50000; //Krams f�r langsames zur�ckgehen der led 2500 ist rel. schon
long previousMicrosSpeed = 0;

int pwr = 128; //Ratio of on to off time
int fakepwr;
int on = HIGH; //Whether the pwm is on the on or off interval

int onwidth = (long)wid*(long)pwr/255;
int offwidth = wid-onwidth;

byte rgb[] = {0,0,0};
void loop()
{
  if(on==HIGH){
    if(micros()-previousMicros > onwidth){
      on = LOW;
      previousMicros = micros();
    }
  }
  else{
    if(micros()-previousMicros > offwidth){
      on = HIGH;
      previousMicros = micros();
    }
  }
  digitalWrite(led,on);
  analogWrite(11,pwr);
  
  //falls pin5 LOW, pwr = analogInput(0)
  if(!digitalRead(5)) fakepwr = analogRead(0)/4;
  
  //langsam dem wert n�hern
  //fakepwr ist der empfangene wert
  //pwr der gerade angezeigte von den leds
  if(vel){
   if(micros()-previousMicrosSpeed > vel){
       if(fakepwr < pwr) pwr--;
       onwidth = (long)wid*(long)pwr/255;
       offwidth = wid-onwidth;
       previousMicrosSpeed = micros();
   }
  }
  else{ pwr = fakepwr; }
  if(fakepwr > pwr) pwr = fakepwr;
  
  while(Firmata.available())
        Firmata.processInput();
  
  
  
  //RGB HSV Stuff
/*  unsigned long rgb = HSVtoRGB(analogRead(0)/3,1.0,1.0);
  byte r = (byte)(rgb >> 16);
  rgb &= 0x0000FFFF;
  byte g = (byte) (rgb >> 8);
  rgb &= 0x000000FF;
  byte b = (byte) (rgb);    */
  
  
  //HSVtoRGB(analogRead(0)/3,1.0,1.0,rgb);
  
  if(digitalRead(5))vel = analogRead(0)*4;
  //Serial.println(map(analogRead(0),0,1024,0,65536),DEC);
  
  /*analogWrite(9,(int)rgb[1]);
  analogWrite(10,(int)rgb[2]);
  analogWrite(11,(int)rgb[0]);
  
  //HSVtoRGB(1,1.0,1.0,&rgb);
  Serial.println((int)rgb[0],DEC);
  Serial.println((int)rgb[1],DEC);
  Serial.println((int)rgb[2],DEC);
  //HSVtoRGB(240,1.0,1.0,&rgb);
  Serial.println((int)rgb[0],DEC);
  Serial.println((int)rgb[1],DEC);
  Serial.println((int)rgb[2],DEC);
  Serial.println();*/
  
  //Helligkeitssensor
  unsigned int j,brightness;
/*
  // Apply reverse voltage, charge up the pin and led capacitance
  pinMode(LED_N_SIDE,OUTPUT);
  pinMode(LED_P_SIDE,OUTPUT);
  digitalWrite(LED_N_SIDE,HIGH);
  digitalWrite(LED_P_SIDE,LOW);

  //wait a bit
  delayMicroseconds(1000);

  // Isolate the pin 2 end of the diode
  pinMode(LED_N_SIDE,INPUT);
  digitalWrite(LED_N_SIDE,LOW);  // turn off internal pull-up resistor

  // Count how long it takes the diode to bleed back down to a logic zero
  for ( j = 0; j < 30000; j++) {
    if ( digitalRead(LED_N_SIDE)==0) {
      brightness = j;
      break;
    }
  }*/
  // You could use 'j' for something useful, but here we are just using the
  // delay of the counting.  In the dark it counts higher and takes longer, 
  // increasing the portion of the loop where the LED is off compared to 
  // the 1000 microseconds where we turn it on.

  // Turn the light on for 1000 microseconds
  /*digitalWrite(LED_P_SIDE,HIGH);
  digitalWrite(LED_N_SIDE,LOW);
  pinMode(LED_P_SIDE,OUTPUT);
  pinMode(LED_N_SIDE,OUTPUT);*/
  // we could turn it off, but we know that is about to happen at the loop() start
  
  //send measurement via serial interface
  Serial.println(brightness,DEC);
}

void analogMessageCallback(byte pin, int value)
{
  switch(pin) {
    case 1 : fakepwr = (byte)value; break;
    case 2 : wid = value; break;
    case 11: fakepwr = (byte)value; break;
  }
  onwidth = (long)wid*(long)pwr/255;
  offwidth = wid-onwidth;
}

int* HSVtoRGB2(int h, float s, float v){
  float r = 0;
  float g = 0;
  float b = 0;
  int ret[3];
  
  int hi = h/60;
  
  float f = ((float)h/60)-hi;
  
  float p = v * (1 - s);
  float q = v * (1 - s * f);
  float t = v* (1 - s*(1-f));
  
  if(hi==0 || hi==6){
    ret[0] = (int) (v*255);
    ret[1] = (int) (t*255);
    ret[2] = (int) (p*255);
  }
  else if(hi==1){
    ret[0] = (int) (q*255);
    ret[1] = (int) (v*255);
    ret[2] = (int) (p*255);
  }else if(hi==2){
    ret[0] = (int) (p*255);
    ret[1] = (int) (v*255);
    ret[2] = (int) (t*255);
  }else if(hi==3){
    ret[0] = (int) (p*255);
    ret[1] = (int) (q*255);
    ret[2] = (int) (v*255);
  }else if(hi==4){
    ret[0] = (int) (t*255);
    ret[1] = (int) (p*255);
    ret[2] = (int) (v*255);
  }else if(hi==5){
    ret[0] = (int) (v*255);
    ret[1] = (int) (p*255);
    ret[2] = (int) (q*255);
  }
  return ret;
}

void HSVtoRGB(int h, float s, float v,byte *rgb){
/*  float r = 0;
  float g = 0;
  float b = 0;*/
  
  int hi = h/60;
  
  float f = ((float)h/60)-hi;
  
  float p = v * (1 - s);
  float q = v * (1 - s * f);
  float t = v* (1 - s*(1-f));
  
  if(hi==0 || hi==6){
    rgb[0] = (byte) (v*255);
    rgb[1] = (byte) (t*255);
    rgb[2] = (byte) (p*255);
  }
  else if(hi==1){
    rgb[0] = (byte) (q*255);
    rgb[1] = (byte) (v*255);
    rgb[2] = (byte) (p*255);
  }else if(hi==2){
    rgb[0] = (byte) (p*255);
    rgb[1] = (byte) (v*255);
    rgb[2] = (byte) (t*255);
  }else if(hi==3){
    rgb[0] = (byte) (p*255);
    rgb[1] = (byte) (q*255);
    rgb[2] = (byte) (v*255);
  }else if(hi==4){
    rgb[0] = (byte) (t*255);
    rgb[1] = (byte) (p*255);
    rgb[2] = (byte) (v*255);
  }else if(hi==5){
    rgb[0] = (byte) (v*255);
    rgb[1] = (byte) (p*255);
    rgb[2] = (byte) (q*255);
  }
  
  /*ret = (int)(r*255) << 16;
  ret = ret | ((int)(g*255) << 8);
  ret = ret | (int)(b*255);*/
  
  //return ret;
}