#include <Wire.h>
#include <VL53L0X.h>

VL53L0X sensor1;
VL53L0X sensor2;

const int XSHUT1 = 3;
const int XSHUT2 = 5;

const int speakerPin1 = 9;
const int speakerPin2 = 10;

const int minDist = 50;
const int maxDist = 4850;

int octaves[5][12] = {
  {131, 139, 147, 156, 165, 175, 185, 196, 208, 220, 233, 247},
  {262, 277, 294, 311, 330, 349, 370, 392, 415, 440, 466, 494},
  {523, 554, 587, 622, 659, 698, 740, 784, 831, 880, 932, 988},
  {1047, 1109, 1175, 1245, 1319, 1397, 1480, 1568, 1661, 1760, 1865, 1976},
  {2093, 2217, 2349, 2489, 2637, 2794, 2960, 3136, 3322, 3520, 3729, 3951}
  };

String notes[12] = {"do", "do#", "ré", "ré#", "mi", "fa", "fa#", "sol", "sol#", "la", "la#", "si"};

void setup()
{
  Serial.begin(9600);
  Wire.begin();
  
  pinMode(XSHUT1, OUTPUT);
  pinMode(XSHUT2, OUTPUT);

  // Shut down both sensors
  digitalWrite(XSHUT1, LOW);
  digitalWrite(XSHUT2, LOW);
  delay(10);

  // Turn on the first sensor
  digitalWrite(XSHUT1, HIGH);
  delay(10);
  sensor1.init();
  sensor1.setAddress(0x30);
  delay(10);

  // Turn on the second sensor
  digitalWrite(XSHUT2, HIGH);
  delay(10);
  sensor2.init();

  sensor1.setTimeout(50000);
  sensor2.setTimeout(50000);
  
  if (!sensor1.init() || !sensor2.init())
  {
    Serial.println("Failed to detect and initialize sensor 1 or sensor 2!");
    while (1) {}
  }

  sensor1.startContinuous();
  sensor2.startContinuous();

  pinMode(speakerPin1, OUTPUT);
  pinMode(speakerPin2, OUTPUT);

}

void loop()
{
  int distance1 = sensor1.readRangeContinuousMillimeters();
  int true_dist1 = distance1 - 50;

  int distance2 = sensor2.readRangeContinuousMillimeters();
  int true_dist2 = distance2 - 50;

  if (sensor1.timeoutOccurred() || sensor2.timeoutOccurred())
  {
    Serial.println("TIMEOUT");
    noTone(speakerPin1);
    noTone(speakerPin2);
    return;
  }
  
  // If both distances are valid
  if ((distance1 >= minDist && distance1 <= maxDist) && (distance2 >= minDist && distance2 <= maxDist))
  {
    int freq = 20;
    int num_octave = true_dist1/120;
    int last_dist = true_dist1%120;
    int ind_note = last_dist/10;

    int time = 100;
    int n = true_dist2/120;
    time = n*200;

    freq = octaves[num_octave][ind_note];
    String note = notes[ind_note];
    Serial.print(note);
    Serial.print(" ");
    Serial.print(num_octave + 2);
    Serial.print(" ");
    Serial.print(freq);
    Serial.println("Hz");

    tone(speakerPin1, freq);
    tone(speakerPin2, freq);

    delay(time);  // The second hand controls the delay

  } 
  
  else
  {
    noTone(speakerPin1);
    noTone(speakerPin2);
  }

}