#define MIDI_CHANNEL 1

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

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

float x, y, z = 0;
SpringMassDamper springX(50.0, 275.0, 1, &x);
SpringMassDamper springY(50.0, 275.0, 1, &y);
SpringMassDamper springZ(10.0, 275.0, 10, &z);

const int basefreq = 100;

long tick = 0;

int fm2, fm3;

void setup() {
  FluteEx.init();
  Serial.begin(115200);
  Serial.println("starts");

  Music.init();

  Music.setGain1(1.0);
  Music.setGain2(1.0);
  Music.setGain3(1.0); 

  usbMIDI.setHandleNoteOff(OnNoteOff);
  usbMIDI.setHandleNoteOn(OnNoteOn);
  usbMIDI.setHandleControlChange(OnControlChange);

  tick = millis();

}


void loop() {
  if(FluteEx.push_button()) x += 1;

  springX.position(x);
  springY.position(springX.X);
  springZ.position(springY.X);
  
  int f1 = basefreq + springX.X;
  int f2 = basefreq + springY.X;
  int f3 = basefreq + springZ.X;
  
  Music.setFrequency1(f1);
  Music.setFrequency2(f2);
  Music.setFrequency3(f3);

  if(millis() - tick > 200) {
//    Serial.println(f3);
    tick = millis();
    Serial.println(fm2);
  }

//  modulation1(); 
//
//  modulation2(); 
  
  modulation3();

  usbMIDI.read();
}

void modulation1() {
  static float indx = 0;
  indx += 0.00003;
  Music.setDetune1(sin(indx) * 0.05126);  
}


float fm2_indx = 0;
float fm2_incr = 0.00003;
void modulation2() {
  fm2_indx += fm2_incr;
  fm2 = sin(fm2_indx) * 0.05946;
  Music.setDetune2(fm2);  
}

float fm3_indx = 0;
float fm3_incr = 0.00003;
void modulation3() {
  fm3_indx += fm3_incr;
  Music.setDetune3(cos(fm3_indx) * 0.05946);  
}