/*configurações bluetooth hc-05
  senha: 2570
  velocidade: 115200
  versão software: 3.0-20170601
  nome K-bot
  modo de operação: slave
*/


//Portas de comunicação bluetooth
#define Tx 3
#define Rx 2

//portas de controle de motores e acessórios
#define In1 5
#define In2 6
#define In3 9
#define In4 10
//       L  C


#define Buzina 11
#define LedF A3
#define LedB A2

//porta do ESC
#define ESC A0

//Porta de sensores
#define WSensor A1

//Bibliotecas
//#include <Servo.h>
#include "neotimer.h"
#include <ServoTimer2.h>
#include <SoftwareSerial.h>

//variáveis
int Pw = 1000;           // Valor inicial em microsegundos (mínimo)
int Max_Pw = 2000;       // Máximo para ESC (não ultrapassar 2000 µs)
int SMin = 1500;         // define o valor minimo para que o sensor desligue a serra após ligada
int Pw_Gain = 10;        // Ganho por ciclo
int Tp_Pw = 100;         // Tempo entre cada ciclo de aumento
int Sv_Pw = 0;           // Define se a serra está ou não ligada
byte DY = 0;
boolean Ats = false;
char Crct = "";
int estadoLed[] = {LOW, LOW};

/*
  Motores Dmax - Emax
  aqui será definido o quanto a velocidade
  dos motores será reduzida para que seja
  feita uma curva ao ir para frente/ré (1 - 100)
*/
byte Dmax = 100;
byte Emax = 100;

byte VMMin = 50; //define a velocidade minima do motor

byte cmm = 0;
byte Pw_Vel[] = {0, 0, 0, 0};
String Text = "";       //guarda todos os comandos recebidos pelo modulo bluetooth
int Nivel[] = {95, 95}; //guarda os valores do eixo x y

SoftwareSerial Bt(Rx, Tx);
ServoTimer2 Esc;
//Servo Esc;
Neotimer Delay0 = Neotimer(Tp_Pw);
Neotimer Delay1 = Neotimer(2000); // Tempo para rearmar ESC
Neotimer Delay2 = Neotimer(200); // tempo de resposta do comando
Neotimer Delay3 = Neotimer(10);
//---------------------------------------------------------------------------------------------------//


bool isValidCommand(String cmd) {
  cmd.trim();

  // Comandos simples: W, A, S, D
  if (cmd == "W" || cmd == "A" || cmd == "S" || cmd == "D") return true;

  // Comandos numéricos com letra final (ex: 123X, 255Y)
  if (cmd.length() >= 2 && cmd.length() <= 5) {
    char lastChar = cmd.charAt(cmd.length() - 1);
    if (lastChar == 'X' || lastChar == 'Y') {
      String numberPart = cmd.substring(0, cmd.length() - 1);
      int val = numberPart.toInt();
      if (val >= 0 && val <= 180) return true;
    }
  }

  return false;
}



//---------------------------------------------------------------------------------------------------//
void setup() {
  Esc.attach(ESC);
  Serial.begin(115200);
  Serial.println("Iniciando monitor");
  Bt.begin(115200);

  // Arming ESC: enviar sinal mínimo e aguardar
  //Esc.writeMicroseconds(1000);
  Esc.write(1000);
  delay(2000);  // Espera o ESC armar (pode ouvir os bipes do motor)

  pinMode(In1, OUTPUT);
  pinMode(In2, OUTPUT);
  pinMode(In3, OUTPUT);
  pinMode(In4, OUTPUT);

  pinMode(WSensor, INPUT);
}

void loop() {


  Mc();
  Bluetooth();
  if (Ats == true) { //verifica se a serra foi ativada
    DY = 1; //guarda 1 em DY para pular o tempo de espera do Delay1
    Serra(); //chama a função de trabalho da serra
  } else {
    //Esc.writeMicroseconds(1000); //desluga o motor da serra
    Esc.write(1000); //desliga o motor da serra
    DY = 0;
    Pw = 1000; //reseta o aumenta de velocidade da serra
    Delay1.repeatReset(); //reseta o tempo de segurança para ligar a serra
  }
}

void Serra() {
  // Esc.writeMicroseconds(Pw); //envia o valor de Pw para aumentar a velocidade da serra
  Esc.write(Pw); //envia o valor de Pw para aumentar a velocidade da serra

  if (Delay1.repeat(1)) { //tempo de segurança para reativamento da serra
    DY = 1;
  }

  if (Pw <= Max_Pw && DY == 1) { //aumenta gradativamente a velocidade da serra caso DY seja 1.
    if (Delay0.repeat()) {
      Pw = Pw + Pw_Gain;
      if (Pw >= Max_Pw) {
        Pw = Max_Pw;
        DY = 0;
      }
    }
  }

  if (digitalRead(WSensor) == HIGH) { //verifica se o sensor está ativo
    if (Pw >= SMin) { //desliga a serra caso a mesma esteja em uma velocidade média apos o ativamento do sensor
      Pw = 1000;
      DY = 0;
      Esc.write(Pw);
      Delay1.repeatReset();
    }
  }

}
/*void Bluetooth() {
  
   if (Bt.available()) {
    char c = Bt.read();

    // Filtro de caracteres ASCII válidos (evita bytes corrompidos)
    if (c >= 32 && c <= 126) {
      if (c == '\n' || c == '\r') {
        if (isValidCommand(Text)) {
          Serial.print("Comando válido: ");
          Serial.println(Text);
          // Aqui você executaria a lógica do comando
          if (Crct == 'X') { //verifica qual é o valor de X
        Serial.println(Text);
        Nivel[0] = Text.toInt(); //Converte o texto em um valor numérico "180X" -> 180 e guarda em Nivel[0]
        Text = "";
      }
      if (Text == 'Y') { //verifica qual é o valor de Y
        Serial.println(Text);
        Nivel[1] = Text.toInt(); //Converte o texto em um valor numérico "180Y" -> 180 e guarda em Nivel[1]
        Text = "";
      }

      if (Text == 'D') { //Comando que ativa a buzina (Prioridade)
        Serial.println(Text);
        tone(Buzina, 391); //liga a buzina
        Text = "";
      } 
  if (Delay2.repeat()) {      
      if (Text == 'S') { //comando que liga ou desliga as luzes traseiras

        if (cmm == 0) {
          Serial.println(Text);
          estadoLed[0] = !estadoLed[0];
          digitalWrite(LedB, estadoLed[0]); //inverte o estado anterior de saida da porta
          cmm = 1; //altera o valor de cmm para o comando não se repetir caso o botão esteja pressionado
        }
        Text = "";

      } else if (Text == 'W') { //Comando que liga ou desliga as luzes dianteiras

        if (cmm == 0) {
          Serial.println(Text);
          estadoLed[1] = !estadoLed[1];
          digitalWrite(LedF, estadoLed[1]); //inverte o estado anterior de saida da porta
          cmm = 1; //altera o valor de cmm para o comando não se repetir caso o botão esteja pressionado
        }
        Text = "";

      } else if (Text == 'A' ) { //comando que liga ou desliga a serra

        if (cmm == 0) {
          Ats = !Ats;
          cmm = 1;
        }
        Text = "";

      } else {
        noTone(Buzina); //desliga a buzina
        cmm = 0;
      }
        } else {
          Serial.print("Comando inválido: ");
          Serial.println(Text);
        }
        Text = ""; // Reset do buffer
      } else {
        Text += c;
      }

      // Limite de segurança para evitar buffer overflow
      if (Text.length() > 10) {
        Text = "";
      }
    } else {
      // Ignora caracteres não imprimíveis (possível corrupção)
    }
  }

   }
  /*if (Bt.available()) { //verifica se algum comando foi recebido (Recebe primeiro um valor numerico e em seguida uma letra)
    Crct = Bt.read(); //guarda um caractere lido en Crct
    Text += Crct; //Junta todos os caracteres recebidos e guarda na String Text


      if (Crct == 'X') { //verifica qual é o valor de X
        Serial.println(Text);
        Nivel[0] = Text.toInt(); //Converte o texto em um valor numérico "180X" -> 180 e guarda em Nivel[0]
        Text = "";
      }
      if (Crct == 'Y') { //verifica qual é o valor de Y
        Serial.println(Text);
        Nivel[1] = Text.toInt(); //Converte o texto em um valor numérico "180Y" -> 180 e guarda em Nivel[1]
        Text = "";
      }

      if (Crct == 'D') { //Comando que ativa a buzina (Prioridade)
        Serial.println(Text);
        tone(Buzina, 391); //liga a buzina
        Text = "";
      } 
  if (Delay2.repeat()) {      
      if (Crct == 'S') { //comando que liga ou desliga as luzes traseiras

        if (cmm == 0) {
          Serial.println(Text);
          estadoLed[0] = !estadoLed[0];
          digitalWrite(LedB, estadoLed[0]); //inverte o estado anterior de saida da porta
          cmm = 1; //altera o valor de cmm para o comando não se repetir caso o botão esteja pressionado
        }
        Text = "";

      } else if (Crct == 'W') { //Comando que liga ou desliga as luzes dianteiras

        if (cmm == 0) {
          Serial.println(Text);
          estadoLed[1] = !estadoLed[1];
          digitalWrite(LedF, estadoLed[1]); //inverte o estado anterior de saida da porta
          cmm = 1; //altera o valor de cmm para o comando não se repetir caso o botão esteja pressionado
        }
        Text = "";

      } else if (Crct == 'A' ) { //comando que liga ou desliga a serra

        if (cmm == 0) {
          Ats = !Ats;
          cmm = 1;
        }
        Text = "";

      } else {
        noTone(Buzina); //desliga a buzina
        cmm = 0;
      }
    }
  }
}*/

void Bluetooth() {
  while (Bt.available()) {
    Crct = Bt.read();

    // Filtro de caracteres ASCII válidos (evita ruído)
    if (Crct >= 32 && Crct <= 126) {
      Text += Crct;

      // Se recebeu um comando direto (1 caractere), já trata
      if (Text == "W" || Text == "A" || Text == "S" || Text == "D") {
        Serial.print("Comando direto: ");
        Serial.println(Text);

        if (Text == "D") tone(Buzina, 391);
        else if (Text == "S") {
          if (cmm == 0) {
            estadoLed[0] = !estadoLed[0];
            digitalWrite(LedB, estadoLed[0]);
            cmm = 1;
          }
        }
        else if (Text == "W") {
          if (cmm == 0) {
            estadoLed[1] = !estadoLed[1];
            digitalWrite(LedF, estadoLed[1]);
            cmm = 1;
          }
        }
        else if (Text == "A") {
          if (cmm == 0) {
            Ats = !Ats;
            cmm = 1;
          }
        }

        Text = "";
      }

      // Comandos com final 'X' ou 'Y' (ex: 120X, 80Y)
      if (Text.endsWith("X") || Text.endsWith("Y")) {
        char eixo = Text.charAt(Text.length() - 1);
        String valorStr = Text.substring(0, Text.length() - 1);
        int valor = valorStr.toInt();

        if (valor >= 0 && valor <= 180) {
          if (eixo == 'X') {
            Nivel[0] = valor;
            Serial.print("Eixo X: ");
            Serial.println(valor);
          } else {
            Nivel[1] = valor;
            Serial.print("Eixo Y: ");
            Serial.println(valor);
          }
        } else {
          Serial.print("Valor inválido: ");
          Serial.println(Text);
        }
        Text = "";
      }

      // Limita o buffer para não acumular lixo
      if (Text.length() > 6) Text = "";
    }
  }

  // Zera cmm se nenhum botão estiver pressionado (evita travamento)
  if (!Bt.available()) {
    cmm = 0;
    noTone(Buzina);
  }
}

void Mc() {
  /* Função de movimento dos motores
     contorle por potenciometro de 0 a 180
     Nivel[] recebe os valores e guarda em (X, Y).
     X define a direção e o quanto o robô conseguirar virar para tal direção
     Y Define a velocidade e se o robô deverá seguir para frente ou para Tras
     110 - 180X define a Esquerda
        80 - 0X define a Direita
     110 - 180Y faz ir para Frente
        80 - 0Y faz ir para Tras
     90 - 100X/Y centro - nada acontece
  */


  if (Nivel[1] >= 150 && Nivel[0] >= 150) { //frente + esquerda
    //motor direito
    analogWrite(In1, 0); //define o quanto a velocidade aumenta para ir para frente
    analogWrite(In2, 0);
    //motor esquerdo

    analogWrite(In3, 255);
    analogWrite(In4, 0);

  } else if (Nivel[1] >= 150 && Nivel[0] <= 60) { //frente + direita
    //motor direito
    analogWrite(In1, 255);
    analogWrite(In2, 0);
    //motor esquerdo
    analogWrite(In3, 0); //define o quanto a velocidade aumenta para ir para frente
    analogWrite(In4, 0);
  } else if (Nivel[1] <= 60 && Nivel[0] >= 150) { //ré + esquerda
    //motor direito
    analogWrite(In2, 255); //define o quanto a velocidade aumenta para ir para tras
    analogWrite(In1, 0);
    //motor esquerdo

    analogWrite(In4, 100);
    analogWrite(In3, 0);

  } else if (Nivel[1] <= 60 && Nivel[0] <= 60) { //ré + direita
    //motor direito

    analogWrite(In2, 100);
    analogWrite(In1, 0);
    //motor esquerdo
    analogWrite(In4, 255); //define o quanto a velocidade aumenta para ir para tras
    analogWrite(In3, 0);
  } else if (Nivel[1] >= 150) { //frente
    //motor direito
    analogWrite(In1, 255);
    analogWrite(In2, 0);
    //motor esquerdo
    analogWrite(In3, 255);
    analogWrite(In4, 0);
  } else if (Nivel[1] <= 60) { //ré
    //motor direito
    analogWrite(In1, 0);
    analogWrite(In2, 255);
    //motor esquerdo
    analogWrite(In3, 0);
    analogWrite(In4, 255);
  } else if (Nivel[0] >= 150) { //esquerda

    //motor direito
    analogWrite(In1, 255);
    analogWrite(In2, 0);
    //motor esquerdo
    analogWrite(In3, 0);
    analogWrite(In4, 255);
  } else if (Nivel[0] <= 60) { //direita

    //motor direito
    analogWrite(In1, 0);
    analogWrite(In2, 255);
    //motor esquerdo
    analogWrite(In3, 255);
    analogWrite(In4, 0);
  } else {
    //motor direito
    analogWrite(In2, 0);
    analogWrite(In1, 0);
    //motor esquerdo
    analogWrite(In4, 0);
    analogWrite(In3, 0);
  }

}