//CONFIG
#define DISTANCIAACTIVACION 15 //Distancia en cm. a la que se activa el cambio de emociones
#define TESTSENSOR 0 //Poner en 0 para que funcione, Poner en 1 para testear el Ultrasonico y los leds

//PINES
#define PINLEDA 3
#define PINLEDB 5
#define PINLEDC 6
#define PINLEDD 9

#define PINUSTRIG 11
#define PINUSECHO 12

//VARIABLES
int distancia = 0;
int emocion = 1;
int emocionanterior = 1;


//FUNCION DEL LED
int elegirled(int valor) {

  switch (valor) {

    case 1:
      return PINLEDA;
      break;

    case 2:
      return PINLEDB;
      break;

    case 3:
      return PINLEDC;
      break;

    case 4:
      return PINLEDD;
      break;

    default:
      return PINLEDA;
      break;
  }
}

// FUNCIONES DEL SENSOR ULTRASONICO
long u_tiempo(int trigger_pin, int echo_pin) {
  digitalWrite(trigger_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigger_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigger_pin, LOW);
  long microseconds = pulseIn(echo_pin, HIGH);
  return microseconds;
}

long u_distancia(int trigger_pin, int echo_pin) {
  long microseconds = u_tiempo(trigger_pin, echo_pin);
  long u_distancia;
  u_distancia = microseconds / 29 / 2;
  if (u_distancia == 0) {
    u_distancia = 999;
  }
  return u_distancia;
}

void setup() {

  Serial.begin(9600);

  pinMode(PINLEDA, OUTPUT);
  pinMode(PINLEDB, OUTPUT);
  pinMode(PINLEDC, OUTPUT);
  pinMode(PINLEDD, OUTPUT);

  pinMode(PINUSTRIG, OUTPUT);
  pinMode(PINUSECHO, INPUT);

  digitalWrite(PINLEDA, HIGH);
  digitalWrite(PINLEDB, TESTSENSOR);
  digitalWrite(PINLEDC, TESTSENSOR);
  digitalWrite(PINLEDD, TESTSENSOR);

}

void loop() {

  distancia = u_distancia(PINUSTRIG, PINUSECHO);

  while (TESTSENSOR == 1) {
    Serial.print("Distancia: ");
    Serial.println(distancia);
    Serial.println("-");

    delay(500);

    distancia = u_distancia(PINUSTRIG, PINUSECHO);
  }

  if (distancia <= DISTANCIAACTIVACION) {

    while (distancia <= DISTANCIAACTIVACION) {
      distancia = u_distancia(PINUSTRIG, PINUSECHO);
      delay(100);
    }

    emocion++;
    if (emocion > 4) {
      emocion = 1;
    }
  }

  if (emocion != emocionanterior) {
    int ledemocion = elegirled(emocion);
    int ledemocionanterior = elegirled(emocionanterior);

    for (int i = 0; i <= 255; i = i + 5) {

      analogWrite(ledemocion, i);
      analogWrite(ledemocionanterior, (255 - i));

      delay(10);

    }

    emocionanterior = emocion;

  }

  delay(100);

}