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

// IR SENSORS

// #define XSHUT_pin4 8 //not required for address change
#define XSHUT_pin3 8
#define XSHUT_pin2 9
#define XSHUT_pin1 10

//ADDRESS_DEFAULT 0b0101001 or 41
// #define Sensor4_newAddress 41  //not required address change
#define Sensor3_newAddress 41
#define Sensor2_newAddress 42
#define Sensor1_newAddress 43

// IR sensors
VL53L0X sensor3;
VL53L0X sensor2;
VL53L0X sensor1;

// bool last_sensor1_state = 0;
// bool last_sensor2_state = 0;
// bool last_sensor3_state = 0;
const int trackCount = 3;
int tracksUp[trackCount];


// https://github.com/arduino/tutorials/blob/master/ArduinoZeroMidi/PitchToNote.h
uint8_t intensity = 127;


void setup() {
  Serial.println("Setup");
  Serial.begin(115200);
  // Set the maximum speed in steps per second:

  // wait until serial port opens for native USB devices
  // while (! Serial) {
  //   delay(1);
  // }

  // VL53L0X
  
  /*WARNING*/
  //Shutdown pins of VL53L0X ACTIVE-LOW-ONLY NO TOLERANT TO 5V will fry them
  pinMode(XSHUT_pin1, OUTPUT);
  pinMode(XSHUT_pin2, OUTPUT);
  pinMode(XSHUT_pin3, OUTPUT);
  
  digitalWrite(XSHUT_pin1, LOW);
  digitalWrite(XSHUT_pin2, LOW);
  digitalWrite(XSHUT_pin3, LOW);

  Wire.begin();

  //Change address of sensor and power up next one

  pinMode(XSHUT_pin1, INPUT);
  // sensor1.init();
  sensor1.setTimeout(500);
  if (!sensor1.init())
  {
    Serial.println("Failed to detect and initialize sensor1!");
    while (1) {}
  }
  sensor1.setAddress(Sensor1_newAddress);
  delay(10);
  
  pinMode(XSHUT_pin2, INPUT);
  // sensor2.init();
  sensor2.setTimeout(500);
  if (!sensor2.init())
  {
    Serial.println("Failed to detect and initialize sensor2!");
    while (1) {}
  }
  sensor2.setAddress(Sensor2_newAddress);
  delay(10);
  
  pinMode(XSHUT_pin3, INPUT);
  // sensor3.init();
  sensor3.setTimeout(500);
  if (!sensor3.init())
  {
    Serial.println("Failed to detect and initialize sensor3!");
    while (1) {}
  }
  sensor3.setAddress(Sensor3_newAddress);
  delay(10);


  // sensor1.setTimeout(0);
  // sensor2.setTimeout(0);
  // sensor3.setTimeout(0);

  // // if (!sensor.init())
  // // {
  // //   Serial.println("Failed to detect and initialize  sensor!");
  // //   while (1) {}
  // // }
  sensor1.startContinuous();
  sensor2.startContinuous();
  sensor3.startContinuous();

  for (int i=0; i<trackCount; ++i) {
		tracksUp[i] = 0;
	}
}

void loop() {
    // Serial.println("loop");


    Serial.print("s1 : ");
    // int state1 = sensor1.readRangeContinuousMillimeters();
    // Serial.print(state1);
    // if ((state1 == 0 || state1 > 50) && tracksUp[0] == 0)
    // {
    //   Serial.print(" ON");
    //   noteOn(0, 84, intensity);
    //   MidiUSB.flush();
    //   tracksUp[0] = 1;
    // }else{
    //   Serial.print(" OFF");
    //   noteOff(0, 84, 0);
    //   MidiUSB.flush();
    //   tracksUp[0] = 0;
    // }
    checkSensor(sensor1, 0, 84);
    Serial.print(" | ");
    
    
    Serial.print("s2 : ");
    // int state2 = sensor2.readRangeContinuousMillimeters();
    // Serial.print(state2);
    // if ((state2 == 0 || state2 > 50) && tracksUp[1] == 0){
    //   Serial.print(" ON");
    //   noteOn(0, 86, intensity);
    //   MidiUSB.flush();
    //   tracksUp[1] = 1;
    // }else{
    //   Serial.print(" OFF");
    //   noteOff(0, 86, 0);
    //   MidiUSB.flush();
    //   tracksUp[1] = 0;
    // }
    checkSensor(sensor2, 1, 86);
    Serial.print(" | ");

    
    Serial.print("s3 : ");
    // int state3 = sensor3.readRangeContinuousMillimeters();
    // Serial.print(state3);
    // // Serial.print(" | ");
    // if ((state3 == 0 || state3 > 50) && tracksUp[2] == 0){
    //   Serial.print(" ON");
    //   noteOn(0, 88, intensity);
    //   MidiUSB.flush();
    //   tracksUp[2] = 1;
    // }else{
    //   Serial.print(" OFF");
    //   noteOff(0, 88, 0);
    //   MidiUSB.flush();
    //   tracksUp[2] = 0;
    // }
    checkSensor(sensor3, 2, 88);
    Serial.println("");
  
  // if (sensor1.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
  // if (sensor2.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
  // if (sensor3.timeoutOccurred()) { Serial.print(" TIMEOUT"); }


}

void checkSensor(VL53L0X sensor, int index, int note){
  int state = sensor.readRangeContinuousMillimeters();
  // Serial.print("s3 : ");
  Serial.print(state);
  // Serial.print(" | ");
  if ((state == 0 || state > 50) && tracksUp[index] == 0){
    Serial.print(" ON");
    noteOn(0, note, intensity);
    MidiUSB.flush();
    tracksUp[index] = 1;
  }else{
    Serial.print(" OFF");
    noteOff(0, note, 0);
    MidiUSB.flush();
    tracksUp[index] = 0;
  }
}

void noteOn(byte channel, byte pitch, byte velocity) {

  midiEventPacket_t noteOn = {0x09, 0x90 | channel, pitch, velocity};

  MidiUSB.sendMIDI(noteOn);
}

void noteOff(byte channel, byte pitch, byte velocity) {

  midiEventPacket_t noteOff = {0x08, 0x80 | channel, pitch, velocity};

  MidiUSB.sendMIDI(noteOff);
}