#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(1.0, 19.0, 7.0);
SpringMassDamper spring1(10.0, 19.0, 7.0);


void setup() {
  
  FluteEx.init();
  
  pinMode(A0, INPUT);
  pinMode(A9, INPUT);
  pinMode(A2, INPUT);
  
  Music.init();
  
  usbMIDI.setHandleNoteOff(OnNoteOff);
  usbMIDI.setHandleNoteOn(OnNoteOn);
  usbMIDI.setHandleControlChange(OnControlChange);
  
  Music.getPreset(12);  
  
  Music.setGain1(1.0);
  Music.setGain2(1.0);
  Music.setGain3(1.0); 
  
  Music.setWaveform1(SQUARE);
  Music.setWaveform2(SQUARE);
  Music.setWaveform3(SQUARE);

  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(A0);
  t = map(t, 0, 1023, 2, 167);
  //t = map(t, 0, 1023, 2, 75); -- great
  delay(t);
  
  r += 0.001;

  int q = 20000;
  int minf = 50, maxf = 20000;    
  
  int f0 = map(ema0, -q, q, minf, maxf); //or
  //float f0 = sin(r) * (float)(t*2);
  Music.setFrequency1(f0);

//  Serial.println(f0);
  
  int f1 = map(pos0, -q, q, minf, maxf);
  Music.setFrequency2(f1);

//  Serial.println(f1);
  
  int f2 = map(pos1, -q, q, minf, maxf);
  Music.setFrequency3(f2);

  Serial.println(f2);
  
  usbMIDI.read();
  
  
  
}