/*                              PROJETO CARRO wrangler DO OTTO
                               *********************************

***********************USANDO ESP32 E NRF24L01***********************

******************************* EXCLUSIVO PARA MEMBROS DO CANAL ARDUINO PARA MODELISMO**********************************

  Desenvolvido por Tázio Fernandes- [email protected]
  C 2023 - Todos os direitos reservados

*/



//#include <RF24.h>   // Biblioteca para controle do NRF24L01
#include <nRF24-SOLDERED.h>   // Biblioteca para controle do NRF24L01
#include <ESP32Servo.h>       // Biblioteca para controle do Servo Motor
//#include <Servo.h>  // Biblioteca para controle do Servo Motor
#include <SPI.h>
//#include <nRF24L01.h>




// Define os pinos do NRF24 no ESP32
RF24 radio(18, 19);  // Pinos CE e CSN do NRF24L01 conectados aos GPIO18 e GPIO19 do ESP32
const byte address[6] = "00001";
int potPos = 0;

struct Veiculo {
  // Faróis
  int farolBaixoPin;
  int farolAltoPin;
  int freioPin;
  int estadoFarol;  // 0: desligado, 1: baixa, 2: alto

  // Joystick e Direção
  int joystickDirecaoPin;  
  Servo servoDirecao;
  int anguloAtual;

  // Joystick e Aceleração
  //int joystickAceleracaoPin; Há um struct para o controle   
  
  
  int velocidadeAtual;

  // Controle dos motores
  int motorEsquerdoDCInFrentePin;
  int motorEsquerdoDCInRePin;
  int motorEsquerdoDCPwmPin;
  
  int motorDireitoDCInFrentePin;
  int motorDireitoDCInRePin;
  int motorDireitoDCPwmPin; // Controlar a velocidade
};

struct Joystick {
  boolean Start;
  boolean Select;
  boolean SetaCima;
  boolean SetaDireita;
  boolean SetaEsquerda;
  boolean SetaBaixo;  
  boolean BotaoTriangulo;
  boolean BotaoCirculo;
  boolean BotaoQuadrado;
  boolean BotaoX;
  byte BotoesRs;
  byte JoyDireitaX;
  byte JoyDireitaY;
  byte JoyEsquerdaX;
  byte JoyEsquerdaY;
};

Veiculo carro;
Joystick myJoystick;

// Pinos dos LEDs, botões e auxiliares
//Freio 16, Farol baixo 17 e Alto 5
const int ledPins[] = {2, 4, 16, 17, 5};
const int motorPins[] = {25, 26};
const int buttonPins[] = {32, 33, 27};
int botaoR1pin = 0;
boolean VBotaoR1 = 0;
int botaoR2pin = 0;
boolean VBotaoR2 = 0;
int botaoR3pin = 0;
boolean VBotaoR3 = 0;
int botaoL1pin = 0;
boolean VBotaoL1 = 0;
int botaoL2pin = 0;
boolean VBotaoL2 = 0;
int botaoL3pin = 0;
boolean VBotaoL3 = 0;
boolean AuxSetaBaixo = 0;
boolean VSetaBaixo = 0;
boolean VSetaAutomatica = 0;
boolean VSetaEsquerda = 0;
boolean VSetaDireita = 0;
int sinal = 1;
int sinal2 = 0;
int valorJoystickDirecao = 90;


// Variáveis para leitura e controle de suavização
int ultimaVelocidade = 0; // Armazena a última velocidade para suavização

// Variável necessária para contagem de tempo para o alerta de bateria baixa
uint32_t Bip; 

void setup() {
  //Radio
  radio.begin();
  radio.openReadingPipe(0, address);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();
  zeraVariaveis();


  // Configuração dos faróis
  carro.farolBaixoPin = ledPins[3];
  carro.farolAltoPin = ledPins[4];
  carro.freioPin = ledPins[2];
  carro.estadoFarol = 0;

  pinMode(carro.farolBaixoPin, OUTPUT);
  pinMode(carro.farolAltoPin, OUTPUT);
  pinMode(carro.freioPin, OUTPUT);

  // Configuração do joystick e servo motor
  carro.joystickDirecaoPin = 34;  // Eixo X do joystick para controle da direção (pino GPIO34)
  carro.servoDirecao.attach(13);  // Pino do Servo motor agora no GPIO13
  carro.anguloAtual = 90;  // Centrando a direção

  // Configuração do joystick para controle da aceleração (entrada analógica no GPIO35)
  //carro.joystickAceleracaoPin = 35; O comando virá do controle

  // Configuração da Ponte H e motores DC (aqui são dois motores)
  carro.motorEsquerdoDCInFrentePin = 14; //Vamos usar somente 1 e será este aqui!
  carro.motorEsquerdoDCInRePin = 12;
  carro.motorEsquerdoDCPwmPin = motorPins[0];

  carro.motorDireitoDCInFrentePin = 27;
  carro.motorDireitoDCInRePin = 25;
  carro.motorDireitoDCPwmPin = motorPins[1];  
  carro.velocidadeAtual = 0; // Carro parado

  // Configuração dos pinos dos motores como saída
  pinMode(carro.motorEsquerdoDCInFrentePin, OUTPUT);
  pinMode(carro.motorEsquerdoDCInRePin, OUTPUT);
  pinMode(carro.motorEsquerdoDCPwmPin, OUTPUT);
  
  pinMode(carro.motorDireitoDCInFrentePin, OUTPUT);
  pinMode(carro.motorDireitoDCInRePin, OUTPUT);
  pinMode(carro.motorDireitoDCPwmPin, OUTPUT);
  
  //pinMode(carro.joystickAceleracaoPin, INPUT); o comando virá do controle

  // Inicializar estado do farol
  atualizarFarol();
  
  // Iniciar direção central
  carro.servoDirecao.write(carro.anguloAtual);

  // Inicializar monitor serial para depuração
  Serial.begin(115200);
}

void loop() {
  Bateria();
  if (radio.available()){

    radio.read(&myJoystick, sizeof(Joystick));
  }
  
  //botaoR1pin = !bitRead(myJoystick.BotoesRs, 0);  REMOVER
  //botaoR2pin = !bitRead(myJoystick.BotoesRs, 1);  REMOVER
  //botaoR3pin = !bitRead(myJoystick.BotoesRs, 2);  REMOVER
  //botaoL1pin = !bitRead(myJoystick.BotoesRs, 3);  REMOVER
  //botaoL2pin = !bitRead(myJoystick.BotoesRs, 4);  REMOVER
  //botaoL3pin = !bitRead(myJoystick.BotoesRs, 5);  REMOVER

  Serial.print("; Botão R1: ");
  Serial.print(botaoR1pin);
  Serial.print("; Botão R2: ");
  Serial.print(botaoR2pin);
  Serial.print("; Botão R3: ");
  Serial.println(botaoR3pin);

  Serial.print("; Botão L1: ");
  Serial.print(botaoL1pin);
  Serial.print("; Botão L2: ");
  Serial.print(botaoL2pin);
  Serial.print("; Botão L3: ");
  Serial.println(botaoL3pin);

  controlarFarol();
  controlarSetas(ledPins[0], ledPins[1]);
  controlarDirecao(ledPins[0], ledPins[1]);
  controlarAceleracao();
  handleBlinkButton(buttonPins[2], ledPins[0], ledPins[1]);
}

void atualizarFarol() {
  switch (carro.estadoFarol) {
    case 0: // Desligado
      digitalWrite(carro.farolBaixoPin, LOW);
      digitalWrite(carro.farolAltoPin, LOW);
      digitalWrite(carro.freioPin, LOW);
      break;
    case 1: // Farol baixo
      digitalWrite(carro.farolBaixoPin, HIGH);
      digitalWrite(carro.farolAltoPin, LOW);
      digitalWrite(carro.freioPin, HIGH);
      break;
    case 2: // Farol alto
      digitalWrite(carro.farolBaixoPin, HIGH);
      digitalWrite(carro.farolAltoPin, HIGH);
      digitalWrite(carro.freioPin, HIGH);
      break;
  }
}

void controlarSetas(int ledPin1, int ledPin2){

  // *BOTÃO SETA PARA BAIXO
    // ACIONA O FREIO DO VEÍCULO PELA PONTE H (Pouco útil em motor com alta redução)
    if (myJoystick.SetaBaixo == HIGH) {
      if (AuxSetaBaixo == LOW) {
        AuxSetaBaixo = HIGH;
        VSetaBaixo = !VSetaBaixo;
        Serial.print("       Botao SetaBaixo Pressionado: ");
        Serial.println(VSetaBaixo);
      }
    } else {
      AuxSetaBaixo = LOW;
    }

    // *BOTÃO R3
    // USADO PARA LIGAR E DESLIGAR A FUNÇÃO DE ACIONAR AS SETAS AUTOMATICAMENTE AO FAZER CURVAS
    if (!bitRead(myJoystick.BotoesRs, 2) == HIGH) {
      if (botaoR3pin == LOW) {
        botaoR3pin = HIGH;
        VBotaoR3 = !VBotaoR3;
        Serial.print("      Botao R3 Pressionado: ");
        Serial.println(VBotaoR3);

        VSetaEsquerda = LOW;
        VSetaDireita = LOW;
        digitalWrite(ledPin1, LOW);  // Apaga Seta Direita
        digitalWrite(ledPin2, LOW);  // Apaga Seta Esquerda
      }
    } else {
      botaoR3pin = LOW;
    }

    if (VBotaoR3 == HIGH) {
      VSetaAutomatica = LOW;
    } else {
      VSetaAutomatica = HIGH;
    }
}

// Controlar o farol
void controlarFarol() {
  static int lastButtonState = LOW;
  int buttonState = digitalRead(buttonPins[1]);
  
  if (buttonState == HIGH && lastButtonState == LOW) {
    carro.estadoFarol = (carro.estadoFarol + 1) % 3; // Alterna entre 0, 1, e 2
    atualizarFarol();
  }
  
  lastButtonState = buttonState;
}

// Controlar a direção com o joystick (Eixo X)
void controlarDirecao(int ledPin1, int ledPin2) {
  int valorJoystickDirecao = analogRead(carro.joystickDirecaoPin); // Ler posição do joystick no eixo X
  
  //int valorJoystickDirecao = myJoystick.JoyDireitaX;
  int angulo = map(valorJoystickDirecao, 0, 2048, 10, 170);   // Mapear para ângulo do servo motor (0 a 170 graus)
  
  if (abs(angulo - carro.anguloAtual) > 2) {  // Evitar pequenos ajustes
    carro.servoDirecao.write(angulo);
    carro.anguloAtual = angulo;
    Serial.print("Direção ajustada para: ");
    Serial.println(angulo);
  }

  // ACIONA A SETA AUTOMATICAMENTE AO FAZER CURVAS
  if (VSetaAutomatica == HIGH) {
      if (valorJoystickDirecao < 85) {  // ACENDE SETA ESQUERDA
        VSetaEsquerda = HIGH;
        VSetaDireita = LOW;
        digitalWrite(ledPin2, LOW);  // Apaga Seta Direita
        sinal = 1;
      } else if (valorJoystickDirecao > 95) {  // ACENDE SETA DIREITA
        VSetaEsquerda = LOW;
        VSetaDireita = HIGH;
        digitalWrite(ledPin1, LOW);  // Apaga Seta Esquerda
        sinal = 1;
      } else {
        if (sinal == 1) {
          VSetaEsquerda = LOW;
          VSetaDireita = LOW;
          digitalWrite(ledPin2, LOW);  // Apaga Seta Esquerda
          digitalWrite(ledPin1, LOW);  // Apaga Seta Direita
          sinal = 0;
        }
      }
    }
}

// Controlar a aceleração do motor DC com o joystick (Eixo Y)
void controlarAceleracao() {
  // Ler o valor do joystick no eixo Y para determinar a velocidade
  //int valorJoystickAceleracao = analogRead(carro.joystickAceleracaoPin);
  int valorJoystickAceleracao = myJoystick.JoyEsquerdaY; //pegando valor vindo pelo radio.

  Serial.print("Valor do Joystick (Eixo Y): ");
  Serial.println(valorJoystickAceleracao);

  // Verificar a posição do joystick e definir a velocidade
  if (valorJoystickAceleracao >= 130) { // Ré

    carro.velocidadeAtual = map(valorJoystickAceleracao, 130, 255, 20, 255);

    digitalWrite(carro.motorEsquerdoDCInFrentePin, LOW);
    digitalWrite(carro.motorEsquerdoDCInRePin, HIGH);
    analogWrite(carro.motorEsquerdoDCPwmPin, carro.velocidadeAtual);

    digitalWrite(carro.motorDireitoDCInFrentePin, LOW);
    digitalWrite(carro.motorDireitoDCInRePin, HIGH);
    analogWrite(carro.motorDireitoDCPwmPin, carro.velocidadeAtual);
    
    //Colocar aqui para acender o led de ré.
    //Colocar aqui para apagar a luz de freio se o farol estiver ligado.
    Serial.print("RÉ: ");
    Serial.print("Valor Velocidade Atual: ");
    Serial.print(carro.velocidadeAtual);
    Serial.print("Valor direto do controle: ");
    Serial.print(map(myJoystick.JoyEsquerdaY, 130, 255, 20, 255));
   
  
  } else if (valorJoystickAceleracao <= 125) { // Frente
    carro.velocidadeAtual = map(valorJoystickAceleracao, 125, 0, 20, 255);

    digitalWrite(carro.motorEsquerdoDCInFrentePin, HIGH);
    digitalWrite(carro.motorEsquerdoDCInRePin, LOW);
    analogWrite(carro.motorEsquerdoDCPwmPin, carro.velocidadeAtual);

    digitalWrite(carro.motorDireitoDCInFrentePin, HIGH);
    digitalWrite(carro.motorDireitoDCInRePin, LOW);
    analogWrite(carro.motorDireitoDCPwmPin, carro.velocidadeAtual);
    //Colocar aqui para apaga a luz de freio. Se o farol estiver ligado 

    Serial.print("FRENTE: ");
    Serial.print("Valor Velocidade Atual: ");
    Serial.print(carro.velocidadeAtual);

    Serial.print("Valor direto do controle: ");
    Serial.print(map(myJoystick.JoyEsquerdaY, 125, 0, 20, 255));

  } else { // parado entre 126 e 129
    carro.velocidadeAtual = 0;
    digitalWrite(carro.motorEsquerdoDCInFrentePin, LOW);
    digitalWrite(carro.motorEsquerdoDCInRePin, LOW);
    analogWrite(carro.motorEsquerdoDCPwmPin, 0);

    digitalWrite(carro.motorDireitoDCInFrentePin, LOW);
    digitalWrite(carro.motorDireitoDCInRePin, LOW);
    analogWrite(carro.motorDireitoDCPwmPin, 0);

    Serial.print("PARADO: ");
    Serial.print("Valor Velocidade Atual: ");
    Serial.print(carro.velocidadeAtual);

    Serial.print("Valor direto do controle: ");
    Serial.print(map(myJoystick.JoyEsquerdaY, 125, 0, 20, 255));
  }
}

// Função para piscar os LEDs ao pressionar o botão de alerta
void handleBlinkButton(int buttonPin, int ledPin1, int ledPin2) {
  if (digitalRead(buttonPin) == HIGH) {
    for (int i = 0; i < 5; i++) {
      digitalWrite(ledPin1, HIGH);
      digitalWrite(ledPin2, HIGH);
      delay(500);
      digitalWrite(ledPin1, LOW);
      digitalWrite(ledPin2, LOW);
      delay(500);
    }
  }
}

// Função para ler a tensão da bateria (adaptada para o ESP32)
void Bateria() {
  int valorADC = analogRead(34); // Ler tensão da bateria no pino GPIO34
  float tensao = (valorADC / 4095.0) * 3.3 * 2; // Converter para a tensão real da bateria (assumindo divisor de tensão 2:1)
  
  if (tensao < 3.3) {
    if (millis() - Bip > 5000) {
      tone(13, 900, 400);  // Emite um bip de alerta
      Bip = millis();
    }
  }
}

void zeraVariaveis(){
  myJoystick.Start = 0;
  myJoystick.Select = 0;
  myJoystick.SetaCima = 0;
  myJoystick.SetaDireita = 0;
  myJoystick.SetaEsquerda = 0;
  myJoystick.SetaBaixo =0;  
  myJoystick.BotaoTriangulo = 0;
  myJoystick.BotaoCirculo = 0;
  myJoystick.BotaoQuadrado = 0;
  myJoystick.BotaoX = 0 ;
  myJoystick.BotoesRs = 0x11111111; //Nenhum botão precionado
  myJoystick.JoyDireitaX = 0;  //Centro. Se nao der certo, colocar 127
  myJoystick.JoyDireitaY = 0;  //Centro. Se nao der certo, colocar 127
  myJoystick.JoyEsquerdaX = 0; //Centro. Se nao der certo, colocar 127
  myJoystick.JoyEsquerdaY = 0; //Centro. Se nao der certo, colocar 127

}