#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 {
  public:
    uint8_t ID_Num_sMOD = 0;
    uint8_t ID_Num_sTOD = 250;
    uint8_t ID_Num_sLRA = 251;
    typedef void (*OnReceiveCallback)(int packetSize, int senderId, byte funcCode, byte* data, int dataLength);

    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 = TWAI_FILTER_CONFIG_ACCEPT_ALL();
      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, 3, &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, 3, &CanTaskTxHandle, APP_CPU_NUM);
      }

      if (CanTaskProcessHandle == NULL) {
        xTaskCreatePinnedToCore(CanService::CanTaskProcessWrapper, "CanTaskProcess", 6144, this, 2, &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);
  
          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) {
            continue;
          }
  
          if (!mensagemJaNaFila(canQueueRx, message)) {
            if (xQueueSend(canQueueRx, &message, 0) != pdTRUE) {
              PrintTela("[CAN] Fila RX cheia! Mensagem descartada.");
            }
            else {
              //PrintTela("[CAN] Mensagem adicionada na fila RX");
            }
          } else {
            PrintTela("[CAN] 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] Mensagem recebida da fila TX");
          if (service->enviarDadosCan(msg)) {
            //PrintTela("[CAN] Mensagem enviada via CAN com sucesso");
          }
          else {
            PrintTela("[CAN] 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) {
      //PrintTela("[CAN] Task de Processamento iniciada");
      CanService *service = static_cast<CanService*>(pvParameters);
      twai_message_t msg;
      while (true) {
        if (xQueueReceive(service->canQueueRx, &msg, portMAX_DELAY) == pdTRUE) {
          //PrintTela("[CAN] Mensagem recebida da fila RX");
          uint8_t funcCode = msg.data[0];
          if (service->_callback) {
            service->_callback(msg.data_length_code, msg.identifier, funcCode, &msg.data[1], msg.data_length_code - 1);
          }
        }
        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] && msgTmp.data[2] == novaMsg.data[2]) {
            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 = 1;
      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;
    }


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

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

#endif