#define MIDI_CHANNEL 1

#include <EEPROM.h>
#include <spi4teensy3.h>
#include <Mcp4251.h>
#include <Wire.h>
#include <Adafruit_TCS34725.h>

#include <Haarnet.h>
#include <HaarnetExtensionFlute.h>

#include <ExpMovAvg.h>
#include <MinMaxLerp.h>
#include <SpringMassDamper.h>

#define PRINTLN(x,y) Serial.print(x); Serial.println(y);

int raw0 = 0;
int ema0 = 0;
ExpMovAvg exmoav(&raw0, &ema0);

//SpringMassDamper spring0(10.0, 19.0, 7.0);
//SpringMassDamper spring1(10.0, 19.0, 7.0);

SpringMassDamper spring0(100.0, 9.0, 7.0);
SpringMassDamper spring1(100.0, 9.0, 7.0);

const int ADC_AVG_RES   = 4;
const int min_ADC = 420;
const int max_ADC = 850;
const int min_delta_t = 0;
const int max_delta_t = 25;


void setup() {
  
  //FluteEx.init();
  
  pinMode(A1, INPUT);
  pinMode(A9, INPUT);
  pinMode(A2, INPUT);

  analogReadAveraging(ADC_AVG_RES);
  
  Music.init();
  
  usbMIDI.setHandleNoteOff(OnNoteOff);
  usbMIDI.setHandleNoteOn(OnNoteOn);
  usbMIDI.setHandleControlChange(OnControlChange);
  
  Music.getPreset(16);  
  
  Music.setGain1(1.0);
  Music.setGain2(1.0);
  Music.setGain3(1.0); 
  
  Music.setWaveform1(TRIANGLE);
  Music.setWaveform2(SQUARE);
  Music.setWaveform3(SINE);

  Serial.begin(115200);
  
}

float r = 0;

void loop() {
  
  int memory_junk[4];
  raw0 = memory_junk[0] + memory_junk[1] + memory_junk[2] + memory_junk[3];
    
  exmoav.calculate();          
  
  int pos0 = spring0.position(ema0);
  int pos1 = spring1.position(pos0);
  
  int t = analogRead(A1);
  if (t < min_ADC || t > max_ADC) return;
  t = map(t, min_ADC, max_ADC, min_delta_t, max_delta_t);
  delay(t);

  Serial.println(t);

  r += 0.001;

  int q = 10000;
  int minf = 50, maxf = 20000;    
  
//  int f0 = map(ema0, -q, q, minf, maxf); //or
  float f0 = sin(r) * (float)(2*t);
  Music.setFrequency1(f0);
  
  int f1 = map(pos0, -q, q, minf, maxf);
  Music.setFrequency2(f1);
  
  int f2 = map(pos1, -q, q, minf, maxf);
  Music.setFrequency3(f2);
  
  usbMIDI.read();
  
  
}