#include "SerialService.h"

#ifndef CANSERVICE_H
#define CANSERVICE_H

#include <Arduino.h>
#include <driver/twai.h>
#include <esp_task_wdt.h>

// Garante o core de aplicação (ESP32-S3)
#ifndef APP_CPU_NUM
#define APP_CPU_NUM 1
#endif

class CanService {
public:
  typedef void (*OnReceiveCallback)(int packetSize, int senderId,
                                    CanMessagePosicaoDados posicao, byte* data, int dataLength);

  // --- IDs especiais do teu protocolo ---
  uint8_t ID_Num_sMOD = 0;
  uint8_t ID_Num_sTOD = 250;
  uint8_t ID_Num_sLRA = 251;

  CanService(uint8_t nodeId, uint32_t baudRate = 250000)
  : _nodeId(nodeId), _baudRate(baudRate) {}

  void begin() {
    PrintTela("[CAN] Iniciando TWAI...");

    // Modo NORMAL (com ACK)
    twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(GPIO_NUM_5, GPIO_NUM_4, TWAI_MODE_NORMAL);
    // Aumenta filas internas do driver para evitar enrosco
    g_config.tx_queue_len = 32;
    g_config.rx_queue_len = 32;
    // (alerts_enabled será reconfigurado depois de start)

    twai_timing_config_t t_config;
    // 500 kbit/s (ajuste se precisar)
    if (_baudRate == 250000)
      t_config = TWAI_TIMING_CONFIG_250KBITS();
    else if (_baudRate == 500000)
      t_config = TWAI_TIMING_CONFIG_500KBITS();

    // Começa aceitando tudo para depurar; depois afinamos a máscara
    twai_filter_config_t f_config = {
      .acceptance_code = (uint32_t(_nodeId) << 21),
      .acceptance_mask = ~(0x7FFu << 21),
      .single_filter = true
    };

    esp_err_t err = twai_driver_install(&g_config, &t_config, &f_config);
    if (err != ESP_OK) {
      PrintTela("[CAN] Erro ao instalar driver: ", false); PrintTela(String(err));
      return;
    }

    err = twai_start();
    if (err != ESP_OK) {
      PrintTela("[CAN] Erro ao iniciar TWAI: ", false); PrintTela(String(err));
      return;
    }

    // Habilita alerts úteis
    uint32_t alert_mask = 0;
    alert_mask |= TWAI_ALERT_BUS_OFF;
    alert_mask |= TWAI_ALERT_RX_QUEUE_FULL;
    alert_mask |= TWAI_ALERT_TX_FAILED;

    #if defined(TWAI_ALERT_PERIPH_ERR)
      alert_mask |= TWAI_ALERT_PERIPH_ERR;
    #elif defined(TWAI_ALERT_PERIPH_RESET)
      alert_mask |= TWAI_ALERT_PERIPH_RESET;
    #endif

    #if defined(TWAI_ALERT_RECOVERY_COMPLETE)
      alert_mask |= TWAI_ALERT_RECOVERY_COMPLETE;
    #elif defined(TWAI_ALERT_RECOVERY_IN_PROGRESS)
      alert_mask |= TWAI_ALERT_RECOVERY_IN_PROGRESS;
    #endif

    twai_reconfigure_alerts(alert_mask, nullptr);

    // --- Filas da nossa arquitetura ---
    callbacksQ = xQueueCreate(64, sizeof(twai_message_t)); // jobs de RX -> callback
    keysQ      = xQueueCreate(64, sizeof(uint16_t));       // chaves coalescentes para TX

    if (!callbacksQ || !keysQ) { PrintTela("[CAN] Falha ao criar filas"); }

    // --- Tasks ---
    xTaskCreatePinnedToCore(CanTaskRxWrapper,   "CanTaskRx",   4096, this, 6, &CanTaskRxHandle,    APP_CPU_NUM);
    xTaskCreatePinnedToCore(CallbackWorkerWrapper,"CallbackWorker",6144,this,5,&CallbackWorkerHandle,APP_CPU_NUM);
    xTaskCreatePinnedToCore(CanTaskTxWrapper,   "CanTaskTx",   4096, this, 4, &CanTaskTxHandle,    APP_CPU_NUM);
    xTaskCreatePinnedToCore(HealthTaskWrapper,  "CanHealth",   4096, this, 3, &HealthTaskHandle,   APP_CPU_NUM);

    // Watchdog por task (vamos resetar dentro dos loops)
    esp_task_wdt_init(5, true);
    esp_task_wdt_add(CanTaskRxHandle);
    esp_task_wdt_add(CallbackWorkerHandle);
    esp_task_wdt_add(CanTaskTxHandle);
    esp_task_wdt_add(HealthTaskHandle);

    PrintTela("[CAN] TWAI iniciado com sucesso");
  }

  // --- Filas e tasks ---
  QueueHandle_t callbacksQ = nullptr;  // fila para o callback worker
  QueueHandle_t keysQ      = nullptr;  // fila de chaves para TX coalescente
  // (Opcional) Se quiser manter uma RX intermediária: QueueHandle_t canQueueRx = nullptr;

  TaskHandle_t CanTaskRxHandle       = NULL;
  TaskHandle_t CallbackWorkerHandle  = NULL;
  TaskHandle_t CanTaskTxHandle       = NULL;
  TaskHandle_t HealthTaskHandle      = NULL;

  // --- Métricas/health ---
  volatile uint64_t last_rx_ts = 0;   // us
  volatile uint64_t last_tx_ts = 0;   // us
  volatile uint32_t rx_drops   = 0;
  volatile uint32_t tx_retries = 0;

  // --- TX coalescing (tabela simples) ---
  static const int MAX_KEYS = 64;     // ajuste conforme nº (pos,addr)
  struct Slot {
    uint16_t key = 0;
    bool     used = false;
    bool     pending = false;         // 🔧 NOVO: chave já enfileirada para envio
    twai_message_t msg;
    uint32_t gen = 0;
  };
  Slot latestByKey[MAX_KEYS];

  // Helpers de chave (pos,addr -> 16 bits)
  static inline uint16_t make_key(const twai_message_t& m) {
    return (m.data_length_code >= 2) ? ( (uint16_t(m.data[0])<<8) | uint16_t(m.data[1]) ) : 0xFFFF;
  }

  bool put_latest(uint16_t key, const twai_message_t& m) {
    int idx = key % MAX_KEYS;
    for (int i=0; i<MAX_KEYS; ++i, idx=(idx+1)%MAX_KEYS) {
      if (!latestByKey[idx].used || latestByKey[idx].key == key) {
        latestByKey[idx].used = true;
        latestByKey[idx].key = key;
        latestByKey[idx].msg = m;
        latestByKey[idx].gen++;
        return true;
      }
    }
    return false;
  }

  bool get_latest(uint16_t key, twai_message_t* out) {
    int idx = key % MAX_KEYS;
    for (int i=0; i<MAX_KEYS; ++i, idx=(idx+1)%MAX_KEYS) {
      if (latestByKey[idx].used && latestByKey[idx].key == key) { *out = latestByKey[idx].msg; return true; }
      if (!latestByKey[idx].used) break;
    }
    return false;
  }

  static void CanTaskRxWrapper(void *pvParameters) {
    static_cast<CanService*>(pvParameters)->CanTaskRx(pvParameters);
  }

  void CanTaskRx(void* pvParameters) {
    twai_message_t message;
    for (;;) {
      // WDT
      esp_task_wdt_reset();

      if (twai_receive(&message, pdMS_TO_TICKS(50)) == ESP_OK) {
        // ignore frames que não vamos tratar
        if (message.data_length_code == 0 || message.extd || message.rtr) {
          continue;
        }

        // marca vida do barramento SEMPRE que recebeu algo válido
        last_rx_ts = esp_timer_get_time();

        if (DebugMode) {
          PrintTela("[CAN RX] ID: 0x" + String(message.identifier, HEX) + " Len: " + String(message.data_length_code));
        }

        // tenta enfileirar pro worker
        if (callbacksQ && xQueueSend(callbacksQ, &message, 0) != pdTRUE) {
          if (DebugMode) {
            PrintTela("Erro ao adicionar mensagem recebida na fila: " + String(rx_drops));
          }
          rx_drops++;

          // (opcional) política "drop oldest": remove 1 e tenta de novo
          twai_message_t dump;
          if (xQueueReceive(callbacksQ, &dump, 0) == pdTRUE) {
            xQueueSend(callbacksQ, &message, 0);
          }

          vTaskDelay(pdMS_TO_TICKS(1)); // dá uma respirada
        }
      }

      vTaskDelay(1);
    }
  }

  static void CallbackWorkerWrapper(void *pvParameters) {
    static_cast<CanService*>(pvParameters)->CallbackWorker(pvParameters);
  }

  void CallbackWorker(void* pvParameters) {
    twai_message_t msg;
    for (;;) {
      // WDT
      esp_task_wdt_reset();

      if (xQueueReceive(callbacksQ, &msg, portMAX_DELAY) == pdTRUE) {
        if (_callback && msg.data_length_code > 0) {
          CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)msg.data[0];
          _callback(msg.data_length_code, msg.identifier, posicao, &msg.data[1], msg.data_length_code - 1);
        }
      }

      // yield leve
      taskYIELD();
    }
  }


  void PublicarTx(const twai_message_t& msg) {
    // Sanidade básica
    if (msg.data_length_code < 2 || msg.data_length_code > 8) return;
    if (msg.extd || msg.rtr) return; // trabalhamos com standard data frames

    uint16_t key = make_key(msg);
    if (key == 0xFFFF) return;

    // Atualiza a versão mais nova na tabela
    int idx = key % MAX_KEYS;
    bool stored = false;
    for (int i=0; i<MAX_KEYS; ++i, idx=(idx+1)%MAX_KEYS) {
      if (!latestByKey[idx].used || latestByKey[idx].key == key) {
        latestByKey[idx].used = true;
        latestByKey[idx].key = key;
        latestByKey[idx].msg = msg;
        latestByKey[idx].gen++;
        stored = true;
        // Se ainda não está pendente, enfileira a chave
        if (!latestByKey[idx].pending && keysQ) {
          if (xQueueSend(keysQ, &key, 0) == pdTRUE) {
            latestByKey[idx].pending = true;
          } else {
            // fila cheia: sem pânico; mantemos a versão mais nova no slot
          }
        }
        break;
      }
    }
    if (!stored) {
      // tabela cheia (raríssimo se MAX_KEYS está adequado) — poderia logar/contar.
    }
  }

  // --- Wrapper padrão ---
  static void CanTaskTxWrapper(void *pvParameters) {
    static_cast<CanService*>(pvParameters)->CanTaskTx(pvParameters);
  }

  // --- Task de TX com retry/backoff e limpeza do "pending" ---
  void CanTaskTx(void* pvParameters) {
    uint16_t key;
    for (;;) {
      // WDT
      esp_task_wdt_reset();

      if (xQueueReceive(keysQ, &key, portMAX_DELAY) == pdTRUE) {
        // busca a msg mais nova dessa chave
        twai_message_t m{};
        int idx = key % MAX_KEYS;
        int foundIdx = -1;
        for (int i=0; i<MAX_KEYS; ++i, idx=(idx+1)%MAX_KEYS) {
          if (latestByKey[idx].used && latestByKey[idx].key == key) {
            m = latestByKey[idx].msg;
            foundIdx = idx;
            break;
          }
          if (!latestByKey[idx].used) break;
        }

        if (foundIdx < 0) {
          // chave desapareceu; nada a fazer
          continue;
        }

        // Garantias do frame (caso quem chamou não tenha setado)
        m.extd = 0;  // standard frame
        m.rtr  = 0;  // data frame

        bool ok = false;
        for (int tent=0; tent<3; ++tent) {
          if (enviarDadosCan(m, pdMS_TO_TICKS(50))) {
            last_tx_ts = esp_timer_get_time();
            ok = true;
            break;
          }
          tx_retries++;
          vTaskDelay(pdMS_TO_TICKS(5));
        }

        // Limpa o "pending" SEMPRE; se falhou, a app pode chamar PublicarTx de novo
        latestByKey[foundIdx].pending = false;

        if (!ok) {
          // Re-enfileira a chave só se ainda houver interesse (a app pode ter publicado algo novo)
          // Re-check: se a slot ainda está usada e não está pendente, refile
          if (latestByKey[foundIdx].used && !latestByKey[foundIdx].pending) {
            if (xQueueSend(keysQ, &key, 0) == pdTRUE) {
              latestByKey[foundIdx].pending = true;
            }
          }
        }
      }

      taskYIELD();
    }
  }

  // --- Versão de enviarDadosCan com timeout custom ---
  bool enviarDadosCan(const twai_message_t& msg, TickType_t timeoutTicks) {
    // Aqui você pode normalizar o ID/dados se precisar
    // Ex.: msg.identifier já deve estar correto para o PC
    esp_err_t err = twai_transmit(&msg, timeoutTicks);
    if (err != ESP_OK) {
      PrintTela("[CAN TX] Falha transmit (" + String((int)err) + ")");
      return false;
    }
    return true;
  }


  static void HealthTaskWrapper(void *pvParameters) {
    static_cast<CanService*>(pvParameters)->HealthTask(pvParameters);
  }

  bool reinit_twai_unsafe_() {
    // ⚠️ Use os mesmos pinos/bitrate do begin(); ajuste se necessário
    twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(GPIO_NUM_5, GPIO_NUM_4, TWAI_MODE_NORMAL);
    g_config.tx_queue_len = 32;
    g_config.rx_queue_len = 32;

    twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
    twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

    if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK) return false;
    if (twai_start() != ESP_OK) { twai_driver_uninstall(); return false; }

    uint32_t alert_mask = 0;
    alert_mask |= TWAI_ALERT_BUS_OFF;
    alert_mask |= TWAI_ALERT_RX_QUEUE_FULL;
    alert_mask |= TWAI_ALERT_TX_FAILED;

    #if defined(TWAI_ALERT_PERIPH_ERR)
      alert_mask |= TWAI_ALERT_PERIPH_ERR;
    #elif defined(TWAI_ALERT_PERIPH_RESET)
      alert_mask |= TWAI_ALERT_PERIPH_RESET;
    #endif

    #if defined(TWAI_ALERT_RECOVERY_COMPLETE)
      alert_mask |= TWAI_ALERT_RECOVERY_COMPLETE;
    #elif defined(TWAI_ALERT_RECOVERY_IN_PROGRESS)
      alert_mask |= TWAI_ALERT_RECOVERY_IN_PROGRESS;
    #endif

    twai_reconfigure_alerts(alert_mask, nullptr);
    return true;
  }

  void HealthTask(void* pvParameters) {
    const uint32_t RX_TIMEOUT_MS = 3000;     // ajuste conforme seu tráfego esperado
    const uint32_t TX_STALL_MS   = 2000;     // quanto tempo uma chave pode ficar sem sair
    const uint8_t  MAX_REC_FAILS = 3;        // depois disso, reboot
    static uint8_t consecutive_rec_fails = 0;

    for (;;) {
      esp_task_wdt_reset();
      uint64_t now = esp_timer_get_time();

      // ---- Alerts do driver ----
      uint32_t alerts = 0;
      if (twai_read_alerts(&alerts, 0) == ESP_OK && alerts) {
        if (alerts & TWAI_ALERT_BUS_OFF) {
          PrintTela("[CAN ALERT] BUS OFF → iniciando recovery...");
          if (twai_initiate_recovery() != ESP_OK) {
            PrintTela("[CAN ALERT] Falha ao iniciar recovery; reiniciando driver...");
            twai_stop(); twai_driver_uninstall();
            if (!reinit_twai_unsafe_()) consecutive_rec_fails++;
            else consecutive_rec_fails = 0;
          }
        }
        #if defined(TWAI_ALERT_RECOVERY_COMPLETE)
          if (alerts & TWAI_ALERT_RECOVERY_COMPLETE) {
            PrintTela("[CAN ALERT] Recovery completo.");
            consecutive_rec_fails = 0;
          }
        #elif defined(TWAI_ALERT_RECOVERY_IN_PROGRESS)
          // Algumas versões só têm "em progresso"; trate como sinal de vida do recovery
          if (alerts & TWAI_ALERT_RECOVERY_IN_PROGRESS) {
            PrintTela("[CAN ALERT] Recovery em progresso (OK).");
            // opcional: zere contador se quiser considerar como sinal saudável
            consecutive_rec_fails = 0;
          }
        #endif
        if (alerts & TWAI_ALERT_RX_QUEUE_FULL) {
          PrintTela("[CAN ALERT] RX interno do driver CHEIO.");
        }
        if (alerts & TWAI_ALERT_TX_FAILED) {
          PrintTela("[CAN ALERT] TX FAILED.");
        }
        #if defined(TWAI_ALERT_PERIPH_ERR)
          if (alerts & TWAI_ALERT_PERIPH_ERR) {
            PrintTela("[CAN ALERT] ERRO no periférico CAN. Reiniciando driver...");
            twai_stop(); twai_driver_uninstall();
            if (!reinit_twai_unsafe_()) consecutive_rec_fails++;
            else consecutive_rec_fails = 0;
          }
        #elif defined(TWAI_ALERT_PERIPH_RESET)
          if (alerts & TWAI_ALERT_PERIPH_RESET) {
            PrintTela("[CAN ALERT] RESET do periférico CAN detectado. Reconfigurando...");
            twai_stop(); twai_driver_uninstall();
            if (!reinit_twai_unsafe_()) consecutive_rec_fails++;
            else consecutive_rec_fails = 0;
          }
        #endif
      }

      // ---- Liveness de RX ----
      if (last_rx_ts && ((now - last_rx_ts) / 1000ULL) > RX_TIMEOUT_MS) {
        PrintTela("[CAN HLTH] Sem RX há " + String((now - last_rx_ts)/1000ULL) + " ms → reiniciando driver...");
        twai_stop(); twai_driver_uninstall();
        if (!reinit_twai_unsafe_()) consecutive_rec_fails++;
        else consecutive_rec_fails = 0;
        last_rx_ts = esp_timer_get_time(); // evita loop de reinicialização
      }

      // ---- Stall de TX (chaves pendentes) ----
      // Se houver muitas chaves esperando por muito tempo, force drenagem
      UBaseType_t pendentes = keysQ ? uxQueueMessagesWaiting(keysQ) : 0;
      if (pendentes > 0 && last_tx_ts && ((now - last_tx_ts) / 1000ULL) > TX_STALL_MS) {
        PrintTela("[CAN HLTH] TX aparentemente parado (" + String(pendentes) + " chaves pendentes).");
        // Estratégia: deixa o CanTaskTx re-enfileirar; se persistir, reinicia driver
        static uint8_t tx_stall_count = 0;
        tx_stall_count++;
        if (tx_stall_count >= 3) {
          PrintTela("[CAN HLTH] TX stall persistente → reiniciando driver...");
          twai_stop(); twai_driver_uninstall();
          if (!reinit_twai_unsafe_()) consecutive_rec_fails++;
          else { consecutive_rec_fails = 0; tx_stall_count = 0; }
          last_tx_ts = esp_timer_get_time();
        }
      }

      // ---- Escalonamento final: reboot do chip se não recuperar ----
      if (consecutive_rec_fails >= MAX_REC_FAILS) {
        PrintTela("[CAN HLTH] Falhas consecutivas ao reiniciar TWAI. Reiniciando ESP32...");
        vTaskDelay(pdMS_TO_TICKS(100));
        esp_restart();
      }

      vTaskDelay(pdMS_TO_TICKS(1000));
    }
  }


  void setReceiveCallback(OnReceiveCallback cb) {
    _callback = cb;
  }

  bool adicionarMensagemFila(const std::vector<uint8_t>& payload) {
    // 1) Sanidade
    if (payload.empty() || payload.size() > 8) {
      PrintTela("[CAN] Payload inválido (0 ou >8 bytes).");
      return false;
    }
    if (payload.size() < 2) { // precisa de pos (byte0) e addr (byte1) para formar a chave
      PrintTela("[CAN] Payload precisa ter pelo menos pos e addr (2 bytes).");
      return false;
    }

    // 2) Monta frame
    twai_message_t msg{};
    // ⚠️ ID: use o ID que o PC espera para respostas desse nó (ajuste se necessário)
    msg.identifier = _nodeId;       // ou outro mapeamento do seu protocolo
    msg.extd = 0;                   // standard frame
    msg.rtr  = 0;                   // data frame
    msg.ss   = 0;
    msg.data_length_code = payload.size();

    for (uint8_t i = 0; i < payload.size(); ++i) {
      msg.data[i] = payload[i];
    }

    // 3) Publica via coalescência (substitui a antiga fila TX)
    PublicarTx(msg);
    return true;
  }


  F_Code FuncaoPorPosicao(CanMessagePosicaoDados posicao) {
    F_Code funcao = F_Code::Nda;
    int pos = static_cast<int>(posicao);
    if (pos >= 0 && pos <= 50) {
      funcao = F_Code::ReqTx;
    }
    else if (pos > 50 && pos <= 100) {
      funcao = F_Code::CfgTx;
    }
    else if (pos > 100 && pos <= 150) {
      funcao = F_Code::CmdTx;
    }
    //PrintTela("Funcao parseada para posicao: " + String(pos) + ", funcao: " + String(funcao));
    return funcao;
  }

  std::vector<uint8_t> MontarFrameReqStatusMod(T_Code D_Code, bool Conectado, int Versao) {
    std::vector<uint8_t> data;
    data.push_back(static_cast<uint8_t>(CanMessagePosicaoDados::Status));
    data.push_back(ID_Num_sMOD);
    data.push_back(static_cast<uint8_t>(D_Code));
    data.push_back(Conectado ? 1 : 0);
    data.push_back(static_cast<uint8_t>(Versao));
    return data;
  }

  std::vector<uint8_t> MontarFrameReqDadosFim(int latencia) {
    std::vector<uint8_t> data;
    data.push_back(static_cast<uint8_t>(CanMessagePosicaoDados::DadosAll));
    data.push_back(ID_Num_sTOD);
    data.push_back(latencia >> 8); data.push_back(latencia & 0xFF);
    return data;
  }


private:
  uint32_t _baudRate = 250000;
  uint8_t  _nodeId   = 0;
  OnReceiveCallback _callback = nullptr;
  bool DebugMode = false;
};

#endif // CANSERVICE_H