#include "Adafruit_VL53L1X.h" //include libraries
#include <Adafruit_NeoPixel.h>
#ifdef __AVR__
#include <avr/power.h>  // Required for 16 MHz Adafruit Trinket
#endif

#define NUMPIXELS 15  // Popular NeoPixel ring size

const char node = '1';  // network address 1..6
const int ledPin = 3;   // the number of the LED pin
int threshold = 50;
int reactionTimeout = 5000; //reaction timeout = 5 seconds
char incomingByte; //declare a character
unsigned long startTime; //declare size variable for number storage, can store 32 bits (4 bytes)

Adafruit_NeoPixel pixels(NUMPIXELS, ledPin, NEO_GRB + NEO_KHZ800); //Store data for interacting with Neopixels.  Length, pin and pixel type are known at compile-time.
Adafruit_VL53L1X vl53 = Adafruit_VL53L1X();

void setup() {
  pixels.begin();  // INITIALIZE NeoPixel strip object
  pixels.setBrightness(50);
  pixels.clear();  // Set all pixel colors to 'off'
  pixels.show();


  Serial.begin(9600); //Sets the data rate in bits per second (baud) for serial data transmission.
  while (!Serial) delay(10);
  // put your setup code here, to run once:

  Wire.begin(); //This function initializes the Wire library and join the I2C bus as a peripheral.
  if (!vl53.begin(0x29, &Wire)) { //if sensor is not working
    Serial.print(F("Error on init of VL sensor: "));
    Serial.println(vl53.vl_status);
    while (1) delay(10);
  }
  if (!vl53.startRanging()) {
    Serial.print(F("Couldn't start ranging: "));
    Serial.println(vl53.vl_status);
    while (1) delay(10);
  }
  Serial.println(F("Ranging started"));

  // Valid timing budgets: 15, 20, 33, 50, 100, 200 and 500ms!
  vl53.setTimingBudget(50); //Valid timing budget for short ranging.
  Serial.print(F("Timing budget (ms): "));
  Serial.println(vl53.getTimingBudget());
}

void loop() {
  if (Serial.available() > 0) { //Get the number of bytes (characters) available for reading from the serial port.
     incomingByte = Serial.read(); //Reads incoming serial data.
    if (incomingByte == node) { //If the number equals the network adress
      colorWipe(pixels.Color(0, 255, 0), 0); //Execute green color
      int16_t distance;
      bool success = false;
      startTime = millis();
      while (millis() - startTime < reactionTimeout) { //While not yet timeout
        if ((millis() - startTime) > (reactionTimeout / 2)){ //If half of timeout
          colorWipe(pixels.Color(255, 150, 0), 0); //execute yellow color
        }
        if (vl53.dataReady()) { //If sensor ready to read
          distance = vl53.distance(); //create distance-data
          if (distance == -1) distance = 4000;
          if (distance < threshold) { //If measured distance is smaller then threshold
            success = true;
            break;
          }
        }
      }
      colorWipe(pixels.Color(0, 0, 0), 0);
      if (success == true) {
        Serial.print(9);
        success = false;
      } else {
        Serial.print(8);
      }
    }
  }

}


void colorWipe(uint32_t c, uint8_t wait) { //Initialize for a ColorWipe. ColorWipe() function initializes the NeoPattern object to execute the ColorWipe pattern. 
  for (uint16_t i = 0; i < pixels.numPixels(); i++) {
    pixels.setPixelColor(i, c);
    pixels.show();
    delay(wait);
  }
}