/*
This example shows how to take simple range measurements with the VL53L1X. The
range readings are in units of mm.
*/

#include <Wire.h>
#include <VL53L1X.h>

VL53L1X sensor;

#define NUM_LEDS 14
#define MAX_RANGE 345
#define MIN_RANGE 110

char DELIMITER = ',';

void setup()
{
  Serial.begin(115200); // Board USB Serial
  Serial1.begin(9600);   // Board RX-TX Serial
  Wire.begin();
  Wire.setClock(400000); // use 400 kHz I2C

  sensor.setTimeout(500);
  if (!sensor.init())
  {
    Serial.println("Failed to detect and initialize sensor!");
    while (1);
  }
  
  // Use long distance mode and allow up to 50000 us (50 ms) for a measurement.
  // You can change these settings to adjust the performance of the sensor, but
  // the minimum timing budget is 20 ms for short distance mode and 33 ms for
  // medium and long distance modes. See the VL53L1X datasheet for more
  // information on range and timing limits.
  sensor.setDistanceMode(VL53L1X::Long);
  sensor.setMeasurementTimingBudget(50000);

  // Start continuous readings at a rate of one measurement every 50 ms (the
  // inter-measurement period). This period should be at least as long as the
  // timing budget.
  sensor.startContinuous(50);
}

void loop()
{
  // measure distance
  uint16_t distance = sensor.read();
  Serial.println(distance);
  if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }

  // calculate relative distance
  if (distance < MIN_RANGE) {
    distance = MIN_RANGE;
  }
  if (distance > MAX_RANGE) {
    distance = MAX_RANGE;
  }
  float location = float(distance - MIN_RANGE) / float(MAX_RANGE - MIN_RANGE) * NUM_LEDS;
  Serial.println(location);

  // parse distance for serial communication
  char ledIndex = (char) location;
  Serial.println( (uint8_t) ledIndex);
  Serial.println();

  // communicate the distance value over serial
  Serial1.write(ledIndex);
}