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

Adafruit_VL53L0X lox = Adafruit_VL53L0X();

// Which pin on the Arduino is connected to the NeoPixels?
#define PIN        4 // On Trinket or Gemma, suggest changing this to 1

// How many NeoPixels are attached to the Arduino?
#define NUMPIXELS 12 // Popular NeoPixel ring size

Adafruit_NeoPixel pixels(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);


void setup() {
  Serial.begin(115200);
  Serial.println("Prueba de sensores laser VL53L0X");
  pinMode(5,OUTPUT); digitalWrite(5,LOW);
  pinMode(6,OUTPUT); digitalWrite(6,LOW);
  pinMode(7,OUTPUT); digitalWrite(7,LOW);
  pixels.begin(); // INITIALIZE NeoPixel strip object (REQUIRED)
  pixels.clear(); // Set all pixel colors to 'off'
  //Efecto inicial de leds para comprobarlos
  int vr=0; int vg=0; int vb=0;
  for(int n=0;n<9;n++){
    switch(n){
      case 0: vr=255;vg=0;vb=0;break;
      case 1: vr=128;vg=128;vb=0;break;
      case 2: vr=0;vg=255;vb=0;break;
      case 3: vr=0;vg=128;vb=128;break;
      case 4: vr=0;vg=0;vb=255;break;
      case 5: vr=0;vg=128;vb=255;break;
      case 6: vr=128;vg=255;vb=255;break;
      case 7: vr=255;vg=255;vb=255;break;
      default: vr=0;vg=0;vb=0;
     }
    for(int i=0;i<NUMPIXELS;i++){
      pixels.setPixelColor(i, pixels.Color(vr,vg, vb));
    }
    pixels.show();
  }
}

//Defino 5 zonas de distancia
int constante1=1500;
int constante2=1000;
int constante3=750;
int constante4=500;
int constante5=250;

void loop(){
  VL53L0X_RangingMeasurementData_t measure;
  int medidaSensor[3];
  for(int i=0;i<3;i++){
    Serial.print("Leyendo sensor ");Serial.println(i+1);
    digitalWrite(5+i,HIGH);
    delay(35);
    if (!lox.begin()) {
      Serial.println("Fallo de inicializar sensor");
    }else{
      lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
      if (measure.RangeStatus != 4) {  // phase failures have incorrect data
        
        int valorTmp=measure.RangeMilliMeter;
        Serial.print("Distance (mm): "); Serial.println(valorTmp);
        if (valorTmp>constante1) valorTmp=constante1; //Si midiera más de 1500, igualamos a 1500 para acotar
        if (valorTmp<0) valorTmp=0; //Si hubiera negativos, igualamos a cero. Por si acaso
        medidaSensor[i]=valorTmp;//Anotamos medida
      } else {
        Serial.println(" out of range ");
        medidaSensor[i]=constante1; //Si fuera de rango, ponemos valor máximo de 1500
      }
    }
    Serial.println("");
    digitalWrite(5+i,LOW);
  }//for
  //Aquí ya hemos obtenido medidas para los 3 sensores
  //Calculamos el valor mínimo
  int valMin=medidaSensor[0];
  if (medidaSensor[1]<valMin) valMin=medidaSensor[1];
  if (medidaSensor[2]<valMin) valMin=medidaSensor[2];
  //
  int ani1r=0;  int ani1g=0;   int ani1b=0;
  int ani2r=0;  int ani2g=0;   int ani2b=0;
  if ((valMin>=constante1)                     ){ani1r=  0;ani1g=255;ani1b=0;  ani2r=  0;ani2g=  0;ani2b=128;}
  if ((valMin>=constante2)&&(valMin<constante1)){ani1r=255;ani1g=255;ani1b=0;  ani2r=  0;ani2g=128;ani2b=  0;}
  if ((valMin>=constante3)&&(valMin<constante2)){ani1r=255;ani1g=128;ani1b=0;  ani2r=  0;ani2g=255;ani2b=  0;}
  if ((valMin>=constante4)&&(valMin<constante3)){ani1r=255;ani1g=  0;ani1b=0;  ani2r=128;ani2g=255;ani2b=  0;}
  if ((valMin>=constante5)&&(valMin<constante4)){ani1r=255;ani1g=  0;ani1b=0;  ani2r=255;ani2g=128;ani2b=  0;}
  if (                      (valMin<constante5)){ani1r=255;ani1g=  0;ani1b=0;  ani2r=255;ani2g=  0;ani2b=  0;}
  

  //Cargo los valores
  for(int n=0;n< 8;n++) {pixels.setPixelColor(n,pixels.Color(ani1r,ani1g,ani1b));}
  for(int n=8;n<12;n++) {pixels.setPixelColor(n,pixels.Color(ani2r,ani2g,ani2b));}
  //Se manda a los leds
  pixels.show();
  //Quitamos Delay para que vaya rápido y sea sensible
  //delay(1000);
}