#include <HardwareSerial.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

// Definições de pinos
#define PIN_M0   7
#define PIN_M1   15
#define PIN_AUX  18

// Definições de Serial
HardwareSerial SerialLoRa(2); // UART2 para o E220
HardwareSerial SerialCAN(1);  // UART1 simulando o CAN

// Estado atual do módulo
enum LoRaMode { NORMAL, WAKE_UP, POWER_SAVING, CONFIG };
LoRaMode currentMode = NORMAL;

// Prototipagem
bool setLoRaMode(LoRaMode mode);
bool checkLoRaConnected();
void taskBridge(void *pvParameters);
void readParameters();
void saveParameters();

// Config inicial
void setup() {
  Serial.begin(115200);
  Serial.println("Iniciando ESP32-S3 + E220...");

  // Inicializa GPIOs
  pinMode(PIN_M0, OUTPUT);
  pinMode(PIN_M1, OUTPUT);
  pinMode(PIN_AUX, INPUT);

  // Inicializa Seriais
  SerialLoRa.begin(9600, SERIAL_8N1, 17, 16); // RX=17, TX=16
  SerialCAN.begin(115200, SERIAL_8N1, 9, 10); // Simulação CAN (mude os pinos conforme seu setup)

  // Coloca o módulo em modo normal
  setLoRaMode(NORMAL);

  delay(500); // Aguarda estabilização

  // Testa conexão
  if (checkLoRaConnected()) {
    requisitarParametrosLoRa();
    Serial.println("LoRa E220 detectado!");
  } else {
    Serial.println("Erro: LoRa E220 não detectado.");
  }

  // Cria a task de ponte entre LoRa e CAN
  xTaskCreatePinnedToCore(
    taskBridge, 
    "TaskBridge", 
    4096, 
    NULL, 
    1, 
    NULL, 
    1
  );
}

void loop() {
  // Você pode colocar comandos de teste aqui se quiser.
}

// Define o modo do LoRa
bool setLoRaMode(LoRaMode mode) {
  // Ajusta os pinos M0 e M1 conforme o modo desejado
  switch (mode) {
    case NORMAL:
      digitalWrite(PIN_M0, LOW);
      digitalWrite(PIN_M1, LOW);
      break;
    case WAKE_UP:
      digitalWrite(PIN_M0, HIGH);
      digitalWrite(PIN_M1, LOW);
      break;
    case POWER_SAVING:
      digitalWrite(PIN_M0, LOW);
      digitalWrite(PIN_M1, HIGH);
      break;
    case CONFIG:
      digitalWrite(PIN_M0, HIGH);
      digitalWrite(PIN_M1, HIGH);
      break;
  }

  currentMode = mode;

  delay(100); // Aguarda sinalização inicial de troca
  
  // ESPERA o AUX ir para HIGH, indicando que a troca completou
  unsigned long timeout = millis() + 1000;
  while (digitalRead(PIN_AUX) == LOW) {
    if (millis() > timeout) {
      Serial.println("Erro: Timeout aguardando AUX após mudança de modo.");
      return false;
    }
  }

  delay(100); // Manual pede 2ms após AUX ficar HIGH
  return true;
}

// Verifica se o E220 responde
bool checkLoRaConnected() {
  if (!setLoRaMode(CONFIG)) {
    Serial.println("Falha ao mudar para CONFIG!");
    return false;
  }

  bool conectado = false;

  SerialLoRa.write(0xC1);
  delay(100);
  while (SerialLoRa.available()) {
    uint8_t dado = SerialLoRa.read();
    if (dado == 0xC1) {
      conectado = true;
    }
  }
  setLoRaMode(NORMAL);

  return conectado;
}

String comandoRecebido = "";

// Task que faz o espelhamento Serial e interpreta comandos de texto
void taskBridge(void *pvParameters) {
  while (true) {
    if (Serial.available()) {
      char c = (char)Serial.read();
      if (c == '\n' || c == '\r') {
        // Quando apertar ENTER, processa o comando recebido
        if (comandoRecebido.length() > 0) {
          if (comandoRecebido == "1") {
            Serial.println("Comando: Requisitar parâmetros do LoRa");
            requisitarParametrosLoRa();
          } 
          else if (comandoRecebido.startsWith("2") && comandoRecebido.length() == 11) {
            // comando: 2 + 10 digitos (2 digitos por byte em hexadecimal)

            Serial.println("Comando: Configurar LoRa com novos parâmetros");

            // Parseia os números
            byte addr = strtol(comandoRecebido.substring(1, 3).c_str(), NULL, 16);
            byte baud = strtol(comandoRecebido.substring(3, 5).c_str(), NULL, 16);
            byte packet = strtol(comandoRecebido.substring(5, 7).c_str(), NULL, 16);
            byte channel = strtol(comandoRecebido.substring(7, 9).c_str(), NULL, 16);
            byte worCycle = strtol(comandoRecebido.substring(9, 11).c_str(), NULL, 16);

            Serial.print("Addr: "); Serial.println(addr, HEX);
            Serial.print("Baud: "); Serial.println(baud, HEX);
            Serial.print("Packet: "); Serial.println(packet, HEX);
            Serial.print("Channel: "); Serial.println(channel, HEX);
            Serial.print("WorCycle: "); Serial.println(worCycle, HEX);

            configurarModuloLoRa(addr, baud, packet, channel, worCycle);
          }
          else {
            Serial.println("Comando inválido.");
          }
          comandoRecebido = ""; // Limpa para próxima leitura
        }
      } 
      else {
        comandoRecebido += c;
      }
    }

    // Dados recebidos do LoRa -> envia para o CAN normalmente
    if (SerialLoRa.available()) {
      while (SerialLoRa.available()) {
        SerialCAN.write(SerialLoRa.read());
      }
    }

    vTaskDelay(1);
  }
}


struct LoRaParametrosModel {
  uint8_t address;
  uint8_t baudAndAirRate;
  uint8_t packetSizeAndPower;
  uint8_t channel;
  uint8_t tranModeAndWorCycle;
};

// Parâmetros globais
LoRaParametrosModel parametrosAtual;

// Método que monta o comando de configuração
bool configurarModuloLoRa(byte addr, byte baud, byte packet, byte channel, byte worCycle) {
  if (!setLoRaMode(CONFIG)) {
    Serial.println("Falha ao mudar para CONFIG!");
    return false;
  }

  limparBufferLoRa();

  uint8_t command[9] = {
    0xC0, // Código para configurar
    0x00, // Registro inicial
    0x06, // Quantidade de registros
    0x00, // Reservado
    addr,
    baud,
    packet,
    channel,
    worCycle
  };

  SerialLoRa.write(command, sizeof(command));
  Serial.println("Comando de configuração enviado!");

  unsigned long startTime = millis();
  bool sucesso = false;

  while (millis() - startTime < 3000) { // timeout 3 segundos
    if (SerialLoRa.available()) {
      uint8_t cabecalho = SerialLoRa.read();
      if (cabecalho == 0xC1) { // 0xC1 é Resposta de Configuração
        sucesso = true;
        Serial.println("Confirmação de configuração recebida!");
      } else {
        Serial.print("Resposta inesperada: 0x");
        Serial.println(cabecalho, HEX);
      }
      break;
    }
    delay(10);
  }

  setLoRaMode(NORMAL);

  return sucesso;
}

// Método que requisita a leitura dos parâmetros atuais
bool requisitarParametrosLoRa() {
  if (!setLoRaMode(CONFIG)) {
    Serial.println("Falha ao mudar para CONFIG!");
    return false;
  }

  limparBufferLoRa();

  uint8_t command[3] = { 0xC1, 0x00, 0x0B };

  SerialLoRa.write(command, sizeof(command));
  Serial.println("Comando de requisição enviado!");

  unsigned long startTime = millis();
  bool sucesso = false;

  while (!sucesso && millis() - startTime < 3000) { // timeout 3 segundos
    if (SerialLoRa.available()) {
      uint8_t cabecalho = SerialLoRa.read();
      if (cabecalho == 0xC1) { // Cabeçalho de resposta correta
        uint8_t startAddr = SerialLoRa.read(); // Deve ser 0x00
        uint8_t dataLen = SerialLoRa.read();   // Deve ser 0x0B

        if (startAddr == 0x00 && dataLen == 0x0B) {
          Serial.println("Parâmetros recebidos corretamente.");

          // Agora ler os registros
          parametrosAtual.address = SerialLoRa.read();
          parametrosAtual.baudAndAirRate = SerialLoRa.read();
          parametrosAtual.packetSizeAndPower = SerialLoRa.read();
          parametrosAtual.channel = SerialLoRa.read();
          parametrosAtual.tranModeAndWorCycle = SerialLoRa.read();
          
          // Exibe os valores lidos
          Serial.println("Parametros lidos do módulo:");
          Serial.print("Address: 0x");
          Serial.println(parametrosAtual.address, HEX);

          Serial.print("Baud and AirRate: 0x");
          Serial.println(parametrosAtual.baudAndAirRate, HEX);

          Serial.print("Packet Size and Power: 0x");
          Serial.println(parametrosAtual.packetSizeAndPower, HEX);

          Serial.print("Channel: 0x");
          Serial.println(parametrosAtual.channel, HEX);

          Serial.print("TranMode and WorCycle: 0x");
          Serial.println(parametrosAtual.tranModeAndWorCycle, HEX);

          // Ignorar os bytes restantes (Reservados, etc)
          for (int i = 0; i < (0x0B - 5); i++) {
            SerialLoRa.read();
          }

          sucesso = true;
        }
      } else if (cabecalho == 0xFC) {
        Serial.println("Modo invalido!");
      }
      /*else {
        Serial.print("Resposta inesperada: 0x");
        Serial.println(cabecalho, HEX);
        Serial.println("Resposta recebida: ");
        while (SerialLoRa.available()) {
          uint8_t byteLido = SerialLoRa.read();
          Serial.print("0x");
          Serial.print(byteLido, HEX);
          Serial.print(" ");
        }
        Serial.println();
      }*/
    }
    delay(10);
  }

  setLoRaMode(NORMAL);

  return sucesso;
}

// Limpa buffer anterior
void limparBufferLoRa() {
  while (SerialLoRa.available()) {
    SerialLoRa.read();
  }
}