#include "SerialService.h"

// CanService.h
#ifndef CANSERVICE_H
#define CANSERVICE_H

/*
#include <CAN.h>
#include <Arduino.h>

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

    CanService(uint8_t nodeId, uint32_t baudRate = 500E3) {
      _baudRate = baudRate;
      _nodeId = nodeId;
    }

    void begin() {
      if (!CAN.begin(_baudRate)) {
        PrintTela("[CAN] Falha ao iniciar");
        return;
      }

      PrintTela("[CAN] Inicializado com sucesso");
      CAN.onReceive(onReceiveWrapper);
      instance = this;
    }

    void loop() {
      // Apenas processa mensagens recebidas via interrupcao
    }

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

    bool enviarMensagem(byte funcCode, const std::vector<uint8_t>& payload) {
        if (payload.size() > 7) return false;
    
        CAN.beginPacket(_nodeId);
        CAN.write(funcCode);
        for (uint8_t b : payload) {
            CAN.write(b);
        }
        return CAN.endPacket();
    }

    std::vector<uint8_t> MontarFrameChk(T_Code D_Code, bool Conectado, int Versao) {
        std::vector<uint8_t> resposta;
        resposta.push_back(static_cast<uint8_t>(D_Code));          // Tipo do módulo
        resposta.push_back(Conectado ? 1 : 0);                     // Conectado
        resposta.push_back(Versao);                                // Versão
      
        return resposta;
    }

  private:
    uint32_t _baudRate;
    uint8_t _nodeId;
    OnReceiveCallback _callback = nullptr;

    static CanService* instance;

    static void onReceiveWrapper(int packetSize) {
      if (instance) instance->onReceive(packetSize);
    }

    void onReceive(int packetSize) {
      int senderId = CAN.packetId();
      byte buffer[8];
      int i = 0;

      while (CAN.available() && i < 8) {
        buffer[i++] = CAN.read();
      }

      byte funcCode = buffer[0];
      if (_callback != nullptr) {
        _callback(packetSize, senderId, funcCode, buffer + 1, i - 1);
      }
    }
};

CanService* CanService::instance = nullptr;
*/

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

class CanService {

  typedef void (*OnReceiveCallback)(int packetSize, int senderId, CanMessagePosicaoDados posicao, byte* data, int dataLength);

  typedef struct {
    OnReceiveCallback callback;
    int packetSize;
    int senderId;
    CanMessagePosicaoDados posicao;
    byte data[7];
    int dataLength;
    long idMsg;
  } CallbackPayload;

  static TaskHandle_t handleCallbackTask;

  public:
    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 = 500000) {
      _nodeId = nodeId;
      _baudRate = baudRate;
    }

    void begin() {
      PrintTela("[CAN] Iniciando TWAI...");
    
      twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(GPIO_NUM_5, GPIO_NUM_4, TWAI_MODE_NORMAL); // modo sem ACK
      twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
      twai_filter_config_t f_config = {
        .acceptance_code = (_nodeId << 21),   // Shiftado para alinhar com o padrão TWAI
        .acceptance_mask = ~(0x7FF << 21),    // Mascara para filtrar apenas esse ID
        .single_filter = true
      };
    
      esp_err_t err;
    
      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;
      }

      if (CanTaskRxHandle == NULL) {
        canQueueRx = xQueueCreate(50, sizeof(twai_message_t));
        if (canQueueRx == NULL) {
          PrintTela("[CAN] Falha ao criar fila RX!");
        }
        xTaskCreatePinnedToCore(CanService::CanTaskRxWrapper, "CanTaskRx", 4096, this, 17, &CanTaskRxHandle, APP_CPU_NUM);
      }

      if (CanTaskTxHandle == NULL) {
        canQueueTx = xQueueCreate(50, sizeof(twai_message_t));
        if (canQueueTx == NULL) {
          PrintTela("[CAN] Falha ao criar fila TX!");
        }
        xTaskCreatePinnedToCore(CanService::CanTaskTxWrapper, "CanTaskTx", 4096, this, 16, &CanTaskTxHandle, APP_CPU_NUM);
      }

      if (CanTaskProcessHandle == NULL) {
        xTaskCreatePinnedToCore(CanService::CanTaskProcessWrapper, "CanTaskProcess", 6144, this, 18, &CanTaskProcessHandle, APP_CPU_NUM);
      }
    
      PrintTela("[CAN] TWAI iniciado com sucesso");
    }


    QueueHandle_t canQueueRx;
    TaskHandle_t CanTaskRxHandle = NULL;
    static void CanTaskRxWrapper(void *pvParameters) {
      CanService *service = static_cast<CanService*>(pvParameters);
      service->CanTaskRx(pvParameters);
    }

    void CanTaskRx(void* pvParameters) {
      twai_message_t message;
      while (true) {
        if (twai_receive(&message, pdMS_TO_TICKS(10)) == ESP_OK) {

          if (message.data_length_code == 0 || message.extd) continue;
  
          bool MensagemTx = message.identifier == static_cast<uint32_t>(_nodeId + 1);
  
          if (DebugMode) {
            PrintTela("[CAN ", false);
            PrintTela(MensagemTx ? "TX" : "RX", false);
            PrintTela("] ID: 0x", false);
            PrintTela(String(message.identifier, HEX), false);
            PrintTela(" Len: ", false);
            PrintTela(String(message.data_length_code), false);
            PrintTela(" Data: ", false);
            for (int i = 0; i < message.data_length_code; i++) {
              PrintTela(String(message.data[i], HEX), false);
              PrintTela(" ", false);
            }
            PrintTela("");
          }
  
          if (MensagemTx) {
            PrintTela("[CAN RX] Ouviu a própria resposta!");
            continue;
          }
  
          if (!mensagemJaNaFila(canQueueRx, message)) {
            
            if (uxQueueSpacesAvailable(canQueueRx) < 5) {
              PrintTela("[CAN RX] Alerta: Fila RX quase cheia (" + String(uxQueueSpacesAvailable(canQueueRx)) + " mensagens restantes)");
            }

            if (xQueueSend(canQueueRx, &message, 0) != pdTRUE) {
              PrintTela("[CAN RX] Fila RX cheia! Mensagem descartada.");
            }
            else {
              //PrintTela("[CAN RX] Mensagem adicionada na fila RX");
            }
          } else {
            PrintTela("[CAN RX] Mensagem duplicada ignorada.");
          }
        }
        vTaskDelay(1);
      }
      
    }

    QueueHandle_t canQueueTx;
    TaskHandle_t CanTaskTxHandle = NULL;
    static void CanTaskTxWrapper(void *pvParameters) {
      CanService *service = static_cast<CanService*>(pvParameters);
      service->CanTaskTx(pvParameters);
    }

    void CanTaskTx(void* pvParameters) {
      CanService *service = static_cast<CanService*>(pvParameters);
      twai_message_t msg;
      while (true) {
        if (xQueueReceive(service->canQueueTx, &msg, portMAX_DELAY) == pdTRUE) {
          //PrintTela("[CAN TX] Mensagem recebida da fila TX");
          if (service->enviarDadosCan(msg)) {
            //PrintTela("[CAN TX] Mensagem enviada via CAN com sucesso");
          }
          else {
            PrintTela("[CAN TX] Erro ao enviar mensagem via CAN");
          }
        }
        vTaskDelay(1);
      }
    }

    TaskHandle_t CanTaskProcessHandle = NULL;
    static void CanTaskProcessWrapper(void *pvParameters) {
      CanService *service = static_cast<CanService*>(pvParameters);
      service->CanTaskProcess(pvParameters);
    }

    void CanTaskProcess(void* pvParameters) {
      CanService *service = static_cast<CanService*>(pvParameters);
      twai_message_t msg;

      while (true) {
        while (uxQueueMessagesWaiting(service->canQueueRx) > 0) {
          if (xQueueReceive(service->canQueueRx, &msg, portMAX_DELAY) == pdTRUE) {
            CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)msg.data[0];

            if (service->_callback) {
              String frameEmProcessamento = "";
              for (int i = 0; i < msg.data_length_code; i++) {
                frameEmProcessamento += String(msg.data[i], HEX) + " ";
              }

              idmsg++;

              CallbackPayload* payload = new CallbackPayload();
              payload->idMsg = idmsg;
              payload->callback = service->_callback;
              payload->packetSize = msg.data_length_code;
              payload->senderId = msg.identifier;
              payload->posicao = posicao;
              memcpy(payload->data, &msg.data[1], msg.data_length_code - 1);
              payload->dataLength = msg.data_length_code - 1;

              // Captura o handle da task atual (quem vai esperar a notificação)
              TaskHandle_t callingTaskHandle = xTaskGetCurrentTaskHandle();

              // Cria um pacote com o payload + handle do notificador
              auto* bundle = new std::pair<CallbackPayload*, TaskHandle_t>(payload, callingTaskHandle);

              PrintTela("[CAN Process] Processando frame: " + frameEmProcessamento);

              String taskName = "TaskCallbackTimeout_" + String(idmsg);

              TaskHandle_t handleTask = nullptr;

              // Cria a task do callback
              BaseType_t status = xTaskCreatePinnedToCore(
                TaskCallbackWrapper,
                taskName.c_str(),
                8192,
                bundle,
                5,
                &handleTask,
                APP_CPU_NUM
              );

              if (status != pdPASS) {
                PrintTela("[CAN Process] ❌ Falha ao criar task de callback!");
                delete payload;
                delete bundle;
                continue;
              }

              if (false && msg.data[1] == ID_Num_sLRA) {
                // Comandos para o LoRa nao precisam aguardar, podem ser processados sem tempo
              }
              else {
                // Aguarda até 5000ms pela notificação de término
                BaseType_t sinal = ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(5000));

                if (sinal == 0) {
                  PrintTela("[CAN Process] ⚠️ Callback demorou demais: " + String(payload->idMsg) + " — timeout ao executar comando: " + frameEmProcessamento);
                  
                  vTaskDelay(pdMS_TO_TICKS(10)); // dá tempo da task morrer sozinha
                  eTaskState estado = eTaskGetState(handleTask);

                  if (estado != eDeleted && estado != eInvalid) {
                    vTaskDelete(handleTask);
                  }
                }
              }
            }
          }
        }

        vTaskDelay(1);
      }
    }




    bool mensagemJaNaFila(QueueHandle_t fila, const twai_message_t& novaMsg) {
      twai_message_t msgTmp;
      UBaseType_t items = uxQueueMessagesWaiting(fila);
    
      for (UBaseType_t i = 0; i < items; i++) {
        if (xQueuePeek(fila, &msgTmp, 0) == pdTRUE) {
          if (msgTmp.identifier == novaMsg.identifier && msgTmp.data[0] == novaMsg.data[0] && msgTmp.data[1] == novaMsg.data[1]) {
            return true; // já tem uma igual
          }
        }
      }
      return false;
    }
    

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

    bool adicionarMensagemFila(const std::vector<uint8_t>& payload) {
      if (payload.size() > 8) return false;
  
      twai_message_t msg;
      msg.identifier = _nodeId; // + 1;
      msg.extd = 0;
      msg.rtr = 0;
      msg.ss = 0;
      msg.data_length_code = payload.size();
  
      for (uint8_t i = 0; i < payload.size(); ++i) {
        msg.data[i] = payload[i]; // 👈 Preenche os dados corretamente
      }
  
      if (!mensagemJaNaFila(canQueueTx, msg)) {
        if (xQueueSend(canQueueTx, &msg, 0) != pdTRUE) {
          PrintTela("[CAN] Fila TX cheia! Mensagem descartada.");
          return false;
        }
        else {
          //PrintTela("[CAN] Mensagem adicionada na fila TX");
          return true;
        }
      }
      else {
        PrintTela("[CAN] Mensagem duplicada ignorada.");
      }
    }

    bool enviarDadosCan(twai_message_t msg) {
      return twai_transmit(&msg, pdMS_TO_TICKS(10)) == ESP_OK;
    }


    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(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;
    }




    static void TaskCallbackWrapper(void* param) {
      auto bundle = static_cast<std::pair<CallbackPayload*, TaskHandle_t>*>(param);
      CallbackPayload* payload = bundle->first;
      TaskHandle_t notifyTo = bundle->second;

      payload->callback(
        payload->packetSize,
        payload->senderId,
        payload->posicao,
        payload->data,
        payload->dataLength
      );

      PrintTela("[CAN Callback] Processamento finalizado: " + String(payload->idMsg));

      // Notifica a task principal que terminou
      xTaskNotifyGive(notifyTo);

      // Limpa memória
      delete payload;
      delete bundle;

      // Encerra a task
      vTaskDelete(NULL);
    }

  private:
    uint8_t _nodeId;
    uint32_t _baudRate;
    OnReceiveCallback _callback = nullptr;
    bool DebugMode = false;
    volatile bool processamentoEmAndamento = false;
    volatile long idmsg = 0;
    
    
};

TaskHandle_t CanService::handleCallbackTask = NULL;

#endif