#include <dreammakerfx.h>
/**
 * This sketch relies on a Sparkfun distance sensor that can be obtained here: https://www.sparkfun.com/products/14722
 * 
 * When the measured distance from the sensor is less than about 2.5 feet, the tones are mixed in.  And when 
 * there is nothing in front of the sensor, the tones turn off.
 */


// These are the include files that comes wiht the 
#include <ComponentObject.h>
#include <RangeSensor.h>
#include <SparkFun_VL53L1X.h>
#include <vl53l1x_class.h>
#include <vl53l1_error_codes.h>

// Two oscillators 
fx_oscillator     therm1(OSC_TRI, 200.0, 0.5);
fx_oscillator     therm2(OSC_RAMP, 200.0, 0.5);

// Delays / echoes for each sensor
fx_delay         therm1_delay(1000.0, 0.6);
fx_delay         therm2_delay(2000.0, 0.6);

// Tone filter 
fx_biquad_filter  filt_out(800, 1, BIQUAD_TYPE_LPF, BIQUAD_ORDER_6);

// Gain control to switch oscillators on and off when sensor detects something
fx_gain     therm1_gain(2.0), therm2_gain(2.0);   

// Mixer to add two oscillators together
fx_mixer_2   mix2;                             

// Phase shifter 
fx_phase_shifter out_phasey(0.5, 0.5, 0.5);

// Output limiter to prevent clipping if resonance is dialed up or delay feedback is 
fx_compressor out_limiter(-10.0, 1000.0, 10.0, 1000.0, 1.0);

// Instance for the distnace sensor (from Sparkfun's exanple code)
SFEVL53L1X distanceSensor;


// An array of note frequencies 
#define TOT_NOTES (60)
float notes[TOT_NOTES];

void setup() {

  // Generate note frequencies starting at a very low A
  int i=0;
  while (i<TOT_NOTES) {
    notes[i] = 55.0 * pow(2.0, (float) i * (1.0/12.0));
    i++;
  }

  // Initialize the sensors
  bool sensor_working = false;
  Wire.begin();
  if (distanceSensor.begin() == 0) //Begin returns 0 on a good init
  {
    sensor_working = true;
  }
  
  // Route audio through our pedal
  pedal.init();

  pedal.route_audio(therm1.output, therm1_gain.input);
  pedal.route_audio(therm2.output, therm2_gain.input);
  
  pedal.route_audio(therm1_gain.output, therm1_delay.input);
  pedal.route_audio(therm2_gain.output, therm2_delay.input);

  pedal.route_audio(therm1_delay.output, mix2.input_1);
  pedal.route_audio(therm2_delay.output, mix2.input_2);
  
  pedal.route_audio(mix2.output, filt_out.input);
  pedal.route_audio(filt_out.output, out_phasey.input);
  pedal.route_audio(out_phasey.output, out_limiter.input);
  pedal.route_audio(out_limiter.output, pedal.amp_out);

  pedal.run();

  // Let the the world know that the sensor is working
  if (sensor_working) {
    Serial.println("Sensor online!");
  }


}

int now = millis();
float distanceInches, distanceFeet;
int incs_pentatonic[]= {3,2,2,3,2};
int note_delta = 0;
int indx_quant = 0;
int l_indx = 2;
void loop() {
  
  // Service DreamMaker FX 
  pedal.service();
  
  // Take a reading from distance sensor every 100 ms.  If we do this too frequently,
  // the sensor stops responding so 50-100ms intervals is the right amount of time
  if (millis() > now + 100) {
    Serial.print("Sensor distance: ");
    now = millis();
    distanceSensor.startRanging();
    int distance = distanceSensor.getDistance(); 
    distanceSensor.stopRanging();
    distanceInches = distance * 0.0393701;
    distanceFeet = distanceInches / 12.0;    
    // Print measured distance - comment this out if you want
    Serial.println(distanceFeet);
  }

  
  float last_5[5];
  if (distanceFeet > 3.5) {
    // If there is nothing in close range of the sensors, turn lights off 
    // and turn volume to zero on oscillators
    therm1_gain.set_gain(0.0);
    therm2_gain.set_gain(0.0);
    digitalWrite(PIN_FOOTSW_LED_2, LOW); 
    digitalWrite(PIN_FOOTSW_LED_1, LOW); 
  } else {

    // Otherwise, if something is close, translate distance to frequency and 
    // find the closest pentatonic note (and harmony)
    float freq_raw = 110 + 110 * distanceFeet;
    float freq_quant;
    
    float dist = 1000.0;
    int i=0, j=0;
    while (i < TOT_NOTES) {
      float d = abs(freq_raw - notes[i]);
      bool update = false;
      if (d< dist) {
        dist = d;
        freq_quant = notes[i] * 0.5;
        indx_quant = i;
        update = true;
      }
      i+= incs_pentatonic[j];
      j++;
      if (j >= 5) {
        j = 0;
      }
      if (update) {
        last_5[4] = last_5[3];
        last_5[3] = last_5[2];
        last_5[2] = last_5[1];
        last_5[1] = last_5[0];
        last_5[0] = freq_quant;
      }

    }

    // Set oscillator gains and frequencies 
    therm1_gain.set_gain(0.5);
    therm2_gain.set_gain(0.25);
    therm2.set_frequency(last_5[l_indx]*2.0);
    therm1.set_frequency(freq_quant);

    // Turn on the footswitch LEDs
    digitalWrite(PIN_FOOTSW_LED_2, HIGH); 
    digitalWrite(PIN_FOOTSW_LED_1, HIGH); 
  }

  // Right pot controls the length of the echo
  if (pedal.pot_right.has_changed()) {
     therm1_delay.set_length_ms(pedal.pot_right.val * 1000.0);
     therm2_delay.set_length_ms(pedal.pot_right.val * 2000.0);
  }

  // Center pot controls low-pass filter
  if (pedal.pot_center.has_changed()) {
    filt_out.set_freq(200.0 + pedal.pot_center.val * 1000);
  }    

  // Right pot controls depth of phase shifter
  if (pedal.pot_left.has_changed()) {
    out_phasey.set_depth(pedal.pot_left.val);
  }     
}