#include <TFT_eSPI.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <HX711.h>
#include <Bounce2.h>
#include <ESP32Servo.h>

#define ECHO_PIN 17
#define TRIG_PIN 16

#define TFT_MISO 19
#define TFT_MOSI 23
#define TFT_SCLK 18
#define TFT_CS   15
#define TFT_DC   2
#define DATA_PIN  4
#define CLOCK_PIN 14
#define BOTON_VACIAR 27
#define BOTON_FINALIZAR 26
#define BOTON_BAJAR 25
#define BOTON_SUBIR 33

HX711 hx711;
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
volatile float units;
Bounce boton_vaciar = Bounce();
Bounce boton_finalizar = Bounce();
Bounce boton_bajar = Bounce();
Bounce boton_subir = Bounce();
int pos = 0;
const int servoPin = 32;
float basura_maxima = 5.0;
float basura_actual = 0.0;
Servo servo;
int estado_motor = 0;
int   contadorLLenado = 0;
int   contadorObjetos = 0;

const unsigned int BAUD_RATE = 9600;

void setup() {
  Serial.begin(115200);
  tft.begin();
  hx711.begin(DATA_PIN, CLOCK_PIN);
  hx711.set_scale(434.f);
  pinMode(BOTON_VACIAR, INPUT);
  pinMode(BOTON_FINALIZAR, INPUT);
  pinMode(BOTON_BAJAR, INPUT);
  pinMode(BOTON_SUBIR, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  boton_vaciar.attach(BOTON_VACIAR);
  boton_vaciar.interval(25);
  boton_finalizar.attach(BOTON_FINALIZAR);
  boton_finalizar.interval(25);
  boton_bajar.attach(BOTON_BAJAR);
  boton_bajar.interval(25);
  boton_subir.attach(BOTON_SUBIR);
  boton_subir.interval(25);
  basura_actual = hx711.get_units();
  servo.attach(servoPin, 500, 2400);
  pantalla();
}

void loop() {
  units = hx711.get_units();
  if (units != basura_actual) {
    basura_actual = units;
    pantalla();
  }
  boton_subir.update();
  if (boton_subir.fell()) {
    imprimirDTTMD("Contenedor", "Abierto");
    estado_motor = 90;
    for (pos = 0; pos <= 90; pos += 1) {
      servo.write(pos);
      delay(15);
      float distance = readDistanceCM();
      if (distance <= 10) {
        contadorObjetos = contadorObjetos + 1;
      }
    }
    imprimirTTTMT("Porfavor", "Cierre", "Contenedor");
    while (true) {
      float distance = readDistanceCM();
      if (distance <= 10) {
        contadorObjetos = contadorObjetos + 1;
      }
      boton_bajar.update();
      if (boton_bajar.fell()) {
        imprimirDTTMD("Cerrando", "Contenedor");
        estado_motor = 0;
        for (pos = 90; pos >= 0; pos -= 1) {
          servo.write(pos);
          delay(15);
        }
        pantalla();
        break;
      }
      servo.write(estado_motor );
    }
  }
  boton_vaciar.update();
  if (boton_vaciar.fell()) {
    modoVaciado();
  }
  servo.write(estado_motor);
  float distance = readDistanceCM();
  if (distance <= 10) {
    contadorLLenado =   contadorLLenado + 1;
  }
  if (  contadorLLenado == 20) {
    contadorLLenado = 0;
    modoVaciado();
  }
}
void modoVaciado() {
  imprimirTTTMT("Entrando en", "Modo de", "Vaciado");
  while (true) {
    boton_finalizar.update();
    if (boton_finalizar.fell()) {
      units = hx711.get_units();
      if (units == 0.0 || units <= 0.1 ) {
        basura_actual = units;
        imprimirDTTMD("Vaciado", "Exitoso");
        delay(2000);
        pantalla();
        break;
      } else {
        imprimirTTTMT("Vaciado", "Erroneo", "........");
        delay(2000);
        imprimirDTTMD("Presione", "De nuevo");
      }
    }
    servo.write(estado_motor);
  }
}
float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}
void pintarNegra() {
  tft.fillScreen(ILI9341_BLACK);
  delay(2000);
}
void imprimirTTTMT(String a, String b, String c) {
  pintarNegra();
  establecerPosicion(32, 40, ILI9341_RED, 3);
  imprimirTexto(a);
  Serial.println(a);
  establecerPosicion(32, 120, ILI9341_RED, 3);
  imprimirTexto(b);
  Serial.println(b);
  establecerPosicion(32, 200, ILI9341_GREEN, 3);
  imprimirTexto(c);
  Serial.println(c);
}
void imprimirDTTMD(String a, String b) {
  pintarNegra();
  establecerPosicion(32, 40, ILI9341_RED, 3);
  imprimirTexto(a);
  Serial.println(a);
  establecerPosicion(32, 120, ILI9341_RED, 3);
  imprimirTexto(b);
  Serial.println(b);
}
void pantalla() {
  pintarNegra();
  establecerPosicion(32, 40, ILI9341_RED, 5);
  imprimirTexto("Basura");
  Serial.println("Basura");
  establecerPosicion(32, 120, ILI9341_RED, 5);
  imprimirTexto("Actual");
  Serial.println("Actual");
  units = hx711.get_units();
  establecerPosicion(32, 200, ILI9341_GREEN, 4);
  imprimirTexto(String(units) + " KG");
  Serial.println(String(units) + " KG");
}

void establecerPosicion(int x, int y, uint16_t color, int tamano) {
  tft.setCursor(x, y);
  tft.setTextColor(color);
  tft.setTextSize(tamano);

}
void imprimirTexto( String texto) {
  tft.println(texto);
}