#include "Adafruit_VL53L0X.h"
#include <LiquidCrystal.h>

//XSHUT Pins
#define XSHUT1 8
#define XSHUT2 9

//Buzzer Pin
#define BUZZER 10

//Screen Pins
#define RS 12
#define EN 11
#define D4 5
#define D5 4
#define D6 3
#define D7 2

//Caracter custom pentru a fi afisat pe display
byte p5[8] = {
  0x1F,
  0x1F,
  0x1F,
  0x1F,
  0x1F,
  0x1F,
  0x1F,
  0x1F
};



//Obiect Senzori Distanta
Adafruit_VL53L0X lox1 = Adafruit_VL53L0X();
Adafruit_VL53L0X lox2 = Adafruit_VL53L0X();

//Obiect LCD
LiquidCrystal lcd(RS, EN, D4, D5, D6, D7);

int dist1, dist2 = 0;

void setup() {
  //Initializam caracterul custom cu id-ul 1
  lcd.createChar(1, p5);
  //Initializam lcd-ul cu marimea 16 x 2
  lcd.begin(16,2);

  //Setare pin buzzer ca output
  pinMode(BUZZER, OUTPUT);

  //Setare pini XSHUT (pentru schimbarea adreselor pe i2c) ca output
  pinMode(XSHUT1, OUTPUT);
  pinMode(XSHUT2, OUTPUT);

  //Setare pininilor XSHUT1 si XSHUT2 pe LOW pentru a reseta senzorul
  delay(10);
  digitalWrite(XSHUT1, LOW);
  digitalWrite(XSHUT2, LOW);

  //Setarea pinilor XSHUT1 si XSHUT2 pe High pentru a reseta senzorul
  delay(10);
  digitalWrite(XSHUT1, HIGH);
  digitalWrite(XSHUT2, HIGH);

  //Setarea pinului XSHUT2 pe low pentru a putea seta adresa primului senzor
  delay(10);
  digitalWrite(XSHUT2, LOW);
  lox1.begin(0x30);

  //Setarea pinului XSHUT2 pe high pentru a seta adresa la al doilea senzor
  delay(10);
  digitalWrite(XSHUT2, HIGH);
  lox2.begin(0x31);
}

void loop() {
  //Obiecte masurare
  VL53L0X_RangingMeasurementData_t measure1;
  VL53L0X_RangingMeasurementData_t measure2;

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

  if (measure1.RangeStatus != 4) {  // eroare in caz ca nu a fost masurat bine, a depasit limita senzorului de masurare
    dist1 = measure1.RangeMilliMeter;
  }
  else { // in caz ca a fost depasita limita senzorului, valoarea distantei va ramane 2001 in loc de a afisa o distanta eronata
    dist1 = 2001;
  }

  if (measure2.RangeStatus != 4) {  // eroare in caz ca nu a fost masurat bine, a depasit limita senzorului de masurare
    dist2 = measure2.RangeMilliMeter;
  }
  else {
    dist2 = 2001; // in caz ca a fost depasita limita senzorului, valoarea distantei va ramane 2001 in loc de a afisa o distanta eronata
  }

  //Afisare pe ecran a distantelor celor 2 senzori
  lcd.setCursor(0,0);
  lcd.print(dist1);
  lcd.print(" - ");
  lcd.print(dist2);

  lcd.setCursor(0,1);

  // Limitare distanta la 1000
  if (dist1 > 1000){
    dist1 = 1000;
  }

  // Limitare distanta la 1000
  if (dist2 > 1000){
    dist2 = 1000;
  }

  int j;

  // Interpolare a valorii masurate
  // Se va lua in considerare valoarea minima citita de cei 2 senzori
  if(dist1 < dist2){
    j = 1 + ((16 - 1) * (((float)dist1 - 1) / (1000 - 1)));
  }
  else {
    j = 1 + ((16 - 1) * (((float)dist1 - 1) / (1000 - 1)));
  }

  // Afisare pe display in functie de valoarea calculata mai sus
  for(int i = 0; i < (int)j; i++){
      lcd.setCursor(i,1);
      lcd.write(1);
      //Activare buzzer
      tone(BUZZER,  (3000 - (100*j)) );
  }

  delay(100);
  //Dezactivare buzzer
  noTone(BUZZER);
  delay(100);
  lcd.clear();

}