/*********************

EIG-2006 - Projet Arduino

Museduino

TRAN-GIRARD Pacien
NICOLE Rémi

**********************/


/***** Parameters *****/

// FHT options
#define FHT_N 256
#define SCALE 256

#define WINDOW 1
#define REORDER 1

#define LOG_OUT 0
#define LIN_OUT 0
#define LIN_OUT8 1

#define OCTAVE 0
#define OCT_NORM 0

// Data aquisition options
#define USE_FAST_AQUISITION 0
#define PRESCALE_FACTOR 256
#define DUMMY_SAMPLES 1
#define WAIT_CYCLES 0
#define TRANSFORM_FACTOR -0.1
#define TRANSFORM_ORIGIN 1

// Data bins
#define BINS 128 // FHT_N /2
#define OCTAVES 8 // log2(FHT_N)

// Note matching options
#define IGNORE_FIRST_BINS 2
#define AMPLITUDE_THRESHOLD 10

// MIDI output
#define MIDI_INSTRUMENT 103
#define MIDI_VOLUME 120
#define MIDI_RESET_PIN 4


/***** System setup *****/

#include <FHT.h>
#include <SoftwareSerial.h>

SoftwareSerial shieldSerial(2, 3); // RX, TX

void setup_midi_shield() {
	// setup soft serial link for MIDI control
	shieldSerial.begin(31250);
	
	// reset the VS1053
	pinMode(MIDI_RESET_PIN, OUTPUT);
	
	digitalWrite(MIDI_RESET_PIN, LOW);
	delay(100);
	
	digitalWrite(MIDI_RESET_PIN, HIGH);
	delay(100);
	
	// set volume and instrument
	talkMIDI(0xB0, 0x07, MIDI_VOLUME);
	talkMIDI(0xC0, MIDI_INSTRUMENT, 0);
}

void setup_serial_link() {
	Serial.begin(115200);
}

void setup_adc() {
	#if USE_FAST_AQUISITION == 1
		ADCSRA = 0xe5; // set the adc to free running mode
		ADMUX = 0x40;	// use adc0
		DIDR0 = 0x01;	// turn off the digital input for adc0
	#endif
}

void setup() {
	setup_serial_link();
	setup_midi_shield();
	setup_adc();
}


/***** Data transforms *****/

void run_fht() {
	#if WINDOW == 1
		fht_window();
	#endif
	
	#if REORDER == 1
		fht_reorder();
	#endif
	
	fht_run();
}

void mag_fht() {
	#if LOG_OUT == 1
		fht_mag_log();
	#elif LIN_OUT == 1
		fht_mag_lin();
	#elif LIN_OUT8 == 1
		fht_mag_lin8();
	#endif
}

void transform_factor() {
	#if LOG_OUT == 1
		uint8_t* data = fht_log_out;
		uint8_t m = 0;
	#elif LIN_OUT == 1
		uint16_t* data = fht_lin_out;
		uint16_t m = 0;
	#elif LIN_OUT8 == 1
		uint8_t* data = fht_lin_out8;
		uint8_t m = 0;
	#endif
	
	for (int i = 0; i < BINS; i++) {
		data[i] *= TRANSFORM_FACTOR * i + TRANSFORM_ORIGIN;
	}
}

int find_max_index() {
	#if LOG_OUT == 1
		uint8_t* data = fht_log_out;
		uint8_t m = 0;
	#elif LIN_OUT == 1
		uint16_t* data = fht_lin_out;
		uint16_t m = 0;
	#elif LIN_OUT8 == 1
		uint8_t* data = fht_lin_out8;
		uint8_t m = 0;
	#endif
	
	int k = 0;
	for (int i = IGNORE_FIRST_BINS; i < BINS; i++) {
		if (data[i] > m && data[i] >= AMPLITUDE_THRESHOLD) {
			m = data[i];
			k = i;
		}
	}
	return k;
}

int find_midi_note(float freq) {
	float delta = 3.4028235E+38; // max float value
	int note = 0;
	for (int i = 0; i < 120; i++) { // exec time must be constant
		float new_delta = abs(calc_midi_note_freq(i) - freq);
		if (new_delta < delta) {
			delta = new_delta;
			note = i;
		}
	}
	return note;
}

float calc_bin_frequency(int k) {
	#if DUMMY_SAMPLES == 0
		return (float(k) - 0.0616541) / 0.0287293;
	#elif DUMMY_SAMPLES == 1
		return (float(k) - 0.00827068) / 0.0573534;
	#endif
}

float calc_midi_note_freq(int midi_note) {
	return 8.1757989156 * pow(2, float(midi_note)/12);
}


/***** I/O wrappers *****/

void talkMIDI(byte cmd, byte data1, byte data2) {
	shieldSerial.write(cmd);
	shieldSerial.write(data1);

	// Some commands only have one data byte.
	// All cmds less than 0xBn have 2 data bytes.
	// (sort of: http://253.ccarh.org/handout/midiprotocol/)
	if( (cmd & 0xF0) <= 0xB0)
		shieldSerial.write(data2);
}

void noteOn(byte channel, byte note, byte attack_velocity) {
	talkMIDI((0x90 | channel), note, attack_velocity);
}

void noteOff(byte channel, byte note, byte release_velocity) {
	talkMIDI((0x80 | channel), note, release_velocity);
}

void aquire_data() {
	noInterrupts();
	
	#if USE_FAST_AQUISITION == 1

		for (int i = 0; i < FHT_N; i++) {
			while(!(ADCSRA & 0x10)); // wait for adc to be ready
			ADCSRA = 0xf5; // restart adc
			byte m = ADCL; // fetch adc data
			byte j = ADCH;
			int k = (j << 8) | m; // form into an int
			k -= 0x0200;			// form into a signed int
			k <<= 6;					// form into a 16b signed int
			fht_input[i] = k * PRESCALE_FACTOR; // put real data into bin
		}

	#else
	
		for (int i = 0; i < FHT_N; i++) {
			for (int s = -1; s < DUMMY_SAMPLES; s++) {
				fht_input[i] = analogRead(A0) * PRESCALE_FACTOR;
			}
		}
		
	#endif
	
	interrupts();
}

void send_fht_output(int max_index, float fund_freq, int midi_note) {
	#if LOG_OUT == 1
		uint8_t* data = fht_log_out;
	#elif LIN_OUT == 1
		uint16_t* data = fht_lin_out;
	#elif LIN_OUT8 == 1
		uint8_t* data = fht_lin_out8;
	#endif
	
	Serial.print("{");
	Serial.print("\"spectrum\":[");
	for (int i = 0; i < BINS; i++) {
		if (i != 0) Serial.print(",");
		Serial.print(data[i]);
	}
	Serial.print("],");
	
	Serial.print("\"fundamental_bin\":");
	Serial.print(max_index);
	Serial.print(",");
	
	Serial.print("\"fundamental_freq\":");
	Serial.print(fund_freq);
	Serial.print(",");
	
	Serial.print("\"midi_note\":");
	Serial.print(midi_note);
	
	Serial.print("}\n");
}


/***** Main program *****/

void loop() {
	int note = 0;
	
	while (1) {
		aquire_data();
		
		run_fht();
		transform_factor();
		mag_fht();
		
		int max_index = find_max_index();
		float fund_freq = calc_bin_frequency(max_index);
		int midi_note = find_midi_note(fund_freq);
		
		send_fht_output(max_index, fund_freq, midi_note);
		
		if (midi_note != note) {
			noteOff(0, note, 60);
			noteOn(0, midi_note, 60);

			note = midi_note;
		}
	}
}