#include <SPI.h>
#include <Adafruit_ILI9341.h>
#include <Arduino.h>
#include <XPT2046_Touchscreen.h>
#include<ESP32Servo.h>
 
//***************** PAQUETE A *****************
Servo myservo_1;
#define SERVO_1 17
#define ENABLE_1 18
#define DIR_1 8           
#define STEP_1 3          
#define pulsador1_1 10     
#define pulsador2_1 9      

//***************** PAQUETE B *****************
Servo myservo_2;
#define SERVO_2 36
#define ENABLE_2 37
#define DIR_2 38           
#define STEP_2 39          
#define pulsador1_2 35      
#define pulsador2_2 48      

//***************** PAQUETE C *****************
Servo myservo_3;
#define SERVO_3 13
#define ENABLE_3 20
#define DIR_3 21          
#define STEP_3 47          
#define pulsador1_3 19      
#define pulsador2_3 14      

//***************** PAQUETE D *****************
#define SENSOR 12

#define TFT_DC 42
#define TFT_CS 1
#define TFT_MOSI 41
#define TFT_CLK 40
#define TFT_MISO 16
#define TFT_RESET 2

#define TIRQ_PIN 4
#define TS_DO 5
#define TS_DIN 6
#define TS_CLK 15
#define CS_PIN 7

//*************DECLARACIÓN DE MÓDULOS************

// Paquete A y B 
void GirarMotores(bool, int, int, int, int, int, int);
// Paquete C
void GirarSoporte(int ,bool ,int, int, int);
// Paquete D
int RESUMEN(int);
void menu_RESUMEN();
void menu();
void CalibrarActuadores();
void contarClamshell();

//************************************************

int sensor_ant=0; 
int motores_mov=0; 
//Sentido del motor
bool HORARIO = HIGH;
bool ANTIHORARIO = LOW;

//CALIBRAR CON OTRO CÓDIGO POR SEPARADO
const int ANG_A_INI = 90;
const int ANG_A_FIN = 150;

const int ANG_B_INI = 90;
const int ANG_B_FIN = 50;

const int ANG_C_INI = 80;
const int ANG_C_FIN = 0;

//**********MODULO D***************************
XPT2046_Touchscreen ts(CS_PIN, TIRQ_PIN); 
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS , TFT_DC,TFT_MOSI,TFT_CLK,TFT_RESET,TFT_MISO); 

int centi=1; 
int permitido_1=0; 
unsigned long tiempo;
int touch=1; int touch_2=0; 
//-------------------------------------------
int long anterior_X_S=0; 
int long anterior_Y_S=0; 
#define BOXSIZE 50
//#define BOXSIZE_2 30
#define BOXSIZE_2 40
int menu_opcion=0;
int centinela;
int ANTERIOR_dist=0;

int cont=0;           //Cant clamshell
int EstadoAnterior = 1; // Inicializamos en 1 porque al principio no debería haber obstáculo
int Estado = 0;

int activa_C; 


//************ DEFINICIÓN DE INTERRUPCIÓN MÓDULO D**********************

//  Se utiliza el pin IQR para la interrupcion de la pantalla si el cursor
//  del touch está en un rango de valores, en este caso dentro de la opcion detener
void touchInterrupt_1() {
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();

  TS_Point p = ts.getPoint();

  delay(40);
  /*Se detecta la interrupcion en la opcion DETENER del menu principal 
    y se queda estancado ahí mientras no se detecta la interrupcion de 
    volver a iniciarlo  
  */
  if (p.x > 1600 && p.x <3000 && p.y > 1900 && p.y <2350) {
    int valido=0;
    Serial.println("Se ingresa la interrupcion");
    while(valido==0){
      digitalWrite(ENABLE_1, HIGH);
      digitalWrite(ENABLE_2, HIGH);
      digitalWrite(ENABLE_3, HIGH);
      TS_Point p = ts.getPoint();
      if(p.x > 1600 && p.x <3000 && p.y > 3200 && p.y <3500){
        valido=1;
      }
    }
  }
}

//Interrupcion cuando se entra a la opcion VER RESUMEN y se necesita


//*************DEFINICIÓN DE MÓDULOS************
void menu(){

  /*
    Se imprime la pantalla del menu principal con las 
    opciones que lo controlan
  */
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();

  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+20, 45);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  tft.println("MENU");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 70, BOXSIZE+50, BOXSIZE, ILI9341_GREEN);
  tft.setCursor(95, 45+10+35);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("INICIAR");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 80+BOXSIZE, BOXSIZE+50, BOXSIZE, ILI9341_WHITE);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+28, BOXSIZE+100);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("DETENER");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 2*BOXSIZE+90, BOXSIZE+50, BOXSIZE, ILI9341_BLUE);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+15, 2*BOXSIZE+110);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("VER RESUMEN");

  tft.setCursor(65,2*BOXSIZE+160);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("Seleccione el modo");
  tft.setCursor(85,2*BOXSIZE+170);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("de operacion");

  tft.setCursor(85,2*BOXSIZE+190);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("!SUERTE :) !");
}

int RESUMEN(int centinela){
  while(centinela){
    int x_Pantalla = tft.width();
    int y_Pantalla = tft.height();
    delay(10);

    TS_Point p = ts.getPoint();

    delay(40);

    Serial.println(p.x);
    Serial.println(p.y);

    if(anterior_X_S!=p.x || anterior_Y_S!=p.y){
      Serial.println("ENTRO NUEVO PUNTO");
          
      if(p.y>=y_Pantalla-(BOXSIZE_2) && p.y<=y_Pantalla-1){
        if((p.x>=0) && p.x<BOXSIZE_2){
          Serial.println("Entro en la opcion STOP");
        }else if(p.x>x_Pantalla-BOXSIZE_2 && p.x<=x_Pantalla-1){
          Serial.println("Entro en la opcion MENU");
          return centinela=0; 
        }
      }
      anterior_X_S=p.x;
      anterior_Y_S=p.y;
    }
  }
  return 0;
}


void menu_RESUMEN(){
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();
  tft.fillRect(0, 0, BOXSIZE_2, BOXSIZE_2, ILI9341_RED);
  tft.setCursor(10,10);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("STOP");

  tft.fillRect(x_Pantalla-BOXSIZE_2-80, 0, BOXSIZE_2, BOXSIZE_2, ILI9341_RED);
  tft.setCursor(x_Pantalla-BOXSIZE_2-25-60,10);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("MENU");
}

void GirarMotores(bool SENTIDO, int PIN_DIR_A, int PIN_STEP_A, 
                  int PIN_EN_A, int PIN_DIR_B, int PIN_STEP_B, 
                  int PIN_EN_B, int FIN_CARRERA_A, int FIN_CARRERA_B){
  
  Serial.println("Girando ambos motores - paquete A y B");

  // Habilitar ambos motores
  digitalWrite(PIN_EN_A, LOW);
  digitalWrite(PIN_EN_B, LOW);  

  // Establecer direcciones
  digitalWrite(PIN_DIR_A, SENTIDO);
  digitalWrite(PIN_DIR_B, SENTIDO);

  // Girar ambos motores hasta que ambos alcancen su limit switch
  do{ 
    digitalWrite(PIN_STEP_A, HIGH);
    digitalWrite(PIN_STEP_B, HIGH);
    delay(5); 
    digitalWrite(PIN_STEP_A, LOW);
    digitalWrite(PIN_STEP_B, LOW);
    delay(5);
  }while (digitalRead(FIN_CARRERA_A) == HIGH && digitalRead(FIN_CARRERA_B) == HIGH);

  delay(1000);
}

void GirarSoporte(int FIN_CARRERA, bool SENTIDO, int PIN_DIR, int PIN_STEP, int PIN_EN){
  Serial.println("Girando motor - paquete C");
  if(digitalRead(FIN_CARRERA)==LOW){
    activa_C=1; 
    digitalWrite(PIN_EN, HIGH);
    return;
  }
  // Habilitar motor
  digitalWrite(PIN_EN, LOW);
  // Establecer direccion
  digitalWrite(PIN_DIR, SENTIDO);
  while (digitalRead(FIN_CARRERA) == HIGH) { 
    digitalWrite(PIN_STEP, HIGH);
    delay(5); 
    digitalWrite(PIN_STEP, LOW);
    delay(5);
  }
  delay(1000); 
}

void CalibrarActuadores() {
  Serial.println("calibrando actuadores");

  //Los servos se inicializa en la posición inicial
  myservo_1.write(ANG_A_INI);   //Servo A
  myservo_2.write(ANG_B_INI);   //Servo B  
  myservo_3.write(ANG_C_INI);   //Servo C 
  delay(1000);  

  //Calibrar Motor A y B
  digitalWrite(ENABLE_1, LOW);  
  digitalWrite(ENABLE_2, LOW);  
  digitalWrite(ENABLE_3, LOW);  

  digitalWrite(DIR_1, ANTIHORARIO);  
  digitalWrite(DIR_2, ANTIHORARIO); 
  digitalWrite(DIR_3, ANTIHORARIO);  

  while (digitalRead(pulsador1_1) == HIGH) { 
    digitalWrite(STEP_1, HIGH);
    delay(5);  
    digitalWrite(STEP_1, LOW);
    delay(5);
  }
  
  while (digitalRead(pulsador1_2) == HIGH) {  
    digitalWrite(STEP_2, HIGH);
    delay(5); 
    digitalWrite(STEP_2, LOW);
    delay(5);
  }

  digitalWrite(ENABLE_1, HIGH); 
  digitalWrite(ENABLE_2, HIGH); 
  digitalWrite(ENABLE_3, HIGH);  

  //Calibrar Motor C
  GirarSoporte(pulsador1_3, HORARIO, DIR_3, STEP_3, ENABLE_3);

  tft.setCursor(10,2*BOXSIZE+70+45);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("ACTUADORES INICIALIZADOS");
}

void contarClamshell(){
  if (Estado == 1 && EstadoAnterior == 0) {
    cont++; // Aumenta el contador cuando el clamshell se haya separado
  }
  // Actualizamos el EstadoAnterior para la siguiente iteración
  EstadoAnterior = Estado;
  delay(100); 
}

void funcion_motores(){
  Serial.println("Entro en la opcion INICIAR");

  /*  Se imprime los funcionamientos de los módulos con 
      sus respectivos servos en la pantalla tft en tiempo real  */
  while(true){

    tft.fillRect(10, 2*BOXSIZE+70+25, 170, 80, ILI9341_BLACK);

    //Se inicia el funcionamiento 
    myservo_3.write(ANG_C_FIN); 
    Serial.println("se activa servo 3 - sube");
    tft.fillRect(10, 2*BOXSIZE+70+35, 150, 10, ILI9341_BLACK);
    tft.setCursor(10,2*BOXSIZE+70+55);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.println("se activa servo 3 - sube");    
    delay(1000);
    
    //GirarMotores(HORARIO, DIR_1, STEP_1, ENABLE_1, DIR_2, STEP_2, ENABLE_2, pulsador1_1, pulsador2_1);
    GirarMotores(ANTIHORARIO, DIR_1, STEP_1, ENABLE_1, DIR_2, STEP_2, ENABLE_2, pulsador1_2, pulsador2_2);
    Serial.println("Clamshell sujetado");
    tft.fillRect(10, 2*BOXSIZE+70+45, 150, 10, ILI9341_BLACK);
    tft.setCursor(10,2*BOXSIZE+70+35);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.println("Clamshell sujetado");
    delay(1000);

    
    GirarSoporte(pulsador2_3, ANTIHORARIO, DIR_3, STEP_3, ENABLE_3);
    Serial.println("Soporte regresa");
    tft.fillRect(10, 2*BOXSIZE+70+55, 150, 10, ILI9341_BLACK);
    tft.setCursor(10,2*BOXSIZE+70+45);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.println("Soporte regresa");
    delay(1000);
    
    myservo_1.write(ANG_A_FIN);  
    Serial.println("se activa servo 1 - baja");
    tft.fillRect(10, 2*BOXSIZE+70+55, 150, 10, ILI9341_BLACK);
    tft.setCursor(10,2*BOXSIZE+70+45);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.println("se activa servo 1 - baja");

    myservo_2.write(ANG_B_FIN); 
    Serial.println("se activa servo 2 - baja");
    tft.fillRect(10, 2*BOXSIZE+70+35, 150, 10, ILI9341_BLACK);
    tft.setCursor(10,2*BOXSIZE+70+55);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.println("se activa servo 2 - baja");

    myservo_3.write(ANG_C_INI);
    Serial.println("se activa servo 3 - baja");
    tft.fillRect(10, 2*BOXSIZE+70+45, 150, 10, ILI9341_BLACK);
    tft.setCursor(10,2*BOXSIZE+70+35);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.println("se activa servo 3 - baja");

    //función para detectar si hay un clamshell y almacenarlo en un contador - sensor inflarrojo
    contarClamshell();

    delay(1000);

    //GirarMotores(ANTIHORARIO, DIR_1, STEP_1, ENABLE_1, DIR_2, STEP_2, ENABLE_2, pulsador1_2, pulsador2_2);
    GirarMotores(HORARIO, DIR_1, STEP_1, ENABLE_1, DIR_2, STEP_2, ENABLE_2, pulsador1_1, pulsador2_1);
    myservo_1.write(ANG_A_INI); 
    Serial.println("se activa servo 1 - sube");
    tft.fillRect(10, 2*BOXSIZE+70+35, 150, 10, ILI9341_BLACK);
    tft.setCursor(10,2*BOXSIZE+70+55);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.println("se activa servo 1 - sube");

    myservo_2.write(ANG_B_INI); 
    Serial.println("se activa servo 2 - sube");
    tft.fillRect(10, 2*BOXSIZE+70+45, 150, 10, ILI9341_BLACK);
    tft.setCursor(10,2*BOXSIZE+70+35);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.println("se activa servo 2 - sube");
    delay(1000);
  }
}

void setup() {
  Serial.begin(115200);
  //***************** PAQUETE A *****************
  pinMode(DIR_1, OUTPUT);  
  pinMode(STEP_1, OUTPUT); 
  pinMode(ENABLE_1, OUTPUT);
  pinMode(pulsador1_1, INPUT_PULLUP); 
  pinMode(pulsador2_1, INPUT_PULLUP); 
  digitalWrite(ENABLE_1, LOW);
  myservo_1.attach(SERVO_1, 600, 2400);

  //***************** PAQUETE B *****************
  pinMode(DIR_2, OUTPUT);  
  pinMode(STEP_2, OUTPUT); 
  pinMode(ENABLE_2, OUTPUT);
  pinMode(pulsador1_2, INPUT_PULLUP); 
  pinMode(pulsador2_2, INPUT_PULLUP);
  digitalWrite(ENABLE_2, LOW);
  myservo_2.attach(SERVO_2, 600, 2400);

  //***************** PAQUETE C *****************
  pinMode(DIR_3, OUTPUT);  
  pinMode(STEP_3, OUTPUT); 
  pinMode(ENABLE_3, OUTPUT); 
  pinMode(pulsador1_3, INPUT_PULLUP); 
  pinMode(pulsador2_3, INPUT_PULLUP);
  digitalWrite(ENABLE_3, LOW); 
  
  myservo_3.attach(SERVO_3, 600, 2400);

  //***************** PAQUETE D *****************
  pinMode(SENSOR, INPUT); 

  SPI.begin(15,5,6);
  tft.begin();
  ts.begin();

  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();
  Serial.println(x_Pantalla);
  Serial.println(y_Pantalla);
  
  ts.setRotation(1);
  tft.setRotation(1);
  Serial.println(x_Pantalla);
  Serial.println(y_Pantalla);
  //digitalWrite(TFT_RESET, LOW);
  tft.fillScreen(ILI9341_BLACK);
  ts.setRotation(1);
  tft.setRotation(1);

  tft.setCursor(25, 80);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(3);
  tft.println("BIENVENIDOS");

  tft.setCursor(65, 125);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.println("SEPARADOR");
  tft.setCursor(105, 145);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.println("DE");
  tft.setCursor(63, 165);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.println("CLAMSHELLS");

  tft.setCursor(45, 220);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("1IEE28 SISTEMAS DIGITALES");

  tft.setCursor(30, 240);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("HORARIO: 05M1");
  tft.setCursor(30, 250);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("PROFESOR: NEILS VILCHEZ");
  tft.setCursor(30, 260);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("EQUIPO: TEAM ROCKET");

  tft.fillScreen(ILI9341_BLACK);
  Serial.println("Termino de pintar");
  delay(1000);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+20, 10);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  tft.println("MENU");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 30+10, BOXSIZE+50, BOXSIZE-10, ILI9341_GREEN);
  tft.setCursor(95, 45+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("INICIAR");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 10+30+BOXSIZE, BOXSIZE+50, BOXSIZE-10, ILI9341_WHITE);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+28, 50+BOXSIZE+5);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("DETENER");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 2*BOXSIZE+30+10, BOXSIZE+50, BOXSIZE-10, ILI9341_BLUE);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+15, 2*BOXSIZE+55);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("VER RESUMEN");

  tft.setCursor(65,2*BOXSIZE+70+25);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("Seleccione el modo");
  tft.setCursor(85,2*BOXSIZE+70+10+55);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("de operacion");

  tft.setCursor(85,2*BOXSIZE+35+10+85);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("!SUERTE :) !");
  

  //attachInterrupt(digitalPinToInterrupt(TIRQ_PIN), touchInterrupt_1, ONHIGH);
  //attachInterrupt(digitalPinToInterrupt(TIRQ_PIN), touchInterrupt_2,ONHIGH );


  //attachInterrupt(digitalPinToInterrupt1(TIRQ_PIN), touchInterrupt_1, ONHIGH);
  //attachInterrupt(digitalPinToInterrupt(TIRQ_PIN), touchInterrupt_2,ONHIGH );
  
  //Inicializar los actuadores de cada paquete: servos y motores
  //FALTO PROBAR CON EL KIT ESTE FUNCIÓN
  //CalibrarActuadores();

}

void loop() {


  //*********************PAQUETE D************
  menu_opcion=0;
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();
  delay(10);
  if (! ts.touched()) {
    return;
  }

  TS_Point p = ts.getPoint();


  delay(40);

  Serial.println(p.x);
  Serial.println(p.y);

  if(p.x>=1600 && p.x<=3000){
    if(p.y>=3200 && p.y<=3500){
      Serial.println("Ingreso en la opcion INICIAR");
      funcion_motores();
    }else if(p.y>=2600 && p.y<=2900){
      Serial.println("Ingreso en la opcion DETENER");
     
    }else if(p.y>=1800 && p.y<2200){
      Serial.println("Ingreso en la opcion VER RESUMEN");
      tft.fillScreen(ILI9341_BLACK);
      Serial.println("Se termino de pintar el fondo de negro");
      centinela=1;
      delay(10);
      menu_RESUMEN();
      while(centinela){
        if(RESUMEN(centinela)==0){
            delay(10);
            menu_opcion=1;          
          break; 
        }
      }
    }
  }
  if(menu_opcion==1){

    /* Se utiliza esta opcion para que la pantalla vuelva al 
      menu principal sin problemas, ni alteraciones en el 
      grafico */ 
    Serial.println("Se resetea la pantalla");
    digitalWrite(TFT_RESET, HIGH);
    tft.fillScreen(ILI9341_BLACK);
    menu();
  }
}