#include <ESP32Servo.h>
#ifdef ESP8266
#include <ESP8266WiFi.h>
#else  //ESP32
#include <WiFi.h>
#endif
#include "ModbusIP_ESP8266.h"
#include "Adafruit_VL53L0X.h"

/////// MODBUS /////////////////
//Offsets o direcciones de registros (Holding registers) para modbus
const int mbBolasRojas = 0;
const int mbBolasVerdes = 1;
const int mbBolasAzules = 2;
const int mbListo = 3;
const int mbReset = 4;
const int mbEstado = 5;
const int mbNivelRojo = 6;
const int mbNivelVerde = 7;
const int mbNivelAzul = 8;

// declaración de pines
const int servoLidarPin = 17;
const int servoRojoPin = 2;
const int servoVerdePin = 4;
const int servoAzulPin = 16;
const int okR = 27;
const int okG = 32;
const int okB = 33;

int bolasRpedidas = 3; //poner a 0 cuando se controle por modbus
int bolasGpedidas = 3;
int bolasBpedidas = 3;
volatile int bolasRservidas = 0;
volatile int bolasGservidas = 0;
volatile int bolasBservidas = 0;
int listo = 0;
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; // delay para no leer dos veces la misma bola
int tiempoCaida = 1000; //tiempo de espera antes de comprobar si ha caido la bola
int timeDebug = 1000; // tiempo para ralentizar el paso entre etapas y poder leer la salida serie
int reintentoBola = 0;

//Ajuste de las posiciones de medición de altura
int gradosReposo = 0;
int gradosR = 60;
int gradosG = 120;
int gradosB = 180;

//Ajuste de grados maximo y mínimo de los servos de cada tolva
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;
int findWiFi = 0;


Servo servoLidar;
Servo servoRojo;
Servo servoVerde;
Servo servoAzul;
ModbusIP modbus;
Adafruit_VL53L0X lox = Adafruit_VL53L0X();
/////////* Añade tu SSID & Clave para acceder a tu Wifi */

const char* ssid = "Grupo-1";      // Tu SSID 2C2_PROTOTIPOS   // COI_0_2Ghz
const char* password = ".Grupo1.";  //Tu Clave
//const char* ssid = "ESJ_ROBOTICS";
//const char* password = "12345678";

//IPAddress ip(192,168,88,253); //IP COCHE JUANMA
IPAddress ip(192, 168, 88, 14);  //IP COCHE BLANCO

IPAddress gateway(192, 168, 88, 1);  // QUITAR ESTA LINEA SI USAS EL DHCP
IPAddress subnet(255, 255, 255, 0);  // QUITAR ESTA LINEA SI USAS EL DHCP
/*
  IPAddress primaryDNS(8, 8, 8, 8);   // optional
  IPAddress secondaryDNS(8, 8, 4, 4); // optional
*/

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 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.

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

  servoLidar.attach(servoLidarPin);
  servoRojo.attach(servoRojoPin);
  servoVerde.attach(servoVerdePin);
  servoAzul.attach(servoAzulPin);

  Serial.begin(115200);
  //servos en posicion incial
  servoLidar.write(gradosReposo);
  servoRojo.write(servoR1 - servoR0);
  servoVerde.write(servoG1 - servoG0);
  servoAzul.write(servoB1 - servoB0);

  WiFi.mode(WIFI_STA);
  WiFi.config(ip, gateway, subnet);
  WiFi.begin(ssid, password);
  //Conectar con SSID de red local
  WiFi.begin(ssid, password);  //Este es el nombre del SSID, se puede añadir el password en el segundo argumento
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.println("WiFi connected");
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());  //Mostrar IP asignada
    findWiFi++;
    if (findWiFi >= 20) {
      break;
    }
    Serial.println("Adafruit VL53L0X test.");
    if (!lox.begin()) {
      Serial.println(F("Failed to boot VL53L0X"));
      while (1);
    }
    lox.startRangeContinuous();

  }

  modbus.server();               //ESP32 en modo servidor
  modbus.addHreg(mbBolasRojas);  //Crear Holding registers en la dirección seleccionada
  modbus.addHreg(mbBolasVerdes);
  modbus.addHreg(mbBolasAzules);
  modbus.addHreg(mbListo);
  modbus.addHreg(mbReset);
  modbus.addHreg(mbEstado);
  modbus.addHreg(mbNivelRojo);
  modbus.addHreg(mbNivelVerde);
  modbus.addHreg(mbNivelAzul);

  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 == 0) {
    switch (estado) { //etapas
      case 0:
        delay(timeDebug);
        if (listo == 1) {
          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 == 0) {
          estado = 0;
        }
        break;

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

    }
  }
  if (reset == 1){
    estado=0;
  }
  modbusUpdate();
}

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() {
  delay (500);
   if (lox.isRangeComplete()) {
    aluraMedida=lox.readRange(); //lectura de la altura medida
  }
}

void modbusUpdate() {
  if (millis() > tiempoMedida + 2000) {
    modbus.task();

    //Actualización de registros recibidos del PLC
    bolasRpedidas = modbus.Hreg(mbBolasRojas);
    bolasGpedidas = modbus.Hreg(mbBolasVerdes);
    bolasBpedidas = modbus.Hreg(mbBolasAzules);
    listo = modbus.Hreg(mbListo);
    reset = modbus.Hreg(mbReset);

    //Envío de registros a PLC
    modbus.Hreg(mbEstado, estado);
    modbus.Hreg(mbNivelRojo, rLevel);
    modbus.Hreg(mbNivelVerde, gLevel);
    modbus.Hreg(mbNivelAzul, bLevel);
    tiempoMedida = millis();
  }
}