#include <Wire.h>
#include "Adafruit_VL53L0X.h"

#define LED_GREEN     10
#define LED_YELLOW    11
#define LED_RED       12
#define LEVEL_TOP     250
#define LEVEL_BOTTOM  100

Adafruit_VL53L0X sensor = Adafruit_VL53L0X();

void setup() {
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_YELLOW, OUTPUT);
  pinMode(LED_RED, OUTPUT);

  Serial.begin(9600);
  while (! Serial) {
    delay(1);
  }
  Serial.println("Range sensor starting...");
  
  Wire.begin();
  if (!sensor.begin()) {
    digitalWrite(LED_RED, HIGH);
    Serial.println(F("Failed to boot VL53L0X"));
    while(1);
  }
  Serial.println("Range sensor started!");
}

void loop() {
  VL53L0X_RangingMeasurementData_t measure;

  sensor.rangingTest(&measure, false);
  Serial.println(measure.RangeMilliMeter );
  if (measure.RangeMilliMeter >= LEVEL_TOP) {
    digitalWrite(LED_GREEN, HIGH);
  } else {
    digitalWrite(LED_GREEN, LOW);
  }
  if (measure.RangeMilliMeter < LEVEL_TOP && 
      measure.RangeMilliMeter > LEVEL_BOTTOM) {
    digitalWrite(LED_YELLOW, HIGH);
  } else {
    digitalWrite(LED_YELLOW, LOW);
  }
  if (measure.RangeMilliMeter <= LEVEL_BOTTOM) {
    digitalWrite(LED_RED, HIGH);
  } else {
    digitalWrite(LED_RED, LOW);
  }
  delay(500);
  digitalWrite(10, LOW);
  delay(250);
}