#include <stdint.h>

// Definições de máscara
#define PRIORITY_MASK 0x1C000000
#define EDP_MASK      0x02000000
#define DP_MASK       0x01000000
#define PF_MASK       0x00FF0000
#define PS_MASK       0x0000FF00
#define SA_MASK       0x000000FF
#define PDU1_PGN_MASK 0x03FF0000
#define PDU2_PGN_MASK 0x03FFFF00

// Exemplo de definição de PGNs específicos
enum PGN {
  VEHICLE_SPEED = 0x00F501,  // PGN para velocidade do veículo (62721)
  COOLANT_TEMP = 0x00FEE9,   // PGN para informação da temperatura do líquido de arrefecimento (65257)
  FUEL_RATE = 0x00F004,      // PGN para informação da taxa de consumo de combustível (61444)
  DIAG_INFO = 0x00FECA,      // PGN para informação de diagnóstico (65226)
};

struct J1939Frame {
  uint8_t priority : 3;      // Prioridade: 3 bits
  uint8_t dataPage : 1;      // Data Page: 1 bit
  uint8_t pduFormat : 8;     // PDU Format: 8 bits (não pode ser otimizado para menos de 8 bits)
  uint8_t pduSpecific : 8;   // PDU Specific: 8 bits
  uint8_t sourceAddress : 8; // Endereço de Origem: 8 bits
  uint32_t pgn : 18;         // PGN extraído: 18 bits, requer mais de 16 bits, então uint32_t é usado
  uint8_t length;            // Comprimento dos dados: 0 a 8
  uint8_t data[8];           // Dados: inicializados com zeros

  // Construtor para inicializar os membros da estrutura com valores padrão
  J1939Frame() : priority(0), dataPage(0), pduFormat(0), pduSpecific(0),
    sourceAddress(0), pgn(0), length(0), data{} {}
};

J1939Frame createFrameWithPGN(uint32_t pgn) {
  J1939Frame frame;
  frame.priority = 6; // Exemplo de valor de prioridade
  frame.dataPage = (pgn >> 16) & 0x1;
  frame.pduFormat = (pgn >> 8) & 0xFF;
  frame.pduSpecific = pgn & 0xFF;
  frame.sourceAddress = 0x01; // Exemplo de endereço de origem

  return frame;
}

uint32_t assembleId(const J1939Frame& frame) {
  // O PGN é montado a partir de DP, PF, e PS
  uint32_t pgn = ((uint32_t)frame.dataPage << 16) | ((uint32_t)frame.pduFormat << 8) | (uint32_t)frame.pduSpecific;

  // Monta o ID de 29 bits seguindo a estrutura J1939
  uint32_t id = 0;

  id |= (frame.priority & 0x7) << 26;
  id |= (pgn & 0x3FFFF) << 8; // Inclui os 18 bits do PGN
  id |= (frame.sourceAddress & 0xFF);

  return id;
}

J1939Frame dissectId(uint32_t id) {
  J1939Frame frame;
  frame.priority = (id >> 26) & 0x7;
  frame.dataPage = (id >> 25) & 0x1;
  frame.pduFormat = (id >> 16) & 0xFF;
  frame.pduSpecific = (id >> 8) & 0xFF;
  frame.sourceAddress = id & 0xFF;

  // Calcula o PGN combinando DP, PF, e PS
  frame.pgn = ((uint32_t)frame.dataPage << 16) | ((uint32_t)frame.pduFormat << 8) | (uint32_t)frame.pduSpecific;

  return frame;
}

uint32_t assembleJ1939Id(uint8_t priority, uint32_t pgn, uint8_t sa) {
  uint32_t id = 0;
  id |= ((uint32_t)priority << 26);
  id |= (pgn << 8);
  id |= sa;
  return id;
}

void getJ1939FromId(uint32_t canId, uint8_t& priority, uint32_t& pgn, uint8_t& da, uint8_t& sa) {
  priority = (PRIORITY_MASK & canId) >> 26;
  uint8_t pf = (canId & PF_MASK) >> 16;
  sa = canId & SA_MASK;

  if (pf >= 240) {
    da = 255; // PDU2 formato, o DA é global (broadcast)
    pgn = (canId & PDU2_PGN_MASK) >> 8;
  } else {
    da = (canId & PS_MASK) >> 8; // PDU1 formato, DA é específico
    pgn = (canId & PDU1_PGN_MASK) >> 8;
  }
}

int serial_putchar(char c, FILE* f) {
  if (c == '\n') serial_putchar('\r', f);
  return Serial.write(c) == 1 ? 0 : 1;
}

void printf_begin(void) {
  // Inicializa o stdout com a função de redirecionamento para Serial.write
  Serial.begin(9600);
  fdevopen(&serial_putchar, 0);
}

void printHex(uint8_t num) {
  // Usa printf para formatar e imprimir o número em base hexadecimal
  printf("%02X", num); // %02X garante a impressão de pelo menos dois dígitos
}

void printPGN(uint32_t pgn) {
  // Assegura a impressão do PGN com até 6 dígitos hexadecimais
  printf("  PGN: 0x%06X\n", pgn);
}

void printCanFrameHex(const J1939Frame& frame) {
  // Imprime o ID, formatando para garantir 8 dígitos hexadecimais
  printf("Frame: 0x00");
  Serial.print(assembleId(frame), HEX);
  printf("  [%d]  ", frame.length);

  // Itera sobre os dados do frame e os imprime em hexadecimal
  for (int i = 0; i < frame.length; i++) {
    printf("%02X ", frame.data[i]);
  }

  printf("\n");
}

void setSpeedData(J1939Frame& frame, float speedKmh) {
  // Converte a velocidade para um valor inteiro mantendo duas casas decimais
  uint16_t speedScaled = static_cast<uint16_t>(speedKmh * 100);

  // Divide o valor de 16 bits em dois bytes e armazena nos dois primeiros bytes do payload
  frame.data[0] = (speedScaled >> 8) & 0xFF; // Byte mais significativo
  frame.data[1] = speedScaled & 0xFF; // Byte menos significativo

  // Preenche o restante do payload com zeros
  for (uint8_t i = 2; i < 8; i++) {
    frame.data[i] = 0x00;
  }

  // Define o comprimento do payload para 8 bytes
  frame.length = 8;
}

float decodeSpeedData(const J1939Frame& frame) {
  // Decodifica o valor de velocidade a partir dos dois primeiros bytes
  uint16_t speedScaled = ((uint16_t)frame.data[0] << 8) | (uint16_t)frame.data[1];

  // Converte de volta para km/h dividindo pelo fator de escala
  return speedScaled / 100.0f;
}

void printDecodedSpeed(const J1939Frame& frame) {
  float speedKmh = decodeSpeedData(frame);
  printf("Speed: ");
  Serial.print(speedKmh, 2); // Imprime com duas casas decimais
  printf(" km/h\n");
}

void sendCANFrame() {
  // Cria um frame com o PGN para a velocidade do veículo
  J1939Frame speedFrame = createFrameWithPGN(VEHICLE_SPEED);
  speedFrame.priority = 5;
  speedFrame.sourceAddress = 0x01;
  setSpeedData(speedFrame, 11.73); // Seta a velocidade do veículo

  // Debug
  printCanFrameHex(speedFrame); // Envia o frame CAN formatado
  printPGN(dissectId(assembleId(speedFrame)).pgn); // Imprime o PGN na serial
  printDecodedSpeed(speedFrame); // Imprime a velocidade na serial
  printf("\n");
}

uint32_t previousMillis = 0; // Armazena a última vez que o frame CAN foi enviado
const uint32_t interval = 1000; // Intervalo entre frames (em milissegundos)

void setup() {
  printf_begin();
}

void loop() {
  uint32_t currentMillis = millis(); // Chamada única a millis() por loop
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis; // Atualiza o tempo do último envio
    sendCANFrame(); // Envia o frame de dados CAN
  }
}