// Choose your I2C implementation before including Tiny4kOLED.h
// The default is selected is Wire.h

// To use the Wire library:
//#include <Wire.h>

// To use the Adafruit's TinyWireM library:
//#include <TinyWireM.h>

// To use the TinyI2C library from https://github.com/technoblogy/tiny-i2c
//#include <TinyI2CMaster.h>

// The blue OLED screen requires a long initialization on power on.
// The code to wait for it to be ready uses 20 bytes of program storage space
// If you are using a white OLED, this can be reclaimed by uncommenting
// the following line (before including Tiny4kOLED.h):
//#define TINY4KOLED_QUICK_BEGIN

#include <Wire.h>   //sensor libraries
#include <VL53L0X.h>//

#include <Tiny4kOLED.h> //OLED library

VL53L0X sensor;
int LED = 1;
int speed_blink = 50;

void blink_led(int time_delay){
  digitalWrite(LED, HIGH);
  delay(time_delay);
  digitalWrite(LED, LOW);
  delay(time_delay);
}

void blink_init(int times_blink){
  int i = 0;
  while(i < times_blink){
    blink_led(speed_blink);
    i++;
  }
}

void setup() {
  pinMode(LED, OUTPUT);
  // Send the initialization sequence to the oled. This leaves the display turned off
  Wire.begin();
  sensor.init();
  sensor.setTimeout(500);
  sensor.startContinuous();
  blink_init(5);
  delay(1000);
  
  oled.begin();
  oled.setFont(FONT8X16);
  // Clear the memory before turning on the display
  oled.clear();
  // Turn on the display
  oled.on();
  // Switch the half of RAM that we are writing to, to be the half that is non currently displayed
  oled.switchRenderFrame();
  blink_init(10);
  delay(1000);
}

void loop() {
  int distance = sensor.readRangeContinuousMillimeters();
  updateDisplay(distance/10);
  
  if(distance < 100){
    digitalWrite(LED, HIGH);
  }
  else{
    digitalWrite(LED, LOW);
  }
  delay(100);
}

void updateDisplay(int dist) {
  // Clear the half of memory not currently being displayed.
  oled.clear();
  oled.setCursor(0, 1);
  oled.print(F("dist: "));
  oled.print(dist);
  oled.print(F("mm"));
  oled.switchFrame();
}