#include "ObjectDetection.h"
#include "TINAH.h"
#include "Adafruit_VL53L0X.h"


constexpr int SHUTDOWNLeft = 0;
constexpr int SHUTDOWNRight = 1;

constexpr int shutdown1 = 1;
constexpr int shutdown2 = 2;




ObjectDetection::ObjectDetection() {

}



boolean ObjectDetection::objectDetect(int courseState) {
  Adafruit_VL53L0X lox = Adafruit_VL53L0X();
  if (!lox.begin()) {
    Serial.println(F("Failed to boot VL53L0X"));
    while(1);
  }

  switch (courseState) {
    case 1:
      analogWrite(SHUTDOWNRight, HIGH);
      analogWrite(SHUTDOWNLeft, LOW);
      break;
    case 2:
      analogWrite(SHUTDOWNRight, LOW);
      analogWrite(SHUTDOWNLeft, HIGH);
      break;
  }

  VL53L0X_RangingMeasurementData_t measure;

  Serial.print("Reading a measurement... ");
  lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

  if (measure.RangeStatus != 4) {  // phase failures have incorrect data
    Serial.print("Distance (mm): "); Serial.println(measure.RangeMilliMeter);
    if(measure.RangeMilliMeter<120){
      Serial.print("EWOOOOKKKK");
      return true;
    }
  } else {
    Serial.println(" out of range ");
    return false;
  }




  return false;
};