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

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

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
#define OLED_RESET    -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3c ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32

Adafruit_VL53L0X sensor = Adafruit_VL53L0X();
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int distance = 8192;
int tmp;

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

  Serial.begin(115200);
  while (! Serial) {
    delay(1);
  }

  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    while(1); // Don't proceed, loop forever
  }
  display.display();
  delay(2000);

  display.clearDisplay();
  
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);

  Wire.begin();
  if (!sensor.begin()) {
    digitalWrite(LED_RED, HIGH);
    while(1);
  }
}

void loop() {
  VL53L0X_RangingMeasurementData_t measure;

  sensor.rangingTest(&measure, false);
  distance = measure.RangeMilliMeter;
  
  if (distance >= LEVEL_TOP) {
    digitalWrite(LED_GREEN, HIGH);
  } else {
    digitalWrite(LED_GREEN, LOW);
  }
  if (distance < LEVEL_TOP && 
      distance > LEVEL_BOTTOM) {
    digitalWrite(LED_YELLOW, HIGH);
  } else {
    digitalWrite(LED_YELLOW, LOW);
  }
  if (distance <= LEVEL_BOTTOM) {
    digitalWrite(LED_RED, HIGH);
  } else {
    digitalWrite(LED_RED, LOW);
  }

  // This communication protocol is used by the program from Week 14
  Serial.write(0xff);Serial.write(0xff);  // start of frame
  Serial.write(highByte(distance));       // high byte
  Serial.write(lowByte(distance));        // low byte

  display.clearDisplay();
  display.setCursor(0,0);
  display.print("Distance:");
  display.setCursor(0,16);
  display.print(distance);
  display.print(" mm");
  display.display();

  delay(500);
  digitalWrite(10, LOW);
  delay(250);
}