#include <LiquidCrystal_I2C.h>
#include <Wire.h>
#include <ESP32Servo.h>

LiquidCrystal_I2C lcd(0x27,20,4);/*instância do objeto lcd_i2c*/
const int LED_1 = 4;
const int LED_2 = 5;
const int LED_3 = 12;
const int LED_4 = 13;

const int LED_VERD = 17;
const int LED_VERM = 18;

const int LDR_1 = 32;
const int LDR_2 = 33;
const int LDR_3 = 34;
const int LDR_4 = 35;
int max_ldr_1 = 0;
int min_ldr_2 = 4096;
int min_ldr_3 = 4096;
int min_ldr_4 = 4096;
int min_ax = 0;

const int btn_direito = 14;
const int btn_centro = 15;
const int btn_esquerdo = 16;


int estado_btn_direito = HIGH;
int estado_btn_centro = HIGH;
int estado_btn_esquerdo = HIGH;

int ultimoEstadoEsquerdo = HIGH;
int ultimoEstadoCentro = HIGH;
int ultimoEstadoDireito = HIGH;


unsigned long debounceDelay = 10;

unsigned long ultimoDebounceTime_esquerdo = 0;
unsigned long ultimoDebounceTime_centro = 0;
unsigned long ultimoDebounceTime_direito = 0;


//variáveis para menus
char *menu_principal [] = {"INICIAR","DADOS","ALTURA"};
char *menu_dados [] = {"GRAVIDADE","VELOCIDADE","ERRO","VOLTAR"};
char *menu_alturas [] = {"30cm","60cm","90cm","VOLTAR"};


bool dentro_m_teste = false;

//fim adição
bool dentro_m_principal = true; // verdade inicialmente, para mostra ele primeiro
bool dentro_m_dados = false;
bool dentro_m_alturas = false;

int contador_m_principal=1, contador_m_dados, contador_m_alturas = 0;

const float alturas [] = {0.30, 0.60, 0.90};
int index_altura = -1;
bool dentro_start = false; // para controle do reset
bool check = false;

unsigned long tempo1, tempo2;
float deltaTempo, gravidade, velocidade, erro = 0;

const float g_referencial = 9.79; // para cálculo do erro percentual
// para colocar os valores convertidos para string
char str_gravidade[15]; 
char str_velocidade[15];
char str_erro[15];
//para controle de exibição dos menus
int anterior_altura_in_contador = 0;
int anterior_principal_in_contador = 0;
int anterior_dados_in_contador = 0;
int aux_dados_in = 0;
//adição
int anterior_teste_in_contador = 0;
//fim adição

volatile bool cancelar_captura = false;//para controle do reset

int aux_altura_in = 0;

const int pinServo = 2;
Servo garra;
bool garra_status = false;


void setup() {
  // put your setup code here, to run once:

  pinMode(LED_1, OUTPUT);
  pinMode(LED_2, OUTPUT);
  pinMode(LED_3, OUTPUT);
  pinMode(LED_4, OUTPUT);
  pinMode(LED_VERD, OUTPUT);
  pinMode(LED_VERM, OUTPUT);

  pinMode(LDR_1, INPUT);
  pinMode(LDR_2, INPUT);
  pinMode(LDR_3, INPUT);
  pinMode(LDR_4, INPUT);

  pinMode(btn_esquerdo, INPUT_PULLUP);
  pinMode(btn_centro, INPUT_PULLUP);
  pinMode(btn_direito, INPUT_PULLUP);

  //ajuste de resolução ADC
  analogReadResolution(12);//12 bits

  //definindo limites do servo
  garra.attach(pinServo,500,2500);
  //movendo servo para 5 graus..
  garra.write(5);

  
  //definindo attachInterrupt para um botão em específico
  attachInterrupt(digitalPinToInterrupt(15),reset,FALLING);


  lcd.init();
  lcd.backlight();
  lcd.setCursor(0,2);
  lcd.print("INICIANDO");
  delay(1000);
  
  print_menu_principal();
  
}

void loop()
{
  int leitura_esquerdo = digitalRead(btn_esquerdo);
  int leitura_centro = digitalRead(btn_centro);
  int leitura_direito = digitalRead(btn_direito);
 //*==================*#*==================*
 //ultimoEstadoEsquerdo
  if(leitura_esquerdo != ultimoEstadoEsquerdo)//navegação decrescente
  {
    ultimoDebounceTime_esquerdo = millis();
    
  }
  if((millis() - ultimoDebounceTime_esquerdo) > debounceDelay)
  {
    if(leitura_esquerdo != estado_btn_esquerdo)
    {
      estado_btn_esquerdo = leitura_esquerdo;
      if(estado_btn_esquerdo == LOW)
      {
        if(dentro_m_principal)//navega dentro do menu principal, decrescente
        {
          if(contador_m_principal > 0)
          {
            contador_m_principal -=1;

          }else{

            contador_m_principal = 2;
          }
          if(anterior_principal_in_contador != contador_m_principal)
          {
            contador_m_dados = 0;
            contador_m_alturas = 0;
           // contador_m_teste = 0;
            anterior_principal_in_contador = contador_m_principal;
            print_menu_principal();
          }
        }
        if(dentro_m_dados)//navega dentro do menu dados, decrescente
        {
          if(contador_m_dados > 0)
          {
            contador_m_dados -= 1;

          }else{

            contador_m_dados = 3;
          }
          if(anterior_dados_in_contador != contador_m_dados)
          {
            contador_m_alturas = 0;
            //contador_m_teste = 0;
            contador_m_principal = 0;
            anterior_dados_in_contador = contador_m_dados;
            print_menu_dados();
          }
        }
        if(dentro_m_alturas)//navega dentro do menu alturas, decrescente
        {
          if(contador_m_alturas > 0)
          {
            contador_m_alturas -= 1;
          }else{
            contador_m_alturas = 3;
          }
          if(anterior_altura_in_contador != contador_m_alturas)
          {
            contador_m_dados = 0;
            //contador_m_teste = 0;
            contador_m_principal = 0;
            anterior_altura_in_contador = contador_m_alturas;
            print_menu_alturas();
          }
        }
      
      }//fim terceiro
     
    }//fim segundo
  }//primeiro if
  ultimoEstadoEsquerdo = leitura_esquerdo;
 
  //*==================*#*==================*
  if(leitura_direito != ultimoEstadoDireito)//navegação crescente
  {
    ultimoDebounceTime_direito = millis();
  }
  if((millis() - ultimoDebounceTime_direito) > debounceDelay)
  {
    if(leitura_direito != estado_btn_direito)
    {
      estado_btn_direito = leitura_direito;
      if(estado_btn_direito == LOW)
      {
        if(dentro_m_principal)//navega dentro do menu principal, crescente
        {
          if(contador_m_principal < 2)
          {
            contador_m_principal += 1;
          }else{
            contador_m_principal = 0;
          }
          if(anterior_principal_in_contador != contador_m_principal)
          {
            contador_m_dados = 0;
            contador_m_alturas = 0;
           // contador_m_teste = 0;
            anterior_principal_in_contador  = contador_m_principal;
            print_menu_principal();
          }

        }
        if(dentro_m_dados)// navega dentro do menu dados, crescente
        {
          if(contador_m_dados < 3)
          {
            contador_m_dados += 1;
          }else{
            contador_m_dados = 0;
          }
          if(anterior_dados_in_contador != contador_m_dados)
          {
            contador_m_alturas = 0;
            contador_m_principal = 0;
           // contador_m_teste = 0;
            anterior_dados_in_contador = contador_m_dados;
            print_menu_dados();
          }
        }
        if(dentro_m_alturas)//navega dentro do menu alturas, crescente
        {
          if(contador_m_alturas < 3)
          {
            contador_m_alturas += 1;
          }else{
            contador_m_alturas = 0;
          }
          if(anterior_altura_in_contador != contador_m_alturas)
          {
            contador_m_dados = 0;
            contador_m_principal = 0;
            //contador_m_teste = 0;
            anterior_altura_in_contador = contador_m_alturas;
            print_menu_alturas();
          }
        }
       
      }
    }
  }
   
  ultimoEstadoDireito = leitura_direito;
  //*==================*#*==================*
  if(leitura_centro != ultimoEstadoCentro)
  {
    ultimoDebounceTime_centro = millis();
  }

  if((millis()) - ultimoDebounceTime_centro > debounceDelay)
  {
    if(leitura_centro != estado_btn_centro)
    {
      estado_btn_centro = leitura_centro;
      if(estado_btn_centro == LOW)//enter foi pressionado, mas onde? Segue..
      {
        if(dentro_m_principal)//se o enter foi pressionado dentro do menu principal, faz
        {
          switch (contador_m_principal)//verifica onde o contador do menu principal está
          {
            case 0: //caso esteja em 0, então o enter foi pressionado para iniciar a captura dos dados
              dentro_m_principal = false;
              dentro_m_alturas = false;
              dentro_m_dados = false;
             dentro_m_teste = false;
              iniciar();
            break;
            case  1:
              dentro_m_principal = false;
              dentro_m_alturas = false;
              dentro_m_teste = false;
              dentro_m_dados = true;
              print_menu_dados(); // caso seja 1, então significa que o usuário quer ver os dados
            break;
            case 2: // caso seja 2, então significa que o usuário quer selecionar a altura
              dentro_m_principal = false;
              dentro_m_alturas = true;
              dentro_m_teste = false;
              dentro_m_dados = false;
              print_menu_alturas();
            break;
            default:
            break;
          }
        }
        if(dentro_m_dados)//enter pressionado dentro do menu dados
        {
          switch (contador_m_dados)
          {
            case 0: //o usuário quer ver a gravidade
              if(aux_dados_in > 0)
              {
                print_gravidade();
                aux_dados_in = 0;
              }else{
                aux_dados_in += 1;
              }
            break;
            case 1:
              print_velocidade(); //o usuário quer ver a velocidade
            break;
            case 2:
              print_erro(); // ... quer ver o erro percentual
            break;
            case 3:
              dentro_m_principal = true;
              dentro_m_alturas = false;
              dentro_m_dados = false;
              dentro_m_teste = false;
              voltar(); //... quer voltar
            break;
            default:
            break;
          }
        }
        if(dentro_m_alturas) // enter pressionado dentro do menu alturas
        {
          switch(contador_m_alturas)
          {
            case 0:/* se em qualquer caso terei de pegar a altura selcionada, então por que não pular as etapas.*/
            case 1:
            case 2:
              if(aux_altura_in > 0)
              {
                index_altura = contador_m_alturas;
                // print_altura_selecionada();
                dentro_m_alturas = false;
                dentro_m_dados = false;
                dentro_m_principal = true;
                aux_altura_in  = 0;
                voltar();
              }
              else{
                aux_altura_in ++;
              }
            break;
            case 3:
              dentro_m_principal =true;
              dentro_m_alturas = false;
              dentro_m_dados = false;
              dentro_m_teste = false;
              voltar();
            break;
            default:
            break;
          }
        }
        
      }// fim 2 if
    }
  }
  ultimoEstadoCentro = leitura_centro;
}


void print_menu_principal() {
  lcd.clear();
  for(int i = 0; i < 3;i++)
  {
    lcd.setCursor(0,i);
    if(contador_m_principal == i)
    {
      lcd.print(" -> ");
      lcd.print(menu_principal[i]);
    }else{
        lcd.print(menu_principal[i]);
    }
    
  }
  if(index_altura != -1)
  {
    lcd.setCursor(0,3);
    lcd.print("ALTURA: ");
    lcd.print(alturas[index_altura]);
  }
 
}


void print_menu_dados()
{ lcd.clear();
  for(int i = 0; i < 4;i++)
  {
    lcd.setCursor(0,i);
    if(contador_m_dados == i)
    {
      lcd.print(" -> ");
      lcd.print(menu_dados[i]);
    }else{
        lcd.print(menu_dados[i]);
    }
    
  }
 
}


void print_menu_alturas()
{ lcd.clear();
  for(int i = 0; i < 4;i++)
  {
    lcd.setCursor(0,i);
    if(contador_m_alturas == i)
    {
      lcd.print(" -> ");
      lcd.print(menu_alturas[i]);
    }else{
        lcd.print(menu_alturas[i]);
    }
    
  }
 
}
void print_msg_iniciando()
{

  lcd.setCursor(0,0);
  lcd.print("Aguarde...");
  lcd.setCursor(0,1);
  lcd.print("Verificando limiares");
  Verificar_limiares();
  unsigned long startMillis = millis();
  unsigned long currentMillis;
  int remainingTime = 10; // Contagem regressiva de 10 segundos

  while (remainingTime > 0)
  {
    currentMillis = millis();
    if (currentMillis - startMillis >= 1000)
    { // A cada 1 segundo
      startMillis = currentMillis;
      remainingTime--;

      lcd.clear();

      // Configura a fonte e exibe a mensagem
      lcd.setCursor(0,0);
      lcd.print("INICIANDO EM...");

      // Exibe os dígitos da contagem regressiva
      char remainingTimeString[4];
      sprintf(remainingTimeString, "%ds", remainingTime);
      lcd.setCursor(0,1); // Fonte grande para os dígitos
      lcd.print(remainingTimeString); 
    }
  }
   captura_tempos(led,ldr);
}

void print_concluido()
{

  lcd.clear();

  dentro_m_alturas = false;
  dentro_start = false;//saiu do iniciar
  dentro_m_dados = false;
  dentro_m_teste = false;
  dentro_m_principal = false;
 
  lcd.setCursor(0,0);
  lcd.print("Concluido!");
  delay(2000);
  contador_m_dados = 0;//isso 
  anterior_dados_in_contador = 0; // e isso, garatem que sempre que for concluido com sucesso vai para o menu dados
  dentro_m_dados = true;
  cancelar_captura = false;
  print_menu_dados();
  
}
void print_cancelando()
{
  lcd.setCursor(0,0);
  lcd.print("CANCELANDO");
  for(int i = 0; i<3;i++)
  {
    lcd.print(".");
    delay(1000);
  }
  contador_m_principal = 0;
  anterior_principal_in_contador = 0;
  dentro_m_principal = true;
  print_menu_principal();
}
void print_gravidade()
{
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("GRAVIDADE:");
  lcd.setCursor(0,1);
  lcd.print(str_gravidade);
  lcd.print(" m/s^2");
}

void print_velocidade()
{
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("VELOCIDADE:");
  lcd.setCursor(0,1);
  lcd.print(str_velocidade);
  lcd.print(" m/s");
}
void print_erro()
{
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("ERRO PERCENT");
  lcd.setCursor(0,1);
  lcd.print(str_erro);
  lcd.print(" %");
}


void iniciar()//inicia o processo de captura
{
  if(index_altura == -1)//caso a altura não esteja selecionada
  {
    dentro_m_alturas = true;
    contador_m_alturas = 0;
    print_menu_alturas();

  }else{

    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("INICIANDO....");
  
    dentro_start = true; 
   // check = false;
    deltaTempo = 0.0;
    gravidade = 0.0;
    velocidade = gravidade;
    erro = velocidade;
    int led, ldr;
    //Apagando todos os leds

    digitalWrite(LED_1, LOW);
    digitalWrite(LED_2, LOW);
    digitalWrite(LED_3, LOW);
    digitalWrite(LED_4, LOW);

    if (index_altura == 0)
    {
      digitalWrite(LED_2, HIGH);
      led = LED_2;
      ldr = LDR_2;
    }
    if (index_altura == 1)
    {
      digitalWrite(LED_3, HIGH);
      led = LED_3;
      ldr = LDR_3;
    }
    if(index_altura == 2)
    {
      digitalWrite(LED_4,HIGH);
      led = LED_4;
      ldr = LDR_4;
    }
    digitalWrite(LED_1, HIGH);
    if(verificarPosicaoInicial())
    {
      if(!garra_status)
      {
        fechar_garra();
      }
      
    }else{
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("CORPO DE PROVA");
      lcd.setCursor(0,1);
      lcd.print("Não está na posicao");
      lcd.setCursor(0,2);
      lcd.print("correta! Tente iniciar");
      lcd.setCursor(0,3);
      lcd.print("novamente....");

      dentro_m_teste = false;
      dentro_m_principal = true;
      contador_m_principal = 0;
      voltar();

    }
    
  }
  
}


bool verificarPosicaoInicial()
{
  
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("COLOQUE O CORPO DE ");
  lcd.setCursor(0,1);
  lcd.print("PROVA NA FRENTE DO ");
  lcd.setCursor(0,2);
  lcd.print("SENSOR 1 ");
  lcd.setCursor(0,3);
  lcd.print("ESPERE O LED VERDE.. ");

  delay(5000);

  if(analogRead(LDR_1) > 3000)
  {
    digitalWrite(LED_VERD,LOW);
    digitalWrite(LED_VERM,HIGH);
  }
  digitalWrite(LED_VERD,HIGH);
  digitalWrite(LED_VERM,LOW);

  unsigned long start =  millis();

  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Ajustando limiar 1...");
  lcd.setCursor(0,1);
  lcd.print("5 segundos");
  while((millis()-start) < 5000)
  {
    int v = 0;
    if(v > max_ldr_1)
    {
      max_ldr_1 = v;
    }

  }
  if(max_ldr_1 < 3000)
  {
    return true;
  }else{
    return false;
  }
  
}
void fechar_garra()//fecha a garra devagar
{
  for(int i = 0; i <= 45; i+=1)
  {
    delay(50);
    garra.write(i);
  }
  garra_status = true;
}

void Verificar_limiares() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("TESTANDO SENSORES");
  lcd.setCursor(0, 1);
  lcd.print("AGUARDE 30s");

  digitalWrite(LED_1, HIGH);
  digitalWrite(LED_2, HIGH);
  digitalWrite(LED_3, HIGH);
  digitalWrite(LED_4, HIGH);
  

  for (int j = 1; j < 3; j++) {
    switch (j) {
      case 1:
        min_ldr_2 = testeIndividual(LED_2, LDR_2, 10000);
        break;
      case 2:
        min_ldr_3 = testeIndividual(LED_3, LDR_3, 10000);
        break;
      case 3:
        min_ldr_4 = testeIndividual(LED_4, LDR_4, 10000);
       break;
    }
  }

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("TESTE CONCLUIDO");
  lcd.setCursor(0, 1);
  lcd.print("AJUSTANDO...");
  delay(100);

  if (min_ldr_2 < 1000 && min_ldr_3 < 1000 && min_ldr_4 < 1000)
  {
    lcd.clear();
    lcd.setCursor(6, 0); // Centraliza o título "LIMIARES" na linha 1
    lcd.print("LIMIARES");

    lcd.setCursor(0, 1); lcd.print("LDR 2: "); lcd.print(min_ldr_2); 
    lcd.setCursor(0, 2); lcd.print("LDR 3: "); lcd.print(min_ldr_3); 
    lcd.setCursor(0, 3); lcd.print("LDR 4: "); lcd.print(min_ldr_4);

    delay(5000);
    dentro_m_teste = false;
    dentro_m_principal = true;
    contador_m_principal = 0;
    voltar();
  }
  else{
    lcd.clear();
    lcd.setCursor(0, 0); lcd.print("PRSS [ENTER] P/ INICIAR");
    lcd.setCursor(0, 1); lcd.print("LDR 2: "); lcd.print(min_ldr_2); 
    lcd.setCursor(0, 2); lcd.print("LDR 3: ");lcd.print(min_ldr_3); 
    lcd.setCursor(0, 3); lcd.print("LDR 4: ");  lcd.print(min_ldr_4);
  }
  
}
int testeIndividual(int led, int ldr,int tempo)
{
  unsigned long tempoInicial = millis();
  int valorMinimo = 1023;

  while(millis() - tempoInicial < tempo)
  {
    int leitura = analogRead(ldr);
    if(leitura < valorMinimo)
    {
      valorMinimo = leitura;
    }

  }
  digitalWrite(led, LOW);
  return valorMinimo;
  
}

void captura_tempos(int led, int ldr) {
    tempo1 = 0;
    tempo2 = 0;
    check = false; // Esta variável garante que a segunda leitura só ocorra após a primeira.
    cancelar_captura = false; // Permite que o usuário cancele a captura.
    
    // Define o limiar mínimo baseado no LED específico
    switch (led) {
        case LED_2:
            min_ax = min_ldr_2;
            break;
        case LED_3:
            min_ax = min_ldr_3;
            break;
        case LED_4:
            min_ax = min_ldr_4;
            break;
    }

    garra.write(20); // Abre a garra para iniciar a queda
    garra_status = false;
    // Enquanto não tiver capturado ambos os tempos e o usuário não tiver cancelado
    while ((tempo1 == 0 || tempo2 == 0) && !cancelar_captura) {
        // Se o primeiro LDR (A2) detectar que o objeto passou
        if (tempo1 == 0 && analogRead(LDR_1) > max_ldr_1) {
            tempo1 = micros(); // Marca o tempo de início
            digitalWrite(LED_1, LOW); // Desliga o primeiro LED após a detecção
            check = true;
        }

        // Se o LDR específico para esse LED detectar que o objeto passou
        if (check && analogRead(ldr) < min_ax) { // Aguarda a detecção no segundo sensor
            tempo2 = micros(); // Marca o tempo de término
            digitalWrite(led, LOW); // Desliga o LED específico
            break; // Sai do loop após capturar ambos os tempos
        }
    }

    if (tempo1 != 0 && tempo2 != 0) {
        calcular(); // Chama a função de cálculo se ambos os tempos forem capturados
    }
}

void calcular()/* está função calcula o tempo de queda, gravidade, velocidade de queda e o erro percentual*/
{
  deltaTempo = (tempo2 - tempo1)/1000000.0; //calcula a variação do tempo

  float d = alturas[index_altura];  // pega a altura selecionada

  gravidade = (2*d)/(deltaTempo*deltaTempo); //calcula a gravidade com base na altura e no tempo
  velocidade = gravidade * deltaTempo; // velocidade calculada com base na gravidade e tempo, mas pode ser utilizado a distância e o tempo
  erro = ((abs(gravidade - g_referencial)) / g_referencial) * 100.0; //erro percentual.

  converter_para_string(); //converter para string
  print_concluido();
}

void converter_para_string()
{
  dtostrf(gravidade, 6, 2, str_gravidade);//converte em 4 casas decimais
  dtostrf(velocidade, 6, 2, str_velocidade);
  dtostrf(erro, 6, 2, str_erro);

  //adicionando as unidades
  snprintf(str_gravidade, sizeof(str_gravidade),"%sm/s^2",str_gravidade);
  snprintf(str_velocidade,sizeof(str_velocidade),"%sm/s",str_velocidade);
  snprintf(str_erro, sizeof(str_erro),"%s %%",str_erro);
}

void reset()/* cancela a captura  por ele na leitura do segundo sensor.*/
{
 if(dentro_start && !garra_status)
 {
  cancelar_captura = true;
  print_cancelando();
 }else if(dentro_start && garra_status){
  print_msg_iniciando();
 }
}
void voltar()
{
  print_menu_principal();
}