#include <Wire.h>
#include <VL6180X.h>

int serial;
//adresses
int adr1 = 81;
int adr2 = 82;
int adr3 = 83;
int adr4 = 84;
//reset pins
int s1 = 2;
int s2 = 4;
int s3 = 7;
int s4 = 8;
//calibration coefficients
uint8_t p2p_1 = 4;
uint8_t p2p_2 = 5;
uint8_t p2p_3 = 4;
uint8_t p2p_4 = 5;

VL6180X sensor;
VL6180X sensor1;
VL6180X sensor2;
VL6180X sensor3;
VL6180X sensor4;

void setup()
{
  pinMode(s1,OUTPUT);
  digitalWrite(s1,HIGH);
  pinMode(s2,OUTPUT);
  digitalWrite(s2,HIGH);
  pinMode(s3,OUTPUT);
  digitalWrite(s3,HIGH);
  pinMode(s4,OUTPUT);
  digitalWrite(s4,HIGH);

  Serial.begin(9600);
  Wire.begin();

  //setting adresses
  digitalWrite(s1,LOW);
  delay(200);
  digitalWrite(s1,HIGH);
  delay(200);
  sensor1.setAddress(adr1);

  sensor1.setTimeout(500);

  digitalWrite(s2,LOW);
  delay(200);
  digitalWrite(s2,HIGH);
  delay(200);
  sensor2.setAddress(adr2);

  sensor2.setTimeout(500);

  digitalWrite(s3,LOW);
  delay(200);
  digitalWrite(s3,HIGH);
  delay(200);
  sensor3.setAddress(adr3);

  sensor3.setTimeout(500);

  digitalWrite(s4,LOW);
  delay(200);
  digitalWrite(s4,HIGH);
  delay(200);
  sensor4.setAddress(adr4);

  sensor4.setTimeout(500);
  //initialization and configuration
  sensor1.init();
  sensor1.configureDefault();
  sensor2.init();
  sensor2.configureDefault();
  sensor3.init();
  sensor3.configureDefault();
  sensor4.init();
  sensor4.configureDefault();

}

void loop()
{
  Serial.print(sensor1.readRangeSingleMillimeters());
  if (sensor1.timeoutOccurred()) { Serial.println(" TIMEOUT"); }
  Serial.print(" ");

  Serial.print(sensor2.readRangeSingleMillimeters());
  if (sensor2.timeoutOccurred()) { Serial.println(" TIMEOUT"); }
  Serial.print(" ");

  Serial.print(sensor4.readRangeSingleMillimeters());
  if (sensor3.timeoutOccurred()) { Serial.println(" TIMEOUT"); }
  Serial.print(" ");

  Serial.print(sensor3.readRangeSingleMillimeters());
  if (sensor4.timeoutOccurred()) { Serial.println(" TIMEOUT"); }
  Serial.println(" ");
}