#ifndef LIDAR_H
#define LIDAR_H

#include <SparkFun_VL53L1X.h>

// LIDAR
SFEVL53L1X distanceSensor;
int budgetIndex = 4;
int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
int LED = LED_BUILTIN;
int lidarThres = 300; // mm 

void LIDAR_Setup(){
  pinMode(LED, OUTPUT);
  digitalWrite(LED, HIGH);
//  if (distanceSensor.begin() == 0)
//    Serial.println("Sensor online!");
  distanceSensor.startRanging();
  distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
  digitalWrite(LED, LOW);
}

float LIDAR_BallCapture(){
  int LIDARVal = 0;
  int ballCapture = 0;
  LIDARVal = distanceSensor.getDistance();
  Serial.print("LIDAR value: ");
  Serial.println(LIDARVal);
  if ((LIDARVal < lidarThres) && (LIDARVal > 0)){
    // Ball captured
    ballCapture = 1; 
  }
  return ballCapture;
}

#endif
