#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;
    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;
      }
    
      PrintTela("[CAN] TWAI iniciado com sucesso");
    }

    void loop() {
      twai_message_t message;
      if (twai_receive(&message, pdMS_TO_TICKS(10)) == ESP_OK) {

        if (message.data_length_code == 0 || message.extd) return;

        byte funcCode = message.data[0];
        
        bool MensagemTx = (int)funcCode % 2 == 0;

        Serial.print("[CAN ");
        Serial.print(MensagemTx ? "TX" : "RX");
        Serial.print("] ID: 0x");
        Serial.print(message.identifier, HEX);
        Serial.print(" Len: ");
        Serial.print(message.data_length_code);
        Serial.print(" Data: ");
        for (int i = 0; i < message.data_length_code; i++) {
          Serial.print(message.data[i], HEX);
          Serial.print(" ");
        }
        Serial.println();

        if (MensagemTx) { // funcCodes impares sao enviados pelo esp, ignorar
          return;
        }

        if (_callback != nullptr) {
          //PrintTela("Entrar no callback...");
          _callback(message.data_length_code, message.identifier, funcCode, &message.data[1], message.data_length_code - 1);
        }
      }
    }

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

    bool enviarMensagem(const std::vector<uint8_t>& payload) {
      if (payload.size() > 8) return false;
  
      twai_message_t msg;
      msg.identifier = _nodeId;
      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
      }
  
      return twai_transmit(&msg, pdMS_TO_TICKS(10)) == ESP_OK;
    }

    bool enviarMensagem(byte funcCode, const std::vector<uint8_t>& payload) {
      if (payload.size() > 7) return false;

      twai_message_t msg;
      msg.identifier = _nodeId;
      msg.extd = 0;
      msg.rtr = 0;
      msg.ss = 1;
      msg.data_length_code = payload.size() + 1;
      msg.data[0] = funcCode;

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

      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