#include <fifo.h>
#include <midi_messages.h>
#include <midi_serialization.h>
#include <usbboard.h>
#include <usbconfig.h>
#include <usbdrv.h>
#include <usbmidi.h>



#include <Wire.h>
#include <VL53L0X.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];

void sendNote(uint8_t channel, uint8_t note, uint8_t velocity) {
	USBMIDI.write((velocity != 0 ? 0x90 : 0x80) | (channel & 0xf));
	USBMIDI.write(note & 0x7f);
	USBMIDI.write(velocity &0x7f);
}


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

  // 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.setAddress(Sensor1_newAddress);
  delay(10);
  
  pinMode(XSHUT_pin2, INPUT);
  sensor2.init();
  sensor2.setAddress(Sensor2_newAddress);
  delay(10);
  
  pinMode(XSHUT_pin3, INPUT);
  sensor3.init();
  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');

    //Handle USB communication
    USBMIDI.poll();

    while (USBMIDI.available()) {
      // We must read entire available data, so in case we receive incoming
      // MIDI data, the host wouldn't get stuck.
      u8 b = USBMIDI.read();
    }

    int state1 = sensor1.readRangeContinuousMillimeters();
    // Serial.print("s1 : ");
    // Serial.print(state1);
    // Serial.print(" | ");
    if (state1 > 0 && tracksUp[0] == 0)
    {
      sendNote(0, 64, 127);
      tracksUp[0] = 1;
    }
    else
    {
      tracksUp[0] = 0;
    }
    
    
    int state2 = sensor2.readRangeContinuousMillimeters();
    // Serial.print("s2 : ");
    // Serial.print(state2);
    // Serial.print(" | ");
    if (state2 > 0 && tracksUp[1] == 0)
    {
      sendNote(0, 65, 127);
      tracksUp[1] = 1;
    }
    else
    {
      tracksUp[1] = 0;
    }

    
    int state3 = sensor3.readRangeContinuousMillimeters();
    // Serial.print("s3 : ");
    // Serial.println(state3);
    // Serial.print(" | ");
    if (state3 > 0 && tracksUp[2] == 0)
    {
      sendNote(0, 66, 127);
      tracksUp[2] = 1;
    }
    else
    {
      tracksUp[2] = 0;
    }

  
  // if (sensor1.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
  // if (sensor2.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
  // if (sensor3.timeoutOccurred()) { Serial.print(" TIMEOUT"); }


	// Flush the output.
	USBMIDI.flush();

}