#include "Adafruit_VL53L0X.h"
// Addresses to be assigned if there are two sensors
#define LOX1_ADDRESS 0x30
#define LOX2_ADDRESS 0x31
// Shutdown pins
#define SHT_LOX1 D3
#define SHT_LOX2 D2

// Motor pins
const int motorPin1 = D9;
// Active buzzer pin
const int buzzerPin = D7;
Adafruit_VL53L0X lox1 = Adafruit_VL53L0X();
Adafruit_VL53L0X lox2 = Adafruit_VL53L0X();
VL53L0X_RangingMeasurementData_t measure1;
VL53L0X_RangingMeasurementData_t measure2;

void setID() {
  digitalWrite(SHT_LOX1, LOW);    
  digitalWrite(SHT_LOX2, LOW);
  delay(10);
  digitalWrite(SHT_LOX1, HIGH);
  digitalWrite(SHT_LOX2, HIGH);
  delay(10);

  digitalWrite(SHT_LOX1, HIGH);
  digitalWrite(SHT_LOX2, LOW);

  if(!lox1.begin(LOX1_ADDRESS)) {
    Serial.println(F("Failed to initialize the first VL53L0X"));
    while(1);
  }
  delay(10);

  digitalWrite(SHT_LOX2, HIGH);
  delay(10);

  if(!lox2.begin(LOX2_ADDRESS)) {
    Serial.println(F("Failed to initialize the second VL53L0X"));
    while(1);
  }
}



void read_dual_sensors() {
  lox1.rangingTest(&measure1, false);
  lox2.rangingTest(&measure2, false);

  if(measure1.RangeMilliMeter < 100 || measure2.RangeMilliMeter < 100) {
    // If you get too close to sensor 1, make motor 1 vibrate and sound the buzzer
    analogWrite(motorPin1, 255); // Vibrate at maximum
  } else {
    analogWrite(motorPin1, 0); // Do not vibrate
  }
  
  if(measure1.RangeMilliMeter < 100 || measure2.RangeMilliMeter < 100) {
    // If you get too close to sensor 1, make motor 1 vibrate and sound the buzzer
    digitalWrite(buzzerPin, HIGH); // Activate buzzer
  } else {
    digitalWrite(buzzerPin, LOW); // Turn off buzzer
  }

  // Print sensor readings
  Serial.print(F("Sensor 1: "));
  if(measure1.RangeStatus != 4) {
    Serial.print(measure1.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }
  Serial.print(F("mm - Sensor 2: "));
  if(measure2.RangeStatus != 4) {
    Serial.print(measure2.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }
  Serial.println();
}




void setup() {
  Serial.begin(115200);

  pinMode(SHT_LOX1, OUTPUT);
  pinMode(SHT_LOX2, OUTPUT);
  pinMode(motorPin1, OUTPUT);
  pinMode(buzzerPin, OUTPUT);
  //pinMode(motorPin2, OUTPUT);

  digitalWrite(SHT_LOX1, LOW);
  digitalWrite(SHT_LOX2, LOW);

  Serial.println(F("Shutdown pins initialized..."));
  Serial.println(F("Both in reset mode... (pins low)"));
  
  Serial.println(F("Starting..."));
  setID();
}

void loop() {
  read_dual_sensors();
  delay(100);
}