#include <ESP32Servo.h>

#define okR  27
#define okG 32
#define okB 33

const int servoLidarPin = 17;
const int servoRojoPin = 2;
const int servoVerdePin = 4;
const int servoAzulPin = 16;
int bolasRpedidas = 3;
int bolasGpedidas = 3;
int bolasBpedidas = 3;
volatile int bolasRservidas = 0;
volatile int bolasGservidas = 0;
volatile int bolasBservidas = 0;
int listo = LOW;
int reset = 0;
int estado; //0=Vacio, 1= Trabajando, 2= Listo, 3= Error...)
int rLevel;
int gLevel;
int bLevel;
unsigned long startTime = 0;
const int timeDebounce = 500;
int tiempoCaida = 1000;
int timeDebug = 1000;
int reintentoBola = 0;

int gradosReposo = 0;
int gradosR = 60;
int gradosG = 120;
int gradosB = 180;

int servoR0 = 0;
int servoR1 = 180;
int posRojo = 0;
int servoG0 = 0;
int servoG1 = 180;
int posVerde = 0;
int servoB0 = 0;
int servoB1 = 180;
bool posAzul = 0;

unsigned long int tiempoMedida = 0;
int tiempoEntreMedidas = 10000;
int delayServo = 1;
int alturaMedida;

Servo servoLidar;
Servo servoRojo;
Servo servoVerde;
Servo servoAzul;

void  IRAM_ATTR detectadoR() { //IRAM_ATTR sirve para que se ejecute en la ram en lugar de en la flash y así es más rápida.
  if (millis() - startTime > timeDebounce) {
    bolasRservidas++;
    startTime = millis();
  }
}

void  IRAM_ATTR detectadoG() {
  if (millis() - startTime > timeDebounce) {
    bolasGservidas++;
    startTime = millis();
  }
}

void  IRAM_ATTR detectadoB() {
  if (millis() - startTime > timeDebounce) {
    bolasBservidas++;
    startTime = millis();
  }
}

void  IRAM_ATTR cambioListo() { //pulsador que fuerza listo desde el plc
  if (millis() - startTime > timeDebounce) {
    listo = !listo;
    startTime = millis();
  }
}



void setup() {

  pinMode(okR, INPUT_PULLUP);
  pinMode(okG, INPUT_PULLUP);
  pinMode(okB, INPUT_PULLUP);
  pinMode(34, INPUT); //pin para pruebas de lectura de nivel con entrada analogica.
  pinMode(18, INPUT_PULLUP);// pin para flip/flop de listo en PLC

  attachInterrupt(digitalPinToInterrupt(okR), detectadoR, RISING);
  attachInterrupt(digitalPinToInterrupt(okG), detectadoG, RISING);
  attachInterrupt(digitalPinToInterrupt(okB), detectadoB, RISING);
  attachInterrupt(digitalPinToInterrupt(18), cambioListo, RISING);

  servoLidar.attach(servoLidarPin);
  servoRojo.attach(servoRojoPin);
  servoVerde.attach(servoVerdePin);
  servoAzul.attach(servoAzulPin);
  Serial.begin(115200);
  servoLidar.write(gradosReposo);
  servoRojo.write(servoR1 - servoR0);
  servoVerde.write(servoG1 - servoG0);
  servoAzul.write(servoB1 - servoB0);
  tiempoMedida = millis();


}

void loop() {
  nivelTolvas();
  delay(10); // this speeds up the simulation
  Serial.print("Rojas: ");
  Serial.print(bolasRservidas);
  Serial.print("/");
  Serial.println(bolasRpedidas);
  Serial.print("Verdes: ");
  Serial.print(bolasGservidas);
  Serial.print("/");
  Serial.println(bolasGpedidas);
  Serial.print("Azules: ");
  Serial.print(bolasBservidas);
  Serial.print("/");
  Serial.println(bolasBpedidas);
  Serial.print("Estado: ");
  Serial.println(estado);
  Serial.print("Listo: ");
  Serial.println(listo);
  Serial.print("Reintento: ");
  Serial.println(reintentoBola);
  Serial.println("");


  if (reset == LOW) {
    switch (estado) { //etapas
      case 0:
        delay(timeDebug);
        if (listo == HIGH) {
          bolasRservidas = 0;
          bolasGservidas = 0;
          bolasBservidas = 0;
          delay(100);
          estado = 1;
          reintentoBola = 0;
        }
        break;

      case 1:
        delay(timeDebug);
        if (bolasRpedidas > bolasRservidas) { //Salida de bola roja
          switch (posRojo) {
            case 0:
              servoRojo.write(servoR0);
              posRojo = 1;
              break;
            case 1:
              servoRojo.write(servoR1);
              posRojo = 0;
              break;
          }
        }
        if (bolasGpedidas > bolasGservidas) { //Salida de bola verde
          switch (posVerde) {
            case 0:
              servoVerde.write(servoG0);
              posVerde = 1;
              break;
            case 1:
              servoVerde.write(servoG1);
              posVerde = 0;
              break;
          }
        }
        if (bolasBpedidas > bolasBservidas) { //Salida de bola azul
          switch (posAzul) {
            case 0:
              servoAzul.write(servoB0);
              posAzul = 1;
              break;
            case 1:
              servoAzul.write(servoB1);
              posAzul = 0;
              break;
          }
        }
        delay(tiempoCaida);
        if (bolasRpedidas <= bolasRservidas && bolasGpedidas <= bolasGservidas && bolasBpedidas <= bolasBservidas) {  // si estan todas las bolas pedidas
          estado = 2;
        }
        reintentoBola++;
        if (reintentoBola >= 10) estado = 3;
        break;

      case 2:
        delay(timeDebug);
        if (listo == LOW) {
          estado = 0;
        }
        break;

      case 3:
        delay(timeDebug);
        Serial.println("Error");
        if (listo == LOW) {
          estado = 0;
        }
        break;
    }
  }
}

void nivelTolvas() {
  if (millis() >= tiempoMedida + tiempoEntreMedidas) {
    for (int i = gradosReposo; i < gradosR; i++) {
      servoLidar.write(i);
      delay(delayServo);
    }
    medirNivel();
    rLevel = alturaMedida;
    for (int i = gradosR; i < gradosG; i++) {
      servoLidar.write(i);
      delay(delayServo);
    }
    medirNivel();
    gLevel = alturaMedida;
    for (int i = gradosG; i < gradosB; i++) {
      servoLidar.write(i);
      delay(delayServo);
    }
    medirNivel();
    bLevel = alturaMedida;
    for (int i = gradosB; i > gradosReposo; i--) {
      servoLidar.write(i);
      delay(delayServo);
    }
    Serial.print("Nivel Rojo: ");
    Serial.println(rLevel);
    Serial.print("Nivel Verde: ");
    Serial.println(gLevel);
    Serial.print("Nivel Azul: ");
    Serial.println(bLevel);
    tiempoMedida = millis();
  }
}

void medirNivel() {
  alturaMedida = map(analogRead(34), 0, 4095, 0, 100) ;
  delay (1000);
}